/*
 * WiFiSense ESP32-S3 — OLED Edition
 * OLED: 0.96" SSD1306 128×64 I2C
 *   SDA → GPIO 6
 *   SCL → GPIO 7
 *
 * Thư viện cần cài (Library Manager):
 *   • Adafruit SSD1306   (by Adafruit)
 *   • Adafruit GFX Library (by Adafruit)
 */

#include <WiFi.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// ─── Cấu hình Wi-Fi ──────────────────────────────────────────────────────────
const char* ssid     = "OrangePiVietnam";
const char* password = "orangepi.vn";

// ─── OLED ────────────────────────────────────────────────────────────────────
#define OLED_WIDTH   128
#define OLED_HEIGHT   64
#define OLED_RESET    -1   // Dùng chân RESET của ESP32
#define OLED_ADDRESS  0x3C

#define OLED_SDA_PIN  6
#define OLED_SCL_PIN  7

Adafruit_SSD1306 display(OLED_WIDTH, OLED_HEIGHT, &Wire, OLED_RESET);

// ─── LED ─────────────────────────────────────────────────────────────────────
constexpr int     LED_PIN       = 2;
constexpr uint8_t LED_ON_LEVEL  = HIGH;
constexpr uint8_t LED_OFF_LEVEL = LOW;

// ─── Thuật toán ──────────────────────────────────────────────────────────────
constexpr size_t   WINDOW_SIZE             = 40;
constexpr size_t   LONG_WINDOW             = 100;
constexpr uint16_t SAMPLE_INTERVAL_MS      = 500;
constexpr uint16_t CALIBRATION_SAMPLES     = 200;
constexpr uint32_t WIFI_CONNECT_TIMEOUT_MS = 30'000;

constexpr float   PROCESS_NOISE            = 0.15f;
constexpr float   MEASUREMENT_NOISE        = 0.3f;
constexpr float   SMOOTH_ALPHA             = 0.15f;
constexpr float   ADAPTIVE_ALPHA           = 0.008f;
constexpr float   ADAPTIVE_BETA            = 0.005f;
constexpr float   SLOW_MOVEMENT_THRESHOLD  = 1.8f;
constexpr float   FAST_MOVEMENT_THRESHOLD  = 3.5f;
constexpr float   Z_SCORE_THRESHOLD        = 2.5f;
constexpr float   PEAK_THRESHOLD           = 2.0f;
constexpr uint8_t PERSISTENCE_REQUIRED     = 3;

// ─── Biến toàn cục ───────────────────────────────────────────────────────────
float  rssiWindow[WINDOW_SIZE];
float  rssiLongWindow[LONG_WINDOW];
size_t windowIndex     = 0;
size_t longWindowIndex = 0;
size_t sampleCount     = 0;

float kalmanEstimate  = -55.0f;
float kalmanError     = 1.0f;
float smoothedRssi    = -55.0f;
float baseline        = -55.0f;
float baselineVariance = 1.0f;

uint8_t  disturbanceCounter = 0;
uint32_t totalDetections    = 0;

// ─── Trạng thái màn hình ─────────────────────────────────────────────────────
// Xoay vòng giữa hai trang: trang 0 = chỉ số chính, trang 1 = chi tiết
enum DisplayPage { PAGE_MAIN = 0, PAGE_DETAIL = 1 };
DisplayPage currentPage     = PAGE_MAIN;
uint32_t    lastPageSwitch  = 0;
constexpr uint32_t PAGE_DURATION_MS = 3000; // chuyển trang mỗi 3 giây

// ═════════════════════════════════════════════════════════════════════════════
// Hàm tiện ích
// ═════════════════════════════════════════════════════════════════════════════

float clampValue(float value, float lower, float upper) {
  return value < lower ? lower : (value > upper ? upper : value);
}

size_t latestWindowIndex(size_t offset) {
  return (windowIndex + WINDOW_SIZE - offset) % WINDOW_SIZE;
}

// ─── Kalman & Smoothing ───────────────────────────────────────────────────────
float kalmanFilter(float measurement) {
  const float priorError = kalmanError + PROCESS_NOISE;
  const float gain = priorError / (priorError + MEASUREMENT_NOISE);
  kalmanEstimate += gain * (measurement - kalmanEstimate);
  kalmanError = (1.0f - gain) * priorError;
  return kalmanEstimate;
}

