#include <Arduino.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <ArduinoJson.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <U8g2_for_Adafruit_GFX.h>
#include "DHT.h"
#include <WebServer.h>

#define PROJECT_TRANSPORT 1
#define PROJECT_MEDICAL   2

#ifndef PROJECT_MODE
  #define PROJECT_MODE PROJECT_MEDICAL
#endif
#ifndef SIMULATION
  #define SIMULATION 1
#endif

#define DHTPIN 4
#define DHTTYPE DHT22

#define PIN_POT1 34
#define PIN_POT2 35

#define TRIG_A 5
#define ECHO_A 18
#define TRIG_B 17
#define ECHO_B 16

#define PIN_BUZZER 15
#define PIN_LED 2
#define PIN_BTN_RST 13

#define OLED_W 128
#define OLED_H 64
#define OLED_ADDR 0x3C
Adafruit_SSD1306 oled(OLED_W, OLED_H, &Wire, -1);
U8G2_FOR_ADAFRUIT_GFX u8;

static const char* WIFI_SSID = "Wokwi-GUEST";
static const char* WIFI_PASS = "";
static const char* MQTT_HOST = "test.mosquitto.org";
static const uint16_t MQTT_PORT = 1883;

static String deviceId = "esp32-wokwi-med";

/* medical topics */
static String tMedTelemetry = "med/telemetry";
static String tMedAlerts    = "med/alerts";
static String tMedCmd       = "med/cmd";

/* transport topics */
static String tTrTelemetry  = "wokwi/iot/" + deviceId + "/telemetry";
static String tTrAnomaly    = "wokwi/iot/" + deviceId + "/anomaly";
static String tTrControl    = "wokwi/iot/" + deviceId + "/control";

WiFiClient wifiClient;
PubSubClient mqtt(wifiClient);
WebServer http(80);

static String lastStatus = "{}";

DHT dht(DHTPIN, DHTTYPE);

static float fmap(float v, float i1, float i2, float o1, float o2){
  return (v - i1) * (o2 - o1) / (i2 - i1) + o1;
}

static float readUS_cm(uint8_t trig, uint8_t echo){
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long dur = pulseIn(echo, HIGH, 30000);
  if (dur == 0) return 9999.0f;
  return dur / 58.0f;
}

static const int BUZ_CH = 3;
static inline void buzTone(int hz){ ledcWriteTone(BUZ_CH, hz); }
static inline void buzOff(){ ledcWriteTone(BUZ_CH, 0); }

struct Button {
  bool prev = true;
  uint32_t lastEdgeMs = 0;

  bool fell(uint32_t now){
    bool cur = digitalRead(PIN_BTN_RST);
    bool event = (cur == LOW && prev == HIGH && (now - lastEdgeMs) > 200);
    if (event) lastEdgeMs = now;
    prev = cur;
    return event;
  }
} btn;

struct OledUi {
  void header(const __FlashStringHelper* title){
    oled.clearDisplay();
    u.setFont(u8g2_font_t0_14b_tf);
    u.setCursor(0, 14);
    u.print(title);
  }
  void showBoot(){
    header(F("Booting..."));
    u.setCursor(0, 34); u.print(F("Init OLED/ADC"));
    oled.display();
  }
} ui;

struct NetManager {
  uint32_t wifiTick = 0;
  uint32_t mqttTick = 0;
  uint32_t backoff = 500;

  void kickWifi(){
    if (WiFi.status() == WL_CONNECTED) return;
    WiFi.mode(WIFI_STA);
    WiFi.begin(WIFI_SSID, WIFI_PASS);
  }

  void wifiWatch(uint32_t now){
    if (now - wifiTick < 1000) return;
    wifiTick = now;
    if (WiFi.status() != WL_CONNECTED) kickWifi();
  }

  void mqttEnsure(uint32_t now){
    if (mqtt.connected()) return;
    if (now - mqttTick < backoff) return;
    mqttTick = now;

    if (mqtt.connect(deviceId.c_str())) {
      backoff = 500;
#if PROJECT_MODE==PROJECT_MEDICAL
      mqtt.subscribe(tMedCmd.c_str());
#else
      mqtt.subscribe(tTrControl.c_str());
#endif
    } else {
      backoff = (backoff < 10000) ? backoff * 2 : 10000;
    }
  }
} net;

static void httpStatus(){
  http.send(200, "application/json", lastStatus);
}

struct BioSample { float hr; float spo2; float tBody; float motion; };
struct BioOut    { float hrF; float spF; float tBody; bool alarm; const char* why; };

#if SIMULATION
static float simPhase = 0.0f;
static const float simDt = 0.02f;

