#include <Servo.h>

// Initializing servos
Servo HR;
Servo HL;
Servo TL;
Servo TR;
Servo BL;
Servo BR; 

// Defining DC motors pins
#define L_in1 6
#define R_in2 7
#define L_in2 4
#define R_in1 9
#define LEFT_MOTOR_ENABLE 5
#define RIGHT_MOTOR_ENABLE 8

// Defining IR sensors pins 
#define LEFT_SENSOR 52
#define LEFT_MIDDLE_SENSOR 50
#define MIDDLE_LEFT_SENSOR 25
#define MIDDLE_RIGHT_SENSOR 27
#define MIDDLE_LEFTMOST_SENSOR 23
#define MIDDLE_RIGHTMOST_SENSOR 29
#define RIGHT_MIDDLE_SENSOR 31
#define RIGHT_SENSOR 46
#define JUNCTION_SENSOR 48

// Defining color sensor pins
#define S0 33
#define S1 35
#define S2 37
#define S3 39
#define sensorOut 41
#define B0 36
#define B1 34
#define B2 32
#define B3 30
#define blueSensorOut 28  

// Initializing integer variables for color sensor RGB values
int redL = 0;
int blueL = 0;
int greenL = 0;
int redR = 0;
int blueR = 0;
int greenR = 0;

// Defining left and right sonars pins
const int trigPinLeft = 47;
const int echoPinLeft = 49;
const int trigPinRight = 53;
const int echoPinRight = 51;

// Defining variables for sonar values
long durationLeft, durationRight;
int distanceLeft, distanceRight;

// Initializing junction counter variables to be used in the code
int junctionCounter = 0 ;
int junctionCounter1 = 0;

// Variable to store the servo position
int pos = 0;    

void setup() {
  // Set all of the motor control pins to outputs
  pinMode(R_in1, OUTPUT);
  pinMode(L_in2, OUTPUT);
  pinMode(R_in2, OUTPUT);
  pinMode(L_in1, OUTPUT);
  pinMode(LEFT_MOTOR_ENABLE, OUTPUT);
  pinMode(RIGHT_MOTOR_ENABLE, OUTPUT);
 
 Serial.begin(9600);
  // Set all of the sensor pins to inputs
  pinMode(LEFT_SENSOR, INPUT);
  pinMode(LEFT_MIDDLE_SENSOR, INPUT);
  pinMode(MIDDLE_LEFT_SENSOR, INPUT);
  pinMode(MIDDLE_RIGHT_SENSOR, INPUT);
  pinMode(MIDDLE_LEFTMOST_SENSOR, INPUT);
  pinMode(MIDDLE_RIGHTMOST_SENSOR, INPUT);
  pinMode(RIGHT_MIDDLE_SENSOR, INPUT);
  pinMode(RIGHT_SENSOR, INPUT);

  // Initialize sonar pins
  pinMode(trigPinLeft, OUTPUT);
  pinMode(echoPinLeft, INPUT);
  pinMode(trigPinRight, OUTPUT);
  pinMode(echoPinRight, INPUT);

  // Initializing servo pins 
  HR.attach(45); // 120 backward, 155 forward (calibrated)
  HL.attach(44);  // 30 forward, 80 backward  (calibrated)
  TL.attach(13);
  TR.attach(11);
  BL.attach(12);
  BR.attach(10);

  // Initializing color sensor pins to required input or output value
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);

  pinMode(B0, OUTPUT);
  pinMode(B1, OUTPUT);
  pinMode(B2, OUTPUT);
  pinMode(B3, OUTPUT);
  pinMode(blueSensorOut, INPUT);
  
  // Setting frequency-scaling to 20% of the color sensors
  digitalWrite(S0,HIGH);
  digitalWrite(S1,LOW);
  digitalWrite(B0,HIGH);
  digitalWrite(B1,LOW);
}

void handleJunction1() {
  stop();
  delay(50);

   while (1){
    int left = measureDistanceLeft();
    if (left > 15 ){
      break;
    }
    else {
      goBackAlign(22,20);
    }        
  }
  
    rotateLeft(50);
  delay(250);
  while(1){
    int middleSensor = digitalRead(MIDDLE_RIGHTMOST_SENSOR);
    rotateLeft(30);
    if(middleSensor == LOW){
      break;
    }
  }

  leftMiniServosDown();
  rightMiniServosDown();

  align(200);

  goStraightAlign(33,30);
  delay(300);
  thumka();
stop();
delay(100);

  while (1)
  {
    detectColorLeft();
  if (greenL<100)
  {
    break;
  }
  else if (redL<100)
  {
    collectLeft();
    break;
  }
  }

  while (1)
  {
      detectColorRight();
  if (greenR<100)
  {
    break;
  }
  else if (redR<100)
  {
    collectRight();
    break;
  }
  }
  goBackAlign(90,80);
delay(600);
  
leftMiniServosUp();
rightMiniServosUp();



rotateLeft(80);
delay(500);
while(1){
    int middleSensor = digitalRead(MIDDLE_RIGHT_SENSOR);
    rotateLeft(80);
    if(middleSensor == LOW){
      stop();
      break;
    }
  }

junctionCounter1 = 0;
junctionCounter = 1;
}

