/**********************************************************************

  Author: Pradeep Yogesh Yadav
  Subject: Electronics and Electrical
  Project: Autonomous Obstacle-Avoiding Robot with Telematics Data 
  (Using Wokwi and Blynk for telematics).

********************************************************************/

//[01] Libraries 

// Blynk Credentials important before include
/* Fill-in information from Blynk Device Info here */
#define BLYNK_TEMPLATE_ID "TMPL3ZkIKcEEa"
#define BLYNK_TEMPLATE_NAME "ESP32"
#define BLYNK_AUTH_TOKEN "Q7o73IDdOkqJqg8ALD8BqQqpPwlakoCl"

#include <WiFi.h> 
#include <WiFiClient.h>
#include <HTTPClient.h>
#include <NTPClient.h>
#include <WiFiUdp.h>
#include <BlynkSimpleEsp32.h>
#include <Wire.h>
#include <DHT.h>                // temp & humidity sensor support library
#include <AccelStepper.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPU6050.h>

//[02] Definations

/* Comment this out to disable prints and save space */
#define BLYNK_PRINT Serial
BlynkTimer timer;

// Your WiFi credentials.
// Set password to "" for open networks.(for wokwi simulator)
char ssid[] = "Wokwi-GUEST";
char pass[] = "";

// Google Apps Script Web App URL
const char* serverName = "https://script.google.com/macros/s/AKfycbyyKv_rAoZzgaPTXDYwVjB-tUhpngKM2rN4nk6-KUKsdzAek8mtJy4ZuPdN__tDB1I2/exec";

Adafruit_SSD1306 display(128,64,&Wire,-1);

Adafruit_MPU6050 mpu;
// Accelerometer variables
float accelX = 0.0, accelY = 0.0, accelZ = 0.0;

#define DHTPIN 25         //Connect Out pin to D2 in NODE MCU
#define DHTTYPE DHT22       // Sensor type
DHT dht(DHTPIN, DHTTYPE);
float t =0;
int h =0;
int hm = 0;
int encounter;
bool robot_status = 0;
int log_count = 0;

float frontDist = 0;
float leftDist = 0;
float rightDist = 0;

// Global variable to store the last displayed message
String lastMessage = "";

//  RGB pin define
#define led1 2
#define led2 4
#define led3 5

//  Buzzer pin
#define buzz 12

//  Pir sensor pin
#define pir 23

// Ultrasonic Sensor Pins
#define LEFT_ECHO_PIN 26
#define LEFT_TRIG_PIN 27
#define FRONT_ECHO_PIN 32
#define FRONT_TRIG_PIN 33
#define RIGHT_ECHO_PIN 34
#define RIGHT_TRIG_PIN 16

// Stepper Motor Pins and Config
#define MOTOR_A1_PIN 17
#define MOTOR_A2_PIN 18
#define MOTOR_B1_PIN 19
#define MOTOR_B2_PIN 14

AccelStepper leftMotor(AccelStepper::FULL2WIRE, MOTOR_A1_PIN, MOTOR_A2_PIN);
AccelStepper rightMotor(AccelStepper::FULL2WIRE, MOTOR_B1_PIN, MOTOR_B2_PIN);


// Define step parameters
const int stepsPerRevolution = 200;  // Number of steps per full revolution
const float wheelCircumference = 30.0;  // Wheel circumference in cm

// State enumeration
enum State {
  STOP,
  MOVE_FORWARD,
  TURN_LEFT,
  TURN_RIGHT
};

State currentState = STOP;  // Initially, the robot is stopped

// Function to set the state
void setState(State newState) {
  if (currentState != newState) { // Only update if the state changes
    currentState = newState;
    Serial.print("State changed to: ");
    switch (newState) {
      case STOP: Serial.println("STOP"); break;
      case MOVE_FORWARD: Serial.println("MOVE_FORWARD"); break;
      case TURN_LEFT: Serial.println("TURN_LEFT"); break;
      case TURN_RIGHT: Serial.println("TURN_RIGHT"); break;
    }
  }
}

// Globals
const int SAFE_DISTANCE = 40;     // Safe distance in cm
const int FORWARD_DISTANCE = SAFE_DISTANCE;//2; // Half of the safe distance for forward motion
bool buttonState = 1;
bool excel_sheet = 1;

// Function Prototypes
void initializeWiFi();
void initializeBlynk();
void initializeDisplay();
void updateDisplay(String message);
void initializeMPU();
void readAccelerometer();
void setupUltrasonicSensors();
long measureDistance(int trigPin, int echoPin);
void moveForward();
void turnLeft();
void turnRight();
void stopMotors();
void obstacleAvoidance();
void executeStateActions();
void excel();
void sendTelematicsData();
void deactivateAlert();
void activateAlert();

//[03]  Callback Functions

