#include "usb_device.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->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_control_interrupt(usb_device_t *device) {
  uint32_t daint = device->device->DAINT;

  if (daint & (1 << USB_OTG_DAINT_IEPINT_Pos)) {
    // in (that means sending data)
  } else if (daint & (1 << USB_OTG_DAINT_OEPINT_Pos)) {
    // out (that means receiving data)
  } else {
    device->state = CONTROL_ERROR;
  }
}

void usb_handle_endpoint_interrupt(usb_device_t* device) {
  // TODO

  if ((device->state < ENUMERATED) || (device->device->DAINT & ((1 << USB_OTG_DAINT_IEPINT_Pos) | (1 << USB_OTG_DAINT_OEPINT_Pos)))) {
    usb_handle_control_interrupt(device);
  } else {
    // new data received, put it to application level?.
  }
}

// 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 = 1; 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->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_interrupt(device);
    return;
  }

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

  device->state = UNKNOWN_INTERRUPT;
}