#include <Wire.h>
#include <Stepper.h>
#include <DHT.h>

// ----- Sensor Addresses -----
const uint8_t MPU_ADDR = 0x68;

// ----- DHT22 Setup -----
#define DHT_PIN PB12
#define DHT_TYPE DHT22
DHT dht(DHT_PIN, DHT_TYPE);

const int stepsPerRevolution = 200;

// Stepper 1 pins (4 wire)
const int stepper1Pin1 = 2;  // Connect these pins accordingly
const int stepper1Pin2 = 3;
const int stepper1Pin3 = 4;
const int stepper1Pin4 = 5;

// Stepper 2 pins (4 wire)
const int stepper2Pin1 = 8;
const int stepper2Pin2 = 9;
const int stepper2Pin3 = 10;
const int stepper2Pin4 = 11;

Stepper stepper1(stepsPerRevolution, stepper1Pin1, stepper1Pin2, stepper1Pin3, stepper1Pin4);
Stepper stepper2(stepsPerRevolution, stepper2Pin1, stepper2Pin2, stepper2Pin3, stepper2Pin4);

// ----- Relay & Buzzer -----
#define RELAY_PIN PB13
#define BUZZER_PIN PB14

// ----- Motion & Temp Thresholds -----
const float ACCEL_THRESHOLD = 1.5; // g units
const float TEMP_THRESHOLD = 30.0; // °C

// Direction toggle for stepper movement
bool movingForward = true;

void setup() {
  Serial.begin(115200);
  Serial1.begin(115200);

  Serial.println("Starting Motion & Environment Control System (DHT22 + Stepper)");

  // Init I2C for MPU6050
  Wire.begin(PB7, PB6);
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();

  // Init DHT
  dht.begin();

  // Set stepper speed (RPM)
  stepper1.setSpeed(20);
  stepper2.setSpeed(20);

  // Init Relay & Buzzer
  pinMode(RELAY_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  // Turn relay off initially (HIGH if active LOW)
  digitalWrite(RELAY_PIN, HIGH);

  Serial.println("Setup complete. System ready.");
  Serial.println("Format: AccelX | AccelY | AccelZ || Temp(°C) | Humidity(%)");
}

void loop() {
  // ----- MPU6050 Read -----
  int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);

  if (Wire.requestFrom(MPU_ADDR, 14) == 14) {
    ax_raw = Wire.read() << 8 | Wire.read();
    ay_raw = Wire.read() << 8 | Wire.read();
    az_raw = Wire.read() << 8 | Wire.read();
    Wire.read(); Wire.read(); // skip temp registers
    gx_raw = Wire.read() << 8 | Wire.read();
    gy_raw = Wire.read() << 8 | Wire.read();
    gz_raw = Wire.read() << 8 | Wire.read();
  } else {
    ax_raw = random(-16384, 16384);
    ay_raw = random(-16384, 16384);
    az_raw = random(-16384, 16384);
    gx_raw = random(-32768, 32767);
    gy_raw = random(-32768, 32767);
    gz_raw = random(-32768, 32767);
  }

  float ax_g = ax_raw / 16384.0;
  float ay_g = ay_raw / 16384.0;
  float az_g = az_raw / 16384.0;

  // ----- DHT22 Read -----
  float tempC = dht.readTemperature();
  float humidity = dht.readHumidity();

  // Check if reads are valid
  if (isnan(tempC) || isnan(humidity)) {
    Serial.println("Failed to read from DHT sensor!");
    tempC = 0;
    humidity = 0;
  }

  // ----- Display Data -----
  Serial.print("Accel(g): X=");
  Serial.print(ax_g, 2);
  Serial.print(" | Y=");
  Serial.print(ay_g, 2);
  Serial.print(" | Z=");
  Serial.print(az_g, 2);
  Serial.print(" || Temp: ");
  Serial.print(tempC, 1);
  Serial.print("°C | Humidity: ");
  Serial.print(humidity, 1);
  Serial.println("%");

  // ----- Motion Control -----
  if (abs(ax_g) > ACCEL_THRESHOLD || abs(ay_g) > ACCEL_THRESHOLD) {
    Serial.println("⚠ Motion Detected → Moving BOTH Steppers & Buzzing!");

    if (movingForward) {
      stepper1.step(stepsPerRevolution);    // clockwise
      stepper2.step(-stepsPerRevolution);   // counterclockwise (opposite)
    } else {
      stepper1.step(-stepsPerRevolution);
      stepper2.step(stepsPerRevolution);
    }
    movingForward = !movingForward;

    digitalWrite(BUZZER_PIN, HIGH);
  } else {
    digitalWrite(BUZZER_PIN, LOW);
  }

  // ----- Temperature Control (Active LOW relay) -----
  if (tempC > TEMP_THRESHOLD) {
    Serial.println("🔥 High Temp → Relay ON!");
    digitalWrite(RELAY_PIN, LOW);  // Relay ON (active LOW)
  } else {
    digitalWrite(RELAY_PIN, HIGH); // Relay OFF
  }

  delay(100);
}