#include "TinyGPSPlus.h"
#include <PID_v1_bc.h> // Include the PID library for controlling the motor speed based on the angle
#include <MPU6050.h> // Include the MPU6050 library
#include <WiFiEsp.h>
#include <PubSubClient.h>
#include <Wire.h>
#include <MPU6050.h>

// Pin Definitions for Stepper Motors
const int stepPin1 = 3;
const int dirPin1 = 2;
const int stepPin2 = 5;
const int dirPin2 = 4;

// Pin Definitions for Ultrasonic Sensor
const int trigPin = 6; // Trig pin for HC-SR04
const int echoPin = 7; // Echo pin for HC-SR04

// Pin Definitions for Gas Sensor, Buzzer, and PIR Sensor
const int gasSensorPin = A0; // MQ-2 analog pin
const int buzzerPin = 8; // Buzzer connected to pin 8
const int pirPin = 10; // PIR sensor pin
const int ledPin = 11; // LED pin for motion indication

// Wi-Fi and MQTT Configuration
const char* ssid = "Orange-44C8";           // Replace with your Wi-Fi SSID
const char* password = "N9F151Q93BT";   // Replace with your Wi-Fi password
const char* mqttServer = "192.168.1.123"; // Replace with your MQTT broker address
const int mqttPort = 1883;                // Replace with your MQTT broker port
const char* mqttUser = "";                // Leave empty if no authentication
const char* mqttPassword = "";            // Leave empty if no authentication

WiFiEspClient espClient;
PubSubClient client(espClient);

// GPS Setup
TinyGPSPlus gps; // Create TinyGPSPlus object

// PID variables
double Setpoint, Input, Output;
double Kp = 2, Ki = 5, Kd = 1; // PID constants (tune these values)
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); // Declare the PID object

// Variables for PIR Sensor
int pirState = LOW; // Start assuming no motion detected
int val = 0; // PIR sensor value

// Autonomous and Remote Control Variables
bool autonomousMode = true; // Start in autonomous mode
bool stopRobot = false; // Robot stop flag

MPU6050 mpu; // Create an MPU6050 object

void setup() {
    Serial.begin(115200); // Communication with PC/Serial Monitor
    Serial1.begin(9600); // Communication with GPS module (TX: 18, RX: 19)

    // Stepper motor pin configuration
    pinMode(stepPin1, OUTPUT);
    pinMode(dirPin1, OUTPUT);
    pinMode(stepPin2, OUTPUT);
    pinMode(dirPin2, OUTPUT);

    // Ultrasonic sensor pin configuration
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);

    // Gas sensor, buzzer, and PIR pin configuration
    pinMode(gasSensorPin, INPUT);
    pinMode(buzzerPin, OUTPUT);
    pinMode(pirPin, INPUT);
    pinMode(ledPin, OUTPUT); 

    Serial.println(F("Robot Initialized")); 
    mpu.initialize(); 

    Setpoint = 20; 
    myPID.SetMode(AUTOMATIC); 
    myPID.SetOutputLimits(-255, 255); 

    connectToWiFi();
    client.setServer(mqttServer, mqttPort);
    client.setCallback(mqttCallback);
  // Initialize I2C communication
  Wire.begin();
  
  // Initialize MPU6050
  mpu.initialize();
  
  // Check if MPU6050 is connected properly
  if (mpu.testConnection()) {
    Serial.println("MPU6050 connected successfully.");
  } else {
    Serial.println("MPU6050 connection failed.");
  }
}

void loop() {
    if (!client.connected()) {
        connectToMQTT();
    }
    client.loop();

    if (stopRobot) {
        stopMotors();
        return;
    }

    if (autonomousMode) {
        performAutonomousNavigation();
    }
    // Read accelerometer and gyroscope data from MPU6050
    int16_t ax, ay, az;  // Declare accelerometer data
    int16_t gx, gy, gz;  // Declare gyroscope data

    mpu.getAcceleration(&ax, &ay, &az);  // Get accelerometer data
    mpu.getRotation(&gx, &gy, &gz);      // Get gyroscope data

    // Send MPU data to MQTT
   char mpuMessage[100];
   snprintf(mpuMessage, sizeof(mpuMessage), "Accel X: %d, Y: %d, Z: %d, Gyro X: %d, Y: %d, Z: %d", ax, ay, az, gx, gy, gz);
   client.publish("mpu", mpuMessage);
}

