/*
Description:  ECT501 - Embedded Systems, Individual task - Fault tolerance
Author:       Valentinas Navickas, Southampton Solent University
Date:         03/12/2019
*/
// include libraries will be used
#include <QTRSensors.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>

// defining sensors, which is part of QTRSensor library
#define NUM_SENSORS             8  // number of sensors used
#define NUM_SAMPLES_PER_SENSOR  4  // average 4 analog samples per sensor reading
#define EMITTER_PIN             2  // emitter is controlled by digital pin 2

/*
define global variables, which will be used later
state to remember state, what it was doing at that time and when it was turning left right and how hard.
<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<------------------------>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>@@@@@@@@@@@@@@@@@@@@@@@@@@
0 - 90Degree Left, 1 - Hard left, 2 - Med left, 3 - Low Left, 4 - MiddleL, 5 - MiddleR, 6 - Low right, 7 - Med right, 8 - Hard right, 9 - 90Degree right, 10 - No black line found
<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<------------------------>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>@@@@@@@@@@@@@@@@@@@@@@@@@@
set to be default state 4 middle for now.
dark is defined when sensor reads 800
waiting is part of delay variable, which could be changed easily.
sumSensor is variable, which stores total sum of sensorValues for later to determine, how many sensors are trigerred
wasStateTen, tooManySensors, reset, noLineTimer is variables to remember, default 0
*/
int sumSensor = 0, wasStateTen = 0, tooManySensors = 0, reset = 0, noLineTimer = 0, state = 4, dark = 800, waiting = 50;

// all 8 QTR sensors are connected to analog inputs 15 through 8, respectively
QTRSensorsAnalog qtra((unsigned char[]) { 15, 14, 13, 12, 11, 10, 9, 8},
NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];

//Motorshield library setup, first motor is set to M1 and other on M4
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(1);
Adafruit_DCMotor *myMotor3 = AFMS.getMotor(4);

void setup() {
  Serial.begin(9600); // set up Serial library at 9600 bps
  AFMS.begin();  // create with the default frequency 1.6KHz
  // RELEASE function of library allows to stop motors if they are for some reason running
  myMotor1->run(RELEASE);
  myMotor3->run(RELEASE);
  pinMode(13, OUTPUT); //set arduino LED to an output
  digitalWrite(13, HIGH);    // turn on Arduino's LED to indicate we are in calibration mode
  Serial.println("Calibrating");

  // make the calibration take about 10 seconds
  for (int i = 0; i < 400; i++)
  {
    qtra.calibrate();       // reads all sensors 10 times at 2.5 ms per six sensors (i.e. ~25 ms per call)
  }
  digitalWrite(13, LOW);     // turn off Arduino's LED to indicate we are through with calibration

  // print the calibration minimum values measured when emitters were on
  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtra.calibratedMinimumOn[i]);
    Serial.print(' ');
  }
  Serial.println();

  // print the calibration maximum values measured when emitters were on
  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtra.calibratedMaximumOn[i]);
    Serial.print(' ');
  }
  Serial.println();
}

