//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 FrontBumper = 2;
int RearBumper = 3;
int LeftSensorPin = 4;
int RightSensorPin = 5;
int forwardLED = 12;
int reverseLED = 11;

int AccelerationSpeed = 105;
int ReverseSpeed = 75;
int SteeringNeutral = 90;
int LeftTurn = 55;
int RightTurn = 125;
int leftsensorState = 0;
int rightsensorState = 0;
int rearsensorState = 0;
volatile int frontbumper;
volatile int rearbumper;


void setup() {
  steering.attach(9);
  throttle.attach(10);
  pinMode(forwardLED, OUTPUT);
  pinMode(reverseLED, OUTPUT);
  pinMode(FrontBumper, INPUT);
  pinMode(LeftSensorPin, INPUT);
  pinMode(RightSensorPin, INPUT);
  pinMode(RearBumper, INPUT);
  Serial.begin(115200);
  digitalWrite(FrontBumper, HIGH);
  digitalWrite(RearBumper, HIGH);
  attachInterrupt(0, frontbumperISR, FALLING);
  attachInterrupt(1, rearbumperISR, FALLING);
  interrupts();
  frontbumper = 0;
  rearbumper = 0;
  MoveForward();
}

void frontbumperISR(){
    Serial.println("Front Bumper Hit!!");
    FrontBrakes();
    frontbumper = 1;
}

void rearbumperISR(){
    Serial.println("Rear Bumper Hit!!");
    RearBrakes();
    rearbumper = 1;
}

void FrontBrakes() {
      throttle.write(70);
      Serial.println("Throttle at 70 (brakes engaged)");
      Serial.println('\n');
}

void RearBrakes() {
    throttle.write(90);
    Serial.println("Throttle at 0");
    Serial.println('\n');
}

void Stop() {
  throttle.write(90);
  Serial.println("Throttle at neutral");
  Serial.println('\n');
}

void MoveForward(){
  throttle.write(AccelerationSpeed);
  digitalWrite(forwardLED, HIGH);
  Serial.print("Throttle @ ");
  Serial.println(AccelerationSpeed);
  Serial.println('\n');
}

void loop() {
  steering.write(SteeringNeutral);
  leftsensorState = digitalRead(LeftSensorPin);
  rightsensorState = digitalRead(RightSensorPin);

  if (frontbumper > 0){
    frontbumper = 0;
    delay(200);
    Stop();
    delay(200);
    leftsensorState = digitalRead(LeftSensorPin);
    rightsensorState = digitalRead(RightSensorPin);
    if (leftsensorState < 1) {
      steering.write(RightTurn);
    }
    if (rightsensorState < 1) {
      steering.write(LeftTurn);
    }
    if (rightsensorState > 0 && leftsensorState > 0) {
      steering.write(LeftTurn);
    }
    throttle.write(ReverseSpeed);
    delay(1350);
    Stop();
    steering.write(SteeringNeutral);
    MoveForward();
  }

  if (rearbumper > 0){
    rearbumper = 0;
    delay(200);
    leftsensorState = digitalRead(LeftSensorPin);
    rightsensorState = digitalRead(RightSensorPin);
    if (leftsensorState < 1) {
      steering.write(RightTurn);
    }
    if (rightsensorState < 1) {
      steering.write(LeftTurn);
    }
    if (rightsensorState > 0 && leftsensorState > 0) {
      steering.write(LeftTurn);
    }
    throttle.write(AccelerationSpeed);
    delay(1350);
    Stop();
    steering.write(SteeringNeutral);
    MoveForward();
  }

 if (leftsensorState < 1) {
   while(leftsensorState < 1) {
     leftsensorState = digitalRead(LeftSensorPin);
     steering.write(RightTurn);
     Serial.print("Steering @ ");
     Serial.println(RightTurn);
     Serial.println('\n');
   }
 }

  if (rightsensorState < 1) {
   while(rightsensorState < 1) {
     rightsensorState = digitalRead(RightSensorPin);
     steering.write(LeftTurn);
     Serial.print("Steering @ ");
     Serial.println(LeftTurn);
     Serial.println('\n');
   }
 }

}