// GROUP 5: AUTOMATIC WASTE SORTING ROBOT    
// GRADE 10-EINSTEIN  

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

// Initialize Servos and LCD
Servo Pipe_Servo;
Servo Gate_Servo;
LiquidCrystal_I2C lcd(0x27, 16, 2);  // Set the LCD address to 0x27 for a 16 chars and 2 line display

// Define the variables
int sensorInd = A0; 
int sensorpin = 2; 
int capValue;  // Value from the capacitive sensor
const int trigPin = 7;
const int echoPin = 8;
int i = 0;
float Pipe_Pos = 0.0;
float Gate_Pos = 0.0;
int n = 0;
long duration;
int distance;

void setup() {
  pinMode(trigPin, OUTPUT); 
  pinMode(echoPin, INPUT); 
  pinMode(sensorpin, INPUT); // Ensure the sensor pin is set to input
  Serial.begin(9600); 
  
  // Attach servos to their pins
  Pipe_Servo.attach(11);
  Gate_Servo.attach(10);
  
  // Initialize LCD
  lcd.init();   
  lcd.backlight(); 
  
  // Initial positions
  Pipe_Pos = Pipe_Servo.read();
  Gate_Pos = Pipe_Servo.read();
  Serial.println("Initial Positions:");
  Serial.print("Motor_PIPE Position: ");
  Serial.println(Pipe_Pos);
  Serial.print("Motor_GATE Position: ");
  Serial.println(Gate_Pos);
  
  // Open pipe and gate
  movePipe(90);
  openGate(25);
}

void loop() {
  delay(1000); // Reduced delay before taking new readings

  // Measure distance using ultrasonic sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;  // Convert time to distance

  // Read capacitive sensor value
  capValue = analogRead(sensorInd);  
  Serial.print("Distance: ");
  Serial.println(distance);
  Serial.print("Capacitive Sensor Value: ");
  Serial.println(capValue);
  
  delay(100); // Reduced delay for stability

  // Check conditions for sorting
  if (capValue > 700) {  // Threshold for plastic detection
    Serial.println("Plastic Detected");
    sortPlastic();
    updateLCD("Plastic", "Detected");
    Serial.println("Motor_PIPE Position: " + String(Pipe_Pos));
    Serial.println("Motor_GATE Position: " + String(Gate_Pos));
  } else if (capValue > 400) {  // Threshold for paper detection
    Serial.println("Paper Detected");
    sortPaper();
    updateLCD("Paper", "Detected");
    Serial.println("Motor_PIPE Position: " + String(Pipe_Pos));
    Serial.println("Motor_GATE Position: " + String(Gate_Pos));
  } else {  // Styrofoam or other material detected
    Serial.println("Styrofoam Detected");
    sortStyrofoam();
    updateLCD("Styrofoam", "Detected");
    Serial.println("Motor_PIPE Position: " + String(Pipe_Pos));
    Serial.println("Motor_GATE Position: " + String(Gate_Pos));
  }
}

// Function to move the pipe to a target position
void movePipe(int targetPos) {
  Pipe_Pos = Pipe_Servo.read();
  if (Pipe_Pos < targetPos) {
    for (i = Pipe_Pos; i <= targetPos; i++) {
      Pipe_Servo.write(i);
      delay(10);  // Reduced delay for faster pipe movement
    }
  } else {
    for (i = Pipe_Pos; i >= targetPos; i--) {
      Pipe_Servo.write(i);
      delay(10);  // Reduced delay for faster pipe movement
    }
  }
  delay(500); // Reduced delay for stability
}

// Function to open the gate
void openGate(int targetGatePos) {
  for (n = 0; n <= targetGatePos; n++) {  
    Gate_Servo.write(n);
    delay(10);  // Reduced delay for faster gate movement
  }   
  delay(500); // Reduced delay for stability
  for (n = targetGatePos; n >= 0; n--) { 
    Gate_Servo.write(n);
    delay(10);  // Reduced delay for faster gate movement
  }
}

// Functions to sort each material with varying positions for both pipe and gate
void sortPaper() {
  movePipe(90);  // Move pipe to paper bin
  openGate(45);  // Open gate halfway for paper
}

void sortPlastic() {
  movePipe(140); // Move pipe to plastic bin
  openGate(90);  // Fully open gate for plastic
}

void sortStyrofoam() {
  movePipe(45);  // Move pipe to styrofoam bin
  openGate(30);  // Open gate slightly for styrofoam
}

// Function to update the LCD with a message
void updateLCD(String line1, String line2) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(line1);
  lcd.setCursor(0, 1);
  lcd.print(line2);
}