// **************************************************
// Notes
// **************************************************
// Future versions:
//   Debugging Support (console based possibly)
//   Memory optmization
//   Message Passing
//
// Known Bugs:
//
// Potential Bugs:
//
// Revision History:
// 

// **************************************************
// Locks & Other Synchronization
// **************************************************
#pragma optsize-

#if (NUM_LOCKS > 0)
void Lock(unsigned char Number) {
  while(1) {
    #asm("cli");
    #endasm
    if (!Locks[Number]) { Locks[Number] = 1; return; }
    #asm("sei");
  };
}

void Unlock(unsigned char Number) {
  Locks[Number] = 0;
}
#endif

// **************************************************
// Add & Delete Jobs
// **************************************************
#pragma optsize+

void NO_TASK(void) { while(1); }

unsigned char Schedule_Delete_HP(void (*Task)(void)) {
  struct JCB_P *Job_HP;
  struct JCB_P *temp;

  // Find Task Node
  if (Job_Table_Head == NULL) return 0;    
  for(Job_HP = Job_Table_Head; (Job_HP != Job_Table_Tail) &&
    (Job_HP->Task != Task); Job_HP = Job_HP->Next);
  if (Job_HP->Task != Task) return 0;

  if (Job_Table_Head == Job_Table_Tail) {
    Job_Table_Head->Task = NULL;
    Job_Table_Head = NULL;
    Job_Table_Tail = NULL;
    return 1;
  }

  // Delete Node
  if (Job_HP == Job_Table_Head) {
    Job_Table_Head = Job_Table_Head->Next;
  } else if (Job_HP == Job_Table_Tail) {
    Job_Table_Tail = Job_Table_Tail->Prev;
  } else {
    temp = Job_HP->Prev;
    temp->Next = Job_HP->Next;
    temp = Job_HP->Next;
    temp->Prev = Job_HP->Prev;
  }
  Job_HP->Task = NULL;

  return 1;
}

unsigned char Schedule_Delete_HA(void (*Task)(void)) {
  struct Queue_Entry_HA *Job_HA;
  struct Queue_Entry_HA *temp;

  // Find Task Node
  if (Queue_HA_Head == NULL) return 0;    
  for(Job_HA = Queue_HA_Head; ((Job_HA != Queue_HA_Tail) & 0x01) && ((Job_HA->Task) != Task); Job_HA = (Job_HA->Next));
  if ((Job_HA->Task) != Task) return 0;

  if (Queue_HA_Head == Queue_HA_Tail) {
    Queue_HA_Head->Task = NULL;
    Queue_HA_Head = NULL;
    Queue_HA_Tail = NULL;
    return 1;
  }

  // Delete Node
  if (Job_HA == Queue_HA_Head) {
    Queue_HA_Head = Queue_HA_Head->Next;
  } else if (Job_HA == Queue_HA_Tail) {
   Queue_HA_Tail = Queue_HA_Tail->Prev;
  } else {
    temp = Job_HA->Prev;
    temp->Next = Job_HA->Next;
    temp = Job_HA->Next;
    temp->Prev = Job_HA->Prev;
  }
  Job_HA->Task = NULL;

  return 1;
}

unsigned char Schedule_Delete_SP(void (*Task)(void)) {
  struct Queue_Entry_SP *Job_SP;
  struct Queue_Entry_SP *temp;
  void (*Task2)(void);

  // Find Task Node
  if (Queue_SP_Head == NULL) return 0;
  for (Job_SP = Queue_SP_Head; ((Job_SP == Queue_SP_Tail) & 0x01) && ((Job_SP->Task) != Task);
    Job_SP = (Job_SP->Next));
  Task2 = Task;
  if (Task2 != Task) return 0;

  if (Queue_SP_Head == Queue_SP_Tail) {
    (Queue_SP_Head->Task) = NULL;
    Queue_SP_Head = NULL;
    Queue_SP_Tail = NULL; 
    return 1;
  }

  // Delete Node
  if (Job_SP == Queue_SP_Head) {
    Queue_SP_Head = (Queue_SP_Head->Next);
  } else if (Job_SP == Queue_SP_Tail) {
    Queue_SP_Tail = (Queue_SP_Tail->Prev);
  } else {
    temp = (Job_SP->Prev);
    temp->Next = (Job_SP->Next);
    temp = (Job_SP->Next);
    temp->Prev = (Job_SP->Prev);
  }
  (Job_SP->Task) = NULL;

  return 1;
}