void connectToWiFi() {
    while (WiFi.status() != WL_CONNECTED) {
        Serial.print("Connecting to Wi-Fi...");
        WiFi.begin(ssid, password);
        delay(2000);
    }
    Serial.println("Connected to Wi-Fi.");
    Serial.print("IP address: ");
    Serial.println(WiFi.localIP());
}

void connectToMQTT() {
    while (!client.connected()) {
        Serial.print("Connecting to MQTT...");
        if (client.connect("RobotClient", mqttUser, mqttPassword)) {
            Serial.println("Connected to MQTT.");
            client.subscribe("robot/control");
        } else {
            Serial.print("Failed. RC=");
            Serial.println(client.state());
            delay(2000);
        }
    }
}

void mqttCallback(char* topic, byte* payload, unsigned int length) {
    String message;
    for (int i = 0; i < length; i++) {
        message += (char)payload[i];
    }

    if (String(topic) == "robot/control") {
        if (message == "stop") {
            stopRobot = true;
            autonomousMode = false;
            publishAlert("Robot stopped remotely.");
        } else if (message == "resume") {
            stopRobot = false;
            autonomousMode = true;
            publishAlert("Robot resumed autonomous navigation.");
        } else if (message == "manual") {
            autonomousMode = false;
            publishAlert("Robot switched to manual control.");
        } else if (message == "autonomous") {
            autonomousMode = true;
            publishAlert("Robot switched to autonomous mode.");
        }
    }
}

void performAutonomousNavigation() {
    while (Serial1.available() > 0) {
        if (gps.encode(Serial1.read())) {
            displayGPSInfo();
        }
    }

    int gasValue = analogRead(gasSensorPin);
    publishSensorData("gas", gasValue);

    if (gasValue > 400) {
        publishAlert("High gas level detected! Activating buzzer.");
        tone(buzzerPin, 2500, 1000); 
        delay(1000); 
        noTone(buzzerPin); 
        Serial.println("High gas level detected! Activating buzzer.");
    }

    val = digitalRead(pirPin);
    if (val == HIGH && pirState == LOW) {
        publishAlert("Motion detected!");
        pirState = HIGH;
        digitalWrite(ledPin, HIGH);
        Serial.println("Motion detected!");
    } else if (val == LOW && pirState == HIGH) {
        publishAlert("Motion ended!");
        pirState = LOW;
        digitalWrite(ledPin, LOW);
        Serial.println("Motion ended!");
    }

    float distance = measureDistance();
    publishSensorData("distance", distance);

    if (distance < 5) {
        publishAlert("Warning: Hole detected!");
        Serial.println("Warning: Hole detected! Attempting to avoid.");
        if (!avoidHole()) {
            publishAlert("Unable to find clear path around hole.");
            stopMotors();
            return;
        }
        delay(1000);
        return;
    }

    if (distance > 15 && distance < 30) {
        publishAlert("Large object detected! Attempting to avoid.");
        Serial.println("Large object detected! Attempting to avoid.");
        if (!avoidLargeObject()) {
            publishAlert("Unable to find clear path around large object.");
            stopMotors();
            return;
        }
        delay(1000);
        return;
    }

    adjustPositionToWall(distance);
    delay(1000);
}

void publishSensorData(const char* topic, float value) {
    char message[50];
    snprintf(message, sizeof(message), "Value: %.2f", value);
    client.publish(topic, message);
    Serial.print("Published "); 
    Serial.print(topic); 
    Serial.print(": "); 
    Serial.println(message); 
}

void publishAlert(const char* alert) {
    client.publish("alerts", alert);
    Serial.print("Alert: ");
    Serial.println(alert); 
}

float measureDistance() { 
    digitalWrite(trigPin, LOW); 
    delayMicroseconds(2); 
    digitalWrite(trigPin, HIGH); 
    delayMicroseconds(10); 
    digitalWrite(trigPin, LOW); 

    long duration = pulseIn(echoPin, HIGH);  
    float distance = (duration * 0.034) / 2;  
    return distance;  
}

