#include "my_os.h"
#include <mega128.h>
//#include "global.h"
#include <stdlib.h>
//#include <inavr.h>

//externe
extern unsigned char t_sph;
extern unsigned char t_spl;
extern unsigned char t_r29;
extern unsigned char t_r28;
extern unsigned char task_time;
extern unsigned char ig,jg,kg,lg,mg,ng;
extern unsigned int global_inc_g;
extern unsigned int global_inc;
extern unsigned int inc;
extern unsigned char temp_g;
extern unsigned char b_start_so;
//extern S8 lcd_sem;
extern unsigned char lcd_coada[NR_MAX_TASK];


//folosita pentru initializarea t_sp si t_r in rutina timer
void get_tsp(void)
{
//initializare stiva (unde se gaseste adresa stivei)
    tem = (unsigned int) ((rtos[task_curent].adr_task)+OSTaskStkSize)>>8;
    t_sph=(unsigned char)(tem&0x00ff);
    t_spl=(unsigned char)((rtos[task_curent].adr_task)+OSTaskStkSize)&0xff;
//initializarea adresa pointer y
    tem = (unsigned int) ((rtos[task_curent].adr_task)+OSTaskStkSize-2)>>8;
    t_r29=(unsigned char)(tem&0x00ff);
    t_r28=(unsigned char)((rtos[task_curent].adr_task)+OSTaskStkSize-2)&0xff;
}

//salvarea variabilelor globale
void push_var(void)
{
//unsigned char ig,jg,kg,lg,mg,ng;
  rtos[task_curent].adr_task[OSTaskStkSize-5]=ig;
  rtos[task_curent].adr_task[OSTaskStkSize-6]=jg;
  rtos[task_curent].adr_task[OSTaskStkSize-7]=kg;
  rtos[task_curent].adr_task[OSTaskStkSize-8]=lg;
  rtos[task_curent].adr_task[OSTaskStkSize-9]=mg;
  rtos[task_curent].adr_task[OSTaskStkSize-10]=ng;
//  rtos[task_curent].adr_task[OSTaskStkSize-11]=(unsigned char)(global_inc_g&0x0ff);
//  rtos[task_curent].adr_task[OSTaskStkSize-12]=(unsigned char)((global_inc_g>>8)&0x0ff);

  rtos[task_curent].adr_task[OSTaskStkSize-11]=temp_g;
 // rtos[task_curent].adr_task[OSTaskStkSize-12]=init_t;
}
unsigned char comuta_task(void)
{
    if(task_varf==-1)//test daca este goala lista
      return 0;
//nou: task-ul este comutat numai daca este activat!
    do
    {
     if(task_curent>=task_coada)
       task_curent=task_varf-1;
     task_curent++;
     tem++;
    }while(!(rtos[task_curent].activ));
    task_time=rtos[task_curent].nr_timpi;
//initializare stiva (unde se gaseste adresa stivei)
    tem = (unsigned int) ((rtos[task_curent].adr_task)+OSTaskStkSize)>>8;
    t_sph=(unsigned char)(tem&0x00ff);
    t_spl=(unsigned char)((rtos[task_curent].adr_task)+OSTaskStkSize)&0xff;
//initializarea pointer y (unde se gaseste adresa registrilor r29 si r28)
    tem = (unsigned int) ((rtos[task_curent].adr_task)+OSTaskStkSize-2)>>8;
    t_r29=(unsigned char)(tem&0x00ff);
    t_r28=(unsigned char)((rtos[task_curent].adr_task)+OSTaskStkSize-2)&0xff;

    return 1;
} 

 //restaurarea noilor variabile locale
void pop_var(void)
{
  ig=rtos[task_curent].adr_task[OSTaskStkSize-5];
  jg=rtos[task_curent].adr_task[OSTaskStkSize-6];
  kg=rtos[task_curent].adr_task[OSTaskStkSize-7];
  lg=rtos[task_curent].adr_task[OSTaskStkSize-8];
  mg=rtos[task_curent].adr_task[OSTaskStkSize-9];
  ng=rtos[task_curent].adr_task[OSTaskStkSize-10];
  temp_g=rtos[task_curent].adr_task[OSTaskStkSize-11];
}  
  
