#include <Arduino.h>
#include <Servo.h>

#define TOXIC_TRIG_PIN 2
#define TOXIC_ECHO_PIN 6
#define ANORGANIC_TRIG_PIN 3
#define ANORGANIC_ECHO_PIN 7
#define ORGANIC_TRIG_PIN 4
#define ORGANIC_ECHO_PIN 8
#define GESTURE_TRIG_PIN 5
#define GESTURE_ECHO_PIN 9
#define TOXIC_FULL_IR_PIN 10
#define ANORGANIC_FULL_IR_PIN 11
#define ORGANIC_FULL_IR_PIN 12
#define MOISTURE_PIN A0
#define TOXIC_SERVO_PIN 13
#define ANORGANIC_SERVO_PIN A1
#define ORGANIC_SERVO_PIN A2
#define TOXIC_LED_PIN A3
#define ANORGANIC_LED_PIN A4
#define ORGANIC_LED_PIN A5

const int FULL_DISTANCE_CM = 5;
const int GESTURE_DISTANCE_CM = 8;
const int MOISTURE_WET_THRESHOLD = 450;
const unsigned long GESTURE_WINDOW_MS = 500;
const unsigned long LID_OPEN_MS = 1500;

Servo toxicServo, anorganicServo, organicServo;
int toxicDistance = 400, anorganicDistance = 400, organicDistance = 400;
bool gesturePresentLast = false;
unsigned long firstGestureAt = 0, nextBinReadAt = 0;
uint8_t gestureCount = 0;

int readDistanceCm(uint8_t trig, uint8_t echo, int maximum) {
  digitalWrite(trig, LOW); delayMicroseconds(3);
  digitalWrite(trig, HIGH); delayMicroseconds(10); digitalWrite(trig, LOW);
  unsigned long duration = pulseIn(echo, HIGH, (unsigned long)maximum * 58UL + 2000UL);
  return duration ? (int)(duration / 58UL) : maximum + 1;
}

bool binIsFull(int distance, uint8_t irPin) { return distance <= FULL_DISTANCE_CM || digitalRead(irPin) == LOW; }
bool materialIsWet() { return analogRead(MOISTURE_PIN) < MOISTURE_WET_THRESHOLD; }

void openLid(Servo &servo, uint8_t ledPin, bool full, const char *name) {
  if (full) { Serial.print(name); Serial.println(" bin is full; lid stays closed."); return; }
  digitalWrite(ledPin, HIGH); servo.write(90); Serial.print("Opening "); Serial.println(name);
  delay(LID_OPEN_MS); servo.write(0); digitalWrite(ledPin, LOW);
}

void updateBinLevels() {
  toxicDistance = readDistanceCm(TOXIC_TRIG_PIN, TOXIC_ECHO_PIN, 30); delay(25);
  anorganicDistance = readDistanceCm(ANORGANIC_TRIG_PIN, ANORGANIC_ECHO_PIN, 30); delay(25);
  organicDistance = readDistanceCm(ORGANIC_TRIG_PIN, ORGANIC_ECHO_PIN, 30);
}

void setup() {
  Serial.begin(115200);
  const uint8_t trigPins[] = {TOXIC_TRIG_PIN, ANORGANIC_TRIG_PIN, ORGANIC_TRIG_PIN, GESTURE_TRIG_PIN};
  const uint8_t echoPins[] = {TOXIC_ECHO_PIN, ANORGANIC_ECHO_PIN, ORGANIC_ECHO_PIN, GESTURE_ECHO_PIN};
  for (uint8_t p : trigPins) pinMode(p, OUTPUT);
  for (uint8_t p : echoPins) pinMode(p, INPUT);
  pinMode(TOXIC_FULL_IR_PIN, INPUT); pinMode(ANORGANIC_FULL_IR_PIN, INPUT); pinMode(ORGANIC_FULL_IR_PIN, INPUT);
  pinMode(TOXIC_LED_PIN, OUTPUT); pinMode(ANORGANIC_LED_PIN, OUTPUT); pinMode(ORGANIC_LED_PIN, OUTPUT);
  digitalWrite(TOXIC_LED_PIN, LOW); digitalWrite(ANORGANIC_LED_PIN, LOW); digitalWrite(ORGANIC_LED_PIN, LOW);
  toxicServo.attach(TOXIC_SERVO_PIN); anorganicServo.attach(ANORGANIC_SERVO_PIN); organicServo.attach(ORGANIC_SERVO_PIN);
  toxicServo.write(0); anorganicServo.write(0); organicServo.write(0);
  updateBinLevels(); nextBinReadAt = millis() + 400; Serial.println("Smart trash sorter ready.");
}

void loop() {
  unsigned long now = millis();
  if (now >= nextBinReadAt) { updateBinLevels(); nextBinReadAt = millis() + 400; }
  if (materialIsWet()) {
    openLid(organicServo, ORGANIC_LED_PIN, binIsFull(organicDistance, ORGANIC_FULL_IR_PIN), "organic");
    gestureCount = 0; firstGestureAt = 0; delay(100); return;
  }
  int gestureDistance = readDistanceCm(GESTURE_TRIG_PIN, GESTURE_ECHO_PIN, 12);
  bool gesturePresent = gestureDistance <= GESTURE_DISTANCE_CM;
  if (gesturePresent && !gesturePresentLast) {
    if (gestureCount == 0 || now - firstGestureAt > GESTURE_WINDOW_MS) { gestureCount = 1; firstGestureAt = now; }
    else { gestureCount++; }
  }
  gesturePresentLast = gesturePresent;
  if (gestureCount >= 2 && now - firstGestureAt <= GESTURE_WINDOW_MS) {
    openLid(toxicServo, TOXIC_LED_PIN, binIsFull(toxicDistance, TOXIC_FULL_IR_PIN), "toxic"); gestureCount = 0; firstGestureAt = 0;
  } else if (gestureCount == 1 && now - firstGestureAt > GESTURE_WINDOW_MS) {
    openLid(anorganicServo, ANORGANIC_LED_PIN, binIsFull(anorganicDistance, ANORGANIC_FULL_IR_PIN), "anorganic"); gestureCount = 0; firstGestureAt = 0;
  }
  delay(20);
}