#include "ESP32Servo.h"

#define PIN_TRIG 3
#define PIN_ECHO 2
#define LED_pin 4
#define dp_pin 26
#define motion_pin 2
#define hand_pin 20
#define ready_pin 14
#define time_pin 27
#define gbtn_pin 15
#define TRIG_PIN 4
#define ECHO_PIN 0
#define servoPin 23
//dp=start dispenser,motion=on if motion,hand=detect hand
//ready=insert hand,time=not yet

const long dp_interval = 1000;
const long distance = 30;
bool faceRec =0;
bool timeYet =0;


Servo servo;

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 sweep(int pos=0){
  for (pos = 0; pos <= 180; pos += 1) {
      servo.write(pos);
      delay(15);
      Serial.println(pos);
  }
}

void dopen(){
  servo.write(180);
}

void dclose(){
  servo.write(0);
}

void dispenser(){
  Serial.println("start dispenser");
  digitalWrite(dp_pin, HIGH);
  delay(dp_interval);
  Serial.println("start dispenser");
  dopen();
  dclose();
  Serial.println("finish dispenser");
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  digitalWrite(dp_pin, LOW);
  pinMode(dp_pin, OUTPUT);
  digitalWrite(motion_pin, LOW);
  pinMode(motion_pin, INPUT);
  digitalWrite(motion_pin, LOW);
  pinMode(hand_pin, OUTPUT);
  digitalWrite(ready_pin, LOW);
  pinMode(ready_pin, OUTPUT);
  digitalWrite(time_pin, LOW);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(time_pin, OUTPUT);
  servo.attach(servoPin, 500, 2400);
}

void loop() {
  int rdistanct = readDistanceCM();
  Serial.println(String(rdistanct) + "," + String(distance));
  if ((distance-5) <= readDistanceCM() && readDistanceCM() <= distance){
    Serial.println(String(readDistanceCM()) + "=" + String(distance));
    if (digitalRead(motion_pin)==HIGH) { //detect human start face recognition
      dispenser();
    } 
    delay(500);
  }


}