/*
  TholusFlow v2 - single-file firmware
  Target: ESP32 + VL53L8CH
  IDE: Arduino IDE

  Required libraries:
  - ArduinoJson
  - arduinoWebSockets (Links2004)
  - VL53L8CH (stm32duino)
*/

#include <WiFi.h>
#include <WebServer.h>
#include <Preferences.h>
#include <ESPmDNS.h>
#include <ArduinoJson.h>
#include <Wire.h>
#include <vl53l8ch.h>
#include <WebSocketsServer.h>
#include <HTTPClient.h>
#include <math.h>

// ============================================================
// BUILD INFO
// ============================================================
static const char* FW_VERSION = "2.0.1";
static const int PROTOCOL_VERSION = 2;

// ============================================================
// PINS / SENSOR
// ============================================================
static const int I2C_SDA_PIN = 21;
static const int I2C_SCL_PIN = 22;
static const int SENSOR_FREQ_HZ = 15;
static const uint8_t SENSOR_RESOLUTION = VL53LMZ_RESOLUTION_8X8;

// ============================================================
// NETWORK / HTTP / WS
// ============================================================
WebServer server(80);
WebSocketsServer webSocket(81);
Preferences preferences;
VL53L8CH sensor(&Wire, -1, -1);

TaskHandle_t SensorTaskHandle = nullptr;
QueueHandle_t eventQueue = nullptr;

const char* HEADER_KEYS[] = {"Authorization"};
const size_t HEADER_KEYS_COUNT = 1;

// ============================================================
// CONFIG / STATE
// ============================================================
struct RuntimeConfig {
  String wifi_ssid;
  String wifi_pass;

  String supabase_url;
  String supabase_key;

  int heightThreshold = 1700;
  bool activeColumns[8] = {true, true, true, true, true, true, true, true};

  int countingLineRow = 3;
  bool topToBottomIsIn = true;
  int maxTracks = 4;
  float eventConfidenceMin = 0.60f;

  String streamModeDefault = "legacy_csv";

  bool paired = false;
  String adminToken;
  String setupCode;
};

RuntimeConfig cfg;

// ============================================================
// DEVICE IDENTITY / STATUS
// ============================================================
String mac_address = "";
String device_id = "";
String ap_name = "";
String ap_pass = "";
String mdns_name = "";

volatile bool wifiConnected = false;
volatile bool sensorReady = false;
volatile uint32_t frameSeq = 0;
volatile uint32_t droppedSensorEvents = 0;
volatile int lastSensorError = 0;
volatile bool lastUploadOk = false;

// ============================================================
// SIMPLE WS CLIENT MODE TRACKING
// ============================================================
enum WsMode : uint8_t {
  WS_MODE_LEGACY = 0,
  WS_MODE_JSON_V2 = 1
};

static const int MAX_WS_CLIENTS_TRACKED = 8;
WsMode wsClientMode[MAX_WS_CLIENTS_TRACKED];
bool wsClientConnected[MAX_WS_CLIENTS_TRACKED];

// ============================================================
// EVENTS
// ============================================================
struct CountEvent {
  char eventId[24];
  char direction[4];
  int distance_mm;
  float confidence;
  uint32_t ts_ms;
};

struct UploadEventQueue {
  static const int CAPACITY = 32;
  CountEvent items[CAPACITY];
  int head = 0;
  int tail = 0;
  int count = 0;
} uploadQueue;

uint32_t nextEventCounter = 1;
SemaphoreHandle_t configMutex = nullptr;

// ============================================================
// TRACKING
// ============================================================
struct Blob {
  bool valid = false;
  float row = 0;
  float col = 0;
  int minDist = 9999;
  int cells = 0;
};

struct Track {
  bool active = false;
  uint32_t id = 0;

  float row = 0;
  float col = 0;
  float prevRow = 0;
  float prevCol = 0;

  int minDist = 9999;
  int ageFrames = 0;
  int seenFrames = 0;
  int lostFrames = 0;

  bool everAboveLine = false;
  bool everBelowLine = false;
  bool counted = false;

  float confidence = 0.0f;
};

static const int MAX_BLOBS = 6;
Track tracks[4];
uint32_t nextTrackId = 1;

// ============================================================
// FORWARD DECLARATIONS
// ============================================================
void loadConfig();
void saveSensorConfig();
void saveWifiCloudConfig();
void savePairingState();
void ensureSetupCodeAndToken();
void parseRoiString(const String& maskStr);
String roiMaskToJson();
String makeJsonError(const char* code, const char* msg);
bool requireAuth();
bool readJsonBody(StaticJsonDocument<1024>& doc);
String generateHexToken(size_t bytesLen);
String makeDeviceSuffix();
String makeProvisioningPassword();
String maskSecret(const String& value, size_t prefix = 2, size_t suffix = 2);
void buildDeviceNames();
void connectNetwork();
void startAPMode();
void startMDNSIfPossible();
void setupHttpApi();
void setupWebSocket();
void processUploadQueue();
bool enqueueUploadEvent(const CountEvent& ev);
bool peekUploadEvent(CountEvent& ev);
void popUploadEvent();
bool sendToSupabase(const CountEvent& ev);

void sensorTask(void* pvParameters);
void handleStatus();
void handleGetConfig();
void handlePair();
void handleSensorConfig();
void handleWifiConfig();
void handleRestart();