static BioSample readBioSim(){
  int p1 = analogRead(PIN_POT1);
  float hrBase = fmap(p1, 0, 4095, 55, 140);

  simPhase += 2.0f * PI * (hrBase / 60.0f) * simDt;
  if (simPhase > 2.0f * PI) simPhase -= 2.0f * PI;

  int p2 = analogRead(PIN_POT2);
  float motion = fmap(p2, 0, 4095, 0.0f, 1.0f);

  float spo2 = 99.0f - motion * 5.0f + (float)(random(-10, 11)) * 0.01f;

  float tAmb = dht.readTemperature();
  if (isnan(tAmb)) tAmb = 24.0f;
  float tBody = tAmb + 3.0f + 0.5f * motion;

  BioSample s;
  s.hr     = hrBase + (motion * 10.0f) * sin(simPhase);
  s.spo2   = spo2;
  s.tBody  = tBody;
  s.motion = motion;
  return s;
}
#else
static BioSample readBioSim(){ return {72, 98, 36.8, 0.1}; }
#endif

/* фільтри: MA5 + SG5 (буфер 64) */
static float hrBuf[64] = {0}, spBuf[64] = {0};
static int wpos = 0, wfill = 0;

static float ma5(const float* a, int n, int i){
  float s = 0; int c = 0;
  for (int d=-2; d<=2; d++){
    int j = i + d;
    if (j < 0) j = 0;
    if (j >= n) j = n-1;
    s += a[j]; c++;
  }
  return s / (float)c;
}

static float sg5(const float* a, int n, int i){
  const int w[5] = {-3, 12, 17, 12, -3};
  float s = 0; const float denom = 35.0f;
  for (int d=-2; d<=2; d++){
    int j = i + d;
    if (j < 0) j = 0;
    if (j >= n) j = n-1;
    s += (float)w[d+2] * a[j];
  }
  return s / denom;
}

static BioOut bioProcess(const BioSample& s){
  hrBuf[wpos] = s.hr;
  spBuf[wpos] = s.spo2;
  wpos = (wpos + 1) & 63;
  if (wfill < 64) wfill++;

  int last = (wfill > 0) ? (wfill - 1) : 0;

  float hrF = 0.5f * ma5(hrBuf, wfill, last) + 0.5f * sg5(hrBuf, wfill, last);
  float spF = 0.6f * ma5(spBuf, wfill, last) + 0.4f * sg5(spBuf, wfill, last);

  bool alarm = false;
  const char* why = "";

  if (spF < 92.5f) { alarm = true; why = "SpO2 low"; }
  if (s.motion > 0.8f) { alarm = true; why = "Motion artifact"; }
  if (hrF > 140.0f || hrF < 55.0f) { alarm = true; why = "HR out-of-range"; }

  return { hrF, spF, s.tBody, alarm, why };
}

static void medicalPublish(const BioSample& s, const BioOut& o){
  StaticJsonDocument<384> d;
  d["deviceId"] = deviceId;
  d["tBodyC"]   = o.tBody;
  d["hrBPM"]    = o.hrF;
  d["spo2"]     = o.spF;
  d["motion"]   = s.motion;
  d["anomaly"]  = o.alarm;
  d["reason"]   = o.why;

  String out;
  serializeJson(d, out);
  lastStatus = out;

  mqtt.publish(tMedTelemetry.c_str(), out.c_str());
  if (o.alarm) mqtt.publish(tMedAlerts.c_str(), out.c_str());
}

static void medicalDraw(const BioOut& o){
  ui.header(F("Medical"));
  u.setCursor(0, 32); u.print(F("HR:")); u.print(o.hrF, 0); u.print(F(" bpm"));
  u.setCursor(0, 48); u.print(F("SpO2:")); u.print(o.spF, 1); u.print(F("%"));
  u.setCursor(0, 64);
  if (o.alarm) { u.print(F("ALERT: ")); u.print(o.why); }
  else        { u.print(F("Status OK")); }

  oled.display();

  if (o.alarm) { digitalWrite(PIN_LED, HIGH); buzTone(1800); }
  else         { digitalWrite(PIN_LED, LOW);  buzOff(); }
}

static void onMqtt(char* topic, byte* payload, unsigned int len){
  StaticJsonDocument<256> doc;
  if (deserializeJson(doc, payload, len)) return;

  const char* cmd = doc["cmd"] | "";
  if (!strcmp(cmd, "Calibrate")){
    for (int i=0;i<64;i++){ hrBuf[i]=0; spBuf[i]=0; }
    wpos = 0; wfill = 0;
    buzTone(2200); delay(120); buzOff();
  }
}

#if PROJECT_MODE==PROJECT_TRANSPORT
struct TrTel { float tC, hum, co2, voc; int occ; bool an; const char* why; } tr;

enum GateState { G_IDLE, G_A, G_B };
static GateState gState = G_IDLE;
static int occupancy = 0;
static bool memA=false, memB=false;

static bool falling(bool& mem, bool sig){
  bool fell = (mem == true && sig == false);
  mem = sig;
  return fell;
}

