#include <syscall-handler.h>
#include <context-switch.h>
#include <priority-q.h>
#include <td.h>
#include <types.h>
#include <scheduler.h>
#include <bwio.h>

void handlerCreate(KernelStruct* Colonel) {
  
  int *SP_Usr = Colonel->Active->sp;
  bwprintf(COM2,"SP in handel %d\n\r", SP_Usr);
  //asm volatile("ldmfd r3!,{r0-r2,r4}\n\t");
  int arg0 = *(SP_Usr+7);
  int arg1 = *(SP_Usr+8);
  int arg2 = *(SP_Usr+9);
  int arg3 = *(SP_Usr+10);
  //bwprintf(COM2,"%d\n\r",arg0);
  //bwprintf(COM2,"%d %d\n\r",SP_Usr,(1+SP_Usr+3));
  bwprintf(COM2,"%d %d %d %d \n\r",arg0,arg1,arg2,arg3);
  
  PrioLvl P = (PrioLvl) (arg0);
  TD* NewTask = CreateTask(Colonel,P,arg1);
  pushToScheduler(Colonel, NewTask);
  Colonel->Active->RetVal = NewTask->TaskID;
}


void Handle(KernelStruct* Colonel, int n) {
  bwprintf(COM2,"n=%d\n\r",n);
  //Colonel->Active->sp = TaskSP;
  
  int *SP_Usr = Colonel->Active->sp;
  switch(n) {
  case SYS_Create:
    //bwprintf(COM2,"before, Args = %d\n\r",Args);
    handlerCreate(Colonel); 
    break;
  case SYS_MyTid:
    (Colonel->Active)->RetVal = (Colonel->Active)->TaskID;
    pushToScheduler(Colonel,Colonel->Active);
    Colonel->Active = NULL;
    break;
  case SYS_ParentTid:
    (Colonel->Active)->RetVal = (Colonel->Active)->ParentID;
    pushToScheduler(Colonel,Colonel->Active);
    Colonel->Active = NULL;
    break;
  case SYS_Pass:
    pushToScheduler(Colonel,Colonel->Active);
    Colonel->Active = NULL;
    break;
  case SYS_Exit:
    (Colonel->Active)->TaskState = Zombie;
    Colonel->Active = NULL;
    break;
  }
}

int Activate(KernelStruct *Colonel,TD* Task, int RetVal) {
  if(Colonel->Active == NULL){
    Colonel->Active = Task;
  }
  bwprintf(COM2,"Activate: before kexit, sp = %d\n\r",Colonel->Active->sp);
  kerxit(Task->sp, RetVal);
  register int TaskSP asm("r1");
  ((Colonel->Active)->sp) =(int *) (TaskSP);
  register int r0 asm("r0");
  int copy = r0;
  bwprintf(COM2,"Stack Pointer%d\n\r",TaskSP);
  bwprintf(COM2,"Back to activate, copy=%d\n\r",copy);
  return copy;
  //return r0;
}