// CANDY-HO Argentina www.candy-ho.com
#include <SoftwareSerial.h>
SoftwareSerial mySerial(10, 11); // RX, TX
#include <Servo.h>
Servo myservo;
int motorIP = 4;
int motorIN = 6;
int motorDP = 3;
int motorDN = 5;
int enableA = A1;
int enableB = A0;
int flag = 6;
char receivedChar;
int rightDistance = 0, leftDistance = 0, middleDistance = 0;
#define trigPin A5
#define echoPin A4
#define carSpeed 150
int SL;
int SR;
int SM;
#define LT_R !digitalRead(2)
#define LT_M !digitalRead(8)
#define LT_L !digitalRead(9)


void setup() {
 pinMode(motorIP, OUTPUT);
 pinMode(motorIN, OUTPUT);
 pinMode(motorDP, OUTPUT);
 pinMode(motorDN, OUTPUT);
 pinMode(trigPin, OUTPUT);
 pinMode(echoPin, INPUT);
 pinMode(LT_R, INPUT);
 pinMode(LT_M, INPUT);
 pinMode(LT_L, INPUT);
 Serial.begin(9600);
 mySerial.begin(9600);
Serial.println("HELLO!");
      delay(1500); 
 flag = 6; // flag is used to give a seamless experience while switching between autopilot and manual mode.
 myservo.attach(13);
}

void loop() {
mySerial.println("");
  while (!mySerial.available()  &&  flag == 6)
  {
;     // does nothing at all.
  }   
    receivedChar = mySerial.read();
  
  Serial.println("Received: ");
  Serial.println(receivedChar);
  
  if (receivedChar == '1') {
      forward();
      flag = 6;
  }
  
else  if (receivedChar == '2') {
      backward();
      flag = 6;
  } 
else  if (receivedChar == '3') {
      right();
      flag = 6;    
  }
else  if (receivedChar == '4') {
        left();
        flag = 6;
  }
else   if (receivedChar == '5') {
        stop();
        flag = 6;
  }
else   if (receivedChar == '6') {
        follow();
        flag = 6;
        stop();
  }
else   if (receivedChar == '7') {
//        prendidas="N";
  //      digitalWrite(luces,LOW);
        flag = 6;
  //      lcd.setCursor(15,1);
  //      lcd.print(prendidas);
  }
    
else  if(receivedChar == '8') // auto pilot)
  {
        flag = 7;
        //analogWrite(luzdelanteraderecha,HIGH);
        Serial.print(flag);
  }
else if(flag == 7)
{
    autopilot();
//  stop();
}

}

void autopilot() {
  // put your main code here, to run repeatedly:

     car();
//     avoid();
     receivedChar = mySerial.read();
     if (receivedChar == '5') {
        stop();
        flag = 6;
        Serial.println("DETECTADO EN LOOP AUTOPILOT");
  }   
     
}


//Defining functions so it's more easy
//motor functions
void motorAforward() {
  digitalWrite (motorIP, HIGH);
  digitalWrite (motorIN, LOW);
}
void motorBforward() {
  digitalWrite (motorDP, HIGH);
  digitalWrite (motorDN, LOW);
}
void motorAbackward() {
  digitalWrite (motorIP, LOW);
  digitalWrite (motorIN, HIGH);
}
void motorBbackward() {
  digitalWrite (motorDP, LOW);
  digitalWrite (motorDN, HIGH);
}
void motorAstop() {
 digitalWrite (motorIP, HIGH);
 digitalWrite (motorIN, HIGH);
}
void motorBstop() {
 digitalWrite (motorDP, HIGH);
 digitalWrite (motorDN, HIGH);
}
void motorAcoast() {
 digitalWrite (motorIP, LOW);
 digitalWrite (motorIN, LOW);
}
void motorBcoast() {
 digitalWrite (motorDP, LOW);
 digitalWrite (motorDN, LOW);
}
void motorAon() {
 digitalWrite (enableA, HIGH);
}
void motorBon() {
 digitalWrite (enableB, HIGH);
}
void motorAoff() {
 digitalWrite (enableA, LOW);
}
void motorBoff() {
 digitalWrite (enableB, LOW);
}
// Movement functions