void start_so(void)
{
 //initializeaza vectorul de taskuri
  task_varf=task_coada=task_curent=-1;//initializeaza index vector taskuri
}     

//functia creaza un task
unsigned char creaza_task(void (*taskf)(void),unsigned char numar)
{
    unsigned int i;
//1.pozitionarea taskului in lista
    if(task_varf==-1)//daca lista este vida
    {
      if(task_coada>=NR_MAX_TASK-1)//daca indexul nu este mai mare decat numarul de task-uri
        return T_EROARE;
      task_coada++;//incrementeaza indexul coada
      task_curent=task_varf=task_coada;
//initializare
    }
    else//daca lista are cel putin un element
    {
      if(task_coada>=NR_MAX_TASK-1)
        return 0;
      task_coada++;
    }
    rtos[task_coada].nr_timpi=numar;//initializare timpi task
    rtos[task_coada].activ=1;//initializare task on/off
    for(i=0;i<OSTaskStkSize;i++)
      rtos[task_coada].adr_task[i]=(unsigned char)i;//initializare stiva in total
//salvarea pointer functie
    i = (unsigned int) taskf;
    rtos[task_coada].adr_task[OSTaskStkSizeHard-1]=(unsigned char)i&0x0ff;//ocmps functie
    rtos[task_coada].adr_task[OSTaskStkSizeHard-2]=(unsigned char)((i>>8)&0x0ff);//ocms functie
//initializarea stivei
    i = (unsigned int) rtos[task_coada].adr_task+OSTaskStkSizeHard-3;
    rtos[task_coada].adr_task[OSTaskStkSize-1]=(unsigned char)i&0xff;
    rtos[task_coada].adr_task[OSTaskStkSize-2]=(unsigned char)(i>>8)&0xff;
//initialiarea pointer y
    i = (unsigned int) rtos[task_coada].adr_task+62;
    rtos[task_coada].adr_task[OSTaskStkSize-3]=(unsigned char)i&0xff;
    rtos[task_coada].adr_task[OSTaskStkSize-4]=(unsigned char)(i>>8)&0xff;

    return 1;
} 
  

//folosita pentru initializarea t_sp si t_r pentru primul task care se va executa
void init_task(void)
{
 //initializare stiva (unde se gaseste adresa stivei)
    task_time=rtos[task_curent].nr_timpi;
    tem = (unsigned int) ((rtos[task_curent].adr_task)+OSTaskStkSize)>>8;
    t_sph=(unsigned char)(tem&0x00ff);
    t_spl=(unsigned char)((rtos[task_curent].adr_task)+OSTaskStkSize)&0xff;
//initializarea pointer y
    tem = (unsigned int) ((rtos[task_curent].adr_task)+OSTaskStkSize-2)>>8;
    t_r29=(unsigned char)(tem&0x00ff);
    t_r28=(unsigned char)((rtos[task_curent].adr_task)+OSTaskStkSize-2)&0xff;
//initialiarea pointer y - la primul task stiva software este pregatita pentru incarcare valori registrii!
    tem = (unsigned int) rtos[task_curent].adr_task+80;
    rtos[task_curent].adr_task[OSTaskStkSize-3]=(unsigned char)tem&0xff;
    rtos[task_curent].adr_task[OSTaskStkSize-4]=(unsigned char)(tem>>8)&0xff;
}  

void PopSP(void)
{
  #asm("cli")
  #asm("  LDS R29,_t_sph");
  #asm("  LDS R28,_t_spl");
  #asm("  LD R16,-Y");
  #asm("  OUT 0x3d,R16");
  #asm("  LD R16,-Y");
  #asm("  OUT 0x3e,R16");

//in plus setare pointer y pentru stiva soft!
  #asm("  LDS R30,_t_r28");
  #asm("  LDS R31,_t_r29");
  #asm("  LD R28,-Z");
  #asm("  LD R29,-Z");

  b_start_so=1;
  #asm("sei")
}