// Inspired by https://www.engineersgarage.com/contribution/arduino-based-wall-following-robot

// Three sensors; here are the pin definitions for left/middle/right sensors

const int EchoL = 2; //LEFT_SENSOR ECHO
const int TrigL = 9; //LEFT_SENSOR TRIG
const int EchoM = 11; //MID_SENSOR ECHO
const int TrigM = 10; //MID_SENSOR TRIG
const int EchoR = 13; //RIGHT_SENSOR ECHO
const int TrigR = 12; //RIGHT_SENSOR TRIG

// MOTORS: right motor uses ENA with IN1 and IN2; left motor uses ENB with IN3 and IN4
const int IN1 = 8;
const int IN2 = 7;
const int IN3 = 4;
const int IN4 = 5;
const int ENA = 6;   // RIGHT MOTOR
const int ENB = 3;   // LEFT MOTOR

// Here we define two constant values for default motor speeds; to be adjusted!
// A = RIGHT motor, B = LEFT motor
// If the robot turns left (anticlockwise), the right motor is too fast (or the left one is slow), reduce vADef or increase vBDef
// If the robot turns right (clockwise), the left motor is too fast (or the right one is slow), reduce vBDef or increase vADef

// const int vADef = 113, vBDef = 110;   // ROBOT 0    DA SISTEMARE
// const int vADef = 100, vBDef = 95;   // ROBOT 1
// const int vADef = 100, vBDef = 100;   // ROBOT 2
// const int vADef = 115, vBDef = 90;   // ROBOT 3
// const int vADef = 100, vBDef = 90;   // ROBOT 4
// const int vADef = 125, vBDef = 125;   // ROBOT 5
// const int vADef = 105, vBDef = 95;   // ROBOT 6
// const int vADef = 125, vBDef = 95;   // ROBOT 7
const int vADef = 105, vBDef = 90;   // ROBOT 8


// The following block of code initializes the range of values which arduino should signal the motor driver to take a right
// turn or left turn or drive forward depending on the distance measured by the ultrasonic sensor.

// const int limit  = 10;
const int maxDistanceToWall = 25;
const int minDistanceToWall = 17;
const int delayAdjustToWall = 60;

bool flagWallWasFound;

