//*******************final user main DEMONTRATE THIS TO TA**********
int main(void){ 

  // Set the clocking to run from PLL at 50 MHz 
  SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_8MHZ);

  OS_Init();           // initialize, disable interrupts

  DataLost = 0;        // lost data between producer and consumer
  NumSamples = 0;
  MaxJitter = 0;       // OS_Time in 20ns units
  MinJitter = 10000000;

//********initialize communication channels
  OS_MailBox_Init();
  OS_Fifo_Init(32);    // ***note*** 4 is not big enough*****

//*******attach background tasks***********
  OS_AddButtonTask(&ButtonPush,2);
  
  OS_AddPeriodicThread(&DAS,PERIOD,0); // 2 kHz real time sampling

  NumCreated = 0 ;
// create initial foreground threads
  NumCreated += OS_AddThread(&Interpreter,128,2); 
  NumCreated += OS_AddThread(&Consumer,128,1); 
  NumCreated += OS_AddThread(&PID,128,3);
  OS_Launch(TIMESLICE); // doesn't return, interrupts enabled in here
  return 0;             // this never executes
}

//*******************final user main DEMONTRATE THIS TO TA**********
int main(void){        // lab 3 real main
  OS_Init();           // initialize, disable interrupts

  DataLost = 0;        // lost data between producer and consumer
  NumSamples = 0;

//********initialize communication channels
  OS_MailBox_Init();
  OS_Fifo_Init(32);    // ***note*** 4 is not big enough*****

//*******attach background tasks***********
  OS_AddButtonTask(&ButtonPush,2);
  OS_AddDownTask(&DownPush,3);
  OS_AddPeriodicThread(&DAS,1,PERIOD,0); // 2 kHz real time sampling

  NumCreated = 0 ;
// create initial foreground threads
  NumCreated += OS_AddThread(&Interpreter,128,2); 
  NumCreated += OS_AddThread(&Consumer,128,1); 
  NumCreated += OS_AddThread(&PID,128,3);

 
  OS_Launch(TIMESLICE); // doesn't return, interrupts enabled in here
  return 0;             // this never executes
}

//*******************final user main DEMONTRATE THIS TO TA**********
int main(void){ 
  OS_Init();           // initialize, disable interrupts
  PortE_Init();
  DataLost = 0;        // lost data between producer and consumer
  NumSamples = 0;
  MaxJitter = 0;       // in 1us units

//********initialize communication channels
  OS_MailBox_Init();
  OS_Fifo_Init(128);    // ***note*** 4 is not big enough*****
	ST7735_InitR(INITR_REDTAB);				   // initialize LCD
	UART_Init();              					 // initialize UART
//*******attach background tasks***********
  OS_AddSW1Task(&SW1Push,2);
//  OS_AddSW2Task(&SW2Push,2);  // add this line in Lab 3
  ADC_Init(4);  // sequencer 3, channel 4, PD3, sampling in DAS()
  OS_AddPeriodicThread(&DAS,PERIOD,1); // 2 kHz real time sampling of PD3

  NumCreated = 0 ;
// create initial foreground threads
  NumCreated += OS_AddThread(&Interpreter,128,2); 
  NumCreated += OS_AddThread(&Consumer,128,1); 
  NumCreated += OS_AddThread(&PID,128,3);  // Lab 3, make this lowest priority
 
  OS_Launch(TIME_2MS); // doesn't return, interrupts enabled in here
  return 0;            // this never executes
}

//************ButtonPush*************
// Called when Select Button pushed
// background threads execute once and return
void ButtonPush(void){
  if(Running==0){
    Running = 1;  // prevents you from starting two robot threads
    NumCreated += OS_AddThread(&Robot,128,1);  // start a 20 second run
	NumCreated += OS_AddThread(&IdleTask,128,1);  // start a 20 second run
  }
}

int testmain1(void){ 
  OS_Init();           // initialize, disable interrupts
  NumCreated = 0 ;
  NumCreated += OS_AddThread(&Thread1,128,1); 
  NumCreated += OS_AddThread(&Thread2,128,2); 
  NumCreated += OS_AddThread(&Thread3,128,3); 
 
  OS_Launch(TIMESLICE); // doesn't return, interrupts enabled in here
  return 0;             // this never executes
}

//******************* test main2 **********
// SYSTICK interrupts, period established by OS_Launch
// Timer interrupts, period established by first call to OS_AddPeriodicThread
int testmain2(void){ 
  OS_Init();           // initialize, disable interrupts
  
  NumCreated = 0 ;
// create initial foreground threads
  NumCreated += OS_AddThread(&TestFile,128,1);  
  NumCreated += OS_AddThread(&IdleTask,128,3); 
 
  OS_Launch(10*TIME_1MS); // doesn't return, interrupts enabled in here
  return 0;               // this never executes
}

