#include <Arduino.h>
#include <AccelStepper.h>

// Pin definitions
const int stepPin = 18;
const int dirPin = 19;

const int trigPin = 15;
const int echoPin = 4;

const int photoresistorPin = 35;
const int potWetPin = 34;
const int potDryPin = 32;

const int servoWetPin = 13;
const int servoDryPin = 12;

const int ledPower = 2;

AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);

enum WasteType { NONE, WET, DRY };

int wetThresholdPhoto = 2000;  // Adjust after calibration
int wetThresholdPot = 2000;
int dryThreshold = 1500;       // Lowered threshold for dry detection

WasteType detectWaste(int &photoValue, int &potWetValue, int &potDryValue) {
  photoValue = analogRead(photoresistorPin);
  potWetValue = analogRead(potWetPin);
  potDryValue = analogRead(potDryPin);

  Serial.print("Photo: "); Serial.print(photoValue);
  Serial.print(", PotWet: "); Serial.print(potWetValue);
  Serial.print(", PotDry: "); Serial.println(potDryValue);

  if (photoValue > wetThresholdPhoto || potWetValue > wetThresholdPot) {
    return WET;
  } else if (potDryValue > dryThreshold) {
    return DRY;
  } else {
    return NONE;
  }
}

void pickAndSort(bool isWet) {
  stepper.moveTo(200);
  while (stepper.distanceToGo() != 0) stepper.run();

  if (isWet) {
    Serial.println("Operating Wet Gripper");
    // Add wet servo control code here
  } else {
    Serial.println("Operating Dry Gripper");
    // Add dry servo control code here
  }

  stepper.moveTo(-200);
  while (stepper.distanceToGo() != 0) stepper.run();

  // Close grippers or release actions here if needed

  stepper.moveTo(0);
  while (stepper.distanceToGo() != 0) stepper.run();
}

long readUltrasonicDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 30000);
  if (duration == 0) return 999;
  return duration * 0.034 / 2;
}

void setup() {
  Serial.begin(115200);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(photoresistorPin, INPUT);
  analogReadResolution(12);

  pinMode(potWetPin, INPUT);
  pinMode(potDryPin, INPUT);

  pinMode(ledPower, OUTPUT);
  digitalWrite(ledPower, HIGH);

  // Initialize servo or PWM pins here

  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(500);
}

void loop() {
  long distance = readUltrasonicDistance();
  Serial.print("Distance: "); Serial.println(distance);

  if (distance < 30) {
    int photoValue, potWetValue, potDryValue;
    WasteType waste = detectWaste(photoValue, potWetValue, potDryValue);

    if (waste == WET) {
      Serial.println("Wet waste detected");
      pickAndSort(true);
    } else if (waste == DRY) {
      Serial.println("Dry waste detected");
      pickAndSort(false);
    } else {
      Serial.println("No waste detected");
    }
  }
  delay(2000);
}