unsigned char Schedule_Delete_SA(void (*Task)(void)) {
  struct Queue_Entry_SA *Job_SA;
  struct Queue_Entry_SA *temp;

  // Find Task Node
  if (Queue_SA_Head == NULL) return 0;    
  for(Job_SA = Queue_SA_Head; ((Job_SA != Queue_SA_Tail) & 0x01) && (Job_SA->Task != Task); 
      Job_SA = (Job_SA->Next));
  if ((Job_SA->Task) != Task) return 0;

  if (Queue_SA_Head == Queue_SA_Tail) {
    (Queue_SA_Head->Task) = NULL;
    Queue_SA_Head = NULL;
    Queue_SA_Tail = NULL;
    return 1;
  }

  // Delete Node
  if (Job_SA == Queue_SA_Head) {
    Queue_SA_Head = (Queue_SA_Head->Next);
  } else if (Job_SA == Queue_SA_Tail) {
    Queue_SA_Tail = (Queue_SA_Tail->Prev);
  } else {
    temp = (Job_SA->Prev);
    temp->Next = (Job_SA->Next);
    temp = (Job_SA->Next);
    (temp->Prev) = (Job_SA->Prev);
  }
  (Job_SA->Task) = NULL;

  return 1;
}
   
unsigned char Schedule_Add_HP(void (*Task)(void), unsigned int Deadline, 
  unsigned int Period, void (*After_Task)(void)) {
  struct JCB_P *New_JCB;
  struct JCB_P *temp;

  if (Job_Table_Head == NULL) {
    // Case: Empty Job Table
    New_JCB = &Job_Table[0];
    Job_Table_Head = &Job_Table[0];
    Job_Table_Tail = &Job_Table[0];
  } else {
    unsigned char i;

    // Find Empty Slot in Array
    for (i = 0; (i < MAX_JOBS) && Job_Table[i].Task; i++);
    if (i == MAX_JOBS) return 0;
    New_JCB = &Job_Table[i];
    if (New_JCB->Task != NULL) return 0;

    // Search For Task to Place After
    if ((After_Task == NULL) || (After_Task == Job_Table_Tail->Task)) {
      New_JCB->Prev = Job_Table_Tail;
      Job_Table_Tail->Next = New_JCB;
      Job_Table_Tail = New_JCB;
    } else {
      struct JCB_P *After_Task_JCB;

      // Insert After Task
      for (After_Task_JCB = Job_Table_Head; (After_Task_JCB->Task != 
        After_Task) && (After_Task_JCB != Job_Table_Tail);
        After_Task_JCB = After_Task_JCB->Next);
      if (After_Task_JCB->Task != After_Task) return 0;
      New_JCB->Next = After_Task_JCB->Next;
      New_JCB->Prev = After_Task_JCB;
      temp = New_JCB->Next;
      temp->Prev = New_JCB;
      After_Task_JCB->Next = New_JCB;
    }
  }
  New_JCB->Deadline = Deadline;
  New_JCB->Period = Period;
  New_JCB->Task = Task;
  New_JCB->After_Task = After_Task;

  return 1;
}

unsigned char Schedule_Add_SP(void (*Task)(void), unsigned int Period, 
  void (*After_Task)(void)) {
  return Schedule_Add_HP(Task, NO_DEADLINE, Period, After_Task);
}  
  
