// IR Sensors
int sensor1 = A0;      // Leftmost sensor
int sensor2 = A1;      // Left sensor
int sensor3 = A2;      // Middle sensor
int sensor4 = A3;      // Right sensor
int sensor5 = A4;      // Rightmost sensor
int sensor6 = A5;      //
 
int sensor[5] = {0, 0, 0, 0, 0,0};

// Motor Variables
int motorInput1 = 7;   // AIN1
int motorInput2 = 8;   // AIN2
int motorInput3 = 5;   // BIN1
int motorInput4 = 4;   // BIN2
int PWMA = 9;
int PWMB = 3;

// Initial Speed of Motor
int initial_motor_speed = 255;

// PID Constants
float Kp = 10;
float Ki = 0;
float Kd = 200;

float error = 0, P = 0, I = 0, D = 0, PID_value = 0;
float previous_error = 0, previous_I = 0;

void setup()
{
  pinMode(sensor1, INPUT);
  pinMode(sensor2, INPUT);
  pinMode(sensor3, INPUT);
  pinMode(sensor4, INPUT);
  pinMode(sensor5, INPUT);

  pinMode(motorInput1, OUTPUT);
  pinMode(motorInput2, OUTPUT);
  pinMode(motorInput3, OUTPUT);
  pinMode(motorInput4, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);

  Serial.begin(9600);
  Serial.println("Started !!");
}

void loop()
{
  read_sensor_values();

  if (error == 100 || error == 105) {
    do {
      analogWrite(PWMA, 100); // Left Motor Speed
      analogWrite(PWMB, 255); // Right Motor Speed
      sharpLeftTurn();
      read_sensor_values();
      if (error == 0) {
        stop_bot();
        delay(200);
      }
    } while (error != 0);
  }  else if (error == 101 || error == 104) {
    analogWrite(PWMA, 255); // Left Motor Speed
    analogWrite(PWMB, 100); // Right Motor Speed
    forward();
    delay(200);

    stop_bot();
    read_sensor_values();
    if (error == 102) {
      do {
        analogWrite(PWMA, 169); // Left Motor Speed
        analogWrite(PWMB, 132); // Right Motor Speed
        sharpRightTurn();
        read_sensor_values();
      } while (error != 0);
    }
  } else if (error == 102) {
    do {
      analogWrite(PWMA, 169); // Left Motor Speed
      analogWrite(PWMB, 132); // Right Motor Speed
      sharpLeftTurn();
      read_sensor_values();
      if (error == 0) {
        stop_bot();
        delay(200);
      }
    } while (error != 0);
  } else if (error == 103) {
    analogWrite(PWMA, 100); // Left Motor Speed
    analogWrite(PWMB, 255); // Right Motor Speed
    forward();
    delay(200);
    read_sensor_values();
  } 
  else {
    calculate_pid();
    motor_control();
  }
}

void read_sensor_values()
{
  sensor[0] = !digitalRead(sensor1);
  sensor[1] = !digitalRead(sensor2);
  sensor[2] = !digitalRead(sensor3);
  sensor[3] = !digitalRead(sensor4);
  sensor[4] = !digitalRead(sensor5);
  sensor[5] = !digitalRead(sensor6);

 if       
      ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) (sensor[5] == 0)
    error = -80;
  else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) && (sensor[5] == 0)
    error = -70;
  else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) && (sensor[5] == 0)
    error = -60;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) && (sensor[5] == 0)
    error = 0;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) && (sensor[5] == 0)
    error = 60;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) && (sensor[5] == 0)
    error = 0;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) && (sensor[5] == 0)
    error = 70; 
  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[3] == 1) && (sensor[4] == 0) && (sensor[2] == 1)) && (sensor[5] == 0) // Turn robot left side
    error = 100;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[3] == 1) && (sensor[4] == 1) && (sensor[2] == 1)) && (sensor[5] == 1) // Turn robot right side
    error = 101;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[3] == 0) && (sensor[4] == 0) && (sensor[2] == 0)) && (sensor[5] == 0) // Make U turn
    error = 102;
  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[3] == 1) && (sensor[4] == 0) && (sensor[2] == 1)) && (sensor[5] == 0) // Turn left side or stop
    error = 103;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[3] == 1) && (sensor[4] == 1) && (sensor[2] == 1)) && (sensor[5] == 1) // Turn right side or stop
    error = 104;
  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[3] == 1) && (sensor[4] == 0) && (sensor[2] == 0)) && (sensor[5] == 0) // Turn left side or stop
    error = 105;
}
}

void calculate_pid()
{
  P = error;
  I = I + previous_I;
  D = error - previous_error;

  PID_value = (Kp * P) + (Ki * I) + (Kd * D);

  previous_I = I;
  previous_error = error;
}

void motor_control()
{
  int left_motor_speed = initial_motor_speed - PID_value + 30;
  int right_motor_speed = initial_motor_speed + PID_value - 39;

  left_motor_speed = constrain(left_motor_speed, -255, 255);
  right_motor_speed = constrain(right_motor_speed, -255, 255);

  Serial.print(" | Error: ");
  Serial.print(error);
  Serial.print(" | PID: ");
  Serial.print(PID_value);
  Serial.print(" | Right Speed: ");
  Serial.print(left_motor_speed);
  Serial.print(" | Left Speed: ");
  Serial.println(right_motor_speed); 

  analogWrite(PWMB, left_motor_speed);
  analogWrite(PWMA, right_motor_speed);

  forward();
}

void forward()
{
  digitalWrite(motorInput1, HIGH);
  digitalWrite(motorInput2, LOW);
  digitalWrite(motorInput3, HIGH);
  digitalWrite(motorInput4, LOW);
}

void reverse()
{
  digitalWrite(motorInput1, LOW);
  digitalWrite(motorInput2, HIGH);
  digitalWrite(motorInput3, LOW);
  digitalWrite(motorInput4, HIGH);
}

void right()
{
  digitalWrite(motorInput1, HIGH);
  digitalWrite(motorInput2, LOW);
  digitalWrite(motorInput3, HIGH);
  digitalWrite(motorInput4, HIGH);
}

void left()
{
  digitalWrite(motorInput1, HIGH);
  digitalWrite(motorInput2, HIGH);
  digitalWrite(motorInput3, HIGH);
  digitalWrite(motorInput4, LOW);
}

void sharpRightTurn() {
  digitalWrite(motorInput1, HIGH);
  digitalWrite(motorInput2, LOW);
  digitalWrite(motorInput3, LOW);
  digitalWrite(motorInput4, HIGH);
}

void sharpLeftTurn() {
  digitalWrite(motorInput1, LOW);
  digitalWrite(motorInput2, HIGH);
  digitalWrite(motorInput3, HIGH);
  digitalWrite(motorInput4, LOW);
}

void stop_bot()
{
  digitalWrite(motorInput1, HIGH);
  digitalWrite(motorInput2, HIGH);
  digitalWrite(motorInput3, HIGH);
  digitalWrite(motorInput4, HIGH);
}