/**
 * @file board_init.c
 * @brief STM32F407 Discovery 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>

/* Board state */
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) {
    /* Reset RCC */
    RCC->CR |= RCC_CR_HSION;
    while (!(RCC->CR & RCC_CR_HSIRDY));

    /* Configure PLL */
    RCC->PLLCFGR = (PLL_M << RCC_PLLCFGR_PLLM_Pos) |
                   (PLL_N << RCC_PLLCFGR_PLLN_Pos) |
                   (((PLL_P >> 1) - 1) << RCC_PLLCFGR_PLLP_Pos) |
                   (PLL_Q << RCC_PLLCFGR_PLLQ_Pos);

    /* Enable PLL */
    RCC->CR |= RCC_CR_PLLON;
    while (!(RCC->CR & RCC_CR_PLLRDY));

    /* Configure flash latency */
    FLASH->ACR = FLASH_ACR_LATENCY_5WS | FLASH_ACR_PRFTEN | FLASH_ACR_ICEN | FLASH_ACR_DCEN;

    /* Configure AHB, APB1, APB2 prescalers */
    RCC->CFGR = RCC_CFGR_HPRE_DIV1 | RCC_CFGR_PPRE1_DIV4 | RCC_CFGR_PPRE2_DIV2;

    /* Switch to PLL */
    RCC->CFGR |= RCC_CFGR_SW_PLL;
    while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);

    /* Update SystemCoreClock variable */
    SystemCoreClockUpdate();
}

/* ============================================================================
 * GPIO Initialization
 * ============================================================================ */
void board_gpio_init(void) {
    /* Enable GPIO clocks */
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();
    __HAL_RCC_GPIOD_CLK_ENABLE();
    __HAL_RCC_GPIOE_CLK_ENABLE();

    /* 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 = 3;  /* GPIOD */
    led_config.pin = 12;
    gpio_init(&led_config);

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

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

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

    /* Configure button */
    GpioPinConfig_t button_config = {
        .port = 0,  /* GPIOA */
        .pin = 0,
        .mode = GPIO_MODE_INPUT,
        .pull = GPIO_PULL_DOWN
    };
    gpio_init(&button_config);

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

/* ============================================================================
 * UART Initialization
 * ============================================================================ */
void board_uart_init(void) {
    /* Configure UART1 */
    UartConfig_t uart1_config = {
        .baudrate = UART1_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(UART1_INSTANCE, &uart1_config);

    /* Configure UART2 */
    UartConfig_t uart2_config = uart1_config;
    uart_init(UART2_INSTANCE, &uart2_config);
}

/* ============================================================================
 * CAN Initialization
 * ============================================================================ */
void board_can_init(void) {
    /* Configure CAN1 */
    CanConfig_t can1_config = {
        .nominal_baudrate = CAN1_BAUDRATE,
        .data_baudrate = CAN1_BAUDRATE,
        .frame_type = CAN_FRAME_CLASSIC,
        .enable_fd = false,
        .enable_automatic_retransmission = true,
        .filter_count = 0
    };

    can_init(&can1_config);

    /* Configure CAN2 */
    CanConfig_t can2_config = can1_config;
    can_init(&can2_config);
}

/* ============================================================================
 * SPI Initialization
 * ============================================================================ */
void board_spi_init(void) {
    /* Configure SPI1 */
    SpiConfig_t spi1_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 = 0,
        .cs_pin = 4
    };

    spi_init(SPI1_INSTANCE, &spi1_config);
}

/* ============================================================================
 * I2C Initialization
 * ============================================================================ */
void board_i2c_init(void) {
    /* Configure I2C1 */
    I2cConfig_t i2c1_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(I2C1_INSTANCE, &i2c1_config);
}

/* ============================================================================
 * ADC Initialization
 * ============================================================================ */
void board_adc_init(void) {
    /* Configure ADC1 */
    AdcConfig_t adc1_config = {
        .resolution = ADC_RESOLUTION_12BIT,
        .mode = ADC_MODE_SCAN,
        .trigger_source = ADC_TRIGGER_TIMER,
        .reference = ADC_REFERENCE_VDD,
        .enable_dma = false,
        .conversion_frequency = 1000,
        .channel_count = 6,
        .channels = {
            {.channel = 0, .sampling_time = ADC_SAMPLING_28_5_CYCLES},
            {.channel = 1, .sampling_time = ADC_SAMPLING_28_5_CYCLES},
            {.channel = 2, .sampling_time = ADC_SAMPLING_28_5_CYCLES},
            {.channel = 3, .sampling_time = ADC_SAMPLING_28_5_CYCLES},
            {.channel = 4, .sampling_time = ADC_SAMPLING_28_5_CYCLES},
            {.channel = 5, .sampling_time = ADC_SAMPLING_28_5_CYCLES}
        }
    };

    adc_init(ADC1_INSTANCE, &adc1_config);
}

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

    pwm_init(PWM_TIMER_INSTANCE, &pwm_config);
}

/* ============================================================================
 * Watchdog Initialization
 * ============================================================================ */
void board_watchdog_init(void) {
    /* Enable IWDG */
    IWDG->KR = 0x5555;  /* Enable write access */
    IWDG->PR = 0x06;    /* Prescaler: 256 */
    IWDG->RLR = 0x0FFF; /* Reload value */
    IWDG->KR = 0xCCCC;  /* Start watchdog */
}

void board_watchdog_service(void) {
    /* Service IWDG */
    IWDG->KR = 0xAAAA;
}

/* ============================================================================
 * LED Functions
 * ============================================================================ */
void board_led_on(uint8_t led) {
    switch (led) {
        case 0:
            gpio_write(3, 12, true);
            break;
        case 1:
            gpio_write(3, 13, true);
            break;
        case 2:
            gpio_write(3, 14, true);
            break;
        case 3:
            gpio_write(3, 15, true);
            break;
    }
}

void board_led_off(uint8_t led) {
    switch (led) {
        case 0:
            gpio_write(3, 12, false);
            break;
        case 1:
            gpio_write(3, 13, false);
            break;
        case 2:
            gpio_write(3, 14, false);
            break;
        case 3:
            gpio_write(3, 15, false);
            break;
    }
}

void board_led_toggle(uint8_t led) {
    switch (led) {
        case 0:
            gpio_toggle(3, 12);
            break;
        case 1:
            gpio_toggle(3, 13);
            break;
        case 2:
            gpio_toggle(3, 14);
            break;
        case 3:
            gpio_toggle(3, 15);
            break;
    }
}

/* ============================================================================
 * Button Functions
 * ============================================================================ */
bool board_button_read(uint8_t button) {
    if (button == 0) {
        return gpio_read(0, 0);
    }
    return false;
}

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

void board_delay_us(uint32_t us) {
    /* Simple busy-wait delay */
    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(ADC1_INSTANCE, 16, &adc_value, 10);

    /* Convert to temperature */
    float voltage = adc_convert_to_voltage(adc_value, ADC_RESOLUTION_12BIT, 3.3f);
    float temperature = ((voltage - 0.76f) / 0.0025f) + 25.0f;

    return temperature;
}

float board_get_voltage(void) {
    /* Read VREFINT */
    uint16_t adc_value = 0;
    adc_read_channel(ADC1_INSTANCE, 17, &adc_value, 10);

    /* Convert to voltage */
    float voltage = adc_convert_to_voltage(adc_value, ADC_RESOLUTION_12BIT, 3.3f);

    return voltage;
}