void webSocketEvent(uint8_t num, WStype_t type, uint8_t* payload, size_t length);

void publishLegacyFrame(const int matrix8x8[64]);
void publishJsonFrame(const int matrix8x8[64], int occupancy);
void publishJsonTracks();
void publishJsonCount(const CountEvent& ev);
void publishJsonDiag(const char* lastErrorMsg);

int buildFilteredMatrix(VL53LMZ_ResultsData& data, int outMatrix[64]);
int extractBlobs(const int matrix8x8[64], Blob blobs[MAX_BLOBS]);
void updateTracksFromBlobs(const Blob blobs[MAX_BLOBS], int blobCount);
void maybeEmitCountEvents();
float computeTrackConfidence(const Track& t);
void cleanupOldTracks();
void createEventId(char* out, size_t outLen);

// ============================================================
// UTIL
// ============================================================
String makeDeviceSuffix() {
  String s = mac_address;
  s.replace(":", "");
  s.toUpperCase();
  if (s.length() >= 6) return s.substring(s.length() - 6);
  return "000000";
}

String generateHexToken(size_t bytesLen) {
  String out;
  out.reserve(bytesLen * 2);
  for (size_t i = 0; i < bytesLen; i++) {
    uint8_t b = (uint8_t)(esp_random() & 0xFF);
    char buf[3];
    snprintf(buf, sizeof(buf), "%02X", b);
    out += buf;
  }
  return out;
}

String makeProvisioningPassword() {
  String setupSeed = cfg.setupCode;
  setupSeed.trim();
  setupSeed.replace(":", "");
  setupSeed.toUpperCase();

  if (setupSeed.length() < 5) {
    setupSeed = makeDeviceSuffix();
  }

  return "TF-" + setupSeed;
}

String maskSecret(const String& value, size_t prefix, size_t suffix) {
  if (value.length() == 0) return "(not-set)";
  if (value.length() <= prefix + suffix) return "(hidden)";

  String masked = value.substring(0, prefix);
  masked += "...";
  masked += value.substring(value.length() - suffix);
  return masked;
}

void buildDeviceNames() {
  String suffix = makeDeviceSuffix();
  if (suffix.length() < 6) suffix = "000000";

  device_id = "tholusflow-" + suffix;
  ap_name = "TholusFlow-" + suffix.substring(suffix.length() - 4);
  mdns_name = "tholus-sensor-" + suffix;
  ap_pass = makeProvisioningPassword();
}

String roiMaskToJson() {
  String s = "[";
  for (int i = 0; i < 8; i++) {
    s += (cfg.activeColumns[i] ? "1" : "0");
    if (i < 7) s += ",";
  }
  s += "]";
  return s;
}

void parseRoiString(const String& maskStr) {
  StaticJsonDocument<256> doc;
  auto err = deserializeJson(doc, maskStr);
  if (err) return;
  JsonArray array = doc.as<JsonArray>();
  if (array.size() < 8) return;
  for (int i = 0; i < 8; i++) {
    cfg.activeColumns[i] = (array[i].as<int>() == 1);
  }
}

void ensureSetupCodeAndToken() {
  if (cfg.setupCode.isEmpty()) {
    cfg.setupCode = makeDeviceSuffix();
  }
  if (cfg.adminToken.isEmpty()) {
    cfg.adminToken = generateHexToken(24);
  }
}

String makeJsonError(const char* code, const char* msg) {
  StaticJsonDocument<256> doc;
  doc["status"] = "error";
  doc["code"] = code;
  doc["message"] = msg;
  String out;
  serializeJson(doc, out);
  return out;
}

bool readJsonBody(StaticJsonDocument<1024>& doc) {
  if (!server.hasArg("plain")) {
    server.send(400, "application/json", makeJsonError("bad_request", "Body non trovato"));
    return false;
  }
  DeserializationError err = deserializeJson(doc, server.arg("plain"));
  if (err) {
    server.send(400, "application/json", makeJsonError("bad_request", "JSON non valido"));
    return false;
  }
  return true;
}

bool requireAuth() {
  String auth = server.header("Authorization");

  if (!cfg.paired) {
    server.send(403, "application/json", makeJsonError("not_paired", "Device non associato"));
    return false;
  }

  if (!auth.startsWith("Bearer ")) {
    server.send(401, "application/json", makeJsonError("unauthorized", "Missing bearer token"));
    return false;
  }

  String token = auth.substring(7);
  token.trim();

  if (token != cfg.adminToken) {
    server.send(401, "application/json", makeJsonError("unauthorized", "Token non valido"));
    return false;
  }

  return true;
}

// ============================================================
// PERSISTENCE
// ============================================================
void loadConfig() {
  preferences.begin("tholus", true);

  cfg.wifi_ssid = preferences.getString("ssid", "");
  cfg.wifi_pass = preferences.getString("pass", "");

  cfg.supabase_url = preferences.getString("sb_url", "");
  cfg.supabase_key = preferences.getString("sb_key", "");

  cfg.heightThreshold = preferences.getInt("height", 1700);
  cfg.countingLineRow = preferences.getInt("line", 3);
  cfg.topToBottomIsIn = preferences.getBool("dir_in", true);
  cfg.maxTracks = preferences.getInt("tracks", 4);
  cfg.eventConfidenceMin = preferences.getFloat("confmin", 0.60f);
  cfg.streamModeDefault = preferences.getString("stream", "legacy_csv");

  String roiMaskStr = preferences.getString("roi", "[1,1,1,1,1,1,1,1]");
  parseRoiString(roiMaskStr);

  cfg.paired = preferences.getBool("paired", false);
  cfg.adminToken = preferences.getString("token", "");
  cfg.setupCode = preferences.getString("setup", "");

  nextEventCounter = preferences.getUInt("evcount", 1);

  preferences.end();
  ensureSetupCodeAndToken();
}