unsigned char Schedule_Add_HA(void (*Task)(void), unsigned int Deadline) {
  unsigned int New_Deadline;
  struct Queue_Entry_HA *New_HA;
  struct Queue_Entry_HA *temp;

  // Determine Deadline
  New_Deadline = System_Time + Deadline;
  if (Queue_HA_Head == NULL) {
    // Case: No Queue Entries
    Queue_HA_Head = &Queue_HA[0];
    Queue_HA_Tail = &Queue_HA[0];
    New_HA = &Queue_HA[0];
  } else {
    struct Queue_Entry_HA *Old_HA;
    struct Queue_Entry_HA *j;
    unsigned char i;

    // Place Node in Queue sorted EDF, except for head
    for (Old_HA = Queue_HA_Head; (Old_HA != Queue_HA_Tail) && 
      (((Old_HA->Deadline) - System_Time) < Deadline); Old_HA = (Old_HA->Next));
    for (i = 0; (i < MAX_QUEUE_HA) && (Queue_HA[i].Task); i++);
    if (i == MAX_QUEUE_HA) return 0;
    New_HA = &Queue_HA[i];
    if ((New_HA->Task) != NULL) return 0;
                                 
    // Add Node
    if (Old_HA == Queue_HA_Head) {
      (New_HA->Prev) = Queue_HA_Head;
      (New_HA->Next) = (Queue_HA_Head->Next);
      if (Queue_HA_Head == Queue_HA_Tail) Queue_HA_Tail = New_HA;
      else {
        temp = (Queue_HA_Head->Next);
        (temp->Prev) = New_HA;
      }
      (Queue_HA_Head->Next) = New_HA;
    } else if (Old_HA == Queue_HA_Tail) {
      if (((Old_HA->Deadline) - System_Time) < Deadline) {
        (New_HA->Prev) = Old_HA;
        (Old_HA->Next) = New_HA;
        Queue_HA_Tail = New_HA;
      } else {
        j = (Old_HA->Prev);
        (j->Next) = New_HA;
        (New_HA->Prev) = (Old_HA->Prev);
        (New_HA->Next) = Old_HA;
        (Old_HA->Prev) = New_HA;
      }
    } else {
      j = (Old_HA->Prev);
      (j->Next) = New_HA;
      (New_HA->Prev) = Old_HA->Prev;
      (Old_HA->Prev) = New_HA;
      (New_HA->Next) = Old_HA;
    }
  }
  (New_HA->Task) = Task;
  (New_HA->Deadline) = New_Deadline;

  return 1;
}

unsigned char Schedule_Add_SA(void (*Task)(void)) {
  struct Queue_Entry_SA *New_SA;    

  if (Queue_SA_Head == NULL) {
    // Case: Empty Queue
    Queue_SA_Head = &Queue_SA[0];
    Queue_SA_Tail = &Queue_SA[0];
    New_SA = &Queue_SA[0];
  } else {
    unsigned char i;

    // Insert Node at End of Queue
    for (i = 0; (i < MAX_QUEUE_SA) && (Queue_SA[i].Task); i++);
    if (i == MAX_QUEUE_SA) return 0;
    New_SA = &Queue_SA[i];
    if (New_SA->Task != NULL) return 0;

    Queue_SA_Tail->Next = New_SA;
    New_SA->Prev = Queue_SA_Tail;
    Queue_SA_Tail = New_SA;
  }
  New_SA->Task = Task;
 
  return 1;
}

// **************************************************
// Scheduler
// **************************************************
#pragma optsize-

void Scheduler (void) {
  struct JCB_P *i;
  unsigned char flag;

  // Remove Missed Deadlines if User Desires
  #if (REMOVE_MISSED_D == 1)
  while ((Queue_HA_Head != NULL) && (System_Time >= Queue_HA_Head->Deadline))
    Schedule_Delete_HA(Queue_HA_Head->Task); 
  #endif

  // Determine if Current HP Task is Valid
  flag = 0;  
  if (Current_HP == NULL) { 
    Current_HP = Queue_HP_Head; 
    flag = 1;
  }
  else {
    i = (Current_HP->Job);

    if ((i->Task) == NULL) {
      Current_HP = Queue_HP_Head; 
      flag = 1;
    }
  }

  // Get Next HP Task & Schedule SP Tasks
  do {
    if (!flag) {
      if (Current_HP == Queue_HP_Tail) Current_HP = Queue_HP_Head;
      else Current_HP = (Current_HP->Next);
    }
    i = (Current_HP->Job);
    if (i->Deadline == NO_DEADLINE)
      Schedule_SP(i->Task);
  } while((i->Deadline) == NO_DEADLINE);

  // Schedule Next Task
  if ((i->Task) == NO_TASK) Schedule_Next_Queue();
  else { 
    Context_New = 1;
    Context_Ex_HP = 1;
    Current_Task = (i->Task);
  }

  // Set Next Interrupt + Reset Overflow Counter
  Current_Time = i->Deadline;
  T0_Compare = 0;
}