void loop() {
// Function to update all sensor readins and variables
updateValues();
//
if(sumSensor >= 3500){
  tooManySensors = 1; //If it triggers too many sensors, which is sum of all current sensor values, set that variable to 1
}
if(sumSensor < 3500){
  tooManySensors = 0; //opposite of above.
}

//if at any time, sensors detects no black line, set state variable to 10
if(sensorValues[0] < dark && sensorValues[1] < dark && sensorValues[2] < dark && sensorValues[3] < dark && sensorValues[4] < dark && sensorValues[5] < dark && sensorValues[6] < dark && sensorValues[7] < dark){
  state = 10;
}

//whatever we set reset = 1, we will trigger this part where it will reset our timer and set state accordigly, which sensor was triggered at that time
if(reset == 1){
  reset = 0;
  noLineTimer = 0;
  wasStateTen = 0;
  rememberState();
}

//////////////WHEN NO WHITE LINE DETECTED, WAIT FOR 3S BEFORE SEARCHING////////////////////////////
// if state was 10 (at any point no black line detected and this part was not reached before
if(state == 10 && wasStateTen == 0){

  //if at any point line is detected, set reset to = 1, which will later reset the state and trigger line following algorithm.
  if(sensorValues[0] > dark || sensorValues[1] > dark || sensorValues[2] > dark || sensorValues[3] > dark || sensorValues[4] > dark || sensorValues[5] > dark || sensorValues[6] > dark || sensorValues[7] > dark){
    reset = 1;
  }

  //If robot has reached timer 3s and during that time it has not found black line
  if(noLineTimer >= 3000){
    //myMotor1->run(BACKWARD);
    //myMotor3->run(FORWARD);
    //myMotor1->setSpeed(75);
    //myMotor3->setSpeed(75);
    Serial.println("no black line FOUND do something like circle");
    wasStateTen = 1; //Once we set this variable, we will never come back here until we reset it
    }

  //If timer is not reached, update timer add 100ms
  else{
      noLineTimer = noLineTimer + 100;
      delay(100);
  }
}

//If robot has waited 3s and did not find line, do this continuisly until it finds
while(wasStateTen == 1){
updateValues();
if(sensorValues[0] > dark || sensorValues[1] > dark || sensorValues[2] > dark || sensorValues[3] > dark || sensorValues[4] > dark || sensorValues[5] > dark || sensorValues[6] > dark || sensorValues[7] > dark){
    wasStateTen = 0;
    reset = 1; //if at any point line is detected, set reset to = 1, which will later reset the state and trigger line following algorithm.
}
if(wasStateTen == 0){
  myMotor1->run(FORWARD);
  myMotor3->run(FORWARD);
  myMotor1->setSpeed(40);
  myMotor3->setSpeed(75);
  delay(waiting);
  }
} //while wasStateTen 1

///////////////////////SIMPLE LINE FOLLOWER/////////////////////////////
//If robot have not triggered many sensors and our current state is not 10(no black line)
if(tooManySensors == 0 && state != 10){
int wasUsed = 0; //Sensor readings could read dark for more than 1 sensor, this will ensure condition is only triggered once
Serial.println("toomanysensors = 0");

  //90 Degree will be checked first before following simple line///////////////////////
  if(sensorValues[0] > dark && sensorValues[1] > dark && sensorValues[2] > dark && sensorValues[3] > dark && wasUsed == 0){
      myMotor1->run(FORWARD);
      myMotor3->run(BACKWARD);
      myMotor1->setSpeed(75);
      myMotor3->setSpeed(25); // was 25
      Serial.println("right for 90 DEGREE");
      wasUsed = 1;
      delay(waiting);
  }
  if(sensorValues[4] > dark && sensorValues[5] > dark && sensorValues[6] > dark && sensorValues[7] > dark && wasUsed == 0){
      myMotor1->run(BACKWARD);
      myMotor3->run(FORWARD);
      myMotor1->setSpeed(25);
      myMotor3->setSpeed(75); // was 25
      Serial.println("left for 90 DEGREE");
      wasUsed = 1;
      delay(waiting);
  }
  //90 Degree will be checked first before following simple line///////////////////////

  //left hard
  if(sensorValues[7] > dark && wasUsed == 0){
    myMotor1->run(BACKWARD);
    myMotor3->run(FORWARD);
    myMotor1->setSpeed(30);
    myMotor3->setSpeed(110);
    Serial.println("left hard when toomanysensor = 0");
    wasUsed = 1;
    delay(waiting);
  }

  //left medium
  if(sensorValues[6] > dark && wasUsed == 0){
    myMotor1->run(BACKWARD);
    myMotor3->run(FORWARD);
    myMotor1->setSpeed(30);
    myMotor3->setSpeed(100);
    Serial.println("left medium when toomanysensor = 0");
    wasUsed = 1;
    delay(waiting);
  }

  //left low
  if(sensorValues[5] > dark && wasUsed == 0){
    myMotor1->run(FORWARD);
    myMotor3->run(FORWARD);
    myMotor1->setSpeed(50);
    myMotor3->setSpeed(75); // was 25
    Serial.println("left low when toomanysensor = 0");
    wasUsed = 1;
    delay(waiting);
  }

  //forward
  if((sensorValues[4] > dark || sensorValues[3] > dark) && wasUsed == 0){
    myMotor1->run(FORWARD);
    myMotor3->run(FORWARD);
    myMotor1->setSpeed(70);
    myMotor3->setSpeed(75);
    Serial.println("forward when toomanysensor = 0");
    wasUsed = 1;
    delay(waiting);
  }

  //right low
  if(sensorValues[2] > dark && wasUsed == 0){
    myMotor1->run(FORWARD);
    myMotor3->run(FORWARD);
    myMotor1->setSpeed(75);
    myMotor3->setSpeed(50); // was 25
    Serial.println("right low when toomanysensor = 0");
    wasUsed = 1;
    delay(waiting);
  }

  //right medium
  if(sensorValues[1] > dark && wasUsed == 0){
    myMotor1->run(FORWARD);
    myMotor3->run(BACKWARD);
    myMotor1->setSpeed(100);
    myMotor3->setSpeed(30); // was 25
    Serial.println("right medium when toomanysensor = 0");
    wasUsed = 1;
    delay(waiting);
  }

  //right hard
  if(sensorValues[0] > dark && wasUsed == 0){
    myMotor1->run(FORWARD);
    myMotor3->run(BACKWARD);
    myMotor1->setSpeed(110);
    myMotor3->setSpeed(30); // was 25
    Serial.println("right hard when toomanysensor = 0");
    wasUsed = 1;
    delay(waiting);
  }
rememberState(); //At the very end it is important to remember, where we triggered sensor and this function allows to remember
}// simple line follower ends here
///////////////////////SIMPLE LINE FOLLOWER/////////////////////////////

/*
state to remember state, when it was turning left right and how hard.
<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<----------->>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
0 - 90Degree Left, 1 - Hard left, 2 - Med left, 3 - Low Left, 4 - MiddleL, 5 - MiddleR, 6 - Low right, 7 - Med right, 8 - Hard right, 9 - 90Degree right, 10 - No black line found
set to be default middle for now.
state 0 and state 9 is not used in this code, but might be utilized later.
*/
//////////////////////// LINE FOLLOWER WHEN TOO MANY SENSORS ARE TRIGERRED ///////////////////////////////////
// essential to have different code to follow spiral line seperatly as it triggers many sensors
//If robot have triggered many sensors and our current state is not equal to state 10(no black line)

if(tooManySensors == 1 && state != 10){
Serial.println("toomanysensors = 1");

  if(sensorValues[0] > dark){
    //if robot wants to turn right hard, check if our state was 7(right med)
    if(state == 7){
      myMotor1->run(FORWARD);
      myMotor3->run(BACKWARD);
      myMotor1->setSpeed(110);
      myMotor3->setSpeed(50); // was 25
      Serial.println("right hard");
      state = 8; //update state that robot was turning right hard at that time
    }
  }
  if(sensorValues[1] > dark){
    //if robot wants to turn right med, check if our state was 6(right low) or state 8(right hard)
    if(state == 6 || state == 8){
      myMotor1->run(FORWARD);
      myMotor3->run(BACKWARD);
      myMotor1->setSpeed(100);
      myMotor3->setSpeed(30); // was 25
      Serial.println("right medium");
      state = 7; //update state that robot was turning right medium at that time
    }
  }
  if(sensorValues[2] > dark){
    //if robot wants to turn right low, check if our state was 5(MiddleR) or state 7(right med)
    if(state == 5 || state == 7){
      myMotor1->run(FORWARD);
      myMotor3->run(FORWARD);
      myMotor1->setSpeed(75);
      myMotor3->setSpeed(50); // was 25
      Serial.println("right low");
      state = 6; //update state that robot was turning right low at that time
    }
  }
  if(sensorValues[3] > dark){
    //if robot wants to turn right low, check if our state was 4(MiddleL) or state 6(low right)
    if(state == 4 || state == 6){
      myMotor1->run(FORWARD);
      myMotor3->run(FORWARD);
      myMotor1->setSpeed(75);
      myMotor3->setSpeed(65);
      Serial.println("Middle to Right");
      state = 5; //update state that robot was turning MiddleR at that time
    }
  }
  if(sensorValues[4] > dark){
    //if robot wants to turn right low, check if our state was 3(low left) or state 5(MiddleR)
    if(state == 3 || state == 5){
      myMotor1->run(FORWARD);
      myMotor3->run(FORWARD);
      myMotor1->setSpeed(65);
      myMotor3->setSpeed(75);
      Serial.println("Middle to Left");
      state = 4; //update state that robot was turning MiddleL at that time
    }
  }
  if(sensorValues[5] > dark){
    //if robot wants to turn right low, check if our state was 2(med left) or state 4(MiddleL)
    if(state == 2 || state == 4){
      myMotor1->run(FORWARD);
      myMotor3->run(FORWARD);
      myMotor1->setSpeed(50);
      myMotor3->setSpeed(75); // was 25
      Serial.println("Left Low");
      state = 3; //update state that robot was turning left low at that time
    }
  }
  if(sensorValues[6] > dark){
    //if robot wants to turn right low, check if our state was 1(Hard left) or state 3(low left)
    if(state == 1 || state == 3){
      myMotor1->run(BACKWARD);
      myMotor3->run(FORWARD);
      myMotor1->setSpeed(30);
      myMotor3->setSpeed(100); // was 25
      Serial.println("Left Middle");
      state = 2; //update state that robot was turning left middle at that time
    }
  }
  if(sensorValues[7] > dark){
    //if robot wants to turn left hard, check if our state was 2(left med)
    if(state == 2){
      myMotor1->run(BACKWARD);
      myMotor3->run(FORWARD);
      myMotor1->setSpeed(50);
      myMotor3->setSpeed(110); // was 25
      Serial.println("Left Hard");
      state = 1; //update state that robot was turning left hard at that time
      ////delay(waiting);(waiting);(waiting);
    }
  }
}//While toomanysensors == 1
//////////////////////// LINE FOLLOWER WHEN TOO MANY SENSORS ARE TRIGERRED ///////////////////////////////////

} //LOOP