void saveSensorConfig() {
  preferences.begin("tholus", false);
  preferences.putInt("height", cfg.heightThreshold);
  preferences.putInt("line", cfg.countingLineRow);
  preferences.putBool("dir_in", cfg.topToBottomIsIn);
  preferences.putInt("tracks", cfg.maxTracks);
  preferences.putFloat("confmin", cfg.eventConfidenceMin);
  preferences.putString("stream", cfg.streamModeDefault);
  preferences.putString("roi", roiMaskToJson());
  preferences.end();
}

void saveWifiCloudConfig() {
  preferences.begin("tholus", false);
  preferences.putString("ssid", cfg.wifi_ssid);
  preferences.putString("pass", cfg.wifi_pass);
  preferences.putString("sb_url", cfg.supabase_url);
  preferences.putString("sb_key", cfg.supabase_key);
  preferences.end();
}

void savePairingState() {
  preferences.begin("tholus", false);
  preferences.putBool("paired", cfg.paired);
  preferences.putString("token", cfg.adminToken);
  preferences.putString("setup", cfg.setupCode);
  preferences.end();
}

// ============================================================
// HTTP API
// ============================================================
void handleStatus() {
  StaticJsonDocument<1024> doc;

  doc["mac"] = mac_address;
  doc["status"] = "alive";
  doc["protocol_version"] = PROTOCOL_VERSION;
  doc["device_id"] = device_id;
  doc["fw_version"] = FW_VERSION;
  doc["paired"] = cfg.paired;
  doc["auth_required"] = true;
  doc["mode"] = wifiConnected ? "sta" : "ap";
  doc["ip"] = wifiConnected ? WiFi.localIP().toString() : WiFi.softAPIP().toString();
  doc["ws_port"] = 81;
  doc["ap_name"] = ap_name;
  doc["mdns_name"] = mdns_name + ".local";
  doc["wifi_connected"] = wifiConnected;

  JsonObject sensorObj = doc.createNestedObject("sensor");
  sensorObj["model"] = "VL53L8CH";
  sensorObj["ready"] = sensorReady;
  sensorObj["resolution"] = "8x8";
  sensorObj["frequency_hz"] = SENSOR_FREQ_HZ;
  sensorObj["last_error"] = lastSensorError;

  JsonObject trackingObj = doc.createNestedObject("tracking");
  trackingObj["algorithm"] = "track-v2";
  trackingObj["heightThreshold"] = cfg.heightThreshold;
  JsonArray roi = trackingObj.createNestedArray("roiMask");
  for (int i = 0; i < 8; i++) roi.add(cfg.activeColumns[i] ? 1 : 0);
  trackingObj["countingLineRow"] = cfg.countingLineRow;
  trackingObj["topToBottomIsIn"] = cfg.topToBottomIsIn;
  trackingObj["maxTracks"] = cfg.maxTracks;
  trackingObj["eventConfidenceMin"] = cfg.eventConfidenceMin;

  JsonObject cloudObj = doc.createNestedObject("cloud");
  cloudObj["enabled"] = true;
  cloudObj["configured"] = (cfg.supabase_url.length() > 0 && cfg.supabase_key.length() > 0);
  cloudObj["queue_depth"] = uploadQueue.count;
  cloudObj["last_upload_ok"] = lastUploadOk;

  JsonObject wsObj = doc.createNestedObject("ws");
  wsObj["legacy_csv"] = true;
  wsObj["json_v2"] = true;

  String out;
  serializeJson(doc, out);
  server.send(200, "application/json", out);
}

void handleGetConfig() {
  if (!requireAuth()) return;

  StaticJsonDocument<1024> doc;
  doc["status"] = "ok";

  JsonObject c = doc.createNestedObject("config");
  c["heightThreshold"] = cfg.heightThreshold;

  JsonArray roi = c.createNestedArray("roiMask");
  for (int i = 0; i < 8; i++) roi.add(cfg.activeColumns[i] ? 1 : 0);

  c["countingLineRow"] = cfg.countingLineRow;
  c["topToBottomIsIn"] = cfg.topToBottomIsIn;
  c["maxTracks"] = cfg.maxTracks;
  c["eventConfidenceMin"] = cfg.eventConfidenceMin;
  c["streamModeDefault"] = cfg.streamModeDefault;

  JsonObject wifi = c.createNestedObject("wifi");
  wifi["ssid"] = cfg.wifi_ssid;
  wifi["configured"] = cfg.wifi_ssid.length() > 0;

  JsonObject cloud = c.createNestedObject("cloud");
  cloud["sb_url"] = cfg.supabase_url;
  cloud["sb_key_configured"] = cfg.supabase_key.length() > 0;

  String out;
  serializeJson(doc, out);
  server.send(200, "application/json", out);
}

