#include <Wire.h>
#include "soc/gpio_struct.h"
#include "esp_task_wdt.h"
#include <Adafruit_VL53L5CX.h>
#include "zh_bitmap.h"

#define LCD_W 240
#define LCD_H 320

// ESP32-2432S028 / CYD 2.8 inch ST7789 internal LCD pins.
#define PIN_SCK  14
#define PIN_SDA  13
#define PIN_CS   15
#define PIN_RST  -1
#define PIN_DC   2
#define PIN_BLK  21

// CN1 sensor header: GND - IO22 - IO27 - 3V3. Any I2C device can go here.
#define I2C_SDA 27
#define I2C_SCL 22

// ESP32-2432S028R resistive touch controller, usually XPT2046.
#define TOUCH_CS   33
#define TOUCH_IRQ  36
#define TOUCH_MOSI 32
#define TOUCH_MISO 39
#define TOUCH_SCLK 25

#define MODE_QUICK 1
#define MODE_LABEL 2
#define TEST_MODE MODE_QUICK

#define SCK_MASK (1UL << PIN_SCK)
#define SDA_MASK (1UL << PIN_SDA)
#define CS_MASK  (1UL << PIN_CS)
#define DC_MASK  (1UL << PIN_DC)
#define BLK_MASK (1UL << PIN_BLK)

#define FAST_HIGH(mask) (GPIO.out_w1ts = (mask))
#define FAST_LOW(mask)  (GPIO.out_w1tc = (mask))

#define SCK_0 FAST_LOW(SCK_MASK)
#define SCK_1 FAST_HIGH(SCK_MASK)
#define SDA_0 FAST_LOW(SDA_MASK)
#define SDA_1 FAST_HIGH(SDA_MASK)
#define CS_0  FAST_LOW(CS_MASK)
#define CS_1  FAST_HIGH(CS_MASK)
#define DC_0  FAST_LOW(DC_MASK)
#define DC_1  FAST_HIGH(DC_MASK)
#define BLK_1 FAST_HIGH(BLK_MASK)

#define BLACK  0x0000
#define WHITE  0xFFFF
#define RED    0xF800
#define GREEN  0x07E0
#define BLUE   0x001F
#define CYAN   0x07FF
#define YELLOW 0xFFE0

static const uint8_t FONT[][6] = {
  {' ', 0x00, 0x00, 0x00, 0x00, 0x00},
  {':', 0x00, 0x36, 0x36, 0x00, 0x00},
  {'0', 0x3E, 0x51, 0x49, 0x45, 0x3E},
  {'1', 0x00, 0x42, 0x7F, 0x40, 0x00},
  {'2', 0x42, 0x61, 0x51, 0x49, 0x46},
  {'3', 0x21, 0x41, 0x45, 0x4B, 0x31},
  {'4', 0x18, 0x14, 0x12, 0x7F, 0x10},
  {'5', 0x27, 0x45, 0x45, 0x45, 0x39},
  {'6', 0x3C, 0x4A, 0x49, 0x49, 0x30},
  {'7', 0x01, 0x71, 0x09, 0x05, 0x03},
  {'8', 0x36, 0x49, 0x49, 0x49, 0x36},
  {'9', 0x06, 0x49, 0x49, 0x29, 0x1E},
  {'A', 0x7E, 0x11, 0x11, 0x11, 0x7E},
  {'B', 0x7F, 0x49, 0x49, 0x49, 0x36},
  {'C', 0x3E, 0x41, 0x41, 0x41, 0x22},
  {'D', 0x7F, 0x41, 0x41, 0x22, 0x1C},
  {'E', 0x7F, 0x49, 0x49, 0x49, 0x41},
  {'F', 0x7F, 0x09, 0x09, 0x09, 0x01},
  {'G', 0x3E, 0x41, 0x49, 0x49, 0x7A},
  {'H', 0x7F, 0x08, 0x08, 0x08, 0x7F},
  {'I', 0x00, 0x41, 0x7F, 0x41, 0x00},
  {'K', 0x7F, 0x08, 0x14, 0x22, 0x41},
  {'L', 0x7F, 0x40, 0x40, 0x40, 0x40},
  {'M', 0x7F, 0x02, 0x0C, 0x02, 0x7F},
  {'N', 0x7F, 0x04, 0x08, 0x10, 0x7F},
  {'O', 0x3E, 0x41, 0x41, 0x41, 0x3E},
  {'P', 0x7F, 0x09, 0x09, 0x09, 0x06},
  {'R', 0x7F, 0x09, 0x19, 0x29, 0x46},
  {'S', 0x46, 0x49, 0x49, 0x49, 0x31},
  {'T', 0x01, 0x01, 0x7F, 0x01, 0x01},
  {'U', 0x3F, 0x40, 0x40, 0x40, 0x3F},
  {'V', 0x1F, 0x20, 0x40, 0x20, 0x1F},
  {'W', 0x3F, 0x40, 0x38, 0x40, 0x3F},
  {'X', 0x63, 0x14, 0x08, 0x14, 0x63},
  {'Y', 0x07, 0x08, 0x70, 0x08, 0x07},
  {'x', 0x44, 0x28, 0x10, 0x28, 0x44},
};