float exponentialSmoothing(float measurement) {
  smoothedRssi = SMOOTH_ALPHA * measurement + (1.0f - SMOOTH_ALPHA) * smoothedRssi;
  return smoothedRssi;
}

// ─── Thống kê ────────────────────────────────────────────────────────────────
float standardDeviation(const float* values, size_t count) {
  if (count == 0) return 0.0f;
  float mean = 0.0f;
  for (size_t i = 0; i < count; ++i) mean += values[i];
  mean /= static_cast<float>(count);
  float variance = 0.0f;
  for (size_t i = 0; i < count; ++i) {
    const float d = values[i] - mean;
    variance += d * d;
  }
  return sqrtf(variance / static_cast<float>(count));
}

float analyzeWindowVariance()    { return standardDeviation(rssiWindow,     WINDOW_SIZE); }
float analyzeLongTermVariance()  { return standardDeviation(rssiLongWindow, LONG_WINDOW); }

float detectRateOfChange() {
  if (sampleCount < 10) return 0.0f;
  float recentMean = 0.0f, oldMean = 0.0f;
  for (size_t i = 1; i <= 5; ++i) {
    recentMean += rssiWindow[latestWindowIndex(i)];
    oldMean    += rssiWindow[latestWindowIndex(i + 5)];
  }
  return fabsf((recentMean - oldMean) / 5.0f);
}

float detectPeak() {
  if (sampleCount < 3) return 0.0f;
  const float current      = rssiWindow[latestWindowIndex(1)];
  const float previous     = rssiWindow[latestWindowIndex(2)];
  const float older        = rssiWindow[latestWindowIndex(3)];
  const float currentDelta  = fabsf(current  - previous);
  const float previousDelta = fabsf(previous - older);
  return currentDelta > previousDelta + 1.0f ? currentDelta : 0.0f;
}

float calculateZScore() {
  if (sampleCount < WINDOW_SIZE) return 0.0f;
  float mean = 0.0f;
  for (size_t i = 0; i < WINDOW_SIZE; ++i) mean += rssiWindow[i];
  mean /= static_cast<float>(WINDOW_SIZE);
  const float stdDev  = fmaxf(analyzeWindowVariance(), 0.1f);
  const float current = rssiWindow[latestWindowIndex(1)];
  return fabsf((current - mean) / stdDev);
}

int calculateConfidence(float variance, float rateOfChange, float peak, float zScore) {
  float c = variance > FAST_MOVEMENT_THRESHOLD ? 75.0f
          : variance > SLOW_MOVEMENT_THRESHOLD ? 50.0f
                                               : 15.0f;
  if (rateOfChange > 2.0f)      c += 15.0f;
  if (peak > PEAK_THRESHOLD)    c += 10.0f;
  if (zScore > Z_SCORE_THRESHOLD) c += 10.0f;
  return static_cast<int>(clampValue(c, 0.0f, 100.0f));
}

const char* getMotionIntensity(float variance, float rateOfChange) {
  if (variance > FAST_MOVEMENT_THRESHOLD && rateOfChange > 2.5f) return "SPRINT";
  if (variance > FAST_MOVEMENT_THRESHOLD)                        return "FAST";
  if (variance > SLOW_MOVEMENT_THRESHOLD && rateOfChange > 1.0f) return "WALKING";
  if (variance > SLOW_MOVEMENT_THRESHOLD)                        return "SLOW";
  return "CALM";
}

void initializeRssiWindows(float initialRssi) {
  for (int i = 0; i < WINDOW_SIZE;  ++i) rssiWindow[i]     = initialRssi;
  for (int i = 0; i < LONG_WINDOW;  ++i) rssiLongWindow[i] = initialRssi;
  windowIndex     = 0;
  longWindowIndex = 0;
  sampleCount     = WINDOW_SIZE;
}

void setDetectionLed(bool detected) {
  digitalWrite(LED_PIN, detected ? LED_ON_LEVEL : LED_OFF_LEVEL);
}

// ═════════════════════════════════════════════════════════════════════════════
// OLED helpers
// ═════════════════════════════════════════════════════════════════════════════