// This function is called every time the Virtual Pin 0 state changes
BLYNK_WRITE(V0)
{
  // Set incoming value from pin V0 to a variable
  int value = param.asInt();
  buttonState = value;
  if(buttonState == 1 ){
    excel_sheet = value;
  }
  // Update state on blynk and esp32 RGB led to red color
  Blynk.virtualWrite(V1, value);
}

// This function is called every time the device is connected to the Blynk.Cloud
BLYNK_CONNECTED()
{
  // Change Web Link Button message to "Congratulations!"
  Blynk.setProperty(V15, "offImageUrl", "https://static-image.nyc3.cdn.digitaloceanspaces.com/general/fte/congratulations.png");
  Blynk.setProperty(V15, "onImageUrl",  "https://static-image.nyc3.cdn.digitaloceanspaces.com/general/fte/congratulations_pressed.png");
  Blynk.setProperty(V15, "url", "https://docs.blynk.io/en/getting-started/what-do-i-need-to-blynk/how-quickstart-device-was-made");
}

// This function sends Arduino's uptime every second to Virtual Pin 2(V2).
void sendTelematicsData()
{
  // You can send any value at any time.
  // Please don't send more that 10 values per second.
  Blynk.virtualWrite(V2, millis() / 1000); 
  Blynk.virtualWrite(V4,t);             // temperature
  Blynk.virtualWrite(V5,h);             // humidity
  Blynk.virtualWrite(V6, leftDist);     // Left distance
  Blynk.virtualWrite(V7, frontDist);    // Front distance
  Blynk.virtualWrite(V8, rightDist);    // Right distance
  Blynk.virtualWrite(V11, hm);          // Human motion presence check
  Blynk.virtualWrite(V12, encounter);   // Number sec human encounters
  Blynk.virtualWrite(V13, robot_status);// robot_status for ON and working condition

  Serial.print("Temperature : ");Serial.print(t);Serial.print("    Humidity : ");Serial.println(h);
  Serial.print("Left: "); Serial.print(leftDist);
  Serial.print(" cm Front: "); Serial.print(frontDist);
  Serial.print(" cm, Right: "); Serial.print(rightDist);Serial.println(" cm");

  //send DHT22 sensor data to google sheets(excel sheets)
  if (excel_sheet==1){
    digitalWrite(led2,LOW);
    digitalWrite(led3,HIGH);
    digitalWrite(led1,LOW);
    excel();
    log_count += 1;
    if(buttonState == 1){
      excel_sheet=0;
      Blynk.virtualWrite(V1, log_count);
      Blynk.virtualWrite(V0, 0);
    }
    robot_status = 1;
    digitalWrite(led2,HIGH);
    digitalWrite(led3,LOW);
    digitalWrite(led1,LOW);
    Blynk.virtualWrite(V13, robot_status);
  }
}

// Initialize WiFi and NTP client
WiFiUDP ntpUDP;
NTPClient timeClient(ntpUDP, "pool.ntp.org", 19800, 60000); // Update every 60 seconds

void setup()
{
  // Debug console
  Serial.begin(115200);
  
  //  Connect to WiFi
  initializeWiFi();

  //  Connect to Blynk
  initializeBlynk();
  
  // Initialize NTP client
  timeClient.begin();
  timeClient.update();

  // init the ssd1306 display
  initializeDisplay();
  
  //  Initialize/begin Sensors and Actuator 
  dht.begin();
  initializeMPU();
  
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);
  pinMode(buzz, OUTPUT);
  pinMode(pir, INPUT);

  // Set the max speed and acceleration for smooth movement
  leftMotor.setMaxSpeed(1000);  // max speed for left motor
  rightMotor.setMaxSpeed(1000); // max speed for right motor
  leftMotor.setAcceleration(500);  // smooth acceleration
  rightMotor.setAcceleration(500); // smooth acceleration

  // Setup Ultrasonic Sensors
  setupUltrasonicSensors();

  // Initial Message
  Serial.println("Robot Initialized");
  
  updateDisplay(" Robot    ready!");

  // Setup a function to be called every second(1000L) reduce if required
  timer.setInterval(2000L, sendTelematicsData);
}

void loop()
{ 
  obstacleAvoidance();  // Handle Obstacle Avoidance
  Blynk.run();
  timer.run();
}

void setupUltrasonicSensors() {
  pinMode(FRONT_TRIG_PIN, OUTPUT);
  pinMode(FRONT_ECHO_PIN, INPUT);
  pinMode(LEFT_TRIG_PIN, OUTPUT);
  pinMode(LEFT_ECHO_PIN, INPUT);
  pinMode(RIGHT_TRIG_PIN, OUTPUT);
  pinMode(RIGHT_ECHO_PIN, INPUT);
}

long measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2; // Convert to cm Distance (cm) = (Duration of pulse in microseconds) * Speed of sound (cm/µs) / 2

}

