// ─────────────────────────────────────────────────────────────────────────────
//  CoAP Smart Home server on ESP32 (Wokwi) — Part 1 + Built-in self-test
//
//  Resources:
//    GET /temp    → temperature value
//    GET /light   → current LED state
//    GET /motion  → motion status
//
//  Self-test client runs in the same sketch, sending CoAP requests every 4s
//  to localhost so you can verify everything works in the Wokwi serial monitor.
// ─────────────────────────────────────────────────────────────────────────────

#include <WiFi.h>
#include <WiFiUdp.h>
#include <DHT.h>

// ── WiFi (Wokwi virtual network) ────────────────────────────────────────────
const char* SSID     = "Wokwi-GUEST";
const char* PASSWORD = "";

// ── Pins ────────────────────────────────────────────────────────────────────
#define DHTPIN   4
#define DHTTYPE  DHT22
#define LED_PIN  2
#define PIR_PIN  13

// ── CoAP constants ──────────────────────────────────────────────────────────
#define COAP_PORT     5683

#define T_CON         0
#define T_NON         1
#define T_ACK         2

#define M_GET         1

#define R_CONTENT     ((2 << 5) | 5)   // 2.05 Content
#define R_NOT_FOUND   ((4 << 5) | 4)   // 4.04 Not Found

#define OPT_URI_PATH  11

// ── Globals ─────────────────────────────────────────────────────────────────
DHT     dht(DHTPIN, DHTTYPE);
WiFiUDP udp;
bool    ledState = false;

// Self-test client globals
WiFiUDP       testUdp;
unsigned long lastTest    = 0;
int           testStep    = 0;
uint16_t      testMsgId   = 0xAA00;
bool          clientReady = false;

// ─────────────────────────────────────────────────────────────────────────────
//  Build a basic CoAP response
// ─────────────────────────────────────────────────────────────────────────────
void sendCoap(IPAddress ip, uint16_t port,
              uint8_t type, uint8_t code, uint16_t msgId,
              const uint8_t* token, uint8_t tokenLen,
              const char* payload) {
  uint8_t buf[128];
  int idx = 0;

  buf[idx++] = (0x40) | (type << 4) | (tokenLen & 0x0F);
  buf[idx++] = code;
  buf[idx++] = (msgId >> 8) & 0xFF;
  buf[idx++] = msgId & 0xFF;

  for (int i = 0; i < tokenLen; i++) buf[idx++] = token[i];

  if (payload && payload[0]) {
    buf[idx++] = 0xFF;
    int pLen = strlen(payload);
    memcpy(buf + idx, payload, pLen);
    idx += pLen;
  }

  udp.beginPacket(ip, port);
  udp.write(buf, idx);
  udp.endPacket();
}

// ─────────────────────────────────────────────────────────────────────────────
//  Parse Uri-Path option(s) into a string like "/temp"
// ─────────────────────────────────────────────────────────────────────────────
String extractPath(uint8_t* pkt, int len, int optStart) {
  String path = "";
  int i = optStart;
  uint16_t prevOpt = 0;

  while (i < len) {
    if (pkt[i] == 0xFF) break;
    uint8_t deltaNibble = (pkt[i] >> 4) & 0x0F;
    uint8_t lenNibble   = pkt[i] & 0x0F;
    i++;

    uint16_t delta = deltaNibble;
    if (deltaNibble == 13)      { delta = pkt[i++] + 13; }
    else if (deltaNibble == 14) { delta = ((uint16_t)pkt[i] << 8 | pkt[i+1]) + 269; i += 2; }

    uint16_t optLen = lenNibble;
    if (lenNibble == 13)      { optLen = pkt[i++] + 13; }
    else if (lenNibble == 14) { optLen = ((uint16_t)pkt[i] << 8 | pkt[i+1]) + 269; i += 2; }

    uint16_t optNum = prevOpt + delta;
    if (optNum == OPT_URI_PATH) {
      path += "/";
      for (int j = 0; j < optLen; j++) path += (char)pkt[i + j];
    }
    i += optLen;
    prevOpt = optNum;
  }
  return path;
}

// ─────────────────────────────────────────────────────────────────────────────
//  Sensor helpers
// ─────────────────────────────────────────────────────────────────────────────
float readTemp() {
  float t = dht.readTemperature();
  if (isnan(t)) {
    t = 22.0 + (random(-20, 50) / 10.0);
  }
  return t;
}

bool readMotion() {
  return digitalRead(PIR_PIN) == LOW;
}

