#include <BluetoothSerial.h>    // For Bluetooth Classic
#include <Wire.h>               // For I2C communication (MLX90614)
#include <Adafruit_MLX90614.h>  // MLX90614 library

// Pin Definitions
// L298N Motor Driver Pins
#define MOTOR_A_EN 16   // Enable Pin for Motor A
#define MOTOR_A_IN1 17  // Input 1 for Motor A
#define MOTOR_A_IN2 5  // Input 2 for Motor A
#define MOTOR_B_EN 23   // Enable Pin for Motor B
#define MOTOR_B_IN1 18  // Input 1 for Motor B
#define MOTOR_B_IN2 19  // Input 2 for Motor B

// Ultrasonic Sensors place at front and back
#define FRONT_TRIG 15   // Front Trigger Pin
#define FRONT_ECHO 2  // Front Echo Pin
#define BACK_TRIG 0   // Back Trigger Pin
#define BACK_ECHO 4   // Back Echo Pin

const int safeDistance = 20; // Safe distance value in cm 

// IR Flame Sensor
#define FLAME_SENSOR 14  // Digital pin for IR flame sensor
bool irSensorLogic = LOW; // Set the sensor logic to detect fire
// Relay
#define RELAY_PIN 12  // Pin to control relay

// MLX90614 Temperature Sensor (uses I2C: SDA = 21, SCL = 22 by default on ESP32)
Adafruit_MLX90614 mlx = Adafruit_MLX90614();

// Bluetooth
BluetoothSerial SerialBT;

// Variables
bool isManualMode = true;           // Tracks current mode
float frontDistance, backDistance;  // Distances from ultrasonic sensors
bool fireDetected = false;          // Fire detection flag
const float tempThreshold = 50.0;   // Temperature threshold (Celsius)
const int flameThreshold = 1000;    // IR sensor analog threshold (adjust based on testing)

// Setup Function
void setup() {
  // Initialize Serial for debugging
  Serial.begin(115200);

  // Initialize Bluetooth
  // SerialBT.begin("FireFighterRobot");  // Name of the Bluetooth device
  Serial.println("Bluetooth started. Pair with 'FireFighterRobot'.");

  // Initialize Motor Pins
  pinMode(MOTOR_A_EN, OUTPUT);
  pinMode(MOTOR_A_IN1, OUTPUT);
  pinMode(MOTOR_A_IN2, OUTPUT);
  pinMode(MOTOR_B_EN, OUTPUT);
  pinMode(MOTOR_B_IN1, OUTPUT);
  pinMode(MOTOR_B_IN2, OUTPUT);

  // Initialize Ultrasonic Sensor Pins
  pinMode(FRONT_TRIG, OUTPUT);
  pinMode(FRONT_ECHO, INPUT);
  pinMode(BACK_TRIG, OUTPUT);
  pinMode(BACK_ECHO, INPUT);

  // Initialize Flame Sensor Pin
  pinMode(FLAME_SENSOR, INPUT);

  // Initialize Relay Pin
  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, LOW);  // Relay off initially

  // Initialize MLX90614 Sensor
  // if (!mlx.begin()) {
  //   Serial.println("Error: Could not initialize MLX90614. Check wiring!");
  //   // while (1)
  //   //   ;  // Halt if sensor fails
  // }
  Serial.println("Setup complete.");
}

// Main Loop
void loop() {
  // Read sensor data
  frontDistance = readUltrasonic(FRONT_TRIG, FRONT_ECHO);
  backDistance = readUltrasonic(BACK_TRIG, BACK_ECHO);
  Serial.print("Front Distance:");
  Serial.print(frontDistance);
  Serial.print(", Back Distance:");
  Serial.println(backDistance);
  
  bool flameValue = digitalRead(FLAME_SENSOR);
  // float objectTemp = mlx.readObjectTempC();

  // Check for fire (IR sensor high reading OR temperature above threshold)
  fireDetected = flameValue == irSensorLogic; //  || objectTemp > tempThreshold);
  // Serial.println("Fire Detected:");
  // Serial.println(fireDetected);

  if (isManualMode) {
    // Manual Mode: Handle Bluetooth commands
    if (Serial.available()) {
      char command = Serial.read();
      Serial.print("Command Received:");
      Serial.println(command);
      handleMovement(command);
    }

    // If fire detected, switch to automatic mode
    if (fireDetected) {
      isManualMode = false;
      stopMotors();
      activateRelay();
      Serial.println("Fire detected! Switching to automatic mode.");
    }
  } else {
    // Automatic Mode: Fight the fire
    if (!fireDetected) {
      // Fire extinguished, return to manual mode
      deactivateRelay();
      stopMotors();
      isManualMode = true;
      Serial.println("Fire extinguished. Returning to manual mode.");
    }
    // Remain stopped and keep relay active while fire is present
  }

  delay(100);  // Small delay to prevent overwhelming the loop
}

