/**
 * @file board_init.c
 * @brief NXP S32K144 EVB board initialization
 */

#include "board.h"
#include "kernel.h"
#include "gpio_driver.h"
#include "uart_driver.h"
#include "can_driver.h"
#include "spi_driver.h"
#include "i2c_driver.h"
#include "adc_driver.h"
#include "pwm_driver.h"
#include <string.h>

static bool board_initialized = false;

/* ============================================================================
 * Board Initialization
 * ============================================================================ */
void board_init(void) {
    if (board_initialized) {
        return;
    }

    /* Initialize clock */
    board_clock_init();

    /* Initialize GPIO */
    board_gpio_init();

    /* Initialize UART */
    board_uart_init();

    /* Initialize CAN */
    board_can_init();

    /* Initialize SPI */
    board_spi_init();

    /* Initialize I2C */
    board_i2c_init();

    /* Initialize ADC */
    board_adc_init();

    /* Initialize PWM */
    board_pwm_init();

    /* Initialize watchdog */
    board_watchdog_init();

    board_initialized = true;
}

/* ============================================================================
 * Clock Initialization
 * ============================================================================ */
void board_clock_init(void) {
    /* Configure system PLL */
    SCG->SPLLCSR = 0;  /* Reset SPLL */
    SCG->SPLLDIV = SCG_SPLLDIV_SPLLDIV1(1) | SCG_SPLLDIV_SPLLDIV2(1);
    SCG->SPLLCFG = SCG_SPLLCFG_MULT(20);  /* 8MHz * 20 = 160MHz */

    /* Enable SPLL */
    SCG->SPLLCSR |= SCG_SPLLCSR_SPLLEN_MASK;

    /* Wait for PLL lock */
    while (!(SCG->SPLLCSR & SCG_SPLLCSR_SPLLVLD_MASK));

    /* Switch to SPLL */
    SCG->RCCR = SCG_RCCR_DIVCORE(1) |
                SCG_RCCR_DIVBUS(4) |   /* 160/4 = 40MHz */
                SCG_RCCR_DIVSLOW(8) |  /* 160/8 = 20MHz */
                SCG_RCCR_SCS(6);       /* SPLL */

    /* Wait for clock switch */
    while ((SCG->CSR & SCG_CSR_SCS_MASK) != SCG_CSR_SCS(6));

    /* Update SystemCoreClock */
    SystemCoreClock = SYSTEM_CLOCK;
}

/* ============================================================================
 * GPIO Initialization
 * ============================================================================ */
void board_gpio_init(void) {
    /* Enable GPIO clocks */
    PCC->PCCn[PCC_PORTA_INDEX] |= PCC_PCCn_CGC_MASK;
    PCC->PCCn[PCC_PORTB_INDEX] |= PCC_PCCn_CGC_MASK;
    PCC->PCCn[PCC_PORTC_INDEX] |= PCC_PCCn_CGC_MASK;
    PCC->PCCn[PCC_PORTD_INDEX] |= PCC_PCCn_CGC_MASK;
    PCC->PCCn[PCC_PORTE_INDEX] |= PCC_PCCn_CGC_MASK;

    /* Configure LEDs */
    GpioPinConfig_t led_config = {
        .mode = GPIO_MODE_OUTPUT,
        .output_type = GPIO_OUTPUT_PUSH_PULL,
        .pull = GPIO_PULL_NONE,
        .speed = GPIO_SPEED_HIGH
    };

    led_config.port = 2;  /* PTC */
    led_config.pin = 0;
    gpio_init(&led_config);

    led_config.pin = 1;
    gpio_init(&led_config);

    led_config.pin = 2;
    gpio_init(&led_config);

    /* Configure buttons */
    GpioPinConfig_t button_config = {
        .port = 2,  /* PTC */
        .pin = 3,
        .mode = GPIO_MODE_INPUT,
        .pull = GPIO_PULL_UP
    };
    gpio_init(&button_config);

    button_config.pin = 4;
    gpio_init(&button_config);

    /* Turn off all LEDs */
    board_led_off(0);
    board_led_off(1);
    board_led_off(2);
}