void Schedule_Next_Queue(void) {
  // Determine Which Task To Run, or Resume One
  if (Context_Ex_HA) return;
  else if (Queue_HA_Head != NULL) { 
    Current_Task = (Queue_HA_Head->Task);
    Context_Ex_HA = 1;
    Context_New = 1;
  } else if (Context_Ex_SP) return;
  else if (Queue_SP_Head != NULL) {
    Current_Task = (Queue_SP_Head->Task);
    Context_Ex_SP = 1;
    Context_New = 1;
  } else if (Context_Ex_SA) return;
  else if (Queue_SA_Head != NULL) {
    Current_Task = (Queue_SA_Head->Task);
    Context_Ex_SA = 1;
    Context_New = 1;    
  } else { 
    Current_Task = NO_TASK;
    Context_Ex_HP = 1;
    Context_New = 1;
  }
}

unsigned char Schedule_SP(void (*Task)(void)) {
  struct Queue_Entry_SP *New_SP;

  // Search for SP Task
  #if (RESCHED_SP == 0)
  for (New_SP = Queue_SP_Head; (New_SP != Queue_SP_Tail) && 
    (New_SP->Task != Task); New_SP = New_SP->Next);
  if (New_SP->Task == Task) return 1;
  #endif
  
  // Case: Empty Queue
  if (Queue_SP_Head == NULL) {
    Queue_SP_Head = &Queue_SP[0];
    Queue_SP_Tail = &Queue_SP[0];
    New_SP = &Queue_SP[0];
  } else {
    unsigned char i;

    // Append Node to End of Queue
    for (i = 0; (i < MAX_QUEUE_SP) && (Queue_SP[i].Task != NULL); i++);
    if (i == MAX_QUEUE_SP) return 0;
    New_SP = &Queue_SP[i];
    if (New_SP->Task != NULL) return 0;

    Queue_SP_Tail->Next = New_SP;
    New_SP->Prev = Queue_SP_Tail;
    Queue_SP_Tail = New_SP;
  }

  New_SP->Task = Task;
  return 1;
}

// **************************************************
// Context Switching
// **************************************************
void Context_Save_State(void) {
  // Return Address
  #asm
  pop r12
  pop r13
  #endasm

  // Stack Pointer
  #asm                         
  in r10, SPL
  in r11, SPH
  #endasm

  // Get New Stacks to Save Context
  #asm
    sts _Context_HP, r16 
  #endasm
  if (Context_Ex_HA) {
    #asm
    ldi r16, LOW(_Context_Regs_HA+CONTEXT_SIZE-1)
    out SPL, r16
    ldi r16, HIGH(_Context_Regs_HA+CONTEXT_SIZE-1)
    out SPH, r16
    #endasm
  } else if (Context_Ex_SP) {
    #asm
    ldi r16, LOW(_Context_Regs_SP+CONTEXT_SIZE-1)
    out SPL, r16
    ldi r16, HIGH(_Context_Regs_SP+CONTEXT_SIZE-1)
    out SPH, r16
    #endasm
  } else if (Context_Ex_SA) {
    #asm
    ldi r16, LOW(_Context_Regs_SA+CONTEXT_SIZE-1)
    out SPL, r16
    ldi r16, HIGH(_Context_Regs_SA+CONTEXT_SIZE-1)
    out SPH, r16
    #endasm
  }
  #asm
    lds r16, _Context_HP
    lds r30, _Context_HP+1
  #endasm

  // Save Context Onto New Stacks
  #asm
  ; Save Stack Pointer
  push r11
  push r10

  ; Compiler
  push r0
  push r1

  ; SREG
  push r9
  #endasm

  #if ((SAVE_REGS == 1) && (REG_ALLOC == 0))
    #asm
    push r14
    push r15
    #endasm
  #endif

  #if ((SAVE_REGS == 1) || (REG_ALLOC == 1))
    #asm
    ; Local Variables
    push r16
    push r17
    push r18
    push r19
    push r20
    push r21
    #endasm
  #endif

  #asm
  ; Compiler
  push r22
  push r23
  push r24
  push r25
  push r26
  push r27
  push r30
  push r31

  ; Data Stack Pointer
  push r29
  push r28
  #endasm

  // Restore Return Address
  #asm
  push r13
  push r12
  #endasm
}

