#include "usb_cdc.h"
#include "usb_util.h"
#include "led.h"
#include "config.h"

// Ring buffer for received data
static uint8_t USBCDC_RXBuffer[CONFIG_USB_RINGBUFFER_SIZE];
// Where the next received byte will be written to
static uint8_t *volatile USBCDC_RXBufferWrite = USBCDC_RXBuffer;
// Where the receive function reads the next byte from the buffer
static uint8_t *volatile USBCDC_RXBufferRead = USBCDC_RXBuffer;

// Is set true when the receive ring buffer is too full to hold another USB
// packet and the USB endpoint is halted.
static volatile bool USBCDC_RXSuspended = false;

// Ring buffer for data to be sent
static uint8_t USBCDC_TXBuffer[CONFIG_USB_RINGBUFFER_SIZE];
// Where the USB transmit procedure will read the next byte from
static uint8_t *volatile USBCDC_TXBufferRead = USBCDC_TXBuffer;
// Where the send routine will write the next byte into the buffer
static uint8_t *volatile USBCDC_TXBufferWrite = USBCDC_TXBuffer;

// True as long as a USB in transmission is ongoing. Is set to false in
// USBCDC_BuildInPacket() after an USB interrupt when the TX ring buffer is
// empty
static volatile bool USBCDC_TXTransferring = false;

USBCDC_LineCoding_t USBCDC_LineCoding =
{
    .dwDTERate = 9600,
    .bCharFormat = 0,
    .bParityType = 0,
    .bDataBits = 8
};

void USBCDC_InitEndpoints(void)
{
    // Endpoint 1: Interrupt endpoint for notifications (buffer size 8)
    USB_BTABLE_ENTRIES[1].COUNT_TX_0 = 0;
    USB_BTABLE_ENTRIES[1].ADDR_TX_0 = 0xc0;

    USB_SetEPR(&(USB->EP1R), USB_EPR_EP_TYPE_INTERRUPT
        | USB_EPR_STAT_TX_NAK | USB_EPR_STAT_RX_DISABLED
        | (1 << USB_EP1R_EA_Pos));

    // Endpoint 2: Data in (buffer size 64)
    USB_BTABLE_ENTRIES[2].COUNT_TX = 0;
    USB_BTABLE_ENTRIES[2].ADDR_TX = 0x100;

    USB_SetEPR(&(USB->EP2R), USB_EPR_EP_TYPE_BULK
        | USB_EPR_STAT_TX_NAK | USB_EPR_STAT_RX_DISABLED
        | (2 << USB_EP2R_EA_Pos));


    // Endpoint 3: Data out (buffer size 64)
#if USBCDC_ENDPOINT_SIZE == 64
    USB_BTABLE_ENTRIES[3].COUNT_RX = USB_EP_RXCOUNT_BL_SIZE | (1 << 10);
#else
#error Endpoint size currently unsupported
#endif
    USB_BTABLE_ENTRIES[3].ADDR_RX = 0x140;

    USB_SetEPR(&(USB->EP3R), USB_EPR_EP_TYPE_BULK
        | USB_EPR_STAT_TX_DISABLED | USB_EPR_STAT_RX_VALID
        | (3 << USB_EP3R_EA_Pos));
}

void USBCDC_HandleLineCodingPacket(int length)
{
    // Sanity check for packet length (the align byte is not counted here,
    // because it is not transmitted)
    if(length != sizeof(USBCDC_LineCoding_t) - 1)
    {
        __asm__ volatile("bkpt");
    }

    USB_PMAToMemory((uint8_t*)&USBCDC_LineCoding, USB_BTABLE_ENTRIES[0].ADDR_RX,
        sizeof(USBCDC_LineCoding_t));
    USB_EP0OutPacketHandler = NULL;
}

void USBCDC_HandleInterfaceSetup(USB_SetupPacket_t *sp,
    const void **reply_data, int *reply_length, uint8_t *reply_response)
{
    switch(sp->bRequest)
    {
        case USBCDC_REQ_GET_LINE_CODING:
            *reply_data = (const uint8_t*)&USBCDC_LineCoding;
            *reply_length = sizeof(USBCDC_LineCoding_t);
            break;

        case USBCDC_REQ_SET_LINE_CODING:
            *reply_response = USB_EP_TX_VALID;
            *reply_data = NULL;

            // The line coding will be sent in a separate packet (which won't be
            // a setup packet). We thus set a handler which will be called for
            // the next out transaction on endpoint 0.
            USB_EP0OutPacketHandler = USBCDC_HandleLineCodingPacket;
            break;

        case USBCDC_REQ_SET_CONTROL_LINE_STATE:
            // Ignored at the moment. wValue contains the control signal bitmap:
            // Bit 0 corresponds to DTR, bit 1 to RTS.
            *reply_response = USB_EP_TX_VALID;
            *reply_data = NULL;
            break;

        default:
            // Not implemented
            __asm__ volatile("bkpt");
            *reply_response = USB_EP_TX_STALL;
            *reply_data = NULL;
            break;
    }
}

