/**
 * @file stm32f4xx_hal.c
 * @brief STM32F4 Hardware Abstraction Layer
 */

#include "stm32f4xx_config.h"
#include "can_driver.h"
#include "uart_driver.h"
#include "spi_driver.h"
#include "i2c_driver.h"
#include "gpio_driver.h"
#include "adc_driver.h"
#include "pwm_driver.h"
#include "stm32f4xx.h"

/* CAN HAL Implementation */
int hal_can_init(uint32_t baudrate, uint8_t frame_type, bool enable_fd) {
    /* Enable CAN clock */
    RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;

    /* Configure CAN GPIO */
    // PB8 - CAN1_RX, PB9 - CAN1_TX
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;

    GPIOB->MODER |= (GPIO_MODER_MODER8_1 | GPIO_MODER_MODER9_1);
    GPIOB->OTYPER &= ~(GPIO_OTYPER_OT_8 | GPIO_OTYPER_OT_9);
    GPIOB->OSPEEDR |= (GPIO_OSPEEDER_OSPEEDR8 | GPIO_OSPEEDER_OSPEEDR9);
    GPIOB->AFR[1] |= (9 << 0) | (9 << 4);  /* AF9 for CAN */

    /* Reset CAN */
    CAN1->MCR |= CAN_MCR_RESET;
    CAN1->MCR &= ~CAN_MCR_RESET;

    /* Exit sleep mode */
    CAN1->MCR &= ~CAN_MCR_SLEEP;

    /* Set baudrate */
    uint32_t prescaler = 0;
    uint32_t time_quantum = 0;

    switch (baudrate) {
        case 125000:
            prescaler = 21;
            time_quantum = 16;
            break;
        case 250000:
            prescaler = 11;
            time_quantum = 15;
            break;
        case 500000:
            prescaler = 5;
            time_quantum = 16;
            break;
        case 1000000:
            prescaler = 3;
            time_quantum = 14;
            break;
        default:
            return -1;
    }

    CAN1->BTR = ((prescaler - 1) << 20) |
                ((time_quantum - 1) << 16) |
                (3 << 20) |  /* SJW = 4 */
                (7 << 16);   /* BS1 = 8 */

    /* Configure filters */
    CAN1->FMR |= CAN_FMR_FINIT;
    CAN1->FM1R &= ~CAN_FM1R_FBM0;  /* Mask mode for filter 0 */
    CAN1->FS1R |= CAN_FS1R_FSC0;   /* 32-bit scale */
    CAN1->FFA1R &= ~CAN_FFA1R_FFA0; /* FIFO 0 */
    CAN1->FMR &= ~CAN_FMR_FINIT;

    /* Enable interrupts */
    CAN1->IER |= CAN_IER_FMPIE0 |  /* FIFO 0 message pending */
                 CAN_IER_TMEIE |   /* Transmit mailbox empty */
                 CAN_IER_BOFIE |   /* Bus-off */
                 CAN_IER_ERRIE;    /* Error */

    /* Normal mode */
    CAN1->MCR &= ~CAN_MCR_SLEEP;

    return 0;
}

int hal_can_send_message(const CanMessage_t* message, uint32_t* mailbox) {
    /* Check for free mailbox */
    if ((CAN1->TSR & CAN_TSR_TME0) != 0) {
        *mailbox = 0;
    } else if ((CAN1->TSR & CAN_TSR_TME1) != 0) {
        *mailbox = 1;
    } else if ((CAN1->TSR & CAN_TSR_TME2) != 0) {
        *mailbox = 2;
    } else {
        return -1;
    }

    /* Configure mailbox */
    CAN_TxMailBox_TypeDef* tx_mailbox = &CAN1->sTxMailBox[*mailbox];

    /* Set ID */
    if (message->id.is_extended) {
        tx_mailbox->TIR = (message->id.id << 3) | CAN_TI0R_IDE;
    } else {
        tx_mailbox->TIR = (message->id.id << 21);
    }

    /* Set data length and data */
    tx_mailbox->TDTR = message->length;

    /* Copy data */
    uint32_t data[2] = {0, 0};
    for (int i = 0; i < message->length; i++) {
        if (i < 4) {
            data[0] |= (message->data[i] << (i * 8));
        } else {
            data[1] |= (message->data[i] << ((i - 4) * 8));
        }
    }

    tx_mailbox->TDLR = data[0];
    tx_mailbox->TDHR = data[1];

    /* Request transmission */
    tx_mailbox->TIR |= CAN_TI0R_TXRQ;

    return 0;
}

