// Inspired by
// https://www.hackster.io/deligence-technologies/obstacle-detection-bot-using-ultrasonic-sensors-and-arduino-cde0b2

// 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 
// 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

// const int vADef = 105 , vBDef = 90;   // ROBOT 3


// Obstacle limit distance in cm
const float limit = 30;




/////////////////////////////////////////////////////////////
// Function prototypes
/////////////////////////////////////////////////////////////

// Motor functions
void mForward(int vA, int vB);
void mBackward(int vA, int vB);
void mRotateLeft(int vA, int vB);
void mRotateRight(int vA, int vB);
void mStop();

// Ultrasonic distance measurement function
float getDistance(int trigPin, int echoPin);


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);

  // Stop the motors and wait, so that the motors do not start immediately after loading the code, or after connecting to pc
  mStop();
  delay(7000);
  
  // Test motors: forward, right, left, backword
  //testMotors();

  // Now go!
  mForward(vADef, vBDef);  // start
}


void loop()
{
//  loop1Sensor();   // One sensor
  loop3Sensors();   // Three sensors
}


//////////////////////////////////////////////////////////////
// One sensor
//////////////////////////////////////////////////////////////

void loop1Sensor()
{
  float middleDistance = getDistance(TrigM, EchoM);
  if (middleDistance < limit) {
    mStop();
    delay(500);
//    mBackward(vADef, vBDef);  // optional backward/delay/stop/delay; problem: when going back, the obstacle is out of reach and the robot goes forward again! Loop...
//    delay(300);
//    mStop();
//    delay(300);
    mRotateLeft(vADef, vBDef);    // rotate left....
    do {              // always reading the sensor: continue rotating if there is an obstacle
      middleDistance = getDistance(TrigM, EchoM);
    } while (middleDistance < limit + 1);
    mStop();
    mForward(vADef, vBDef);
  }  // if
}



//////////////////////////////////////////////////////////////
// Three sensors
//////////////////////////////////////////////////////////////

int block = 0; // Used to manage la variabile block serve per gestire situazioni
                //di stallo; se per 5 volte il robot non
                //riesce a muoversi in avanti, effettuerà
                //una rotazione su se stesso per un tempo casuale
                //in questo modo le letture dei sensori saranno cambiate
                //e il robot dovrebbe riuscire a trovare il
                //modo giusto per andare via

void loop3Sensors()
{
// Three variables for the three distances read by the US sensors
  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);

  if (middleDistance < limit) { // front obstacle within "limit" cm, stop
    mStop();
    block++;
    delay(500);
    if (leftDistance >= rightDistance) { // turn left if left obstacle is farther than left one
      mRotateLeft90();
    } else {                             // else turn right
      mRotateRight90();
    }
  } else if (leftDistance < limit/2) {  // left (not front) *very near* obstacle, stop then turn a little right
    mStop();
    block++;
    delay(500);
    mRotateRight45();
  } else if (rightDistance < limit/2) { // right (not front) *very near* obstacle, stop then turn a little left
    mStop();
    block++;
    delay(500);
    mRotateLeft45();
  } else if (middleDistance < 2 * limit) { // far front obstacle, within 2 * "limit" (but not within "limit"), decelerate
    mForward(vADef * 0.8, vBDef * 0.8);
    block = 0;
  } else {
    mForward(vADef, vBDef);  // otherwise, the default speed
    block = 0;
  }

  if (block > 5 ) {   // if for five times the robot was blocked, try to get out of the (probably symmetric) obstacle structure with a random rotation
    mRotateLeft(vADef, vBDef);
    long randomDelay = random(200, 1000);
    delay(randomDelay); 
    mStop();
  }
 
}



/////////////////////////////////////////////////////////////
// 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);
}

void mRotateRight90() {
  mRotateRight(vADef, vBDef);
  delay(350);
  mStop();
}

void mRotateLeft90() {
  mRotateLeft(vADef, vBDef);
  delay(350);
  mStop();
}

