/*
 * Fire Fighting Robot
 * v1.0
 */

#include <Servo.h>

const int DELAY_MAIN = 1000;

const int SMOKE_TRIGGER = 150;
const int OBSTACLE_TRIGGER = 5;     // 5 cm
const int LEFT_MOTOR_SPEED = 255;   // 0 - 255
const int RIGHT_MOTOR_SPEED = 255;  // 0 - 255
const int EXTINGUISHER_SERVO_START_ANGLE = 30;
const int EXTINGUISHER_SERVO_END_ANGLE = 50;  // Resting position
const int EXTINGUISH_TIMES = 2;
const int SENSOR_SERVO_START_ANGLE = 80;
const int SENSOR_SERVO_END_ANGLE = 100;

const int SMOKE_SENSOR_PIN = A0;
const int FLAME_SENSOR_LEFT_PIN = A5;
const int FLAME_SENSOR_RIGHT_PIN = A3;
const int FLAME_SENSOR_CENTER_PIN = A4;
const int US_TRIG_PIN = A2;
const int US_ECHO_PIN = A1;
const int MOTOR_ENA_PIN = 6;
const int MOTOR_IN1_PIN = 10;  // Right
const int MOTOR_IN2_PIN = 4;   // Right
const int MOTOR_IN3_PIN = 7;   // Left
const int MOTOR_IN4_PIN = 8;   // Left
const int MOTOR_ENB_PIN = 5;
const int EXTINGUISHER_SERVO_PIN = 9;
const int SENSOR_SERVO_PIN = 11;

Servo extinguisherServo;
Servo sensorServo;

bool smokeDetected = false;
bool leftFlameDetected = false;
bool rightFlameDetected = false;
bool centerFlameDetected = false;
bool obstacleDetected = false;
bool leftHasNoObstacle = true;
bool rightHasNoObstacle = true;

void setupFlameSensor() {
  pinMode(FLAME_SENSOR_LEFT_PIN, INPUT_PULLUP);
  pinMode(FLAME_SENSOR_RIGHT_PIN, INPUT_PULLUP);
  pinMode(FLAME_SENSOR_CENTER_PIN, INPUT_PULLUP);
}

void setupUltrasonic() {
  pinMode(US_TRIG_PIN, OUTPUT);
  pinMode(US_ECHO_PIN, INPUT);
}

void setupMotorPins() {
  pinMode(MOTOR_IN1_PIN, OUTPUT);
  pinMode(MOTOR_IN2_PIN, OUTPUT);
  pinMode(MOTOR_IN3_PIN, OUTPUT);
  pinMode(MOTOR_IN4_PIN, OUTPUT);
}

void controlMotor(const int IN1, const int IN2, const int IN3, const int IN4) {
  digitalWrite(MOTOR_IN1_PIN, IN1);
  digitalWrite(MOTOR_IN2_PIN, IN2);
  digitalWrite(MOTOR_IN3_PIN, IN3);
  digitalWrite(MOTOR_IN4_PIN, IN4);
}

void controlSpeed(const int ENA, const int ENB) {
  analogWrite(MOTOR_ENA_PIN, ENA);
  analogWrite(MOTOR_ENB_PIN, ENB);
}

void stopMotion() {
  controlMotor(LOW, LOW, LOW, LOW);
  controlSpeed(0, 0);
}

void motorLeft() {
  controlMotor(HIGH, LOW, LOW, HIGH);
  controlSpeed(RIGHT_MOTOR_SPEED, LEFT_MOTOR_SPEED);
}

void motorRight() {
  controlMotor(LOW, HIGH, HIGH, LOW);
  controlSpeed(RIGHT_MOTOR_SPEED, LEFT_MOTOR_SPEED);
}

void motorBackward() {
  controlMotor(LOW, HIGH, LOW, HIGH);
  controlSpeed(RIGHT_MOTOR_SPEED, LEFT_MOTOR_SPEED);
}

void motorForward() {
  controlMotor(HIGH, LOW, HIGH, LOW);
  controlSpeed(RIGHT_MOTOR_SPEED, LEFT_MOTOR_SPEED);
}

void detectFlame() {
  int flameLeft = !digitalRead(FLAME_SENSOR_LEFT_PIN);
  int flameRight = !digitalRead(FLAME_SENSOR_RIGHT_PIN);
  int flameCenter = !digitalRead(FLAME_SENSOR_CENTER_PIN);

  Serial.print("FLAME_LEFT: ");
  Serial.print(flameLeft);
  Serial.print(", FLAME_CENTER: ");
  Serial.print(flameCenter);
  Serial.print(", FLAME_RIGHT: ");
  Serial.println(flameRight);

  // Flame on Left Side
  if (flameLeft == HIGH && (flameCenter == HIGH || flameRight == LOW)) {
    leftFlameDetected = true;
    rightFlameDetected = false;
    centerFlameDetected = false;
    Serial.println("Left Flame Detected!");
  }
  // Flame on Right Side
  else if (flameRight == HIGH && (flameCenter == HIGH || flameLeft == LOW)) {
    rightFlameDetected = true;
    leftFlameDetected = false;
    centerFlameDetected = false;
    Serial.println("Right Flame Detected!");
  }
  // Flame on Center Side
  else if (flameCenter == HIGH) {
    centerFlameDetected = true;
    leftFlameDetected = false;
    rightFlameDetected = false;
    Serial.println("Center Flame Detected!");
  }
  // No Flame
  else {
    centerFlameDetected = false;
    leftFlameDetected = false;
    rightFlameDetected = false;
    Serial.println("No Flame Detected.");
  }
}

