#include "scheduler.h"
#include <MeAurigaLab.h>

MeLineFollower lineSensor(PORT_9);
MeUltrasonicSensor opticalSensor(PORT_10);

TaskHandle_t xHandle1 = NULL;
TaskHandle_t xHandle2 = NULL;
TaskHandle_t xHandle3 = NULL;
TaskHandle_t xHandle4 = NULL;
TaskHandle_t xHandle5 = NULL;

MeEncoderOnBoard leftMotor(SLOT1);
MeEncoderOnBoard rightMotor(SLOT2);

/*/////////  GLOBAL //////////*/
int lineSensorData;
bool obstacleFound = false;
int leftMotorSpeed;
int rightMotorSpeed;
bool lineFound = false;

/*////////////////////////////*/

/*////  INIT MOTOR  ////*/
void isr_process_leftMotor(void)
{
  if(digitalRead(leftMotor.getPortB()) == 0)
  {
    leftMotor.pulsePosMinus();
  }
  else
  {
    leftMotor.pulsePosPlus();
  }
}

void isr_process_rightMotor(void)
{
  if(digitalRead(rightMotor.getPortB()) == 0){
    rightMotor.pulsePosMinus();
  }
  else
  {
    rightMotor.pulsePosPlus();
  }
}
/*//////////////////////*/

/*////  TaskReadLineSensor  ////*/
void TaskReadLineSensor(void *a)
{
  lineSensorData = lineSensor.readSensors();
}
/*//////////////////////////////*/

/*////  TaskReadOpticalSensor  ////*/
void TaskReadOpticalSensor(void *a)
{

  if(opticalSensor.distanceCm() < 15)
  {
    obstacleFound = true;
  }
  else
  {
    obstacleFound = false;
  }
}
/*//////////////////////////////*/

/*////  TaskSetMotorSpeed ////*/
void TaskSetMotorSpeed(void *a)
{
  leftMotor.setMotorPwm(-leftMotorSpeed);
  rightMotor.setMotorPwm(rightMotorSpeed);
}
/*////////////////////////////*/

/*////  TaskFollowLine  ////*/
void TaskFollowLine(void *a)
{
  static int lastTurn;

  if(!obstacleFound && lineFound) //Go in here only if no obstacle is found and the initial line has been found atleast once! check TaskSearchForLine
  {
    switch(lineSensorData)
    {
      case 0: //Forward
      leftMotorSpeed =  50;
      rightMotorSpeed = 50;
      lastTurn = 0;
      break;

      case 1: //Left
      leftMotorSpeed =  50;
      rightMotorSpeed = -50;
      lastTurn = 1;
      break;

      case 2: //Right
      leftMotorSpeed =  -50;
      rightMotorSpeed = 50;
      lastTurn = 2;
      break;

      case 3: //NoLine
        switch(lastTurn)  //Make correction if it was about to turn but then went outside the line
        {
          case 0: //It went forward
          leftMotorSpeed =  50;
          rightMotorSpeed = 50;
          break;

          case 1: //It went left
          leftMotorSpeed =  50;
          rightMotorSpeed = -50;
          break;

          case 2: //It went right
          leftMotorSpeed =  -50;
          rightMotorSpeed = 50;
          break;
        }
      break;
    }
  }
}
/*//////////////////////////*/

/*////  TaskSearchForLine ////*/
void TaskSearchForLine(void *a)
{
  if(lineSensorData == 3 && !lineFound)
  {
    //Searching
    Serial.println("Searching");
  }
  else
  {
    //Found it
    lineFound = true;
  }
}
/*////////////////////////////*/



void setup() {

  attachInterrupt(leftMotor.getIntNum(), isr_process_leftMotor, RISING);
  attachInterrupt(rightMotor.getIntNum(), isr_process_rightMotor, RISING);

  Serial.begin(9600);

  TCCR1A = _BV(WGM10);
  TCCR1B = _BV(CS11) | _BV(WGM12);

  TCCR2A = _BV(WGM21) | _BV(WGM20);
  TCCR2B = _BV(CS21);

  vSchedulerInit();

  vSchedulerPeriodicTaskCreate(TaskReadOpticalSensor, "opticalSensorData", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle1, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(10), pdMS_TO_TICKS(10), pdMS_TO_TICKS(10));

  vSchedulerPeriodicTaskCreate(TaskReadLineSensor, "lineSensorData", configMINIMAL_STACK_SIZE, NULL , 2, &xHandle2, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(10), pdMS_TO_TICKS(10), pdMS_TO_TICKS(10));

  vSchedulerPeriodicTaskCreate(TaskSetMotorSpeed, "setMotorSpeed", configMINIMAL_STACK_SIZE, NULL , 3, &xHandle3, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(10), pdMS_TO_TICKS(10), pdMS_TO_TICKS(10));

  vSchedulerPeriodicTaskCreate(TaskFollowLine, "followLine", configMINIMAL_STACK_SIZE, NULL , 4, &xHandle4, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(10), pdMS_TO_TICKS(10), pdMS_TO_TICKS(10));

  vSchedulerPeriodicTaskCreate(TaskSearchForLine, "searchForLine", configMINIMAL_STACK_SIZE, NULL , 5, &xHandle4, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(10), pdMS_TO_TICKS(10), pdMS_TO_TICKS(10));



  vSchedulerStart();
}

void loop() {
  // put your main code here, to run repeatedly:

}