#define BLYNK_AUTH_TOKEN "6RCSgp8uC4Qbp-OGiQqLCvtS7aI6WYfq"
#define BLYNK_PRINT Serial
#define BLYNK_TEMPLATE_ID "TMPLcszvWkO8"
#define BLYNK_DEVICE_NAME "Flood"

#include <WiFi.h>
#include <WiFiClient.h>
#include <BlynkSimpleEsp32.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include "ThingsBoard.h"
#include <ESP32Servo.h>
#include "DHTesp.h"

void setGate();

char auth[] = "6RCSgp8uC4Qbp-OGiQqLCvtS7aI6WYfq";
char ssid[] = "Wokwi-GUEST";
char pass[] = "";

#define CURRENT_FIRMWARE_TITLE    "TEST"
#define CURRENT_FIRMWARE_VERSION  "1.0.0"

// #define WIFI_SSID           "Wokwi-GUEST"
// #define WIFI_PASSWORD       ""

#define TOKEN               "K2Vfl5Fl3pk5gnDjVTdX"
#define THINGSBOARD_SERVER  "thingsboard.cloud"

BlynkTimer timer;

#define PIN_TRIG 4
#define PIN_ECHO 2

const int servoPin1 = 21;
const int servoPin2 = 22;
const int servoPin3 = 23;

const int DHT_PIN = 12;

const int buzzer = 15; //inisialisai buzzer pada pin 15

//Di bawah ini adalah variable yang dapat di ubah
float distance = 0;
float ketinggian = 0;

Servo gate1;
Servo gate2;
Servo gate3;
int pos = 0;

WiFiClient espClient;
ThingsBoard tb(espClient);
int status = WL_IDLE_STATUS;

void InitWiFi()
{
  Serial.println("Connecting to AP ...");
  WiFi.begin(ssid, pass);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("Connected to AP");
}

void reconnect() {
  status = WiFi.status();
  if ( status != WL_CONNECTED) {
    WiFi.begin(ssid, pass);
    while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.print(".");
    }
    Serial.println("Connected to AP");
  }
}

//Fungsi Membaca jarak
void read_distance(){
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  
  int duration = pulseIn(PIN_ECHO, HIGH);//Mengukur durasi pulse dari pin echo
  distance = duration * 0.034 / 2; //Menghitung jarak
  ketinggian = 400 - distance;

  // Serial.print("Jarak: ");
  // Serial.print(distance);
  // Serial.println("CM");

  // Serial.print("Ketinggian Air: ");
  // Serial.print(ketinggian);
  // Serial.println("CM");
}

void setup() {
Serial.begin(115200);
Blynk.begin(auth, ssid, pass);
  Serial.print("Connecting to WiFi");
  WiFi.begin("Wokwi-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }
  Serial.println(" Connected!");
  Serial.println("START");

pinMode(PIN_TRIG, OUTPUT);
pinMode(PIN_ECHO, INPUT);
pinMode(buzzer, OUTPUT);

gate1.attach(servoPin1, 500, 2400);
gate2.attach(servoPin2, 500, 2400);
gate3.attach(servoPin3, 500, 2400);

timer.setInterval(1000L, send_blynk);
  
}

void loop() {
  if (WiFi.status() != WL_CONNECTED) {
    reconnect();
  }

  if (!tb.connected()) {
    // Connect to the ThingsBoard
    Serial.print("Connecting to: ");
    Serial.print(THINGSBOARD_SERVER);
    Serial.print(" with token ");
    Serial.println(TOKEN);
    if (!tb.connect(THINGSBOARD_SERVER, TOKEN)) {
      Serial.println("Failed to connect");
      return;
    }
  }
setGate();
read_distance();//membaca sensor jarak 
send_blynk();//mengirim data ke blynk 
send_tb(); //mengirim data ke ThingsBoard
Blynk.run();
timer.run();
tb.loop();
}

void send_tb(){
  tb.sendTelemetryFloat("Ketinggian_Air", ketinggian);
  Serial.print(" Ketinggian Air: ");
  Serial.print(ketinggian);
  Serial.println(" CM");
}

void send_blynk(){
Blynk.virtualWrite(V0, ketinggian);
  //WASPADA
  if(ketinggian > 300){
    Blynk.virtualWrite(V12, 0);
    Blynk.virtualWrite(V13, 1);
    Blynk.virtualWrite(V14, 0);
    set_buzzer();
  }
  //SIAGA
  if(ketinggian > 200 && ketinggian < 300){
    Blynk.virtualWrite(V12, 1);
    Blynk.virtualWrite(V13, 0);
    Blynk.virtualWrite(V14, 0);
  }
  //NORMAL
  if(ketinggian < 200){
    Blynk.virtualWrite(V12, 0);
    Blynk.virtualWrite(V13, 0);
    Blynk.virtualWrite(V14, 1);
  }
}

void setGate(){
  // bool siaga1 = distance < 100;
  if (ketinggian > 300){
    gate1.write(180);
    gate2.write(180);
    gate3.write(180);
    Serial.println("PINTU AIR WASPADA");
  }
  if(ketinggian > 200 && ketinggian < 300){
    gate1.write(180);
    gate2.write(90);
    Serial.println("PINTU AIR SIAGA 2");
  }
  if(ketinggian > 150 && ketinggian < 200){
    gate1.write(90);
    Serial.println("PINTU AIR SIAGA 1");
  }
  if(ketinggian < 150){
    gate1.write(pos);
    gate2.write(pos);
    gate3.write(pos);
    Serial.println("PINTU AIR NORMAL");
  }
}

void set_buzzer(){
  tone(buzzer, 1000);
  delay(3000);
  tone(buzzer, 500);
  delay(500);
  noTone(buzzer);
  delay(500);
}












// void openServo(){
//   for (pos = 0; pos <= 180; pos += 1) {
//     gate1.write(pos);
//     delay(15);
//   }
// }

// void closeServo(){
//     for (pos = 180; pos >= 0; pos -= 1) {
//     gate1.write(pos);
//     delay(15);
//   }
// }