void Context_Set_State(void) {
  // Return Address
  #asm
  pop r12
  pop r13
  #endasm

  if (Context_New) {
    // Case: New Context
    Context_New = 0;

    // Set Stacks
    #asm("sts _Context_HP, r16");      
    if (Context_Ex_HP) {
      #asm
      ldi r16, LOW(_Context_HP+DSTACK_HP+HSTACK_HP-1);
      out SPL, r16
      ldi r16, HIGH(_Context_HP+DSTACK_HP+HSTACK_HP-1);
      out SPH, r16
      
      ldi r16, LOW(_Context_HP+DSTACK_HP-1);
      mov r28, r16
      ldi r16, HIGH(_Context_HP+DSTACK_HP-1);
      mov r29, r16
      #endasm
    } else if (Context_Ex_HA) {
      #asm
      ldi r16, LOW(_Context_HA+DSTACK_HA+HSTACK_HA-1);
      out SPL, r16
      ldi r16, HIGH(_Context_HA+DSTACK_HA+HSTACK_HA-1);
      out SPH, r16
      
      ldi r16, LOW(_Context_HA+DSTACK_HA-1);
      mov r28, r16
      ldi r16, HIGH(_Context_HA+DSTACK_HA-1);
      mov r29, r16
      #endasm
    } else if (Context_Ex_SP) {
      #asm
      ldi r16, LOW(_Context_SP+DSTACK_SP+HSTACK_SP-1);
      out SPL, r16
      ldi r16, HIGH(_Context_SP+DSTACK_SP+HSTACK_SP-1);
      out SPH, r16
      
      ldi r16, LOW(_Context_SP+DSTACK_SP-1);
      mov r28, r16
      ldi r16, HIGH(_Context_SP+DSTACK_SP-1);
      mov r29, r16
      #endasm     
    } else if (Context_Ex_SA) {
      #asm
      ldi r16, LOW(_Context_SA+DSTACK_SA+HSTACK_SA-1);
      out SPL, r16
      ldi r16, HIGH(_Context_SA+DSTACK_SA+HSTACK_SA-1);
      out SPH, r16
      
      ldi r16, LOW(_Context_SA+DSTACK_SA-1);
      mov r28, r16
      ldi r16, HIGH(_Context_SA+DSTACK_SA-1);
      mov r29, r16
      #endasm
    }
    #asm("lds r16, _Context_HP");

    // Reset SREG
    #asm
    ldi r16, INIT_SREG
    mov r9, r16
    #endasm

    // Load Task
    #asm
    lds r10, _Current_Task
    lds r11, _Current_Task+1
    push r10
    push r11
    #endasm
  } else {
    // Case: Restoring Context

    // Swtich to Stack Containing Saved Context
    if (Context_Ex_HA) {
      #asm
      ldi r16, LOW(_Context_Regs_HA);
      mov r10, r16
      ldi r16, HIGH(_Context_Regs_HA);
      mov r11, r16      
      #endasm
    } else if (Context_Ex_SP) {
      #asm
      ldi r16, LOW(_Context_Regs_SP);
      mov r10, r16
      ldi r16, HIGH(_Context_Regs_SP);
      mov r11, r16
      #endasm
    } else if (Context_Ex_SA) {
      #asm
      ldi r16, LOW(_Context_Regs_SA);
      mov r10, r16
      ldi r16, HIGH(_Context_Regs_SA);
      mov r11, r16
      #endasm
    }

    // Restore Context
    #asm
    out SPL, r10
    out SPH, r11

    pop r28
    pop r29

    pop r31
    pop r30
    pop r27
    pop r26
    pop r25
    pop r24
    pop r23
    pop r22
    #endasm

    #if ((SAVE_REGS == 1) || (REG_ALLOC == 1))
      #asm
      pop r21
      pop r20
      pop r19
      pop r18
      pop r17
      pop r16
      #endasm
    #endif

    #if ((SAVE_REGS == 1) && (REG_ALLOC == 0))
      #asm
      push r15
      push r14
      #endasm
    #endif

    #asm
      pop r9
      pop r1
      pop r0

      pop r10
      pop r11
      out SPH, r11
      out SPL, r10
    #endasm
  }

  // Restore Resturn Address
  #asm
  push r13
  push r12
  #endasm
}

void Context_Set_OS_State(void) {
  #asm
  pop r12
  pop r13
  #endasm

  // Set OS to use Stacks from HP, since they don't save context
  #asm
  ; Set Data & Stack Pointers
  ldi r28, LOW(_Context_HP+DSTACK_HP+HSTACK_HP-1)
  ldi r29, HIGH(_Context_HP+DSTACK_HP+HSTACK_HP-1)
  out SPH, r29
  out SPL, r28
  ldi r28, LOW(_Context_HP+DSTACK_HP-1)
  ldi r29, HIGH(_Context_HP+DSTACK_HP-1)
  #endasm

  #asm
  push r13
  push r12
  #endasm
}

