#include "sensorHandler.h"
#include <ESP32Servo.h>

#define SERVO_PIN 18

IRsensor IRsensor1(12); // object declaration
IRsensor IRsensor2(13);
IRsensor IRsensor3(14);
IRsensor IRsensor4(15);


Servo servo;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  IRsensor1.begin();
  servo.attach(SERVO_PIN);
  servo.write(90);

  Serial.println("Hello, ESP32-S2!");
}

void loop() {
  // put your main code here, to run repeatedly:

  Serial.printf("IR State1: %d\n", IRsensor1.getRead());
  Serial.printf("IR State2: %d\n", IRsensor2.getRead());
  Serial.printf("IR State3: %d\n", IRsensor3.getRead());
  Serial.printf("IR State4: %d\n", IRsensor4.getRead());
  Serial.println();

  gate();
  //tinggi_sampah();
  //fuzzifikasi();

  //Serial.printf("Pos Servo: %d\n", servo.getRead());

  delay(300); // this speeds up the simulation
}

void gate()
{
  if (!IRsensor1.getRead() or !IRsensor2.getRead() || !IRsensor3.getRead() || !IRsensor4.getRead())
   {
     servo.write(90);
   }
   else{
     servo.write(0);
  }
}