static void spiSendByte(uint8_t value) {
  for (uint8_t i = 0; i < 8; i++) {
    SCK_0;
    if (value & 0x80) {
      SDA_1;
    } else {
      SDA_0;
    }
    value <<= 1;
    SCK_1;
    SCK_0;
  }
}

static void sendCmd(uint8_t value) {
  DC_0;
  spiSendByte(value);
}

static void sendData(uint8_t value) {
  DC_1;
  spiSendByte(value);
}

static void setWindow(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1) {
  sendCmd(0x2A);
  sendData(x0 >> 8);
  sendData(x0 & 0xFF);
  sendData(x1 >> 8);
  sendData(x1 & 0xFF);

  sendCmd(0x2B);
  sendData(y0 >> 8);
  sendData(y0 & 0xFF);
  sendData(y1 >> 8);
  sendData(y1 & 0xFF);

  sendCmd(0x2C);
}

static void drawPixel(uint16_t x, uint16_t y, uint16_t color) {
  if (x >= LCD_W || y >= LCD_H) return;
  setWindow(x, y, x, y);
  sendData(color >> 8);
  sendData(color & 0xFF);
}

static void fillRect(uint16_t x, uint16_t y, uint16_t w, uint16_t h, uint16_t color) {
  if (x >= LCD_W || y >= LCD_H) return;
  if (x + w > LCD_W) w = LCD_W - x;
  if (y + h > LCD_H) h = LCD_H - y;
  setWindow(x, y, x + w - 1, y + h - 1);
  for (uint32_t i = 0; i < (uint32_t)w * h; i++) {
    sendData(color >> 8);
    sendData(color & 0xFF);
  }
}

static void fillScreen(uint16_t color) {
  setWindow(0, 0, LCD_W - 1, LCD_H - 1);
  for (uint32_t i = 0; i < (uint32_t)LCD_W * LCD_H; i++) {
    sendData(color >> 8);
    sendData(color & 0xFF);
  }
}

static const uint8_t *glyphFor(char c) {
  for (uint8_t i = 0; i < sizeof(FONT) / sizeof(FONT[0]); i++) {
    if (FONT[i][0] == c) return &FONT[i][1];
  }
  return &FONT[0][1];
}

static void drawChar(uint16_t x, uint16_t y, char c, uint16_t color, uint8_t scale) {
  uint16_t bg = BLACK;
  const uint8_t *g = glyphFor(c);
  uint16_t w = 6 * scale;
  uint16_t h = 8 * scale;
  setWindow(x, y, x + w - 1, y + h - 1);
  for (uint8_t py = 0; py < h; py++) {
    uint8_t row = py / scale;
    for (uint8_t px = 0; px < w; px++) {
      uint8_t col = px / scale;
      bool on = false;
      if (col < 5 && row < 7) {
        on = g[col] & (1 << row);
      }
      uint16_t out = on ? color : bg;
      sendData(out >> 8);
      sendData(out & 0xFF);
    }
  }
}

static void drawCharBg(uint16_t x, uint16_t y, char c, uint16_t color, uint16_t bg, uint8_t scale) {
  const uint8_t *g = glyphFor(c);
  uint16_t w = 6 * scale;
  uint16_t h = 8 * scale;
  setWindow(x, y, x + w - 1, y + h - 1);
  for (uint8_t py = 0; py < h; py++) {
    uint8_t row = py / scale;
    for (uint8_t px = 0; px < w; px++) {
      uint8_t col = px / scale;
      bool on = false;
      if (col < 5 && row < 7) {
        on = g[col] & (1 << row);
      }
      uint16_t out = on ? color : bg;
      sendData(out >> 8);
      sendData(out & 0xFF);
    }
  }
}

static void drawTextBg(uint16_t x, uint16_t y, const char *text, uint16_t color, uint16_t bg, uint8_t scale) {
  while (*text) {
    drawCharBg(x, y, *text++, color, bg, scale);
    x += 6 * scale;
  }
}

static void drawText(uint16_t x, uint16_t y, const char *text, uint16_t color, uint8_t scale) {
  drawTextBg(x, y, text, color, BLACK, scale);
}

static void drawBitmapText(uint16_t x, uint16_t y, const BitmapText &bt, uint16_t color, uint16_t bg, uint8_t scale) {
  uint16_t w = bt.w * scale;
  uint16_t h = bt.h * scale;
  setWindow(x, y, x + w - 1, y + h - 1);
  uint8_t bytesPerRow = (bt.w + 7) / 8;
  for (uint16_t py = 0; py < h; py++) {
    uint8_t srcY = py / scale;
    for (uint16_t px = 0; px < w; px++) {
      uint8_t srcX = px / scale;
      uint8_t b = pgm_read_byte(&bt.data[srcY * bytesPerRow + srcX / 8]);
      bool on = b & (0x80 >> (srcX & 7));
      uint16_t out = on ? color : bg;
      sendData(out >> 8);
      sendData(out & 0xFF);
    }
  }
}