void handleJunction2() {
  stop();
  delay(50);

   while (1){
    int junction = digitalRead(JUNCTION_SENSOR);
    if (junction == HIGH){
      stop();
      break;      
    }
    else{
      goStraight(20);
    }
  }

  
    rotateLeft(50);
  delay(250);
  while(1){
    int middleSensor = digitalRead(MIDDLE_RIGHT_SENSOR);
    rotateLeft(30);
    if(middleSensor == LOW){
      stop();
      break;
    }
  }

  leftMiniServosDown();
  rightMiniServosDown();

  align(100);
  goStraightAlign(33,30);
  delay(300); 
  thumka();
stop();
delay(10);

  while (1)
  {
  detectColorLeft();
  if (greenL<100)
  {
    break;
  }
  else if (redL<100)
  {
    collectLeft();
    break;
  }
  }

  while (1)
  {
  detectColorRight();
  if (greenR<100)
  {
    break;
  }
  else if (redR<100)
  {
    collectRight();
    break;
  }
  }
  goBack(20);
  delay(600);
leftMiniServosUp();
rightMiniServosUp();

goBackAlign(35,30);
delay(250);

rotateRight(50);
delay(410);

junctionCounter1 = 0;
junctionCounter = 2;
}

void handleJunction3(){
  stop();
  delay(50);
  while (1){
    int left = measureDistanceLeft();
    if (left == 16){
      stop();
      delay(50);
      break;
    }
    else {
      goStraight(20);
    }        
  }

  rotateLeft(50);
  delay(250);
  while(1){
    int middleSensor = digitalRead(MIDDLE_RIGHT_SENSOR);
    rotateLeft(30);
    if(middleSensor == LOW){
      break;
    }
  }

  leftMiniServosDown();
  rightMiniServosDown();

  align(100);
  
  goStraightAlign(22,20);
  delay(400);
  thumka();
  stop();
  delay(10);
  while (1)
  {
  detectColorLeft();
  if (greenL<100)
  {
    break;
  }
  else if (redL<100)
  {
    collectLeft();
    break;
  }
  }

  while (1)
  {
  detectColorRight();
  if (greenR<100)
  {
    break;
  }
  else if (redR<100)
  {
    collectRight();
    break;
  }
  }

  goBackAlign(25,20);
  delay(600);

leftMiniServosUp();
rightMiniServosUp();

stop();
delay(50);

goBackAlign(60,50);
delay(110);

rotateRight(50);
delay(420);

dropOff(600, 420);
stop();
delay(1500);

leftMiniServosUp();
rightMiniServosUp();
dropOff(500, 300);
stop();
delay(1000);

park(70,140);
delay(600);

stop();
delay(10000);
}

// ALL FUNCTIONS

int measureDistanceLeft() {           
  digitalWrite(trigPinLeft, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinLeft, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinLeft, LOW);
  long durationLeft = pulseIn(echoPinLeft, HIGH);
  int distanceLeft = durationLeft * 0.034 / 2;
  return distanceLeft;
}

int measureDistanceRight() {
  digitalWrite(trigPinRight, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinRight, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinRight, LOW);
  long durationRight = pulseIn(echoPinRight, HIGH);
  int distanceRight = durationRight * 0.034 / 2;
  return distanceRight;
}

void stop() {
  digitalWrite(RIGHT_MOTOR_ENABLE, LOW);
  analogWrite(R_in1, 0);
  digitalWrite(R_in2, LOW);
  analogWrite(L_in1, 0);
  digitalWrite(L_in2, LOW);
}

void rotateLeft(int a) {
  digitalWrite(R_in2, LOW);
  analogWrite(R_in1, a);
  analogWrite(L_in2, a);
  digitalWrite(L_in1, LOW);  
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
}

void rotateRight(int a) {
  analogWrite(R_in2, a);
  digitalWrite(R_in1, LOW);
  digitalWrite(L_in2, LOW);
  analogWrite(L_in1, a);  
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH); 
}
void goStraight(int a) {
  digitalWrite(R_in2, LOW);
  analogWrite(R_in1, a);
  digitalWrite(L_in2, LOW);
  analogWrite(L_in1, a);
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
}