void USBCDC_HandleDataOut(void)
{
    // Use a local variable for the ring buffer write pointer to avoid the
    // volatile access overhead (which is fine because the pointer isn't
    // modified anywhere else
    uint8_t *ring_buffer = USBCDC_RXBufferWrite;

    // Copy packet from PMA into the ring buffer. Duplicating the code here is
    // easier than reusing USB_PMAToMemory() because the PMA's addressing
    // weirdness creates a lot of edge cases with the ring buffer wraparound.
    int packet_length = USB_BTABLE_ENTRIES[3].COUNT_RX & 0x3ff;
    uint16_t pma_offset = USB_BTABLE_ENTRIES[3].ADDR_RX;
    uint8_t *pma = (uint8_t*)(USB_PMA_ADDR + 2 * pma_offset);
    for(int i = 0; i < packet_length; i++)
    {
        *ring_buffer++ = *pma++;
        if(ring_buffer >= USBCDC_RXBuffer + CONFIG_USB_RINGBUFFER_SIZE)
        {
            ring_buffer = USBCDC_RXBuffer;
        }
        if(i & 1)
        {
            pma += 2;
        }
    }

    // Set the ring buffer write pointer one element past the last one written
    // in this function
    USBCDC_RXBufferWrite = ring_buffer;

    // Determine the free space in the ring buffer and if another packet will
    // fit in
    int ring_buffer_space = USBCDC_RXBufferRead - USBCDC_RXBufferWrite - 1;
    if(ring_buffer_space < 0)
    {
        ring_buffer_space += CONFIG_USB_RINGBUFFER_SIZE;
    }
    if(ring_buffer_space >= USBCDC_ENDPOINT_SIZE)
    {
        // Set EP3 ready for the next packet only when there is enough space in
        // the ring buffer
        USB_SetEPRXStatus(&(USB->EP3R), USB_EP_RX_VALID);
    }
    else
    {
        USB_SetEPRXStatus(&(USB->EP3R), USB_EP_RX_NAK);
        USBCDC_RXSuspended = true;
    }
}

int USBCDC_ReceiveData(void *buffer, int length)
{
    int available = USBCDC_RXBufferWrite - USBCDC_RXBufferRead;
    if(available < 0)
    {
        available += CONFIG_USB_RINGBUFFER_SIZE;
    }
    if(available < length)
    {
        length = available;
    }
    if(length <= 0)
    {
        return 0;
    }
    int buffer_length = length;

    int until_wraparound = USBCDC_RXBuffer + CONFIG_USB_RINGBUFFER_SIZE
        - USBCDC_RXBufferRead;
    if(buffer_length >= until_wraparound)
    {
        memcpy(buffer, USBCDC_RXBufferRead, until_wraparound);
        buffer_length -= until_wraparound;
        buffer += until_wraparound;
        USBCDC_RXBufferRead = USBCDC_RXBuffer;
    }

    memcpy(buffer, USBCDC_RXBufferRead, buffer_length);
    USBCDC_RXBufferRead += buffer_length;

    // If receiving has been suspended because the ring buffer is too full,
    // resume if there is now enough space for another packet
    if(USBCDC_RXSuspended)
    {
        int ring_buffer_free = USBCDC_RXBufferRead - USBCDC_RXBufferWrite - 1;
        if(ring_buffer_free >= USBCDC_ENDPOINT_SIZE)
        {
            USB_SetEPRXStatus(&(USB->EP3R), USB_EP_RX_VALID);
            USBCDC_RXSuspended = false;
        }
    }

    return length;
}

