#include <avr/io.h>
#include <Arduino.h>
#include <TimerOne.h>


/*
*
*Defines e variáveis do Kernel
*
*/
#define MaxNumberTask 16
#define SizeTaskStack 64 // Tamanho da pilha da tarefa
unsigned int NumberTaskAdd=-1;
#define KERNEL 0
#define MAX_NAME_LENGTH 30
char myName[MAX_NAME_LENGTH];

enum Taskstates{
  INITIAL,
  READY,
  RUNNING,
  DEAD,
  BLOCKED
};

typedef struct 
{
    short count;
    int sem_queue[MaxNumberTask], tail, header;
}sem_t;

typedef struct
{
  int CallNumber;
  unsigned char *p0;
  unsigned char *p1;
  unsigned char *p2;
  unsigned char *p3;
}Parameters;

typedef struct {
    int Tid;
    const char *name;
    void (*task)();
    unsigned short Prio;
    float Time;
    unsigned short Join;
    unsigned short State;
    uint8_t Stack[SizeTaskStack]; // Vetor de pilha
    uint8_t* P; // Ponteiro de pilha
} TaskDescriptor;
TaskDescriptor Descriptors[MaxNumberTask]; // Array de descritores de tarefas

Parameters *kernelargs ;

/* 
*
*Serviços do kernel
*
*/
enum sys_temCall{
  TASKCREATE,
  SEM_WAIT,
  SEM_POST,
  SEM_INIT,
  WRITELCDN,
  WRITELCDS,
  EXITTASK,
  SLEEP,
  MSLEEP,
  USLEEP,
  LIGALED,
  START, 
  TASKJOIN,
  SETMYNAME,
  NKPRINT,
  GETMYNUMBER,
  NKREAD,
};

volatile int TaskRunning = 0;
/* 
*
*Aplicação
*
*/

#define NUM_TASKS 4
volatile int16_t taskCounter[NUM_TASKS] = {0, 0, 0, 0}; // Contadores para as tarefas
volatile int16_t valor = 0 ;

/*
*
* Rotinas do kernel
*
*
*/

/*
*
* Task kernel (sys_call)
*
*
*/

/*
*
* User Call
*
*
*/
void semwait(sem_t *semaforo)
{
  Parameters arg;
  arg.CallNumber=SEM_WAIT;
  arg.p0=(unsigned char *)semaforo;
  callsvc(&arg);
}

void sempost(sem_t *semaforo)
{
  Parameters arg;
  arg.CallNumber=SEM_POST;
  arg.p0=(unsigned char *)semaforo;
  callsvc(&arg);
}

void seminit(sem_t *semaforo, int ValorInicial)
{
  Parameters arg;
  arg.CallNumber=SEM_INIT;
  arg.p0=(unsigned char *)semaforo;
  arg.p1=(unsigned char *)ValorInicial;
  callsvc(&arg);
}

void saveContext(TaskDescriptor* task) {
    asm volatile (
        "push r0 \n\t"
        "in r0, __SREG__ \n\t"
        "cli \n\t"
        "push r0 \n\t"
        "push r1 \n\t"
        "clr r1 \n\t"
        "push r2 \n\t"
        "push r3 \n\t"
        "push r4 \n\t"
        "push r5 \n\t"
        "push r6 \n\t"
        "push r7 \n\t"
        "push r8 \n\t"
        "push r9 \n\t"
        "push r10 \n\t"
        "push r11 \n\t"
        "push r12 \n\t"
        "push r13 \n\t"
        "push r14 \n\t"
        "push r15 \n\t"
        "push r16 \n\t"
        "push r17 \n\t"
        "push r18 \n\t"
        "push r19 \n\t"
        "push r20 \n\t"
        "push r21 \n\t"
        "push r22 \n\t"
        "push r23 \n\t"
        "push r24 \n\t"
        "push r25 \n\t"
        "push r26 \n\t"
        "push r27 \n\t"
        "push r28 \n\t"
        "push r29 \n\t"
        "push r30 \n\t"
        "push r31 \n\t"
        "in %A0, __SP_L__ \n\t"
        "in %B0, __SP_H__ \n\t"
        : "=r" (task->P)
    );
}

