// Wearable Health Monitoring (ESP32)
// Template ID di Blynk: TMPL6429nNFzr
// Virtual pins:
//   V0 = HR (float)
//   V1 = Glucose (float)
//   V2 = Pitch (float)
//   V4 = Status text + color (Label/Value Display)  <-- menggunakan V4 sekarang
//   V10 = Status numeric (0=Normal,1=Alert,2=Emergency)
//   V20 = Reset alarm button (write, 1 = pressed) (auto-reset to 0)
// Replace WIFI_SSID, WIFI_PASS, and BLYNK_AUTH_TOKEN with your own values

#define BLYNK_TEMPLATE_ID "TMPL6429nNFzr"
#define BLYNK_TEMPLATE_NAME "Wearable Health Monitoring"
#define BLYNK_AUTH_TOKEN "wGuXdcNaEjjI331x0lhCKX_Uu7f58o9g"

#include <Wire.h>
#include <WiFi.h>
#include <BlynkSimpleEsp32.h>
#include "MPU6050.h"
#include <math.h>

///// CONFIG /////
const char ssid[] = "Wokwi-GUEST";
const char pass[] = "";

// Pins (hardware)
const int PIN_POT_HR    = 35; // ADC1_CH7 (input only)
const int PIN_POT_GLUC  = 34; // ADC1_CH6 (input only)
const int PIN_LED       = 13;
const int PIN_BUZZER    = 12;
const int SDA_PIN       = 21;
const int SCL_PIN       = 22;

MPU6050 mpu;

// Timing
unsigned long lastSend = 0;
const unsigned long SEND_INTERVAL = 1000; // ms

// EMA smoothing
float alpha = 0.12f;
float hr_smoothed = 0;
float gluc_smoothed = 0;
float pitch_smoothed = 0;

// Thresholds & hysteresis (adjust saat kalibrasi)
const float HR_ALERT = 100.0;
const float HR_EMERGENCY = 140.0;
const float HR_ALERT_LOW = 95.0;

const float GLUC_ALERT = 70.0;      // mapped unit
const float GLUC_EMERGENCY = 40.0;
const float GLUC_ALERT_LOW = 75.0;

const float PITCH_ALERT = 45.0;     // degrees
const float PITCH_EMERGENCY = 70.0;
const float PITCH_ALERT_LOW = 40.0;

const unsigned long T_WINDOW = 2000; // ms sustained required

enum State { NORMAL=0, ALERT=1, EMERGENCY=2 };
State currentState = NORMAL;

unsigned long hr_cond_start = 0;
unsigned long gluc_cond_start = 0;
unsigned long pitch_cond_start = 0;

bool hr_condition = false;
bool gluc_condition = false;
bool pitch_condition = false;

