#include <SoftwareSerial.h>
SoftwareSerial mySerial(0,1);
long duration;
int distance, l, r, f;
int US_M_trigPin = A2, US_M_echoPin = A3;
int US_L_trigPin = A0, US_L_echoPin = A1;
int US_R_trigPin = A4, US_R_echoPin = A5;
int m12 = 7;
int mbl = 6;
int mbr = 12;
int m11 = 4;
int enal = 3;
int enar = 9;
int enabl = 5;
int enabr = 11;
int buzzer = 2;
bool isdriving;
int dir=1;
int delayTime = 60;
int SPEED = 100; //h4ighest  = 255 range == 1 < 255
void setup()
{
    Serial.begin(9600);
    mySerial.begin(9600);
    while(!Serial)
    {
      ;
    }

    pinMode(US_L_trigPin, OUTPUT);
    pinMode(US_L_echoPin, INPUT);
    pinMode(US_M_trigPin, OUTPUT);
    pinMode(US_M_echoPin, INPUT);
    pinMode(US_R_trigPin, OUTPUT);
    pinMode(US_R_echoPin, INPUT);
    pinMode(buzzer, OUTPUT);
    pinMode(m12, OUTPUT);
    pinMode(mbl, OUTPUT);
    pinMode(mbr, OUTPUT);
    pinMode(m11, OUTPUT);
    pinMode(enal, OUTPUT);
    pinMode(enar, OUTPUT);
    pinMode(enabr, OUTPUT);
    pinMode(enabl, OUTPUT);
    analogWrite(enar, SPEED);
    analogWrite(enal, SPEED);
    analogWrite(enabl, SPEED);
    analogWrite(enabr , SPEED);


    isdriving = true;
}

void loop()
{
    if(l > 50 && r >50 && f>50){
      dir=drive();
    }
    else if(l<15 || r< 15 || f<15){
        dir = stopcar();
      }
    else if(r > 15 && f>15 && r>l){
      dir=moveleft(l);
//      delay(delayTime);
    }
    else if(l>15 && f>15 && l>r){
      dir=moveright(r);
//      delay(delayTime);
    }
    else{
      dir = stopcar();
    }


    getdistances();

//    delay(100);
}
//delay(100);

void getdistances()
{
    sonar(US_M_trigPin, US_M_echoPin);
    f = distance;
    sonar(US_L_trigPin, US_L_echoPin);
    l = distance;
    sonar(US_R_trigPin, US_R_echoPin);
    r = distance;
    Serial.print("L:");
    Serial.println(l);
    delayMicroseconds(1);
    Serial.print("F:");
    Serial.println(f);
    delayMicroseconds(1);
    Serial.print("R:");
    Serial.println(r);
    delayMicroseconds(1);
    Serial.print("dir:");
    Serial.println(dir);
    delayMicroseconds(1);

}

int stopcar()
{
    digitalWrite(m11, LOW);
    digitalWrite(m12, LOW);
    digitalWrite(mbl, LOW);
    digitalWrite(mbr, LOW);
    digitalWrite(buzzer, HIGH);
    return -1;
}

int drive()
{

    analogWrite(enal, SPEED);
    analogWrite(enar, SPEED);
    analogWrite(enabl, SPEED);
    analogWrite(enabr, SPEED);
    digitalWrite(m11, HIGH);
    digitalWrite(m12, HIGH);
    digitalWrite(mbl, HIGH);
    digitalWrite(mbr, HIGH);
    digitalWrite(buzzer, LOW);
    return 1;
}

int moveleft(int rpm)
{
  analogWrite(enal, 255);
    analogWrite(enar, 255);
    analogWrite(enabl, 255);
    analogWrite(enabr, 255);
    digitalWrite(m11, HIGH);
    digitalWrite(m12, LOW);
    digitalWrite(mbl, HIGH);
    digitalWrite(mbr, LOW);

    digitalWrite(buzzer, LOW);
    return 0;
}

int moveright(int rpm)
{
  analogWrite(enal, 255);
    analogWrite(enar, 255);
    analogWrite(enabl, 255);
    analogWrite(enabr, 255);
    digitalWrite(m11, LOW);
    digitalWrite(m12, HIGH);
    digitalWrite(mbl, LOW);
    digitalWrite(mbr, HIGH);

    digitalWrite(buzzer, LOW);
    return 2;
}

void sonar(int trigPin, int echoPin)
{
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration = pulseIn(echoPin, HIGH);
    // Calculating the distance
    distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)
}