#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 lineFound = false;
int lineSensorData;
int leftMotorSpeed;
int rightMotorSpeed;
/*These are used when avoiding potential obstacles  */
int leftTurn = 0;
int rightTurn = 0;

/*////  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)
  {
    if(obstacleFound)
    {
      obstacleWasFound = true;

      if(leftTurn > rightTurn)
      {
        if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
        {
          //Hard-left
          leftMotorSpeed = -50;
          rightMotorSpeed = 150;
          xSemaphoreGive(SetMotorSpeedData);
        }
      }
      else
      {
        if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
        {
          //Hard-right
          leftMotorSpeed = 150;
          rightMotorSpeed = -50;
          xSemaphoreGive(SetMotorSpeedData);
        }
      }
    }
    xSemaphoreGive(ReadOpticalSensorData);
  }
}

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

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

            case 1: //Left
            leftMotorSpeed = -150;
            rightMotorSpeed = 150;
            lastTurn = 1;
            leftTurn++;
            obstacleWasFound = false;
            break;

            case 2: //Right
            leftMotorSpeed = 150;
            rightMotorSpeed = -150;
            lastTurn = 2;
            rightTurn++;
            obstacleWasFound = false;
            break;

            case 3: //No line
            if(obstacleWasFound)
            {
              leftMotorSpeed =  125;
              rightMotorSpeed = 125;
            }
            else
            {
              switch(lastTurn)
              {
                case 0: //Reverse
                leftMotorSpeed =  -150;
                rightMotorSpeed = -150;
                break;

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

                case 2: //Right
                leftMotorSpeed =  100;
                rightMotorSpeed = -100;
                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:

}