// Pin Definitions for Servo Motor and Linear Actuator
#define SERVO_PIN_1 33     // Servo Motor Pin (Enable)
#define SERVO_PIN_2 34     // Servo Motor Pin (Input 1 - Clockwise)
#define SERVO_PIN_3 35     // Servo Motor Pin (Input 2 - Counter Clockwise)

#define ACTUATOR_PIN_1 36  // Linear Actuator Pin (Enable)
#define ACTUATOR_PIN_2 37  // Linear Actuator Pin (Input 3 - Clockwise)
#define ACTUATOR_PIN_3 38  // Linear Actuator Pin (Input 4 - Counter Clockwise)

// Pin Definitions for Sensors
#define SEED_SENSOR 31     // Inductive Sensor for Seed Dropping
#define HOLE_SENSOR 32     // Inductive Sensor for Hole Distance

// LED Indicators for Seed Dropping and Hole Detection
#define SEED_LED 8         // LED for Seed Dropping Detection
#define HOLE_LED 9         // LED for Hole Detection

// Stop-and-Go System Pins
#define TRIG_PIN_1 22      // Ultrasonic sensor Trigger pin for Stop-and-Go
#define ECHO_PIN_1 23      // Ultrasonic sensor Echo pin for Stop-and-Go
#define STOP_BRAKE 2       // Stop/Brake pin (Red LED)
#define FORWARD 3          // Forward pin (Green LED)

// Seed Container Monitoring Pins
#define TRIG_PIN_2 24      // Ultrasonic sensor Trigger pin for Seed Container
#define ECHO_PIN_2 25      // Ultrasonic sensor Echo pin for Seed Container
#define GREEN_LED 4        // Green LED (Full)
#define YELLOW_LED 5       // Yellow LED (Medium)
#define BLUE_LED 6         // Blue LED (Low)
#define RED_LED 7          // Red LED (Empty)

// Variables for ultrasonic sensors
long duration1, duration2;
int distance1, distance2;

// Seed Container Depth
const int containerDepth = 400;  // Maximum depth in cm

// Servo Motor Setup
#include <Servo.h>
Servo seedServo; // Servo object to control seed dropping mechanism

void setup() {
  Serial.begin(9600); // Start serial communication for debugging

  // Set pin modes for Stop-and-Go system
  pinMode(TRIG_PIN_1, OUTPUT);
  pinMode(ECHO_PIN_1, INPUT);
  pinMode(STOP_BRAKE, OUTPUT);
  pinMode(FORWARD, OUTPUT);

  // Set pin modes for Seed Container system
  pinMode(TRIG_PIN_2, OUTPUT);
  pinMode(ECHO_PIN_2, INPUT);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(BLUE_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);

  // Set sensor pins for Seed Dropping and Hole Distance Detection
  pinMode(SEED_SENSOR, INPUT);
  pinMode(HOLE_SENSOR, INPUT);

  // Set LED pins for Seed Dropping and Hole Detection
  pinMode(SEED_LED, OUTPUT);
  pinMode(HOLE_LED, OUTPUT);

  // Attach the Servo motor to pin 33
  seedServo.attach(SERVO_PIN_1);
}

