// Comportamento misto:
// (0) stato iniziale di attesa, per dare il tempo di porre il robot nell'ambiente;
// (1) esploratore: gira evitando gli ostacoli (in futuro potra' anche, per esempio, seguire la luce),
//     comportamento seguito per un tempo casuale o per un tempo fisso (al momento, per 10 secondi), 
//     poi passa al comportamento 2;
// (2) alla ricerca di un posto comodo per dormire: siccome il robot... ama gli spazi aperti, gira intorno a
//     se stesso per un paio di secondi: se non trova ostacoli a meno di 30-50 cm, si ferma e
//     passa allo stato 3; se trova ostacoli, ripassa allo stato 1;
//     (in realta' serve perche' nello stato 4 il robot vuole spazio per arretrare!)
// (3) robot addormentato, fermo ma vigile (magari, con un cicalino, lo facciamo russare...);
//     se sente un ostacolo davanti a se', il robot si sveglia e si spaventa, e passa allo stato 4;
// (4) robot in fuga: arretra velocemente, poi gira in una direzione fissa o a caso (al momento gira a destra) 
//     e torna al comportamento 1;
//
//                      ____________________
//                     |                    |
//                     v                    |
//              0 ---> 1 ---> 2 ---> 3 ---> 4
//                     ^      |
//                     |______|

#define statusWait         0
#define statusExplore      1
#define statusSearchForBed 2
#define statusSleep        3
#define statusEscape       4

int robotStatus = 0;

// 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 = 120, vBDef = 120;   // ROBOT 5    (LEFT MOTOR WANTS A LARGER VALUE)
// const int vADef = 105, vBDef = 95;   // ROBOT 6
const int vADef = 125, vBDef = 95;   // ROBOT 7
// const int vADef = 105, vBDef = 90;   // ROBOT 8


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

// Will contain start and current time (since arduino was turned on) in ms
unsigned long startTime, currentTime;

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

  robotStatus = statusWait;
}

void loop() {
  switch (robotStatus)
  {
    case statusWait:   // Stop the motors and wait, so that the motors do not start immediately after loading the code, or after connecting to the pc
      {
        statusWaitFunction();
        break;
      }
    case statusExplore:
      {
        statusExploreFunction();
        break;
      }
    case statusSearchForBed:
      {
        statusSearchForBedFunction();
        break;
      }
    case statusSleep:
      {
        statusSleepFunction();
        break;
      }
    case statusEscape:
      {
        statusEscapeFunction();
        break;
      }
  }
}  // end loop

void statusWaitFunction()
{
  mStop();
  delay(7000);
  robotStatus = statusExplore;
  mForward(vADef, vBDef);  // start
  startTime = millis();
}

void statusExploreFunction()
{
  loop3Sensors();   // Three sensors
  currentTime = millis();
  if (currentTime - startTime > 10000)
    robotStatus = statusSearchForBed;
}

void statusSearchForBedFunction()
{
  float middleDistance;
  bool flag = false;    // this flag is used to state that there was an obstacle within 30 cm
  startTime = millis();
  mRotateLeft(vADef, vBDef);    // rotate left....
  do {              // always reading the sensor: continue rotating if there is an obstacle
    middleDistance = getDistance(TrigM, EchoM);
    if (middleDistance < 30) flag = true;
    currentTime = millis();
  } while (currentTime - startTime < 3000 && flag == false);
  mStop();
  if (flag == true)
    robotStatus = statusExplore;
  else
    robotStatus = statusSleep;

}
void statusSleepFunction()
{
  mStop();
  float middleDistance;
  do {              // always reading the sensor: continue until there is an obstacle
    middleDistance = getDistance(TrigM, EchoM);
  } while (middleDistance > limit);
  robotStatus = statusEscape;
}

void statusEscapeFunction()
{
  mBackward(1.5 * vADef, 1.5 * vBDef);
  delay(600);
  mStop();
  delay(1000);
  mRotateRight90();
  robotStatus = statusExplore;
  startTime = millis();
}

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