// Pesticide Spraying Robot
// v1.0
/*
  SPRAY ROUTINE
  - spray
  - 5 seconds
  - stop spray
  EDGE DETECTION ROUTINE
  - check front
  - if not edge:
    FORWARD MOTOR ROUTINE
    - forward motor
    - 2 seconds
    - stop motor
    - LOOP TO SPRAY ROUTINE
  - else:
    - back motor
    - 2 seconds
    - stop motor
    - LOOP TO SPRAY ROUTINE
*/

// Constants for motor speeds and distance limits
const float LEFT_MOTOR_SPEED = 255; // Speed for the left motor (0 to 255)
const float RIGHT_MOTOR_SPEED = 255; // Speed for the right motor (0 to 255)
const float FRONT_DISTANCE_LIMIT = 8.0; // Front distance limit in centimeters
const float BACK_DISTANCE_LIMIT = 8.0; // Back distance limit in centimeters
const unsigned long SPRAY_DELAY = 2000; // 2 seconds
const unsigned long FORWARD_MOVE_DELAY = 2000;  // 2 seconds
const unsigned long BACKWARD_MOVE_DELAY = 2000;  // 2 seconds

// Motor pins
const int MOTOR_ENA_PIN = 11;
const int MOTOR_IN1_PIN = 10;
const int MOTOR_IN2_PIN = 9;
const int MOTOR_IN3_PIN = 7;
const int MOTOR_IN4_PIN = 8;
const int MOTOR_ENB_PIN = 6;

// Ultrasonic sensor pins
const int US_TRIG_PIN_1 = 4;
const int US_ECHO_PIN_1 = 5;
const int US_TRIG_PIN_2 = 13;
const int US_ECHO_PIN_2 = 12;

// Relay pin
const int relayPin = 3;

// Variables to store distance readings
float frontDistance = 0.00;
float backDistance = 0.00;

bool checkFront = true;
bool isFrontEdgeDetected = false;
bool isBackEdgeDetected = false;

// Function to initialize motor control pins
void setupMotorPins() {
  pinMode(MOTOR_IN1_PIN, OUTPUT);
  pinMode(MOTOR_IN2_PIN, OUTPUT);
  pinMode(MOTOR_IN3_PIN, OUTPUT);
  pinMode(MOTOR_IN4_PIN, OUTPUT);
}

// Function to initialize ultrasonic sensor pins
void setupUltrasonicPins() {
  pinMode(US_TRIG_PIN_1, OUTPUT);
  pinMode(US_ECHO_PIN_1, INPUT);
  pinMode(US_TRIG_PIN_2, OUTPUT);
  pinMode(US_ECHO_PIN_2, INPUT);
}

// Function to control motor direction
void controlMotor(const int IN1, const int IN2, const int IN3, const int IN4) {
  digitalWrite(MOTOR_IN1_PIN, IN1);
  digitalWrite(MOTOR_IN2_PIN, IN2);
  digitalWrite(MOTOR_IN3_PIN, IN3);
  digitalWrite(MOTOR_IN4_PIN, IN4);
}

// Function to control motor speed
void controlSpeed(const int ENB, const int ENA) {
  analogWrite(MOTOR_ENA_PIN, ENA);
  analogWrite(MOTOR_ENB_PIN, ENB);
}

// Function to read distance from ultrasonic sensor
float readUltrasonic(const int trigPin, const int echoPin) {
  // Trigger ultrasonic pulse
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure pulse duration
  long duration = pulseIn(echoPin, HIGH);

  // Calculate distance based on duration
  float distance = duration * 0.034 / 2;

  return distance;
}

void setupRelayPin() {
  pinMode(relayPin, OUTPUT);
  turnOffRelay(); // Ensure relay starts off
}

void turnOnRelay() {
  digitalWrite(relayPin, LOW);
}

void turnOffRelay() {
  digitalWrite(relayPin, HIGH);
}

// Function to get front distance from ultrasonic sensor
float getFrontDistanceFromUltrasonic() {
  return readUltrasonic(US_TRIG_PIN_1, US_ECHO_PIN_1);
}

// Function to get back distance from ultrasonic sensor
float getBackDistanceFromUltrasonic() {
  return readUltrasonic(US_TRIG_PIN_2, US_ECHO_PIN_2);
}

// Function to get and display front distance
void getFrontDistance() {
  frontDistance = getFrontDistanceFromUltrasonic();
  displayFrontDistance();
}

// Function to get and display back distance
void getBackDistance() {
  backDistance = getBackDistanceFromUltrasonic();
  displayBackDistance();
}

// Function to display front distance
void displayFrontDistance() {
  Serial.print("Front Distance: ");
  Serial.print(frontDistance, 2);
  Serial.println("cm");
}

// Function to display back distance
void displayBackDistance() {
  Serial.print("Back Distance: ");
  Serial.print(backDistance, 2);
  Serial.println("cm");
}

// Function to stop motor
void stopMoving() {
  controlMotor(LOW, LOW, LOW, LOW);
  controlSpeed(0, 0);
}

// Function to move forward
void moveForward() {
  controlMotor(HIGH, LOW, HIGH, LOW);
  controlSpeed(LEFT_MOTOR_SPEED, RIGHT_MOTOR_SPEED);
}

// Function to move backward
void moveBackward() {
  controlMotor(LOW, HIGH, LOW, HIGH);
  controlSpeed(LEFT_MOTOR_SPEED, RIGHT_MOTOR_SPEED);
}

// Detect front edge
void detectFrontEdge() {
  getFrontDistance();
  if (frontDistance <= FRONT_DISTANCE_LIMIT) {
    isFrontEdgeDetected = true;
  } else {
    isFrontEdgeDetected = false;
  }
}

// Detect back edge
void detectBackEdge() {
  getBackDistance();
  if (backDistance <= BACK_DISTANCE_LIMIT) {
    isBackEdgeDetected = true;
  } else {
    isBackEdgeDetected = false;
  }
}

void spray() {
  turnOnRelay();
  delay(SPRAY_DELAY);
  turnOffRelay();
}

void detectFrontOrBackEdge() {
  if (checkFront) {
    detectFrontEdge();
  } else {
    detectBackEdge();
  }
}

void moveForwardOrBackward() {
  if (checkFront && isFrontEdgeDetected) {
    // If front edge is detected, move backward
    moveBackward();
    delay(BACKWARD_MOVE_DELAY);
    checkFront = false;
  } else if (!checkFront && isBackEdgeDetected) {
    // If back edge is detected, move forward
    moveForward();
    delay(FORWARD_MOVE_DELAY);
    checkFront = true;
  } else if (checkFront) {
    // Move forward if no edge is detected
    moveForward();
    delay(FORWARD_MOVE_DELAY);
  } else {
    // Move backward if no edge is detected
    moveBackward();
    delay(BACKWARD_MOVE_DELAY);
  }
  stopMoving();
}

// Setup function
void setup() {
  // Initialize motor and ultrasonic sensor pins
  setupRelayPin();
  setupMotorPins();
  setupUltrasonicPins();

  // Initialize serial communication for debugging
  Serial.begin(9600);
  delay(2000);
}

// Main loop
void loop() {
  spray();
  detectFrontOrBackEdge();
  moveForwardOrBackward();

  delay(10);
}