void forward()
{
motorAforward();
motorBforward();
//delay(DELAY);
Serial.println("foward");
//Serial.println("Moving forward");
}


void backward()
{

//Serial.println("bk");
Serial.println("Moving back");
motorAbackward();
motorBbackward();
//delay(DELAY);
}


void left()
{
Serial.println("lf");
//Serial.println("Turning LEFT");
motorAbackward();
motorBforward();

}


void right()
{
Serial.println("rg");
//Serial.println("Turning Right");
motorAforward();
motorBbackward();
}

void stop()
{
Serial.println("stop");
//Serial.println("Stopping");
digitalWrite(motorIP,LOW);
digitalWrite(motorIN,LOW);
digitalWrite(motorDP,LOW);
digitalWrite(motorDN,LOW);
}


void coast (int duration) {
 motorAcoast();
 motorBcoast();
 delay (duration);
}
void breakRobot (int duration) {
 motorAstop();
 motorBstop();
 delay (duration);
}
void disableMotors() {
 motorAoff();
 motorBoff();
}
void enableMotors() {
 motorAon();
 motorBon();
}

int distance() {
  int duration, distance;
  String dato, distancia;
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(500);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
  distancia=String(distance);
  if (distance > 999) dato = "999";
  if (distance < 100) dato = "0" + distancia;
  if (distance < 10) dato = "00" + distancia;
//  lcd.setCursor(11,0);
//  lcd.print("D=" );
//  lcd.setCursor(13,0);
//  lcd.print(dato);
//  return distance;
}


void car() {
    myservo.write(90);  //setservo position according to scaled value
    delay(100); 
    middleDistance = Distance_test();

    if(middleDistance <= 40) {     
      stop();
      delay(500);                         
      myservo.write(40);          
      delay(100);      
      rightDistance = Distance_test();
      
      delay(500);
      myservo.write(90);              
      delay(500);                                                  
      myservo.write(140);              
      delay(500); 
      leftDistance = Distance_test();
      
      delay(100);
      myservo.write(90);              
      delay(100);
      if(rightDistance > leftDistance) {
        right();
        delay(360);
      }
      else if(rightDistance < leftDistance) {
        left();
        delay(360);
      }
      else if((rightDistance <= 20) || (leftDistance <= 20)) {
        backward();
        delay(180);
      }
      else {
        forward();
      }
    }  
    else {
        forward();
    }                     
}

int Distance_test() {
  digitalWrite(trigPin, LOW); 
  String dato, distancia;  
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);  
  delayMicroseconds(20);
  digitalWrite(trigPin, LOW);   
  float Fdistance = pulseIn(echoPin, HIGH);  
  Fdistance= Fdistance / 58;
  distancia=String(Fdistance);
  if (Fdistance > 999) dato = "999";
  if (Fdistance < 100) dato = "0" + distancia;
  if (Fdistance < 10) dato = "00" + distancia;
//  lcd.setCursor(11,0);
//  lcd.print("D=" );
//  lcd.setCursor(13,0);
//  lcd.print(dato);       
  return (int)Fdistance;
}  


void backward2 (int duration) {
 motorAbackward();
 motorBbackward();
 delay (duration);
}
void right2 (int duration) {
 motorAbackward();
 motorBforward();
 delay (duration);
 }
 
void avoid()
{
    backward2(300);
    right2(360);
}


void follow()
{
  int aux=0;
  while (aux == 0)
 { 
  receivedChar = mySerial.read();
  if (receivedChar == '5') {
        stop();
        aux=1;
  }
  SL = digitalRead(LT_L);
  SR = digitalRead(LT_R);
  SM = digitalRead(LT_M);
  Serial.println("--");
  Serial.println(SL);
  Serial.println(SM);
  Serial.println(SR);
    Serial.println(receivedChar);
 // delay(1599);
  if (SM == HIGH)
   {
    forward();
       }
   else
    {
    if (SL == HIGH & SR == LOW)
    {
    left();
    delay(3);
    }
    else
    {
    if (SR == HIGH & SL == LOW) 
    {
    right();
    delay(3);
    }
    else 
        {
    backward();
    }
        }
         }
   }
}