// Build a USB in packet out of data from the transmit ring buffer and mark it
// valid for the host to fetch it
static void USBCDC_BuildInPacket(void)
{
    // Wait while there's still data being transmitted
    while(USB_BTABLE_ENTRIES[2].COUNT_TX);

    int packet_length = 0;

    int ring_buffer_bytes = USBCDC_TXBufferWrite - USBCDC_TXBufferRead;

    if(ring_buffer_bytes < 0)
    {
        // Ring buffer read address is bigger than the write address, meaning
        // the write has wrapped around. Let's first deal with the data until
        // the end of the buffer
        int until_wraparound = USBCDC_TXBuffer + CONFIG_USB_RINGBUFFER_SIZE
            - USBCDC_TXBufferRead;
        if(until_wraparound > USBCDC_ENDPOINT_SIZE)
        {
            // More bytes to be sent than there is space in the PMA buffer
            packet_length = USBCDC_ENDPOINT_SIZE;
            USB_MemoryToPMA(USB_BTABLE_ENTRIES[2].ADDR_TX, USBCDC_TXBufferRead,
                packet_length);
            USBCDC_TXBufferRead += packet_length;
        }
        else
        {
            // The rest of the buffer until the end of the ring buffer will fit
            // into the PMA buffer, so we can transmit everything and reset the
            // read pointer to the ring buffer's beginning.
            packet_length = until_wraparound;
            USB_MemoryToPMA(USB_BTABLE_ENTRIES[2].ADDR_TX, USBCDC_TXBufferRead,
                packet_length);
            USBCDC_TXBufferRead = USBCDC_TXBuffer;
        }
    }
    else if(ring_buffer_bytes > 0)
    {
        // This /could/ have been an `if` instead of an `else if` to fill up
        // the PMA buffer completely if the ring buffer contents wrap around and
        // are larger than the PMA area if both parts are combined. However,
        // the PMA area only supports 16-bit accesses and an unaligned read
        // pointer would lead to problems at the wraparound boundary unless
        // specially handeled. Just sending two packets here is way simpler
        // than adding handling for all border cases.

        if(ring_buffer_bytes > USBCDC_ENDPOINT_SIZE)
        {
            packet_length = USBCDC_ENDPOINT_SIZE;
        }
        else
        {
            packet_length = ring_buffer_bytes;
        }

        USB_MemoryToPMA(USB_BTABLE_ENTRIES[2].ADDR_TX, USBCDC_TXBufferRead,
            packet_length);
        USBCDC_TXBufferRead += packet_length;
    }

    if(packet_length != 0)
    {
        USBCDC_TXTransferring = true;

        // Mark the in packet, which is now stored in PMA memory, as valid
        USB_BTABLE_ENTRIES[2].COUNT_TX = packet_length;
        USB_SetEPTXStatus(&(USB->EP2R), USB_EP_TX_VALID);
    }
}

void USBCDC_SendData(const void *data, int length)
{
    while(length > 0)
    {
        // Block until ring buffer is no longer full
        int ring_buffer_space = 0;
        while(ring_buffer_space == 0)
        {
            // The ring buffer is empty when read and write pointer are the
            // same. It is considered full when the write pointer is one spot
            // before the read pointer. One index is thus unused to make the
            // distinction between empty and full possible.
            ring_buffer_space = USBCDC_TXBufferRead - USBCDC_TXBufferWrite - 1;
            if(ring_buffer_space < 0)
            {
                ring_buffer_space += CONFIG_USB_RINGBUFFER_SIZE;
            }
        }

        // If more data is supplied than would fit in the ring buffer, copy as
        // much as possible in this iteration.
        int store_length = ring_buffer_space;
        if(length < store_length)
        {
            store_length = length;
        }
        length -= store_length;

        // If copying all data into the ring buffer needs a write pointer
        // wraparound, copy the part before the ring buffer 'end' first
        int until_wraparound = USBCDC_TXBuffer + CONFIG_USB_RINGBUFFER_SIZE
            - USBCDC_TXBufferWrite;
        if(store_length >= until_wraparound)
        {
            memcpy(USBCDC_TXBufferWrite, data, until_wraparound);
            USBCDC_TXBufferWrite = USBCDC_TXBuffer;
            store_length -= until_wraparound;
            data += until_wraparound;
        }

        memcpy(USBCDC_TXBufferWrite, data, store_length);
        USBCDC_TXBufferWrite += store_length;
        data += store_length;

        // If there isn't already data being transferred, the process has to be
        // kicked off. The later USB packets will be built from the USB transfer
        // complete interrupts.
        if(!USBCDC_TXTransferring)
        {
            USBCDC_BuildInPacket();
        }
    }
}

void USBCDC_HandleDataInComplete(void)
{
    if(USBCDC_TXBufferRead == USBCDC_TXBufferWrite)
    {
        USBCDC_TXTransferring = false;
    }
    else
    {
        USBCDC_BuildInPacket();
    }
}