#include <stdio.h>
#include <ctype.h>
#include <stdlib.h>

#include "em_msc.h"
#include "em_cmu.h"
#include "em_usb.h"
#include "em_chip.h"
#include "em_core.h"
#include "em_usart.h"
#include "em_device.h"

#include "descriptors.h"

/* USB CDC definitions */

#define CDC_BULK_EP_SIZE    USB_FS_BULK_EP_MAXSIZE
#define CDC_USB_RX_BUF_SIZ  CDC_BULK_EP_SIZE
#define CDC_USB_TX_BUF_SIZ  32
#define MSC_BUF_SIZ         1024

STATIC_UBUF(usbRxBuffer, CDC_USB_RX_BUF_SIZ);
STATIC_UBUF(mscBuffer,   MSC_BUF_SIZ);

static volatile int glo_xferred;

/* USB CDC function prototypes */

static int UsbDataReceived(USB_Status_TypeDef status, uint32_t xferred, uint32_t remaining);

/* Debug messages */

char serialOutBuffer[127];

void serialOut(char *message) {
    for (uint32_t i = 0; i < strlen(message); i++ ) {
        USART_Tx(USART1, message[i]);
        /* Wait until TXC is set to 1 (last character finished sending). */
        while (!(USART1->IF & 0x1));
    }
}

/* USB CDC functions */

static int CDC_SetupCmd(const USB_Setup_TypeDef *setup) {
    return USB_STATUS_REQ_UNHANDLED;
}

static int UsbDataReceived(USB_Status_TypeDef status, uint32_t xferred, uint32_t remaining) {
    glo_xferred = xferred;
    if ((status == USB_STATUS_OK) && (xferred > 0)) {
        USBD_Read(CDC_EP_DATA_OUT, (void*)usbRxBuffer, CDC_USB_RX_BUF_SIZ, UsbDataReceived);
    }
    return USB_STATUS_OK;
}

static void CDC_StateChangeEvent(USBD_State_TypeDef oldState, USBD_State_TypeDef newState) {
    if (newState == USBD_STATE_CONFIGURED) {
        USBD_Read(CDC_EP_DATA_OUT, (void*)usbRxBuffer, CDC_USB_RX_BUF_SIZ, UsbDataReceived);
    }
}

/* USB callbacks */

static const USBD_Callbacks_TypeDef callbacks = {
    .usbReset        = NULL,
    .usbStateChange  = CDC_StateChangeEvent,
    .setupCmd        = CDC_SetupCmd,
    .isSelfPowered   = NULL,
    .sofInt          = NULL
};

static const USBD_Init_TypeDef usbInitStruct = {
    .deviceDescriptor    = &USBDESC_deviceDesc,
    .configDescriptor    = USBDESC_configDesc,
    .stringDescriptors   = USBDESC_strings,
    .numberOfStrings     = sizeof(USBDESC_strings) / sizeof(void*),
    .callbacks           = &callbacks,
    .bufferingMultiplier = USBDESC_bufferingMultiplier,
    .reserved            = 0
};

#define BASE_ADDR      0xE000
#define USB_BLOCK_SIZE 32

typedef int (*fptr) (void);

typedef enum {
    SYS_LED_0,
    SYS_LED_1
} SystemCall;

SL_RAMFUNC_DEFINITION_BEGIN
static bool writeFlashPage(uint32_t* dest, uint32_t* src) {
    MSC_Status_TypeDef status;
    CORE_DECLARE_IRQ_STATE;
    CORE_ENTER_ATOMIC();
    MSC->LOCK = MSC_UNLOCK_CODE;
    status = MSC_ErasePage(dest);
    if (status == mscReturnOk) {
        status = MSC_WriteWord(dest, src, FLASH_PAGE_SIZE);
    }
    MSC->LOCK = 0;
    CORE_EXIT_ATOMIC();
    return (status == mscReturnOk);
}
SL_RAMFUNC_DEFINITION_END

void readBinary(uint32_t size, uint32_t* addr) {
    glo_xferred = 0;
    memset(mscBuffer, 0, sizeof(mscBuffer));
    int msc_offset = 0;
    while (1) {
        if (glo_xferred > 0) {
            if (strncmp((char*)usbRxBuffer, "end_binary", 10) == 0) return;

            // Serial out.
            memset(serialOutBuffer, 0, sizeof(serialOutBuffer));
            for (int i = 0; i < glo_xferred; i++) {
                serialOutBuffer[i] = usbRxBuffer[i];
                if (isprint(serialOutBuffer[i]) == 0) {
                    serialOutBuffer[i] = '.';
                }
            }
            serialOut(serialOutBuffer);

            // Msc.
            for (int i = glo_xferred; i < USB_BLOCK_SIZE; i++) usbRxBuffer[i] = 0;
            memcpy(mscBuffer + msc_offset, usbRxBuffer, USB_BLOCK_SIZE);
            msc_offset += 32;
            if (msc_offset == MSC_BUF_SIZ) {
                writeFlashPage(addr, (uint32_t*)mscBuffer);
                addr += (FLASH_PAGE_SIZE / 4);
                msc_offset = 0;
            }
        }
    }
}

