#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

/********* 硬件与显示 *********/
#define SCREEN_WIDTH    128
#define SCREEN_HEIGHT   64
#define SCREEN_ADDRESS  0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1, 1700000UL, 1700000UL);

// 触摸引脚（按官方 pinout）P03,P04,P02,P27,P01,P26
#define N_TOUCH 6
int touch_pins[N_TOUCH] = {3, 4, 2, 27, 1, 26};

/********* 时序设置 *********/
const uint32_t SAFE_WINDOW_MS   = 3000;  // 上电安全窗
volatile uint16_t T_MAX_US      = 500;   // 测时上限（µs）
const uint32_t UI_INTERVAL_MS   = 50;    // OLED 刷新
bool enable_csv_serial          = false; // 串口CSV输出（默认关）

/********* 状态缓存 *********/
unsigned long t_boot = 0;
uint16_t touch_us[N_TOUCH]      = {0};
bool     touched[N_TOUCH]       = {0};
uint32_t last_ui_ms = 0;
uint32_t last_csv_ms = 0;

/********* 辅助 *********/
static inline void usb_friendly_yield() { yield(); }

// 单通道测时：放电→上拉→计时到高电平，带超时保护
static inline uint16_t measure_touch_us(int pin) {
  pinMode(pin, OUTPUT);
  digitalWriteFast(pin, LOW);
  delayMicroseconds(25);

  pinMode(pin, INPUT_PULLUP);
  unsigned long t0 = micros();

  while (!digitalReadFast(pin)) {
    if ((uint16_t)(micros() - t0) >= T_MAX_US) break;
    usb_friendly_yield();
  }
  return (uint16_t)(micros() - t0);
}

/********* 串口命令 *********/
void print_help() {
  Serial.println("Commands:");
  Serial.println("  h        : help");
  Serial.println("  p        : toggle CSV print");
  Serial.println("  m <us>   : set T_MAX_US (default 500)");
}

void handle_serial_cmd() {
  if (!Serial.available()) return;
  String s = Serial.readStringUntil('\n');
  s.trim();
  if (!s.length()) return;

  if (s.equalsIgnoreCase("h")) { print_help(); return; }
  if (s.equalsIgnoreCase("p")) { enable_csv_serial = !enable_csv_serial; Serial.print("CSV: "); Serial.println(enable_csv_serial ? "ON":"OFF"); return; }
  if (tolower(s.charAt(0)) == 'm') {
    long v = s.substring(1).toInt();
    if (v >= 100 && v < 60000) { T_MAX_US = (uint16_t)v; Serial.print("T_MAX_US = "); Serial.println(T_MAX_US); }
    else Serial.println("Invalid T_MAX");
    return;
  }

  Serial.println("Unknown command. 'h' for help.");
}

/********* OLED 绘制（自适应缩放） *********/
void draw_ui() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  // 顶部：显示参数
  display.setCursor(0, 0);
  display.print("6 ch live test");

  // 动态满幅
  uint16_t dyn_max = 0;
  for (int i = 0; i < N_TOUCH; i++) dyn_max = max(dyn_max, touch_us[i]);
  if (dyn_max < 20) dyn_max = 20;

  // 每行布局
  const int x0 = 0;
  const int y0 = 10;
  const int row_h = 9;
  const int label_w = 28;
  const int bar_x = x0 + label_w;
  const int bar_w = SCREEN_WIDTH - bar_x - 2;
  const int bar_h = row_h - 2;

  for (int i = 0; i < N_TOUCH; i++) {
    int y = y0 + i * row_h;

    // 标签
    display.setCursor(x0, y);
    display.print("ch");
    display.print(i);
    display.print(':');

    // 条形图
    uint16_t v = touch_us[i];
    if (v > T_MAX_US) v = T_MAX_US;
    int w = (int)((uint32_t)v * bar_w / (uint32_t)dyn_max);

    display.drawRect(bar_x, y, bar_w, bar_h, SSD1306_WHITE);
    if (w > 0) display.fillRect(bar_x + 1, y + 1, max(0, w - 2), max(0, bar_h - 2), SSD1306_WHITE);

    // 行尾显示触发 1/0（非0即1）
    display.setCursor(SCREEN_WIDTH - 18, y);
    display.print(touched[i] ? '1' : '0');
  }

  display.display();
}

/********* Arduino 标准入口 *********/
void setup() {
  pinMode(LED_BUILTIN, OUTPUT);

  Serial.begin(115200);
  t_boot = millis();

  // I2C 与 OLED
  Wire.begin();                 // 默认 Wire 即 GPIO4/5
  Wire.setClock(400000);        // 不稳可降到 100000
  delay(50);
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println("SSD1306 init failed!");
  } else {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println("XIAO RP2040 Touch UI");
    display.println("OLED 128x64 @0x3C");
    display.println("Logic: !=0 pressed");
    display.display();
  }

  if (Serial) {
    Serial.println("Ready. Type 'h' for help.");
  }
}

void loop() {
  // 安全窗
  if (millis() - t_boot < SAFE_WINDOW_MS) {
    digitalWrite(LED_BUILTIN, (millis()/250) % 2);
    handle_serial_cmd();
    delay(50);
    return;
  }

  // 采样六通道并判定（非0即触发）
  for (int i = 0; i < N_TOUCH; i++) {
    touch_us[i] = measure_touch_us(touch_pins[i]);
    touched[i]  = (touch_us[i] != 0);
  }

  // 串口命令
  handle_serial_cmd();

  // OLED 刷新
  uint32_t now = millis();
  if (now - last_ui_ms >= UI_INTERVAL_MS) {
    last_ui_ms = now;
    draw_ui();

    // 心跳
    digitalWrite(LED_BUILTIN, HIGH); delay(2);
    digitalWrite(LED_BUILTIN, LOW);
  }

  // 可选 CSV 输出
  if (enable_csv_serial && (now - last_csv_ms >= UI_INTERVAL_MS)) {
    last_csv_ms = now;
    for (int i = 0; i < N_TOUCH; i++) {
      Serial.print(touch_us[i]);
      Serial.write(i < N_TOUCH - 1 ? ',' : ' ');
    }
    for (int i = 0; i < N_TOUCH; i++) {
      Serial.print(touched[i] ? 1 : 0);
      Serial.write(i < N_TOUCH - 1 ? ',' : '\n');
    }
  }

  usb_friendly_yield();
}