// Helper: map float like map() but returns float
float fmap(float x, float in_min, float in_max, float out_min, float out_max){
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// --- Blynk status UI helpers ---
// Colors (hex): adjust jika mau
const char* COLOR_NORMAL = "#00C853";   // hijau
const char* COLOR_ALERT  = "#FFAB00";   // oranye
const char* COLOR_EMERG  = "#D50000";   // merah

// Update label text + color on Blynk (V4 used for status widget)
void updateBlynkStatusUI(State s) {
  String txt;
  const char* color = COLOR_NORMAL;
  switch (s) {
    case NORMAL:    txt = "NORMAL";    color = COLOR_NORMAL; break;
    case ALERT:     txt = "PERINGATAN";color = COLOR_ALERT;  break;
    case EMERGENCY: txt = "DARURAT";   color = COLOR_EMERG;  break;
  }
  Blynk.virtualWrite(V4, txt);             // tulis teks ke V4
  Blynk.setProperty(V4, "color", color);   // ubah warna widget V4
}

// Optional: log event for emergency (configure Event "emergency_alert" in Blynk dashboard)
void notifyIfNeeded(State s) {
  if (s == EMERGENCY) {
    Blynk.logEvent("emergency_alert", "⚠️ DARURAT terdeteksi pada wearable!");
  }
}

// ---------------- Non-blocking alarm state ----------------
// Alarm control (non-blocking)
bool alarmActive = false;
int alarmMode = 0; // 0 = none, 1 = alert pattern, 2 = emergency pattern
unsigned long alarmStart = 0;
unsigned long lastAlarmToggle = 0;
int alarmPhase = 0; // toggle phase counter

// Set alarm mode (called from handleStateChange)
void setAlarmMode(int mode) {
  alarmMode = mode;
  alarmActive = (mode != 0);
  alarmStart = millis();
  lastAlarmToggle = 0;
  alarmPhase = 0;
  if (!alarmActive) {
    digitalWrite(PIN_LED, LOW);
    digitalWrite(PIN_BUZZER, LOW);
  }
}

// Non-blocking alarm handler: panggil di loop()
void handleAlarm() {
  if (!alarmActive) return;
  unsigned long now = millis();

  if (alarmMode == 1) { // ALERT: short 3-beep/blink bursts with pauses
    // pattern: ON 120ms / OFF 120ms - 3 cycles, then pause 2000ms
    unsigned long phaseDur = (alarmPhase % 2 == 0) ? 120 : 120;
    if (now - lastAlarmToggle >= phaseDur) {
      lastAlarmToggle = now;
      bool on = (alarmPhase % 2 == 0);
      digitalWrite(PIN_LED, on ? HIGH : LOW);
      digitalWrite(PIN_BUZZER, on ? HIGH : LOW);
      alarmPhase++;
      if (alarmPhase >= 6) { // finished 3 cycles (on/off x3)
        // wait for 2000ms total since alarmStart, then restart pattern
        if (now - alarmStart >= 2000) {
          alarmStart = now;
          alarmPhase = 0;
          lastAlarmToggle = now;
        } else {
          digitalWrite(PIN_LED, LOW);
          digitalWrite(PIN_BUZZER, LOW);
        }
      }
    }
  } else if (alarmMode == 2) { // EMERGENCY: continuous fast pattern
    unsigned long phaseDur = (alarmPhase % 2 == 0) ? 140 : 80;
    if (now - lastAlarmToggle >= phaseDur) {
      lastAlarmToggle = now;
      bool on = (alarmPhase % 2 == 0);
      digitalWrite(PIN_LED, on ? HIGH : LOW);
      digitalWrite(PIN_BUZZER, on ? HIGH : LOW);
      alarmPhase++;
      // continuous until muted/reset
    }
  }
}

// ---------------- sensor reads & state machine ----------------
void setup() {
  Serial.begin(115200);
  delay(100);
  Wire.begin(SDA_PIN, SCL_PIN);

  // ADC config: resolusi dan atenuasi agar analogRead menghasilkan 0..4095 untuk 0..3.3V
  analogReadResolution(12); // 12-bit
  analogSetPinAttenuation(PIN_POT_HR, ADC_11db);
  analogSetPinAttenuation(PIN_POT_GLUC, ADC_11db);

  pinMode(PIN_LED, OUTPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  digitalWrite(PIN_LED, LOW);
  digitalWrite(PIN_BUZZER, LOW);

  // MPU6050 init
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
  } else {
    Serial.println("MPU6050 ready.");
  }

  // Blynk connection
  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);

  // debug: print status
  Serial.print("WiFi SSID: "); Serial.println(WiFi.SSID());
  Serial.print("Blynk connected: "); Serial.println(Blynk.connected() ? "YES" : "NO");

  // init smoothing with first reads
  hr_smoothed = readHR_raw();
  gluc_smoothed = readGlucose_raw();
  pitch_smoothed = readPitch_raw();
  lastSend = millis();

  // Ensure UI shows initial state
  updateBlynkStatusUI(currentState);
  Blynk.virtualWrite(V10, (int)currentState);
}

// main loop
void loop() {
  Blynk.run();
  unsigned long now = millis();

  // Read sensors
  float hr = readHR_raw();
  float gluc = readGlucose_raw();
  float pitch = readPitch_raw();

  // EMA smoothing
  hr_smoothed = alpha * hr + (1.0 - alpha) * hr_smoothed;
  gluc_smoothed = alpha * gluc + (1.0 - alpha) * gluc_smoothed;
  pitch_smoothed = alpha * pitch + (1.0 - alpha) * pitch_smoothed;

  // Detection with hysteresis + time window
  // HR high
  if (hr_smoothed >= HR_ALERT) {
    if (!hr_condition) { hr_cond_start = now; hr_condition = true; }
  } else {
    if (hr_smoothed <= HR_ALERT_LOW) { hr_condition = false; hr_cond_start = 0; }
  }

  // Glucose low
  if (gluc_smoothed <= GLUC_ALERT) {
    if (!gluc_condition) { gluc_cond_start = now; gluc_condition = true; }
  } else {
    if (gluc_smoothed >= GLUC_ALERT_LOW) { gluc_condition = false; gluc_cond_start = 0; }
  }

  // Pitch tilt/fall
  if (fabs(pitch_smoothed) >= PITCH_ALERT) {
    if (!pitch_condition) { pitch_cond_start = now; pitch_condition = true; }
  } else {
    if (fabs(pitch_smoothed) <= PITCH_ALERT_LOW) { pitch_condition = false; pitch_cond_start = 0; }
  }

  // Update finite-state
  updateState();

  // Periodic send to Blynk
  if (now - lastSend >= SEND_INTERVAL) {
    lastSend = now;
    Blynk.virtualWrite(V0, hr_smoothed);
    Blynk.virtualWrite(V1, gluc_smoothed);
    Blynk.virtualWrite(V2, pitch_smoothed);
    // Numeric state goes to V10 (avoid collision with V4 text)
    Blynk.virtualWrite(V10, (int)currentState);
    Serial.printf("HR: %.1f | Gluc: %.1f | Pitch: %.1f | State: %d\n", hr_smoothed, gluc_smoothed, pitch_smoothed, (int)currentState);
  }

  // Non-blocking alarm handler (keputusan ON/OFF lakukan di sini)
  handleAlarm();

  // small yield (helps WiFi and background tasks)
  delay(1);
}

