/**
 * @file s32k14x_hal.c
 * @brief NXP S32K14x Hardware Abstraction Layer
 */

#include "s32k14x_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 "S32K144.h"

/* CAN HAL Implementation */
int hal_can_init(uint32_t baudrate, uint8_t frame_type, bool enable_fd) {
    /* Enable FLEXCAN0 clock */
    PCC->PCCn[PCC_FlexCAN0_INDEX] |= PCC_PCCn_CGC_MASK;

    /* Configure CAN pins */
    // PTE4 - CAN0_RX, PTE5 - CAN0_TX
    PCC->PCCn[PCC_PORTE_INDEX] |= PCC_PCCn_CGC_MASK;
    PORTE->PCR[4] = PORT_PCR_MUX(5);  /* CAN0_RX */
    PORTE->PCR[5] = PORT_PCR_MUX(5);  /* CAN0_TX */

    /* Reset FLEXCAN */
    CAN0->MCR |= CAN_MCR_SOFTRST_MASK;
    while (CAN0->MCR & CAN_MCR_SOFTRST_MASK);

    /* Configure for CAN FD if enabled */
    if (enable_fd) {
        CAN0->MCR |= CAN_MCR_FDEN_MASK;  /* Enable FD */
    }

    /* Set baudrate */
    uint32_t prescaler = BUS_FREQUENCY / (baudrate * 10);  /* 10 time quanta */
    CAN0->CTRL1 = CAN_CTRL1_PRESDIV(prescaler - 1) |
                  CAN_CTRL1_PSEG1(3) |
                  CAN_CTRL1_PSEG2(2) |
                  CAN_CTRL1_PROPSEG(4);

    /* Configure message buffers */
    CAN0->RXMGMASK = 0x1FFFFFFF;  /* Accept all IDs */
    CAN0->RX14MASK = 0x1FFFFFFF;
    CAN0->RX15MASK = 0x1FFFFFFF;

    /* Enable interrupts */
    CAN0->IMASK1 |= CAN_IMASK1_BUF31TO0M_MASK;
    CAN0->MCR |= CAN_MCR_IRMQ_MASK;  /* Individual RX masking */

    /* Normal mode */
    CAN0->MCR &= ~CAN_MCR_HALT_MASK;
    while (CAN0->MCR & CAN_MCR_FRZACK_MASK);

    return 0;
}

int hal_can_send_message(const CanMessage_t* message, uint32_t* mailbox) {
    /* Find free message buffer */
    *mailbox = 0;
    while (*mailbox < 32) {
        if ((CAN0->IFLAG1 & (1 << *mailbox)) != 0) {
            break;
        }
        (*mailbox)++;
    }

    if (*mailbox >= 32) {
        return -1;
    }

    /* Configure message buffer */
    CAN0->RAMn[*mailbox * 4 + 1] = (message->id.id << 18) |
                                    (message->id.is_extended ? 1 << 29 : 0) |
                                    (message->length << 16);

    /* Copy data */
    for (int i = 0; i < message->length; i += 4) {
        uint32_t data = 0;
        for (int j = 0; j < 4 && (i + j) < message->length; j++) {
            data |= (message->data[i + j] << (j * 8));
        }
        CAN0->RAMn[*mailbox * 4 + 2 + (i / 4)] = data;
    }

    /* Enable transmission */
    CAN0->RAMn[*mailbox * 4] = CAN_WORD0_IDE_MASK |
                               CAN_WORD0_SRR_MASK |
                               CAN_WORD0_ESI_MASK |
                               CAN_WORD0_CODE(0xC);  /* TX data */

    return 0;
}

int hal_can_receive_message(CanMessage_t* message) {
    /* Check for received messages */
    uint32_t iflag = CAN0->IFLAG1;
    if (iflag == 0) {
        return -1;
    }

    /* Find received message buffer */
    uint32_t mailbox = 0;
    while (mailbox < 32) {
        if (iflag & (1 << mailbox)) {
            break;
        }
        mailbox++;
    }

    if (mailbox >= 32) {
        return -1;
    }

    /* Read message */
    uint32_t word0 = CAN0->RAMn[mailbox * 4];
    uint32_t word1 = CAN0->RAMn[mailbox * 4 + 1];

    /* Check if RX buffer */
    if ((word0 & CAN_WORD0_CODE_MASK) != CAN_WORD0_CODE(0x4)) {
        CAN0->IFLAG1 = (1 << mailbox);  /* Clear flag */
        return -1;
    }

    /* Get ID */
    message->id.is_extended = (word0 & CAN_WORD0_IDE_MASK) != 0;
    if (message->id.is_extended) {
        message->id.id = (word0 & CAN_WORD0_ID_MASK) >> 0;
    } else {
        message->id.id = (word0 & CAN_WORD0_ID_MASK) >> 18;
    }

    /* Get data length */
    message->length = (word1 & CAN_WORD1_DLC_MASK) >> 16;

    /* Get data */
    for (int i = 0; i < message->length; i += 4) {
        uint32_t data = CAN0->RAMn[mailbox * 4 + 2 + (i / 4)];
        for (int j = 0; j < 4 && (i + j) < message->length; j++) {
            message->data[i + j] = (data >> (j * 8)) & 0xFF;
        }
    }

    /* Clear flag */
    CAN0->IFLAG1 = (1 << mailbox);

    return 0;
}

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

    if (gpio_port == NULL || port_config == NULL) {
        return;
    }

    /* Enable clock */
    PCC->PCCn[PCC_PORTA_INDEX + port] |= PCC_PCCn_CGC_MASK;
    PCC->PCCn[PCC_GPIOA_INDEX + port] |= PCC_PCCn_CGC_MASK;

    /* Configure pin mux */
    switch (mode) {
        case GPIO_MODE_INPUT:
            port_config->PCR[pin] = PORT_PCR_MUX(1);
            gpio_port->PDDR &= ~(1 << pin);
            break;
        case GPIO_MODE_OUTPUT:
            port_config->PCR[pin] = PORT_PCR_MUX(1);
            gpio_port->PDDR |= (1 << pin);
            break;
        default:
            break;
    }
}

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

    if (gpio_port == NULL) {
        return;
    }

    if (value) {
        gpio_port->PSOR = (1 << pin);
    } else {
        gpio_port->PCOR = (1 << pin);
    }
}

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

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

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

/* Helper functions */
static GPIO_Type* get_gpio_port(uint8_t port) {
    switch (port) {
        case 0: return PTA;
        case 1: return PTB;
        case 2: return PTC;
        case 3: return PTD;
        case 4: return PTE;
        default: return NULL;
    }
}

static PORT_Type* get_port_config(uint8_t port) {
    switch (port) {
        case 0: return PORTA;
        case 1: return PORTB;
        case 2: return PORTC;
        case 3: return PORTD;
        case 4: return PORTE;
        default: return NULL;
    }
}