void goStraightAlign(int a, int b) {
  digitalWrite(R_in2, LOW);
  analogWrite(R_in1, a);
  digitalWrite(L_in2, LOW);
  analogWrite(L_in1, b);
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
}

void turnLeft(int a) {
  analogWrite(R_in1, a);
  digitalWrite(R_in2, LOW);
  digitalWrite(L_in1, 0);
  analogWrite(L_in2, LOW);
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
}

void turnRight(int a) {
  digitalWrite(R_in2, LOW);
  analogWrite(R_in1, 0);
  digitalWrite(L_in2, LOW);
  analogWrite(L_in1, a);
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
}

void turnSlightlyLeft(int sl, int max) {
  digitalWrite(R_in2, LOW);
  analogWrite(R_in1, max);
  digitalWrite(L_in2, LOW);
  analogWrite(L_in1, sl);
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
}

void park(int sr, int max) {
  analogWrite(R_in2, sr);
  digitalWrite(R_in1, LOW);
  analogWrite(L_in2, max);
  digitalWrite(L_in1, LOW);
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
}

void turnSlightlyRight(int sr, int max) {
  digitalWrite(R_in2, LOW);
  analogWrite(R_in1, sr);
  digitalWrite(L_in2, LOW);
  analogWrite(L_in1, max);
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
}

void goBack(int a) {
  analogWrite(R_in2, a);
  digitalWrite(R_in1, LOW);
  analogWrite(L_in2, a);
  digitalWrite(L_in1, LOW);
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
}

void goBackAlign(int a, int b) {
  analogWrite(R_in2, a);
  digitalWrite(R_in1, LOW);
  analogWrite(L_in2, b);
  digitalWrite(L_in1, LOW);
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
}

void rightMiniServosDown(){
    TR.write(20); 
    BR.write(0);
}

 void rightMiniServosDownDrop(){
    TR.write(0); 
    BR.write(0);
}

void leftMiniServosDown(){
    TL.write(160);
    BL.write(180);
}

void leftMiniServosDownDrop(){
    TL.write(180);
    BL.write(180);
}

void rightMiniServosUp(){
    TR.write(180); 
    BR.write(180);  
}

void leftMiniServosUp(){
    TL.write(0); 
    BL.write(0);  
}

void leftMainForward(){
     for (int pos = 80; pos >= 30; pos -= 1) { 
    HL.write(pos);              
    delay(15);                       
  }
}

void rightMainForward(){
  for (int pos = 105; pos <= 165; pos += 1) {
    HR.write(pos);              
    delay(15);                       
  }
}

void leftMainBackward(){
  for (int pos = 30; pos <= 80; pos += 1) {
    HL.write(pos);              
    delay(15);                       
  }
}

void rightMainBackward(){
      for (int pos = 165; pos >= 125; pos -= 1) { 
    HR.write(pos);              
    delay(15);                       
  }
}
void rightMainBackward1(){  // For the correction of right main servo in the start
  HR.write(125);
}

void collectLeft(){
    leftMainForward();
    leftMiniServosUp();
    leftMainBackward();
}

void collectRight(){
    rightMainForward();
    rightMiniServosUp();
    rightMainBackward();
}

void align (int a){
  goBack(20);
  delay(a);
  
    goStraight(10);
    while (1)
    {
      int leftMiddleSensor = digitalRead(LEFT_MIDDLE_SENSOR);
      int middleLeftSensor = digitalRead(MIDDLE_LEFT_SENSOR);
      int middleRightSensor = digitalRead(MIDDLE_RIGHT_SENSOR);
      int middleLeftmostSensor = digitalRead(MIDDLE_LEFTMOST_SENSOR);
      int middleRightmostSensor = digitalRead(MIDDLE_RIGHTMOST_SENSOR);
      int rightMiddleSensor = digitalRead(RIGHT_MIDDLE_SENSOR);
      int distanceLeft = measureDistanceLeft();
  if (distanceLeft<3){
    break;
  }
  else if (middleLeftSensor == LOW && middleRightSensor == LOW && middleLeftmostSensor == LOW && middleRightmostSensor == LOW) {
    goStraightAlign(22,20);
  }
  else if (middleLeftSensor == LOW && middleRightSensor == LOW&& middleRightmostSensor == LOW){
    turnSlightlyRight(22,26);
  }
  else if (middleLeftSensor == LOW && middleRightSensor == LOW && middleLeftmostSensor == LOW ){
    turnSlightlyLeft(29,20);
  }
  else if (middleRightSensor == LOW){
    turnSlightlyRight(20,26);
  }
  else if (middleLeftSensor == LOW){
    turnSlightlyLeft(29,17);
  }
  else if (leftMiddleSensor == LOW) {
    turnLeft(50);
  }
  else if (rightMiddleSensor == LOW) {
    turnRight(50);
  
  }
    }
}

