#include "usb_device.h"
#include "usb.h"
#include <stdlib.h>
#include <stddef.h>
#include <stm32h747xx.h>
#include "registers.h"

/* USB_OTG_GlobalTypeDef */

usb_device_t* usb1_device;

void* usb_device_init(void* peripheral_address, usb_class_t* class, void* buffer) {
  if (buffer == NULL) {
    buffer = (void*)malloc(sizeof(usb_device_t));
  }

  usb_device_t* device = (usb_device_t*)buffer;

  device->state = INIT;
  device->core = peripheral_address + USB_OTG_GLOBAL_BASE;
  device->device = peripheral_address + USB_OTG_DEVICE_BASE;
  device->out = peripheral_address + USB_OTG_OUT_ENDPOINT_BASE;
  device->in = peripheral_address + USB_OTG_IN_ENDPOINT_BASE;
  device->fifos = (usb_fifo_t*)(((uint8_t*)device->core) + USB_OTG_FIFO_BASE);
  device->endpoint_count = 8;

  // TODO: clarify how this should work...
  usb1_device = device;

  return device;
}

void usb_device_reset(void* device_ptr) {
  usb_device_t* device = (usb_device_t*)device_ptr;

  // Idle
  while ((device->core->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0);
  // Reset
  device->core->GRSTCTL |= USB_OTG_GRSTCTL_CSRST;
  // Wait for reset
  while (device->core->GRSTCTL & USB_OTG_GRSTCTL_CSRST);
  // Wait for idle
  while ((device->core->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0);
}

void usb_device_clear_interrupts(void* device_ptr) {
  usb_device_t* device = (usb_device_t*)device_ptr;
  device->core->GINTSTS = 0; // clear interrupts

  // for other interrupts the rx queue has to be read
}

void usb_device_wait_for_handshake(void* device_ptr) {
  usb_device_t* device = (usb_device_t*)device_ptr;

  while (device->state != ENUMERATED);
}

void usb_device_setup(void* device_ptr) {
  RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN | RCC_AHB1ENR_USB1OTGHSULPIEN;

  volatile uint32_t dummy;
  dummy = RCC->AHB1ENR;
  dummy = RCC->AHB1ENR;

  NVIC_SetPriority(OTG_HS_IRQn, 1);
  NVIC_EnableIRQ(OTG_HS_IRQn);

  // 1. core initialization
  usb_device_t* device = (usb_device_t*)device_ptr;

  device->device->DCTL |= USB_OTG_DCTL_SDIS;

  PWR->CR3 |= PWR_CR3_USB33DEN;
  while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0);

  device->core->GCCFG |= USB_OTG_GCCFG_PWRDWN;

  device->core->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD;

  usb_device_reset(device_ptr);

  // global interrupt mask
  // rx fifo non empty
  // periodic tx fifo empty level
  usb_device_clear_interrupts(device_ptr);
  device->core->GAHBCFG = USB_OTG_GAHBCFG_GINT | USB_OTG_GAHBCFG_TXFELVL | USB_OTG_GAHBCFG_HBSTLEN_2;

  // hnp capable, srp capable, otg hs timeout, usb turnaround
  // TODO: based on enumeration speed set TOCAL - fs timeout calibration
  device->core->GUSBCFG = USB_OTG_GUSBCFG_FDMOD | USB_OTG_GUSBCFG_ULPIEVBUSI | USB_OTG_GUSBCFG_ULPIEVBUSD | USB_OTG_GUSBCFG_TRDT_3 | USB_OTG_GUSBCFG_TRDT_2;

  // TODO: are these needed? device mode is forced...
  // unmask otg interrupt mask
  /* device->core->GINTMSK |= USB_OTG_GINTMSK_OTGINT | USB_OTG_GINTMSK_MMISM; */

  // device initialization

  // descdma, device speed, non-zero-length status out handshake, periodic frame interval
  device->device->DCFG = 0;

  // TODO device threshold control register IF DMA

  // unmask interrupts usb reset, enumeration done, early suspend, usb suspend, sof
  device->core->GINTMSK |= USB_OTG_GINTMSK_ENUMDNEM | USB_OTG_GINTMSK_RSTDEM | USB_OTG_GINTMSK_USBRST | USB_OTG_GINTMSK_ESUSPM | USB_OTG_GINTMSK_SOFM | USB_OTG_GINTMSK_RXFLVLM;

  // sdis bit
  device->device->DCTL = ~(USB_OTG_DCTL_SDIS);

  // wait for usbrst interrupt - the reset
  /* while (((device->core->GINTSTS) & USB_OTG_GINTSTS_USBRST) == 0) {} */

  // wait for enumdne interrupt - end of reset
  /* while (((device->core->GINTSTS) & USB_OTG_GINTSTS_ENUMDNE) == 0) {} */
}

void usb_handle_endpoint_int(usb_device_t* device) {
  device->core->GRXSTSP;
  device->core->GRXFSIZ;
}

typedef enum {
  PACKET_GLOBAL_OUT_NAK = 1,
  PACKET_OUT_DATA_PACKET_RCVD = 2,
  PACKET_OUT_TRANSFER_COMPLETED = 3,
  PACKET_SETUP_TRANSACTION_COMPLETED = 4,
  PACKET_SETUP_DATA_PACKET_RECEIVED = 6,
} packet_status_t;

typedef enum {
  DATA0 = 0,
  DATA1 = 1,
  DATA2 = 2,
  MDATA = 3
} packet_dpid_t;

typedef struct {
  packet_status_t packet_status;
  packet_dpid_t dpid;
  uint8_t byte_count;
  uint8_t endpoint_num;
} packet_info_t;

void usb_handle_rxflvl_control_int(usb_device_t *device,
                                   packet_info_t *packet_info) {
  uint32_t dummy;
  uint8_t data[64];
  uint32_t *fifo = device->fifos[0].data;

  if (packet_info->packet_status == PACKET_SETUP_TRANSACTION_COMPLETED) {

  } else if (packet_info->packet_status == PACKET_SETUP_DATA_PACKET_RECEIVED) {
    /* dummy = *fifo; // internal info */
    // SAVE data
    usb_generic_read((uint8_t*)&device->received_setup_command, 8, fifo);

    // 00000000010000000000000000000000
    // 00000000101010000000000000000000

    dummy = *fifo; // the last that will trigger another interrupt
  } else if (packet_info->byte_count != 0) {
    usb_generic_read(data, packet_info->byte_count, fifo);
  }
}

void usb_handle_rxflvl_int(usb_device_t *device) {
  // Disable the interrupt
  reg_clear_bits(&device->core->GINTMSK, USB_OTG_GINTMSK_RXFLVLM);

  /* uint32_t rx_data; */
  /* while ((rx_data = device->core->GRXSTSP) != 0) { */
  while (device->core->GINTSTS & USB_OTG_GINTSTS_RXFLVL) {
    uint32_t status_data = device->core->GRXSTSP;

    /* uint8_t status_phase_start = reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_STSPHST); */
    packet_info_t packet_info = {
      .packet_status = (packet_status_t)reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_PKTSTS_Pos, 0xF),
      .dpid = (packet_dpid_t)reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_DPID_Pos, 0x3),
      .byte_count = reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_BCNT_Pos, 0x7FF),
      .endpoint_num = reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_EPNUM_Pos, 0xF),
    };

    if (packet_info.endpoint_num == 0) {
      usb_handle_rxflvl_control_int(device, &packet_info);
    } else {
      // TODO: ...
    }
  }

  // Read info about the endoint etc.

  // Re-enable the interrupt
  reg_set_bits(&device->core->GINTMSK, USB_OTG_GINTMSK_RXFLVLM);
}