/*
 * Vẽ thanh ngang [x,y] rộng maxW, chiều cao h, giá trị val trong [0,100]
 */
void drawBar(int x, int y, int maxW, int h, int val) {
  display.drawRect(x, y, maxW, h, WHITE);
  int fill = (int)((float)val / 100.0f * (maxW - 2));
  if (fill > 0) display.fillRect(x + 1, y + 1, fill, h - 2, WHITE);
}

/*
 * Trang 0: Thông tin chính
 * ┌──────────────────────────────┐
 * │ WiFiSense        [MOTION]    │  ← dòng tiêu đề + trạng thái
 * │ RSSI: -67 dBm   Conf: 85%   │
 * │ Conf [████████░░░░░░░]       │
 * │ Qual [██████████░░░░░]       │
 * │ Base:-65.3  Var:2.1  #12    │  ← dòng tóm tắt
 * └──────────────────────────────┘
 */
void drawPageMain(int rawRssi, int confidence, int quality,
                  float variance, bool detected) {
  display.clearDisplay();

  // ── Dòng 1: tiêu đề + trạng thái ──
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.print("WiFiSense");

  // Hộp trạng thái bên phải
  const char* status = detected ? " MOTION " : " CLEAR  ";
  int sx = 128 - 6 * strlen(status);
  if (detected) {
    display.fillRect(sx - 1, 0, 128 - sx + 1, 8, WHITE);
    display.setTextColor(BLACK);
  }
  display.setCursor(sx, 0);
  display.print(status);
  display.setTextColor(WHITE);

  // ── Dòng 2: RSSI & Conf số ──
  display.setCursor(0, 11);
  display.printf("RSSI:%4d dBm", rawRssi);
  display.setCursor(84, 11);
  display.printf("C:%d%%", confidence);

  // ── Thanh Confidence ──
  display.setCursor(0, 22);
  display.print("Conf");
  drawBar(26, 22, 102, 7, confidence);

  // ── Thanh Quality ──
  display.setCursor(0, 32);
  display.print("Qual");
  drawBar(26, 32, 102, 7, quality);

  // ── Dòng cuối: baseline, variance, tổng detections ──
  display.setCursor(0, 43);
  display.printf("Base:%.1f", baseline);
  display.setCursor(64, 43);
  display.printf("Var:%.1f", variance);
  display.setCursor(0, 54);
  display.printf("Detections: %lu", (unsigned long)totalDetections);

  display.display();
}

/*
 * Trang 1: Chi tiết kỹ thuật
 * ┌──────────────────────────────┐
 * │ --- Detail ---               │
 * │ Kalman: -66.43               │
 * │ Smooth: -66.21               │
 * │ ZScore:  1.87                │
 * │ Rate:    0.6  [WALKING]      │
 * └──────────────────────────────┘
 */
void drawPageDetail(float kalmanFiltered, float smoothFiltered,
                    float zScore, float rateOfChange, float variance) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);

  display.setCursor(0, 0);
  display.print("--- Detail ---");

  display.setCursor(0, 11);
  display.printf("Kalman: %.2f dBm", kalmanFiltered);

  display.setCursor(0, 21);
  display.printf("Smooth: %.2f dBm", smoothFiltered);

  display.setCursor(0, 31);
  display.printf("ZScore: %.2f", zScore);

  display.setCursor(0, 41);
  display.printf("Rate:   %.1f", rateOfChange);

  // Nhãn cường độ chuyển động ở góc phải dưới
  const char* intensity = getMotionIntensity(variance, rateOfChange);
  int lx = 128 - 6 * (int)strlen(intensity);
  display.setCursor(lx, 41);
  display.print(intensity);

  // Trang số nhỏ
  display.setCursor(110, 56);
  display.print("2/2");

  display.display();
}

// ── Màn hình khởi động ────────────────────────────────────────────────────────
void oledSplash() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(16, 10);
  display.print("WiFiSense v2");
  display.setCursor(10, 22);
  display.print("ESP32-S3 + OLED");
  display.setCursor(4, 36);
  display.print("orangepi.vn");
  display.display();
  delay(1500);
}

