#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_device.h"

#include "descriptors.h"
#include "serial.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);

/* 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) {
    /* Get the stack pointer. */
    uint32_t *sp;
    __asm__ volatile ("mrs %0, msp\n" : "=rm" (sp));
    /* Stack contains:
     *   0 - Stacked r0.
     *   1 - Stacked r1.
     *   2 - Stacked r2.
     *   3 - Stacked r3.
     *   4 - Stacked r12.
     *   5 - Stacked lr.
     *   6 - Stacked pc.
     *   7 - Stacked xPSR.
     */
    uint16_t *addr = sp[6];
    uint16_t instr = *addr;

    memset(serialOutBuffer, 0, sizeof(serialOutBuffer));
    sprintf(serialOutBuffer, "Hardfault encountered at address %p, instruction %x.\n", addr, instr);
    serialOut(serialOutBuffer);

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

void SVC_Handler(void) {
    /* Get the stack pointer. */
    uint32_t *sp;
    __asm__ volatile ("mrs %0, msp\n" : "=rm" (sp));
    /* Stack contains:
     *   0 - Stacked r0.
     *   1 - Stacked r1.
     *   2 - Stacked r2.
     *   3 - Stacked r3.
     *   4 - Stacked r12.
     *   5 - Stacked lr.
     *   6 - Stacked pc.
     *   7 - Stacked xPSR.
     */
    /* Get SVC number from first byte of instruction before the stacked PC. */
    uint8_t svc_number = ((uint8_t*)(sp[6]))[-2];

    messageOut("System call.\n");

    switch(svc_number) {
        case SYS_LED_0:
            GPIO_PinModeSet(gpioPortF, 4, gpioModePushPull, 1);
            break;

        case SYS_LED_1:
            GPIO_PinModeSet(gpioPortF, 5, gpioModePushPull, 1);
            break;

        default:

            messageOut("Unrecognized system call.\n");
            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 testProgram[] = {
        /* Address at which to jump. */
        0x04,
        0xe0,
        0x00,
        0x00,
        /* svc. */
        0x00,
        0xdf,
        /* bx lr. */
        0x70,
        0x47
    };
    writeFlashPage(0xe000, testProgram);

    /* Main loop */
    usbLoop();
}