#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Define pins
#define TRIG_PIN 9
#define ECHO_PIN 10
#define GREEN_LED_PIN 7
#define RED_LED_PIN 8
#define SERVO_PIN 6

// Constants
#define LARGE_BAG_DISTANCE 100
#define SMALL_BAG_DISTANCE 50
#define LARGE_BAG_LIMIT_PER_MINUTE 10
#define SMALL_BAG_LIMIT_PER_MINUTE 10
#define LARGE_BAG_DETECTION_WINDOW 10000 // in milliseconds
#define SMALL_BAG_DETECTION_WINDOW 6000  // in milliseconds
#define TIMEOUT 2500 // 2.5 seconds

// Variables
Servo conveyor;
unsigned long largeBagTimestamps[LARGE_BAG_LIMIT_PER_MINUTE];
unsigned long smallBagTimestamps[SMALL_BAG_LIMIT_PER_MINUTE];
int largeBagCount = 0;
int smallBagCount = 0;
bool systemHalted = false;
LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  Serial.begin(9600);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  conveyor.attach(SERVO_PIN);
  conveyor.write(0); // Initialize conveyor position

  lcd.begin(16, 2);
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Status: Running");
  lcd.setCursor(0, 1);
  lcd.print("Bags: 0L, 0S");

  digitalWrite(GREEN_LED_PIN, HIGH);
  digitalWrite(RED_LED_PIN, LOW);
}

void loop() {
  if (!systemHalted) {
    int distance = getDistance();

    if (distance <= LARGE_BAG_DISTANCE && distance > SMALL_BAG_DISTANCE) {
      detectBag("Large");
    } else if (distance <= SMALL_BAG_DISTANCE) {
      detectBag("Small");
    }

    checkOverload();

    delay(500); // Adjust delay as needed
  }

  // Reset the counts every minute
  if (millis() % 60000 < 500) {
    largeBagCount = 0;
    smallBagCount = 0;
    systemHalted = false;
    digitalWrite(GREEN_LED_PIN, HIGH);
    digitalWrite(RED_LED_PIN, LOW);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Status: Running");
    lcd.setCursor(0, 1);
    lcd.print("Bags: 0L, 0S");
  }
}

int getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  unsigned long duration = pulseIn(ECHO_PIN, HIGH);
  int distance = duration * 0.034 / 2;

  return distance;
}

void detectBag(String bagType) {
  unsigned long currentTime = millis();

  if (bagType == "Large") {
    if (largeBagCount < LARGE_BAG_LIMIT_PER_MINUTE) {
      largeBagTimestamps[largeBagCount++] = currentTime;
      Serial.println("Large bag detected");
      updateLCD();

      delay(TIMEOUT);
      conveyor.write(180);
      delay(TIMEOUT);
      conveyor.write(0);
    }
  } else if (bagType == "Small") {
    if (smallBagCount < SMALL_BAG_LIMIT_PER_MINUTE) {
      smallBagTimestamps[smallBagCount++] = currentTime;
      Serial.println("Small bag detected");
      updateLCD();

      delay(TIMEOUT);
      conveyor.write(180);
      delay(TIMEOUT);
      conveyor.write(0);
    }
  }
}

void checkOverload() {
  unsigned long currentTime = millis();
  int largeBagsInWindow = 0;
  int smallBagsInWindow = 0;

  // Check large bags within the last 10 seconds
  for (int i = 0; i < largeBagCount; i++) {
    if (currentTime - largeBagTimestamps[i] <= LARGE_BAG_DETECTION_WINDOW) {
      largeBagsInWindow++;
    }
  }

  // Check small bags within the last 6 seconds
  for (int i = 0; i < smallBagCount; i++) {
    if (currentTime - smallBagTimestamps[i] <= SMALL_BAG_DETECTION_WINDOW) {
      smallBagsInWindow++;
    }
  }

  // Check if system should halt
  if (largeBagsInWindow >= 2 || smallBagsInWindow >= 2) {
    systemHalted = true;
    digitalWrite(GREEN_LED_PIN, LOW);
    digitalWrite(RED_LED_PIN, HIGH);
    Serial.println("System halted due to overload");
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Status: Halted");
  } else {
    digitalWrite(GREEN_LED_PIN, HIGH);
    digitalWrite(RED_LED_PIN, LOW);
    Serial.println("System running");
  }
}

void updateLCD() {
  lcd.clear();
  lcd.setCursor(0, 0);
  if (systemHalted) {
    lcd.print("Status: Halted");
  } else {
    lcd.print("Status: Running");
  }
  lcd.setCursor(0, 1);
  lcd.print("Bags: ");
  lcd.print(largeBagCount);
  lcd.print("L, ");
  lcd.print(smallBagCount);
  lcd.print("S");
}