#pragma savereg-

interrupt [TIM0_OVF] void System_Timer(void) {
  // No Context Switch: 19 <= Cycles <= 21
  
  #asm
  in r9, SREG

  ; System_Time++ && T0_Compare++
  clr r10
  clr r11
  inc r11
  add r3, r11
  adc r4, r10  
  add r5, r11
  adc r6, r10

  ; if (T0_Compare > Current_Time) context_switch else dont_context_switch
  cp r8, r6
  brsh NO_CONTEXT_SWITCH
  brne GO
  cp r7, r5
  brsh NO_CONTEXT_SWITCH
  clr r5
  clr r6
  
  GO:
    sts _Context_HP+1, r30 ; bit regs overwrite this reg
  #endasm

  if (!Context_Ex_HP) Context_Save_State();
  else Context_Ex_HP = 0;

  Context_Set_OS_State();
  Scheduler();
  Context_Set_State();

  #asm  
  NO_CONTEXT_SWITCH:
  out SREG, r9
  #endasm
}

#pragma savereg+

void Job_Return(void) {
  unsigned char r;
                             
  r = 1;
  // Delete Task From Queues
  if (Context_Ex_HP && (Current_Task != NO_TASK)) {
    Context_Ex_HP = 0;
  } else if (Context_Ex_HA) {
    r = Schedule_Delete_HA(Queue_HA_Head->Task);
    Context_Ex_HA = 0;
  } else if (Context_Ex_SP) {
    r = Schedule_Delete_SP(Queue_SP_Head->Task);
    Context_Ex_SP = 0;
  } else if (Context_Ex_SA) {
    r = Schedule_Delete_SA(Queue_SA_Head->Task);
    Context_Ex_SA = 0;
  }
  if (!r) { 
      // **
    return;
  }

  Context_Set_OS_State();

  // Remove Missed Deadlines if Desired
  #if (REMOVE_MISSED_D == 1)
  while ((Queue_HA_Head != NULL) && (System_Time >= (Queue_HA_Head->Deadline)))
    Schedule_Delete_HA(Queue_HA_Head->Task); 
  #endif

  Schedule_Next_Queue();
  Context_Set_State();

  #asm("reti");
}

#pragma optsize+

// **************************************************
// Initialization
// **************************************************
void Init_OS(void) {
  #asm
  pop r10
  pop r11
  #endasm

  // Set Timers &External RAM
  TIMSK = INIT_TIMSK;
  MCUCR = INIT_MCUCR;
  TCCR0 = INIT_TCCR0;

  // Reduce Warning Generated by Compiler
  FILL1 = 0;
  FILL2 = 0;
  FILL3 = 0;
  Context_HP[0] = 0;
  Context_HA[0] = 0;
  Context_SP[0] = 0;
  Context_SA[0] = 0;
  Context_Regs_HA[0] = 0;
  Context_Regs_SA[0] = 0;
  Context_Regs_SP[0] = 0;
  OS_RESERVED_1 = 0;
  OS_RESERVED_4 = 0;
  OS_RESERVED_5 = 0;

  // Enable UART
  #if (DEBUG == 1)
    UCR = 0x10 + 0x08;
    UBRR = 25;
  #endif

  Context_Set_OS_State();

  #asm
  push r11
  push r10
  #endasm
}

void Init_Start(void) {
  Schedule_Compute();

  OS_RESERVED_2 = 0;
  OS_RESERVED_3 = 0;

  // Jump to First Task
  Scheduler();
  Context_Set_State();
  #asm
  pop r31
  pop r30
  sei
  ijmp
  #endasm
}

// **************************************************
// Creates a hard period schedule
// **************************************************
/*Determines the least common multiple of the periods of the jobs
 *in the periodic job table.
 */
long LeastCommonMult(void) {
  long multiple1, multiple2, lastPeriod;
  int period;
  struct JCB_P *job;   

  job = &Job_Table[0];
  lastPeriod = job->Period;

  //iterate through the job table
  while (job != Job_Table_Tail) {
    job = (job->Next);
    period = (job->Period);

    if ((job->Period) > 0) {
      multiple1 = lastPeriod;
      multiple2 = period;

      while (multiple1 != multiple2) {
        if (multiple1 < multiple2) multiple1 = multiple1 + lastPeriod;
        else multiple2 = multiple2 + period;
      }

      lastPeriod = multiple1;
    }
  }

  return lastPeriod;
}


