/*
  Circuit Atlas reference sketch
  Project 301 - Arduino Based Autonomous Fire Fighting Robot

  Target board: Arduino Uno R3
  Required library: Servo (installable from Arduino Library Manager)

  IMPORTANT ELECTRICAL SAFETY
  - Never power the water pump, drive motors, or servos from an Arduino I/O pin.
  - Use an L293D (or equivalent driver) for the two drive motors.
  - Use a logic-level MOSFET or a rated relay module for the 12 V pump.
  - Fit a flyback diode across the pump when using a discrete MOSFET driver.
  - Use suitable external supplies and connect every supply ground together.
  - Keep water, tubing, and leaks physically separated from all electronics.

  The source document contains a block diagram but no pin assignments. The
  pin map below is a tested-for-conflicts reference layout; update it to match
  your build before applying motor or pump power.
*/

#include <Servo.h>

// L293D motor driver. Servo disables PWM on Uno pins 9 and 10, so the motor
// enable pins deliberately use PWM pins 5 and 6.
constexpr uint8_t LEFT_MOTOR_IN1 = 2;
constexpr uint8_t LEFT_MOTOR_IN2 = 3;
constexpr uint8_t RIGHT_MOTOR_IN1 = 4;
constexpr uint8_t RIGHT_MOTOR_IN2 = 7;
constexpr uint8_t LEFT_MOTOR_ENABLE = 5;
constexpr uint8_t RIGHT_MOTOR_ENABLE = 6;

// HC-SR04 obstacle sensor, mounted on the scan servo.
constexpr uint8_t ULTRASONIC_TRIG = 8;
constexpr uint8_t ULTRASONIC_ECHO = 9;
constexpr uint8_t SCAN_SERVO_PIN = 10;

// Fire suppression hardware.
constexpr uint8_t NOZZLE_SERVO_PIN = 11;
constexpr uint8_t PUMP_DRIVER_PIN = 12;
constexpr uint8_t BUZZER_PIN = 13;
constexpr uint8_t FLAME_SENSOR_PIN = A0;

// Adjust these values during the dry bench test.
constexpr bool FLAME_READING_DECREASES_NEAR_FIRE = true;
constexpr int FLAME_THRESHOLD = 430;       // Uno ADC range: 0 to 1023.
constexpr int OBSTACLE_DISTANCE_CM = 25;
constexpr uint8_t DRIVE_SPEED = 175;       // PWM range: 0 to 255.
constexpr uint8_t TURN_SPEED = 165;
constexpr unsigned long MAX_SPRAY_TIME_MS = 8000;
constexpr unsigned long FIRE_CLEAR_CONFIRM_MS = 1200;

constexpr int SCAN_CENTER_DEG = 90;
constexpr int SCAN_LEFT_DEG = 150;
constexpr int SCAN_RIGHT_DEG = 30;
constexpr int NOZZLE_MIN_DEG = 55;
constexpr int NOZZLE_MAX_DEG = 125;
constexpr int NOZZLE_CENTER_DEG = 90;

Servo scanServo;
Servo nozzleServo;

enum class RobotMode : uint8_t {
  Patrol,
  Extinguishing
};

RobotMode mode = RobotMode::Patrol;
unsigned long lastDiagnosticMs = 0;

void setMotor(uint8_t in1, uint8_t in2, uint8_t enablePin, int speedValue) {
  const int limitedSpeed = constrain(abs(speedValue), 0, 255);

  if (speedValue > 0) {
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
  } else if (speedValue < 0) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
  } else {
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
  }

  analogWrite(enablePin, limitedSpeed);
}

void drive(int leftSpeed, int rightSpeed) {
  setMotor(LEFT_MOTOR_IN1, LEFT_MOTOR_IN2, LEFT_MOTOR_ENABLE, leftSpeed);
  setMotor(RIGHT_MOTOR_IN1, RIGHT_MOTOR_IN2, RIGHT_MOTOR_ENABLE, rightSpeed);
}

void stopDrive() {
  drive(0, 0);
}

int readFlameSensor() {
  long total = 0;
  constexpr uint8_t sampleCount = 8;

  for (uint8_t sample = 0; sample < sampleCount; ++sample) {
    total += analogRead(FLAME_SENSOR_PIN);
    delayMicroseconds(300);
  }

  return static_cast<int>(total / sampleCount);
}

bool flameDetected(int reading) {
  return FLAME_READING_DECREASES_NEAR_FIRE
    ? reading <= FLAME_THRESHOLD
    : reading >= FLAME_THRESHOLD;
}

long readDistanceCm() {
  digitalWrite(ULTRASONIC_TRIG, LOW);
  delayMicroseconds(3);
  digitalWrite(ULTRASONIC_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRASONIC_TRIG, LOW);

  // A 30 ms timeout is roughly five metres and prevents a missing echo from
  // blocking the robot indefinitely.
  const unsigned long echoTimeUs = pulseIn(ULTRASONIC_ECHO, HIGH, 30000UL);
  if (echoTimeUs == 0) {
    return 400;
  }

  return static_cast<long>((echoTimeUs * 0.0343F) / 2.0F);
}