void turnLeft() {
  Serial.println("Turning left...");
  stopMotion();
  motorLeft();
}


void turnRight() {
  Serial.println("Turning right...");
  stopMotion();
  motorRight();
}

void aimFlame() {
  while (leftFlameDetected || rightFlameDetected) {
    if (leftFlameDetected) {
      turnLeft();
    } else if (rightFlameDetected) {
      turnRight();
    }
    delay(DELAY_MAIN);
    stopMotion();

    detectFlame();
    delay(DELAY_MAIN);
    if (!leftFlameDetected && !rightFlameDetected) {
      break;
    }
  }
}


void trigger() {
  Serial.println("Extinguihsing Flame...");

  static int pos = 0;
  for (int i = 0; i < EXTINGUISH_TIMES; i++) {
    for (pos = EXTINGUISHER_SERVO_START_ANGLE; pos <= EXTINGUISHER_SERVO_END_ANGLE; pos += 1) {
      // in steps of 1 degree
      extinguisherServo.write(pos);
      delay(15);
    }
    delay(2000);
    for (pos = EXTINGUISHER_SERVO_END_ANGLE; pos >= EXTINGUISHER_SERVO_START_ANGLE; pos -= 1) {
      extinguisherServo.write(pos);
      delay(15);
    }
    delay(1000);
  }
}

void extinguishFlame() {
  if (centerFlameDetected) {
    stopMotion();
    while (centerFlameDetected) {
      trigger();
      delay(DELAY_MAIN);
      detectFlame();
      delay(DELAY_MAIN);
      if (!centerFlameDetected) {
        break;
      }
    }
  }
}

void detectSmoke() {
  int smoke = analogRead(SMOKE_SENSOR_PIN);

  Serial.print("SMOKE VALUE: ");
  Serial.println(smoke);

  if (smoke >= SMOKE_TRIGGER) {
    Serial.println("Smoke Detected!");
    smokeDetected = true;
  } else {
    smokeDetected = false;
  }
}

float getUltrasonicValue() {
  digitalWrite(US_TRIG_PIN, LOW);
  delayMicroseconds(2);

  // Send Pulse
  digitalWrite(US_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(US_TRIG_PIN, LOW);

  // Get Sound Distance
  long duration = pulseIn(US_ECHO_PIN, HIGH);

  // Calculatge actual distance in cm
  float distanceCM = duration * 0.0343 / 2;

  Serial.print("OBSTACLE_DISTANCE_CM: ");
  Serial.print(distanceCM, 2);
  Serial.println("cm");

  return distanceCM;
}

void detectObstacle() {
  if (smokeDetected) {

    float distanceCM = getUltrasonicValue();
    if (distanceCM <= OBSTACLE_TRIGGER) {
      obstacleDetected = true;
    } else {
      obstacleDetected = false;
    }
  } else {
    obstacleDetected = false;
  }
}

void moveBack() {
  Serial.println("Moving Backward...");
  stopMotion();
  motorBackward();
}

void rotateDetector() {
  Serial.println("Rotating Ultrasonic...");
  static int pos = 0;
  
  for (pos = 90; pos >= SENSOR_SERVO_START_ANGLE; pos -= 1) {
    sensorServo.write(pos);
    delay(15);
  }
  for (pos = SENSOR_SERVO_START_ANGLE; pos <= 90; pos += 1) {
    sensorServo.write(pos);
    delay(15);
    if (pos == SENSOR_SERVO_START_ANGLE) {
      float distanceCM = getUltrasonicValue();
      if (distanceCM <= OBSTACLE_TRIGGER) {
        leftHasNoObstacle = true;
        Serial.println("LEFT SIDE: Clear!");
      } else {
        rightHasNoObstacle = false;
      }
    }
  }
  
  for (pos = 90; pos <= SENSOR_SERVO_END_ANGLE; pos += 1) {
    sensorServo.write(pos);
    delay(15);
    if (pos == SENSOR_SERVO_START_ANGLE) {
      float distanceCM = getUltrasonicValue();
      if (distanceCM <= OBSTACLE_TRIGGER) {
        leftHasNoObstacle = false;
      } else {
        rightHasNoObstacle = true;
        Serial.println("RIGHT SIDE: Clear!");
      }
    }
  }
  for (pos = SENSOR_SERVO_END_ANGLE; pos >= 90; pos -= 1) {
    sensorServo.write(pos);
    delay(15);
  }
}

void avoidObstacle() {
  if (obstacleDetected) {
    moveBack();
    delay(DELAY_MAIN);
    rotateDetector();
    if (leftHasNoObstacle) {
      turnLeft();
    } else if (rightHasNoObstacle) {
      turnRight();
    }
    delay(DELAY_MAIN);
    stopMotion();
    obstacleDetected = false;
    leftHasNoObstacle = false;
    rightHasNoObstacle = false;
  }
}

void moveForward() {
  Serial.println("Moving Forward...");
  motorForward();
}

void roam() {
  if (smokeDetected) {
    moveForward();
  } else {
    stopMotion();
  }
}

void setup() {
  setupFlameSensor();
  setupUltrasonic();
  setupMotorPins();
  extinguisherServo.attach(EXTINGUISHER_SERVO_PIN);
  sensorServo.attach(SENSOR_SERVO_PIN);
  Serial.begin(9600);
  stopMotion();
}

void loop() {
  // Flame Detection and Extinguisher Routine
  detectFlame();
  aimFlame();
  
  extinguishFlame();

  // Smoke Detection
  detectSmoke();

  // Object Detection and Avoidance Routine
  detectObstacle();
  avoidObstacle();

  // Roaming
  roam();


  delay(DELAY_MAIN);
}