static uint16_t centeredX(const BitmapText &bt, uint8_t scale) {
  uint16_t w = bt.w * scale;
  if (w >= LCD_W) return 0;
  return (LCD_W - w) / 2;
}

static void tftInit() {
  pinMode(PIN_SCK, OUTPUT);
  pinMode(PIN_SDA, OUTPUT);
  pinMode(PIN_CS, OUTPUT);
  pinMode(PIN_DC, OUTPUT);
  pinMode(PIN_BLK, OUTPUT);

  SCK_1;
  CS_1;
  BLK_1;
  delay(100);
  CS_0;

  sendCmd(0x11);
  delay(120);
  sendCmd(0x3A); sendData(0x05);
  sendCmd(0xC5); sendData(0x1A);
  sendCmd(0x36); sendData(0x00);
  sendCmd(0xB2); sendData(0x05); sendData(0x05); sendData(0x00); sendData(0x33); sendData(0x33);
  sendCmd(0xB7); sendData(0x05);
  sendCmd(0xBB); sendData(0x3F);
  sendCmd(0xC0); sendData(0x2C);
  sendCmd(0xC2); sendData(0x01);
  sendCmd(0xC3); sendData(0x0F);
  sendCmd(0xC4); sendData(0x20);
  sendCmd(0xC6); sendData(0x01);
  sendCmd(0xD0); sendData(0xA4); sendData(0xA1);
  sendCmd(0xE8); sendData(0x03);
  sendCmd(0xE9); sendData(0x09); sendData(0x09); sendData(0x08);

  sendCmd(0xE0);
  sendData(0xD0); sendData(0x05); sendData(0x09); sendData(0x09);
  sendData(0x08); sendData(0x14); sendData(0x28); sendData(0x33);
  sendData(0x3F); sendData(0x07); sendData(0x13); sendData(0x14);
  sendData(0x28); sendData(0x30);

  sendCmd(0xE1);
  sendData(0xD0); sendData(0x05); sendData(0x09); sendData(0x09);
  sendData(0x08); sendData(0x03); sendData(0x24); sendData(0x32);
  sendData(0x32); sendData(0x3B); sendData(0x14); sendData(0x13);
  sendData(0x28); sendData(0x2F);

  // This 2.8 inch ST7789 board shows inverted colors if INVON is used.
  sendCmd(0x20);
  sendCmd(0x29);
  CS_0;
}

// Standard I2C bus recovery: a device that got interrupted mid-byte can
// hold SDA low forever and jam the whole bus for every other device on it.
// Clocking SCL up to 9 times lets a stuck slave finish/release SDA, then a
// manual STOP condition puts the bus back to idle. Cheap to call any time
// (returns almost instantly) since it exits immediately once SDA is high.
static void recoverBus() {
  Wire.end();
  pinMode(I2C_SDA, INPUT_PULLUP);
  pinMode(I2C_SCL, OUTPUT);
  digitalWrite(I2C_SCL, HIGH);
  delayMicroseconds(5);
  for (uint8_t i = 0; i < 9 && !digitalRead(I2C_SDA); i++) {
    digitalWrite(I2C_SCL, LOW);
    delayMicroseconds(5);
    digitalWrite(I2C_SCL, HIGH);
    delayMicroseconds(5);
  }
  pinMode(I2C_SDA, OUTPUT);
  digitalWrite(I2C_SDA, LOW);
  delayMicroseconds(5);
  digitalWrite(I2C_SCL, LOW);
  delayMicroseconds(5);
  digitalWrite(I2C_SCL, HIGH);
  delayMicroseconds(5);
  digitalWrite(I2C_SDA, HIGH);
  delayMicroseconds(5);
  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(100000);
  Wire.setTimeOut(8);
  delay(5);
}

// Runs every loop, not just on a button tap: if a bad module leaves SDA or
// SCL stuck low, attempt recoverBus() immediately instead of just reporting
// "bus low" forever and waiting for someone to notice and unplug it.
static bool checkBusIdle(bool &sdaHigh, bool &sclHigh) {
  Wire.end();
  pinMode(I2C_SDA, INPUT_PULLUP);
  pinMode(I2C_SCL, INPUT_PULLUP);
  delay(8);
  sdaHigh = digitalRead(I2C_SDA);
  sclHigh = digitalRead(I2C_SCL);

  if (!sdaHigh || !sclHigh) {
    recoverBus();
    sdaHigh = digitalRead(I2C_SDA);
    sclHigh = digitalRead(I2C_SCL);
    return sdaHigh && sclHigh;
  }

  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(100000);
  Wire.setTimeOut(8);
  delay(5);
  return true;
}