// ----- sensor read functions -----
// Proxy HR: potensiometer read -> map to 40..180 bpm
float readHR_raw() {
  int raw = analogRead(PIN_POT_HR); // 0..4095
  float bpm = fmap(raw, 0.0, 4095.0, 40.0, 180.0);
  return bpm;
}
// Proxy Glucose: potensiometer read -> map to 20..200 units
float readGlucose_raw() {
  int raw = analogRead(PIN_POT_GLUC);
  float g = fmap(raw, 0.0, 4095.0, 20.0, 200.0);
  return g;
}
// Pitch from MPU6050 (accel-based estimate)
float readPitch_raw() {
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);
  float fx = ax / 16384.0;
  float fy = ay / 16384.0;
  float fz = az / 16384.0;
  float pitch = atan2(fx, sqrt(fy*fy + fz*fz)) * 180.0 / PI;
  return pitch;
}

// ----- state machine & handlers -----
void updateState() {
  unsigned long now = millis();
  bool hr_sustained = hr_condition && (now - hr_cond_start >= T_WINDOW);
  bool gluc_sustained = gluc_condition && (now - gluc_cond_start >= T_WINDOW);
  bool pitch_sustained = pitch_condition && (now - pitch_cond_start >= T_WINDOW);

  State newState = NORMAL;

  // emergency immediate thresholds
  if (hr_smoothed >= HR_EMERGENCY || gluc_smoothed <= GLUC_EMERGENCY || fabs(pitch_smoothed) >= PITCH_EMERGENCY) {
    newState = EMERGENCY;
  } else {
    int sustainedCount = (hr_sustained?1:0) + (gluc_sustained?1:0) + (pitch_sustained?1:0);
    if (sustainedCount >= 2) newState = EMERGENCY;
    else if (hr_sustained || gluc_sustained || pitch_sustained) newState = ALERT;
    else newState = NORMAL;
  }

  if (newState != currentState) {
    handleStateChange(newState);
    currentState = newState;
  }
}

void handleStateChange(State newState) {
  String detail = "";
  switch (newState) {
    case NORMAL:
      detail = "Normal";
      setAlarmMode(0); // matikan alarm
      break;
    case ALERT:
      detail = "Alert:";
      if (hr_condition) detail += " HR";
      if (gluc_condition) detail += " Gluc";
      if (pitch_condition) detail += " Pitch";
      setAlarmMode(1); // pattern pendek
      break;
    case EMERGENCY:
      detail = "Emergency:";
      if (hr_smoothed >= HR_EMERGENCY) detail += " HR_HIGH";
      if (gluc_smoothed <= GLUC_EMERGENCY) detail += " GLUC_LOW";
      if (fabs(pitch_smoothed) >= PITCH_EMERGENCY) detail += " FALL";
      setAlarmMode(2); // pattern continuous
      break;
  }

  // Update UI + numeric state
  updateBlynkStatusUI(newState);
  Blynk.virtualWrite(V10, (int)newState);
  Serial.println(detail);

  // notify via event if needed
  notifyIfNeeded(newState);
}

// ----- Blynk handlers -----
// Tombol reset alarm di dashboard (V20). Jika ditekan (1), matikan alarm (acknowledge).
// Auto-reset switch ke OFF setelah ditekan.
BLYNK_WRITE(V20) {
  int v = param.asInt();
  if (v == 1) {
    setAlarmMode(0);
    // auto-reset the dashboard switch back to OFF (0)
    Blynk.virtualWrite(V20, 0);
    updateBlynkStatusUI(currentState);
    Blynk.virtualWrite(V10, (int)currentState);
    Serial.println("Alarm reset via dashboard (V20) - auto-reset switch.");
  }
}