void moveForward() {
  moveForwardDistance(FORWARD_DISTANCE);
  updateDisplay(" forward   motion!");
}

void turnLeft() {
  turnAngle(-90);
  updateDisplay(" Turn left!");
}

void turnRight() {
  turnAngle(90);
  updateDisplay(" Turn right!");
}

void stopMotors() {
  leftMotor.setSpeed(0); // stop left motor
  rightMotor.setSpeed(0); // stop right motor
  leftMotor.runSpeed(); // stop the motor
  rightMotor.runSpeed(); // stop the motor
  updateDisplay(" Turn left!");
}


// Function to move forward a specific distance
void moveForwardDistance(float distance) {
  float distancePerStep = wheelCircumference / stepsPerRevolution;  // cm per step
  int steps = distance / distancePerStep;  // Calculate how many steps to move
  leftMotor.move(steps);  
  rightMotor.move(steps);  
  while (leftMotor.isRunning() || rightMotor.isRunning()) {
    leftMotor.run();  
    rightMotor.run(); 
  }
}

// Function to turn the robot by a specific angle
void turnAngle(float angle) {
  int steps = (angle / 1.8) * stepsPerRevolution;  // Steps for turning
  leftMotor.move(steps);  
  rightMotor.move(-steps);  
  while (leftMotor.isRunning() || rightMotor.isRunning()) {
    leftMotor.run();  
    rightMotor.run(); 
  }
}

void obstacleAvoidance() {
  //Read Temperature and humidity of environment
  h = dht.readHumidity();
  t = dht.readTemperature(); // or dht.readTemperature(true) for Fahrenheit
  if (isnan(h) || isnan(t)) {
    Serial.println("Failed to read from DHT sensor!");
    return;
  }

  // Measure robot distance for nearest obstacle in front,left and right
  frontDist = measureDistance(FRONT_TRIG_PIN, FRONT_ECHO_PIN);
  leftDist = measureDistance(LEFT_TRIG_PIN, LEFT_ECHO_PIN);
  rightDist = measureDistance(RIGHT_TRIG_PIN, RIGHT_ECHO_PIN);

  // Human presence detection
  hm = digitalRead(pir);
  if (hm > 0) {
    updateDisplay("   Move   aside!");
    encounter++;
    activateAlert(); // Turn on LED and buzzer
    setState(STOP);  // Stop the robot if a human is detected
    executeStateActions();
    return;          // Stop motion when human detected
  } else {
    deactivateAlert(); // Turn off LED and buzzer
  }

  // Check if the robot is upright or fallen
  readAccelerometer();
  if (abs((accelZ/10) + 1) < 1.5) {  // Check if the Z-axis value is close to -1g
    robot_status = 1;
    Serial.println("Robot is upright.");
  } else {
    robot_status = 0;
    setState(STOP);
    executeStateActions();
    Serial.println("Robot has fallen.");
  }

  //make motion 
  if (robot_status and !hm ){
    
    // Obstacle avoidance logic
    if (frontDist > SAFE_DISTANCE && leftDist > SAFE_DISTANCE && rightDist > SAFE_DISTANCE) {
        // All directions are clear
        setState(MOVE_FORWARD);
    } else if (frontDist > SAFE_DISTANCE) {
        // Forward is clear, move forward regardless of side distances
        setState(MOVE_FORWARD);
    } else if (frontDist <= SAFE_DISTANCE) {// Step 1: Check if front is blocked
        // Obstacle ahead, decide to stop and turn

        setState(STOP);
        executeStateActions();

        // Step 2: Determine initial turn direction
        // Weighted logic for turning
        if (leftDist > rightDist && leftDist > SAFE_DISTANCE) {
            setState(TURN_LEFT); // Turn left if left side is significantly clearer
        } else if (rightDist > leftDist && rightDist > SAFE_DISTANCE) {
            setState(TURN_RIGHT); // Turn right if right side is significantly clearer
        } else if (leftDist <= SAFE_DISTANCE && rightDist <= SAFE_DISTANCE) {
            // No clear path on either side; prioritize one (e.g., right as default)
            setState(TURN_RIGHT);
        } else {
            // If both sides are blocked, turn around (180°)
            turnAngle(180);
            setState(STOP);
            // Execute actions based on current state
            executeStateActions();
            return; // Exit the function to avoid recursion
        }
      
        executeStateActions();

        // Step 3: Move forward by half of safe distance
        setState(MOVE_FORWARD);
        executeStateActions();

        // Step 4: Turn back to face the original obstacle direction
        if (rightDist > leftDist) {
          setState(TURN_RIGHT);
        } else {
          setState(TURN_LEFT);
        }
        executeStateActions();

        // Step 5: Move forward by half of safe distance again
        if (frontDist > SAFE_DISTANCE) {
          setState(MOVE_FORWARD);
        }
        executeStateActions();

        // Step 6: Check right and adjust
        if (rightDist > SAFE_DISTANCE) {
          setState(TURN_RIGHT);
        }
        executeStateActions();

        // Step 7: Move forward if path is clear
        if (frontDist > SAFE_DISTANCE) {
          setState(MOVE_FORWARD);
        }
        executeStateActions();

        // Step 8: Check left and adjust
        if (leftDist > SAFE_DISTANCE) {
          setState(TURN_LEFT);
        }
        executeStateActions();

        // At this point, the robot should have completed a "C" curve,
        // avoided the obstacle, and is ready to resume forward motion.
    } else {
        // Backup or recovery logic (if needed, e.g., if stuck or cornered)
        setState(STOP);
        turnAngle(180);
    }
    // Execute actions based on current state
    executeStateActions();
  }
}

