/**
 * @file port.c
 * @brief NXP S32K architecture specific port
 * @note S32K is designed for automotive body and safety applications
 */

#include "kernel.h"
#include "task.h"
#include "scheduler.h"
#include "portmacro.h"
#include "S32K144.h"
#include "interrupt_manager.h"

/* Context storage */
uint32_t* current_task_sp = NULL;
uint32_t* next_task_sp = NULL;

/* LPIT configuration */
#define LPIT_CHANNEL        0
#define LPIT_TICK_PERIOD    1000  /* 1ms at 1MHz */

/* Interrupt priorities */
#define SYSTICK_PRIORITY    15
#define PENDSV_PRIORITY     15
#define SVC_PRIORITY        0

/* Initialize Architecture Port */
void port_init(void) {
    /* Disable global interrupts */
    __disable_irq();

    /* Configure system clock */
    /* Assuming 8MHz external crystal, PLL to 160MHz */
    SCG->SPLLCSR = SCG_SPLLCSR_SPLLEN_MASK;
    SCG->SPLLDIV = SCG_SPLLDIV_SPLLDIV1(1) | SCG_SPLLDIV_SPLLDIV2(1);
    SCG->SPLLCFG = SCG_SPLLCFG_MULT(20);  /* 8MHz * 20 = 160MHz */

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

    /* Switch to PLL */
    SCG->RCCR = SCG_RCCR_DIVCORE(1) | SCG_RCCR_DIVBUS(2) |
                SCG_RCCR_DIVSLOW(4) | SCG_RCCR_SCS(6);

    /* Enable clock to LPIT */
    PCC->PCCn[PCC_LPIT0_INDEX] = PCC_PCCn_PCS(6) | PCC_PCCn_CGC_MASK;

    /* Configure LPIT for system tick */
    LPIT0->MCR = LPIT_MCR_M_CEN_MASK;  /* Enable module */

    /* Configure channel 0 */
    LPIT0->TMR[LPIT_CHANNEL].TVAL = LPIT_TICK_PERIOD;
    LPIT0->TMR[LPIT_CHANNEL].TCTRL =
        LPIT_TMR_TCTRL_T_EN_MASK |      /* Enable timer */
        LPIT_TMR_TCTRL_MODE_MASK;       /* 32-bit counter mode */

    /* Enable LPIT interrupt */
    LPIT0->MIER |= (1 << LPIT_CHANNEL);

    /* Set interrupt priority */
    NVIC_SetPriority(LPIT0_IRQn, SYSTICK_PRIORITY);
    NVIC_EnableIRQ(LPIT0_IRQn);

    /* Configure watchdog */
    WDOG->CNT = 0x1000;  /* 4s timeout */
    WDOG->TOVAL = 0x1000;
    WDOG->CS = WDOG_CS_EN_MASK | WDOG_CS_CLK(1) |
               WDOG_CS_WIN_MASK | WDOG_CS_UPDATE_MASK;

    /* Enable faults */
    SCB->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk |
                  SCB_SHCSR_BUSFAULTENA_Msk |
                  SCB_SHCSR_USGFAULTENA_Msk;

    /* Enable global interrupts */
    __enable_irq();
}

/* Start First Task */
void port_start_first_task(void) {
    TaskHandle_t first_task = scheduler_get_current_task();

    if (first_task == NULL) {
        return;
    }

    /* Set PSP */
    __set_PSP((uint32_t)first_task->stack_pointer);

    /* Switch to PSP */
    __set_CONTROL(0x02);
    __ISB();

    /* Restore context */
    __asm volatile (
        "LDMIA R0!, {R4-R11}\n"
        "MSR PSP, R0\n"
        "MOV LR, #0xFFFFFFFD\n"
        "BX LR\n"
        :
        : "r" (first_task->stack_pointer)
    );
}

/* Context Switch */
void port_context_switch(uint32_t* current_context, uint32_t* next_context) {
    current_task_sp = current_context;
    next_task_sp = next_context;

    /* Trigger PendSV */
    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;

    /* Data synchronization barrier */
    __DSB();
    __ISB();
}