void mRotateRight45() {
  mRotateRight(vADef, vBDef);
  delay(175);
  mStop();
}

void mRotateLeft45() {
  mRotateLeft(vADef, vBDef);
  delay(175);
  mStop();
}

///////////////////////////////////////////////////////////////////////////
// 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)
}


/////////////////////////////////////////////////////
// TEST MOTORS
/////////////////////////////////////////////////////

void testMotors()
{
  mForward(vADef, vBDef);
  delay(2000);
  mStop();
  delay(1000);
  mRotateRight90();
  delay(1000);
  mRotateLeft90();
  delay(1000);
  mBackward(vADef, vBDef);
  delay(2000);
  mStop();
  delay(3000);
}



/////////////////////////////////////////////////////
// OTHER STUFF
/////////////////////////////////////////////////////

void loop1()
{
  float leftDistance, rightDistance, middleDistance;
  leftDistance = getDistance(TrigL, EchoL);
  delay(10);
  middleDistance = getDistance(TrigM, EchoM);
  delay(10);
  rightDistance = getDistance(TrigR, EchoR);
  delay(10);
  Serial.println("leftDistance\tmiddleDistance\trightDistance\tStatus\n");
  Serial.print(leftDistance);
  Serial.print("cm\t\t");
  Serial.print(middleDistance);
  Serial.print("cm\t\t");
  Serial.print(rightDistance);
  Serial.print("cm\t\t");

  // if the obstacle is in front
  if (middleDistance <= 50)
  {
    /*  _mStop();
      delay(1000);*/

    if (rightDistance > leftDistance)   // front left
    {
      if ((rightDistance <= 30) && (leftDistance <= 30))  // front left AND near enough, stop, go back, but don't go right!?
      { mStop();
        delay(200);
        mBackward(vADef, vBDef);
        delay(1000);
      }
      else {
        //   mBackward();
        //  delay(3000);
        mRotateRight(vADef, vBDef);
        delay(500);
      }
    }
    else if (rightDistance < leftDistance)
    {
      if ((rightDistance <= 30) && (leftDistance <= 30))
      {
        mStop();
        delay(200);
        mBackward(vADef, vBDef);
        delay(1000);
      }
      else {
        //      mBackward(vADef, vBDef);
        //   delay(3000);
        mRotateLeft(vADef, vBDef);
        delay(500);
      }
    }
  }   // if middleDistance <= 50
  else if (rightDistance <= 15)   // middleDistance > 50
  {
    mRotateLeft(vADef, vBDef);
    delay(500);
  }
  else if (leftDistance <= 15)
  {
    mRotateRight(vADef, vBDef);
    delay(500);
  }
  else {
    mForward(vADef, vBDef);
  }
  //delay(500);

}


void loop2()
{
  mForward(vADef, vBDef);
}




// Decrease speed when facing an obstacle

void loop3()
{
  int r = (int) getDistance(TrigM, EchoM);

  Serial.print("middleDistance ");
  Serial.print(r);
  Serial.print("cm\n");

  if (r > 35 || r == 0) //lo zero è dovuto a possibili errori di recezione del segnale
  {
    mForward(vADef, vBDef);
  }
  else if (r <= 35 && r > 25)
  {
    mForward(vADef * 0.9, vBDef * 0.9);
  }
  else if (r <= 25 && r > 15)
  {
    mForward(vADef * 0.8, vBDef * 0.8);
  }
  else if ( r <= 15 && r != 0) //lo zero è dovuto a possibili errori di ricezione del segnale, ora pero' la getDistance non puo' restituire 0
  {
    mStop();
    delay(500);
    //    mBackward(vADef * 0.8, vBDef * 0.8);
    //    delay(500);
    //    mStop();
    //    delay(500);
    mRotateLeft(vADef * 0.8, vBDef * 0.8);  // begins to turn left
    while ((r = (int) getDistance(TrigM, EchoM)) < 15) {
      Serial.print("middleDistance while turning");
      Serial.print(r);
      Serial.print("cm\n");
    };
  }

}