long lookAt(int angle) {
  scanServo.write(constrain(angle, 0, 180));
  delay(350);
  return readDistanceCm();
}

void avoidObstacle() {
  stopDrive();
  delay(120);

  const long leftDistance = lookAt(SCAN_LEFT_DEG);
  const long rightDistance = lookAt(SCAN_RIGHT_DEG);
  scanServo.write(SCAN_CENTER_DEG);
  delay(180);

  if (leftDistance < OBSTACLE_DISTANCE_CM && rightDistance < OBSTACLE_DISTANCE_CM) {
    drive(-TURN_SPEED, -TURN_SPEED);
    delay(420);
  }

  if (leftDistance >= rightDistance) {
    drive(-TURN_SPEED, TURN_SPEED);
  } else {
    drive(TURN_SPEED, -TURN_SPEED);
  }
  delay(430);
  stopDrive();
}

void patrol() {
  const long frontDistance = readDistanceCm();

  if (frontDistance <= OBSTACLE_DISTANCE_CM) {
    avoidObstacle();
  } else {
    drive(DRIVE_SPEED, DRIVE_SPEED);
  }
}

void setPump(bool enabled) {
  digitalWrite(PUMP_DRIVER_PIN, enabled ? HIGH : LOW);
}

void extinguishFire() {
  stopDrive();
  tone(BUZZER_PIN, 2400);
  setPump(true);

  const unsigned long sprayStartedMs = millis();
  unsigned long clearStartedMs = 0;
  int nozzleAngle = NOZZLE_MIN_DEG;
  int sweepStep = 3;

  while (millis() - sprayStartedMs < MAX_SPRAY_TIME_MS) {
    nozzleServo.write(nozzleAngle);

    nozzleAngle += sweepStep;
    if (nozzleAngle >= NOZZLE_MAX_DEG || nozzleAngle <= NOZZLE_MIN_DEG) {
      sweepStep = -sweepStep;
      nozzleAngle = constrain(nozzleAngle, NOZZLE_MIN_DEG, NOZZLE_MAX_DEG);
    }

    const int flameReading = readFlameSensor();
    if (!flameDetected(flameReading)) {
      if (clearStartedMs == 0) {
        clearStartedMs = millis();
      }
      if (millis() - clearStartedMs >= FIRE_CLEAR_CONFIRM_MS) {
        break;
      }
    } else {
      clearStartedMs = 0;
    }

    delay(35);
  }

  setPump(false);
  noTone(BUZZER_PIN);
  nozzleServo.write(NOZZLE_CENTER_DEG);
  delay(350);
  mode = RobotMode::Patrol;
}

void printDiagnostics(int flameReading) {
  if (millis() - lastDiagnosticMs < 500) {
    return;
  }

  lastDiagnosticMs = millis();
  Serial.print(F("Flame ADC: "));
  Serial.print(flameReading);
  Serial.print(F(" | Fire: "));
  Serial.print(flameDetected(flameReading) ? F("YES") : F("no"));
  Serial.print(F(" | Distance cm: "));
  Serial.println(readDistanceCm());
}

void setup() {
  pinMode(LEFT_MOTOR_IN1, OUTPUT);
  pinMode(LEFT_MOTOR_IN2, OUTPUT);
  pinMode(RIGHT_MOTOR_IN1, OUTPUT);
  pinMode(RIGHT_MOTOR_IN2, OUTPUT);
  pinMode(LEFT_MOTOR_ENABLE, OUTPUT);
  pinMode(RIGHT_MOTOR_ENABLE, OUTPUT);
  pinMode(ULTRASONIC_TRIG, OUTPUT);
  pinMode(ULTRASONIC_ECHO, INPUT);
  pinMode(PUMP_DRIVER_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(FLAME_SENSOR_PIN, INPUT);

  // Establish a safe output state before attaching actuators.
  stopDrive();
  setPump(false);
  noTone(BUZZER_PIN);

  scanServo.attach(SCAN_SERVO_PIN);
  nozzleServo.attach(NOZZLE_SERVO_PIN);
  scanServo.write(SCAN_CENTER_DEG);
  nozzleServo.write(NOZZLE_CENTER_DEG);

  Serial.begin(115200);
  Serial.println(F("Circuit Atlas fire-fighting robot starting"));
  delay(800);
}

void loop() {
  const int flameReading = readFlameSensor();
  printDiagnostics(flameReading);

  if (flameDetected(flameReading)) {
    mode = RobotMode::Extinguishing;
  }

  if (mode == RobotMode::Extinguishing) {
    extinguishFire();
  } else {
    patrol();
  }
}