void loop() {
  // Get distance from Stop-and-Go sensor
  distance1 = getDistance(TRIG_PIN_1, ECHO_PIN_1);
  Serial.print("Stop-and-Go Distance: ");
  Serial.print(distance1);
  Serial.println(" cm");

  // Stop-and-Go logic
  if (distance1 < 20) {
    digitalWrite(STOP_BRAKE, HIGH);  // Stop
    digitalWrite(FORWARD, LOW);
    Serial.println("Status: STOP (Obstacle detected)");
  } else {
    digitalWrite(STOP_BRAKE, LOW);
    digitalWrite(FORWARD, HIGH);  // Move Forward
    Serial.println("Status: GO (Path clear)");
  }

  // Get distance from Seed Container sensor
  distance2 = getDistance(TRIG_PIN_2, ECHO_PIN_2);

  // Calculate percentage for Seed Container
  int percentage = map(distance2, containerDepth, 0, 0, 100);
  percentage = constrain(percentage, 0, 100); // Ensure it stays within 0-100%

  Serial.print("Seed Container Distance: ");
  Serial.print(distance2);
  Serial.print(" cm, Percentage: ");
  Serial.print(percentage);
  Serial.println("%");

  // LED Logic for Seed Container
  if (percentage <= 25) {
    setLEDs(HIGH, LOW, LOW, LOW);  // Red LED (Empty)
  } else if (percentage <= 50) {
    setLEDs(LOW, HIGH, LOW, LOW);  // Blue LED (Low)
  } else if (percentage <= 75) {
    setLEDs(LOW, LOW, HIGH, LOW);  // Yellow LED (Medium)
  } else {
    setLEDs(LOW, LOW, LOW, HIGH);  // Green LED (Full)
  }

  // Seed Dropping Detection
  int seedState = digitalRead(SEED_SENSOR);
  if (seedState == HIGH) {
    digitalWrite(SEED_LED, HIGH);  // Turn ON LED when seed is detected
    Serial.println("Seed Dropped! (Metal detected)");
    powerUpServo();
    dropSeed();  // Activate servo to drop the seed
  } else {
    digitalWrite(SEED_LED, LOW);   // Turn OFF LED when no seed detected
    powerDownServo();
  }

  // Hole Position Detection
  int holeState = digitalRead(HOLE_SENSOR);
  if (holeState == HIGH) {
    digitalWrite(HOLE_LED, HIGH);  // Turn ON LED when hole marker is detected
    Serial.println("Hole Position Detected! (Metal marker detected)");
    powerUpActuator();
    moveActuator();  // Activate actuator to adjust the hole position
  } else {
    digitalWrite(HOLE_LED, LOW);   // Turn OFF LED
    powerDownActuator();
  }

  delay(500);  // Small delay for stabilization
}

// Function to get distance using an ultrasonic sensor
int getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;
  return distance;
}

// Function to control Seed Container LEDs
void setLEDs(int red, int blue, int yellow, int green) {
  digitalWrite(RED_LED, red);
  digitalWrite(BLUE_LED, blue);
  digitalWrite(YELLOW_LED, yellow);
  digitalWrite(GREEN_LED, green);
}

// Function to drop seed using the Servo Motor
void dropSeed() {
  seedServo.write(90);  // Move servo to 90 degrees (Clockwise) to drop seed
  delay(500);           // Wait for the seed to drop
  seedServo.write(0);   // Move servo back to 0 degrees (Counter Clockwise)
}

// Power up the Servo Motor (Enable 1, Clockwise input)
void powerUpServo() {
  digitalWrite(SERVO_PIN_1, HIGH);  // Power up the servo
  digitalWrite(SERVO_PIN_2, HIGH);  // Enable clockwise direction
  digitalWrite(SERVO_PIN_3, LOW);   // Disable counterclockwise direction
}

// Power down the Servo Motor
void powerDownServo() {
  digitalWrite(SERVO_PIN_1, LOW);   // Power down the servo
  digitalWrite(SERVO_PIN_2, LOW);   // Disable clockwise direction
  digitalWrite(SERVO_PIN_3, LOW);   // Disable counterclockwise direction
}

// Function to move Linear Actuator based on hole detection
void moveActuator() {
  digitalWrite(ACTUATOR_PIN_1, HIGH);  // Enable actuator
  digitalWrite(ACTUATOR_PIN_2, HIGH);  // Move actuator clockwise (Adjust hole)
  digitalWrite(ACTUATOR_PIN_3, LOW);   // Disable counterclockwise movement
  delay(1000); // Actuator movement time
  digitalWrite(ACTUATOR_PIN_1, LOW);   // Disable actuator after movement
  digitalWrite(ACTUATOR_PIN_2, LOW);   // Reset clockwise input
  digitalWrite(ACTUATOR_PIN_3, LOW);   // Reset counterclockwise input
}

// Power up the Linear Actuator (Enable 1, Clockwise input)
void powerUpActuator() {
  digitalWrite(ACTUATOR_PIN_1, HIGH);  // Power up the actuator
  digitalWrite(ACTUATOR_PIN_2, HIGH);  // Enable clockwise direction
  digitalWrite(ACTUATOR_PIN_3, LOW);   // Disable counterclockwise direction
}

// Power down the Linear Actuator
void powerDownActuator() {
  digitalWrite(ACTUATOR_PIN_1, LOW);   // Power down the actuator
  digitalWrite(ACTUATOR_PIN_2, LOW);   // Disable clockwise direction
  digitalWrite(ACTUATOR_PIN_3, LOW);   // Disable counterclockwise direction
}