#include <AFMotor.h>
 
static constexpr int startupSleepTime = 5000;
static constexpr int loopSleepTime = 10;
 
// MOTOR DECLARATIONS
static AF_DCMotor motors[2] = {
  AF_DCMotor(2, MOTOR12_1KHZ), 
  AF_DCMotor(3, MOTOR12_1KHZ)
};
static constexpr int motorSpeed = 200;
static constexpr int LRTurnDelay = 1200;
 
// SENSOR DECLARATIONS
static constexpr int triggers[3] = {A0, A1, A2}, echoes[3] = {A3, A4, A5};
static constexpr float speedOfSound = 0.0343;
static float distances[3] = {0};
static float minDistance = 5.0;
 
enum SensorPosition {
  FRONT_LEFT = 0,
  FRONT_RIGHT = 2,
  BACK = 1
};
 
// MOTOR FUNCTIONS
void initMotors() {
  motors[0].setSpeed(motorSpeed);
  motors[1].setSpeed(motorSpeed);
}
 
void goForward() {
  motors[0].run(FORWARD);
  motors[1].run(BACKWARD);
}
 
void goBackward() {
  motors[0].run(BACKWARD);
  motors[1].run(FORWARD);
}
 
void turnLeft() {
  motors[0].run(BACKWARD);
  motors[1].run(BACKWARD);
  delay(LRTurnDelay);
}
 
void turnRight() {
  motors[0].run(FORWARD);
  motors[1].run(FORWARD);
  delay(LRTurnDelay);
}
 
void brake() {
  motors[0].run(RELEASE);
  motors[1].run(RELEASE);  
}
 
// SENSOR FUNCTIONS
void initSensors() {
  for (int i = 0; i < 3; i++) {
    pinMode(triggers[i], OUTPUT);
    pinMode(echoes[i], INPUT);
  }
}
 
void sendSensorsImpulse() {
  for (int i = 0; i < 3; i++) {
    // Trigger sensor   
    digitalWrite(triggers[i], LOW);
    delayMicroseconds(2);
    digitalWrite(triggers[i], HIGH);
    delayMicroseconds(10);
    digitalWrite(triggers[i], LOW);
 
    // Compute distance
    float durations = pulseIn(echoes[i], HIGH);
    distances[i] = (durations / 2) * speedOfSound;
  }
}
 
bool isCollision(SensorPosition pos) {
  return distances[pos] <= minDistance;
}
 
void setup() {
  delay(startupSleepTime);
 
  // Enable serial debugging
  Serial.begin(9600);
 
  initMotors();
  initSensors();
}
 
void loop() {
  sendSensorsImpulse();
 
  // Check for collisions:
  // Left and right side collision
  if(isCollision(SensorPosition::FRONT_LEFT) && isCollision(SensorPosition::FRONT_RIGHT)) {
    goBackward();
    delay(LRTurnDelay);
  }
  // Left side collision
  else if(isCollision(SensorPosition::FRONT_LEFT)) {
    turnRight();
    goForward();
  }
  // Right side collision
  else if(isCollision(SensorPosition::FRONT_RIGHT)) {
    turnLeft();
    goForward();
  }
 
  // Back side collision
  if(isCollision(SensorPosition::BACK)) {
    goForward();
  }
 
  delay(loopSleepTime);
}