void handlePair() {
  StaticJsonDocument<1024> doc;
  if (!readJsonBody(doc)) return;

  if (cfg.paired) {
    server.send(403, "application/json", makeJsonError("forbidden", "Device gia' associato"));
    return;
  }

  String setupCode = doc["setup_code"] | "";
  String clientName = doc["client_name"] | "";

  if (setupCode.length() == 0 || clientName.length() == 0) {
    server.send(400, "application/json", makeJsonError("bad_request", "setup_code o client_name mancanti"));
    return;
  }

  if (setupCode != cfg.setupCode) {
    server.send(401, "application/json", makeJsonError("unauthorized", "setup_code non valido"));
    return;
  }

  cfg.paired = true;
  if (cfg.adminToken.isEmpty()) cfg.adminToken = generateHexToken(24);
  savePairingState();

  StaticJsonDocument<512> outDoc;
  outDoc["status"] = "paired";
  outDoc["token"] = cfg.adminToken;
  outDoc["device_id"] = device_id;
  outDoc["protocol_version"] = PROTOCOL_VERSION;

  String out;
  serializeJson(outDoc, out);
  server.send(200, "application/json", out);
}

void handleSensorConfig() {
  if (!requireAuth()) return;

  StaticJsonDocument<1024> doc;
  if (!readJsonBody(doc)) return;

  bool changed = false;

  if (xSemaphoreTake(configMutex, pdMS_TO_TICKS(100)) == pdTRUE) {
    if (doc.containsKey("heightThreshold")) {
      cfg.heightThreshold = doc["heightThreshold"].as<int>();
      changed = true;
    }

    if (doc.containsKey("roiMask")) {
      JsonArray arr = doc["roiMask"].as<JsonArray>();
      if (arr.size() == 8) {
        for (int i = 0; i < 8; i++) cfg.activeColumns[i] = (arr[i].as<int>() == 1);
        changed = true;
      }
    }

    if (doc.containsKey("countingLineRow")) {
      int v = doc["countingLineRow"].as<int>();
      if (v >= 0 && v <= 6) {
        cfg.countingLineRow = v;
        changed = true;
      }
    }

    if (doc.containsKey("topToBottomIsIn")) {
      cfg.topToBottomIsIn = doc["topToBottomIsIn"].as<bool>();
      changed = true;
    }

    if (doc.containsKey("maxTracks")) {
      int v = doc["maxTracks"].as<int>();
      if (v >= 1 && v <= 4) {
        cfg.maxTracks = v;
        changed = true;
      }
    }

    if (doc.containsKey("eventConfidenceMin")) {
      float v = doc["eventConfidenceMin"].as<float>();
      if (v >= 0.0f && v <= 1.0f) {
        cfg.eventConfidenceMin = v;
        changed = true;
      }
    }

    if (doc.containsKey("streamModeDefault")) {
      String m = doc["streamModeDefault"].as<String>();
      if (m == "legacy_csv" || m == "json_v2") {
        cfg.streamModeDefault = m;
        changed = true;
      }
    }

    xSemaphoreGive(configMutex);
  }

  if (changed) saveSensorConfig();

  StaticJsonDocument<512> outDoc;
  outDoc["status"] = "ok";
  outDoc["restart_required"] = false;

  JsonObject applied = outDoc.createNestedObject("applied");
  applied["heightThreshold"] = cfg.heightThreshold;
  JsonArray roi = applied.createNestedArray("roiMask");
  for (int i = 0; i < 8; i++) roi.add(cfg.activeColumns[i] ? 1 : 0);
  applied["countingLineRow"] = cfg.countingLineRow;
  applied["topToBottomIsIn"] = cfg.topToBottomIsIn;
  applied["maxTracks"] = cfg.maxTracks;
  applied["eventConfidenceMin"] = cfg.eventConfidenceMin;
  applied["streamModeDefault"] = cfg.streamModeDefault;

  String out;
  serializeJson(outDoc, out);
  server.send(200, "application/json", out);
}

void handleWifiConfig() {
  if (!requireAuth()) return;

  StaticJsonDocument<1024> doc;
  if (!readJsonBody(doc)) return;

  if (doc.containsKey("ssid")) cfg.wifi_ssid = doc["ssid"].as<String>();
  if (doc.containsKey("password")) cfg.wifi_pass = doc["password"].as<String>();
  if (doc.containsKey("sb_url")) cfg.supabase_url = doc["sb_url"].as<String>();
  if (doc.containsKey("sb_key")) cfg.supabase_key = doc["sb_key"].as<String>();

  saveWifiCloudConfig();

  StaticJsonDocument<256> outDoc;
  outDoc["status"] = "ok";
  outDoc["restart_required"] = true;

  String out;
  serializeJson(outDoc, out);
  server.send(200, "application/json", out);
}

void handleRestart() {
  if (!requireAuth()) return;

  StaticJsonDocument<128> doc;
  doc["status"] = "ok";
  doc["restarting"] = true;
  String out;
  serializeJson(doc, out);
  server.send(200, "application/json", out);

  delay(250);
  ESP.restart();
}

