/**
 * @file port.c
 * @brief ARM Cortex-M0 architecture specific port
 * @note Cortex-M0 is used in low-power automotive applications
 */

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

/* Global variables for context switching */
uint32_t* current_task_sp = NULL;
uint32_t* next_task_sp = NULL;

/* Initialize Architecture Port */
void port_init(void) {
    /* Configure SysTick for 1ms interrupts */
    SysTick->LOAD = (SystemCoreClock / 1000) - 1;
    SysTick->VAL = 0;
    SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
                    SysTick_CTRL_TICKINT_Msk |
                    SysTick_CTRL_ENABLE_Msk;

    /* Set lowest priority for SysTick and PendSV */
    NVIC_SetPriority(SysTick_IRQn, 3);
    NVIC_SetPriority(PendSV_IRQn, 3);

    /* Enable 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 to task stack pointer */
    __set_PSP((uint32_t)first_task->stack_pointer);

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

    /* Restore context and start task */
    __asm volatile (
        "POP {R4-R7}\n"
        "MOV R8, R4\n"
        "MOV R9, R5\n"
        "MOV R10, R6\n"
        "MOV R11, R7\n"
        "POP {R4-R7}\n"
        "POP {R0-R3}\n"
        "POP {R12}\n"
        "POP {LR}\n"
        "POP {PC}\n"
    );
}

/* Context Switch Trigger */
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;
}

/* Yield from ISR */
void port_yield_from_isr(void) {
    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
}

/* 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;
}

/* 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;

    /* Align stack to 8 bytes */
    stack_ptr = (uint32_t*)((uint32_t)stack_ptr & ~0x7);

    /* Initial stack frame for Cortex-M0 */
    *(--stack_ptr) = 0x01000000;                          /* xPSR */
    *(--stack_ptr) = (uint32_t)task_function;              /* PC */
    *(--stack_ptr) = 0xFFFFFFFD;                           /* LR (return to thread mode) */
    *(--stack_ptr) = 0x00000000;                           /* R12 */
    *(--stack_ptr) = 0x00000003;                           /* R3 */
    *(--stack_ptr) = 0x00000002;                           /* R2 */
    *(--stack_ptr) = 0x00000001;                           /* R1 */
    *(--stack_ptr) = (uint32_t)parameters;                 /* R0 */

    /* Additional registers */
    *(--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;
}

/* SysTick Handler */
void SysTick_Handler(void) {
    /* Increment tick count */
    extern void kernel_tick_handler(void);
    kernel_tick_handler();
}

/* PendSV Handler */
void PendSV_Handler(void) {
    /* Save current context */
    __asm volatile (
        "MRS R0, PSP\n"
        "SUBS R0, R0, #32\n"
        "STMIA R0!, {R4-R7}\n"
        "MOV R4, R8\n"
        "MOV R5, R9\n"
        "MOV R6, R10\n"
        "MOV R7, R11\n"
        "STMIA R0!, {R4-R7}\n"
        "SUBS R0, R0, #32\n"
        "LDR R1, =current_task_sp\n"
        "STR R0, [R1]\n"
    );

    /* Load next context */
    __asm volatile (
        "LDR R0, =next_task_sp\n"
        "LDR R1, [R0]\n"
        "LDMIA R1!, {R4-R7}\n"
        "MOV R8, R4\n"
        "MOV R9, R5\n"
        "MOV R10, R6\n"
        "MOV R11, R7\n"
        "LDMIA R1!, {R4-R7}\n"
        "MSR PSP, R1\n"
        "BX LR\n"
    );
}

/* SVC Handler */
void SVC_Handler(void) {
    /* Handle system calls */
    __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"
        "BX LR\n"
    );
}

/* Hard Fault Handler */
void HardFault_Handler(void) {
    /* Save fault information */
    uint32_t fault_address;
    uint32_t fault_status;

    __asm volatile (
        "MRS %0, BFAR\n"
        "MRS %1, BFSR\n"
        : "=r" (fault_address), "=r" (fault_status)
    );

    /* Call fault handler */
    extern void fault_handler_process(uint32_t type, uint32_t address, uint32_t status);
    fault_handler_process(1, fault_address, fault_status);

    /* Infinite loop */
    while(1);
}