#include <errno.h>
#include <malloc.h>
#include <sys/stat.h>
#include <unistd.h>

/* Board */
#include "pin_mux.h"

/* SDK drivers */
#include "fsl_usart.h"

/* FreeRTOS */
#include "FreeRTOS.h"
#include "event_groups.h"
#include "semphr.h"
#include "task.h"

#define APP_STDIO_UART_ID           0
#define APP_STDIO_UART_INSTANCE     USART0
#define APP_STDIO_UART_CLOCK_ATTACH kFRO12M_to_FLEXCOMM0
#define APP_STDIO_UART_TX_FIFO_SIZE 16
#define APP_STDIO_UART_RX_FIFO_SIZE 16
#define APP_STDIO_UART_TX_BEAT_SIZE (APP_STDIO_UART_TX_FIFO_SIZE - 1)
#define APP_STDIO_UART_RX_BEAT_SIZE (APP_STDIO_UART_RX_FIFO_SIZE - 4) /* Save room for delayed reception */

#define APP_STDIO_UART_IRQ_NR      FLEXCOMM0_IRQn
#define APP_STDIO_UART_IRQ_PRIO    (configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY + 1)
#define APP_STDIO_UART_IRQ_HANDLER FLEXCOMM0_IRQHandler

#define APP_STDIO_UART_EVENT_TX_DONE (1 << 0U)
#define APP_STDIO_UART_EVENT_RX_DONE (1 << 1U)
#define APP_STDIO_UART_EVENT_ERROR   (1 << 2U)

static SemaphoreHandle_t s_malloc_lock    = NULL;

static SemaphoreHandle_t  s_stdio_tx_lock = NULL;
static SemaphoreHandle_t  s_stdio_rx_lock = NULL;
static EventGroupHandle_t s_stdio_event   = NULL;

/* ========== Internal Use Functions ========== */
static int app_stdio_init(void) {
    int ret         = 0;

    s_stdio_tx_lock = xSemaphoreCreateMutex();
    if (s_stdio_tx_lock == NULL) {
        return -1;
    }

    s_stdio_rx_lock = xSemaphoreCreateMutex();
    if (s_stdio_rx_lock == NULL) {
        ret = -1;
        goto free_tx_sem_exit;
    }

    s_stdio_event = xEventGroupCreate();
    if (s_stdio_event == NULL) {
        ret = -1;
        goto free_rx_sem_exit;
    }

    CLOCK_AttachClk(APP_STDIO_UART_CLOCK_ATTACH);

    usart_config_t usart_config;
    USART_GetDefaultConfig(&usart_config);

    usart_config.baudRate_Bps    = 115200U;
    usart_config.txWatermark     = 0U;
    usart_config.rxWatermark     = APP_STDIO_UART_RX_BEAT_SIZE;
    usart_config.enableTx        = true;
    usart_config.enableRx        = true;
    usart_config.bitCountPerChar = kUSART_8BitsPerChar;
    usart_config.stopBitCount    = kUSART_OneStopBit;
    usart_config.parityMode      = kUSART_ParityDisabled;

    uint32_t stdio_fc_clk_freq   = CLOCK_GetFlexCommClkFreq(APP_STDIO_UART_ID);
    if (USART_Init(APP_STDIO_UART_INSTANCE, &usart_config, stdio_fc_clk_freq) != kStatus_Success) {
        ret = -1;
        goto free_event_exit;
    }

    USART_SetTxFifoWatermark(APP_STDIO_UART_INSTANCE, 0); /* Has to be set to 0 to detect TX done condition */
    USART_SetRxFifoWatermark(APP_STDIO_UART_INSTANCE, APP_STDIO_UART_RX_BEAT_SIZE); /* Might be adjusted on-the-fly */

    NVIC_SetPriority(APP_STDIO_UART_IRQ_NR, APP_STDIO_UART_IRQ_PRIO);
    EnableIRQ(APP_STDIO_UART_IRQ_NR);

    return ret;

free_event_exit:
    vEventGroupDelete(s_stdio_event);

free_rx_sem_exit:
    vSemaphoreDelete(s_stdio_rx_lock);

free_tx_sem_exit:
    vSemaphoreDelete(s_stdio_tx_lock);

    return ret;
}