void setupHttpApi() {
  server.collectHeaders(HEADER_KEYS, HEADER_KEYS_COUNT);

  server.on("/api/status", HTTP_GET, handleStatus);
  server.on("/api/config", HTTP_GET, handleGetConfig);
  server.on("/api/pair", HTTP_POST, handlePair);
  server.on("/api/config", HTTP_POST, handleSensorConfig);
  server.on("/api/wifi", HTTP_POST, handleWifiConfig);
  server.on("/api/restart", HTTP_POST, handleRestart);

  server.onNotFound([]() {
    server.send(404, "application/json", makeJsonError("not_found", "Endpoint non trovato"));
  });

  server.begin();
}

// ============================================================
// WS
// ============================================================
void setupWebSocket() {
  for (int i = 0; i < MAX_WS_CLIENTS_TRACKED; i++) {
    wsClientMode[i] = WS_MODE_LEGACY;
    wsClientConnected[i] = false;
  }

  webSocket.begin();
  webSocket.onEvent(webSocketEvent);
}

void webSocketEvent(uint8_t num, WStype_t type, uint8_t* payload, size_t length) {
  if (num >= MAX_WS_CLIENTS_TRACKED) return;

  switch (type) {
    case WStype_CONNECTED:
      wsClientConnected[num] = true;
      wsClientMode[num] = (cfg.streamModeDefault == "json_v2") ? WS_MODE_JSON_V2 : WS_MODE_LEGACY;
      break;

    case WStype_DISCONNECTED:
      wsClientConnected[num] = false;
      wsClientMode[num] = WS_MODE_LEGACY;
      break;

    case WStype_TEXT: {
      StaticJsonDocument<256> doc;
      DeserializationError err = deserializeJson(doc, payload, length);
      if (err) return;

      String msgType = doc["type"] | "";
      if (msgType == "subscribe") {
        String mode = doc["mode"] | "";
        if (mode == "json_v2") wsClientMode[num] = WS_MODE_JSON_V2;
        else wsClientMode[num] = WS_MODE_LEGACY;

        StaticJsonDocument<256> ack;
        ack["type"] = "subscribed";
        ack["mode"] = (wsClientMode[num] == WS_MODE_JSON_V2) ? "json_v2" : "legacy_csv";
        ack["protocol_version"] = PROTOCOL_VERSION;
        String out;
        serializeJson(ack, out);
        webSocket.sendTXT(num, out);
      }
      break;
    }

    default:
      break;
  }
}

void publishLegacyFrame(const int matrix8x8[64]) {
  String payload;
  payload.reserve(64 * 5);

  for (int i = 0; i < 64; i++) {
    payload += String(matrix8x8[i]);
    if (i < 63) payload += ",";
  }

  for (int i = 0; i < MAX_WS_CLIENTS_TRACKED; i++) {
    if (wsClientConnected[i] && wsClientMode[i] == WS_MODE_LEGACY) {
      webSocket.sendTXT(i, payload);
    }
  }
}

void publishJsonFrame(const int matrix8x8[64], int occupancy) {
  StaticJsonDocument<1024> doc;
  doc["type"] = "frame";
  doc["seq"] = frameSeq;
  doc["ts_ms"] = millis();

  JsonArray arr = doc.createNestedArray("matrix");
  for (int i = 0; i < 64; i++) arr.add(matrix8x8[i]);

  doc["occupancy_estimate"] = occupancy;
  doc["sensor_ready"] = sensorReady;

  String out;
  serializeJson(doc, out);

  for (int i = 0; i < MAX_WS_CLIENTS_TRACKED; i++) {
    if (wsClientConnected[i] && wsClientMode[i] == WS_MODE_JSON_V2) {
      webSocket.sendTXT(i, out);
    }
  }
}

void publishJsonTracks() {
  StaticJsonDocument<1024> doc;
  doc["type"] = "tracks";
  doc["seq"] = frameSeq;
  doc["ts_ms"] = millis();

  JsonArray arr = doc.createNestedArray("tracks");
  for (int i = 0; i < 4; i++) {
    if (!tracks[i].active) continue;

    JsonObject t = arr.createNestedObject();
    t["id"] = tracks[i].id;
    t["row"] = tracks[i].row;
    t["col"] = tracks[i].col;
    t["distance_mm"] = tracks[i].minDist;
    t["confidence"] = tracks[i].confidence;

    String state = "tracking";
    if (!tracks[i].counted) {
      if (tracks[i].row < tracks[i].prevRow) state = "moving_up";
      else if (tracks[i].row > tracks[i].prevRow) state = "moving_down";
    }
    t["state"] = state;
  }

  String out;
  serializeJson(doc, out);

  for (int i = 0; i < MAX_WS_CLIENTS_TRACKED; i++) {
    if (wsClientConnected[i] && wsClientMode[i] == WS_MODE_JSON_V2) {
      webSocket.sendTXT(i, out);
    }
  }
}

void publishJsonCount(const CountEvent& ev) {
  StaticJsonDocument<512> doc;
  doc["type"] = "count";
  doc["ts_ms"] = ev.ts_ms;
  doc["event_id"] = ev.eventId;
  doc["direction"] = ev.direction;
  doc["distance_mm"] = ev.distance_mm;
  doc["confidence"] = ev.confidence;

  String out;
  serializeJson(doc, out);

  for (int i = 0; i < MAX_WS_CLIENTS_TRACKED; i++) {
    if (wsClientConnected[i] && wsClientMode[i] == WS_MODE_JSON_V2) {
      webSocket.sendTXT(i, out);
    }
  }
}

