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

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

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

/*////  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();
  }
}



/*////  TASKS ////*/
TaskHandle_t  xHandle1 = NULL;
TaskHandle_t  xHandle2 = NULL;
TaskHandle_t  xHandle3 = NULL;
TaskHandle_t  xHandle4 = NULL;
TaskHandle_t  xHandle5 = NULL;
TaskHandle_t  xHandle6 = NULL;

/*////  SEMAPHORES  ////*/
SemaphoreHandle_t ReadOpticalSensorData;
SemaphoreHandle_t ReadLineSensorData;
SemaphoreHandle_t SetMotorSpeedData;

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

/*////  READ-TASKS ////*/
void TaskReadOpticalSensor(void *a)
{
  if(xSemaphoreTake(ReadOpticalSensorData, 5) == pdTRUE)
  {
    if(opticalSensor.distanceCm() <= 20)
    {
      obstacleFound = true;
    }
    else
    {
      obstacleFound = false;
    }
    xSemaphoreGive(ReadOpticalSensorData);
  }
  //vTaskDelay(1);  // one tick delay (15ms) in between reads for stability
}

void TaskReadLineSensor(void *a)
{
  if(xSemaphoreTake(ReadLineSensorData, 5) == pdTRUE)
  {
    lineSensorData = lineSensor.readSensors();
    xSemaphoreGive(ReadLineSensorData);
  }
   //vTaskDelay(1);  // one tick delay (15ms) in between reads for stability
}

/*////  MOTOR-TASK  ////*/
void TaskApplyMotorSpeed(void *a)
{
  if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
  {
    leftMotor.setMotorPwm(leftMotorSpeed);
    rightMotor.setMotorPwm(-rightMotorSpeed);
    xSemaphoreGive(SetMotorSpeedData);
  }
}

/*////  OBSTACLE-AVOIDANCE  ////*/
void TaskAvoidObstacle(void *a)
{
  if(xSemaphoreTake(ReadOpticalSensorData, 5) == pdTRUE)
  {
    //Go here until their is no obstacle in sight
    if(obstacleFound)
    {
      //Reverse
      obstacleWasFound = true;
      if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
      {
        leftMotorSpeed = -100;
        rightMotorSpeed = -100;
        xSemaphoreGive(SetMotorSpeedData);
      }
    }
    //After the robot has reversed we need to perfom the "left-arch manuver"
    else if(obstacleWasFound)
    {
      //Make left-turn
      if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
      {
        leftMotorSpeed = -180;
        rightMotorSpeed = 180;
        xSemaphoreGive(SetMotorSpeedData);
        makeArch = true;
        obstacleWasFound = false;
      }
      delay(400);
      /*//Make left-arch
      if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
      {
        leftMotorSpeed = 150;
        rightMotorSpeed = 90;
        xSemaphoreGive(SetMotorSpeedData);
      }
      delay(1500);
      obstacleWasFound = false;*/
    }

    else if(makeArch)
    {
      if(lineSensorData == 3)
      {
        if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
        {
          leftMotorSpeed = 150;
          rightMotorSpeed = 90;
          xSemaphoreGive(SetMotorSpeedData);
        }
      }
      else
      {
        makeArch = false;
      }
    }
    xSemaphoreGive(ReadOpticalSensorData);
  }
}

/*////  LINE-TASKS ////*/
void TaskFollowTheLine(void *a)
{
  static int lastTurn;

  if(xSemaphoreTake(ReadOpticalSensorData, 5) == pdTRUE)
  {
    if(lineFound && !obstacleFound && !makeArch)
    {
      if(xSemaphoreTake(ReadLineSensorData, 5) == pdTRUE)
      {
        if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
        {
          switch(lineSensorData)
          {
            case 0: //Forward
            leftMotorSpeed =  120;
            rightMotorSpeed = 120;
            lastTurn = 0;
            obstacleWasFound = false;
            break;

            case 1: //Left
            leftMotorSpeed = -120;
            rightMotorSpeed = 120;
            lastTurn = 1;
            obstacleWasFound = false;
            break;

            case 2: //Right
            leftMotorSpeed = 120;
            rightMotorSpeed = -120;
            lastTurn = 2;
            obstacleWasFound = false;
            break;

            case 3: //No line
            switch(lastTurn)
            {
              case 0: //Reverse
              leftMotorSpeed =  -90;
              rightMotorSpeed = -90;
              break;

              case 1: //Left
              leftMotorSpeed = -160;
              rightMotorSpeed = 160;
              break;

              case 2: //Right
              leftMotorSpeed =  160;
              rightMotorSpeed = -160;
              break;
            }
            break;
          }
          xSemaphoreGive(SetMotorSpeedData);
        }
        xSemaphoreGive(ReadLineSensorData);
      }
    }
    xSemaphoreGive(ReadOpticalSensorData);
  }
}

void TaskFindTheLine(void *a)
{
  if(xSemaphoreTake(ReadLineSensorData, 5) == pdTRUE)
  {
    if(lineSensorData != 3) //3 == no line
    {
      lineFound = true; //This is always true when the initial line is found.
    }
    else
    {
      //Search for the initial line. This happens at the start of the program
    }
    xSemaphoreGive(ReadLineSensorData);
  }
}



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);

  /*////  SEMAPHORES  ////*/
  ReadOpticalSensorData = xSemaphoreCreateMutex();
  ReadLineSensorData = xSemaphoreCreateMutex();
  SetMotorSpeedData = xSemaphoreCreateMutex();

  vSchedulerInit();

  /*////  TASKS ////*/
  vSchedulerPeriodicTaskCreate(TaskReadOpticalSensor, "t1", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle1, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));

  vSchedulerPeriodicTaskCreate(TaskReadLineSensor, "t2", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle2, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));

  vSchedulerPeriodicTaskCreate(TaskApplyMotorSpeed, "t3", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle3, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));

  vSchedulerPeriodicTaskCreate(TaskAvoidObstacle, "t4", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle4, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));

  vSchedulerPeriodicTaskCreate(TaskFollowTheLine, "t5", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle5, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));

  vSchedulerPeriodicTaskCreate(TaskFindTheLine, "t6", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle6, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));


  vSchedulerStart();

}

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

}