#include <SoftwareSerial.h>
  bool autoDriveCheck = false;

  const int inputMidLeft = 12;
  const int inputMidRight = 8;
  const int inputFarLeft = 13;
  const int inputFarRight = 7;

  bool midLeftVal;
  bool midRightVal;
  bool farLeftVal;
  bool farRightVal;


  //definations af motorenes pins
  const int motor1A = 11;
  const int motor1B = 10;
  const int motor2A = 6;
  const int motor2B = 5;
  const int Gun = 4;

  int BluetoothTX = 0;
  int BluetoothRX = 1;

  SoftwareSerial bluetooth(BluetoothTX, BluetoothRX);

  String state;

  int verdi = 0;

void setup() {
  // put your setup code here, to run once:

  pinMode(motor1A, OUTPUT);
  pinMode(motor1B, OUTPUT);
  pinMode(motor2A, OUTPUT);
  pinMode(motor2B, OUTPUT);

  //senscore
  pinMode(inputMidLeft, INPUT);
  pinMode(inputMidRight, INPUT);
  pinMode(inputFarLeft, INPUT);
  pinMode(inputFarRight, INPUT);
  pinMode(Gun, OUTPUT);

  bluetooth.begin(115200);
  Serial.begin(115200);

}

//Opdater bluetooth inputtet hvis en bluetooth input er givet.
void loop() {
  // put your main code here, to run repeatedly:
while (bluetooth.available()>0){
   delay(10);
   char c = bluetooth.read();
   state += c;
}

//Check om autodrive værdien er ændre og store det i en boolean værdi. Hvis ikke, fortsæt.
  if (state = "A1"){
    autoDriveCheck = true;
  }
  else if (state = "A0")
      autoDriveCheck = false;

  if (autoDriveCheck = true)
  {
    //senscore
  midLeftVal = digitalRead(inputMidLeft);
  midRightVal = digitalRead(inputMidRight);
  farLeftVal = digitalRead(inputFarLeft);
  farRightVal = digitalRead(inputFarRight);

  verdi = farLeftVal*1+midLeftVal*2+midRightVal*4+farRightVal*8;

  switch (verdi) {
    case 15:
    //bak
      analogWrite (motor1A, 0);
      analogWrite (motor1B, 255);
      analogWrite (motor2A, 0);
      analogWrite (motor2B, 255);
      delay(0100);
        break;
    /*case 6:
      //Frem
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
        break;*/
    case 4:
    //meget skarpt right
      analogWrite (motor1A, 0);
      analogWrite (motor1B, 255);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
        break;
    case 2:
    //meget skarpt left
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 0);
      analogWrite (motor2B, 255);
        break;
    case 0:
    //Frem
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
        break;
    case 9:
    //Frem
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
        break;
    case 5:
    //frem
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
        break;
    case 10:
    //frem
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
        break;
    case 3:
    //Right
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 190);
      analogWrite (motor2B, 0);
        break;
     case 11:
    //Right
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 190);
      analogWrite (motor2B, 0);
        break;
    case 13:
    //Left
      analogWrite (motor1A, 190);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
        break;
    case 12:
    //Left
      analogWrite (motor1A, 190);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
        break;
    case 7:
    //Sharp Right
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 0);
      analogWrite (motor2B, 0);
        break;
    case 1:
    //Sharp Right
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 0);
      analogWrite (motor2B, 0);
        break;
    case 14:
    //Sharp Left
      analogWrite (motor1A, 0);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
        break;
    case 8:
    //Sharp Left
      analogWrite (motor1A, 0);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
        break;
  }
  }
  else {
     if (state.length() > 0) {
   Serial.println(state );

   if(state == "a")
      {
    analogWrite (motor1A, 255);
    analogWrite (motor1B, 0);
    analogWrite (motor2A, 255);
    analogWrite (motor2B, 0);
      }
   else if(state == "b")
  {
    analogWrite (motor1A, 0);
    analogWrite (motor1B, 0);
    analogWrite (motor2A, 0);
    analogWrite (motor2B, 0);
    }

      if(state == "c")
      {
    analogWrite (motor1A, 0);
    analogWrite (motor1B, 0);
    analogWrite (motor2A, 255);
    analogWrite (motor2B, 0);
      }
   else if(state == "d")
  {
    analogWrite (motor1A, 0);
    analogWrite (motor1B, 0);
    analogWrite (motor2A, 0);
    analogWrite (motor2B, 0);
    }

      if(state == "e")
      {
    analogWrite (motor1A, 255);
    analogWrite (motor1B, 0);
    analogWrite (motor2A, 0);
    analogWrite (motor2B, 0);
      }
   else if(state == "f")
  {
    analogWrite (motor1A, 0);
    analogWrite (motor1B, 0);
    analogWrite (motor2A, 0);
    analogWrite (motor2B, 0);
    }

      if(state == "g")
      {
    analogWrite (motor1A, 0);
    analogWrite (motor1B, 255);
    analogWrite (motor2A, 0);
    analogWrite (motor2B, 255);
      }
   else if(state == "h")
  {
    analogWrite (motor1A, 0);
    analogWrite (motor1B, 0);
    analogWrite (motor2A, 0);
    analogWrite (motor2B, 0);
    }

      if(state == "q")
      {
    analogWrite (Gun, 255);
      }
   else if(state == "r")
  {
    analogWrite (Gun, 0);
    }

    state ="";}
  }
}