// ─────────────────────────────────────────────────────────────────────────────
//  Self-test client functions
// ─────────────────────────────────────────────────────────────────────────────
void sendTestRequest(const char* path) {
  uint8_t buf[64];
  int idx = 0;
  testMsgId++;

  // Header: Ver=1, Type=CON, TKL=0, Code=GET
  buf[idx++] = 0x40;
  buf[idx++] = M_GET;
  buf[idx++] = (testMsgId >> 8) & 0xFF;
  buf[idx++] = testMsgId & 0xFF;

  // Encode Uri-Path option, splitting on '/'
  String p = String(path);
  if (p.startsWith("/")) p = p.substring(1);
  uint16_t prevOpt = 0;
  int start = 0;
  while (start <= (int)p.length()) {
    int slash = p.indexOf('/', start);
    String seg = (slash == -1) ? p.substring(start) : p.substring(start, slash);
    if (seg.length() > 0) {
      uint16_t delta = OPT_URI_PATH - prevOpt;
      buf[idx++] = (delta << 4) | (seg.length() & 0x0F);
      for (int i = 0; i < (int)seg.length(); i++) buf[idx++] = seg[i];
      prevOpt = OPT_URI_PATH;
    }
    if (slash == -1) break;
    start = slash + 1;
  }

  testUdp.beginPacket(WiFi.localIP(), COAP_PORT);
  testUdp.write(buf, idx);
  testUdp.endPacket();
  Serial.printf("\n>>> TEST GET %s (msgId=%u)\n", path, testMsgId);
}

void checkTestResponse() {
  int sz = testUdp.parsePacket();
  if (sz <= 0) return;
  uint8_t buf[256];
  int len = testUdp.read(buf, sizeof(buf));
  if (len < 4) return;

  uint8_t code = buf[1];
  uint8_t cls  = (code >> 5) & 0x07;
  uint8_t det  = code & 0x1F;

  // Skip token and find payload marker
  int i = 4 + (buf[0] & 0x0F);
  while (i < len && buf[i] != 0xFF) i++;
  String payload = "";
  if (i < len) {
    i++;
    while (i < len) payload += (char)buf[i++];
  }
  Serial.printf("<<< Response %u.%02u  payload=\"%s\"\n", cls, det, payload.c_str());
}

void runSelfTest() {
  if (!clientReady && WiFi.status() == WL_CONNECTED) {
    testUdp.begin(5684);  // different port from the server
    clientReady = true;
    Serial.println("\n=== Self-test client ready on port 5684 ===");
    Serial.println("=== Cycling through /temp, /light, /motion every 4s ===\n");
  }
  if (!clientReady) return;

  checkTestResponse();

  unsigned long now = millis();
  if (now - lastTest > 4000) {
    lastTest = now;
    const char* paths[] = { "/temp", "/light", "/motion" };
    sendTestRequest(paths[testStep % 3]);
    testStep++;
  }
}

// ─────────────────────────────────────────────────────────────────────────────
//  Setup
// ─────────────────────────────────────────────────────────────────────────────
void setup() {
  Serial.begin(115200);
  delay(100);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);
  pinMode(PIR_PIN, INPUT_PULLUP);
  dht.begin();

  Serial.print("Connecting to WiFi");
  WiFi.begin(SSID, PASSWORD);
  while (WiFi.status() != WL_CONNECTED) { delay(300); Serial.print("."); }
  Serial.println();
  Serial.print("IP: ");
  Serial.println(WiFi.localIP());

  udp.begin(COAP_PORT);
  Serial.println("CoAP server listening on UDP 5683");
  Serial.println("Resources:  /temp   /light   /motion");
}

// ─────────────────────────────────────────────────────────────────────────────
//  Main loop
// ─────────────────────────────────────────────────────────────────────────────
void loop() {
  // ── Handle incoming CoAP packets (server side) ──
  int pktSize = udp.parsePacket();
  if (pktSize > 0) {
    uint8_t buf[256];
    int len = udp.read(buf, sizeof(buf));
    IPAddress  remoteIp   = udp.remoteIP();
    uint16_t   remotePort = udp.remotePort();

    if (len >= 4) {
      uint8_t  type     = (buf[0] >> 4) & 0x03;
      uint8_t  tokenLen = buf[0] & 0x0F;
      uint8_t  code     = buf[1];
      uint16_t msgId    = ((uint16_t)buf[2] << 8) | buf[3];

      if (tokenLen <= 8) {
        uint8_t token[8] = {};
        for (int i = 0; i < tokenLen; i++) token[i] = buf[4 + i];

        String path = extractPath(buf, len, 4 + tokenLen);
        uint8_t respType = (type == T_CON) ? T_ACK : T_NON;

        Serial.printf("[CoAP] GET %s from %s:%u\n",
                      path.c_str(), remoteIp.toString().c_str(), remotePort);

        if (code != M_GET) {
          sendCoap(remoteIp, remotePort, respType, R_NOT_FOUND,
                   msgId, token, tokenLen, "Method not allowed");
        } else if (path == "/temp") {
          float t = readTemp();
          char payload[16];
          snprintf(payload, sizeof(payload), "%.1f C", t);
          sendCoap(remoteIp, remotePort, respType, R_CONTENT,
                   msgId, token, tokenLen, payload);

        } else if (path == "/light") {
          sendCoap(remoteIp, remotePort, respType, R_CONTENT,
                   msgId, token, tokenLen, ledState ? "ON" : "OFF");

        } else if (path == "/motion") {
          bool m = readMotion();
          sendCoap(remoteIp, remotePort, respType, R_CONTENT,
                   msgId, token, tokenLen, m ? "DETECTED" : "CLEAR");

        } else {
          sendCoap(remoteIp, remotePort, respType, R_NOT_FOUND,
                   msgId, token, tokenLen, "Not Found");
        }
      }
    }
  }

  // ── Run the built-in test client ──
  runSelfTest();
}