static uint8_t probeAddress(uint8_t addr) {
  Wire.beginTransmission(addr);
  return Wire.endTransmission();
}

static void drawHeader(const char *status, uint16_t color) {
  fillScreen(BLACK);
  fillRect(0, 0, LCD_W, 38, BLUE);
  drawTextBg(60, 8, "I2C TESTER", WHITE, BLUE, 2);
  drawText(10, 58, status, color, 4);
}

static void drawHeaderZh(const BitmapText &status, uint16_t color) {
  fillScreen(BLACK);
  fillRect(0, 0, LCD_W, 38, BLUE);
  drawTextBg(60, 8, "I2C TESTER", WHITE, BLUE, 2);
  uint8_t scale = (status.w * 3 <= LCD_W) ? 3 : 2;
  drawBitmapText(centeredX(status, scale), 58, status, color, BLACK, scale);
}

// Sensirion SGP4x-family sensors (SGP41 etc.) speak a 2-byte-command +
// CRC-checked-word protocol at this fixed address. When that address is the
// one found on the bus we can run a real self-test / serial-read / raw
// measurement; for any other address we fall back to a generic raw read.
static const uint8_t SENSIRION_RAW_ADDR = 0x59;

// ST VL53L5CX 8x8 ToF sensor, driven through the real Adafruit driver
// (handles the internal firmware upload + ranging) instead of the blind
// generic raw read, since this chip needs a proper init sequence.
static const uint8_t VL53L5CX_ADDR = VL53L5CX_DEFAULT_ADDRESS;
static Adafruit_VL53L5CX vl53l5cx;
static bool vl53l5Mode = false;
static bool vl53l5Ready = false;

#if TEST_MODE == MODE_LABEL
static const uint32_t FULL_WARMUP_MS = 30000;
static const uint8_t FULL_STABLE_SAMPLES = 15;
static const uint16_t FULL_MAX_DELTA_OK = 500;
#else
static const uint32_t FULL_WARMUP_MS = 8000;
static const uint8_t FULL_STABLE_SAMPLES = 4;
static const uint16_t FULL_MAX_DELTA_OK = 1000;
#endif

static uint8_t activeAddr = 0;
static uint8_t devState = 255;
static uint32_t devConnectCount = 0;
static uint32_t devStartMs = 0;
static uint32_t devLastMs = 0;
static uint32_t devSerialLo = 0;
static uint16_t devLastA = 0;
static uint16_t devLastB = 0;
static uint16_t devMaxDeltaA = 0;
static uint16_t devMaxDeltaB = 0;
static uint8_t devSamples = 0;
static bool devFirstRaw = true;
static bool devSelfTestOk = false;
static bool sensirionMode = false;
static bool fullTestMode = false;
static bool suppressNextConnectCount = false;

static void drawModeButtons() {
  uint16_t quickBg = fullTestMode ? 0x4208 : GREEN;
  uint16_t fullBg = fullTestMode ? GREEN : 0x4208;
  fillRect(0, 288, 60, 32, quickBg);
  fillRect(60, 288, 60, 32, fullBg);
  fillRect(120, 288, 60, 32, RED);
  fillRect(180, 288, 60, 32, CYAN);
  drawTextBg(12, 296, "IIC", BLACK, quickBg, 2);
  drawTextBg(66, 296, "FULL", BLACK, fullBg, 2);
  drawTextBg(132, 296, "CLR", WHITE, RED, 2);
  drawTextBg(186, 296, "RFSH", BLACK, CYAN, 2);
}

static void drawRefreshing() {
  fillScreen(BLACK);
  fillRect(0, 0, LCD_W, 38, CYAN);
  drawTextBg(30, 8, "I2C TESTER", BLACK, CYAN, 2);
  drawTextBg(15, 130, "REFRESH", CYAN, BLACK, 5);
  delay(300);
}

static uint8_t sensirionCrc(const uint8_t *data) {
  uint8_t crc = 0xFF;
  for (uint8_t i = 0; i < 2; i++) {
    crc ^= data[i];
    for (uint8_t bit = 0; bit < 8; bit++) {
      crc = (crc & 0x80) ? (crc << 1) ^ 0x31 : (crc << 1);
    }
  }
  return crc;
}

static bool wordCrcOk(uint8_t msb, uint8_t lsb, uint8_t crc) {
  uint8_t data[2] = {msb, lsb};
  return sensirionCrc(data) == crc;
}

static bool sensirionSendCommand(uint16_t cmd, const uint8_t *params = nullptr, uint8_t paramLen = 0) {
  Wire.beginTransmission(activeAddr);
  Wire.write(cmd >> 8);
  Wire.write(cmd & 0xFF);
  for (uint8_t i = 0; i < paramLen; i++) Wire.write(params[i]);
  return Wire.endTransmission() == 0;
}

static bool sensirionRead(uint8_t *buf, uint8_t len) {
  uint8_t got = Wire.requestFrom(activeAddr, len);
  if (got != len) return false;
  for (uint8_t i = 0; i < len; i++) buf[i] = Wire.read();
  return true;
}

