#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 SIZE 100

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

PeriodicEvent_t periodic_event_threads[PERIODIC_THREADS];

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

void RunPeriodicEvents(void);

void __attribute__((naked))
SysTick_Handler(void) {                 // 1) Saves R0-R3,R12,LR,PC,PSR
    __asm(  // 2) Prevent interrupt during switch
            // 3) Save remaining regs r4-11
            // 4) R0=pointer to run_pt, old thread
            // 5) Save SP into TCB
            // 6) R1 = run_pt, new thread
            // 7) new thread SP; SP = run_pt->sp;
            // 8) restore regs r4-11
            // 9) tasks run with interrupts enabled
          );          // 10) restore R0-R3,R12,LR,PC,PSR
}

void __attribute__((naked))
StartOS(void) {
    __asm(// currently running thread
          // R2 = value of run_pt
          // new thread SP; SP = run_pt->sp;
          // restore regs r4-11
          // restore regs r0-3
          // discard LR from initial stack
          // start location
          // discard PSR
          // Enable interrupts at processor level
          );          // start first thread
}

void SetInitialStack(int idx) {
	// Write this code
}

// 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)) {
    // Write this code
}

void AddPeriodicEventThreads(PeriodicEvent_t *periodic_events) {
    // Write this code
}

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
    StartOS();                              // start on the first task
}

void RunPeriodicEvents(void) {
    // Write this code (no sleep decreasing)
}

void Scheduler(void) {
    // Write this code (no sleep check)
}

void Suspend(void) {
    SysTick->VAL = 0;       // any write to current clears it
    SCB->ICSR |= SCB_ICSR_PENDSTSET_Msk;   // trigger SysTick
}

// Decrement semaphore
void Wait(int32_t *sema_pt) {
    // Write this code similar to RTOS3
}

// Increment semaphore
void Signal(int32_t *sema_pt) {
    // Write this code similar to RTOS3
}