void restoreContext(TaskDescriptor* task) {
    asm volatile (
        "out __SP_L__, %A0 \n\t"
        "out __SP_H__, %B0 \n\t"
        "pop r31 \n\t"
        "pop r30 \n\t"
        "pop r29 \n\t"
        "pop r28 \n\t"
        "pop r27 \n\t"
        "pop r26 \n\t"
        "pop r25 \n\t"
        "pop r24 \n\t"
        "pop r23 \n\t"
        "pop r22 \n\t"
        "pop r21 \n\t"
        "pop r20 \n\t"
        "pop r19 \n\t"
        "pop r18 \n\t"
        "pop r17 \n\t"
        "pop r16 \n\t"
        "pop r15 \n\t"
        "pop r14 \n\t"
        "pop r13 \n\t"
        "pop r12 \n\t"
        "pop r11 \n\t"
        "pop r10 \n\t"
        "pop r9 \n\t"
        "pop r8 \n\t"
        "pop r7 \n\t"
        "pop r6 \n\t"
        "pop r5 \n\t"
        "pop r4 \n\t"
        "pop r3 \n\t"
        "pop r2 \n\t"
        "pop r1 \n\t"
        "pop r0 \n\t"
        "out __SREG__, r0 \n\t"
        "pop r0 \n\t"
        : : "r" (task->P)
    );
}
void callsvc(Parameters *args)
{
  noInterrupts();
  kernelargs = args ;
  interrupts();
  saveContext(&Descriptors[TaskRunning]) ;
  restoreContext(&Descriptors[0]);
  //DoSystemCall(0,args);
}
void switchTask() {
    saveContext(&Descriptors[TaskRunning]) ;
    TaskRunning = (TaskRunning + 1) % NUM_TASKS ; ;
   // while (Descriptors[TaskRunning].State == BLOCKED) {
    //      TaskRunning = (TaskRunning + 1) % NumberTaskAdd; ;
   //  }
   restoreContext(&Descriptors[TaskRunning]);   
}

void sys_taskcreate(int *tid, void (*taskFunction)(void)) {

    NumberTaskAdd++ ;
    *tid = NumberTaskAdd;
    Descriptors[NumberTaskAdd].Tid=*tid;
    Descriptors[NumberTaskAdd].State=READY;
    if (*tid == 0) {
        Descriptors[NumberTaskAdd].State=BLOCKED;
    }
    Descriptors[NumberTaskAdd].Join=0;
    Descriptors[NumberTaskAdd].Time=0;
    Descriptors[NumberTaskAdd].Prio=0;
    uint8_t* stack = Descriptors[*tid].Stack + SizeTaskStack - 1;
    Descriptors[*tid].P = stack;

    *(stack--) = ((uint16_t)taskFunction) & 0xFF; // PC low byte
    *(stack--) = ((uint16_t)taskFunction >> 8) & 0xFF; // PC high byte
    *(stack--) = 0x00; // R0
    *(stack--) = 0x80; // SREG with global interrupts enabled

    for (int i = 1; i < 32; i++) {
        *(stack--) = i; // Initialize all other registers with their number
    }

    Descriptors[*tid].P = stack;
}

void sys_getmynumber(int *number)
{
   *number=Descriptors[TaskRunning].Tid;
}

void sys_setmyname(const char *name)
{
  Descriptors[TaskRunning].name=name;
}

void sys_getmyname(const char *name)
{
  strcpy(name, Descriptors[TaskRunning].name);
}

