// ====================================================================
// Task 3: MQTT + JSON (Pico W, Wokwi)
// Receives control commands via MQTT, publishes sensor data
// ====================================================================
#include <WiFi.h>
#include <PubSubClient.h>
#include "SimpleJson.h"
// -- WiFi (Wokwi) ---------------------------------------------------
const char* WIFI_SSID = "Wokwi-GUEST";
const char* WIFI_PASS = "";
// -- MQTT ------------------------------------------------------------
//const char* MQTT_BROKER = "fbe-mqtt.hs-weingarten.de";
//const char* MQTT_BROKER = "141.69.95.10";
const char* MQTT_BROKER = "test.mosquitto.org";
const int MQTT_PORT = 1883;
const char* TOPIC_DATA = "iem/task3/pico/data";
const char* TOPIC_CMD = "iem/task3/pico/cmd";
// -- Robustness ------------------------------------------------------
const char* SHARED_TOKEN = "iem2026";
const char* EXPECTED_SOURCE = "nano";
const unsigned long WATCHDOG_TIMEOUT = 10000;
const int INTERVAL_MIN = 50;
const int INTERVAL_MAX = 2000;
// -- Hardware --------------------------------------------------------
const int LED_PIN = 28;
const int BUTTON_PIN = 2;
const int POT_PIN = 26;
// -- State -----------------------------------------------------------
bool blinkEnabled = true;
bool ledState = false;
int overrideInterval = -1; // -1 = poti controls
unsigned long lastValidCmd = 0;
bool inSafeState = false;
int expectedSeqNr = -1;
unsigned long seqOutgoing = 0;
// Statistics
unsigned long msgAccepted = 0, msgRejectedJson = 0;
unsigned long msgRejectedAuth = 0, msgSeqGaps = 0;
WiFiClient wifiClient;
PubSubClient mqtt(wifiClient);
SimpleJson jsonOut, jsonIn;

// --------------------------------------------------------------------
// WiFi
// --------------------------------------------------------------------
void setupWiFi() {
  Serial1.print("Connecting to WiFi: ");
  Serial1.println(WIFI_SSID);
  WiFi.begin(WIFI_SSID, WIFI_PASS);
  int retries = 0;
  while (WiFi.status() != WL_CONNECTED && retries < 50) {
    delay(200);
    Serial1.print(".");
    retries++;
  }
  Serial1.println();
  if (WiFi.status() == WL_CONNECTED) {
    Serial1.print("WiFi connected, IP: ");
    Serial1.println(WiFi.localIP());
  } else {
    Serial1.println("WiFi connect failed, continuing anyway...");
  }
}

// --------------------------------------------------------------------
// Safe state
// --------------------------------------------------------------------
void enterSafeState() {
  if (inSafeState) return;
  inSafeState = true;
  overrideInterval = -1; // back to poti
  Serial1.println("[SAFE] Entering safe state: using local potentiometer");
}

void leaveSafeState() {
  if (!inSafeState) return;
  inSafeState = false;
  Serial1.println("[SAFE] Leaving safe state: remote control active");
}

// --------------------------------------------------------------------
// Validation + sequence
// --------------------------------------------------------------------
bool validateMessage(const SimpleJson& msg) {
  String token = msg.getString("token");
  String source = msg.getString("source");
  if (token != SHARED_TOKEN) {
    msgRejectedAuth++;
    Serial1.println("[AUTH] Invalid token, message rejected");
    return false;
  }
  if (source != EXPECTED_SOURCE) {
    msgRejectedAuth++;
    Serial1.println("[AUTH] Invalid source, message rejected");
    return false;
  }
  return true;
}

bool checkSequence(const SimpleJson& msg) {
  int seq = msg.getInt("seq");
  if (expectedSeqNr == -1) {
    // first message or resync
    expectedSeqNr = seq + 1;
    return true;
  }
  if (seq == expectedSeqNr) {
    expectedSeqNr++;
    return true;
  }
  if (seq > expectedSeqNr) {
    Serial1.print("[SEQ] Gap detected. Expected ");
    Serial1.print(expectedSeqNr);
    Serial1.print(" got ");
    Serial1.println(seq);
    msgSeqGaps++;
    expectedSeqNr = seq + 1; // resync, but accept
    return true;
  }
  // replay / old
  Serial1.print("[SEQ] Replay/old message discarded, seq=");
  Serial1.println(seq);
  return false;
}