static bool sensirionReadSerial(uint32_t &serialLo) {
  uint8_t b[9];
  if (!sensirionSendCommand(0x3682)) return false;
  delay(2);
  if (!sensirionRead(b, sizeof(b))) return false;
  if (!wordCrcOk(b[0], b[1], b[2])) return false;
  if (!wordCrcOk(b[3], b[4], b[5])) return false;
  if (!wordCrcOk(b[6], b[7], b[8])) return false;
  serialLo = ((uint32_t)b[3] << 24) | ((uint32_t)b[4] << 16) | ((uint32_t)b[6] << 8) | b[7];
  return true;
}

static bool sensirionSelfTest() {
  uint8_t b[3];
  if (!sensirionSendCommand(0x280E)) return false;
  delay(330);
  if (!sensirionRead(b, sizeof(b))) return false;
  if (!wordCrcOk(b[0], b[1], b[2])) return false;
  return (b[1] & 0x03) == 0;
}

static const uint8_t SENSIRION_DEFAULT_RH_T[] = {0x80, 0x00, 0xA2, 0x66, 0x66, 0x93};

static bool sensirionConditioning(uint16_t &voc) {
  uint8_t b[3];
  if (!sensirionSendCommand(0x2612, SENSIRION_DEFAULT_RH_T, sizeof(SENSIRION_DEFAULT_RH_T))) return false;
  delay(55);
  if (!sensirionRead(b, sizeof(b))) return false;
  if (!wordCrcOk(b[0], b[1], b[2])) return false;
  voc = ((uint16_t)b[0] << 8) | b[1];
  return true;
}

static bool sensirionMeasureRaw(uint16_t &voc, uint16_t &nox) {
  uint8_t b[6];
  if (!sensirionSendCommand(0x2619, SENSIRION_DEFAULT_RH_T, sizeof(SENSIRION_DEFAULT_RH_T))) return false;
  delay(55);
  if (!sensirionRead(b, sizeof(b))) return false;
  if (!wordCrcOk(b[0], b[1], b[2])) return false;
  if (!wordCrcOk(b[3], b[4], b[5])) return false;
  voc = ((uint16_t)b[0] << 8) | b[1];
  nox = ((uint16_t)b[3] << 8) | b[4];
  return true;
}

// Works for any I2C device: just read raw bytes back, no register/command
// is written first. Good enough as a generic "is it alive and consistent"
// probe when we don't know the chip's real protocol.
static bool genericReadRaw(uint16_t &a, uint16_t &b) {
  uint8_t buf[4];
  uint8_t got = Wire.requestFrom(activeAddr, (uint8_t)4);
  if (got != 4) return false;
  for (uint8_t i = 0; i < 4; i++) buf[i] = Wire.read();
  a = ((uint16_t)buf[0] << 8) | buf[1];
  b = ((uint16_t)buf[2] << 8) | buf[3];
  return true;
}

static void resetSession() {
  devStartMs = millis();
  devLastMs = 0;
  devSerialLo = 0;
  devLastA = 0;
  devLastB = 0;
  devMaxDeltaA = 0;
  devMaxDeltaB = 0;
  devSamples = 0;
  devFirstRaw = true;
  devSelfTestOk = false;
}

// Re-checks the currently tracked address first (cheap, happens every
// loop). Only runs a full 1-126 bus scan when nothing is tracked yet or the
// tracked device just disappeared, so plugging in ANY I2C module is picked
// up automatically without hardcoding which address to expect.
static bool findActiveAddr() {
  if (activeAddr != 0) {
    if (probeAddress(activeAddr) == 0) return true;
    activeAddr = 0;
  }

  uint8_t candidate = 0;
  for (uint8_t addr = 1; addr < 127; addr++) {
    if (probeAddress(addr) == 0) {
      candidate = addr;
      break;
    }
  }
  if (candidate == 0) return false;

  // A freshly-mated connector can bounce, and a module mid power-on can
  // glitch-ACK the wrong address for a few ms. Don't latch on the first
  // hit alone: let it settle, then re-scan and only commit if the same
  // address confirms. This is what "first read shows the wrong address,
  // retesting fixes it" was — nothing wrong with the module, we were just
  // reading before the bus/power had settled.
  delay(30);
  for (uint8_t addr = 1; addr < 127; addr++) {
    if (probeAddress(addr) == 0) {
      if (addr == candidate) {
        activeAddr = addr;
        return true;
      }
      return false;
    }
  }
  return false;
}

// I2C only tells us "did anything ACK" — it cannot tell "empty socket" apart
// from "something is plugged in but not answering" (dead chip, cold solder
// joint, swapped pins, no power reaching it). Say so instead of implying a
// diagnosis the hardware can't actually make.
static void drawNoDevice() {
  drawHeaderZh(ZH_NO_BT, RED);
  drawTextBg(18, 190, "NO RESPONSE", YELLOW, BLACK, 3);
  drawTextBg(18, 224, "CHECK MODULE", WHITE, BLACK, 2);
  drawTextBg(18, 254, "T:", WHITE, BLACK, 3);
  char line[12];
  snprintf(line, sizeof(line), "%lu", devConnectCount);
  drawTextBg(58, 254, line, YELLOW, BLACK, 3);
  drawModeButtons();
}

