#include <CytronMotorDriver.h> // Including the Cytron motor driver library
#include "CytronMotorDriver.h"
#include <Servo.h> // Including servo motor's Library
#include <LiquidCrystal.h> // Including the LCD Library
//Configuring the motors
CytronMD motor1 (PWM_DIR, 3, 23); // Declaring the pins for PWM (SPEED) and DIR (DIRECTION) for front motor on the right
CytronMD motor2 (PWM_DIR, 4, 24); // Front motor on the left
CytronMD motor3 (PWM_DIR, 10, 26); // Back motor on the left
CytronMD motor4 (PWM_DIR, 9, 27); // Back motor on the right
int Position = 90;
int servoPin = 13;
int echoPin = 12; // Echo pin of the UltraSnonic Sensor
int TrigPin = 11; // Trigger Pin of the UltraSonic Sensor
Servo Direction; // Creating a servo object
float PingTime; // Time for ping to travel and come back
float speedofsound = 1234.8 ; // Speed of sound in Kilometers Per hour
float Distance;
int CustomKey = 100;
int DistanceLeft, DistanceRight; // Distance left and right for servomotor
LiquidCrystal lcd (22, 28, 30, 31, 32, 33); // Defining the pins for the LCD Screen
int MotionPin = 25; // defining pin taking input from motion detector
int MotionState = 0; // Giving an initial value to the Motion State
int HoldState; // This var holds the Automatic State till it's code finishes compiling or interrupted by a command from the CU
float GetDistance() // fuction used to get distance from the obstacle to the US-Sensor
{
  digitalWrite(TrigPin, LOW); // Setting trigger pin for the US-Sensor to low value
  delayMicroseconds(2000); // Pausing for a bit to make the signal settle
  digitalWrite(TrigPin, HIGH); // Setting the trigger pin for the US-Sensor to High Value
  delayMicroseconds(15); // pausing for a small amout of time
  digitalWrite(TrigPin, LOW); //Setting the trigger pin for the US-Sensor to Low again

  PingTime = pulseIn(echoPin , HIGH); // Measure Ping Time at echopin in MicroSeconds
  PingTime = PingTime / 1000000.; // Converting ping time to seconds as it's in Microseconds
  PingTime = PingTime / 3600.; // Converting Ping time to Hours

  float result = speedofsound * PingTime; // Distance in Kilometers
  result = result / 2; // The Distance from the Sensor to target
  result = result * 100000; // Convert the distance from kilometers to Centemeters
  return result; // returning the value of the result distance
}
int F = 0, B = 0, R = 0, L = 0, M = 0, A = 0;
double Pulse; // Declaring the pulses sent by the metal detector
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Direction.attach(servoPin);
  pinMode(TrigPin, OUTPUT); // Outputting the value Read
  pinMode(echoPin, INPUT); // Reading From the Sensor
  lcd.begin(16,2);
  lcd.print("Speed: "); // Displaying the word Speed on the first line
  pinMode(MotionPin, INPUT); // Declaring the Motion pin as an input
  pinMode(8, INPUT); // Declaring pin 8 as an input for the metal detector
  pinMode(7, OUTPUT); // Declaring pin 5 as an output from the metal detector
}

