#define BLYNK_TEMPLATE_ID "TMPL6rMv3KUNG"
#define BLYNK_TEMPLATE_NAME "ParkingSensor"
#define BLYNK_AUTH_TOKEN "1mZc4hKAs-7GlzIzzuFAdMIXCfgGTdD1"
#define BLYNK_PRINT Serial

// Include necessary libraries for the project
#include "CTBot.h"
#include <WiFi.h>  // wifi
#include <WiFiClient.h>
#include <ArduinoJson.h>
#include <BlynkSimpleEsp32.h>
#include <ESP32Servo.h>

// Define the pin assignments for various components
#define ECHO_B_PIN 15
#define TRIG_B_PIN 2
#define ECHO_A_PIN 32
#define TRIG_A_PIN 33
#define SERVO_PIN 27
#define Red 14
#define Yellow 12
#define Green 13
#define buzzer 26
#define DISTANCE_THRESHOLD  80 // Threshold distance for servo control

// Create an instance of the Servo class to control a servo motor
Servo servo;

// Declare variables for ultrasonic sensor A measurements
float duration_us, distance_cm;

// Configure the Telegram bot settings
String token = "6481832459:AAHqqR_W0GTRYZU7eUQncoLWNvIBOd1a-rE"; // Your Telegram bot token
const int bot_id = 1521343130; // Your Telegram user ID

// Configure Blynk settings
char auth[] = BLYNK_AUTH_TOKEN; // Your Blynk authentication token
char ssid[] = "Wokwi-GUEST"; // Your WiFi SSID
char pass[] = ""; // Your WiFi password

CTBot mybot; // Create an instance of the CTBot class for Telegram bot interaction
BlynkTimer timer; // Create a timer instance for Blynk-related tasks

// Declare variables for ultrasonic sensor B measurements and Blynk
float distance = 0;
int pinValue2; // A placeholder value for Blynk control

// Function to measure distance using ultrasonic sensor B
void cal_distance(){
  // Trigger the ultrasonic sensor by sending a pulse
  digitalWrite(TRIG_B_PIN, LOW);
  delay(10);
  digitalWrite(TRIG_B_PIN, HIGH);
  delay(10);
  digitalWrite(TRIG_B_PIN, LOW);

  // Measure the duration of the echo pulse
  int duration = pulseIn(ECHO_B_PIN, HIGH);
  // Calculate the distance in centimeters
  distance = duration * 0.034 / 2 + 1;

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");
}

// Function to control LED indicators based on the distance from ultrasonic sensor B
void output_distance(){
  // Determine the LED color based on the measured distance
  if(distance > 40) {
    Serial.println("Safe");
    digitalWrite(Green, HIGH); // Green LED indicates safe distance
  }
  else if(distance > 20 && distance < 40) {
    Serial.println("Caution");
    digitalWrite(Yellow, HIGH); // Yellow LED indicates caution distance
  }
  else if(distance < 20) {
    Serial.println("Stop");
    digitalWrite(Red, HIGH); // Red LED indicates stop distance
  }
}

// Function to handle Telegram bot commands and interactions
void command(){
  TBMessage msg;

  // If the measured distance is less than 20 cm
  if(distance < 20) {
    tone(buzzer, 300); // Produce a tone using the buzzer
    mybot.sendMessage(bot_id, "Stop, distance is less than 20 cm!");
    
    // Check for new messages from the bot
    if(mybot.getNewMessage(msg)) {
      Serial.println("Command Entered: " + msg.text);
      String command = msg.text;
      
      // Process specific commands received from the bot
      if(command == "On") {
        mybot.sendMessage(bot_id, "Alarm is enabled");
      }
      if(command == "Off") {
        noTone(buzzer);
        mybot.sendMessage(bot_id, "Alarm disabled");
      }
    }
  }
  else {
    noTone(buzzer); // Turn off the buzzer if distance is safe
  }
}

// Blynk function to update app widgets and LED indicators
void kirim_blynk(){
  // Update Blynk app widgets with the distance values and LED states
  Blynk.virtualWrite(V0, distance);
  if(distance > 40) {
    Blynk.virtualWrite(V1, 1);
    Blynk.virtualWrite(V2, 0);
    Blynk.virtualWrite(V3, 0);
  }
  else if(distance >= 20 && distance <= 40) {
    Blynk.virtualWrite(V1, 0);
    Blynk.virtualWrite(V2, 1);
    Blynk.virtualWrite(V3, 0);
  }
  else if(distance < 20) {
    Blynk.virtualWrite(V1, 0);
    Blynk.virtualWrite(V2, 0);
    Blynk.virtualWrite(V3, 1);
  }
  // Update the Blynk app with the 'pinValue2' value
  Blynk.virtualWrite(V4, pinValue2);
}

