#include <Servo.h>
#include "pitches.h"
#include <DHT.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// --- Display Settings ---
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// --- DHT Settings ---
#define DHTPIN 6
#define DHTTYPE DHT22
DHT dht(DHTPIN, DHTTYPE);

// --- Pins ---
#define BUZZER_PIN 7
#define MQ2_PIN A0
#define STEP_UP 4
#define DIR_UP 5
#define STEP_DOWN 2
#define DIR_DOWN 3
#define ENABLE_PIN 8
#define TRIG_PIN 12
#define ECHO_PIN 11
#define SERVO_PIN 9

// --- Constants ---
const int COLLISION_DIST = 5;
const int SAFE_DIST = 45;
const int BASE_SPEED = 8;
const int REVERSE_SPEED = 20;
const int TURN_SPEED = 30;
const int START_BEEP_DIST = 40;
const int SCAN_DELAY = 400;
unsigned long lastSensorPrintTime = 0;
const unsigned long sensorPrintInterval = 3000;  // 3 seconds

Servo scanner;

float temperature = 0;
float humidity = 0;
int gasLevel = 0;

// --- Setup ---
void setup() {
  pinMode(STEP_UP, OUTPUT);
  pinMode(DIR_UP, OUTPUT);
  pinMode(STEP_DOWN, OUTPUT);
  pinMode(DIR_DOWN, OUTPUT);
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, LOW);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);

  scanner.attach(SERVO_PIN);
  scanner.write(90);  // Forward

  Serial.begin(9600);
  dht.begin();

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("OLED init failed"));
    while (1);
  }

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println("Robot Ready");
  display.display();
  delay(2000);
}

// --- Motor Control ---
void stepMotors(int steps, int speedDelay) {
  for (int i = 0; i < steps; i++) {
    digitalWrite(STEP_UP, HIGH); digitalWrite(STEP_DOWN, HIGH);
    delayMicroseconds(2);
    digitalWrite(STEP_UP, LOW); digitalWrite(STEP_DOWN, LOW);
    delayMicroseconds(speedDelay);
  }
}

void moveForward(int steps, int speedDelay) {
  digitalWrite(DIR_UP, HIGH); digitalWrite(DIR_DOWN, HIGH);
  stepMotors(steps, speedDelay);
}

void moveBackward(int steps, int speedDelay) {
  digitalWrite(DIR_UP, LOW); digitalWrite(DIR_DOWN, LOW);
  stepMotors(steps, speedDelay);
}

void turnLeft(int steps, int speedDelay) {
  digitalWrite(DIR_UP, LOW); digitalWrite(DIR_DOWN, HIGH);
  stepMotors(steps, speedDelay);
}

void turnRight(int steps, int speedDelay) {
  digitalWrite(DIR_UP, HIGH); digitalWrite(DIR_DOWN, LOW);
  stepMotors(steps, speedDelay);
}

void stopMotors() {
  digitalWrite(ENABLE_PIN, HIGH);
  delay(100);
  digitalWrite(ENABLE_PIN, LOW);
}

// --- Sensors ---
long readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 20000);
  if (duration == 0) return -1;
  return duration * 0.034 / 2;
}

int scanDirection(int angle) {
  scanner.write(angle);
  delay(SCAN_DELAY);
  return readDistanceCM();
}

int readGasLevel() {
  return analogRead(MQ2_PIN);  // Raw value 0-1023
}

void updateDisplay(float temp, float hum, int gasRawVal) {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(1);

  display.print("Temp: ");
  display.print(temp);
  display.println(" C");

  display.print("Humidity: ");
  display.print(hum);
  display.println(" %");

  display.print("Gas Raw: ");
  display.print(gasRawVal);
  
  display.setCursor(0, 40);
  display.print("Air Quality: ");
  if (gasRawVal >= 210 && gasRawVal <= 900) {
    display.println("Good");
  } else if (gasRawVal > 905) {
    display.println("Bad");
  } else {
    display.println("Unknown");
  }

  display.display();
}


// --- Buzzer Alert Based on Distance ---
void buzzerAlert(long distance) {
  static bool inCollision = false;

  // Out of range: stop buzzer
  if (distance <= 0 || distance > START_BEEP_DIST) {
    noTone(BUZZER_PIN);
    inCollision = false;
    return;
  }

  // If collision distance reached, turn on continuous beep
  if (distance <= COLLISION_DIST) {
    if (!inCollision) {
      tone(BUZZER_PIN, NOTE_C6);  // Only trigger once
      inCollision = true;
    }
    return;
  }

  // Not in collision anymore, ensure continuous tone is stopped
  if (inCollision) {
    noTone(BUZZER_PIN);
    inCollision = false;
  }

  // Distance between COLLISION_DIST and START_BEEP_DIST → beep with intervals
  int beepInterval = map(distance, COLLISION_DIST, START_BEEP_DIST, 100, 500);
  beepInterval = constrain(beepInterval, 100, 500);

  tone(BUZZER_PIN, NOTE_A4);
  delay(beepInterval / 2);
  noTone(BUZZER_PIN);
  delay(beepInterval / 2);
}


void safeBeep() {
  tone(BUZZER_PIN, NOTE_C5, 150);
  delay(150);
  noTone(BUZZER_PIN);
}

// --- Main Loop ---
void loop() {
  temperature = dht.readTemperature();
  humidity = dht.readHumidity();
  gasLevel = readGasLevel();
  
  if (isnan(temperature) || isnan(humidity)) {
    temperature = 0;
    humidity = 0;
  }

  updateDisplay(temperature, humidity, gasLevel);

  long dist = readDistanceCM();
  Serial.print("Distance: "); Serial.println(dist);
  // Print sensor readings every 3 seconds
  if (millis() - lastSensorPrintTime >= sensorPrintInterval) {
    lastSensorPrintTime = millis();
    Serial.print("Temperature: ");
    Serial.print(temperature);
    Serial.print(" C, Humidity: ");
    Serial.print(humidity);
    Serial.print(" %, Gas Level: ");
    Serial.println(gasLevel);
  }

  buzzerAlert(dist);

  if (dist > 0 && dist <= COLLISION_DIST) {
    Serial.println("Obstacle! Reversing...");
    stopMotors();
    delay(200);

    while (true) {
    dist = readDistanceCM();
    buzzerAlert(dist); 

    if (dist > SAFE_DIST || dist == -1) {
    noTone(BUZZER_PIN);  // Stop buzzer immediately once safe
    break;
    }

    moveBackward(20, REVERSE_SPEED);
    delay(50);
    }


    safeBeep();
    Serial.println("Safe Distance Reached.!");
    int leftDist = scanDirection(150);
    delay(1000);
    int rightDist = scanDirection(30);
    delay(1000);
    scanner.write(90);
    //delay(200);

    if (leftDist == -1) leftDist = 0;
    if (rightDist == -1) rightDist = 0;

    if (leftDist > rightDist) {
      Serial.println("Turning LEFT");
      turnLeft(200, TURN_SPEED);
    } else {
      Serial.println("Turning RIGHT");
      turnRight(200, TURN_SPEED);
    }

    stopMotors();
    delay(300);
  } else {
    moveForward(30, BASE_SPEED);
  }

  delay(50);
}