// --------------------------------------------------------------------
// Process command
// --------------------------------------------------------------------
void processCommand(const SimpleJson& cmd) {
  if (cmd.hasKey("blinkEnabled")) {
    blinkEnabled = cmd.getBool("blinkEnabled");
    Serial1.print("[CMD] blinkEnabled = ");
    Serial1.println(blinkEnabled ? "true" : "false");
  }

  if (cmd.hasKey("ledOn")) {
    bool on = cmd.getBool("ledOn");
    ledState = on;
    digitalWrite(LED_PIN, on ? HIGH : LOW);
    Serial1.print("[CMD] ledOn = ");
    Serial1.println(on ? "true" : "false");
  }

  if (cmd.hasKey("interval")) {
    int iv = cmd.getInt("interval");
    if (iv < INTERVAL_MIN || iv > INTERVAL_MAX) {
      Serial1.print("[CMD] interval out of range: ");
      Serial1.println(iv);
    } else {
      overrideInterval = iv;
      Serial1.print("[CMD] overrideInterval = ");
      Serial1.println(overrideInterval);
    }
  }

  if (cmd.hasKey("useLocalPot")) {
    bool useLocal = cmd.getBool("useLocalPot");
    if (useLocal) {
      overrideInterval = -1;
      Serial1.println("[CMD] Using local potentiometer again");
    }
  }
}

// --------------------------------------------------------------------
// MQTT callback
// --------------------------------------------------------------------
void mqttCallback(char* topic, byte* payload, unsigned int length) {
  if (length >= 512) {
    Serial1.println("[MQTT] Payload too large");
    return;
  }

  char buf[512];
  memcpy(buf, payload, length);
  buf[length] = '\0';

  if (!jsonIn.parse(buf)) {
    msgRejectedJson++;
    Serial1.println("[MQTT] JSON parse failed");
    return;
  }

  if (!validateMessage(jsonIn)) return;
  if (!checkSequence(jsonIn)) return;

  lastValidCmd = millis();
  leaveSafeState();
  msgAccepted++;

  processCommand(jsonIn);
}

// --------------------------------------------------------------------
// MQTT reconnect
// --------------------------------------------------------------------
void mqttReconnect() {
  while (!mqtt.connected()) {
    Serial1.print("Connecting to MQTT...");
    if (mqtt.connect("pico-task3")) {
      Serial1.println("connected");
      mqtt.subscribe(TOPIC_CMD);
      Serial1.print("Subscribed to ");
      Serial1.println(TOPIC_CMD);
    } else {
      Serial1.print("failed, rc=");
      Serial1.print(mqtt.state());
      Serial1.println(" retry in 2s");
      delay(2000);
    }
  }
}

// --------------------------------------------------------------------
// Publish sensor data
// --------------------------------------------------------------------
void publishSensorData(int potValue, unsigned long blinkInterval) {
  jsonOut.clear();
  jsonOut.setString("token", SHARED_TOKEN);
  jsonOut.setString("source", "pico");
  jsonOut.setInt("seq", (int)seqOutgoing++);
  jsonOut.setInt("potValue", potValue);
  jsonOut.setBool("blinkEnabled", blinkEnabled);
  jsonOut.setInt("interval", (int)blinkInterval);
  jsonOut.setBool("ledState", ledState);
  jsonOut.setBool("safeState", inSafeState);
  jsonOut.setInt("uptime", (int)(millis() / 1000));

  char buf[256];
  jsonOut.toCharArray(buf, sizeof(buf));
  mqtt.publish(TOPIC_DATA, buf);
}

// --------------------------------------------------------------------
// Setup / loop
// --------------------------------------------------------------------
unsigned long lastBlink = 0;
unsigned long lastPublish = 0;

void setup() {
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  Serial1.begin(115200);
  delay(500);

  setupWiFi();
  mqtt.setServer(MQTT_BROKER, MQTT_PORT);
  mqtt.setCallback(mqttCallback);

  lastValidCmd = millis();
}

void loop() {
  if (!mqtt.connected()) {
    mqttReconnect();
  }
  mqtt.loop();

  // Watchdog
  if (!inSafeState && (millis() - lastValidCmd > WATCHDOG_TIMEOUT)) {
    enterSafeState();
  }

  // Button toggles blinkEnabled (local override like Task 1)
  static bool lastButton = HIGH;
  bool btn = digitalRead(BUTTON_PIN);
  if (lastButton == HIGH && btn == LOW) {
    blinkEnabled = !blinkEnabled;
    Serial1.print("[BTN] blinkEnabled = ");
    Serial1.println(blinkEnabled ? "true" : "false");
  }
  lastButton = btn;

  // Read potentiometer and determine interval
  int potValue = analogRead(POT_PIN);
  unsigned long blinkInterval;
  if (overrideInterval > 0 && !inSafeState) {
    blinkInterval = overrideInterval;
  } else {
    blinkInterval = map(potValue, 0, 4095, INTERVAL_MIN, INTERVAL_MAX);
  }

  // Blink logic
  if (blinkEnabled) {
    if (millis() - lastBlink >= blinkInterval) {
      ledState = !ledState;
      digitalWrite(LED_PIN, ledState ? HIGH : LOW);
      lastBlink = millis();
    }
  }

  // Publish sensor data every 500 ms
  if (millis() - lastPublish >= 500) {
    publishSensorData(potValue, blinkInterval);
    lastPublish = millis();
  }
}