#include <ESP32Servo.h>
#define TRIG 12
#define ECHO 14
#define LED_R 25
#define LED_G 33
#define LED_Y 26
#define SERVO 13
#define LDR 32

float CalculateDist();
void LEDMode(int, int);

Servo servo_A;
int duty_Cycle = 0;
int duty_Step = 10;
int ldr_Val;
int state = 0; // 0: rest, 1: car-in, 2: card-ok
bool cardShow = false;
bool stateIn[3] = {0, 0, 0};
bool isBlink = false;
float blink_Timer = 0;
float reset_Timer = 0;

void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, OUTPUT);
  pinMode(LED_R, OUTPUT);
  pinMode(LED_G, OUTPUT);
  pinMode(LED_Y, OUTPUT);
  servo_A.attach(SERVO);
  ledcSetup(2, 1000, 8);
  ledcAttachPin(LED_R, 2);
  Serial.println("========== Start the Program ==========");
}

void loop() {
  if(state == 0){
    if(stateIn[0] == 0){
      Serial.println("Waiting for the car...");
      servo_A.write(0);
      stateIn[0] = 1;
    }
    LEDMode(0, 2);
    float dist = CalculateDist();
    if(dist < 100){
      state = 1;
      stateIn[0] = 0;
    }
  }else if(state == 1){
    if(stateIn[1] == 0){
      Serial.println("The car is close to the door. Please show your card.");
      stateIn[1] = 1;
    }
    LEDMode(1, 0);
    ldr_Val = analogRead(LDR);
    if(ldr_Val > 3600){
      state = 2;
      stateIn[1] = 0;
    }
  }else if(state == 2){
    if(stateIn[2] == 0){
      Serial.println("Identity verification successful!");
      Serial.println("You are good to go~");
      reset_Timer = millis();
      stateIn[2] = 1;
      servo_A.write(100);
    }
    LEDMode(2, 0);
    delay(10000);
  }
}

float CalculateDist(){
  digitalWrite(TRIG, LOW);
  delayMicroseconds(5);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  float temp_Dist = pulseIn(ECHO, HIGH) / 29.4 / 2;
  return temp_Dist;
}

void LEDMode(int _state, int _step){
  if(_state == 0){
    if(duty_Cycle >= 155){
      duty_Step = -_step;
    }else if(duty_Cycle <= 0){
      duty_Step = _step;
    }
    duty_Cycle = duty_Cycle + duty_Step;
    ledcWrite(2, duty_Cycle);
  }else if(_state == 1){
    ledcWrite(2, 0);
    if(millis() - blink_Timer > 600){
      isBlink = !isBlink;
      blink_Timer = millis();
    }
    digitalWrite(LED_Y, isBlink);
  }else if(_state == 2){
    digitalWrite(LED_Y, LOW);
    digitalWrite(LED_G, HIGH);
  }
}