// Laundry monitor (low power): XIAO ESP32-C6 + MPU6050 -> Zigbee Binary Input (ZHA)
//
// Duty cycle (deep sleep between polls; Zigbee is only started when we need to report):
//   IDLE     wake every IDLE_POLL_SEC, take one vibration reading.
//            If active, run a short validation loop; if confirmed -> report ON.
//   RUNNING  wake every RUN_POLL_SEC. If still active, stay RUNNING.
//            If quiet, drop into QUIET.
//   QUIET    wake every QUIET_POLL_SEC. If activity returns -> RUNNING.
//            After QUIET_STOP_SEC of continuous quiet -> report OFF, back to IDLE.
//   A heartbeat re-reports the current state every HEARTBEAT_SEC so ZHA keeps the
//   device "available".
//
// Arduino IDE setup (esp32 core 3.2+):
//   Board:            XIAO_ESP32C6
//   Zigbee Mode:      Zigbee end device
//   Partition Scheme: Zigbee 4MB with spiffs
//   (USB CDC On Boot: Enabled if you want Serial output over USB)
//
// Wiring: MPU6050 VCC->3V3, GND->GND, SDA->D4 (GPIO22), SCL->D5 (GPIO23)
//
// Set TUNING_MODE 1 first: it stays awake, skips Zigbee and prints vibration levels
// so you can pick START_THRESHOLD_G for your machines.

#ifndef ZIGBEE_MODE_ED
#error "Select Tools > Zigbee Mode > Zigbee end device"
#endif

#include "Zigbee.h"
#include <Wire.h>
#include "esp_sleep.h"

#define TUNING_MODE        0
#define USE_BATTERY_SOURCE 0   // 1 = advertise battery power source (some core versions crashed with
                               // binary input + battery; leave 0 unless you need it)
#define BOOT_BUTTON        9   // hold ~3 s while resetting/powering up to factory reset Zigbee

// ---- Tuning ----
const float    START_THRESHOLD_G  = 0.02f;  // vibration std-dev (g) counted as "active"
const int      WINDOW_MS          = 3000;   // length of one measurement window
const int      SAMPLE_US          = 10000;  // ~100 Hz sampling

const uint32_t IDLE_POLL_SEC      = 60;     // poll rate while off
const uint32_t RUN_POLL_SEC       = 180;    // poll rate while running
const uint32_t QUIET_POLL_SEC     = 30;     // faster polling once quiet is first seen
const uint32_t QUIET_STOP_SEC     = 120;    // continuous quiet before reporting OFF
const uint32_t HEARTBEAT_SEC      = 1800;   // re-report state at least this often

const int      VALIDATE_WINDOWS   = 6;      // validation loop: 6 windows (~18 s) ...
const int      VALIDATE_NEEDED    = 4;      // ... at least 4 must be active

const uint32_t JOIN_TIMEOUT_MS_FIRST = 180000; // first boot: time to put coordinator in pairing mode
const uint32_t JOIN_TIMEOUT_MS       = 20000;  // normal wake: give up and retry on next wake

const uint8_t MPU_ADDR = 0x68;

enum : uint8_t { ST_IDLE = 0, ST_RUNNING = 1, ST_QUIET = 2 };

// Survives deep sleep (reset on power loss)
RTC_DATA_ATTR uint8_t  state          = ST_IDLE;
RTC_DATA_ATTR uint32_t quietSec       = 0;
RTC_DATA_ATTR uint32_t sinceReportSec = 0;
RTC_DATA_ATTR bool     needReport     = true;   // first boot reports initial OFF

#define BINARY_DEVICE_ENDPOINT_NUMBER 1
ZigbeeBinary zbBinary = ZigbeeBinary(BINARY_DEVICE_ENDPOINT_NUMBER);

// ---------------------------------------------------------------- MPU6050
void mpuWrite(uint8_t reg, uint8_t val) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.write(val);
  Wire.endTransmission();
}

void mpuWake() {
  mpuWrite(0x6B, 0x00);   // clear SLEEP, internal 8 MHz clock
  mpuWrite(0x6C, 0x07);   // put gyros in standby, we only need the accelerometer
  delay(50);              // let the accelerometer settle
}

void mpuSleep() {
  mpuWrite(0x6B, 0x40);   // SLEEP bit
}

bool readAccel(float &ax, float &ay, float &az) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom(MPU_ADDR, (uint8_t)6) != 6) return false;
  int16_t x = (Wire.read() << 8) | Wire.read();
  int16_t y = (Wire.read() << 8) | Wire.read();
  int16_t z = (Wire.read() << 8) | Wire.read();
  ax = x / 16384.0f;   // +/-2 g range
  ay = y / 16384.0f;
  az = z / 16384.0f;
  return true;
}

// Std-dev of acceleration magnitude over one window (orientation independent).
float measureVibration() {
  double sum = 0, sumSq = 0;
  int n = 0;
  uint32_t start = millis();
  while (millis() - start < (uint32_t)WINDOW_MS) {
    float ax, ay, az;
    if (readAccel(ax, ay, az)) {
      float m = sqrtf(ax * ax + ay * ay + az * az);
      sum += m;
      sumSq += (double)m * m;
      n++;
    }
    delayMicroseconds(SAMPLE_US);
  }
  if (n < 20) return 0;
  double mean = sum / n;
  double var = sumSq / n - mean * mean;
  return var > 0 ? sqrt(var) : 0;
}

