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

/*////  READ-TASKS ////*/
void TaskReadOpticalSensor(void *a)
{
  for(;;)
  {
    if(xSemaphoreTake(ReadOpticalSensorData, 0) == 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)
{
  for(;;)
  {
    if(xSemaphoreTake(ReadLineSensorData, 0) == 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, 3) == pdTRUE)
  {
    leftMotor.setMotorPwm(leftMotorSpeed);
    rightMotor.setMotorPwm(-rightMotorSpeed);
    xSemaphoreGive(SetMotorSpeedData);
  }
}

/*////  OBSTACLE-AVOIDANCE  ////*/
void TaskAvoidObstacle(void *a)
{
  if(obstacleFound && !obstacleWasFound)
  {
      //Hard left
      if(xSemaphoreTake(SetMotorSpeedData, 3) == pdTRUE)
      {
        leftMotorSpeed = -50;
        rightMotorSpeed = 150;
        obstacleWasFound = true;
        xSemaphoreGive(SetMotorSpeedData);
      }
  }
  else if(obstacleWasFound)
  {
    if(xSemaphoreTake(SetMotorSpeedData, 3) == pdTRUE)
    {
      leftMotorSpeed =  50;
      rightMotorSpeed = 50;
      obstacleWasFound = false;
      xSemaphoreGive(SetMotorSpeedData);
    }
  }
}

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

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

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

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

      case 3: //No line
      if(strike > 100){

      }
      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(ReadOpticalSensorData);
    xSemaphoreGive(ReadLineSensorData);
  }
}

void TaskFindTheLine(void *a)
{
  if(xSemaphoreTake(ReadLineSensorData, 3) == pdTRUE)
  {
    if(lineSensorData != 3) //3 == no line
    {
      lineFound = true; //This is always true when the initial line is found.
    }
    else
    {
      lineFound = false;
      //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(10), pdMS_TO_TICKS(10), pdMS_TO_TICKS(10));

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

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

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

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

  vSchedulerPeriodicTaskCreate(TaskFindTheLine, "t6", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle6, 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:

}