int executeBinary(fptr f_ptr) {
    return (*f_ptr)();
}

void HardFault_Handler(void) {
    memset(serialOutBuffer, 0, sizeof(serialOutBuffer));
    sprintf(serialOutBuffer, "Hard fault at address ");
    serialOut(serialOutBuffer);

    /* Fetch which of MSP or PSP is being used into r0. */
    __asm__ volatile (
        /* PSP never used currently.
            "TST lr, #4\n"
            "ITE NEQ\n"
            "MRS r0, PSP\n"
        */
        "mrs r0, MSP\n"
    );

    /* Fetch program counter of the failed instruction into addr. */
    register int32_t* addr __asm__("r3");
    __asm__ volatile ("ldr r3, [r0, #0x18]\n");

    memset(serialOutBuffer, 0, sizeof(serialOutBuffer));
    sprintf(serialOutBuffer, "%p\n", addr);
    serialOut(serialOutBuffer);

    /* Force CPU into debug mode. */
    __asm__ volatile ("bkpt #1");
}

void SVC_Handler(void) {
    memset(serialOutBuffer, 0, sizeof(serialOutBuffer));
    sprintf(serialOutBuffer, "Supervisor call.\n");
    serialOut(serialOutBuffer);

    __asm__ volatile (
        "mrs r0, MSP\n"
    );

    register int32_t msp __asm__("r1");
    SystemCall op;


    switch(op) {
        case SYS_LED_0:
            GPIO_PinModeSet(gpioPortF, 4, gpioModePushPull, 1);
            break;
        case SYS_LED_1:
            GPIO_PinModeSet(gpioPortF, 5, gpioModePushPull, 1);
            break;
    }
}

void usbLoop() {
    int counter = 0;
    while (1) {
        if (glo_xferred > 0) {
            counter += 1;
            if (strncmp((char*)usbRxBuffer, "binary", 6) == 0) {
                char* end;
                uint32_t size        = strtol((char*)usbRxBuffer + 7, &end, 10);
                uint32_t page_offset = strtol(end + 1, &end, 10);
                uint32_t *addr       = (uint32_t*)(BASE_ADDR + FLASH_PAGE_SIZE * page_offset);

                sprintf(serialOutBuffer, "SIZE %lu ADDR %p\n", size, addr);
                serialOut(serialOutBuffer);

                readBinary(size, addr);
            }
            else {
                if (strncmp((char*)usbRxBuffer, "execute", 7) == 0) {
                    char* end;
                    uint32_t exe_id = strtol((char*)usbRxBuffer + 8, &end, 10);
                    // sprintf(serialOutBuffer, "%lu\n", exe_id);
                    // serialOut(serialOutBuffer);
                    uint32_t* _reset_ptr = (uint32_t*)(BASE_ADDR + exe_id * FLASH_PAGE_SIZE);
                    fptr f_ptr = (fptr)(*_reset_ptr);
                    executeBinary(f_ptr);
                } else {
                    sprintf(serialOutBuffer, "%s\n", usbRxBuffer);
                    serialOut(serialOutBuffer);
                }
            }
            glo_xferred = 0;
        }
    }
}


int main(void) {

    CHIP_Init();

    /* Enable dHFXO and high frequency peripherals */
    CMU_ClockSelectSet(cmuClock_HF, cmuSelect_HFXO);
    CMU_ClockEnable(cmuClock_HFPER, true);

    /* Enabled GPIO and USART1 */
    CMU_ClockEnable(cmuClock_GPIO, true);
    CMU_ClockEnable(cmuClock_USART1, true);

    /* Set enable and Tx pin high */
    GPIO_PinModeSet(gpioPortA, 9, gpioModePushPull, 1);
    GPIO_PinModeSet(gpioPortF, 2, gpioModePushPull, 1);

    /* Set up interface */
    USART_InitAsync_TypeDef init = USART_INITASYNC_DEFAULT;
    USART_InitAsync(USART1, &init);
    USART1->ROUTE |= USART_ROUTE_TXPEN | USART_ROUTE_LOCATION_LOC4;

    /* Initialise MSC. */
    MSC_Init();

    /* Initialise USB */
    USBD_Init(&usbInitStruct);

    uint8_t test[128];
    test[0] = 0x12;
    test[1] = 0x34;
    test[3] = 0x56;
    test[4] = 0x78;
    writeFlashPage(0xe000, test);

    /* Main loop */
    usbLoop();
}