/*
 *Each time a new schedule is computed, we must first remove all of
 *the no_task jobs we created and scheduled, because the were just fillers,
 *so they do not need to be factored into the new schedule.
 *
 *A spacer job is a job that was scheduled to do nothing so that the hard
 *periodic queue would have a task prepared at all times.  When the os chooses
 *a task to run, it looks for any other tasks (soft periodic, or aperiodic) to 
 *run before executing the no_task.
 *
 * The spacer tasks are always added to the beginning of the job_table so we
 * can assume that once we reach a user's job, there are no more no_tasks
 * further in the list.
 */
inline void removeSpacerJobs(void) {
  struct JCB_P *jIndex;
  struct JCB_P *jIndexDelete;

  //if the first job does not correspond to a spacer tasks, there are no
  //spacer tasks to remove.
  if ((Job_Table_Head->Task) != NO_TASK) return;

  
  jIndex = Job_Table_Head;
  //iterate through the job table to find the first user job.
  while ((jIndex != Job_Table_Tail) && (jIndex->Task == NO_TASK)) 
    jIndex = (jIndex->Next);

  jIndexDelete = Job_Table_Head;
  //remove all the jobs before the first user job
  while (jIndexDelete != jIndex) {
    (jIndexDelete->Task) = NULL;
    jIndexDelete = (jIndexDelete->Next);
  }

  //if everything in the table was a no task
  if ((Job_Table_Tail == jIndex) && ((Job_Table_Tail->Task) == NULL)) {
    (jIndex->Task) = NULL;
    Job_Table_Head = &Job_Table[0];
    Job_Table_Tail = &Job_Table[0];
  }
  else Job_Table_Head = jIndex;
}

/*
 *Removes all the elements of the hard periodic schedule, by setting their job
 *pointer to 0.
 */
void clearSchedule() {
  unsigned char i;

  for (i=0; i < MAX_QUEUE_HP; i++) Queue_HP[i].Job=NULL;
}

/**
 *Computes a schedule for the hard and soft periodic tasks.
 *Returns 1 if the jobs are schedulable.  If this is true, the schedule will
 *be stored in the hard periodic queue.
 *Returns 0 if the jobs aren't schedulable.
 *
 */
