#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;
bool autoDriveCheck = false;

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

}


void frem (){
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
}

void bak (){
      analogWrite (motor1A, 0);
      analogWrite (motor1B, 255);
      analogWrite (motor2A, 0);
      analogWrite (motor2B, 255);
}

void nej(){
      analogWrite (motor1A, 0);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 0);
      analogWrite (motor2B, 0);
}

 void right(){
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 190);
      analogWrite (motor2B, 0);
 }

 void left(){
      analogWrite (motor1A, 190);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);

 }

 void sright(){
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 0);
      analogWrite (motor2B, 0);
 }

void sleft(){
       analogWrite (motor1A, 0);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
}

void msright(){
      analogWrite (motor1A, 0);
      analogWrite (motor1B, 255);
      analogWrite (motor2A, 255);
      analogWrite (motor2B, 0);
}

void msleft(){
      analogWrite (motor1A, 255);
      analogWrite (motor1B, 0);
      analogWrite (motor2A, 0);
      analogWrite (motor2B, 255);
}

void loop() {


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


   if (state == "i"){
    autoDriveCheck = !autoDriveCheck;
   }



if (autoDriveCheck == false) {

  if (state.length() > 0) {
   //Serial.println("false");
   Serial.print(state);
   Serial.println("False");

   if(state == "a"){
     frem();
      }
   else if(state == "b"){
    nej();
    }

      if(state == "c"){
    sleft();
      }
   else if(state == "d"){
    nej();
    }

      if(state == "e"){
    sright();
      }
   else if(state == "f")  {
    nej();
    }

      if(state == "g"){
    bak();
      }
   else if(state == "h")  {
    nej();
    }

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

    state ="";}
}





if (autoDriveCheck == true){


if (state.length() > 0) {
   //Serial.println("true");
   Serial.print(state);
   Serial.println("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
    bak();
        break;
    case 4:
      msright();
        break;
    case 2:
      msleft();
        break;
    case 0:
      frem();
        break;
    case 9:
      frem();
        break;
    case 5:
      frem();
        break;
    case 10:
      frem();
        break;
    case 3:
      right();
        break;
     case 11:
      right();
        break;
    case 13:
      left();
        break;
    case 12:
      left();
        break;
    case 7:
      sright();
        break;
    case 1:
      sright();
        break;
    case 14:
      sleft();
        break;
    case 8:
      sleft();
        break;


  }
}
}
}