#include <Servo.h>
// Include the TimerOne Library from Paul Stoffregen
#include "TimerOne.h"

const int ENA = 6;
const int IN1 = 7;
const int IN2 = 5;
const int IN3 = 4;
const int IN4 = 10;
const int ENB = 9;
const int trigPin = 13;
const int echoPin = 12;
int Tentativi = 0;
int lSPEED = 160;
int rSPEED = 160;
int SensorAngle [7] = {0, 30, 60, 90, 120, 150, 180};
int SensorDistance [7] = {0, 0, 0, 0, 0, 0, 0};
int Angle ;
int Distance = 0;
int OldDistance;
int DistanzaLetta;
int MoveT;
float rotation1;
float rotation2;
boolean MOTOR = true;
boolean MoveHead = true;
// Integers for pulse counters
unsigned int counter1 = 0;
unsigned int counter2 = 0;
float diskslots = 20;
bool flag = false;
Servo servoHead;                    // servo instance
const byte MOTOR1 = 2;  // Motor 1 Interrupt Pin - INT 0
const byte MOTOR2 = 3;  // Motor 2 Interrupt Pin - INT 1

void ISR_timerone()
{
  Timer1.detachInterrupt();  // Stop the timer
  Serial.print("Motor Speed 1: ");
  float rotation1 = (counter1 / diskslots) * 60.00;  // calculate RPM for Motor 1
  Serial.print(rotation1);
  Serial.print(" RPM - ");
  counter1 = 0;  //  reset counter to zero
  Serial.print("Motor Speed 2: ");
  float rotation2 = (counter2 / diskslots) * 60.00;  // calculate RPM for Motor 2
  Serial.print(rotation2);
  Serial.println(" RPM");
  counter2 = 0;  //  reset counter to zero
  Timer1.attachInterrupt( ISR_timerone );  // Enable the timer
}

void setup() {
  Serial.begin(9600);
  pinMode(ENA, OUTPUT);             // left motor speed control
  pinMode(IN1, OUTPUT);             // left motor control
  pinMode(IN2, OUTPUT);             // left motor control
  pinMode(IN3, OUTPUT);             // right motor control
  pinMode(IN4, OUTPUT);             // right motor control
  pinMode(ENB, OUTPUT);             // right motor speed control
  pinMode(trigPin, OUTPUT);         // Ultrasonic sensor
  pinMode(echoPin, INPUT);          // Ultrasonic sensor
  pinMode(2, INPUT);                // left motor interrupt 0
  servoHead.attach(11);             // servo pin for 'head'
  Angle = SensorAngle [3];
  ServoScan();
  meanDistance();
  rSPEED = 180;
  lSPEED = 180;
  Timer1.initialize(1000000); // set timer for 1sec
  attachInterrupt(digitalPinToInterrupt (MOTOR1), contastep1, RISING);  // Increase counter 1 when speed sensor pin goes High
  attachInterrupt(digitalPinToInterrupt (MOTOR2), contastep2, RISING);  // Increase counter 2 when speed sensor pin goes High
  Timer1.attachInterrupt( ISR_timerone ); // Enable the timer
}

void loop() {

  if (meanDistance() > 30) {
    MoveForward();
    OldDistance = DistanzaLetta;
    Tentativi = 0;
  }
  while (meanDistance() > 30) {

    if (OldDistance == DistanzaLetta) {
      Tentativi++;  // Motors goes but distance is always the same
    }
    else {
      Tentativi = 0;
    }
    if (Tentativi >= 3) {
      MoveStop();
      while (meanDistance() > 30 && DistanzaLetta == OldDistance) {
        Serial.println("WAITING TO BE MOVED MANUALLY");
      }
      Tentativi = 0;
    }
    OldDistance = DistanzaLetta;
  }
  ChooseDirection();
}