void displayGPSInfo() {
    if (gps.location.isValid()) {
        char gpsMessage[100];
        snprintf(gpsMessage, sizeof(gpsMessage), "Lat: %.6f, Lng: %.6f", gps.location.lat(), gps.location.lng());
        client.publish("gps", gpsMessage);
        Serial.print("GPS Location: ");
        Serial.println(gpsMessage);
    } else {
        client.publish("gps", "INVALID GPS DATA");
        Serial.println("GPS Location: INVALID GPS DATA");
    }
}

void stopMotors() { 
    digitalWrite(stepPin1, LOW); 
    digitalWrite(stepPin2, LOW); 
}

bool avoidHole() { 
    const int turnDuration = 500; 
    const int maxAttempts = 3;

    for(int attempt = 1; attempt <= maxAttempts; attempt++) { 
        turnLeft(); 
        delay(turnDuration); 

        float newDistanceLeft = measureDistance(); 

        if(newDistanceLeft > Setpoint + 5) { 
            return true; 
        } 

        turnRight(); 
        delay(turnDuration); 

        float newDistanceRight = measureDistance(); 

        if(newDistanceRight > Setpoint + 5) { 
            return true; 
        } 

        stopMotors();  
        delay(turnDuration * attempt);  
    } 

    return false;  
}

bool avoidLargeObject() { 
    const int turnDuration = 500; 
    const int maxAttempts = 3;

    for(int attempt = 1; attempt <= maxAttempts; attempt++) { 
        turnLeft(); 
        delay(turnDuration); 

        float newDistanceLeft = measureDistance(); 

        if(newDistanceLeft > Setpoint + 5) { 
            return true; 
        } 

        turnRight(); 
        delay(turnDuration); 

        float newDistanceRight = measureDistance(); 

        if(newDistanceRight > Setpoint + 5) { 
            return true;  
        } 

        stopMotors();  
        delay(turnDuration * attempt);  
    }  

    return false;  
}

void adjustPositionToWall(float distance) {
    if (distance < Setpoint - 5) {
        publishAlert("Too close to wall! Adjusting position.");
        Serial.println("Too close to wall! Adjusting position.");
        turnAwayFromWall();
        delay(1000);
    } else if (distance > Setpoint + 5) {
        publishAlert("Too far from wall! Adjusting towards it.");
        Serial.println("Too far from wall! Adjusting towards it.");
        turnTowardsWall();
        delay(1000);
    }
}

// Function to Turn Left when Avoiding Hole or Large Object 
void turnLeft() { 
   digitalWrite(dirPin1, LOW); 
   digitalWrite(dirPin2, HIGH); 

   for(int i=0;i<200;i++) { 
      digitalWrite(stepPin1,HIGH); 
      digitalWrite(stepPin2,HIGH); 
      delayMicroseconds(500); 
      digitalWrite(stepPin1,LOW); 
      digitalWrite(stepPin2,LOW); 
      delayMicroseconds(500);   
   } 

stopMotors();  
}

// Function to Turn Right when Avoiding Hole or Large Object 
void turnRight() { 
     digitalWrite(dirPin1,HIGH); 
     digitalWrite(dirPin2,LOW);

     for(int i=0;i<200;i++) {   
         digitalWrite(stepPin1,HIGH);
         digitalWrite(stepPin2,HIGH);
         delayMicroseconds(500);
         digitalWrite(stepPin1,LOW);
         digitalWrite(stepPin2,LOW);
         delayMicroseconds(500);
     }

     stopMotors();   
}

// Placeholder functions for turning away from walls.
void turnAwayFromWall() {
     digitalWrite(dirPin1,HIGH);
     digitalWrite(dirPin2,HIGH);

     for(int i=0;i<200;i++) {   
         digitalWrite(stepPin1,HIGH);
         digitalWrite(stepPin2,HIGH);
         delayMicroseconds(500);
         digitalWrite(stepPin1,LOW);
         digitalWrite(stepPin2,LOW);
         delayMicroseconds(500);
     }

     stopMotors();   
}

void turnTowardsWall() {
     digitalWrite(dirPin1,LOW);
     digitalWrite(dirPin2,HIGH);

     for(int i=0;i<200;i++) {   
         digitalWrite(stepPin1,HIGH);
         digitalWrite(stepPin2,HIGH);
         delayMicroseconds(500);
         digitalWrite(stepPin1,LOW);
         digitalWrite(stepPin2,LOW);
         delayMicroseconds(500);
     }

     stopMotors();   
}