int hal_can_receive_message(CanMessage_t* message) {
    /* Check if message available */
    if ((CAN1->RF0R & CAN_RF0R_FMP0) == 0) {
        return -1;
    }

    /* Get message */
    CAN_FIFOMailBox_TypeDef* rx_mailbox = &CAN1->sFIFOMailBox[0];

    /* Get ID */
    if (rx_mailbox->RIR & CAN_RI0R_IDE) {
        message->id.is_extended = true;
        message->id.id = (rx_mailbox->RIR >> 3) & 0x1FFFFFFF;
    } else {
        message->id.is_extended = false;
        message->id.id = (rx_mailbox->RIR >> 21) & 0x7FF;
    }

    /* Get data length */
    message->length = rx_mailbox->RDTR & CAN_RDT0R_DLC;

    /* Get data */
    uint32_t data_low = rx_mailbox->RDLR;
    uint32_t data_high = rx_mailbox->RDHR;

    for (int i = 0; i < message->length; i++) {
        if (i < 4) {
            message->data[i] = (data_low >> (i * 8)) & 0xFF;
        } else {
            message->data[i] = (data_high >> ((i - 4) * 8)) & 0xFF;
        }
    }

    /* Release FIFO */
    CAN1->RF0R |= CAN_RF0R_RFOM0;

    return 0;
}

/* GPIO HAL Implementation */
void hal_gpio_init(uint8_t port, uint8_t pin, GpioMode_t mode) {
    GPIO_TypeDef* gpio_port = get_gpio_port(port);

    if (gpio_port == NULL) {
        return;
    }

    /* Enable GPIO clock */
    RCC->AHB1ENR |= (1 << port);

    /* Configure mode */
    uint32_t moder_value = 0;
    switch (mode) {
        case GPIO_MODE_INPUT:
            moder_value = 0x00;
            break;
        case GPIO_MODE_OUTPUT:
            moder_value = 0x01;
            break;
        case GPIO_MODE_ALTERNATE:
            moder_value = 0x02;
            break;
        case GPIO_MODE_ANALOG:
            moder_value = 0x03;
            break;
    }

    gpio_port->MODER &= ~(0x03 << (pin * 2));
    gpio_port->MODER |= (moder_value << (pin * 2));
}

void hal_gpio_write(uint8_t port, uint8_t pin, bool value) {
    GPIO_TypeDef* gpio_port = get_gpio_port(port);

    if (gpio_port == NULL) {
        return;
    }

    if (value) {
        gpio_port->BSRR = (1 << pin);
    } else {
        gpio_port->BSRR = (1 << (pin + 16));
    }
}

bool hal_gpio_read(uint8_t port, uint8_t pin) {
    GPIO_TypeDef* gpio_port = get_gpio_port(port);

    if (gpio_port == NULL) {
        return false;
    }

    return (gpio_port->IDR & (1 << pin)) != 0;
}

/* UART HAL Implementation */
int hal_uart_init(uint8_t instance, UartConfig_t* config) {
    USART_TypeDef* uart = get_uart_instance(instance);

    if (uart == NULL) {
        return -1;
    }

    /* Enable clock */
    if (instance == 0) {
        RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
    } else {
        RCC->APB1ENR |= (RCC_APB1ENR_USART2EN << (instance - 1));
    }

    /* Configure baudrate */
    uint32_t clock = (instance == 0 || instance == 5) ? APB2_CLOCK : APB1_CLOCK;
    uart->BRR = clock / config->baudrate;

    /* Configure control registers */
    uart->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_UE;

    if (config->data_bits == UART_DATA_BITS_9) {
        uart->CR1 |= USART_CR1_M;
    }

    uart->CR2 = 0;
    if (config->stop_bits == UART_STOP_BITS_2) {
        uart->CR2 |= USART_CR2_STOP_1;
    }

    /* Enable interrupts */
    if (config->enable_rx) {
        uart->CR1 |= USART_CR1_RXNEIE;
    }

    return 0;
}

/* Helper functions */
static GPIO_TypeDef* get_gpio_port(uint8_t port) {
    switch (port) {
        case 0: return GPIOA;
        case 1: return GPIOB;
        case 2: return GPIOC;
        case 3: return GPIOD;
        case 4: return GPIOE;
        case 5: return GPIOF;
        case 6: return GPIOG;
        case 7: return GPIOH;
        default: return NULL;
    }
}

static USART_TypeDef* get_uart_instance(uint8_t instance) {
    switch (instance) {
        case 0: return USART1;
        case 1: return USART2;
        case 2: return USART3;
        case 3: return UART4;
        case 4: return UART5;
        case 5: return USART6;
        default: return NULL;
    }
}