// ESP32 FreeRTOS Door opening

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

const int trig  = 25;  
const int echo  = 33; 
const int led_r = 23;
const int led_g = 16;

const int servoPin = 15;
Servo servo;

#define SCREEN_WIDTH 128 
#define SCREEN_HEIGHT 64 

Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

int pos = 0;
unsigned long duration; // biến đo thời gian
int distance,status = 0, wait = 0;

TaskHandle_t distance_rtos;
TaskHandle_t button_state;

void ledOpen(){
  digitalWrite(led_g,HIGH);
  digitalWrite(led_r,LOW);
}

void ledClose(){
  digitalWrite(led_g,LOW);
  digitalWrite(led_r,HIGH);
}

void display(void *parameter){
  for(;;){
    oled.clearDisplay();
    oled.setTextSize(1);         
    oled.setTextColor(WHITE);    
    oled.setCursor(20, 2);  
    if (status==0){
      oled.println("Door is closing");
      oled.setCursor(40,40);
      oled.setTextSize(2);         
      oled.println("Stop");
    }
    else if(status==1){
      oled.println("Door is opening");
      oled.setCursor(20,40);
      oled.setTextSize(2);         
      oled.println("Come in");
    }     
    oled.display();
    delay(200);
  }
}

void ultrasonic(void *parameter){
  for(;;){
    digitalWrite(trig,0);   // tắt chân trig
    delayMicroseconds(2);
    digitalWrite(trig,1);   // phát xung từ chân trig
    delayMicroseconds(5);   // xung có độ dài 5 microSeconds
    digitalWrite(trig,0);   // tắt chân trig
    duration = pulseIn(echo,HIGH);  
    distance = int(duration/2/29.412);
    vTaskDelay(pdMS_TO_TICKS(200)); 
  }
}

void detect(void *parameter){
  for(;;){
  if (distance < 50){
    servo.write(90);
    wait = 2;
    status = 1;
      ledOpen();
    }
    if (distance >=50 && wait==2){
      ledOpen();
      status = 1;
      while(wait>0){
        wait--;
        delay(1000);
      }
      // wait = 0;
    }
    if (distance >=50 && wait ==0){
      servo.write(0);
      status = 0;
      ledClose();
    }
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(led_r,OUTPUT);
  pinMode(led_g,OUTPUT);
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("failed to start SSD1306 OLED"));
    while (1);
  }
  servo.attach(servoPin, 500, 2400);
  delay(2000);         // wait two seconds for initializing
  oled.clearDisplay(); // clear display
  xTaskCreate(
    display,"Displayoled",2048,NULL,0,NULL
  );
  xTaskCreate(
    ultrasonic,"Khoangcach",2048,NULL,3,&distance_rtos
  );
  xTaskCreate(
    detect,"Dectect_object",2048,NULL,2,NULL
  );
}

void loop() {
  
}