#define IN1 2
#define IN2 3
#define IN3 4
#define IN4 5
#define WHITE_LED 6
#define IR_LED 7
#define TRIG_PIN 12
#define ECHO_PIN 13
#define SERVO3_PIN 9

#include <Servo.h>

Servo servo3;

void setup() {
  Serial.begin(9600);
    pinMode(IN1, OUTPUT);
      pinMode(IN2, OUTPUT);
        pinMode(IN3, OUTPUT);
          pinMode(IN4, OUTPUT);
            pinMode(WHITE_LED, OUTPUT);
              pinMode(IR_LED, OUTPUT);
                pinMode(TRIG_PIN, OUTPUT);
                  pinMode(ECHO_PIN, INPUT);
                    servo3.attach(SERVO3_PIN);
                    }

                    void loop() {
                      if (Serial.available() > 0) {
                          char value = Serial.read();
                              Serial.println(value);
                                  if (value == 'A') {
                                        Forward();
                                            } else if (value == 'B') {
                                                  Backward();
                                                      } else if (value == 'C') {
                                                            Left();
                                                                } else if (value == 'D') {
                                                                      Right();
                                                                          } else if (value == 'E') {
                                                                                // Servo Two Up
                                                                                    } else if (value == 'F') {
                                                                                          // Servo Two Down
                                                                                              } else if (value == 'G') {
                                                                                                    // Servo One Left
                                                                                                        } else if (value == 'H') {
                                                                                                              // Servo One Right
                                                                                                                  } else if (value == 'I') {
                                                                                                                        digitalWrite(WHITE_LED, !digitalRead(WHITE_LED));
                                                                                                                            } else if (value == 'J') {
                                                                                                                                  // Toggle Red LEDs
                                                                                                                                      } else if (value == 'K') {
                                                                                                                                            ActivateAutoNav();
                                                                                                                                                } else if (value == 'L') {
                                                                                                                                                      // Mode Four (Activate White LEDs on Camera)
                                                                                                                                                          } else if (value == 'M') {
                                                                                                                                                                // Navigate Higher LEDs
                                                                                                                                                                    } else {
                                                                                                                                                                          Serial.println("Invalid command");
                                                                                                                                                                              }
                                                                                                                                                                                }
                                                                                                                                                                                }

                                                                                                                                                                                void Forward() {
                                                                                                                                                                                  digitalWrite(IN1, HIGH);
                                                                                                                                                                                    digitalWrite(IN2, LOW);
                                                                                                                                                                                      digitalWrite(IN3, HIGH);
                                                                                                                                                                                        digitalWrite(IN4, LOW);
                                                                                                                                                                                        }

                                                                                                                                                                                        void Backward() {
                                                                                                                                                                                          digitalWrite(IN1, LOW);
                                                                                                                                                                                            digitalWrite(IN2, HIGH);
                                                                                                                                                                                              digitalWrite(IN3, LOW);
                                                                                                                                                                                                digitalWrite(IN4, HIGH);
                                                                                                                                                                                                }

                                                                                                                                                                                                void Stop() {
                                                                                                                                                                                                  digitalWrite(IN1, LOW);
                                                                                                                                                                                                    digitalWrite(IN2, LOW);
                                                                                                                                                                                                      digitalWrite(IN3, LOW);
                                                                                                                                                                                                        digitalWrite(IN4, LOW);
                                                                                                                                                                                                        }

                                                                                                                                                                                                        void Left() {
                                                                                                                                                                                                          digitalWrite(IN1, HIGH);
                                                                                                                                                                                                            digitalWrite(IN2, LOW);
                                                                                                                                                                                                              digitalWrite(IN3, LOW);
                                                                                                                                                                                                                digitalWrite(IN4, HIGH);
                                                                                                                                                                                                                }

                                                                                                                                                                                                                void Right() {
                                                                                                                                                                                                                  digitalWrite(IN1, LOW);
                                                                                                                                                                                                                    digitalWrite(IN2, HIGH);
                                                                                                                                                                                                                      digitalWrite(IN3, HIGH);
                                                                                                                                                                                                                        digitalWrite(IN4, LOW);
                                                                                                                                                                                                                        }

                                                                                                                                                                                                                        void ActivateAutoNav() {
                                                                                                                                                                                                                          int duration, distance;
                                                                                                                                                                                                                            digitalWrite(TRIG_PIN, HIGH);
                                                                                                                                                                                                                              delay(1);
                                                                                                                                                                                                                                digitalWrite(TRIG_PIN, LOW);
                                                                                                                                                                                                                                  duration = pulseIn(ECHO_PIN, HIGH);
                                                                                                                                                                                                                                    distance = (duration / 2) * 0.0343;
                                                                                                                                                                                                                                      if (distance < 20) {
                                                                                                                                                                                                                                          Backward();
                                                                                                                                                                                                                                              ScanForPath();
                                                                                                                                                                                                                                                } else {
                                                                                                                                                                                                                                                    Forward();
                                                                                                                                                                                                                                                      }
                                                                                                                                                                                                                                                      }

                                                                                                                                                                                                                                                      void ScanForPath() {
                                                                                                                                                                                                                                                        servo3.write(0);
                                                                                                                                                                                                                                                          delay(500);
                                                                                                                                                                                                                                                            int duration, distance;
                                                                                                                                                                                                                                                              digitalWrite(TRIG_PIN, HIGH);
                                                                                                                                                                                                                                                                delay(1);
                                                                                                                                                                                                                                                                  digitalWrite(TRIG_PIN, LOW);
                                                                                                                                                                                                                                                                    duration = pulseIn(ECHO_PIN, HIGH);
                                                                                                                                                                                                                                                                      distance = (duration / 2) * 0.0343;
                                                                                                                                                                                                                                                                        if (distance >= 20) {
                                                                                                                                                                                                                                                                            Forward();
                                                                                                                                                                                                                                                                              } else {
                                                                                                                                                                                                                                                                                  servo3.write(180);
                                                                                                                                                                                                                                                                                      delay(500);
                                                                                                                                                                                                                                                                                          digitalWrite(TRIG_PIN, HIGH);
                                                                                                                                                                                                                                                                                              delay(1);
                                                                                                                                                                                                                                                                                                  digitalWrite(TRIG_PIN, LOW);
                                                                                                                                                                                                                                                                                                      duration = pulseIn(ECHO_PIN, HIGH);
                                                                                                                                                                                                                                                                                                          distance = (duration / 2) * 0.0343;
                                                                                                                                                                                                                                                                                                              if (distance >= 20) {
                                                                                                                                                                                                                                                                                                                    Forward();
                                                                                                                                                                                                                                                                                                                        } else {
                                                                                                                                                                                                                                                                                                                              Stop();
                                                                                                                                                                                                                                                                                                                                  }
                                                                                                                                                                                                                                                                                                                                    }
                                                                                                                                                                                                                                                                                                                                    }