static void occUpdate(){
  bool nearA = readUS_cm(TRIG_A, ECHO_A) < 80.0f;
  bool nearB = readUS_cm(TRIG_B, ECHO_B) < 80.0f;

  bool fallA = falling(memA, nearA);
  bool fallB = falling(memB, nearB);

  switch (gState){
    case G_IDLE:
      if (nearA && !nearB) gState = G_A;
      else if (nearB && !nearA) gState = G_B;
      break;
    case G_A:
      if (fallB) { occupancy++; gState = G_IDLE; }
      else if (fallA && !nearB) gState = G_IDLE;
      break;
    case G_B:
      if (fallA) { occupancy = max(0, occupancy-1); gState = G_IDLE; }
      else if (fallB && !nearA) gState = G_IDLE;
      break;
  }
}

static bool co2Spike(float c){
  static float buf[60];
  static int p=0, fill=0;

  buf[p] = c;
  p = (p + 1) % 60;
  if (fill < 60) fill++;

  if (fill < 10) return false;

  float m=0;
  for (int i=0;i<fill;i++) m += buf[i];
  m /= (float)fill;

  float v=0;
  for (int i=0;i<fill;i++){ float d = buf[i]-m; v += d*d; }
  float s = (fill>1) ? sqrt(v/(fill-1)) : 0.0f;

  float z = (s>1e-6f) ? (c-m)/s : 0.0f;
  return (fabs(z) > 2.5f) || (c > 1500.0f);
}

static void trDraw(){
  ui.header(F("Transport"));
  u.setCursor(0, 32); u.print(F("CO2:")); u.print(tr.co2, 0); u.print(F("ppm"));
  u.setCursor(0, 48); u.print(F("VOC:")); u.print(tr.voc, 0); u.print(F("ppb"));
  u.setCursor(0, 64); u.print(F("Occ:")); u.print(tr.occ);
  if (tr.an) u.print(F(" ALERT"));
  oled.display();
}

static void trPublish(){
  StaticJsonDocument<384> d;
  d["deviceId"]=deviceId; d["tC"]=tr.tC; d["hum"]=tr.hum;
  d["co2ppm"]=tr.co2; d["vocppb"]=tr.voc; d["occupancy"]=tr.occ;
  d["anomaly"]=tr.an; d["reason"]=tr.why;

  String out; serializeJson(d, out);
  lastStatus = out;

  mqtt.publish(tTrTelemetry.c_str(), out.c_str());
  if (tr.an) mqtt.publish(tTrAnomaly.c_str(), out.c_str());
}
#endif

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

  pinMode(PIN_BUZZER, OUTPUT);
  pinMode(PIN_LED, OUTPUT);
  pinMode(PIN_BTN_RST, INPUT_PULLUP);

  pinMode(TRIG_A, OUTPUT); pinMode(ECHO_A, INPUT);
  pinMode(TRIG_B, OUTPUT); pinMode(ECHO_B, INPUT);

  ledcAttachPin(PIN_BUZZER, BUZ_CH);

  dht.begin();
  Wire.begin(); // SDA=21, SCL=22

  if (!oled.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    while (1) delay(100);
  }
  u.begin(oled);

  mqtt.setServer(MQTT_HOST, MQTT_PORT);
  mqtt.setCallback(onMqtt);

  /* SAFE BOOT: 2с без WiFi/MQTT/HTTP */
  uint32_t t0 = millis();
  while (millis() - t0 < 2000) {
    ui.showBoot();
    delay(200);
  }

  net.kickWifi();
  uint32_t w0 = millis();
  while (WiFi.status() != WL_CONNECTED && millis() - w0 < 5000) {
    delay(100);
  }

  http.on("/status", httpStatus);
  http.begin();
}


void loop(){
  uint32_t now = millis();

  if (btn.fell(now)){
 
  }

  net.wifiWatch(now);
  net.mqttEnsure(now);

  mqtt.loop();
  http.handleClient();

#if PROJECT_MODE==PROJECT_MEDICAL
  static uint32_t tSample=0, tUi=0, tPub=0;
  static BioSample s;
  static BioOut out;

  if (now - tSample >= 20) { // 50 Hz
    tSample = now;
    s = readBioSim();
    out = bioProcess(s);
  }
  if (now - tUi >= 200) { // ~5 Hz
    tUi = now;
    medicalDraw(out);
  }
  if (now - tPub >= 2000) { // 2 s
    tPub = now;
    medicalPublish(s, out);
  }
#else
  static uint32_t tOcc=0, tRead=0, tUi=0;

  if (now - tOcc >= 250) { tOcc = now; occUpdate(); }

  if (now - tRead >= 1000) {
    tRead = now;
    float t = dht.readTemperature();
    float h = dht.readHumidity();
    if (isnan(t) || isnan(h)) { t = 0; h = 0; }

    int raw1 = analogRead(PIN_POT1);
    int raw2 = analogRead(PIN_POT2);

    tr.tC = t; tr.hum = h;
    tr.co2 = fmap(raw1, 0, 4095, 400, 5000);
    tr.voc = fmap(raw2, 0, 4095, 0, 1200);
    tr.occ = occupancy;

    tr.an = co2Spike(tr.co2);
    tr.why = tr.an ? ((tr.co2 > 1500) ? "CO2>1500" : "z-spike") : "";

    trPublish();
  }

  if (now - tUi >= 250) { tUi = now; trDraw(); }
#endif
}