#include <SoftwareSerial.h>
int BluetoothTX = 3;
int BluetoothRX = 2;

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

bool midLeftVal;
bool midRightVal;
bool farLeftVal;
bool farRightVal;
int autoDriveCheck = 0;

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

int verdi = 0;

const int Gun = 4;

SoftwareSerial bluetooth(BluetoothTX, BluetoothRX);

String state =  "";

void setup() {
  // Defination af outpusts
    bluetooth.begin(115200);
    //Serial.begin(115200);
    pinMode(motor1A, OUTPUT);
    pinMode(motor1B, OUTPUT);
    pinMode(motor2A, OUTPUT);
    pinMode(motor2B, OUTPUT);
    pinMode(Gun, OUTPUT);

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

   if (state == "q"){
    autoDriveCheck = 1;
  }else if (state == "w"){
      autoDriveCheck = 0;
}
}


void loop() {

if (state == "q"){
    autoDriveCheck = 1;
  }
if (state == "w"){
    autoDriveCheck = 0;
}

 while (bluetooth.available()>0){
   delay(10);
   char c = bluetooth.read();
   state += c;
 }

if (autoDriveCheck == 0) {

  if (state.length() > 0 && autoDriveCheck == 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 ="";}
}

if (autoDriveCheck == 1){


if (state.length() > 0 && autoDriveCheck == 1) {
   Serial.println(state );


  //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;

  state ="";
  }
}
}
}