//------------cmdPing-----------------
// Control the ping sensors.
void cmdPing(void){
	if(FirstRun){                                            // only allowed to add ping threads once
		FirstRun = 0;			                           
		OS_AddThread(&Ping1, 128, 1);                          // sends a pulse to the ping sensor #1, 10 times a second
		OS_AddThread(&PingDisplay, 128, 6);                    // display ping results
		printf("Ping sensors added. Displaying data to ST7735.\n\r");
	}
	else{
		printf("Error, Ping sensors already added.\n\r");
	}	
}

int testmain1(void){       // testmain1
  OS_Init();          // initialize, disable interrupts
  NumCreated = 0 ;
  NumCreated += OS_AddThread(&Thread1,128,1); 
  NumCreated += OS_AddThread(&Thread2,128,2); 
  NumCreated += OS_AddThread(&Thread3,128,3); 
  // Count1 runs, Count2 and Count3 are 0 because of starvation
  // change priority to equal to see round robin
  OS_Launch(TIMESLICE); // doesn't return, interrupts enabled in here
  return 0;             // this never executes
}

int testmain5(void){       // Testmain5
  volatile unsigned long delay;
  OS_Init();           // initialize, disable interrupts
  NumCreated = 0 ;
  NumCreated += OS_AddThread(&Thread6,128,2); 
  NumCreated += OS_AddThread(&Thread7,128,1); 
  OS_AddPeriodicThread(&TaskA,(TIME_1MS*111)/100,0);           // 1 ms, higher priority
  OS_AddPeriodicThread(&TaskB,TIME_1MS,1);         // 2 ms, lower priority
  OS_Launch(TIMESLICE); // 2ms, doesn't return, interrupts enabled in here
  return 0;             // this never executes
}

int main2(void)
{
  #if PROFILING == 1
    volatile unsigned long delay;
  #endif
	/* Initialize 8MHz clock */
	SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_XTAL_8MHZ | SYSCTL_OSC_MAIN);
  OLED_Init(15);
  ADC_Init(1000);
  ADC_Open(0);
  ADC_Open(1);
  SH_Init();
  OS_Init();
  OS_MailBox_Init();
  SH_Init();
  
  //********initialize communication channels
  OS_MailBox_Init();
  OS_Fifo_Init(32);
  
  NumCreated = 0;
  NumSamples = 0;
  MaxJitter = 0;       // OS_Time in 20ns units
  MinJitter = 10000000;
  
  #if PROFILING
    // intialize port b pins as specified by PINS mask
    // for digital output for use in profiling threads
    SYSCTL_RCGC2_R |= SYSCTL_RCGC2_GPIOB;
    delay = SYSCTL_RCGC2_R;
    GPIO_PORTB_DIR_R |= PINS;
    GPIO_PORTB_DEN_R |= PINS;
    GPIO_PORTB_DATA_R &= ~PINS;
  #endif
  
  // testing/debugging stuff
  OS_Add_Periodic_Thread(&DAS,2,1);
//  OS_AddButtonTask(&dummyButtonTask, 1);
  OS_AddButtonTask(&ButtonPush, 1);
  OS_AddDownTask(&ButtonPush, 1);
  
//   NumCreated += OS_AddThread(&jerkTask, 0, 6);
//   NumCreated += OS_AddThread(&dummyTask3, 0, 7);
//   NumCreated += OS_AddThread(&dummyTask1, 0, 2);
  NumCreated += OS_AddThread(&PID, 128, 5);
//   NumCreated += OS_AddThread(&dummyTask2, 0, 2);
  NumCreated += OS_AddThread(&Consumer, 128, 0);
  NumCreated += OS_AddThread(&SH_Shell, 128, 6);
  OS_Launch(TIMESLICE);
	
	/* Loop indefinitely */
  while(1);
}

// --------------------------- OS_Launch -----------------------------
void OS_Launch(unsigned long timeSlice){
	SysTick_InitInt(timeSlice);                     //For preemptive context switching
	OS_AddPeriodicThread(&RunTimeUpdate, 2, 128);  
	OS_AddPeriodicThread(&DecrementSleep, 2, 1);
  OS_AddThread(&NoCrash,128,7); //to guarantee there is always a thread in the actives list	
  OS_AddThread(&OS_Display,128,1);          
	OS_AddThread(&OS_ProcessInterpreter,128,3);
	
	RunPt = Actives;
	StartOS(); 
  EnableInterrupts();
}

int testmain3(void){   // Testmain3
  Count4 = 0;          
  OS_Init();           // initialize, disable interrupts
// Count2 + Count5 should equal Count1 (Count5 may be zero)
  NumCreated = 0 ;
  OS_AddButtonTask(&BackgroundThread5c,2);
  NumCreated += OS_AddThread(&Thread2c,128,3); 
  NumCreated += OS_AddThread(&Thread3c,128,3); 
  NumCreated += OS_AddThread(&Thread4c,128,3); 
  OS_Launch(TIMESLICE); // doesn't return, interrupts enabled in here
  return 0;  // this never executes
}