/* Initialize Task Stack */
uint32_t* port_initialize_task_stack(TaskFunction_t task_function,
                                     void* parameters,
                                     uint32_t* stack_top) {
    uint32_t* stack_ptr = stack_top;

    /* 8-byte alignment */
    stack_ptr = (uint32_t*)((uint32_t)stack_ptr & ~0x7);

    /* Exception frame */
    *(--stack_ptr) = 0x01000000;                    /* xPSR */
    *(--stack_ptr) = (uint32_t)task_function;        /* PC */
    *(--stack_ptr) = 0xFFFFFFFD;                     /* LR */
    *(--stack_ptr) = 0x00000000;                     /* R12 */
    *(--stack_ptr) = 0x00000003;                     /* R3 */
    *(--stack_ptr) = 0x00000002;                     /* R2 */
    *(--stack_ptr) = 0x00000001;                     /* R1 */
    *(--stack_ptr) = (uint32_t)parameters;           /* R0 */

    /* Additional context */
    *(--stack_ptr) = 0x0000000B;                     /* R11 */
    *(--stack_ptr) = 0x0000000A;                     /* R10 */
    *(--stack_ptr) = 0x00000009;                     /* R9 */
    *(--stack_ptr) = 0x00000008;                     /* R8 */
    *(--stack_ptr) = 0x00000007;                     /* R7 */
    *(--stack_ptr) = 0x00000006;                     /* R6 */
    *(--stack_ptr) = 0x00000005;                     /* R5 */
    *(--stack_ptr) = 0x00000004;                     /* R4 */

    return stack_ptr;
}

/* LPIT Interrupt Handler */
void LPIT0_IRQHandler(void) {
    /* Clear interrupt flag */
    LPIT0->MSR |= (1 << LPIT_CHANNEL);

    /* Call kernel tick handler */
    extern void kernel_tick_handler(void);
    kernel_tick_handler();

    /* Service watchdog */
    WDOG->CNT = 0xB480;  /* Refresh sequence */
    WDOG->CNT = 0x4B80;
}

/* PendSV Handler */
__attribute__((naked)) void PendSV_Handler(void) {
    __asm volatile (
        "MRS R0, PSP\n"
        "STMDB R0!, {R4-R11}\n"
        "LDR R1, =current_task_sp\n"
        "STR R0, [R1]\n"
        "LDR R0, =next_task_sp\n"
        "LDR R1, [R0]\n"
        "LDMIA R1!, {R4-R11}\n"
        "MSR PSP, R1\n"
        "BX LR\n"
    );
}

/* SVC Handler */
__attribute__((naked)) void SVC_Handler(void) {
    __asm volatile (
        "TST LR, #4\n"
        "ITE EQ\n"
        "MRSEQ R0, MSP\n"
        "MRSNE R0, PSP\n"
        "LDR R0, [R0, #24]\n"
        "LDRB R0, [R0, #-2]\n"
        "PUSH {LR}\n"
        "BL svc_handler\n"
        "POP {LR}\n"
        "BX LR\n"
    );
}

/* Fault Handlers */
void MemManage_Handler(void) {
    uint32_t fault_address = SCB->MMFAR;
    uint32_t fault_status = SCB->CFSR;

    extern void fault_handler_process(uint32_t type, uint32_t address, uint32_t status);
    fault_handler_process(2, fault_address, fault_status);

    while(1) {
        /* Safe state */
        WDOG->CNT = 0xB480;
        WDOG->CNT = 0x4B80;
    }
}

void BusFault_Handler(void) {
    uint32_t fault_address = SCB->BFAR;
    uint32_t fault_status = SCB->CFSR;

    extern void fault_handler_process(uint32_t type, uint32_t address, uint32_t status);
    fault_handler_process(3, fault_address, fault_status);

    while(1) {
        WDOG->CNT = 0xB480;
        WDOG->CNT = 0x4B80;
    }
}

void UsageFault_Handler(void) {
    uint32_t fault_status = SCB->CFSR;

    extern void fault_handler_process(uint32_t type, uint32_t address, uint32_t status);
    fault_handler_process(4, 0, fault_status);

    while(1) {
        WDOG->CNT = 0xB480;
        WDOG->CNT = 0x4B80;
    }
}

void HardFault_Handler(void) {
    uint32_t fault_status = SCB->HFSR;

    extern void fault_handler_process(uint32_t type, uint32_t address, uint32_t status);
    fault_handler_process(1, 0, fault_status);

    while(1) {
        WDOG->CNT = 0xB480;
        WDOG->CNT = 0x4B80;
    }
}

/* Enter Critical Section */
void port_disable_interrupts(void) {
    __disable_irq();
}

/* Exit Critical Section */
void port_enable_interrupts(void) {
    __enable_irq();
}

/* Get Current Exception Number */
uint32_t port_get_current_exception(void) {
    return (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) >> SCB_ICSR_VECTACTIVE_Pos;
}