#include <Servo.h>

// Pin definitions
const int trigPin = 3;
const int echoPin = 2;
const int servoPin = 5;
int eme_light = 12;
int servo_light = 8;

// Variables for ultrasonic sensor
long duration;
int distance;

// Variables for bag counting and timing
int largeBagCount = 0;
unsigned long lastLargeBagTime = 0;

// Threshold distances for large and small bags (example values)
const int largeBagDistance = 100; // Distance threshold for large bags in cm

// Safety mechanism time window
const int largeBagTimeWindow = 10000; // 10 seconds for large bags

// Servo motor object
Servo myServo;

void setup() {
  // Initialize serial communication
  Serial.begin(9600);
  
  // Set up pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(eme_light, OUTPUT);
  pinMode(servo_light, OUTPUT);
  
  // Attach the servo motor
  myServo.attach(servoPin);
  myServo.write(0); // Start position
}

void loop() {
  // Clear the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  // Set the trigPin HIGH for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Read the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  
  // Calculate the distance
  distance = duration * 0.034 / 2;
  
  // Print the distance to the Serial Monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Check for large bag detection
  if (distance < largeBagDistance) {
    largeBagCount++;
    lastLargeBagTime = millis();
    Serial.println("Large bag detected!");
    removeBag();
    delay(1000); // Wait for 5 seconds after removing a bag
  } else {
    Serial.println("No object detected.");
  }
  
  // Safety mechanism for large bags
  if (largeBagCount > 2 && (millis() - lastLargeBagTime) <= largeBagTimeWindow) {
    Serial.println("Safety halt: Too many large bags detected!");
    digitalWrite(eme_light, HIGH);
    haltAssemblyLine();
  }

  // Reset bag counts if outside time window
  if ((millis() - lastLargeBagTime) > largeBagTimeWindow) {
    largeBagCount = 0;
  }
  delay(1000);
}

// Function to remove a bag using the servo motor
void removeBag() {
  myServo.write(90); // Move the servo to 90 degrees to push the bag off the line
  digitalWrite(servo_light, HIGH);
  delay(500); // Wait for 0.5 seconds
  myServo.write(0); // Move the servo back to the start position
  digitalWrite(servo_light, LOW);
}

// Function to halt the assembly line
void haltAssemblyLine() {
  // Here you can add code to halt the assembly line, such as stopping a motor or triggering an alarm
  Serial.println("Assembly line halted!");
  while (true) {
    // Infinite loop to halt the system
  }

}