// ── Màn hình trạng thái kết nối Wi-Fi ────────────────────────────────────────
void oledStatus(const char* msg, bool clear = true) {
  if (clear) display.clearDisplay();
  // Cuộn lên: in ở dòng cuối rồi scroll
  display.setTextSize(1);
  display.setTextColor(WHITE);
  // Tìm dòng trống kế tiếp (đơn giản: in lên dòng 56)
  display.setCursor(0, 56);
  display.print(msg);
  display.display();
}

// ── Màn hình log cuộn (dùng trong khi kết nối / calibrate) ───────────────────
#define LOG_LINES 8
char logBuf[LOG_LINES][22]; // 21 ký tự + null
uint8_t logHead = 0;

void logPush(const char* line) {
  strncpy(logBuf[logHead], line, 21);
  logBuf[logHead][21] = '\0';
  logHead = (logHead + 1) % LOG_LINES;
}

void logRender() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  for (int i = 0; i < LOG_LINES; ++i) {
    int idx = (logHead + i) % LOG_LINES;
    display.setCursor(0, i * 8);
    display.print(logBuf[idx]);
  }
  display.display();
}

// ═════════════════════════════════════════════════════════════════════════════
// Wi-Fi & Calibration (với OLED log)
// ═════════════════════════════════════════════════════════════════════════════

bool connectToWifi() {
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);

  char tmp[22];
  snprintf(tmp, sizeof(tmp), "Connecting...");
  logPush(tmp);
  logRender();

  Serial.print("Connecting to Wi-Fi");
  const uint32_t startedAt = millis();
  uint8_t dotCount = 0;

  while (WiFi.status() != WL_CONNECTED) {
    if (millis() - startedAt >= WIFI_CONNECT_TIMEOUT_MS) {
      logPush("Timeout! Restart");
      logRender();
      Serial.println("\nTimeout. Restarting...");
      return false;
    }
    delay(500);
    Serial.print('.');

    dotCount++;
    if (dotCount % 10 == 0) {
      snprintf(tmp, sizeof(tmp), "Wait %lus...", (millis() - startedAt) / 1000UL);
      logPush(tmp);
      logRender();
    }
  }

  snprintf(tmp, sizeof(tmp), "IP:%s", WiFi.localIP().toString().c_str());
  logPush(tmp);
  snprintf(tmp, sizeof(tmp), "RSSI:%d dBm", WiFi.RSSI());
  logPush(tmp);
  logRender();

  Serial.printf("\nConnected. IP: %s, RSSI: %d dBm\n",
                WiFi.localIP().toString().c_str(), WiFi.RSSI());
  delay(800);
  return true;
}

void calibrate() {
  char tmp[22];
  snprintf(tmp, sizeof(tmp), "Calibrating %u smpl", CALIBRATION_SAMPLES);
  logPush(tmp);
  logPush("Keep room EMPTY!");
  logRender();

  Serial.printf("Calibrating %u samples; keep room empty...\n", CALIBRATION_SAMPLES);

  float calibrationMean = 0.0f;
  for (uint16_t i = 0; i < CALIBRATION_SAMPLES; ++i) {
    calibrationMean += static_cast<float>(WiFi.RSSI());
    delay(30);

    // Cập nhật tiến trình mỗi 25 mẫu
    if (i % 25 == 0) {
      snprintf(tmp, sizeof(tmp), "Cal %d/%d", i, CALIBRATION_SAMPLES);
      logPush(tmp);
      logRender();
    }
  }

  baseline        = calibrationMean / static_cast<float>(CALIBRATION_SAMPLES);
  kalmanEstimate  = baseline;
  smoothedRssi    = baseline;
  initializeRssiWindows(baseline);

  snprintf(tmp, sizeof(tmp), "Base: %.2f dBm", baseline);
  logPush(tmp);
  logPush("Cal done! Starting.");
  logRender();
  delay(1000);

  Serial.printf("Calibration done. Baseline: %.2f dBm\n", baseline);
}

// ═════════════════════════════════════════════════════════════════════════════
// setup() & loop()
// ═════════════════════════════════════════════════════════════════════════════