void publishJsonDiag(const char* lastErrorMsg) {
  StaticJsonDocument<256> doc;
  doc["type"] = "diag";
  doc["ts_ms"] = millis();
  doc["wifi_connected"] = wifiConnected;
  doc["cloud_queue_depth"] = uploadQueue.count;

  if (lastErrorMsg) doc["last_error"] = lastErrorMsg;
  else doc["last_error"] = nullptr;

  String out;
  serializeJson(doc, out);

  for (int i = 0; i < MAX_WS_CLIENTS_TRACKED; i++) {
    if (wsClientConnected[i] && wsClientMode[i] == WS_MODE_JSON_V2) {
      webSocket.sendTXT(i, out);
    }
  }
}

// ============================================================
// UPLOAD QUEUE
// ============================================================
bool enqueueUploadEvent(const CountEvent& ev) {
  if (uploadQueue.count >= UploadEventQueue::CAPACITY) return false;
  uploadQueue.items[uploadQueue.tail] = ev;
  uploadQueue.tail = (uploadQueue.tail + 1) % UploadEventQueue::CAPACITY;
  uploadQueue.count++;
  return true;
}

bool peekUploadEvent(CountEvent& ev) {
  if (uploadQueue.count <= 0) return false;
  ev = uploadQueue.items[uploadQueue.head];
  return true;
}

void popUploadEvent() {
  if (uploadQueue.count <= 0) return;
  uploadQueue.head = (uploadQueue.head + 1) % UploadEventQueue::CAPACITY;
  uploadQueue.count--;
}

void createEventId(char* out, size_t outLen) {
  String suffix = makeDeviceSuffix();
  snprintf(out, outLen, "%s-%08lu", suffix.c_str(), (unsigned long)nextEventCounter++);
  preferences.begin("tholus", false);
  preferences.putUInt("evcount", nextEventCounter);
  preferences.end();
}

bool sendToSupabase(const CountEvent& ev) {
  if (WiFi.status() != WL_CONNECTED) return false;
  if (cfg.supabase_url.isEmpty() || cfg.supabase_key.isEmpty()) return false;

  HTTPClient http;
  if (!http.begin(cfg.supabase_url)) return false;

  http.addHeader("apikey", cfg.supabase_key);
  http.addHeader("Authorization", "Bearer " + cfg.supabase_key);
  http.addHeader("Content-Type", "application/json");

  StaticJsonDocument<512> doc;
  doc["event_id"] = ev.eventId;
  doc["dome_id"] = device_id;
  doc["mac"] = mac_address;
  doc["direction"] = ev.direction;
  doc["distance_mm"] = ev.distance_mm;
  doc["confidence"] = ev.confidence;
  doc["ts_ms"] = ev.ts_ms;
  doc["fw_version"] = FW_VERSION;

  String payload;
  serializeJson(doc, payload);

  int code = http.POST(payload);
  http.end();

  return (code >= 200 && code < 300);
}

void processUploadQueue() {
  static uint32_t lastAttemptMs = 0;
  if (millis() - lastAttemptMs < 250) return;
  lastAttemptMs = millis();

  CountEvent ev;
  if (!peekUploadEvent(ev)) return;

  bool ok = sendToSupabase(ev);
  lastUploadOk = ok;

  if (ok) popUploadEvent();
}

// ============================================================
// SENSOR / TRACKING
// ============================================================
static inline bool isValidTargetStatus(uint8_t status) {
  return (status == 5 || status == 9);
}

int buildFilteredMatrix(VL53LMZ_ResultsData& data, int outMatrix[64]) {
  int nonZero = 0;

  if (xSemaphoreTake(configMutex, pdMS_TO_TICKS(10)) != pdTRUE) {
    for (int i = 0; i < 64; i++) outMatrix[i] = 0;
    return 0;
  }

  for (int zone = 0; zone < 64; zone++) {
    int col = zone % 8;
    int dist = 0;

    if (!cfg.activeColumns[col]) {
      outMatrix[zone] = 0;
      continue;
    }

    if (data.nb_target_detected[zone] > 0) {
      int idx = zone * VL53LMZ_NB_TARGET_PER_ZONE;
      int candidate = data.distance_mm[idx];
      uint8_t status = data.target_status[idx];

      if (isValidTargetStatus(status) && candidate > 50 && candidate <= cfg.heightThreshold) {
        dist = candidate;
      }
    }

    outMatrix[zone] = dist;
    if (dist > 0) nonZero++;
  }

  xSemaphoreGive(configMutex);
  return nonZero;
}