// Motor Control Functions
void moveForward() {
  if (frontDistance > safeDistance) {  // Safe distance (adjust as needed)
    digitalWrite(MOTOR_A_EN, HIGH);  // Full speed
    digitalWrite(MOTOR_B_EN, HIGH);
    digitalWrite(MOTOR_A_IN1, HIGH);
    digitalWrite(MOTOR_A_IN2, LOW);
    digitalWrite(MOTOR_B_IN1, HIGH);
    digitalWrite(MOTOR_B_IN2, LOW);
  } else {
    stopMotors();
    Serial.println("Obstacle ahead! Stopping.");
  }
}

void moveBackward() {
  if (backDistance > safeDistance) {  // Safe distance (adjust as needed)
    digitalWrite(MOTOR_A_EN, HIGH);  // Full speed
    digitalWrite(MOTOR_B_EN, HIGH);
    digitalWrite(MOTOR_A_IN1, LOW);
    digitalWrite(MOTOR_A_IN2, HIGH);
    digitalWrite(MOTOR_B_IN1, LOW);
    digitalWrite(MOTOR_B_IN2, HIGH);
  } else {
    stopMotors();
    Serial.println("Obstacle behind! Stopping.");
  }
}

void turnLeft() {
  // No obstacle check for turning (adjust if needed)
    digitalWrite(MOTOR_A_EN, HIGH);  // Full speed
    digitalWrite(MOTOR_B_EN, HIGH);
  digitalWrite(MOTOR_A_IN1, LOW);
  digitalWrite(MOTOR_A_IN2, HIGH);
  digitalWrite(MOTOR_B_IN1, HIGH);
  digitalWrite(MOTOR_B_IN2, LOW);
}

void turnRight() {
  // No obstacle check for turning (adjust if needed)
    digitalWrite(MOTOR_A_EN, HIGH);  // Full speed
    digitalWrite(MOTOR_B_EN, HIGH);
  digitalWrite(MOTOR_A_IN1, HIGH);
  digitalWrite(MOTOR_A_IN2, LOW);
  digitalWrite(MOTOR_B_IN1, LOW);
  digitalWrite(MOTOR_B_IN2, HIGH);
}

void stopMotors() {
    digitalWrite(MOTOR_A_EN, LOW);  // Full speed
    digitalWrite(MOTOR_B_EN, LOW);
  digitalWrite(MOTOR_A_IN1, LOW);
  digitalWrite(MOTOR_A_IN2, LOW);
  digitalWrite(MOTOR_B_IN1, LOW);
  digitalWrite(MOTOR_B_IN2, LOW);
}

// Ultrasonic Distance Measurement
float readUltrasonic(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 30000);  // Timeout after 30ms
  if (duration == 0) return 400;                  // Max distance if no echo
  float distance = duration * 0.034 / 2;          // Distance in cm
  return distance;
}

// Relay Control Functions
void activateRelay() {
  digitalWrite(RELAY_PIN, HIGH);  // Turn on sprinkler
  Serial.println("Sprinkler ON.");
}

void deactivateRelay() {
  digitalWrite(RELAY_PIN, LOW);  // Turn off sprinkler
  Serial.println("Sprinkler OFF.");
}

// Handle Bluetooth Movement Commands
void handleMovement(char command) {
  switch (command) {
    case 'F':  // Forward
      moveForward();
      break;
    case 'B':  // Backward
      moveBackward();
      break;
    case 'L':  // Left
      turnLeft();
      break;
    case 'R':  // Right
      turnRight();
      break;
    case 'S':  // Stop
      stopMotors();
      break;
    default:
      stopMotors();
      Serial.println("Unknown command.");
      break;
  }
}