#include <Servo.h>

// Define pins
const int irSensorPin = A0;
const int servoRoofPin = 5;
const int servoPlasticPin = 4;
const int servoPaperPin = 3;
const int trigPinRoof = 32;
const int echoPinRoof = 33;

// Define thresholds for IR sensor
const int materialThreshold = 30;  // Above 30 is paper, below or equal is plastic

// Define servo objects
Servo servoRoof;
Servo servoPlastic;
Servo servoPaper;

// Define servo positions
const int roofOpenPosition = 180;  
const int roofClosePosition = 0;   
const int plasticOpenPosition = 0;   
const int plasticClosePosition = 90; 
const int paperOpenPosition = 180;   
const int paperClosePosition = 90;   

// Define delay for slow servo movement
const int servoDelay = 15; 

// Variables to track the current state
bool isRoofOpen = false;
bool isObjectPresent = false;
String lastDetectedMaterial = "none";

// Define ultrasonic sensor threshold (in cm)
const int ultrasonicThreshold = 50; // If distance is LESS than this, the roof opens

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

  // Initialize IR sensor pin
  pinMode(irSensorPin, INPUT);

  // Initialize ultrasonic sensor pins
  pinMode(trigPinRoof, OUTPUT);
  pinMode(echoPinRoof, INPUT);

  // Attach servos to their respective pins
  servoRoof.attach(servoRoofPin);
  servoPlastic.attach(servoPlasticPin);
  servoPaper.attach(servoPaperPin);

  // Initialize servos to their closed positions
  servoRoof.write(roofClosePosition);
  servoPlastic.write(plasticClosePosition);
  servoPaper.write(paperClosePosition);

  Serial.println("Smart Trash Sorter initialized");
  
  // Run unit tests
  runUnitTests();
}

void loop() {
  // Check if a person is detected using the ultrasonic sensor
  bool personDetected = detectPerson();

  if (personDetected && !isRoofOpen) {
    openRoof();
    isRoofOpen = true;
  } else if (!personDetected && isRoofOpen) {
    closeRoof();
    isRoofOpen = false;
  }

  // Read the IR sensor value
  int irValue = analogRead(irSensorPin);
  Serial.print("IR Sensor Value: ");
  Serial.println(irValue);

  // Determine if the object is paper or plastic
  String detectedMaterial = "none";
  if (irValue <= materialThreshold) {
    detectedMaterial = "plastic";
  } else {
    detectedMaterial = "paper";
  }

  // Sort the object if a new material is detected
  if (detectedMaterial != "none" && detectedMaterial != lastDetectedMaterial) {
    Serial.print("Detected: ");
    Serial.println(detectedMaterial);
    
    if (detectedMaterial == "plastic") {
      sortPlastic();
    } else if (detectedMaterial == "paper") {
      sortPaper();
    }
    
    lastDetectedMaterial = detectedMaterial;
    isObjectPresent = true;
  } else if (irValue == 0 && isObjectPresent) {
    // No object detected, reset the state
    Serial.println("No object detected");
    lastDetectedMaterial = "none";
    isObjectPresent = false;
  }

  // Add a small delay to avoid rapid looping
  delay(500);
}

bool detectPerson() {
  // Send a short pulse to the ultrasonic sensor
  digitalWrite(trigPinRoof, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinRoof, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinRoof, LOW);

  // Measure the duration of the echo pulse
  long duration = pulseIn(echoPinRoof, HIGH);

  // Calculate the distance in centimeters
  float distance = duration * 0.034 / 2;

  // If the distance is less than the threshold, consider it as a person detected
  return (distance < ultrasonicThreshold);
}

void openRoof() {
  for (int pos = roofClosePosition; pos <= roofOpenPosition; pos++) {
    servoRoof.write(pos);
    delay(servoDelay);
  }
  Serial.println("Roof opened");
}

void closeRoof() {
  for (int pos = roofOpenPosition; pos >= roofClosePosition; pos--) {
    servoRoof.write(pos);
    delay(servoDelay);
  }
  Serial.println("Roof closed");
}

void sortPlastic() {
  for (int pos = plasticClosePosition; pos >= plasticOpenPosition; pos--) {
    servoPlastic.write(pos);
    delay(servoDelay);
  }
  delay(1000); // Wait for the object to fall
  for (int pos = plasticOpenPosition; pos <= plasticClosePosition; pos++) {
    servoPlastic.write(pos);
    delay(servoDelay);
  }
  Serial.println("Sorted plastic");
}

void sortPaper() {
  for (int pos = paperClosePosition; pos <= paperOpenPosition; pos++) {
    servoPaper.write(pos);
    delay(servoDelay);
  }
  delay(1000); // Wait for the object to fall
  for (int pos = paperOpenPosition; pos >= paperClosePosition; pos--) {
    servoPaper.write(pos);
    delay(servoDelay);
  }
  Serial.println("Sorted paper");
}

void runUnitTests() {
  Serial.println("Running unit tests...");
  delay(1000);

  // Test plastic detection
  Serial.println("Testing plastic detection...");
  simulateIRValue(20);
  delay(2000);

  // Test paper detection
  Serial.println("Testing paper detection...");
  simulateIRValue(40);
  delay(2000);

  // Test person detection (close)
  Serial.println("Testing person detection (close)...");
  simulateUltrasonicDistance(30);
  delay(2000);

  // Test person detection (far)
  Serial.println("Testing person detection (far)...");
  simulateUltrasonicDistance(70);
  delay(2000);

  Serial.println("Unit tests completed.");
}

void simulateIRValue(int value) {
  Serial.print("Simulated IR Value: ");
  Serial.println(value);
  
  String detectedMaterial = (value <= materialThreshold) ? "plastic" : "paper";
  Serial.print("Detected Material: ");
  Serial.println(detectedMaterial);
  
  if (detectedMaterial == "plastic") {
    sortPlastic();
  } else {
    sortPaper();
  }
}

void simulateUltrasonicDistance(float distance) {
  Serial.print("Simulated Distance: ");
  Serial.print(distance);
  Serial.println(" cm");
  
  if (distance < ultrasonicThreshold) {
    Serial.println("Person detected, opening roof");
    openRoof();
  } else {
    Serial.println("No person detected, closing roof");
    closeRoof();
  }
}