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

Servo steering;
Servo throttle;
int bumpswitchPin = 2;
int forwardLED = 12;
int reverseLED = 11;
int bumpswitchState = 0;
int AccelerationSpeed = 105;
int ReverseSpeed = 75;
int SteeringNeutral = 90;
int LeftTurn = 55;
int RightTurn = 125;
int LeftSensorPin = 3;
int RightSensorPin = 4;
int RearSensorPin = 5;
int leftsensorState = 0;
int rightsensorState = 0;
int rearsensorState = 0;
volatile int bumper;


void setup() {
  steering.attach(9);
  throttle.attach(10);
  pinMode(forwardLED, OUTPUT);
  pinMode(reverseLED, OUTPUT);
  pinMode(bumpswitchPin, INPUT);
  pinMode(LeftSensorPin, INPUT);
  pinMode(RightSensorPin, INPUT);
  pinMode(RearSensorPin, INPUT);
  Serial.begin(115200);
  digitalWrite(bumpswitchPin, HIGH);
  attachInterrupt(0, bumperISR, FALLING);
  interrupts();
  bumper = 0;
  MoveForward();
}

void bumperISR(){
    digitalWrite(forwardLED, LOW);
    digitalWrite(reverseLED, LOW);
    Serial.println("Bumper Hit!!");
    Brakes();
    bumper = 1;
}

void Brakes() {
  throttle.write(0);
  Serial.println("Brakes on!!");
  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 (bumper > 0) {
    delay(200);
    Stop();
    delay(200);
    steering.write(LeftTurn);
    delay(5);
    throttle.write(ReverseSpeed);
    digitalWrite(reverseLED, HIGH);
    Serial.print("Throttle @ ");
    Serial.println(ReverseSpeed);
    Serial.println('\n');
    delay(1350);
    Stop();
    digitalWrite(reverseLED, LOW);
    steering.write(SteeringNeutral);
    delay(5);
    delay(500);
    bumper = 0;
    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');
   }
 }

}