#include <Servo.h>
Servo upServo;
Servo downServo;

#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2); //0x3f

#define LM_PWM 3   //  PWM for L_Motor  
#define LM_DIR 2   //  direction for L_Motor  1-forward  0-backward
#define RM_PWM 5   //  PWM for R_Motor
#define RM_DIR 4   //  direction for R_Motor  1-forward  0-backward
#define FLed 6     //  Front White leds 
#define RLed 7     //  Right Yelow leds
#define BLed 8     //  Back  Red leds
#define LLed 9     //  Left  Yelow leds
#define Trig 10
#define Echo 11
long width;
int  distance;
int  i, R, L, M, pos = 90, dpos = 90;
String VoiceCmd;

// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-

int UltraSonicRead(int USTrig, int USEcho)
{
  digitalWrite(USTrig, 0);
  delayMicroseconds(2);
  digitalWrite(USTrig, 1);
  delayMicroseconds(10);
  digitalWrite(USTrig, 0);
  width = pulseIn(USEcho, 1);
  distance = width / 58.2;
  return distance;
}

// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-

void MotorsPWM(int LM_Speed, int RM_Speed)
{
  if (LM_Speed >= 0)
    digitalWrite(LM_DIR, 0);
  else
  {
    LM_Speed += 255;
    digitalWrite(LM_DIR, 1);
  }

  if (RM_Speed >= 0)
    digitalWrite(RM_DIR, 0);
  else
  {
    RM_Speed += 255;
    digitalWrite(RM_DIR, 1);
  }

  analogWrite(LM_PWM, LM_Speed);
  analogWrite(RM_PWM, RM_Speed);
}

// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-

void Leds(char led)
{
  switch (led)
  {
    case 'f': digitalWrite(FLed, 1);
      digitalWrite(RLed, 0);
      digitalWrite(BLed, 0);
      digitalWrite(LLed, 0);
      break;
    case 'r': digitalWrite(FLed, 0);
      digitalWrite(RLed, 1);
      digitalWrite(BLed, 0);
      digitalWrite(LLed, 0);
      break;
    case 'b': digitalWrite(FLed, 0);
      digitalWrite(RLed, 0);
      digitalWrite(BLed, 1);
      digitalWrite(LLed, 0);
      break;
    case 'l': digitalWrite(FLed, 0);
      digitalWrite(RLed, 0);
      digitalWrite(BLed, 0);
      digitalWrite(LLed, 1);
      break;
    case 's': digitalWrite(FLed, 0);
      digitalWrite(RLed, 0);
      digitalWrite(BLed, 1);
      digitalWrite(LLed, 0);
      break;
  }
}

// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-

void Lcd(char Dir)
{
  lcd.setCursor(0, 0);
  lcd.print("Robot Direction");
  lcd.setCursor(2, 1);
  switch (Dir)
  {
    case 'f': lcd.print("Go Forword... ");
      break;
    case 'l': lcd.print("Go Left... ");
      break;
    case 'r': lcd.print("Go Right... ");
      break;
    case 'b': lcd.print("Go Backword... ");
      break;
    case 's': lcd.print("Check distance ");
      break;
  }
}

// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-

void setup ()
{
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  upServo.attach(15);
  downServo.attach(14);
  upServo.write(pos);
  downServo.write(dpos);
  for (i = 2 ; i < 10 ; i++)
    pinMode(i, OUTPUT);
  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);
  lcd.clear();
  lcd.setCursor(6, 0);
  lcd.print("IRobot");
  lcd.setCursor(0, 1);
  lcd.print("Robot initialize");
  delay(2000);
}

// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-

void loop ()
{
  VoiceCmd = "";
  while (!Serial.available());
  VoiceCmd = Serial.readString();
  while (VoiceCmd == "start")
  {
    lcd.clear();
    while (UltraSonicRead(Trig, Echo) > 30 && !Serial.available())
    {
      MotorsPWM(170, 170);
      Leds('f');
      Lcd('f');
      for (dpos = 170; dpos >= 10; dpos -= 10)
      {
        downServo.write(dpos);
        delay(10);
      }
      for (dpos = 10; dpos <= 170; dpos += 10)
      {
        downServo.write(dpos);
        delay(10);
      }
      if (Serial.available())
      {
        VoiceCmd = Serial.readString();
        if (VoiceCmd == "stop")
        {
          MotorsPWM(0, 0);
          Leds('s');
          Lcd('s');
        }
      }
    }
    MotorsPWM(0, 0);
    Leds('s');
    lcd.clear();
    Lcd('s');

    for (pos = 90; pos >= 30; pos -= 10)
    {
      upServo.write(pos);
      delay(15);
    }
    R = UltraSonicRead(Trig, Echo);
    delay(50);
    for (pos = 30; pos <= 150; pos += 10)
    {
      upServo.write(pos);
      delay(15);
    }
    L = UltraSonicRead(Trig, Echo);
    delay(50);

    if (L > R && L > 30)
    {
      upServo.write(90);
      MotorsPWM(-170, 170);
      Leds('l');
      lcd.clear();
      Lcd('l');
      delay(400);
      MotorsPWM(0, 0);
      delay(100);
    }
    else if (R > L && R > 30)
    {
      upServo.write(90);
      MotorsPWM(170, -170);
      Leds('r');
      lcd.clear();
      Lcd('r');
      delay(400);
      MotorsPWM(0, 0);
      delay(100);
    }
    else
    {
      upServo.write(90);
      MotorsPWM(0, 0);
      delay(100);
      MotorsPWM(-170, -170);
      Leds('b');
      lcd.clear();
      Lcd('b');
      delay(600);
      MotorsPWM(0, 0);
      delay(100);
      if (L > R)
      {
        MotorsPWM(-170, 170);
        Leds('l');
        lcd.clear();
        Lcd('l');
        delay(400);
        MotorsPWM(0, 0);
        delay(100);
      }
      else
      {
        MotorsPWM(170, -170);
        Leds('r');
        lcd.clear();
        Lcd('r');
        delay(400);
        MotorsPWM(0, 0);
        delay(100);
      }
    }
  }
  lcd.clear();
  while (VoiceCmd == "stop" && !Serial.available())
  {
    MotorsPWM(0, 0);
    Leds('s');
    Lcd('s');
  }
}