unsigned char Schedule_Compute(void){
  struct Queue_Entry_HP *qCurrent;
  struct Queue_Entry_HP *qNew; 
  struct Queue_Entry_HP *qIndex;
  struct Queue_Entry_HP *qLast;
  struct Queue_Entry_HP *qNext;   
  struct Queue_Entry_HP *qTemp;

  struct JCB_P *job;
  struct JCB_P *jCurrent; 
  long H, offset, theta, space;

  int newSpace;
  unsigned char i, newIndex;
  unsigned char Ri;
  unsigned char finishOff;

  removeSpacerJobs();
  clearSchedule();
  H = LeastCommonMult();                            //get the hyperperiod
  job = Job_Table_Head;
 
  if (Job_Table_Head == NULL) return 0;
  if (Job_Table_Tail == NULL) return 0;

  qIndex = &Queue_HP[0];
  Queue_HP_Head = &Queue_HP[0];

  Ri = H / (job->Period);               //# of times the job must be
 				 	//scheduled within the hyperperiod
  
  //Schedule the first task (1st occurrence) of the first job.
  qLast = &Queue_HP[0];
  (qLast->Start) = 0;
  (qLast->End) = (job->Deadline);
  (qLast->Job) = Job_Table_Head;                                                                       
  offset = (job->Period);
  qNext = qLast;

  if (job == Job_Table_Tail) Queue_HP_Tail = Queue_HP_Head;

  //schedule the rest of the tasks for the first job.
  for (i = 1; i < Ri; i++) {
    qNext = &Queue_HP[i];
    (qNext->Start) = offset;
    (qNext->End) = offset + (job->Deadline);

    (qLast->Next) = qNext;
    (qNext->Prev) = qLast;

    (qNext->Job) = job;
    offset = offset + (job->Period);

    qLast = qNext;
  }

  qNext = &Queue_HP[i];
  (qNext->Start) = H;
  (qNext->End) = H;
   
  (qNext->Prev) = qLast;
   
  (qLast->Next) = qNext;
  (qNext->Prev) = qLast;
   
  (qNext->Job) = 1;   

  Queue_HP_Tail = qNext;
  
  job = Job_Table_Head;
   
  //while there exist more jobs in the job table
  while (job != &Job_Table_Tail[0]) {
    job = (job->Next);
    Ri = H / (job->Period);
	    
    qCurrent = qIndex;
	
    //the start time of the first task of the ith job.
    theta = qCurrent->End + SCHEDULE_TIME;
    offset = theta; 

    //repeat until a valid schedule is found for the ith job.
    //a 0 will be returned if one doesn't exist.
    while(1) {
      offset = theta;

      for (i = 0; i < Ri; i++) {
        while (((qCurrent->End) + SCHEDULE_TIME) <= offset) {
          if (qCurrent == Queue_HP_Tail) return 0;
          qCurrent = (qCurrent->Next);
        }

        if (((offset + job->Deadline + SCHEDULE_TIME) > (qCurrent->Start)) ||
          (offset == (qCurrent->Start))) {			
          theta = theta + qCurrent->End + SCHEDULE_TIME - offset;
          qCurrent = qIndex;

          if (theta > (job->Period))
            return 0;
          break;
        }
        
        offset = offset + (job->Period);
      }

      if (i >= Ri) break;
    }

    //if we got here, there exists a valid schedule for the ith job,  so 
    //now, place the tasks of job i into the periodic queue.
    qCurrent = qIndex;
	offset = theta;
	finishOff = 0;
		
    for (i = 0; i < Ri; i++) {		    
      while (((qCurrent->End) + SCHEDULE_TIME) <= offset) {
        if (qCurrent == Queue_HP_Tail) return 0;
        else qCurrent = (qCurrent->Next);
      }
    }		

    for (newIndex = 0; (newIndex < MAX_QUEUE_HP) && 
      Queue_HP[newIndex].Job; newIndex++);
			
    if (newIndex == MAX_QUEUE_HP)
	  return 0;

    qNew = &Queue_HP[newIndex];
			
    (qNew->Job) = job;
    (qNew->Start) = offset;
    (qNew->End) = offset + (job->Deadline);
			
    (qNew->Next) = qCurrent;
    (qNew->Prev) = (qCurrent->Prev);
			   
    qTemp = (qCurrent->Prev);
    (qTemp->Next) = qNew;
    (qCurrent->Prev) = qNew;
    offset = offset + (job->Period);
  }
		
  qIndex = (qIndex->Next);

  qLast = Queue_HP_Head;
  qNext = qLast;
  
  //go through and fill all the untaken spaces in the queue with spacer tasks-
  //tasks that run while(1) loops.
  while(qLast != Queue_HP_Tail) {
    qNext = (qLast->Next);
    space = (qNext->Start) - (qLast->End);
    
    while (space > (SCHEDULE_TIME + SCHEDULE_TIME)) {
      for (newIndex = 0; (newIndex < MAX_QUEUE_HP) && 
        Queue_HP[newIndex].Job; newIndex++);

      if (newIndex == MAX_QUEUE_HP) return 0;

      qCurrent = &Queue_HP[newIndex];

      if (space > MAX_SPACE) {
        newSpace = MAX_SPACE;
        space = space - MAX_SPACE;
      } else {
        newSpace = space;
        space=0;
      }

      jCurrent = Job_Table_Head;
       
      while (((jCurrent->Task) == NO_TASK) && ((jCurrent->Deadline) != newSpace))
        jCurrent = (jCurrent->Next);

      if ((jCurrent->Task) == NO_TASK)
        (qCurrent->Job) = jCurrent;
      else { 
        for (newIndex = 0; (newIndex < MAX_JOBS) && 
          Job_Table[newIndex].Task; newIndex++);
        if (newIndex == MAX_JOBS)
        return 0;  

        jCurrent = &Job_Table[newIndex];
        (jCurrent->Next) = Job_Table_Head;
        (Job_Table_Head->Prev) = jCurrent;
        Job_Table_Head = jCurrent;
        (Job_Table_Head->Task) = NO_TASK;
        (jCurrent->Deadline) = newSpace;
        (qCurrent->Job) = Job_Table_Head;      
      }

      (qLast->Next) = qCurrent;
      (qNext->Prev) = qCurrent;
      (qCurrent->Next) = qNext;       
    }

    qLast = qNext;     
  }

  (Queue_HP_Tail->Job) = 0;
  Queue_HP_Tail = (Queue_HP_Tail->Prev);

  //A;; the tasks have been scheduled and exists in Queue_HP
  return 1; 
}