#include <Servo.h>
#include <DHT.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

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

// ----- Pins -----
#define BUTTON_PIN 3
#define JOY_X A1
#define JOY_Y A2
#define JOY_BTN 4
#define PIR_PIN 5
#define RED_LED 6
#define SERVO1_PIN 9
#define SERVO2_PIN 10
#define ULTRA_TRIG 8
#define ULTRA_ECHO 12
#define MQ2_PIN A0

// ----- Servos -----
Servo servo1;
Servo servo2;

// ----- LCD -----
LiquidCrystal_I2C lcd(0x27, 20, 4);

// ----- Variables -----
int mode = 0; // 0=Manual, 1=Auto
bool lastButtonState = HIGH;
bool buttonPressed = false;

bool lastJoyBtn = HIGH;
bool redLedManualToggle = false;

float temp, hum;
int gasLevel;
int pirState;
long distance;

// --- Manual mode servo positions ---
float servo1Pos = 0;
float servo2Pos = 0;

// --- Sweep recording ---
struct SweepData {
  int angle1;
  int angle2;
  int angle3;
  float temp1, temp2, temp3;
  float hum1, hum2, hum3;
  long distance1, distance2, distance3;
  int gas1, gas2, gas3;
};
SweepData lastSweep;

// --- Timing for smooth manual movement ---
unsigned long lastMoveTime = 0;
const float maxSpeedDegPerSec = 10; // Max 5° per second

void setup() {
  Serial.begin(9600);

  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);

  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(JOY_BTN, INPUT_PULLUP);
  pinMode(PIR_PIN, INPUT);
  pinMode(RED_LED, OUTPUT);

  pinMode(ULTRA_TRIG, OUTPUT);
  pinMode(ULTRA_ECHO, INPUT);

  dht.begin();
  lcd.init();
  lcd.backlight();
  lcd.clear();

  servo1.write(servo1Pos);
  servo2.write(servo2Pos);

  Serial.println("System Ready. Manual Mode Active");
}

void loop() {
  unsigned long currentMillis = millis();

  // --- Button press detection for mode toggle ---
  bool buttonState = digitalRead(BUTTON_PIN);
  if (lastButtonState == HIGH && buttonState == LOW) {
    buttonPressed = true;
  }
  lastButtonState = buttonState;

  if (buttonPressed) {
    mode = !mode;
    buttonPressed = false;

    // Reset motors ONLY when entering Auto mode
    if (mode == 1) {
      servo1Pos = 0;
      servo2Pos = 0;
      servo1.write(servo1Pos);
      servo2.write(servo2Pos);
    }

    if (mode == 0) Serial.println("Manual Mode Active");
    else Serial.println("Auto Mode Active");
  }

  // --- Joystick button LED toggle ---
  bool joyBtnState = digitalRead(JOY_BTN);
  if (lastJoyBtn == HIGH && joyBtnState == LOW) {
    redLedManualToggle = !redLedManualToggle;
  }
  lastJoyBtn = joyBtnState;

  if (mode == 0) digitalWrite(RED_LED, redLedManualToggle ? HIGH : LOW);

  // --- Read sensors ---
  pirState = digitalRead(PIR_PIN);
  gasLevel = analogRead(MQ2_PIN);
  temp = dht.readTemperature();
  hum = dht.readHumidity();
  distance = readDistance();

  // --- Display live sensor data on LCD ---
  lcd.setCursor(0,0);
  lcd.print("Temp: "); lcd.print(temp); lcd.print("C ");
  lcd.setCursor(19,0);
  lcd.print(mode == 1 ? "a" : "m");

  lcd.setCursor(0,1);
  lcd.print("Hum: "); lcd.print(hum); lcd.print("%       ");

  lcd.setCursor(0,2);
  lcd.print("Dist: "); lcd.print(distance); lcd.print("cm       ");

  lcd.setCursor(0,3);
  lcd.print("Gas: "); lcd.print(gasLevel); lcd.print("ppm ");
  lcd.print("Mo: "); lcd.print(pirState ? "Yes" : "No ");

  // --- Mode Handling ---
  if (mode == 0) {
    // Manual mode: smooth joystick control
    float target1 = map(analogRead(JOY_X), 0, 1023, 0, 180);
    float target2 = map(analogRead(JOY_Y), 0, 1023, 0, 180);

    float dt = (currentMillis - lastMoveTime) / 1000.0; // seconds
    float maxStep = maxSpeedDegPerSec * dt;

    // Servo1
    if (abs(target1 - servo1Pos) > maxStep) {
      servo1Pos += (target1 > servo1Pos ? 1 : -1) * maxStep;
    } else {
      servo1Pos = target1;
    }

    // Servo2
    if (abs(target2 - servo2Pos) > maxStep) {
      servo2Pos += (target2 > servo2Pos ? 1 : -1) * maxStep;
    } else {
      servo2Pos = target2;
    }

    servo1.write(servo1Pos);
    servo2.write(servo2Pos);

    lastMoveTime = currentMillis;
  } 
  else {
    // Auto mode
    if (pirState == HIGH) {
      digitalWrite(RED_LED, HIGH);
      performSweepSequence();
      digitalWrite(RED_LED, LOW);
      printSweepData(); // Serial Monitor only
    }
  }

  delay(20); // Small delay for smoothness
}