static void drawLost() {
  drawHeaderZh(ZH_NO_BT, RED);
  drawBitmapText(40, 118, ZH_REMOVED_BT, YELLOW, BLACK, 2);
  fillRect(0, 244, LCD_W, 64, RED);
  drawTextBg(48, 258, "LOST", WHITE, RED, 4);
}

static void drawQuickOk() {
  fillScreen(BLACK);
  fillRect(0, 0, LCD_W, 38, BLUE);
  drawTextBg(60, 8, fullTestMode ? "I2C FULL" : "I2C QUICK", WHITE, BLUE, 2);
  drawTextBg(60, 58, "OK", GREEN, BLACK, 9);
  drawTextBg(18, 136, "ADDR", CYAN, BLACK, 3);
  fillRect(8, 174, 224, 70, 0x03E0);
  char addrLabel[8];
  snprintf(addrLabel, sizeof(addrLabel), "0x%02X", activeAddr);
  drawTextBg(28, 184, addrLabel, BLACK, 0x03E0, 7);
  char line[16];
  snprintf(line, sizeof(line), "T:%lu", devConnectCount);
  drawTextBg(18, 250, line, YELLOW, BLACK, 4);
  drawModeButtons();
}

static void drawBad(const char *reason) {
  fillScreen(BLACK);
  fillRect(0, 0, LCD_W, 38, RED);
  drawTextBg(10, 8, "I2C TEST", WHITE, RED, 2);
  drawTextBg(34, 78, "BAD", RED, BLACK, 8);
  drawTextBg(14, 184, reason, YELLOW, BLACK, 3);
  drawTextBg(14, 246, "CHANGE MOD", WHITE, BLACK, 3);
}

static void drawWarm(uint8_t secondsLeft, uint16_t voc) {
  fillScreen(BLACK);
  fillRect(0, 0, LCD_W, 38, BLUE);
  drawTextBg(60, 8, "I2C FULL", WHITE, BLUE, 2);
  drawTextBg(18, 70, "WARM", YELLOW, BLACK, 6);
  char line[18];
  snprintf(line, sizeof(line), "%02us", secondsLeft);
  drawTextBg(68, 142, line, WHITE, BLACK, 5);
  snprintf(line, sizeof(line), "VOC %u", voc);
  drawTextBg(18, 214, line, GREEN, BLACK, 3);
  drawTextBg(18, 252, "SELF OK", GREEN, BLACK, 3);
  drawModeButtons();
}

static void drawFullResult(uint16_t a, uint16_t b, uint16_t da, uint16_t db, const char *labelA, const char *labelB) {
  fillScreen(BLACK);
  fillRect(0, 0, LCD_W, 38, BLUE);
  bool invalid = a == 0 || b == 0 || a == 65535 || b == 65535;
  bool stable = devSamples >= FULL_STABLE_SAMPLES && devMaxDeltaA < FULL_MAX_DELTA_OK && devMaxDeltaB < FULL_MAX_DELTA_OK;

  drawTextBg(60, 8, "I2C FULL", WHITE, BLUE, 2);
  drawTextBg(8, 54, labelA, CYAN, BLACK, 4);
  char line[18];
  snprintf(line, sizeof(line), "%u", a);
  drawTextBg(86, 54, line, GREEN, BLACK, 4);
  drawTextBg(8, 110, labelB, CYAN, BLACK, 4);
  snprintf(line, sizeof(line), "%u", b);
  drawTextBg(86, 110, line, GREEN, BLACK, 4);
  snprintf(line, sizeof(line), "D %u/%u", da, db);
  drawTextBg(12, 170, line, YELLOW, BLACK, 3);
  snprintf(line, sizeof(line), "S:%u T:%lu", devSamples, devConnectCount);
  drawTextBg(12, 212, line, WHITE, BLACK, 3);

  uint16_t bar = invalid ? RED : (stable ? GREEN : YELLOW);
  fillRect(0, 246, LCD_W, 42, bar);
  drawTextBg(stable ? 70 : 40, 254, invalid ? "BAD" : (stable ? "PASS" : "WAIT"), BLACK, bar, 3);
  drawModeButtons();
}

