// include libraries will be used
#include <NewPing.h>
#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 Gnd_PIN 34                       /////////// Ground PIN
#define Echo_PIN 31                      /////////// Signal Out PIN
#define Trig_PIN 33                      /////////// Return Signal
#define Vcc_PIN 40                       /////////// 5vdc supply
#define Max_Dist1 3000                    /////////// Setting Max Distance uS can sense for

/////////// Sensor 2 - RH Side ///////////
#define Gnd2_PIN 22                      /////////// Ground PIN
#define Echo2_PIN 24                     /////////// Signal Out PIN
#define Trig2_PIN 26                     /////////// Return Signal
#define Vcc2_PIN 28                      /////////// 5vdc supply
#define Max_Dist2 3000
/*
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
*/

// all 8 QTR sensors are connected to analog inputs 15 through 8, respectively
QTRSensorsAnalog qtra((unsigned char[]) { A15, A14, A13, A12, A11, A10, A9, A8},
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);
                        /////////// Setting line sensors dark position to be 800
int Program = 0;                        /////////// Initial Setting of the Program Data Register
int Follower = 0;                       /////////// Initial Setting of the Line Follower Program
int Distance1;                          /////////// Ultrasonic sensor 1
int Distance2;                          /////////// Ultrasonic sensor 2
int dark = 800;
int waiting = 1;

NewPing sonar1(Trig_PIN, Echo_PIN, Max_Dist1);    /////////// NewPing setup of pins and maximum distance.
NewPing sonar2(Trig2_PIN, Echo2_PIN, Max_Dist2);  //////

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);
  /////////// Setting Pins to I/Os - Sensor 1 ///////////
  pinMode(Gnd_PIN, OUTPUT);             //Setting the ground pin to an output
  digitalWrite(Gnd_PIN, LOW);           //Setting the ground pin low, 0v
  pinMode(Vcc_PIN, OUTPUT);             //Setting the supply voltage to an output
  digitalWrite(Vcc_PIN, HIGH);          //Setting the supply voltage to 5v
  pinMode(Echo_PIN, INPUT);              //Setting ECHO pin to an input
  pinMode(Trig_PIN, OUTPUT);             //Setting TRIGGER pin to an output

/////////// Setting Pins to I/Os - Sensor 2 ///////////
  pinMode(Gnd2_PIN, OUTPUT);             //Setting the ground pin to an output
  digitalWrite(Gnd2_PIN, LOW);           //Setting the ground pin low, 0v
  pinMode(Vcc2_PIN, OUTPUT);             //Setting the supply voltage to an output
  digitalWrite(Vcc2_PIN, HIGH);          //Setting the supply voltage to 5v
  pinMode(Echo2_PIN, INPUT);              //Setting ECHO pin to an input
  pinMode(Trig2_PIN, OUTPUT);             //Setting TRIGGER pin to an output
  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() {
/////////// Ultrasonic Sensor Serial Coms to display value //////////
unsigned int position = qtra.readLine(sensorValues);
for (unsigned char i = 0; i < NUM_SENSORS; i++)
{
  Serial.print(sensorValues[i]);
  Serial.print('\t');
}
  unsigned int uS1 = sonar1.ping();       // Send ping, get ping time in microseconds (uS).
  Distance1 = (uS1 / US_ROUNDTRIP_CM);
  delay(50);
  unsigned int uS2 = sonar2.ping();       // Send ping, get ping time in microseconds (uS).
  Distance2 = (uS2 / US_ROUNDTRIP_CM);

/////////// Serial Print Ultrasonic Sensor 1 ///////////
  Serial.print("Ping1: ");
  Serial.print(Distance1);                // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  Serial.print("cm  ");                   // Serial Display Units in Centimeters

/////////// Serial Print Ultrasonic Sensor 2 ///////////
  Serial.print("Ping2: ");
  Serial.print(Distance2);                // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  Serial.println("cm");                   // Serial Display Units in Centimeters










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

  //left hard
  if(sensorValues[7] > dark && wasUsed == 0){
    myMotor1->run(BACKWARD);
    myMotor3->run(FORWARD);
    myMotor1->setSpeed(30);
    myMotor3->setSpeed(110);
    Serial.println("left hard");
    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");
    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");
    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");
    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");
    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");
    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");
    wasUsed = 1;
    delay(waiting);
  }
  if ((Distance1 <= 15) && (Distance1 != 0)){
    Program = 1;                   /////////// Detect obstacle at <=10cm - Set 1 in Data Register
    myMotor1->run(RELEASE);
    myMotor3->run(RELEASE);
    Serial.println("Program set to 1");
    delay(1000);
  }
}// simple line follower ends here
///////////////////////SIMPLE LINE FOLLOWER///////////////


/////////// If the Ultrasonic Sensor 1 detects an object less than 10cm, a 1 is set in Program Data Register to initiate Obstacle Avoidance ///////////
if(Program == 1){
  Serial.println("Im in Program 1");
  if((Distance2 <= 15) && (Distance2 != 0)){                   /////////// Once uS2 detects object, +1 into Program register to move program on
    Program = 2;
}
    myMotor1->run(BACKWARD);
    myMotor3->run(FORWARD);
    myMotor1->setSpeed(50);
    myMotor3->setSpeed(50);
    Serial.println("Turn left");
}

///////////////////////////////////
if(Program == 2){
  Serial.println("Im in program 2");
  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)){
    Program = 0;
    myMotor1->run(RELEASE);
    myMotor3->run(RELEASE);
    Serial.println("Set program to Line Follower");
    delay (1000);
}
    myMotor1->run(FORWARD);
    myMotor3->run(FORWARD);
    myMotor1->setSpeed(60);
    myMotor3->setSpeed(40);
    Serial.println("Driving in arc");
}
} //LOOP