void loop() {
  // put your main code here, to run repeatedly:
  MotionState = digitalRead(MotionPin); // Reading if motion accurs
  if (MotionState == HIGH){ // This activates the Automatic mode
    HoldState = HIGH; // Holding the Automatic State
  }
  if (HoldState == HIGH || M == HIGH);{
    Distance = GetDistance();
    lcd.clear();
    lcd.setCursor(0, 1);
    lcd.print(CustomKey);
    if (Distance > 2){
      motor1.setSpeed(CustomKey); // motor 1 runs forward with "CustomKey" Speed
      motor2.setSpeed(CustomKey); // motor 2 runs forward with "CustomKey" Speed
      motor3.setSpeed(CustomKey);
      motor4.setSpeed(CustomKey);
    }
    else if (Distance <= 2){
      motor1.setSpeed(0); // Stop motor 1
      motor2.setSpeed(0); // Stop motor 2
      motor3.setSpeed(0); // stop motor 3
      motor4.setSpeed(0); // Stop motor 4
      Position = 20;
      Direction.write(Position); // Giving the Servo the wanted position
      Distance = GetDistance();
      DistanceLeft = Distance ;
      Position = 170;
      Direction.write(Position);
      Distance = GetDistance();
      DistanceRight = Distance;
      }
      if (DistanceRight > DistanceLeft){
        motor1.setSpeed(-50); // setting motor 1 to half speed and moving backwards
        motor2.setSpeed(100); // setting motor 2 to normal speed and moving forward
        motor3.setSpeed(100); // setting motor 3 to normal speed and moving forward
        motor4.setSpeed(-50); // setting motor 4 to half speed and moving backwards
        delay(500);
      }
      else {
        motor1.setSpeed(100);
        motor2.setSpeed(-50);
        motor3.setSpeed(-50);
        motor4.setSpeed(100);
        delay(500); // delaying the robot for a bit
      }
      digitalWrite(7, HIGH); // Giving a High value to the pin 7 connected to the metal detector sensor circuit
      delayMicroseconds(5000); // Giving a delay before the low value
      digitalWrite(7, LOW); // Giving a Low pulse from the same pin
      delayMicroseconds(100);
      Pulse = pulseIn(8, HIGH, 5000); // Give pulses to pin 8 with a T of 5 sec
      if (Pulse > 920) // if the sensor detects metal
      {
        digitalWrite(6, HIGH); /* Giving an Active High signal to a NO Relay
        that provides 24 Voltages to a pneumatic mechanical  cylinder
        (Double Acting Cylinder ) that pushes the Flag Up */
      }
  }
  // Code for manual Control
  if (Serial.available() > 0){
    if (Serial.read() == '1'){
      F == HIGH;
    }
    else if (Serial.read() == '2'){
      B == HIGH;
    }
    else if (Serial.read() == '3'){
      R == HIGH;
    }
    else if (Serial.read() == '4'){
      L == HIGH;
    }
    else if (Serial.read() == '5'){
      M = HIGH;
    }
    else if (Serial.read() == '6'){
      A = HIGH;
    }
    else if (Serial.read() == '7'){
      CustomKey = Serial.read(); // Reading the next serial value which is the speed
    }
    else if (Serial.read() == "10"){
      F = LOW;
    }
    else if (Serial.read() == "20"){
      B = LOW;
    }
    else if (Serial.read() == "30"){
      R = LOW;
    }
    else if (Serial.read() == "40"){
      L = LOW;
    }
    else if (Serial.read() == "50"){
      M = LOW;
    }
    else if (Serial.read() == "60"){
      A = LOW;
    }

  }
  if (F == HIGH){ // Code for moving forward Manually
    motor1.setSpeed(CustomKey);
    motor2.setSpeed(CustomKey);
    motor3.setSpeed(CustomKey);
    motor4.setSpeed(CustomKey);
  }
  else if (B == HIGH){ // Code for moving backwards manually
    motor1.setSpeed(-CustomKey);
    motor2.setSpeed(-CustomKey);
    motor3.setSpeed(-CustomKey);
    motor4.setSpeed(-CustomKey);
  }
  else if (R == HIGH){ // Code for rotating right manually
    motor1.setSpeed(-CustomKey + (CustomKey/2)); // setting it to be half speed
    motor2.setSpeed(CustomKey);
    motor3.setSpeed(CustomKey);
    motor4.setSpeed(-CustomKey + (CustomKey / 2)); // Setting this motor for half speed chosen
  }
  else if (L == HIGH){ //Code for rotating left manually
    motor1.setSpeed(CustomKey);
    motor2.setSpeed(-CustomKey + (CustomKey)/2);
    motor3.setSpeed(-CustomKey + (CustomKey)/2);
    motor4.setSpeed(CustomKey);
  }
}