void sys_semwait(sem_t *semaforo)
{
     semaforo->count--;
     if(semaforo->count < 0)
     {
	     semaforo->sem_queue[semaforo->tail] = TaskRunning;
	     Descriptors[TaskRunning].State = BLOCKED ;
	     semaforo->tail++;
	     if(semaforo->tail == MaxNumberTask-1) semaforo->tail = 0;
	    //   Dispatcher();
     }
}

void sys_sempost(sem_t *semaforo)
{
     semaforo->count++;
     if(semaforo->count <= 0)
     {
       Descriptors[semaforo->sem_queue[semaforo->header]].State = READY;
       //InsertReadyList(semaforo->sem_queue[semaforo->header]);
       semaforo->header++;
       if(semaforo->header == MaxNumberTask-1) semaforo->header = 0;
     }
}

void sys_seminit(sem_t *semaforo, int ValorInicial)
{
     semaforo->count = ValorInicial;
     semaforo->header = 0;
     semaforo->tail = 0;
}
void kernel() {
  int16_t v ;
    Serial.print("Kernel: ");
    v = 5555 ;
    Serial.println(valor, DEC);
    v = 3333 ;
    Serial.println(valor, DEC);
    delay(1000);
   /* switch(kernelargs->CallNumber){
    case TASKCREATE:
      sys_taskcreate((int *)kernelargs->p0,(void(*)())kernelargs->p1);
      break;
    case SEM_WAIT:
      sys_semwait((sem_t *)kernelargs->p0);
      break;
    case SEM_POST:
      sys_sempost((sem_t *)kernelargs->p0);
      break;
    case SEM_INIT:
      sys_seminit((sem_t *)kernelargs->p0,(int)kernelargs->p1);
      break;
    case WRITELCDN:
      // LCDcomando((int)arg->p1);
      // LCDnum((int)arg->p0);
      break;
    case WRITELCDS:
      // LCDcomando((int)arg->p1);
      // LCDputs((char*)arg->p0);
      break;
    case EXITTASK:
      //sys_taskexit();
      break;
    case SLEEP:
     // sys_sleep((int)arg->p0);
      break;
    case MSLEEP:
     // sys_msleep((int)arg->p0);
      break;
    case USLEEP:
      //sys_usleep((int)arg->p0);
      break;
    case LIGALED:
     // sys_ligaled((int)arg->p0);
      break;
    case START:
     // sys_start((int)arg->p0);
      break;
    case TASKJOIN:
     // sys_taskjoin((int)arg->p0);
      break;
    case SETMYNAME:
      sys_setmyname((const char *)kernelargs->p0);
      break;
    case NKPRINT:
      // sys_nkprint((char *)arg->p0,(void *)arg->p1);
       break;
    case GETMYNUMBER:
       sys_getmynumber((int *)kernelargs->p0);
       break;
    case NKREAD:
       //sys_nkread((char *)arg->p0,(void *)arg->p1);
       break;
    default:
       break;
  }*/
  // RestoreContext(Descriptors[TaskRunning].SP);
}


void idle() {
    while (1) {
       //Serial.println("Idle");
    }
}

/*
*
* Aplicação
*
*
*/
void task1() {     
    while (1) {
      valor++;
      Serial.print("Produzido: ") ;
      Serial.println(valor, DEC);
      delay(1000);
    }
}

void task2() {
    while (1) {
        Serial.print("Consumido: ");
        Serial.println(valor, DEC);
        delay(1000);
    }
}
void setup() {
    Serial.begin(9600);
    volatile int tid0, tid1, tid2, tid3 ;
    
    sys_taskcreate(&tid0, kernel);
    sys_taskcreate(&tid1, idle);
    sys_taskcreate(&tid2, task1);
    sys_taskcreate(&tid3, task2);


    Timer1.initialize(500000); // Half a second
    Timer1.attachInterrupt(switchTask);
   
    sei(); // Habilita interrupções globais
  // Restaura o contexto da primeira tarefa para iniciar sua execução (idle)
    restoreContext(&Descriptors[2]);
}

void loop() {
    while (1);
}