//STEERING POSITIONS:
//Neutral:90
//Left: 55
//Right: 125

//THROTTLE POSITIONS
//Full Reverse: 0
//Full Ahead: 180
//Normal Reverse: 75
//Normal Ahead: 105
//Neutral: 90

#include <Servo.h>
#include <avr/interrupt.h>

Servo steering;
Servo throttle;
int FrontSensorPin = 2;
int LeftSensorPin = 3;
int RightSensorPin = 4;
int LEDPin = 13;
int AccelerationSpeed = 103;
int ReverseSpeed = 77;
int Neutral = 90;
int LeftTurn = 55;
int RightTurn = 125;
int leftsensorState = 0;
int rightsensorState = 0;
volatile int frontsensorstate;

void setup() {
  steering.attach(5);
  throttle.attach(6);
  pinMode(FrontSensorPin, INPUT);
  pinMode(LeftSensorPin, INPUT);
  pinMode(RightSensorPin, INPUT);
  pinMode(LEDPin, OUTPUT);
  digitalWrite(FrontSensorPin, HIGH);
  attachInterrupt(0, frontsensorISR, FALLING);
  interrupts();
  frontsensorstate = 0;
  MoveForward();
}

void frontsensorISR(){
    FrontBrakes();
    frontsensorstate = 1;
    digitalWrite(LEDPin, LOW);
}

void FrontBrakes() {
      throttle.write(ReverseSpeed);
}

void Stop() {
  throttle.write(Neutral);
}

void MoveForward(){
  throttle.write(AccelerationSpeed);
  digitalWrite(LEDPin, HIGH);
}

void TurnLeft(){
  while(digitalRead(RightSensorPin) < 1){
    steering.write(LeftTurn);
  }
  delay(100);
  steering.write(Neutral);
}

void TurnRight(){
  while(digitalRead(LeftSensorPin) < 1){
    steering.write(RightTurn);
  }
  delay(100);
  steering.write(Neutral);
}

void BackUp() {
    throttle.write(ReverseSpeed);
    delay(2500);
    Stop();
    delay(500);
}

void ReverseAway(){
    throttle.write(ReverseSpeed);
    delay(1350);
    Stop();
    delay(500);
    steering.write(Neutral);
    delay(500);
    MoveForward();
}

void loop() {
  steering.write(Neutral); //set steering to neutral - can't do this in setup
  if (frontsensorstate > 0){  //sensor state value - this is set high in interrup routine
    frontsensorstate = 0; //reset sensor state
    delay(200); //need to wait before brakes are released
    Stop(); //release brakes, set throttle to neutral
    delay(200); //need to wait before re-engaging throttle
    if (digitalRead(LeftSensorPin) < 1) { //check if obstacles present to left
      steering.write(RightTurn);
      ReverseAway();
    }
    if (digitalRead(RightSensorPin) < 1) { //check if obstacles present to right
      steering.write(LeftTurn);
      ReverseAway();
    }
    if (digitalRead(RightSensorPin) > 0 && digitalRead(LeftSensorPin) > 0) { //check if no obstacles present either side
      steering.write(LeftTurn);
      ReverseAway();
    }
    if (digitalRead(RightSensorPin) < 1 && digitalRead(LeftSensorPin) < 1) { //check if obstacles present either side
      BackUp();
    }
  }

 if (digitalRead(LeftSensorPin) < 1) { //check if obstacles present to left
   TurnRight();
 }

  if (digitalRead(RightSensorPin) < 1) { //check if obstacles present to right
   TurnLeft();
 }

  if (digitalRead(LeftSensorPin) < 1 && digitalRead(LeftSensorPin) < 1) { //check if obstacles present either side
   throttle.write(Neutral);
  }
}