int testmain4(void){   // Testmain4
  Count4 = 0;          
  OS_Init();           // initialize, disable interrupts
  NumCreated = 0 ;
  OS_AddPeriodicThread(&BackgroundThread1d,1,PERIOD,0); 
  OS_AddButtonTask(&BackgroundThread5d,2);
  NumCreated += OS_AddThread(&Thread2d,128,2); 
  NumCreated += OS_AddThread(&Thread3d,128,3); 
  NumCreated += OS_AddThread(&Thread4d,128,3); 
  OS_Launch(TIMESLICE/16); // doesn't return, interrupts enabled in here
  return 0;  // this never executes
}

int testmain2(void){  // testmain2
  OS_Init();           // initialize, disable interrupts
  NumCreated = 0 ;
  NumCreated += OS_AddThread(&Thread1b,128,1); 
  NumCreated += OS_AddThread(&Thread2b,128,1); 
  NumCreated += OS_AddThread(&Thread3b,128,3);
  OS_AddPeriodicThread(&Dummy1, PERIOD, 1);
  OS_AddPeriodicThread(&Dummy2, PERIOD, 2); 
 
  OS_Launch(TIMESLICE); // doesn't return, interrupts enabled in here
  return 0;             // this never executes
}

// *********** OS_AddThreads **************
// add three foreground threads to the scheduler
// Inputs: three pointers to a void/void foreground tasks
// Outputs: 1 if successful, 0 if this thread can not be added
int OS_AddThreads(void(*task0)(void), void(*task1)(void), void(*task2)(void)) {

  int result = 0;
  result += OS_AddThread(task0, STACKSIZE, 0);
  result += OS_AddThread(task1, STACKSIZE, 0);
  result += OS_AddThread(task2, STACKSIZE, 0);

  if(result == 3) {
    return 1;
  } else {
    return 0;
  }
}

//******************* test main2 **********
// SYSTICK interrupts, period established by OS_Launch
// Timer interrupts, period established by first call to OS_AddPeriodicThread
int testmain2(void){ 
  OS_Init();           // initialize, disable interrupts

//*******attach background tasks***********
  OS_AddPeriodicThread(&disk_timerproc,10*TIME_1MS,0);   // time out routines for disk
  
  NumCreated = 0 ;
// create initial foreground threads
  NumCreated += OS_AddThread(&TestFile,128,1);  
  NumCreated += OS_AddThread(&IdleTask,128,3); 
 
  OS_Launch(10*TIME_1MS); // doesn't return, interrupts enabled in here
  return 0;               // this never executes
}

int Testmain5(void){       // Testmain5
  volatile unsigned long delay;
  SYSCTL_RCGC2_R |= SYSCTL_RCGC2_GPIOB; // activate port B
  delay = SYSCTL_RCGC2_R;     // allow time to finish activating
  GPIO_PORTB_DIR_R |= 0x07;   // make PB2, PB1, PB0 out
  GPIO_PORTB_DEN_R |= 0x07;   // enable digital I/O on PB2, PB1, PB0
  OS_Init();           // initialize, disable interrupts
  NumCreated = 0 ;
  NumCreated += OS_AddThread(&Thread6,128,2); 
  NumCreated += OS_AddThread(&Thread7,128,1); 
  OS_AddPeriodicThread(&TaskA,TIME_1MS,0);           // 1 ms, higher priority
  OS_AddPeriodicThread(&TaskB,2*TIME_1MS,1);         // 2 ms, lower priority
 
  OS_Launch(TIMESLICE); // 2ms, doesn't return, interrupts enabled in here
  return 0;             // this never executes
}

//************ButtonPush*************
// Called when Select Button pushed
// Adds another foreground task
// background threads execute once and return
void ButtonPush(void){
  if(OS_AddThread(&ButtonWork,100,1)){
    NumCreated++; 
  }
  OS_Kill();
  OS_Delay(OS_ARBITRARY_DELAY);
}

//************SW2Push*************
// Called when SW2 Button pushed, Lab 3 only
// Adds another foreground task
// background threads execute once and return
void SW2Push(void){
  if(OS_MsTime() > 20){ // debounce
    if(OS_AddThread(&ButtonWork,100,4)){
      NumCreated++; 
    }
    OS_ClearMsTime();  // at least 20ms between touches
  }
}

int main(void){
    OS_Init();
    LEDsInit();
    switchesInit(&leftSwitch, &rightSwitch, SWITCH_PRIO);
    SBInit(SYS_CLK_FREQ/SBUS_FREQ, SBUS_ID, SBUS_PRIO);
    OS_AddThread(&IdleTask, 128, 7);
    OS_Launch(SYS_CLK_FREQ/OS_FREQ);
    return 0;
}