static void recordSample(uint16_t a, uint16_t b, const char *labelA, const char *labelB) {
  if (devFirstRaw) {
    devFirstRaw = false;
    devLastA = a;
    devLastB = b;
    return;
  }
  uint16_t da = abs((int32_t)a - (int32_t)devLastA);
  uint16_t db = abs((int32_t)b - (int32_t)devLastB);
  if (da > devMaxDeltaA) devMaxDeltaA = da;
  if (db > devMaxDeltaB) devMaxDeltaB = db;
  devLastA = a;
  devLastB = b;
  devSamples++;
  devState = 3;
  drawFullResult(a, b, da, db, labelA, labelB);
  Serial.print("raw ");
  Serial.print(labelA);
  Serial.print("=");
  Serial.print(a);
  Serial.print(" ");
  Serial.print(labelB);
  Serial.print("=");
  Serial.print(b);
  Serial.print(" d=");
  Serial.print(da);
  Serial.print("/");
  Serial.println(db);
}

static void i2cTestLoop() {
  bool sdaHigh = false;
  bool sclHigh = false;
  if (!checkBusIdle(sdaHigh, sclHigh)) {
    if (devState != 0) {
      devState = 0;
      drawHeaderZh(ZH_BUS_BT, RED);
      if (!sdaHigh) drawText(10, 132, "SDA LOW", YELLOW, 3);
      if (!sclHigh) drawText(10, 174, "SCL LOW", YELLOW, 3);
      drawText(10, 238, "SHORT/BAD WIRE", RED, 2);
    }
    return;
  }

  if (!findActiveAddr()) {
    if (devState == 2 || devState == 3) {
      devState = 4;
      drawLost();
      return;
    }
    if (devState != 1) {
      devState = 1;
      drawNoDevice();
    }
    return;
  }

  if (!fullTestMode) {
    if (devState != 3) {
      devState = 3;
      if (suppressNextConnectCount) {
        suppressNextConnectCount = false;
      } else {
        devConnectCount++;
      }
      drawQuickOk();
      Serial.print("quick OK addr=0x");
      Serial.println(activeAddr, HEX);
    }
    return;
  }

  if (devState != 2 && devState != 3) {
    if (suppressNextConnectCount) {
      suppressNextConnectCount = false;
    } else {
      devConnectCount++;
    }
    resetSession();
    devState = 3;
    sensirionMode = (activeAddr == SENSIRION_RAW_ADDR);
    vl53l5Mode = (activeAddr == VL53L5CX_ADDR);
    vl53l5Ready = false;
    fillScreen(BLACK);
    fillRect(0, 0, LCD_W, 38, BLUE);
    drawTextBg(60, 8, "I2C FULL", WHITE, BLUE, 2);
    if (sensirionMode) {
      drawTextBg(12, 66, "SELF TEST", YELLOW, BLACK, 3);
      if (!sensirionReadSerial(devSerialLo)) {
        drawBad("SERIAL ERR");
        devState = 5;
        return;
      }
      if (!sensirionSelfTest()) {
        drawBad("SELF TEST");
        devState = 5;
        return;
      }
    } else if (vl53l5Mode) {
      drawTextBg(12, 66, "TOF INIT", YELLOW, BLACK, 3);
      // begin() uploads ~80KB of firmware into the sensor over I2C and can
      // legitimately take several seconds — pull the loop task off the
      // watchdog for just this one call so that doesn't look like a hang.
      esp_task_wdt_delete(NULL);
      bool tofOk = vl53l5cx.begin(VL53L5CX_ADDR, &Wire, 400000) &&
                   vl53l5cx.setResolution(64) &&
                   vl53l5cx.setRangingFrequency(15) &&
                   vl53l5cx.startRanging();
      esp_task_wdt_add(NULL);
      if (!tofOk) {
        drawBad("TOF INIT ERR");
        devState = 5;
        return;
      }
      vl53l5Ready = true;
    } else {
      drawTextBg(12, 66, "RAW SCAN", YELLOW, BLACK, 3);
    }
    devSelfTestOk = true;
  }

  if (millis() - devLastMs < 1000) return;
  devLastMs = millis();

  if (sensirionMode) {
    uint32_t elapsed = millis() - devStartMs;
    if (elapsed < FULL_WARMUP_MS) {
      uint16_t voc = 0;
      if (!sensirionConditioning(voc)) {
        drawBad("COND ERR");
        devState = 5;
        return;
      }
      uint8_t left = (FULL_WARMUP_MS - elapsed + 999) / 1000;
      drawWarm(left, voc);
      return;
    }

    uint16_t voc = 0, nox = 0;
    if (!sensirionMeasureRaw(voc, nox)) {
      drawBad("READ ERR");
      devState = 5;
      return;
    }
    recordSample(voc, nox, "VOC", "NOX");
  } else if (vl53l5Mode) {
    if (!vl53l5cx.isDataReady()) return;
    VL53L5CX_ResultsData results;
    if (!vl53l5cx.getRangingData(&results)) {
      drawBad("TOF READ ERR");
      devState = 5;
      return;
    }
    // 8x8 grid, row-major: zone 27 sits near the middle, zone 0 is a corner.
    uint16_t center = (uint16_t)results.distance_mm[27];
    uint16_t corner = (uint16_t)results.distance_mm[0];
    recordSample(center, corner, "CTR", "COR");
  } else {
    uint16_t a = 0, b = 0;
    if (!genericReadRaw(a, b)) {
      drawBad("READ ERR");
      devState = 5;
      return;
    }
    recordSample(a, b, "A", "B");
  }
}