/* ============================================================================
 * UART Initialization
 * ============================================================================ */
void board_uart_init(void) {
    /* Configure UART0 */
    UartConfig_t uart_config = {
        .baudrate = UART0_BAUDRATE,
        .data_bits = UART_DATA_BITS_8,
        .stop_bits = UART_STOP_BITS_1,
        .parity = UART_PARITY_NONE,
        .flow_control = UART_FLOW_CONTROL_NONE,
        .enable_rx = true,
        .enable_tx = true,
        .use_dma = false
    };

    uart_init(UART0_INSTANCE, &uart_config);
}

/* ============================================================================
 * CAN Initialization
 * ============================================================================ */
void board_can_init(void) {
    /* Configure CAN0 */
    CanConfig_t can_config = {
        .nominal_baudrate = CAN0_BAUDRATE,
        .data_baudrate = CAN0_BAUDRATE,
        .frame_type = CAN_FRAME_CLASSIC,
        .enable_fd = true,  /* S32K144 supports CAN FD */
        .enable_automatic_retransmission = true,
        .filter_count = 0
    };

    can_init(CAN0_INSTANCE, &can_config);
}

/* ============================================================================
 * SPI Initialization
 * ============================================================================ */
void board_spi_init(void) {
    /* Configure SPI0 */
    SpiConfig_t spi_config = {
        .mode = SPI_MODE_0,
        .clock_speed = SPI_CLOCK_8MHZ,
        .data_order = SPI_DATA_ORDER_MSB_FIRST,
        .data_size = 8,
        .use_dma = false,
        .enable_hardware_cs = false,
        .cs_polarity = SPI_CS_ACTIVE_LOW,
        .cs_port = 1,
        .cs_pin = 0
    };

    spi_init(SPI0_INSTANCE, &spi_config);
}

/* ============================================================================
 * I2C Initialization
 * ============================================================================ */
void board_i2c_init(void) {
    /* Configure I2C0 */
    I2cConfig_t i2c_config = {
        .speed = I2C_SPEED_FAST,
        .addressing_mode = I2C_ADDRESSING_7BIT,
        .own_address = 0x50,
        .enable_general_call = false,
        .enable_clock_stretching = true,
        .use_dma = false
    };

    i2c_init(I2C0_INSTANCE, &i2c_config);
}

/* ============================================================================
 * ADC Initialization
 * ============================================================================ */
void board_adc_init(void) {
    /* Configure ADC0 */
    AdcConfig_t adc_config = {
        .resolution = ADC_RESOLUTION_12BIT,
        .mode = ADC_MODE_SCAN,
        .trigger_source = ADC_TRIGGER_SOFTWARE,
        .reference = ADC_REFERENCE_VDD,
        .enable_dma = false,
        .conversion_frequency = 1000,
        .channel_count = 8,
        .channels = {
            {.channel = 0, .sampling_time = ADC_SAMPLING_13_5_CYCLES},
            {.channel = 1, .sampling_time = ADC_SAMPLING_13_5_CYCLES},
            {.channel = 2, .sampling_time = ADC_SAMPLING_13_5_CYCLES},
            {.channel = 3, .sampling_time = ADC_SAMPLING_13_5_CYCLES},
            {.channel = 4, .sampling_time = ADC_SAMPLING_13_5_CYCLES},
            {.channel = 5, .sampling_time = ADC_SAMPLING_13_5_CYCLES},
            {.channel = 6, .sampling_time = ADC_SAMPLING_13_5_CYCLES},
            {.channel = 7, .sampling_time = ADC_SAMPLING_13_5_CYCLES}
        }
    };

    adc_init(ADC0_INSTANCE, &adc_config);
}