// NOTE: this is irq handler
void otg_hs_handler(void) {

  usb_device_t* device = usb1_device;

  // Reset detected
  if (device->core->GINTSTS & USB_OTG_GINTSTS_USBRST) {
    reg_set_bits(&device->core->GINTMSK, USB_OTG_GINTMSK_IEPINT | USB_OTG_GINTMSK_OEPINT);
    reg_set_bits(&device->device->DOEPMSK, USB_OTG_GINTMSK_IEPINT | USB_OTG_GINTMSK_OEPINT);

    // TODO: should this be done for ep 0 as well?
    for (int ep = 0; ep < device->endpoint_count; ep++) {
      USB_OTG_OUTEndpointTypeDef *out = device->out + ep;
      out->DOEPCTL |= USB_OTG_DOEPCTL_SNAK;
    }
    device->device->DAINTMSK |= 1 << USB_OTG_DAINTMSK_IEPM_Pos;
    device->device->DAINTMSK |= 1 << USB_OTG_DAINTMSK_OEPM_Pos;

    device->device->DOEPMSK |= USB_OTG_DOEPMSK_STUPM | USB_OTG_DOEPMSK_XFRCM;
    device->device->DIEPMSK |= USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_TOM;

    // 512 bytes
    device->core->GRXFSIZ = 512 / 4;
    // 64 bytes, beginning of ram
    device->core->DIEPTXF[0] = (0) << USB_OTG_DIEPTXF_INEPTXSA_Pos | (64 / 4) << USB_OTG_DIEPTXF_INEPTXFD_Pos;

    // 3 packets to receive as setup
    reg_write_bits_pos(&device->out->DOEPTSIZ, 3, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);

    device->state = RESET;
    // clear it
    reg_set_bits(&device->core->GINTSTS,  USB_OTG_GINTSTS_USBRST);

    return;
  }

  // Reset ended
  if (device->core->GINTSTS & USB_OTG_GINTSTS_ENUMDNE) {
    // The enumerated speed
    /* (device->device->DSTS & USB_OTG_DSTS_ENUMSPD_Msk) >> USB_OTG_DSTS_ENUMSPD_Pos; */
    // We do not need to know the speed, it can be either full speed or high
    // speed, both have maximum size 64 bytes. Only low speed or super high
    // speed would differ, but these are not supported!
    reg_write_bits_pos(&device->in->DIEPCTL, 64, USB_OTG_DIEPCTL_MPSIZ_Pos, 0x7FFUL);

    device->out->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;

    device->state = RESET_DONE;
    // clear it
    reg_set_bits(&device->core->GINTSTS,  USB_OTG_GINTSTS_ENUMDNE);

    return;
  }

  // Start of frame
  if (device->core->GINTSTS & USB_OTG_GINTSTS_SOF) {
    // Nothing to do?
    reg_set_bits(&device->core->GINTSTS,  USB_OTG_GINTSTS_SOF);
    return;
  }

  // OTG interrupt, should not be triggered since forced device mode
  if (device->core->GINTSTS & USB_OTG_GINTSTS_OTGINT) {
    device->state = OTG_ERROR;
    reg_set_bits(&device->core->GINTSTS,  USB_OTG_GINTSTS_OTGINT);
    return;
  }

  // OTG interrupt, should not be triggered since forced device mode
  if (device->core->GINTSTS & USB_OTG_GINTSTS_MMIS) {
    device->state = OTG_ERROR;
    reg_set_bits(&device->core->GINTSTS,  USB_OTG_GINTSTS_MMIS);
    return;
  }

  // Setup, data...
  if (device->core->GINTSTS & (USB_OTG_GINTSTS_OEPINT | USB_OTG_GINTSTS_IEPINT)) {
    // TODO
    usb_handle_endpoint_int(device);
    return;
  }

  if (device->core->GINTSTS & (USB_OTG_GINTSTS_RXFLVL)) {
    // TODO
    usb_handle_rxflvl_int(device);
    return;
  }

  device->state = UNKNOWN_INTERRUPT;
}