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

#define DHT_PIN 15
#define LED_GREEN 5
#define LED_YELLOW 18
#define LED_RED 19
#define WEIGHT_KG 70.0f
#define STRIDE_M 0.75f

LiquidCrystal lcd1(14, 27, 26, 25, 33, 32);
LiquidCrystal lcd2(4, 12, 23, 2, 13, 3);

const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqttServer = "broker.emqx.io";

WiFiClient espClient;
PubSubClient client(espClient);

DHTesp dht;
Adafruit_MPU6050 mpu;

unsigned long lastMsg = 0;
unsigned long lastStepMs = 0;
long steps = 0;

float ema = 1.0f;
const float alpha = 0.12f;
const float stepThresh = 0.18f;
const unsigned long stepDebounceMs = 300;
bool peakArmed = true;

unsigned long winStart = 0;
unsigned long winMs = 10000;
long winSteps = 0;
float spm = 0.0f;

void reconnect() {
  while (!client.connected()) {
    String cid = String("esp32-fit-") + String((uint32_t)ESP.getEfuseMac(), HEX);
    client.connect(cid.c_str());
    if (!client.connected()) delay(250);
  }
}

void clearLine(LiquidCrystal& lcd, uint8_t row) {
  lcd.setCursor(0, row);
  for (int i = 0; i < 16; i++) lcd.print(' ');
  lcd.setCursor(0, row);
}

void setup() {
  Serial.begin(115200);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_YELLOW, LOW);
  digitalWrite(LED_RED, LOW);

  lcd1.begin(16, 2);
  lcd2.begin(16, 2);
  clearLine(lcd1, 0); lcd1.print("Fitness Tracker");
  clearLine(lcd2, 0); lcd2.print("Display Ready");
  delay(500);

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) delay(200);
  client.setServer(mqttServer, 1883);

  dht.setup(DHT_PIN, DHTesp::DHT22);

  Wire.begin();
  while (!mpu.begin()) { delay(50); }
  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  winStart = millis();
}

void loop() {
  if (!client.connected()) reconnect();
  client.loop();

  TempAndHumidity th = dht.getTempAndHumidity();

  sensors_event_t a, g, t;
  mpu.getEvent(&a, &g, &t);

  float mag = sqrtf(a.acceleration.x*a.acceleration.x +
                    a.acceleration.y*a.acceleration.y +
                    a.acceleration.z*a.acceleration.z);
  float accG = mag / 9.80665f;
  ema = alpha * accG + (1.0f - alpha) * ema;
  float hp = accG - ema;

  unsigned long now = millis();
  if (peakArmed && hp > stepThresh && (now - lastStepMs) > stepDebounceMs) {
    steps++;
    winSteps++;
    lastStepMs = now;
    peakArmed = false;
  }
  if (hp < 0) peakArmed = true;

  if (now - winStart >= winMs) {
    spm = (winSteps * 60000.0f) / (float)winMs;
    winSteps = 0;
    winStart = now;
  }

  float distance_km = (steps * STRIDE_M) / 1000.0f;
  float kcal = distance_km * WEIGHT_KG * 1.036f;
  float kJ = kcal * 4.184f;

  bool fever = (!isnan(th.temperature) && th.temperature >= 37.5f);
  if (fever) {
    digitalWrite(LED_RED, HIGH);
    digitalWrite(LED_YELLOW, LOW);
    digitalWrite(LED_GREEN, LOW);
  } else if (spm >= 60.0f) {
    digitalWrite(LED_GREEN, HIGH);
    digitalWrite(LED_YELLOW, LOW);
    digitalWrite(LED_RED, LOW);
  } else if (spm >= 20.0f) {
    digitalWrite(LED_GREEN, LOW);
    digitalWrite(LED_YELLOW, HIGH);
    digitalWrite(LED_RED, LOW);
  } else {
    digitalWrite(LED_GREEN, LOW);
    digitalWrite(LED_YELLOW, LOW);
    digitalWrite(LED_RED, LOW);
  }

  clearLine(lcd1, 0);
  lcd1.print("T:");
  if (!isnan(th.temperature)) lcd1.print(String(th.temperature, 1));
  else lcd1.print("--");
  lcd1.print(" H:");
  if (!isnan(th.humidity)) lcd1.print(String(th.humidity, 0));
  else lcd1.print("--");
  lcd1.print("%");

  clearLine(lcd1, 1);
  lcd1.print("S:");
  lcd1.print(steps);
  lcd1.print(" K:");
  lcd1.print(kcal, 1);

  clearLine(lcd2, 0);
  lcd2.print("SPM:");
  lcd2.print(spm, 0);
  lcd2.print(" D:");
  lcd2.print(distance_km, 2);

  clearLine(lcd2, 1);
  lcd2.print("kJ:");
  lcd2.print(kJ, 0);
  lcd2.print(" Kcal:");
  lcd2.print(kcal, 0);

  if (now - lastMsg > 800) {
    lastMsg = now;
    String payload = String("{\"temp\":") + (isnan(th.temperature)?String("null"):String(th.temperature,2)) +
                     ",\"hum\":" + (isnan(th.humidity)?String("null"):String(th.humidity,1)) +
                     ",\"steps\":" + String(steps) +
                     ",\"spm\":" + String(spm,1) +
                     ",\"kcal\":" + String(kcal,2) +
                     ",\"kJ\":" + String(kJ,1) +
                     ",\"distance_km\":" + String(distance_km,3) + "}";
    client.publish("/fitness", payload.c_str());
  }
}