#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_MLX90614.h>

// Initialize MLX90614 temperature sensor
Adafruit_MLX90614 mlx = Adafruit_MLX90614();

// Motor driver pins
const int ENA = 9;
const int ENB = 10;
const int IN1 = 5;
const int IN2 = 6;
const int IN3 = 7;
const int IN4 = 8;

// Flame sensor pins
const int flameSensor1 = 2;
const int flameSensor2 = 3;
const int flameSensor3 = 4;

// Relay pin
// const int relayPin = 11;

// Thresholds
const float safeDistanceTemp = 35.0; // Stop moving closer if temperature exceeds this
const float confirmFireTemp = 30.0;  // Confirm fire if temperature exceeds this

// Function prototypes
void moveForward();
void stopMotors();
void rotateLeft();
void rotateRight();
void extinguishFire();

void setup()
{
  // Motor pins
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // Flame sensors and relay
  pinMode(flameSensor1, INPUT);
  pinMode(flameSensor2, INPUT);
  pinMode(flameSensor3, INPUT);
  // pinMode(relayPin, OUTPUT);

  // Start MLX90614
  mlx.begin();

  Serial.begin(9600);
  Serial.println("Flame Extinguisher Robot Initialized");
}

void loop()
{
  // Read flame sensors
  bool fireLeft = digitalRead(flameSensor1); // bool fireLeft = digitalRead(flameSensor1);
  bool fireCenter = digitalRead(flameSensor2);
  bool fireRight = digitalRead(flameSensor3); // bool fireRight = digitalRead(flameSensor3);

  // Read temperature
  float temp = mlx.readObjectTempC();
  Serial.print("Temperature: ");
  Serial.println(temp);

  // Check for fire direction
  if (!fireCenter)
  {
    Serial.println("Fire detected ahead!");
    // moveForward();
  }
  else if (!fireLeft)
  {
    Serial.println("Fire detected on the left!");
    rotateLeft();
  }
  else if (!fireRight)
  {
    Serial.println("Fire detected on the right!");
    rotateRight();
  }
  else
  {
    Serial.println("No fire detected.");
    stopMotors();
  }

  if (temp > safeDistanceTemp)
  {
    Serial.println("Too close to fire! Stopping and extinguishing...");
    stopMotors();
    // extinguishFire();
  }
  else if (temp > confirmFireTemp)
  {
    // Fire confirmed; move closer cautiously
    Serial.println("Fire confirmed, moving forward cautiously...");
    moveForward();
  }
  else
  {
    Serial.println("Fire not intense enough, no action taken.");
    stopMotors();
  }

  delay(200); // Short delay for stability
}

// Move robot forward
void moveForward()
{
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, 70); // Moderate speed
  analogWrite(ENB, 70);
}

// Stop motors
void stopMotors()
{
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);

  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

// Rotate left
void rotateLeft()
{
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);

  analogWrite(ENA, 70); // Reduced speed for smoother turn
  analogWrite(ENB, 70); // Reduced speed for smoother turn
}

// Rotate right
void rotateRight()
{
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);

  analogWrite(ENA, 70); // Reduced speed for smoother turn
  analogWrite(ENB, 70); // Reduced speed for smoother turn
}

// Extinguish fire
// void extinguishFire()
// {
//   digitalWrite(relayPin, HIGH); // Turn on pump
//   delay(3000);                  // Pump for 5 seconds
//   digitalWrite(relayPin, LOW);  // Turn off pump
// }