int extractBlobs(const int matrix8x8[64], Blob blobs[MAX_BLOBS]) {
  bool visited[64] = {false};
  int blobCount = 0;

  auto idx = [](int r, int c) { return r * 8 + c; };

  for (int r = 0; r < 8; r++) {
    for (int c = 0; c < 8; c++) {
      int start = idx(r, c);
      if (visited[start] || matrix8x8[start] == 0) continue;
      if (blobCount >= MAX_BLOBS) continue;

      int queue[64];
      int qh = 0, qt = 0;
      queue[qt++] = start;
      visited[start] = true;

      int cells = 0;
      float rowSum = 0;
      float colSum = 0;
      int minDist = 9999;

      while (qh < qt) {
        int cur = queue[qh++];
        int cr = cur / 8;
        int cc = cur % 8;
        int dist = matrix8x8[cur];

        cells++;
        rowSum += cr;
        colSum += cc;
        if (dist < minDist) minDist = dist;

        const int dr[4] = {-1, 1, 0, 0};
        const int dc[4] = {0, 0, -1, 1};

        for (int k = 0; k < 4; k++) {
          int nr = cr + dr[k];
          int nc = cc + dc[k];
          if (nr < 0 || nr >= 8 || nc < 0 || nc >= 8) continue;
          int ni = idx(nr, nc);
          if (visited[ni]) continue;
          if (matrix8x8[ni] == 0) continue;
          visited[ni] = true;
          queue[qt++] = ni;
        }
      }

      blobs[blobCount].valid = true;
      blobs[blobCount].row = rowSum / max(1, cells);
      blobs[blobCount].col = colSum / max(1, cells);
      blobs[blobCount].minDist = minDist;
      blobs[blobCount].cells = cells;
      blobCount++;
    }
  }

  return blobCount;
}

float trackDistance(const Track& t, const Blob& b) {
  float dr = t.row - b.row;
  float dc = t.col - b.col;
  return sqrtf(dr * dr + dc * dc);
}

void updateTracksFromBlobs(const Blob blobs[MAX_BLOBS], int blobCount) {
  bool blobUsed[MAX_BLOBS] = {false};

  for (int i = 0; i < 4; i++) {
    if (!tracks[i].active) continue;

    int bestBlob = -1;
    float bestDist = 9999.0f;

    for (int b = 0; b < blobCount; b++) {
      if (!blobs[b].valid || blobUsed[b]) continue;
      float d = trackDistance(tracks[i], blobs[b]);
      if (d < bestDist && d <= 2.5f) {
        bestDist = d;
        bestBlob = b;
      }
    }

    if (bestBlob >= 0) {
      tracks[i].prevRow = tracks[i].row;
      tracks[i].prevCol = tracks[i].col;
      tracks[i].row = blobs[bestBlob].row;
      tracks[i].col = blobs[bestBlob].col;
      tracks[i].minDist = blobs[bestBlob].minDist;
      tracks[i].ageFrames++;
      tracks[i].seenFrames++;
      tracks[i].lostFrames = 0;
      tracks[i].confidence = min(1.0f, 0.20f + 0.12f * tracks[i].seenFrames + 0.05f * blobs[bestBlob].cells);

      if (tracks[i].row <= cfg.countingLineRow) tracks[i].everAboveLine = true;
      if (tracks[i].row > cfg.countingLineRow) tracks[i].everBelowLine = true;

      blobUsed[bestBlob] = true;
    } else {
      tracks[i].lostFrames++;
    }
  }

  for (int b = 0; b < blobCount; b++) {
    if (!blobs[b].valid || blobUsed[b]) continue;

    for (int i = 0; i < 4; i++) {
      if (!tracks[i].active) {
        tracks[i].active = true;
        tracks[i].id = nextTrackId++;
        tracks[i].row = blobs[b].row;
        tracks[i].col = blobs[b].col;
        tracks[i].prevRow = blobs[b].row;
        tracks[i].prevCol = blobs[b].col;
        tracks[i].minDist = blobs[b].minDist;
        tracks[i].ageFrames = 1;
        tracks[i].seenFrames = 1;
        tracks[i].lostFrames = 0;
        tracks[i].counted = false;
        tracks[i].everAboveLine = (tracks[i].row <= cfg.countingLineRow);
        tracks[i].everBelowLine = (tracks[i].row > cfg.countingLineRow);
        tracks[i].confidence = 0.30f + 0.05f * blobs[b].cells;
        blobUsed[b] = true;
        break;
      }
    }
  }

  cleanupOldTracks();
}

void cleanupOldTracks() {
  for (int i = 0; i < 4; i++) {
    if (!tracks[i].active) continue;
    if (tracks[i].lostFrames > 4 || tracks[i].ageFrames > 120) {
      tracks[i] = Track();
    }
  }
}

float computeTrackConfidence(const Track& t) {
  float c = 0.25f;
  c += min(0.45f, t.seenFrames * 0.06f);
  if (t.everAboveLine && t.everBelowLine) c += 0.20f;
  c += min(0.10f, max(0, 2000 - t.minDist) / 20000.0f);
  if (c > 1.0f) c = 1.0f;
  return c;
}

void maybeEmitCountEvents() {
  for (int i = 0; i < 4; i++) {
    if (!tracks[i].active || tracks[i].counted) continue;
    if (tracks[i].seenFrames < 2) continue;

    bool crossedDown = (tracks[i].prevRow <= cfg.countingLineRow && tracks[i].row > cfg.countingLineRow);
    bool crossedUp = (tracks[i].prevRow > cfg.countingLineRow && tracks[i].row <= cfg.countingLineRow);

    if (!crossedDown && !crossedUp) continue;

    CountEvent ev{};
    createEventId(ev.eventId, sizeof(ev.eventId));
    ev.distance_mm = tracks[i].minDist;
    ev.confidence = computeTrackConfidence(tracks[i]);
    ev.ts_ms = millis();

    if (ev.confidence < cfg.eventConfidenceMin) {
      tracks[i].counted = true;
      continue;
    }

    bool isIn = false;
    if (cfg.topToBottomIsIn) {
      isIn = crossedDown;
    } else {
      isIn = crossedUp;
    }

    snprintf(ev.direction, sizeof(ev.direction), "%s", isIn ? "IN" : "OUT");

    if (!xQueueSend(eventQueue, &ev, 0)) {
      droppedSensorEvents++;
    } else {
      publishJsonCount(ev);
    }

    tracks[i].counted = true;
  }
}