/*
state to remember state, when it was turning left right and how hard.
<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<----------->>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
0 - 90Degree Left, 1 - Hard left, 2 - Med left, 3 - Low Left, 4 - MiddleL, 5 - MiddleR, 6 - Low right, 7 - Med right, 8 - Hard right, 9 - 90Degree right, 10 - No black line found
set to be default middle for now.
*/
void updateValues(){
  for (unsigned char i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(sensorValues[i]);
    Serial.print('\t');
  }
  for (int i = 0; i < NUM_SENSORS; i++){
    sumSensor += sensorValues[i];
  }
  Serial.print(state);
  Serial.print('\t');
  Serial.println(sumSensor);
  Serial.print("No line timer ");
  Serial.print(noLineTimer);
}

void rememberState(){
  if(sensorValues[0] > dark){
  Serial.println("Reseted state to 8");
  state = 8;
  }
  if(sensorValues[1] > dark){
  Serial.println("Reseted state to 7");
  state = 7;
  }
  if(sensorValues[2] > dark){
  Serial.println("Reseted state to 6");
  state = 6;
  }
  if(sensorValues[3] > dark){
  Serial.println("Reseted state to 5");
  state = 5;
  }
  if(sensorValues[4] > dark){
  Serial.println("Reseted state to 4");
  state = 4;
  }
  if(sensorValues[5] > dark){
  Serial.println("Reseted state to 3");
  state = 3;
  }
  if(sensorValues[6] > dark){
  Serial.println("Reseted state to 2");
  state = 2;
  }
  if(sensorValues[7] > dark){
  Serial.println("Reseted state to 1");
  state = 1;
  }
}