/* ============================================================================
 * PWM Initialization
 * ============================================================================ */
void board_pwm_init(void) {
    /* Configure PWM */
    PwmConfig_t pwm_config = {
        .frequency_hz = PWM_TIMER_FREQUENCY,
        .alignment = PWM_ALIGNMENT_EDGE,
        .period_ticks = 10000,
        .prescaler = PWM_TIMER_PRESCALER,
        .channel_count = 4,
        .channels = {
            {.channel = 0, .duty_cycle = 0},
            {.channel = 1, .duty_cycle = 0},
            {.channel = 2, .duty_cycle = 0},
            {.channel = 3, .duty_cycle = 0}
        },
        .enable_fault_protection = true,
        .fault_action = PWM_FAULT_DISABLE
    };

    pwm_init(PWM_INSTANCE, &pwm_config);
}

/* ============================================================================
 * Watchdog Initialization
 * ============================================================================ */
void board_watchdog_init(void) {
    /* Configure WDOG */
    WDOG->CNT = 0x1000;  /* Timeout value */
    WDOG->TOVAL = 0x1000;
    WDOG->CS = WDOG_CS_EN_MASK |
               WDOG_CS_CLK(1) |
               WDOG_CS_WIN_MASK |
               WDOG_CS_UPDATE_MASK;
}

void board_watchdog_service(void) {
    /* Service WDOG */
    WDOG->CNT = 0xB480;
    WDOG->CNT = 0x4B80;
}

/* ============================================================================
 * LED Functions
 * ============================================================================ */
void board_led_on(uint8_t led) {
    switch (led) {
        case 0:
            gpio_write(2, 0, false);  /* Active low */
            break;
        case 1:
            gpio_write(2, 1, false);
            break;
        case 2:
            gpio_write(2, 2, false);
            break;
    }
}

void board_led_off(uint8_t led) {
    switch (led) {
        case 0:
            gpio_write(2, 0, true);
            break;
        case 1:
            gpio_write(2, 1, true);
            break;
        case 2:
            gpio_write(2, 2, true);
            break;
    }
}

void board_led_toggle(uint8_t led) {
    switch (led) {
        case 0:
            gpio_toggle(2, 0);
            break;
        case 1:
            gpio_toggle(2, 1);
            break;
        case 2:
            gpio_toggle(2, 2);
            break;
    }
}

/* ============================================================================
 * Button Functions
 * ============================================================================ */
bool board_button_read(uint8_t button) {
    switch (button) {
        case 0:
            return !gpio_read(2, 3);  /* Active low */
        case 1:
            return !gpio_read(2, 4);
        default:
            return false;
    }
}

/* ============================================================================
 * Delay Functions
 * ============================================================================ */
void board_delay_ms(uint32_t ms) {
    kernel_delay(ms);
}

void board_delay_us(uint32_t us) {
    uint32_t count = us * (SystemCoreClock / 1000000) / 5;
    while (count--) {
        __NOP();
    }
}

/* ============================================================================
 * Sensor Functions
 * ============================================================================ */
float board_get_temperature(void) {
    /* Read internal temperature sensor */
    uint16_t adc_value = 0;
    adc_read_channel(ADC0_INSTANCE, 26, &adc_value, 10);  /* Internal temp sensor */

    /* Convert to temperature (S32K144 specific formula) */
    float voltage = adc_convert_to_voltage(adc_value, ADC_RESOLUTION_12BIT, 3.3f);
    float temperature = 25.0f - ((voltage - 0.716f) / 0.00162f);

    return temperature;
}

float board_get_voltage(void) {
    /* Read bandgap reference */
    uint16_t adc_value = 0;
    adc_read_channel(ADC0_INSTANCE, 27, &adc_value, 10);  /* Bandgap */

    float voltage = adc_convert_to_voltage(adc_value, ADC_RESOLUTION_12BIT, 3.3f);
    return voltage;
}