static bool touchWasDown = false;
static uint32_t touchDownMs = 0;
static uint32_t lastTouchActionMs = 0;
static uint16_t lastTouchX = 0;
static uint16_t lastTouchY = 0;

static void touchInit() {
  pinMode(TOUCH_CS, OUTPUT);
  pinMode(TOUCH_SCLK, OUTPUT);
  pinMode(TOUCH_MOSI, OUTPUT);
  pinMode(TOUCH_MISO, INPUT);
  pinMode(TOUCH_IRQ, INPUT_PULLUP);
  digitalWrite(TOUCH_CS, HIGH);
  digitalWrite(TOUCH_SCLK, LOW);
  digitalWrite(TOUCH_MOSI, LOW);
}

static uint8_t touchTransfer(uint8_t value) {
  uint8_t in = 0;
  for (uint8_t i = 0; i < 8; i++) {
    digitalWrite(TOUCH_MOSI, (value & 0x80) ? HIGH : LOW);
    value <<= 1;
    digitalWrite(TOUCH_SCLK, HIGH);
    delayMicroseconds(1);
    in <<= 1;
    if (digitalRead(TOUCH_MISO)) in |= 1;
    digitalWrite(TOUCH_SCLK, LOW);
    delayMicroseconds(1);
  }
  return in;
}

static uint16_t touchRead12(uint8_t command) {
  digitalWrite(TOUCH_CS, LOW);
  touchTransfer(command);
  uint16_t hi = touchTransfer(0x00);
  uint16_t lo = touchTransfer(0x00);
  digitalWrite(TOUCH_CS, HIGH);
  return ((hi << 8) | lo) >> 3;
}

static bool touchDown(uint16_t &rawX, uint16_t &rawY) {
  if (digitalRead(TOUCH_IRQ) != LOW) return false;
  rawX = touchRead12(0xD0);
  rawY = touchRead12(0x90);
  return rawX > 100 && rawX < 4000 && rawY > 100 && rawY < 4000;
}

static void handleTouch() {
  uint16_t rawX = 0;
  uint16_t rawY = 0;
  bool down = touchDown(rawX, rawY);
  uint32_t now = millis();

  if (down && !touchWasDown) {
    touchDownMs = now;
  }
  if (down) {
    lastTouchX = rawX;
    lastTouchY = rawY;
  }

  if (!down && touchWasDown && now - lastTouchActionMs > 350) {
    lastTouchActionMs = now;
    bool longPress = now - touchDownMs >= 1000;
    uint16_t buttonAxis = lastTouchX;
    // Screen is split into 4 equal 60px zones left-to-right: IIC/FULL/CLR/RFSH.
    // Touch raw X runs inverted (high raw = screen left), so thresholds
    // fall as buttonAxis increases from RFSH (rightmost) to IIC (leftmost).
    if (longPress) {
      devConnectCount = 0;
      suppressNextConnectCount = fullTestMode;
      Serial.println("touch clear count (long press)");
    } else if (buttonAxis >= 3025) {
      fullTestMode = false;
      suppressNextConnectCount = true;
      Serial.println("touch mode: quick");
    } else if (buttonAxis >= 2050) {
      fullTestMode = true;
      suppressNextConnectCount = true;
      Serial.println("touch mode: full");
    } else if (buttonAxis >= 1075) {
      devConnectCount = 0;
      suppressNextConnectCount = fullTestMode;
      Serial.println("touch clear count");
    } else {
      drawRefreshing();
      Serial.println("touch refresh");
    }

    recoverBus();
    if (findActiveAddr()) {
      if (fullTestMode) {
        devState = 255;
        resetSession();
      } else {
        devState = 3;
        drawQuickOk();
      }
    } else {
      devState = 1;
      drawNoDevice();
    }
  }
  touchWasDown = down;
}

void setup() {
  Serial.begin(115200);
  delay(500);
  Serial.println("ESP32-2432S028 I2C Tester");

  // Safety net: some bad/miswired modules can wedge the I2C driver in a
  // blocking call that recoverBus() never gets a chance to run (control
  // never returns to our code). If loop() stops feeding this watchdog for
  // 4s straight, the chip force-reboots itself instead of staying frozen
  // until someone finds the physical reset button.
  esp_task_wdt_config_t wdtConfig = {
    .timeout_ms = 4000,
    .idle_core_mask = 0,
    .trigger_panic = true,
  };
  esp_task_wdt_init(&wdtConfig);
  esp_task_wdt_add(NULL);

  tftInit();
  touchInit();
  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(100000);
  Wire.setTimeOut(8);
  delay(100);
  i2cTestLoop();
}

void loop() {
  esp_task_wdt_reset();
  delay(120);
  handleTouch();
  i2cTestLoop();
}