#include <Servo.h>
#include <Stepper.h>

#define ECHO_PIN 2
#define TRIG_PIN 3
#define BUTTON1_PIN 8
#define BUTTON2_PIN 12

Servo myservo1;
Servo myservo2;

int buahMerah;
int buahHijau;
int buah =0;
int konveyor =1;

int lastState1 = HIGH;
int lastState2 = HIGH;

int pirState1 = LOW;
int inputMerah = 10;

int pirState2 = LOW;
int inputHijau = 7;

int val = 0;
int val2 = 0;

const int stepsPerRevolution = 200;
Stepper myStepper(stepsPerRevolution, 0, 9, 10, 11);

void yor(){
  if (konveyor == 1 ) {
    Serial.println("konveyor Jalan");
    
  }
  else {
    Serial.println("konveyor Mati");
  }
}
void setup() {
  Serial.begin(115200);

  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUTTON1_PIN, INPUT_PULLUP);
  pinMode(BUTTON2_PIN, INPUT_PULLUP);
  
  myservo1.attach(5);
  myservo2.attach(6);

  myservo1.write(0);
  myservo2.write(0);

  pinMode(inputMerah, INPUT);
  pinMode(inputHijau, INPUT);

  myStepper.setSpeed(100);
}

float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

void loop() {
  yor();
  myStepper.step(stepsPerRevolution);

  int value1 = digitalRead((BUTTON1_PIN));
  int value2 = digitalRead((BUTTON2_PIN));
  float distance = readDistanceCM();

  bool isNearby = distance < 10;
  digitalWrite(LED_BUILTIN, isNearby);

  Serial.print("Deteksi Buah : ");
  if (distance < 10 ){
      Serial.println("Ada");
      buah = 1;
      konveyor = 0;
  }
  else{
      Serial.print("Tidak Ada\n");
      buah = 0;
      konveyor = 1;
  }
  if (buah == 1){
    Serial.println("Deteksi Warna");
     if (lastState1 != value1) {
       lastState1 = value1;
       if (value1 == LOW) {
         Serial.println("Merah");
         buahMerah =1;
         myservo1.write(60);
       }
     }
     if (lastState2 != value2) {
       lastState2 = value2;
       if (value2 == LOW) {
         Serial.println("Hijau");
         buahHijau =1;
         myservo2.write(60);
       }
      }
    if (buahHijau == 1 || buahMerah == 1){
      konveyor = 1;
    }
   }

  val = digitalRead(inputMerah);
  if (val == HIGH) {           
    if (pirState1 == LOW) {
      myservo1.write(0);
      buahMerah = 0;
      pirState1 = HIGH;
    }
  } else {
    if (pirState1 == HIGH) {
      pirState1 = LOW;
    }
  }

  val2 = digitalRead(inputHijau);
  if (val2 == HIGH) {           
    if (pirState2 == LOW) {
      myservo2.write(0);
      buahHijau = 0;
      pirState2 = HIGH;
    }
  } else {
    if (pirState2 == HIGH) {
      pirState2 = LOW;
    }
  }
  delay(2000);
}