#include <Wire.h>
#include "soc/gpio_struct.h"

#define PIN_SCK  12
#define PIN_SDA  11
#define PIN_RST  8
#define PIN_DC   9
#define PIN_BLK  7

#define I2C_SDA 4
#define I2C_SCL 5

#define SCK_MASK (1UL << PIN_SCK)
#define SDA_MASK (1UL << PIN_SDA)
#define RST_MASK (1UL << PIN_RST)
#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 RST_0 FAST_LOW(RST_MASK)
#define RST_1 FAST_HIGH(RST_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 bool previousFound[128];
static uint8_t previousState = 255;
static bool previousSdaHigh = true;
static bool previousSclHigh = true;
static bool activeSensorCounted = false;
static uint32_t testedOkCount = 0;
static uint32_t scanNumber = 0;

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 >= 240 || y >= 240) 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 >= 240 || y >= 240) return;
  if (x + w > 240) w = 240 - x;
  if (y + h > 240) h = 240 - 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, 239, 239);
  for (uint32_t i = 0; i < 240UL * 240UL; 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 tftInit() {
  pinMode(PIN_SCK, OUTPUT);
  pinMode(PIN_SDA, OUTPUT);
  pinMode(PIN_RST, OUTPUT);
  pinMode(PIN_DC, OUTPUT);
  pinMode(PIN_BLK, OUTPUT);

  SCK_1;
  BLK_1;
  RST_0;
  delay(1000);
  RST_1;
  delay(1000);

  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);

  sendCmd(0x21);
  sendCmd(0x29);
}

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);
  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(100000);
  Wire.setTimeOut(8);
  delay(5);
  return sdaHigh && sclHigh;
}

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

static void drawAddressGrid(bool found[]) {
  char label[5];
  uint8_t count = 0;
  uint8_t onlyAddr = 0;
  for (uint8_t addr = 1; addr < 127; addr++) {
    if (found[addr]) {
      count++;
      onlyAddr = addr;
    }
  }

  if (count == 1) {
    snprintf(label, sizeof(label), "0x%02X", onlyAddr);
    fillRect(36, 118, 168, 52, 0x03E0);
    drawTextBg(58, 126, label, BLACK, 0x03E0, 5);
    return;
  }

  uint8_t index = 0;
  for (uint8_t addr = 1; addr < 127; addr++) {
    if (!found[addr]) continue;
    uint16_t x = 10 + (index % 2) * 116;
    uint16_t y = 116 + (index / 2) * 42;
    if (y > 194) {
      drawText(10, 226, "MORE", YELLOW, 2);
      break;
    }
    fillRect(x - 4, y - 5, 102, 38, 0x03E0);
    snprintf(label, sizeof(label), "0x%02X", addr);
    drawTextBg(x, y, label, BLACK, 0x03E0, 4);
    index++;
  }
}

static void scanI2C() {
  scanNumber++;
  bool sdaHigh = false;
  bool sclHigh = false;
  if (!checkBusIdle(sdaHigh, sclHigh)) {
    activeSensorCounted = false;
    if (previousState == 0 && previousSdaHigh == sdaHigh && previousSclHigh == sclHigh) {
      return;
    }
    previousState = 0;
    previousSdaHigh = sdaHigh;
    previousSclHigh = sclHigh;
    drawHeader("BUS LOW", RED);
    if (!sdaHigh) drawText(10, 112, "SDA LOW", YELLOW, 4);
    if (!sclHigh) drawText(10, 152, "SCL LOW", YELLOW, 4);
    drawText(10, 204, "SHORT", RED, 3);
    Serial.println("I2C bus low: check short/bad contact");
    return;
  }

  bool found[128] = {};
  int count = 0;
  int newCount = 0;
  int lostCount = 0;

  for (uint8_t addr = 1; addr < 127; addr++) {
    Wire.beginTransmission(addr);
    uint8_t error = Wire.endTransmission();
    found[addr] = (error == 0);
    if (found[addr]) {
      count++;
      if (!previousFound[addr] && scanNumber > 1) newCount++;
    } else if (previousFound[addr] && scanNumber > 1) {
      lostCount++;
    }
  }

  uint8_t state = count == 0 ? 1 : 2;
  if (count == 0) {
    activeSensorCounted = false;
  } else if (!activeSensorCounted) {
    testedOkCount++;
    activeSensorCounted = true;
  }

  bool changed = previousState != state;
  for (uint8_t addr = 1; addr < 127; addr++) {
    if (found[addr] != previousFound[addr]) {
      changed = true;
      break;
    }
  }

  if (!changed) {
    return;
  }

  previousState = state;
  previousSdaHigh = sdaHigh;
  previousSclHigh = sclHigh;

  char line[18];
  if (count == 0) {
    drawHeader("NO SENSOR", RED);
    drawText(10, 116, "NOT FOUND", YELLOW, 3);
    drawText(10, 158, "CHECK WIRE", WHITE, 2);
    drawText(10, 186, "VCC GND SDA SCL", WHITE, 1);
  } else {
    drawHeader("SENSOR OK", GREEN);
    snprintf(line, sizeof(line), "ADDR:%d", count);
    drawText(10, 86, line, YELLOW, 3);
    snprintf(line, sizeof(line), "TEST:%lu", testedOkCount);
    drawText(126, 90, line, CYAN, 2);
    drawAddressGrid(found);
  }

  if (newCount || lostCount) {
    snprintf(line, sizeof(line), "NEW:%d LOST:%d", newCount, lostCount);
    fillRect(0, 218, 240, 22, lostCount ? RED : GREEN);
    drawTextBg(10, 222, line, WHITE, lostCount ? RED : GREEN, 2);
  }

  for (uint8_t addr = 0; addr < 128; addr++) {
    previousFound[addr] = found[addr];
  }

  Serial.print("scan ");
  Serial.print(scanNumber);
  Serial.print(": ");
  Serial.print(count);
  Serial.println(" device(s)");
}

static void scanI2COld() {
  fillScreen(BLACK);
  drawText(18, 16, "I2C SCANNER", CYAN, 2);
  drawText(18, 42, "SDA:4 SCL:5", WHITE, 1);

  int count = 0;
  int y = 68;
  char line[16];

  for (uint8_t addr = 1; addr < 127; addr++) {
    Wire.beginTransmission(addr);
    uint8_t error = Wire.endTransmission();
    if (error == 0) {
      snprintf(line, sizeof(line), "FOUND 0x%02X", addr);
      Serial.println(line);
      drawText(18, y, line, GREEN, 2);
      y += 22;
      count++;
      if (y > 210) break;
    }
  }

  if (count == 0) {
    drawText(18, 82, "NO DEVICE", RED, 2);
    drawText(18, 112, "CHECK WIRE", YELLOW, 1);
  } else {
    snprintf(line, sizeof(line), "TOTAL:%d", count);
    drawText(18, y + 8, line, YELLOW, 1);
  }
}

void setup() {
  Serial.begin(115200);
  delay(500);
  Serial.println("GMT130 ST7789 I2C Scanner");

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

void loop() {
  delay(120);
  scanI2C();
}