void setup() {
  Serial.begin(9600);

  // Initialize the used pins
  pinMode(EchoL, INPUT);
  pinMode(TrigL, OUTPUT);
  pinMode(EchoM, INPUT);
  pinMode(TrigM, OUTPUT);
  pinMode(EchoR, INPUT);
  pinMode(TrigR, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  flagWallWasFound =  false;

  statusWaitFunction();
  mForward(vADef, vBDef);
  delay(200);

}

void loop() {

  float leftDistance, rightDistance, middleDistance;
  leftDistance   = getDistance(TrigL, EchoL);
  delay(10);    // It is better to wait for US to fade
  middleDistance = getDistance(TrigM, EchoM);
  delay(10);
  rightDistance  = getDistance(TrigR, EchoR);


  // The main operation in the code is implemented by a decision making loop which is called in the loop() function.
  // Measure the distance between the robot and the front obstacle if present, or the wall, and the robot and the  wall the robot is moving along


  // The following if condition is used to check the distance between the front sensor and the object, if the object is
  // closer according to pre-determined range, it calls the mBackward(), mRotateLeft() and the mForward function with some delay in-between.

  //  if (middleDistance < limit)
  if (false)
  {
    mStop();
    delay(500);
    mBackward(vADef, vBDef);
    delay(400);
    mRotateLeft(vADef, vBDef);
    delay(300);
    mForward(vADef, vBDef);
  }
  // The following if condition is used to check the distance between the side wall
  // sensor and the side wall. If the distance measured by the sensor is at desired range it calls mForward() function.
  else if (rightDistance > minDistanceToWall && rightDistance < maxDistanceToWall)
  {
    mForward(vADef, vBDef);
    flagWallWasFound = true;
//    delay(200);
  }
  // The following if conditions are used to check the distance between the side wall and the sensor and check for closer and
  // far value. If the distance becomes close, it calls the function drive_left() and drive_forward() function and if the
  // distance becomes far it calls the function drive_right() and mForward() function.
  else if (rightDistance < minDistanceToWall)
  {
    mRotateLeft(vADef, vBDef);
    delay(delayAdjustToWall);
    mForward(vADef, vBDef);
    //   delay(delayAdjustToWall);
    //   mRotateRight(vADef, vBDef);
    //  delay(delayAdjustToWall);
    //    mForward(vADef, vBDef);
  }
  else if (rightDistance > maxDistanceToWall)
    if (flagWallWasFound)
    {
      mRotateRight(vADef, vBDef);
      delay(delayAdjustToWall);
      mForward(vADef, vBDef);
      //   delay(delayAdjustToWall);
      //   mRotateLeft(vADef, vBDef);
      //   delay(delayAdjustToWall);
      //   mForward(vADef, vBDef);
    } else  mForward(vADef, vBDef);

}  // end loop


void statusWaitFunction()
{
  mStop();
  delay(7000);
  mForward(vADef, vBDef);  // start
}





/////////////////////////////////////////////////////////////
// Motor functions
/////////////////////////////////////////////////////////////

// To move the robot forward, the right motor must rotate clockwise (LOW/HIGH)
// while the left motor must rotate anti-clockwise (HIGH/LOW) (the opposite if
// the motor are connected with wires inverted).
// We first set motor rotation, then we five the PWM signal.

void mForward(int vA, int vB)
{
  Serial.println("ROBOT_MOVING_FORWARD");
  digitalWrite(IN1, HIGH);  // SET RIGHT MOTOR CCW
  digitalWrite(IN2, LOW);   // SET RIGHT MOTOR CCW
  digitalWrite(IN3, LOW);   // SET LEFT MOTOR CW
  digitalWrite(IN4, HIGH);  // SET LEFT MOTOR CW
  analogWrite(ENA, vA);    // GIVE CURRENT TO PWM PIN, RIGHT MOTOR
  analogWrite(ENB, vB);    // GIVE CURRENT TO PWM PIN, LEFT MOTOR
}

// Like mForward, but exactly the opposite, so the right motor LOW/HIGH and
// the left motor HIGH/LOW.

void mBackward(int vA, int vB)
{
  Serial.println("ROBOT_MOVING_BACKWARD");
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, vA);
  analogWrite(ENB, vB);
}

// Robot rotates left: wheels must turn in opposite directions, so the motors
// must be given HIGH/LOW both.

void mRotateLeft(int vA, int vB)
{
  Serial.println("ROBOT_MOVING_LEFT");
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, vA);
  analogWrite(ENB, vB);
}

// Like mRotateLeft, but the opposite: LOW/HIGH and LOW/HIGH.

void mRotateRight(int vA, int vB)
{
  Serial.println("ROBOT_MOVING_RIGHT");
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  analogWrite(ENA, vA);
  analogWrite(ENB, vB);
}

// To stop the motors, just give LOW everywhere.
void mStop()
{
  Serial.println("ROBOT_STOP");
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
}

///////////////////////////////////////////////////////////////////////////
// Ultrasonic distance measurement function (cm) with lock error correction
///////////////////////////////////////////////////////////////////////////

float getDistance(int trigPin, int echoPin) // returns the distance (cm)
{
  long duration;
  float distance;
  digitalWrite(trigPin, HIGH);   // We send a 10us trigger pulse: HIGH...
  delayMicroseconds(10);         // ...wait...
  digitalWrite(trigPin, LOW);    // LOW!
  duration = pulseIn(echoPin, HIGH, 20000); // We wait for the echo to come back, with a
  // timeout of 20ms, which corresponds
  // approximately to 3 m (exactly 343 cm);
  // pulseIn will return 0 if it timed out.
  // (or if echoPin was already to 1, but it should not happen)
  if (duration == 0) // If we time out, sometimes the sensor hangs! In this case we manually set the echoPin to LOW, then prepare a large duration value.
  {
    pinMode(echoPin, OUTPUT); // Then we set echo pin to output mode
    digitalWrite(echoPin, LOW); // We send a LOW pulse to the echo pin
    delayMicroseconds(200);
    pinMode(echoPin, INPUT); // And finally we come back to input mode
    duration = 20000;  // if it timed out, return 20 ms -> 343 cm
  }
  distance = (duration / 2.0) * 0.0343; // /2 because the pulse is going and coming back; also, /29.1
  return distance;                      // We return the result (0 or 343 cm if we timed out, see the "if" statement above)
}