void ChooseDirection() {
  MoveStop();
  for (Tentativi = 1; Tentativi <= 3; Tentativi++) {
    for (int a = 0; a < 7; a++) {           /* distance measuring */
      Angle = SensorAngle [a];
      ServoScan();
      SensorDistance [a] = meanDistance();
    }
    int Dir2Go = 0;                         /* choose where ia it more space found to move? */
    int MaxDistance = SensorDistance[0];
    for ( int a = 0 ; a < 7 ; a++) {
      if (SensorDistance[a] > MaxDistance) {
        Dir2Go = a;
        MaxDistance = SensorDistance [a];
      }
    }
    if (MaxDistance <= 30) {
      MoveT = 500;                 // indietro
      Serial.println("BACKWARD");
      MoveBack();
      MoveT = 600;
      MoveRight();
      MoveStop();
    }
    else {
      if (Dir2Go < 3) {           // sinistra
        Serial.println("LEFT");
        MoveT = 500;
        MoveBack();
        MoveT = (200  * (3 - Dir2Go));
        MoveLeft();
        MoveStop();
        break;
      }
      else {                        //destra
        Serial.println("RIGHT");
        MoveT = 500;
        MoveBack();
        MoveT = (200 * (Dir2Go - 3));
        MoveRight();
        MoveStop();
        break;
      }
    }
  }
  Angle = SensorAngle [3];
  ServoScan();
  if (Tentativi >= 3) {
    //  CANT FIND ENOUGH SPACE TO MOVE!
    while (meanDistance() <= 30) {
      Serial.println("PANIC!");
    }
  }
  else {
    Tentativi = 0;
  }
}
// ------------------------------------------------------------------------------------------
void ServoScan()  {
  if (MoveHead == true) {
    Serial.println ("SERVOSCAN");
    servoHead.write(Angle);
    delay(15);
  }
}
//-------------------------------------------------------------------------------------------
float pingSensor() // returns the distance (cm)
{
  long duration;
  float distance;
  digitalWrite(trigPin, HIGH); // We send a 10us pulse
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH, 20000); // We wait for the echo to come back, with a
  if (duration == 0) // if we timed out
  {
    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 finaly we come back to input mode
  }
  distance = (duration / 2.0) / 29.1;
  return distance; //We return the result. Here you can find a 0 if we timed out
}

//-------------------------------------------------------------------------
float meanDistance() {
  float mD = 0;                 // conterranno la media delle letture, e le letture stesse
  float DX [15];
  for (int a = 0; a < 15; a++) {
    DX[a] = pingSensor();
    delay(20);   // DONT TOUCH!!!!
  }
  for (int a = 0; a < 15; a++) {
    mD = mD + DX [a];
  }
  mD = mD / 15;
  DistanzaLetta = mD;
  return mD;
}

//---------------------------------------------------------------------------------------------

void MoveBack() {
  if (MOTOR) {
    Serial.println("INDIETRO");
    analogWrite(ENA, lSPEED);
    digitalWrite(IN1, HIGH );
    digitalWrite(IN2, LOW);
    analogWrite(ENB, rSPEED);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    delay (MoveT);
    MoveStop();
  }
}

void MoveStop() {
  Serial.println("STOP");
  analogWrite(ENA, 0);
  digitalWrite(IN1, LOW );
  digitalWrite(IN2, LOW);
  analogWrite(ENB, 0);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

void MoveForward() {
  Serial.println("FORWARD");
  if (MOTOR) {
    analogWrite(ENA, lSPEED);
    digitalWrite(IN1, LOW );
    digitalWrite(IN2, HIGH);
    analogWrite(ENB, rSPEED);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
  }
}

void MoveLeft() {
  if (MOTOR) {
    analogWrite(ENA, lSPEED);
    digitalWrite(IN1, HIGH );
    digitalWrite(IN2, LOW);
    analogWrite(ENB, rSPEED);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    delay (MoveT);
    MoveStop();
  }
}

void MoveRight() {
  if (MOTOR) {
    analogWrite(ENA, lSPEED);
    digitalWrite(IN1, LOW );
    digitalWrite(IN2, HIGH);
    analogWrite(ENB, rSPEED);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    delay (MoveT);
    MoveStop();
  }
}

void contastep1() {
  counter1++;
}
void contastep2()  
{
  counter2++;  // increment Motor 2 counter value
}