#include <ESP32Servo.h>
#include <Ultrasonic.h>

#define servoPin  16
#define refillPin 17
#define manualPin 18
#define trigPin   26   // Ultrasonic Trigger pin
#define echoPin   25   // Ultrasonic Echo pin
#define redLED    14   // Red LED pin
#define greenLED  12  // Green LED pin

boolean manualState = false;
boolean refillState = false;

unsigned long currStop = 0;
unsigned long prevStop = 0;

int pos = 0;
boolean default_pos = false;

Servo myServo;
Ultrasonic ultrasonic(trigPin, echoPin);

void IRAM_ATTR intteruptManualorAuto() {
  currStop = millis();
  if(currStop - prevStop > 250){
    if(manualState==true) {
      manualState = false;
      default_pos = true;
    }
    else {
      manualState = true;
      refillState = false;
      default_pos = true;
    }
    prevStop = currStop;
  }
}

void IRAM_ATTR intteruptStartStopRefill() {
  currStop = millis();
  if(currStop - prevStop > 250){
    if(refillState==true) refillState = false;
    else refillState = true;
    prevStop = currStop;
  }
}

void startRefill(){
  for (pos = 0; pos < 90; pos += 1) {
    myServo.write(pos);
    delay(10);
  }
}

void stopRefill(){
  for (pos = 90; pos > 0; pos -= 1) {
    myServo.write(pos);
    delay(10);
  }
}

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

  pinMode(manualPin, INPUT_PULLUP);
  pinMode(refillPin, INPUT_PULLUP);
  pinMode(redLED, OUTPUT);
  pinMode(greenLED, OUTPUT);
  
  myServo.attach(servoPin, 500, 2400);

  attachInterrupt(manualPin, intteruptManualorAuto, HIGH);
  attachInterrupt(refillPin, intteruptStartStopRefill, HIGH);
}

void loop() {
  if(default_pos){
    if(pos==90) stopRefill();
    default_pos = false;
  }

  if(manualState){
    // Manual mode
    Serial.print("Status: Manual");
    if(refillState) {
      Serial.println(" *** Start Refill food");
      if(pos==0) startRefill();
    }
    else {
      Serial.println(" *** Stop Refill food");
      if(pos==90) stopRefill();
    }
  } else {
    // Automatic mode
    Serial.println("Status: Automatic");
    long distance = ultrasonic.read();
    if(distance < 10) {
      digitalWrite(greenLED, HIGH);
      digitalWrite(redLED, LOW);
      if(pos == 0) startRefill();
    } else if(distance > 20) {
      digitalWrite(redLED, HIGH);
      digitalWrite(greenLED, LOW);
      if(pos == 90) stopRefill();
    } else {
      digitalWrite(redLED, LOW);
      digitalWrite(greenLED, LOW);
    }
  }
  delay(5000);
}