void APP_STDIO_UART_IRQ_HANDLER(void) {
    uint32_t event_flag         = 0UL;

    uint32_t interrupts_enabled = USART_GetEnabledInterrupts(APP_STDIO_UART_INSTANCE);
    uint32_t fifo_int_status    = APP_STDIO_UART_INSTANCE->FIFOINTSTAT;

    USART_ClearStatusFlags(APP_STDIO_UART_INSTANCE, kUSART_AllClearFlags);

    if ((interrupts_enabled & kUSART_TxLevelInterruptEnable) && (fifo_int_status & USART_FIFOINTSTAT_TXLVL_MASK)) {
        USART_DisableInterrupts(APP_STDIO_UART_INSTANCE, kUSART_TxLevelInterruptEnable);
        event_flag |= APP_STDIO_UART_EVENT_TX_DONE;
    }

    if ((interrupts_enabled & kUSART_RxLevelInterruptEnable) && (fifo_int_status & USART_FIFOINTSTAT_RXLVL_MASK)) {
        USART_DisableInterrupts(APP_STDIO_UART_INSTANCE, kUSART_RxLevelInterruptEnable);
        event_flag |= APP_STDIO_UART_EVENT_RX_DONE;
    }

    BaseType_t higher_prio_task_woken = pdFALSE;

    xEventGroupSetBitsFromISR(s_stdio_event, event_flag, &higher_prio_task_woken);
    portYIELD_FROM_ISR(higher_prio_task_woken);
}

int app_syscalls_init(void) {
    s_malloc_lock = xSemaphoreCreateRecursiveMutex();
    if (s_malloc_lock == NULL) {
        return -1;
    }

    if (app_stdio_init() != 0) {
        return -2;
    }

    return 0;
}

void __malloc_lock(struct _reent *reent) {
    xSemaphoreTakeRecursive(s_malloc_lock, portMAX_DELAY);
}

void __malloc_unlock(struct _reent *reent) {
    xSemaphoreGiveRecursive(s_malloc_lock);
}

int _read(int file, char *ptr, int len) {
    if (file != STDIN_FILENO) {
        return 0;
    }

    xSemaphoreTake(s_stdio_rx_lock, portMAX_DELAY);

    USART_SetRxFifoWatermark(APP_STDIO_UART_INSTANCE, 0);
    USART_EnableInterrupts(APP_STDIO_UART_INSTANCE, kUSART_RxLevelInterruptEnable);

    xEventGroupWaitBits(s_stdio_event, APP_STDIO_UART_EVENT_RX_DONE, pdTRUE, pdFALSE, portMAX_DELAY);

    uint8_t rx_level = USART_GetRxFifoCount(APP_STDIO_UART_INSTANCE);

    if (rx_level > len) {
        rx_level = len;
    }

    for (uint8_t i = 0; i < rx_level; i++) {
        ptr[i] = USART_ReadByte(APP_STDIO_UART_INSTANCE);
    }

    xSemaphoreGive(s_stdio_rx_lock);
    return rx_level;
}

int _write(int file, char *buf, int len) {
    if (file != STDOUT_FILENO && file != STDERR_FILENO) {
        return 0;
    }

    xSemaphoreTake(s_stdio_tx_lock, portMAX_DELAY);

    int xfer_count      = len / APP_STDIO_UART_TX_BEAT_SIZE;
    int bytes_remaining = len % APP_STDIO_UART_TX_BEAT_SIZE;
    if (bytes_remaining) {
        xfer_count++;
    }

    for (int i = 0; i < xfer_count; i++) {
        uint32_t tx_bytes = APP_STDIO_UART_TX_BEAT_SIZE;

        if (i == xfer_count - 1) {
            if (bytes_remaining) {
                tx_bytes = bytes_remaining;
            }
        }

        /* Hold TX operation before enabling level interrupt, otherwise a level interrupt will fire instantly */
        APP_STDIO_UART_INSTANCE->CTL |= USART_CTL_TXDIS_MASK;

        /* Fill FIFO */
        for (uint32_t j = 0; j < tx_bytes; j++) {
            USART_WriteByte(APP_STDIO_UART_INSTANCE, buf[i * APP_STDIO_UART_TX_BEAT_SIZE + j]);
        }

        USART_EnableInterrupts(APP_STDIO_UART_INSTANCE, kUSART_TxLevelInterruptEnable);

        /* Begin TX transfer */
        APP_STDIO_UART_INSTANCE->CTL &= ~(USART_CTL_TXDIS_MASK);

        /* Wait for TX complete */
        xEventGroupWaitBits(s_stdio_event, APP_STDIO_UART_EVENT_TX_DONE, pdTRUE, pdFALSE, portMAX_DELAY);
    }

    xSemaphoreGive(s_stdio_tx_lock);

    return len;
}

int _open(const char *path, int flags, ...) {
    return -1;
}

int _close(int file) {
    errno = -EBADF;
    return -1;
}

int _fstat(int file, struct stat *st) {
    if ((STDOUT_FILENO == file) || (STDERR_FILENO == file)) {
        st->st_mode = S_IFCHR;
        return 0;
    } else {
        errno = EBADF;
        return -1;
    }
}

int _getpid() {
    return 1;
}

int _isatty(int file) {
    return 1;
}

int _kill(int pid, int sig) {
    errno = -EINVAL;

    return -1;
}

int _lseek(int file, int offset, int whence) {
    return 0;
}