bool validateRunning() {
  int active = 0;
  for (int i = 0; i < VALIDATE_WINDOWS; i++) {
    float v = measureVibration();
    if (v > START_THRESHOLD_G) active++;
    Serial.printf("  validate %d/%d: %.4f g (active so far: %d)\n", i + 1, VALIDATE_WINDOWS, v, active);
  }
  return active >= VALIDATE_NEEDED;
}

// ---------------------------------------------------------------- Zigbee
void zigbeeInit() {
  zbBinary.setManufacturerAndModel("DIY", "LaundryMonitor");
  zbBinary.addBinaryInput();
  zbBinary.setBinaryInputApplication(BINARY_INPUT_APPLICATION_TYPE_HVAC_FAN_STATUS); // cosmetic
  zbBinary.setBinaryInputDescription("Running");
#if USE_BATTERY_SOURCE
  zbBinary.setPowerSource(ZB_POWER_SOURCE_BATTERY, 100);
#endif
  Zigbee.addEndpoint(&zbBinary);

  esp_zb_cfg_t cfg = ZIGBEE_DEFAULT_ED_CONFIG();
  cfg.nwk_cfg.zed_cfg.keep_alive = 10000;
  // false = keep network credentials in NVS so we rejoin after deep sleep
  if (!Zigbee.begin(&cfg, false)) {
    Serial.println("Zigbee failed to start, rebooting");
    ESP.restart();
  }
}

bool sendState(bool on, uint32_t timeoutMs) {
  uint32_t t = millis();
  while (!Zigbee.connected()) {
    if (millis() - t > timeoutMs) {
      Serial.println("Zigbee join/rejoin timed out");
      return false;
    }
    delay(100);
  }
  zbBinary.setBinaryInput(on);
  zbBinary.reportBinaryInput();
  Serial.printf("Reported: %s\n", on ? "RUNNING" : "OFF");
  delay(1000);   // give the radio time to send and get the ACK before we sleep
  return true;
}

// ---------------------------------------------------------------- main
void goToSleep(uint32_t sec) {
  Serial.printf("Sleeping %lu s (state=%d)\n", (unsigned long)sec, state);
  Serial.flush();
  mpuSleep();
  esp_sleep_enable_timer_wakeup((uint64_t)sec * 1000000ULL);
  esp_deep_sleep_start();
}

void setup() {
  Serial.begin(115200);
  delay(200);
  pinMode(BOOT_BUTTON, INPUT_PULLUP);

  Wire.begin();   // XIAO C6 default: SDA=GPIO22, SCL=GPIO23

  // Factory reset: hold BOOT for 3 s at power-up / reset
  if (digitalRead(BOOT_BUTTON) == LOW) {
    uint32_t t = millis();
    while (digitalRead(BOOT_BUTTON) == LOW && millis() - t < 3000) delay(50);
    if (millis() - t >= 3000) {
      Serial.println("Factory resetting Zigbee...");
      zigbeeInit();
      Zigbee.factoryReset();   // reboots
    }
  }

  mpuWake();

#if TUNING_MODE
  Serial.println("TUNING MODE: printing vibration (g std-dev) every window");
  while (true) {
    float v = measureVibration();
    Serial.printf("vibration=%.4f g  %s\n", v, v > START_THRESHOLD_G ? "ACTIVE" : "quiet");
  }
#endif

  bool coldBoot = (esp_sleep_get_wakeup_cause() == ESP_SLEEP_WAKEUP_UNDEFINED);

  float v = measureVibration();
  bool active = v > START_THRESHOLD_G;
  Serial.printf("wake: vibration=%.4f g active=%d state=%d\n", v, active, state);

  uint32_t sleepSec = IDLE_POLL_SEC;
  bool changed = false;

  switch (state) {
    case ST_IDLE:
      sleepSec = IDLE_POLL_SEC;
      if (active && validateRunning()) {
        state = ST_RUNNING;
        sleepSec = RUN_POLL_SEC;
        changed = true;
      }
      break;

    case ST_RUNNING:
      if (active) {
        sleepSec = RUN_POLL_SEC;
      } else {
        state = ST_QUIET;
        quietSec = 0;
        sleepSec = QUIET_POLL_SEC;
      }
      break;

    case ST_QUIET:
      if (active) {
        state = ST_RUNNING;
        quietSec = 0;
        sleepSec = RUN_POLL_SEC;
      } else {
        quietSec += QUIET_POLL_SEC;
        if (quietSec >= QUIET_STOP_SEC) {
          state = ST_IDLE;
          quietSec = 0;
          sleepSec = IDLE_POLL_SEC;
          changed = true;
        } else {
          sleepSec = QUIET_POLL_SEC;
        }
      }
      break;
  }

  if (changed) needReport = true;
  if (!needReport) {
    sinceReportSec += sleepSec;
    if (sinceReportSec >= HEARTBEAT_SEC) needReport = true;   // heartbeat
  }

  if (needReport) {
    bool on = (state != ST_IDLE);   // QUIET still counts as running until the timeout
    zigbeeInit();
    if (sendState(on, coldBoot ? JOIN_TIMEOUT_MS_FIRST : JOIN_TIMEOUT_MS)) {
      needReport = false;
      sinceReportSec = 0;
    }   // on failure needReport stays set and we retry on the next wake
  }

  goToSleep(sleepSec);
}

void loop() {
  // never reached: setup() ends in deep sleep
}