#include <stdio.h>
#include <stdlib.h>

#include <setjmp.h>

#include "osp.h"



// Task Config
#define TASKS_SIZE 3
#define TASK_STACK_SIZE 256

// Sem config
#define SEMS_SIZE 2



typedef enum { 
    Clear = 0x0000, Running = 0x0001, Ready = 0x0002, Block = 0x0004
}os_tstate;

typedef struct {
    int state;
    int sem;
    jmp_buf regs;
    unsigned short stack[TASK_STACK_SIZE];
} taskType;

typedef struct {
    int value;
} semType;


jmp_buf osRegs;

taskType tasks[TASKS_SIZE];
int taskIndex;

semType sems[SEMS_SIZE];

//-------------

void osSemInit(int s, int v) {
    
   sems[s].value = v; 
}


void osSemP(int s){  
    
    --sems[s].value;    
    if (sems[s].value < 0) {  
        tasks[taskIndex].sem = s; 
        tasks[taskIndex].state = Block;
        if (!setjmp( tasks[taskIndex].regs)) {
            longjmp(osRegs, 1);
        }
    }
}


void osSemV(int s){
    int i;
    
    ++sems[s].value; 
    if (sems[s].value <= 0) {
        i = taskIndex; 
        do {
            i = (i + 1) % TASKS_SIZE;
        } while ( tasks[i].sem != s); 
        tasks[i].sem = -1;
        tasks[i].state = Ready;
    }
}


void osInit(void){
    int i;
    
    taskIndex = -1;
    for(i = 0; i < TASKS_SIZE; i++) {
        tasks[i].state = Clear;
        tasks[i].sem = -1; 
    }
    
    for(i = 0; i < SEMS_SIZE; i++) { 
        sems[i].value = 1;
    }
}

void osBegin(void){
    
    setjmp(osRegs);
    
    do {
        taskIndex = (taskIndex + 1) % TASKS_SIZE;
    } while ( tasks[taskIndex].state != Ready); 
    
    tasks[taskIndex].state = Running;
    longjmp(tasks[taskIndex].regs, 1);
}


void osTaskCreate(int i, void (*taskPtr)(void)){
    
    tasks[i].state = Ready;
    tasks[i].sem = -1;

    setjmp(tasks[i].regs);

    // Magic stuff here !!!     

    *((void **) &tasks[i].regs[0]._jb[16]) = &tasks[i].stack[TASK_STACK_SIZE -1];
    *((void **) &tasks[i].regs[0]._jb[18]) = &tasks[i].stack[TASK_STACK_SIZE -1];
    *((void (**) (void)) &tasks[i].regs[0]._jb[21]) = taskPtr;   
}


void osTaskPreempt(void){
    
    if (!setjmp( tasks[taskIndex].regs)) {
        tasks[taskIndex].state = Ready;
        longjmp(osRegs, 1);
    }
}