/*
  :Version 1.0
  :Author: Dragos Calin
  :Email: [email protected]
  :License: BSD
  :Date: 06/04/2020
*/
 
/*define the sensor's pins*/
uint8_t lefttrigPin = 53;
uint8_t leftechoPin = A0;

uint8_t cenlefttrigPin = 51;
uint8_t cenleftechoPin = A1;

uint8_t centrigPin = 49;
uint8_t cenechoPin = A2;

uint8_t cenrighttrigPin = 47;
uint8_t cenrightechoPin = A3;

uint8_t righttrigPin = 45;
uint8_t rightechoPin = A4;
 
unsigned long timerStart = 0;
int TIMER_TRIGGER_HIGH = 10;
int TIMER_LOW_HIGH = 2;
 
float left_time, cenleft_time, cen_time, cenright_time, right_time;

float left_distance, cenleft_distance, cen_distance, cenright_distance, right_distance;
 
float left_distance_final, cenleft_distance_final, cen_distance_final, cenright_distance_final, right_distance_final;

int count;
int sensors_reading[5];


/*The states of an ultrasonic sensor*/
enum SensorStates {
  TRIG_LOW,
  TRIG_HIGH,
  ECHO_HIGH
};
 
SensorStates _sensorState = TRIG_LOW;
 
void startTimer() {
  timerStart = millis();
}
 
bool isTimerReady(int mSec) {
  return (millis() - timerStart) < mSec;
}
 
/*Sets the data rate in bits per second and configures the pins */
void setup() {
  Serial.begin(9600);
  pinMode(lefttrigPin, OUTPUT);
  pinMode(leftechoPin, INPUT);

  pinMode(cenlefttrigPin, OUTPUT);
  pinMode(cenleftechoPin, INPUT);
  
  pinMode(centrigPin, OUTPUT);
  pinMode(cenechoPin, INPUT);
  
  pinMode(cenrighttrigPin, OUTPUT);
  pinMode(cenrightechoPin, INPUT);
  
  pinMode(righttrigPin, OUTPUT);
  pinMode(rightechoPin, INPUT);

  us_sensors();
  for(int i=0; i <5; i++){
    Serial.print(sensors_reading[i]);
    Serial.print(" ");
  }
}
 
int us_sensors() {
  for (int iter = 0; iter< 31; iter++) {
    /*Switch between the ultrasonic sensor states*/
    switch (_sensorState) {
      /* Start with LOW pulse to ensure a clean HIGH pulse*/
      case TRIG_LOW: {
          digitalWrite(lefttrigPin, LOW);
          digitalWrite(cenlefttrigPin, LOW);
          digitalWrite(centrigPin, LOW);
          digitalWrite(cenrighttrigPin, LOW);
          digitalWrite(righttrigPin, LOW);

          startTimer();
          if (isTimerReady(TIMER_LOW_HIGH)) {
            _sensorState = TRIG_HIGH;
          }
        } break;
        
      /*Triggered a HIGH pulse of 10 microseconds*/
      case TRIG_HIGH: {
          digitalWrite(lefttrigPin, HIGH);
          digitalWrite(cenlefttrigPin, HIGH);
          digitalWrite(centrigPin, HIGH);
          digitalWrite(cenrighttrigPin, HIGH);
          digitalWrite(righttrigPin, HIGH);
          startTimer();
          if (isTimerReady(TIMER_TRIGGER_HIGH)) {
            _sensorState = ECHO_HIGH;
          }
        } break;
  
      /*Measures the time that ping took to return to the receiver.*/
      case ECHO_HIGH: {
          digitalWrite(lefttrigPin, LOW);
          left_time = pulseIn(leftechoPin, HIGH);
          left_distance = left_time * 0.034/2;
          left_distance_final = left_distance_final + left_distance;

          digitalWrite(cenlefttrigPin, LOW);
          cenleft_time = pulseIn(cenleftechoPin, HIGH);
          cenleft_distance = cenleft_time * 0.034/2;
          cenleft_distance_final = cenleft_distance_final + cenleft_distance;

          digitalWrite(centrigPin, LOW);
          cen_time = pulseIn(cenechoPin, HIGH);
          cen_distance = cen_time * 0.034/2;
          cen_distance_final = cen_distance_final + cen_distance;

          digitalWrite(cenrighttrigPin, LOW);
          cenright_time = pulseIn(cenrightechoPin, HIGH);
          cenright_distance = cenright_time * 0.034/2;
          cenright_distance_final = cenright_distance_final + cenright_distance;

          digitalWrite(righttrigPin, LOW);
          right_time = pulseIn(rightechoPin, HIGH);
          right_distance = right_time * 0.034/2;
          right_distance_final = right_distance_final + right_distance;

          count = count + 1;

          _sensorState = TRIG_LOW;
        } break;
        
    }//end switch
  
  }//end loop
  sensors_reading[0] = left_distance_final/count;
  sensors_reading[1] = cenleft_distance_final/count;
  sensors_reading[2] = cen_distance_final/count;
  sensors_reading[3] = cenright_distance_final/count;
  sensors_reading[4] = right_distance_final/count;

  left_distance_final = 0;
  cenleft_distance_final = 0;
  cen_distance_final = 0;
  cenright_distance_final = 0;
  right_distance_final = 0;
  count = 0;
}

// bool forward_check() {
//   //Checking if it is moving forward and distance is less than 40m
//   if ((sensors_reading[2] < 40) && (move_forward == true) { 
//     //If there is an obstace in fornt, check right & left sensor.
//     //If distance of left sensor is > right, turn 90 deg left.
//     if ((sensors_reading[0]) > sensors_reading[4]) {
//       //turn 270 deg.
//       //Make this turned direction forward now.
//       //I guess calling forward check again will do it.
//     }
//     //If distance of right sensor is > left, turn 90 deg right.
//     else {
//       //turn 90 deg.
//     }
//   }
// }

void loop(){
  us_sensors();
  for(int i=0; i <5; i++){
    Serial.print(sensors_reading[i]);
    Serial.print(" ");
  }
  Serial.print("\n");
  // if (sensors_reading[2] > 40){
  //   forward();
  // }
  // if ((sensors_reading[2] < 40) && (move_forward == true) {

  }
  //

//   if ((2 > 1) && (5 > 2)) {
//     Serial.println("2 is greater than 1");
//   }
//   else {
//     if (2 < 3) {
//       Serial.print("2 is less than 3\n");
//     }

//   }

// }

// Now I am trying to create an array which will store where are the obstacles
// Taking inputs from Ultrasonics Sensor
// stored inputs to obstacle in array
// Direct them to the global Obstacle Detector

// Declaring array to store Global Obstacle Detector
// int GOD[];
//Motor Code
// if