#include <scheduler.h>
#include <priority-q.h>
#include <td.h>
#include <bwio.h>
// int schedulerInit() {
//     return 0;
// } // add the IDLE task with lowest priority and first USER task maybe


//extern int PushToTDPQ(TDPQ* PQ, TD* Task);

int pushToScheduler (KernelStruct* Colonel, TD* Task) {
  int returncode= PushToTDPQ(&(Colonel->ArrayPQ)[Task->TaskPriority],Task);
  //bwprintf(COM2,"return code of push onto tdpq %d\n\r",returncode);
  return returncode;
}

TD* getNextTaskScheduler (KernelStruct* Colonel) {
  TD *Task;
  TD *First;
  int i = 0;
  TDPQ *PQ;
  int Len;
  for(;i<MAX_NUM_PRIORITY;i++){
    //Temp = NULL;
    PQ = &(Colonel->ArrayPQ)[i];
    if(TDPQReady(PQ) && PQ->Length >0){
      Len = PQ->Length;
      First = NULL;
      Task = TDPQGetStart(PQ);
      TDPQPopStart(PQ);
      while ((Len > 0) && Task->TaskState != Ready) {
	if(First == Task) break;
	if(First == NULL) First = Task;
	PushToTDPQ(PQ, Task);
	Task = TDPQGetStart(PQ);
	TDPQPopStart(PQ);
	Len --;
      }
      if(Task != First){
	bwprintf(COM2,"scheduler: picked Task%d, state:%d\n\r",Task->TaskID,Task->TaskState);
	return Task;
      }
    }
  }
  return NULL;
}