// 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

void setup() {
  Serial.begin(9600);  // Initialize serial communication

  // 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);
}

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)
  }

  delay(500);  // Stabilization delay
}

// 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);
}