void setup() {
  Serial.begin(115200);
  pinMode(LED_PIN, OUTPUT);
  setDetectionLed(false);

  // Khởi OLED trước tiên
  Wire.begin(OLED_SDA_PIN, OLED_SCL_PIN);
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) {
    Serial.println("SSD1306 init failed — check wiring!");
    // Không dừng, tiếp tục chạy với Serial
  }
  display.cp437(true);
  display.setTextWrap(false);

  oledSplash();

  Serial.println("\n========== WiFiSense ESP32-S3 OLED ==========");

  if (!connectToWifi()) {
    delay(2'000);
    ESP.restart();
  }
  calibrate();

  Serial.println("Running. RSSI metrics active.");
  display.clearDisplay();
  display.display();
  lastPageSwitch = millis();
}

void loop() {
  // ── Reconnect nếu mất kết nối ──────────────────────────────────────────
  if (WiFi.status() != WL_CONNECTED) {
    setDetectionLed(false);
    logPush("WiFi lost!");
    logPush("Reconnecting...");
    logRender();
    if (!connectToWifi()) {
      delay(2'000);
    }
    return;
  }

  // ── Lấy mẫu ────────────────────────────────────────────────────────────
  const int   rawRssi        = WiFi.RSSI();
  const float kalmanFiltered = kalmanFilter(static_cast<float>(rawRssi));
  const float smoothFiltered = exponentialSmoothing(kalmanFiltered);

  rssiWindow[windowIndex]       = smoothFiltered;
  rssiLongWindow[longWindowIndex] = smoothFiltered;
  windowIndex     = (windowIndex + 1) % WINDOW_SIZE;
  longWindowIndex = (longWindowIndex + 1) % LONG_WINDOW;
  ++sampleCount;

  // ── Tính toán ───────────────────────────────────────────────────────────
  const float variance      = analyzeWindowVariance();
  const float longVariance  = analyzeLongTermVariance();
  const float rateOfChange  = detectRateOfChange();
  const float peak          = detectPeak();
  const float zScore        = calculateZScore();
  const int   confidence    = calculateConfidence(variance, rateOfChange, peak, zScore);
  const float signalQuality = clampValue(100.0f - longVariance * 20.0f, 0.0f, 100.0f);

  baseline        = baseline        * (1.0f - ADAPTIVE_ALPHA) + smoothFiltered * ADAPTIVE_ALPHA;
  baselineVariance = baselineVariance * (1.0f - ADAPTIVE_BETA)  + variance * ADAPTIVE_BETA;

  const bool motion = variance > SLOW_MOVEMENT_THRESHOLD ||
                      (zScore > Z_SCORE_THRESHOLD && peak > PEAK_THRESHOLD);
  disturbanceCounter = motion ? disturbanceCounter + 1 : 0;

  const bool detected = disturbanceCounter >= PERSISTENCE_REQUIRED;
  if (detected && disturbanceCounter == PERSISTENCE_REQUIRED) {
    ++totalDetections;
  }
  setDetectionLed(detected);

  // ── Hiển thị OLED — xoay trang ─────────────────────────────────────────
  // if (millis() - lastPageSwitch >= PAGE_DURATION_MS) {
  //  currentPage    = (currentPage == PAGE_MAIN) ? PAGE_DETAIL : PAGE_MAIN;
  //  lastPageSwitch = millis();
  // }

  if (currentPage == PAGE_MAIN) {
    drawPageMain(rawRssi, confidence, (int)signalQuality, variance, detected);
  } else {
    drawPageDetail(kalmanFiltered, smoothFiltered, zScore, rateOfChange, variance);
  }

  // ── Serial (vẫn giữ để debug) ───────────────────────────────────────────
  Serial.printf(
      "Raw:%d,Kalman:%.2f,Smooth:%.2f,Baseline:%.2f,Var:%.2f,Z:%.2f,Conf:%d\n",
      rawRssi, kalmanFiltered, smoothFiltered, baseline, variance, zScore, confidence);
  Serial.printf(
      "[%s] Conf:%d%% | Qual:%d%% | Rate:%.1f | Total:%lu\n",
      getMotionIntensity(variance, rateOfChange), confidence,
      (int)signalQuality, rateOfChange, (unsigned long)totalDetections);

  delay(SAMPLE_INTERVAL_MS);
}