#include <WiFi.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <PubSubClient.h>
#include <math.h>

// ---------- WiFi Wokwi ----------
const char* WIFI_SSID = "Wokwi-GUEST";
const char* WIFI_PASS = "";

// ---------- MQTT ----------
const char* MQTT_HOST = "broker.hivemq.com";
const int MQTT_PORT = 1883;

// Dung tien to rieng cho du an
const char* BASE_TOPIC = "fall05_demo_8264";

char telemetryTopic[100];
char ledTopic[100];
char buzzerTopic[100];

WiFiClient wifiClient;
PubSubClient mqtt(wifiClient);
Adafruit_MPU6050 mpu;

// ---------- GPIO ----------
const int LED_PIN = 18;
const int BUZZER_PIN = 19;

// ---------- Fall detection ----------
const float FREE_FALL_G = 0.45;
const float IMPACT_G = 2.40;
const float TILT_LIMIT_DEG = 55.0;

const unsigned long CONFIRM_MS = 1500;
const unsigned long EVENT_WINDOW_MS = 5000;
const unsigned long SAMPLE_MS = 100;

unsigned long lastSample = 0;
unsigned long lastPublish = 0;
unsigned long eventTime = 0;
unsigned long tiltStart = 0;

bool abnormalMotionSeen = false;
bool tiltTiming = false;
bool fallAlarm = false;

// Manual requests; automatic alarm has priority
bool manualLed = false;
bool manualBuzzer = false;

float ax = 0, ay = 0, az = 0;
float accelMagnitude = 0;
float pitchDeg = 0, rollDeg = 0, tiltDeg = 0;

bool ledState = false;
bool buzzerState = false;

// ---------- MQTT message callback ----------
void mqttCallback(char* topic, byte* payload,
                  unsigned int length) {
  String message;
  for (unsigned int i = 0; i < length; i++) {
    message += (char)payload[i];
  }
  message.trim();
  message.toUpperCase();

  if (strcmp(topic, ledTopic) == 0) {
    if (message == "ON") manualLed = true;
    if (message == "OFF") manualLed = false;
  }

  if (strcmp(topic, buzzerTopic) == 0) {
    if (message == "ON") manualBuzzer = true;
    if (message == "OFF") manualBuzzer = false;
  }

  Serial.print("MQTT command: ");
  Serial.print(topic);
  Serial.print(" = ");
  Serial.println(message);
}

void connectWiFi() {
  WiFi.mode(WIFI_STA);
  WiFi.begin(WIFI_SSID, WIFI_PASS);

  Serial.print("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(250);
    Serial.print(".");
  }

  Serial.println("\nWiFi connected");
  Serial.println(WiFi.localIP());
}

void connectMQTT() {
  while (!mqtt.connected()) {
    Serial.print("Connecting to MQTT...");

    // Unique client ID
    String clientId = "FallESP32-" +
                      String((uint32_t)ESP.getEfuseMac(), HEX);

    if (mqtt.connect(clientId.c_str())) {
      Serial.println("connected");

      mqtt.subscribe(ledTopic);
      mqtt.subscribe(buzzerTopic);
    } else {
      Serial.print("failed, state=");
      Serial.println(mqtt.state());
      delay(2000);
    }
  }
}

void readSensor() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Convert m/s^2 to g
  ax = a.acceleration.x / 9.80665;
  ay = a.acceleration.y / 9.80665;
  az = a.acceleration.z / 9.80665;

  accelMagnitude = sqrt(
    ax * ax + ay * ay + az * az
  );

  pitchDeg = atan2(
    -ax, sqrt(ay * ay + az * az)
  ) * 180.0 / PI;

  rollDeg = atan2(ay, az) * 180.0 / PI;

  tiltDeg = max(abs(pitchDeg), abs(rollDeg));
}

void updateFallDetection() {
  unsigned long now = millis();

  // Step 1: abnormal acceleration event
  if (accelMagnitude < FREE_FALL_G ||
      accelMagnitude > IMPACT_G) {
    abnormalMotionSeen = true;
    eventTime = now;
    tiltTiming = false;
  }

  // Step 2: after the event, check abnormal posture
  if (abnormalMotionSeen &&
      now - eventTime <= EVENT_WINDOW_MS) {

    if (tiltDeg >= TILT_LIMIT_DEG) {
      if (!tiltTiming) {
        tiltTiming = true;
        tiltStart = now;
      }

      if (now - tiltStart >= CONFIRM_MS) {
        fallAlarm = true;
      }
    } else {
      tiltTiming = false;
    }

  } else if (abnormalMotionSeen) {
    abnormalMotionSeen = false;
    tiltTiming = false;
  }
}

void updateOutputs() {
  // The automatic alarm takes priority
  ledState = manualLed || fallAlarm;
  buzzerState = manualBuzzer || fallAlarm;

  digitalWrite(LED_PIN, ledState ? HIGH : LOW);

  // Wokwi buzzer: HIGH activates the buzzer
  digitalWrite(BUZZER_PIN,
               buzzerState ? HIGH : LOW);
}

void publishTelemetry() {
  char payload[300];

  snprintf(
    payload, sizeof(payload),
    "{\"ax\":%.3f,\"ay\":%.3f,\"az\":%.3f,"
    "\"accel\":%.3f,\"pitch\":%.2f,"
    "\"roll\":%.2f,\"tilt\":%.2f,"
    "\"fall\":%d,\"led\":%d,\"buzzer\":%d}",
    ax, ay, az, accelMagnitude,
    pitchDeg, rollDeg, tiltDeg,
    fallAlarm ? 1 : 0,
    ledState ? 1 : 0,
    buzzerState ? 1 : 0
  );

  mqtt.publish(telemetryTopic, payload);
  Serial.println(payload);
}

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

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

  snprintf(telemetryTopic, sizeof(telemetryTopic),
           "%s/telemetry", BASE_TOPIC);
  snprintf(ledTopic, sizeof(ledTopic),
           "%s/command/led", BASE_TOPIC);
  snprintf(buzzerTopic, sizeof(buzzerTopic),
           "%s/command/buzzer", BASE_TOPIC);

  Wire.begin(21, 22);

  if (!mpu.begin()) {
    Serial.println("MPU6050 not found!");
    while (true) delay(100);
  }

  mpu.setAccelerometerRange(
    MPU6050_RANGE_8_G
  );
  mpu.setGyroRange(
    MPU6050_RANGE_500_DEG
  );
  mpu.setFilterBandwidth(
    MPU6050_BAND_21_HZ
  );

  Serial.println("MPU6050 ready");

  connectWiFi();

  mqtt.setServer(MQTT_HOST, MQTT_PORT);
  mqtt.setCallback(mqttCallback);
  mqtt.setBufferSize(512);
  mqtt.setKeepAlive(30);

  connectMQTT();
}

void loop() {
  if (WiFi.status() != WL_CONNECTED) {
    connectWiFi();
  }

  if (!mqtt.connected()) {
    connectMQTT();
  }
  mqtt.loop();

  unsigned long now = millis();

  if (now - lastSample >= SAMPLE_MS) {
    lastSample = now;

    readSensor();
    updateFallDetection();
    updateOutputs();
  }

  if (now - lastPublish >= 500) {
    lastPublish = now;
    publishTelemetry();
  }
}