// ============================================================
// SENSOR TASK
// ============================================================
void sensorTask(void* pvParameters) {
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
  sensor.begin();

  uint8_t status = sensor.init();
  lastSensorError = status;

  if (status != 0) {
    sensorReady = false;
    publishJsonDiag("sensor_init_failed");
    vTaskDelete(nullptr);
    return;
  }

  sensor.set_resolution(SENSOR_RESOLUTION);
  sensor.set_ranging_frequency_hz(SENSOR_FREQ_HZ);
  sensor.set_target_order(VL53LMZ_TARGET_ORDER_CLOSEST);
  sensor.start_ranging();
  sensorReady = true;

  int matrix8x8[64];

  while (true) {
    uint8_t ready = 0;
    sensor.check_data_ready(&ready);

    if (ready) {
      VL53LMZ_ResultsData data;
      sensor.get_ranging_data(&data);

      frameSeq++;
      buildFilteredMatrix(data, matrix8x8);

      Blob blobs[MAX_BLOBS];
      for (int i = 0; i < MAX_BLOBS; i++) blobs[i] = Blob();

      int blobCount = extractBlobs(matrix8x8, blobs);

      updateTracksFromBlobs(blobs, blobCount);
      maybeEmitCountEvents();

      publishLegacyFrame(matrix8x8);
      publishJsonFrame(matrix8x8, blobCount);
      publishJsonTracks();
    }

    vTaskDelay(pdMS_TO_TICKS(10));
  }
}

// ============================================================
// NETWORK
// ============================================================
void startAPMode() {
  WiFi.mode(WIFI_AP);
  WiFi.softAP(ap_name.c_str(), ap_pass.c_str());
  wifiConnected = false;

  Serial.println("[AP] SSID: " + ap_name);
  Serial.println("[AP] PASS: " + maskSecret(ap_pass, 3, 2));
  Serial.println("[AP] IP: " + WiFi.softAPIP().toString());
}

void startMDNSIfPossible() {
  if (wifiConnected) {
    if (MDNS.begin(mdns_name.c_str())) {
      MDNS.addService("http", "tcp", 80);
      MDNS.addService("ws", "tcp", 81);
      Serial.println("[mDNS] " + mdns_name + ".local");
    }
  }
}

void connectNetwork() {
  if (cfg.wifi_ssid.isEmpty()) {
    startAPMode();
    return;
  }

  WiFi.mode(WIFI_STA);
  WiFi.begin(cfg.wifi_ssid.c_str(), cfg.wifi_pass.c_str());

  Serial.println("[WiFi] Connessione a " + cfg.wifi_ssid);

  int attempts = 0;
  while (WiFi.status() != WL_CONNECTED && attempts < 30) {
    delay(500);
    Serial.print(".");
    attempts++;
  }
  Serial.println();

  if (WiFi.status() == WL_CONNECTED) {
    wifiConnected = true;
    Serial.println("[WiFi] Connesso. IP: " + WiFi.localIP().toString());
  } else {
    Serial.println("[WiFi] Fallita, passo in AP");
    startAPMode();
  }
}

// ============================================================
// SETUP / LOOP
// ============================================================
void setup() {
  Serial.begin(115200);
  delay(800);

  WiFi.mode(WIFI_STA);
  delay(100);

  mac_address = WiFi.macAddress();
  buildDeviceNames();

  configMutex = xSemaphoreCreateMutex();
  eventQueue = xQueueCreate(16, sizeof(CountEvent));

  loadConfig();

  mac_address = WiFi.macAddress();
  buildDeviceNames();

  Serial.println("======================================");
  Serial.println("TholusFlow v2 starting");
  Serial.println("FW: " + String(FW_VERSION));
  Serial.println("MAC: " + mac_address);
  Serial.println("DEVICE: " + device_id);
  Serial.println("SETUP CODE: " + maskSecret(cfg.setupCode, 2, 2));
  Serial.println("AP PASSWORD RULE: TF-<setup code>");
  Serial.println("======================================");

  connectNetwork();
  startMDNSIfPossible();
  setupHttpApi();
  setupWebSocket();

  xTaskCreatePinnedToCore(
    sensorTask,
    "SensorTask",
    16384,
    nullptr,
    1,
    &SensorTaskHandle,
    0
  );

  Serial.println("[SYSTEM] Ready");
}

void loop() {
  server.handleClient();
  webSocket.loop();

  CountEvent ev;
  while (xQueueReceive(eventQueue, &ev, 0) == pdPASS) {
    if (!enqueueUploadEvent(ev)) {
      droppedSensorEvents++;
      publishJsonDiag("upload_queue_full");
    }
  }

  processUploadQueue();

  static uint32_t lastDiagMs = 0;
  if (millis() - lastDiagMs > 3000) {
    publishJsonDiag(nullptr);
    lastDiagMs = millis();
  }

  delay(2);
}