#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");
  AP *Args = Colonel->Active->Args;
  int A0 = (int)Args->arg0;
  int A1 = (int)Args->arg1;
  int A2 = (int)Args->arg2;
  int A3 = (int)Args->arg3;
  //((Colonel->Active)->sp) ++;
  //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",A0,A1,A2,A3);
  
  PrioLvl P = (PrioLvl) (A0);
  TD* NewTask = CreateTask(Colonel,P,(void *)A1);
  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;
  }
}

void fakeExit(TD* Task){
  asm volatile("ldmfd r0, {r0-r3}\n\t");
  register int r1 asm("r0");
  int one = r1;
  register int r2 asm("r1");
  int lr = r2;
  register int r3 asm("r2");
  int two = r3;
  register int r4 asm("r3");
  int zero = r4;

  bwprintf(COM2,"Fake Exit:r1 %d, lr %d, r2 %d, r0 %d\n\r",one,lr,two,zero);
}

int Activate(KernelStruct *Colonel,TD* Task) {
  if(Colonel->Active == NULL){
    Colonel->Active = Task;
  }
  //fakeExit(Task);
  //bwprintf(COM2,"Activate: sp %d, lr %d\n\r",Task->sp,Task->lr);
  kerxit(Task);
  //register int r1 asm("r1");
  //((Colonel->Active)->Arg) =(AP *)(r1);
  register int r0 asm("r0");
  int SWI = r0;
  //bwprintf(COM2,"Argument: %d\n\r",TaskSP);
  //bwprintf(COM2,"Back to activate, SWI=%d,Lr = %d\n\r",SWI,Task->TaskID);
  return SWI;
  //return r0;
}