// --- Ultrasonic distance ---
long readDistance() {
  digitalWrite(ULTRA_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(ULTRA_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRA_TRIG, LOW);
  long dur = pulseIn(ULTRA_ECHO, HIGH);
  return dur * 0.034 / 2;
}

// --- Sweep sequence ---
void performSweepSequence() {
  servo1.write(45);
  servo2.write(90); delay(500);
  recordSweepData(0);

  for (int pos = 0; pos <= 180; pos += 5) {
    servo1.write(pos); delay(30);
    if (pos == 90) recordSweepData(1);
  }

  servo1.write(90); delay(300);
  servo2.write(135); delay(500);

  for (int pos = 0; pos <= 180; pos += 5) {
    servo1.write(pos); delay(30);
    if (pos == 90) recordSweepData(2);
  }

  servo1.write(90);
  servo2.write(90); delay(500);
}

// --- Record sensor data ---
void recordSweepData(int stage) {
  switch(stage){
    case 0:
      lastSweep.angle1 = servo1.read();
      lastSweep.temp1 = temp;
      lastSweep.hum1 = hum;
      lastSweep.distance1 = distance;
      lastSweep.gas1 = gasLevel;
      break;
    case 1:
      lastSweep.angle2 = servo1.read();
      lastSweep.temp2 = temp;
      lastSweep.hum2 = hum;
      lastSweep.distance2 = distance;
      lastSweep.gas2 = gasLevel;
      break;
    case 2:
      lastSweep.angle3 = servo1.read();
      lastSweep.temp3 = temp;
      lastSweep.hum3 = hum;
      lastSweep.distance3 = distance;
      lastSweep.gas3 = gasLevel;
      break;
  }
}

// --- Print sweep data to Serial ---
void printSweepData() {
  Serial.println("=== Auto Sweep Data ===");
  Serial.print("Angle1: "); Serial.print(lastSweep.angle1);
  Serial.print(" Temp: "); Serial.print(lastSweep.temp1);
  Serial.print(" Hum: "); Serial.print(lastSweep.hum1);
  Serial.print(" Dist: "); Serial.print(lastSweep.distance1);
  Serial.print(" Gas: "); Serial.println(lastSweep.gas1);

  Serial.print("Angle2: "); Serial.print(lastSweep.angle2);
  Serial.print(" Temp: "); Serial.print(lastSweep.temp2);
  Serial.print(" Hum: "); Serial.print(lastSweep.hum2);
  Serial.print(" Dist: "); Serial.print(lastSweep.distance2);
  Serial.print(" Gas: "); Serial.println(lastSweep.gas2);

  Serial.print("Angle3: "); Serial.print(lastSweep.angle3);
  Serial.print(" Temp: "); Serial.print(lastSweep.temp3);
  Serial.print(" Hum: "); Serial.print(lastSweep.hum3);
  Serial.print(" Dist: "); Serial.print(lastSweep.distance3);
  Serial.print(" Gas: "); Serial.println(lastSweep.gas3);

  Serial.println("=======================");
}