#include "usb.h"
#include "usb_device.h"
#include <stm32h747xx.h>

typedef union {
  uint32_t word;
  uint16_t halfwords[2];
  uint8_t bytes[4];
} usb_data_t;

void usb_generic_send(USB_OTG_INEndpointTypeDef *endpoint,
                      uint8_t *data,
                      uint16_t size,
                      volatile uint32_t *fifo_tx_target) {
  if (endpoint->DIEPCTL & USB_OTG_DIEPCTL_EPENA) {
    // this is bad! Can't send the packet, this shouldn't get here, ever.
    while (endpoint->DIEPCTL & USB_OTG_DIEPCTL_EPENA);
  }

  // TODO: generic max packet size
  uint16_t packet_count = size + 63 / 64;

  endpoint->DIEPTSIZ = (packet_count << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | (size << USB_OTG_DIEPTSIZ_XFRSIZ_Pos);

  usb_data_t tx_data;
  uint8_t subWordBytes = size % 4;
  uint8_t wordCount = size / 4;

  for (uint8_t i = 0; i < wordCount; i++) {
    tx_data.word = *((uint32_t*)data);
    *fifo_tx_target = tx_data.word;
    data += 4;
  }

  // TODO: check where to put it... beginning, end...
  if (subWordBytes != 0) {
    tx_data.word = 0;
    for (uint8_t i = 0; i < subWordBytes; i++) {
      tx_data.bytes[i] = *(data + i);
    }
    *fifo_tx_target = tx_data.word;
  }

  endpoint->DIEPCTL |= USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA;
}

void usb_generic_read(uint8_t *data, uint16_t size, volatile uint32_t *fifo_rx_source) {
  usb_data_t rx_data;
  uint8_t subWordBytes = size % 4;
  uint8_t wordCount = size / 4;

  for (uint8_t i = 0; i < wordCount; i++) {
    rx_data.word = *fifo_rx_source;
    *((uint32_t*)data) = rx_data.word;
    data += 4;
  }

  // TODO: check where to put it... beginning, end...
  if (subWordBytes > 0) {
    rx_data.word = 0;
    for (uint8_t i = 0; i < subWordBytes; i++) {
      *(data + i) = rx_data.bytes[i];
    }
  }
}

void usb_send_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                         usb_descriptor_t *descriptor,
                         volatile uint32_t *fifo_tx_target) {
  usb_generic_send(endpoint, (uint8_t*)descriptor, descriptor->bLength, fifo_tx_target);
}

void usb_send_configuration_descriptor(USB_OTG_INEndpointTypeDef* endpoint,
                                       usb_class_t *class,
                                       volatile uint32_t *fifo_tx_target) {
  class->configuration_descriptor.header.bDescriptorType = DESCRIPTOR_CONFIGURATION;
  class->configuration_descriptor.header.bLength = sizeof(usb_configuration_descriptor_t);
  class->interface_descriptor.header.bDescriptorType = DESCRIPTOR_INTERFACE;
  class->interface_descriptor.header.bLength = sizeof(usb_interface_descriptor_t);
  uint16_t total_length =
    sizeof(usb_configuration_descriptor_t) +
    sizeof(usb_interface_descriptor_t) +
    class->interface_descriptor.bNumEndpoints * sizeof(usb_endpoint_descriptor_t);

  for (int i = 0; i < class->interface_descriptor.bNumEndpoints; i++) {
    class->endpoint_descriptors[i].header.bDescriptorType = DESCRIPTOR_ENDPOINT;
    class->endpoint_descriptors[i].header.bLength = sizeof(usb_endpoint_descriptor_t);
  }

  // NOTE: since the memory layout is: configuration, interface, endpoints,
  // we can send directly like this.
  usb_generic_send(endpoint,
                   (uint8_t*)&class->configuration_descriptor,
                   total_length, fifo_tx_target);
}
void usb_send_device_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                usb_device_descriptor_t *descriptor,
                                volatile uint32_t *fifo_tx_target) {
  descriptor->header.bDescriptorType = DESCRIPTOR_DEVICE;
  descriptor->header.bLength = sizeof(usb_device_descriptor_t);
  usb_send_descriptor(endpoint, &descriptor->header, fifo_tx_target);
}
void usb_send_device_qualifier_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                          usb_device_qualifier_t *descriptor,
                                   volatile uint32_t *fifo_tx_target) {
  descriptor->header.bDescriptorType = DESCRIPTOR_DEVICE_QUALIFIER;
  descriptor->header.bLength = sizeof(usb_device_qualifier_t);
  usb_send_descriptor(endpoint, &descriptor->header, fifo_tx_target);
}
void usb_send_interface_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                   usb_interface_descriptor_t *descriptor,
                                   volatile uint32_t *fifo_tx_target) {
  descriptor->header.bDescriptorType = DESCRIPTOR_INTERFACE;
  descriptor->header.bLength = sizeof(usb_interface_descriptor_t);
  usb_send_descriptor(endpoint, &descriptor->header, fifo_tx_target);
}
void usb_send_endpoint_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                  usb_endpoint_descriptor_t *descriptor,
                                  volatile uint32_t *fifo_tx_target) {
  descriptor->header.bDescriptorType = DESCRIPTOR_ENDPOINT;
  descriptor->header.bLength = sizeof(usb_endpoint_descriptor_t);
  usb_send_descriptor(endpoint, &descriptor->header, fifo_tx_target);
}

void usb_send_string_descriptor_zero(USB_OTG_INEndpointTypeDef *endpoint,
                                     usb_string_descriptor_zero_t *string_descriptor,
                              volatile uint32_t *fifo_tx_target) {
  usb_data_t data;
  data.word = 0;

  data.bytes[0] = string_descriptor->header.bLength;
  data.bytes[1] = string_descriptor->header.bDescriptorType;

  if (string_descriptor->header.bLength == 2) {
    *fifo_tx_target = data.word;
    return;
  }

  data.halfwords[1] = string_descriptor->wLANGID[0];

  *fifo_tx_target = data.word;

  // TODO: Now the rest only from wLANGID[1..]
  if (string_descriptor->header.bLength > 4) {
    usb_generic_send(endpoint,
                     (uint8_t *)&string_descriptor->wLANGID[1],
                     string_descriptor->header.bLength - 4,
                     fifo_tx_target);
  }
}
void usb_send_unicode_string_descriptor(
    USB_OTG_INEndpointTypeDef *endpoint,
    usb_unicode_string_descriptor_t *string_descriptor,
    volatile uint32_t *fifo_tx_target) {
  usb_data_t data = { .word = 0 };
  data.halfwords[0] = *((uint16_t*)string_descriptor);
  if (string_descriptor->bString != 0 && string_descriptor->header.bLength > 2) {
    // NOTE: if the string had length of just one, there would be two bytes
    // read here. That shouldn't usually matter much, so we don't care about
    // that here.
    data.halfwords[1] = *((uint16_t*)string_descriptor->bString);
  }
  *fifo_tx_target = data.word;

  if (string_descriptor->header.bLength > 4) {
    usb_generic_send(endpoint, &string_descriptor->bString[2],
                     string_descriptor->header.bLength - 4,
                     fifo_tx_target);
  }
}