#include <stdlib.h>

#include "rtos.h"

#include "clock.h"
#include "cpu.h"
#include "timer_a1.h"

#include "inc/msp432p401r.h"

#define THREADS 2
#define PERIODIC_THREADS 2
#define STACKSIZE 100

typedef struct Tcb {
    int32_t*    sp;        // stack pointer
    struct Tcb* next;      // linked-list
    int32_t*    blocked;   // shows on which semaphore it is blocked
    int32_t     sleep;
} Tcb_t;

PeriodicEvent_t periodic_event_threads[PERIODIC_THREADS];

Tcb_t   tcbs[THREADS];
Tcb_t*  run_pt;
int32_t stacks[THREADS][STACKSIZE];

void RunPeriodicEvents(void);

void __attribute__((naked)) SysTick_Handler(void) {
    __asm volatile (    
        "CPSID   I              \n" // disable interrupts
        "PUSH    {R4-R11}       \n" // save  R4-R11
        "LDR     R0, =run_pt    \n" // R0 = pointer to run_pt

        "LDR     R1, [R0]       \n" // R1 = run_pt
        "STR     SP, [R1]       \n" // save current SP

        "PUSH    {R0, LR}       \n" // save R0, LR
        "BL      Scheduler      \n" // call scheduler
        "POP     {R0, LR}       \n" // restore R0, LR

        "LDR     R1, [R0]       \n" // R1 = run_pt
        "LDR     SP, [R1]       \n" // restore new SP

        "POP     {R4-R11}       \n" // restore R4-R11
        "CPSIE   I              \n" // enable interrupts
        "BX      LR             \n" // exit exception
        ".align 4               \n"
    );
}

void __attribute__((naked))
StartOS(void) {
    __asm volatile (
        "LDR     R0, =run_pt        \n" // address of run_pt
        "LDR     R1, [R0]           \n" // R1 = run_pt
        "LDR     SP, [R1]           \n" // SP = run_pt->sp

        "POP     {R4-R11}           \n" // restore R4-R11
        "POP     {R0-R3}            \n" // restore R0-R3
        "POP     {R12}              \n" // restore R12
        "ADD     SP, SP, #4         \n" // skip LR
        "POP     {LR}               \n" // load thread start address
        "ADD     SP, SP, #4         \n" // skip PSR
        
        "CPSIE   I                  \n" // enable interrupts
        "BX      LR                 \n" // start thread
        ".align 4                   \n"
    );
}


void SetInitialStack(int i) {
    tcbs[i].sp = &stacks[i][STACKSIZE-16];
    stacks[i][STACKSIZE-1] = 0x01000000; // xPSR thumb bit

    stacks[i][STACKSIZE-16] = 16;   // R4
    stacks[i][STACKSIZE-15] = 15;   // R5
    stacks[i][STACKSIZE-14] = 14;   // R6
    stacks[i][STACKSIZE-13] = 13;   // R7
    stacks[i][STACKSIZE-12] = 12;   // R8
    stacks[i][STACKSIZE-11] = 11;   // R9
    stacks[i][STACKSIZE-10] = 10;   // R10
    stacks[i][STACKSIZE-9]  = 9;    // R11
    stacks[i][STACKSIZE-8]  = 8;    // R0
    stacks[i][STACKSIZE-7]  = 7;    // R1
    stacks[i][STACKSIZE-6]  = 6;    // R2
    stacks[i][STACKSIZE-5]  = 5;    // R3
    stacks[i][STACKSIZE-4]  = 4;    // R12
    stacks[i][STACKSIZE-3]  = 0;    // LR (0 causes fault if thread returns)

}

// Initialize operating system, disable interrupts
// Initialize OS controlled I/O: systick, bus clock as fast as possible
void RoundRobinInit(void) {
    CPU_cpsid();
    ClockInit48MHz();
    // Write TimerA1Init here 1000 Hz
}

void AddThreads(void (**threads)(void)) {
    uint32_t i = 0;
    for (; i < THREADS; ++i) {
        SetInitialStack(i);
        stacks[i][STACKSIZE-2] = (int32_t)threads[i];
        tcbs[i].next = &tcbs[ (i+1) % THREADS ];
        tcbs[i].blocked = 0;
        tcbs[i].sleep = 0;
        // tcbs[i].priority = p0;
    }
    run_pt = &tcbs[0];
}

void AddPeriodicEventThreads(PeriodicEvent_t *periodic_events) {
    uint32_t i = 0;
    for (;i < PERIODIC_THREADS; ++i) {
        periodic_event_threads[i] = periodic_events[i];
    }
}

void RoundRobinLaunch(uint32_t time_slice_cycles) {
    SysTick->CTRL = 0;                      // 1) disable SysTick during setup
    SysTick->LOAD = time_slice_cycles - 1;  // 2) reload value sets period
    SysTick->VAL = 0;                       // 3) any write to current clears it
    SCB->SHP[11] = 7 << 5;                  // set priority into top 3 bits of 8-bit register
    SysTick->CTRL = 0x00000007;             // 4) enable SysTick with core clock and interrupts

    TimerA1Init(RunPeriodicEvents, 5000);   // 100Hz
    StartOS();                              // start on the first task
}

void RunPeriodicEvents(void) {
    int32_t i;
    for (i = 0; i < THREADS; ++i) {
        if (tcbs[i].sleep) {
            --(tcbs[i].sleep);
        }
    }
    for (i = 0; i < PERIODIC_THREADS; ++i) {
        periodic_event_threads[i].event();
    }
}

void Scheduler(void) {
    Tcb_t* t = run_pt->next;
    while (t->blocked || t->sleep) {
        t = t->next;
    }
    
    run_pt = t;
}

void Suspend(void) {
    SysTick->VAL = 0; // clear
    SCB->ICSR |= SCB_ICSR_PENDSTSET_Msk;   // trigger
}

void Sleep(int32_t time) {
    run_pt->sleep = time;
    Suspend();
}

// Decrement semaphore
void Wait(int32_t *sema_pt) {
    CPU_cpsid();
    --(*sema_pt);
    if((*sema_pt) < 0) {
        run_pt->blocked = sema_pt; // reason it is blocked
        
        CPU_cpsie();
        Suspend();
    }
    CPU_cpsie();
}

// Increment semaphore
void Signal(int32_t *sema_pt) {
    Tcb_t *pt;
    CPU_cpsid();
    ++(*sema_pt);
    if ((*sema_pt) <= 0) {
        pt = run_pt->next;
        while(pt->blocked != sema_pt) {
            pt = pt->next;
        }
        pt->blocked = 0;
    }
    CPU_cpsie();
}