// Blynk function to update app widget and control servo position
void servo_blynk(){
  // Update Blynk app with the distance value for servo control
  Blynk.virtualWrite(V5, distance_cm);
  
  // Control the servo position based on the distance value
  if(distance_cm > 80) {
  }
  else if(distance_cm < 80) {
  }
}

BLYNK_WRITE(V6) 
{
  int pinValue = param.asInt();
  if (pinValue == 1) {    // if Button sends 1
    servo.write(0);
    servo_blynk();             // start the function cradle
    Blynk.run(); // Run rest of show in-between waiting for this loop to repeat or quit.
    int pinValue = 0;  // Set V0 status to 0 to quit, unless button is still pushed (as per below)
    Blynk.syncVirtual(V6); // ...Then force BLYNK_WRITE(V6) function check of button status to determine if repeating or done.
  }
  else{
    servo.write(90);
  }
}

BLYNK_WRITE(V4) 
{
  int pinValue2 = param.asInt();
  if (pinValue2 == 1) {    // if Button sends 1
    tone(buzzer, 300);
    kirim_blynk();             // start the function cradle
    Blynk.run(); // Run rest of show in-between waiting for this loop to repeat or quit.
    int pinValue2 = 0;  // Set V0 status to 0 to quit, unless button is still pushed (as per below)
    Blynk.syncVirtual(V4); // ...Then force BLYNK_WRITE(V4) function check of button status to determine if repeating or done.
  }
  else{
    noTone(buzzer);
  }
}

// Setup function executed once on startup
void setup() {
  Serial.begin(115200); // Initialize serial communication
  Blynk.begin(auth, ssid, pass); // Initialize Blynk with authentication and WiFi info
  pinMode(TRIG_B_PIN, OUTPUT); // Set ultrasonic sensor B trigger pin as output
  pinMode(ECHO_B_PIN, INPUT);// Set ultrasonic sensor B echo pin as input
  pinMode(Red, OUTPUT); // Set LED pins as outputs
  pinMode(Yellow, OUTPUT);
  pinMode(Green, OUTPUT);
  pinMode(buzzer, OUTPUT); // Set buzzer pin as output
  timer.setInterval(2000L, kirim_blynk); 
  timer.setInterval(2000L, servo_blynk);

  Serial.println("Start Telegram Bot");
  mybot.wifiConnect(ssid, pass); // Connect to WiFi for Telegram bot
  mybot.setTelegramToken(token); // Set Telegram bot token

  if(mybot.testConnection()) {
    Serial.println("Connection Successful");
  }
  else {
    Serial.println("Connection Failed");
  }

  pinMode(TRIG_A_PIN, OUTPUT); // Set ultrasonic sensor A trigger pin as output
  pinMode(ECHO_A_PIN, INPUT);  // Set ultrasonic sensor A echo pin as output
  servo.attach(SERVO_PIN);   // Attach servo to the specified pin
  servo.write(0); // Initialize servo position
}

// Main loop function
void loop() {
  digitalWrite(Red, LOW);
  digitalWrite(Yellow, LOW);
  digitalWrite(Green, LOW);

  cal_distance(); // Measure distance using ultrasonic sensor B

  output_distance(); // Control LEDs based on distance

  command(); // Handle Telegram bot commands and interactions

  Blynk.run(); // Run Blynk communication
  timer.run(); // Run Blynk timers for periodic tasks
  delay(1000);

  // Measure distance using ultrasonic sensor A
  digitalWrite(TRIG_A_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_A_PIN, LOW);

  // Measure duration of pulse from ECHO pin
  duration_us = pulseIn(ECHO_A_PIN, HIGH);
  // Calculate the distance
  distance_cm = 0.017 * duration_us;

  // Control the servo based on distance value from ultrasonic sensor A
  if (distance_cm < DISTANCE_THRESHOLD)
    servo.write(0); // rotate servo motor to 90 degree
  else
    servo.write(90);  // rotate servo motor to 0 degree

  // print the value to Serial Monitor
  Serial.print("Distance: ");
  Serial.print(distance_cm);
  Serial.println(" cm");

  delay(500);
}