/*
if(sensorValues[0] > dark || sensorValues[1] > dark || sensorValues[2] > dark){
  if(sensorValues[2] > dark){
  myMotor1->run(FORWARD);
  myMotor3->run(BACKWARD);
  myMotor1->setSpeed(75);
  myMotor3->setSpeed(50); // was 25
  Serial.println("right low");
  state = 5;
  lcd.setCursor(0,0);
  lcd.print("right LOW    ");
  ////delay(waiting);(waiting);(50);
  }
  else if(sensorValues[1] > dark && state >= 5){
    myMotor1->run(FORWARD);
    myMotor3->run(BACKWARD);
    myMotor1->setSpeed(100);
    myMotor3->setSpeed(30); // was 25
    Serial.println("right medium");
    state = 6;
    lcd.setCursor(0,0);
    lcd.print("right MEDIUM  ");
    ////delay(waiting);(waiting);(50);
  }
  else if(sensorValues[0] > dark && state >= 5){
    myMotor1->run(FORWARD);
    myMotor3->run(FORWARD);
    myMotor1->setSpeed(110);
    myMotor3->setSpeed(30); // was 25
    Serial.println("right hard");
    state = 7;
    lcd.setCursor(0,0);
    lcd.print("right HARD   ");
    ////delay(waiting);(waiting);(50);
  }
  else{
    state = 4;
    ////delay(waiting);(waiting);(50);
  }
}


if(sensorValues[7] > dark || sensorValues[6] > dark || sensorValues[5] > dark){
if(sensorValues[5] > dark){
  myMotor1->run(FORWARD);
  myMotor3->run(BACKWARD);
  myMotor1->setSpeed(50);
  myMotor3->setSpeed(75); // was 25
  Serial.println("left low");
  state = 3;
  lcd.setCursor(0,0);
  lcd.print("left LOW   ");
  ////delay(waiting);(waiting);(50);
}
else if(sensorValues[6] > dark && state >= 3){
    myMotor1->run(FORWARD);
    myMotor3->run(BACKWARD);
    myMotor1->setSpeed(30);
    myMotor3->setSpeed(100); // was 25
    Serial.println("left medium");
    state = 2;
    lcd.setCursor(0,0);
    lcd.print("left MEDIUM");
    ////delay(waiting);(waiting);(50);
}
else if(sensorValues[7] > dark && state >= 3){
    myMotor1->run(BACKWARD);
    myMotor3->run(BACKWARD);
    myMotor1->setSpeed(30);
    myMotor3->setSpeed(110); // was 25
    Serial.println("left hard");
    state = 1;
    lcd.setCursor(0,0);
    lcd.print("left HARD  ");
    ////delay(waiting);(waiting);(50);
}
else{
  state = 4;
  ////delay(waiting);(waiting);(50);
}
}
if(sensorValues[4] > dark && sensorValues[3] > dark){
myMotor1->run(FORWARD);
myMotor3->run(BACKWARD);
myMotor1->setSpeed(75);
myMotor3->setSpeed(70);
Serial.println("foward");
state = 4;
//////delay(waiting);(waiting); (1000);
lcd.setCursor(0,0);
lcd.print("Forward");
}
//////////////////////////// 90 degree angles///////////////////////////////
if(sensorValues[4] > dark && sensorValues[3] > dark && sensorValues[0] > dark && sensorValues[1] > dark && sensorValues[2] > dark){
  myMotor1->run(FORWARD);
  myMotor3->run(FORWARD);
  myMotor1->setSpeed(75);
  myMotor3->setSpeed(25); // was 25
  Serial.println("right for 90 DEGREE");
  state = 8;
  lcd.setCursor(0,0);
  lcd.print("right for 90 DEGREE    ");
  ////delay(waiting);(waiting);(100);
  }
if(sensorValues[4] > dark && sensorValues[3] > dark && sensorValues[7] > dark && sensorValues[6] > dark && sensorValues[5] > dark){
  myMotor1->run(BACKWARD);
  myMotor3->run(BACKWARD);
  myMotor1->setSpeed(25);
  myMotor3->setSpeed(75); // was 25
  Serial.println("left for 90 DEGREE");
  state = 0;
  ////delay(waiting);(waiting);(100);
}
//////////////////////////// 90 degree angles///////////////////////////////
if()
  myMotor1->run(FORWARD);
  myMotor3->run(BACKWARD);
  myMotor1->setSpeed(s);
  myMotor3->setSpeed(t); // was 25
  s = s + 5;
  t = t = 3
  ////delay(waiting);(waiting);(100);
}
else{
  s = 50;
  t = 30;
}
*/

/*
if(sensorValues[0] > 800)
{
lcd.setCursor(0,0);
lcd.print("Reading 1");
}
if(sensorValues[1] > 800)
{
lcd.setCursor(0,0);
lcd.print("Reading 2");
}
if(sensorValues[2] > 800)
{
lcd.setCursor(0,0);
lcd.print("Reading 3");
}
if(sensorValues[3] > 800)
{
lcd.setCursor(0,0);
lcd.print("Reading 4");
}
if(sensorValues[4] > 800)
{
lcd.setCursor(0,0);
lcd.print("Reading 5");
}
if(sensorValues[5] > 800)
{
lcd.setCursor(0,0);
lcd.print("Reading 6");
}
if(sensorValues[6] > 800)
{
lcd.setCursor(0,0);
lcd.print("Reading 7");
}
if(sensorValues[7] > 800)
{
lcd.setCursor(0,0);
lcd.print("Reading 8");
}
*/