void thumka(){
  turnLeft(50);
  delay(100);
  turnRight(50);
  delay(100);
  goStraight(50);
  delay(50);

}

void dropOff(int a, int b){
  goBack(50);
  delay(a);
  leftMiniServosDownDrop();
  rightMiniServosDownDrop();
  goStraightAlign(160,200);
  delay(b);
  goBack(150);
  delay(20);  
}

void detectColorLeft(){
  // Setting red filtered photodiodes to be read
  digitalWrite(S2,LOW);
  digitalWrite(S3,LOW);
  // Reading the output frequency
  redL = pulseIn(sensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  redL = map(redL, 25,72,0,255);
  delay(100);
  // Setting Green filtered photodiodes to be read
  digitalWrite(S2,HIGH);
  digitalWrite(S3,HIGH);
  // Reading the output frequency
  greenL = pulseIn(sensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  greenL = map(greenL, 30,90,0,255);
  delay(100);
  // Setting Blue filtered photodiodes to be read
  digitalWrite(S2,LOW);
  digitalWrite(S3,HIGH);
  // Reading the output frequency
  blueL = pulseIn(sensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  blueL = map(blueL, 25,70,0,255);
  delay(100);
}

void detectColorRight(){
  // Setting red filtered photodiodes to be read
  digitalWrite(B2,LOW);
  digitalWrite(B3,LOW);
  // Reading the output frequency
  redR = pulseIn(blueSensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  redR = map(redR, 25,72,0,255);
  delay(100);
  // Setting Green filtered photodiodes to be read
  digitalWrite(B2,HIGH);
  digitalWrite(B3,HIGH);
  // Reading the output frequency
  greenR = pulseIn(blueSensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  greenR = map(greenR, 30,90,0,255);
  delay(100);
  // Setting Blue filtered photodiodes to be read
  digitalWrite(B2,LOW);
  digitalWrite(B3,HIGH);
  // Reading the output frequency
  blueR = pulseIn(blueSensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  blueR = map(blueR, 25,70,0,255);
  delay(100);
}

void loop() {
  int leftSensor = digitalRead(LEFT_SENSOR);
  int leftMiddleSensor = digitalRead(LEFT_MIDDLE_SENSOR);
  int middleLeftSensor = digitalRead(MIDDLE_LEFT_SENSOR);
  int middleRightSensor = digitalRead(MIDDLE_RIGHT_SENSOR);
  int middleLeftmostSensor = digitalRead(MIDDLE_LEFTMOST_SENSOR);
  int middleRightmostSensor = digitalRead(MIDDLE_RIGHTMOST_SENSOR);
  int rightMiddleSensor = digitalRead(RIGHT_MIDDLE_SENSOR);
  int rightSensor = digitalRead(RIGHT_SENSOR);
  if (leftSensor == LOW  && rightSensor == LOW) {  
    if (junctionCounter == 0) {
      goStraight(80);
      delay (100);
      leftMainBackward();
      rightMainBackward1();
             junctionCounter++;
      while (1){
        if (leftSensor == LOW || rightSensor == LOW){
          goBack(100);
          delay(30);
          handleJunction1();
          junctionCounter++;
          break;
        }
        else{
          goStraight(80);          
        }
      } 
                        
    }
      else if (junctionCounter == 2) {
      handleJunction2();
           junctionCounter++;
    } else if (junctionCounter == 3) {
      handleJunction3();
           junctionCounter++;
    }
  }

  else if(middleLeftSensor == LOW || middleRightSensor == LOW){
    goStraight(100);
  }
  
  else if (leftMiddleSensor == LOW) {
    if (middleLeftmostSensor == LOW && leftMiddleSensor == LOW)
    {
    turnSlightlyLeft(70,58);  
  }
  else {
    turnLeft(50);
  }
  }
  else if (rightMiddleSensor == LOW) {
    if (middleRightmostSensor == LOW && rightMiddleSensor == LOW){
    turnSlightlyRight(58,70);
  }
  else {
    turnRight(50);
  }
  }
  else if (leftSensor == LOW) {
   turnLeft(50);
  }
  else if (rightSensor == LOW) {
    turnRight(50);
  }
}