void executeStateActions(){
  // Check the current state and update motor actions accordingly
  switch (currentState) {
    case STOP:
      stopMotors();
      Blynk.virtualWrite(V9, 0);    // leftMotor
      Blynk.virtualWrite(V10, 0);  // right motor
      break;
    case MOVE_FORWARD:
      moveForward();
      Blynk.virtualWrite(V9, 1);    // leftMotor
      Blynk.virtualWrite(V10, 1);  // right motor
      break;
    case TURN_LEFT:
      turnLeft();
      Blynk.virtualWrite(V9, 0);    // leftMotor
      Blynk.virtualWrite(V10, 1);  // right motor
      break;
    case TURN_RIGHT:
      turnRight();
      Blynk.virtualWrite(V9, 1);    // leftMotor
      Blynk.virtualWrite(V10, 0);  // right motor
      break;
  }
}

void excel(){
  if (WiFi.status() == WL_CONNECTED) { // Check if ESP32 is connected to WiFi
    HTTPClient http;
    http.begin(serverName);
    http.addHeader("Content-Type", "application/json");
    // Fetch time from NTP client
    timeClient.update();
    String formattedTime = timeClient.getFormattedTime();
    // Prepare JSON payload with NTP time, sensor data, and button state
    String jsonData = "{\"method\":\"append\", \"temperature\":" + String(t) +
                      ", \"humidity\":" + String(h) +
                      ", \"timestamp\":\"" + formattedTime + "\"" +
                      ", \"buttonState\":" + String(buttonState ? "true" : "false") +
                      ", \"accelX\":" + String(accelX) +
                      ", \"accelY\":" + String(accelY) +
                      ", \"accelZ\":" + String(accelZ) + "}";
  
    Serial.println(jsonData);

    // Send HTTP POST request
    int httpResponseCode = http.POST(jsonData);
    if (httpResponseCode > 0) {
      String response = http.getString();
      Serial.println(httpResponseCode);
     // Serial.println(response);
    } else {
      Serial.println("Error on sending POST: " + String(httpResponseCode));
    }
    http.end(); // Close connection
  } else {
    Serial.println("WiFi Disconnected");
  }
}

// Initialize WiFi Connection
void initializeWiFi() {
  WiFi.begin("Wokwi-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
  }
  Serial.println("\nWiFi Connected. \nIP Address: " + WiFi.localIP().toString());
}

// Initialize Blynk
void initializeBlynk() {
  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);
}

// Initialize OLED Display
void initializeDisplay() {
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("SSD1306 Initialization Failed!");
    while (true);
  }
  updateDisplay("Display Ready");
}

// Update Display with a Message
void updateDisplay(String message) {
  if (message != lastMessage) {
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 10);
  display.println(message);
  display.display();
  // Update the last displayed message to the new message
  lastMessage = message;
  }
}

// Initialize MPU6050
void initializeMPU() {
  if (!mpu.begin()) {
    Serial.println("MPU6050 Initialization Failed!");
    updateDisplay("MPU Error");
    while(1){delay(10);}
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  updateDisplay(" MPU6050   ready!");
  Serial.println("MPU6050 Initialized.");
}

// Read Accelerometer Data
void readAccelerometer() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Store accelerometer data in global variables
  accelX = a.acceleration.x;
  accelY = a.acceleration.y;
  accelZ = a.acceleration.z;
}


// Function to activate alert (LED and buzzer)
void activateAlert() {
  digitalWrite(led1, HIGH);
  digitalWrite(led2, LOW);
  digitalWrite(led3, LOW);
  digitalWrite(buzz, HIGH);
  Serial.println(hm > 0 ? "human_present: yes" : "human_present: no");
}

// Function to deactivate alert
void deactivateAlert() {
  digitalWrite(led1, LOW);
  digitalWrite(led2, HIGH);
  digitalWrite(led3, LOW);
  digitalWrite(buzz, LOW);
}