// QPAD diagnostic firmware v2 for Seeed XIAO RP2040.
//
// Touch timing and the QPAD pin mapping are adapted from Quentin Bolsee's
// MIT-licensed QPAD-xiao test_touch_RP2040 example:
// https://forge.cba.mit.edu/quentin.bolsee/qpad-xiao
//
// Version 2 adds repeated sampling and release debouncing after physical tests
// showed that a held touch can contain brief low readings.

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

namespace {

constexpr uint8_t kLedRedPin = 17;
constexpr uint8_t kLedGreenPin = 16;
constexpr uint8_t kLedBluePin = 25;
constexpr uint8_t kOledSdaPin = 6;
constexpr uint8_t kOledSclPin = 7;
constexpr uint8_t kScreenAddress = 0x3C;

constexpr uint8_t kTouchCount = 6;
constexpr uint8_t kTouchPins[kTouchCount] = {3, 4, 2, 27, 1, 26};
constexpr uint16_t kTouchMaximum = 200;
constexpr uint16_t kTouchSettleMicroseconds = 25;
constexpr uint8_t kSamplesPerReading = 4;
constexpr uint8_t kReleaseDebounceSamples = 24;

constexpr unsigned long kDisplayPeriodMs = 100;
constexpr unsigned long kStreamPeriodMs = 200;
constexpr size_t kCommandBufferSize = 64;

Adafruit_SSD1306 display(128, 64, &Wire, -1, 1700000UL, 1700000UL);

uint16_t touchValues[kTouchCount] = {};
bool touchedNow[kTouchCount] = {};
uint8_t releaseCounts[kTouchCount] = {};
uint16_t touchThreshold = 6;
uint32_t touchEventCount = 0;
bool streamEnabled = false;
bool displayReady = false;

char commandBuffer[kCommandBufferSize] = {};
size_t commandLength = 0;
unsigned long lastDisplayMs = 0;
unsigned long lastStreamMs = 0;

void writeActiveLowLed(uint8_t pin, bool enabled) {
  digitalWrite(pin, enabled ? LOW : HIGH);
}

void setRgb(bool red, bool green, bool blue) {
  writeActiveLowLed(kLedRedPin, red);
  writeActiveLowLed(kLedGreenPin, green);
  writeActiveLowLed(kLedBluePin, blue);
}

void setLedForTouchMask(uint8_t mask) {
  if (mask == 0) {
    setRgb(false, false, false);
    return;
  }

  uint8_t firstPad = 0;
  while (firstPad < kTouchCount && !(mask & (1U << firstPad))) ++firstPad;

  switch (firstPad) {
    case 0: setRgb(true, false, false); break;
    case 1: setRgb(false, true, false); break;
    case 2: setRgb(false, false, true); break;
    case 3: setRgb(true, true, false); break;
    case 4: setRgb(false, true, true); break;
    default: setRgb(true, false, true); break;
  }
}

uint16_t measureTouchSample(uint8_t pin) {
  pinMode(pin, OUTPUT);
  digitalWriteFast(pin, LOW);
  delayMicroseconds(kTouchSettleMicroseconds);
  pinMode(pin, INPUT_PULLUP);

  uint16_t elapsed = 0;
  while (!digitalReadFast(pin) && elapsed < kTouchMaximum) ++elapsed;
  return elapsed;
}

uint16_t measureTouch(uint8_t pin) {
  uint16_t maximum = 0;
  for (uint8_t i = 0; i < kSamplesPerReading; ++i) {
    uint16_t sample = measureTouchSample(pin);
    if (sample > maximum) maximum = sample;
  }
  return maximum;
}

uint8_t touchMask() {
  uint8_t mask = 0;
  for (uint8_t i = 0; i < kTouchCount; ++i) {
    if (touchedNow[i]) mask |= 1U << i;
  }
  return mask;
}

void reportEvent(uint8_t pad, bool touched) {
  ++touchEventCount;
  Serial.print("event,pad,");
  Serial.print(pad);
  Serial.print(touched ? ",down," : ",up,");
  Serial.println(touchValues[pad]);
}

void updateTouch() {
  for (uint8_t i = 0; i < kTouchCount; ++i) {
    bool previous = touchedNow[i];
    touchValues[i] = measureTouch(kTouchPins[i]);

    if (touchValues[i] > touchThreshold) {
      touchedNow[i] = true;
      releaseCounts[i] = 0;
    } else if (touchedNow[i]) {
      if (releaseCounts[i] < kReleaseDebounceSamples) ++releaseCounts[i];
      if (releaseCounts[i] >= kReleaseDebounceSamples) {
        touchedNow[i] = false;
        releaseCounts[i] = 0;
      }
    } else {
      releaseCounts[i] = 0;
    }

    if (touchedNow[i] != previous) reportEvent(i, touchedNow[i]);
  }
  setLedForTouchMask(touchMask());
}

void printStatus() {
  Serial.println("QPAD status v2");
  Serial.print("threshold=");
  Serial.println(touchThreshold);
  Serial.print("samples-per-reading=");
  Serial.println(kSamplesPerReading);
  Serial.print("release-debounce-samples=");
  Serial.println(kReleaseDebounceSamples);
  Serial.print("stream=");
  Serial.println(streamEnabled ? "on" : "off");
  Serial.print("oled=");
  Serial.println(displayReady ? "ready" : "not-found");
  Serial.print("pins=");
  for (uint8_t i = 0; i < kTouchCount; ++i) {
    if (i) Serial.print(',');
    Serial.print(kTouchPins[i]);
  }
  Serial.println();
  Serial.print("values=");
  for (uint8_t i = 0; i < kTouchCount; ++i) {
    if (i) Serial.print(',');
    Serial.print(touchValues[i]);
  }
  Serial.println();
  Serial.print("mask=0x");
  Serial.println(touchMask(), HEX);
  Serial.print("events=");
  Serial.println(touchEventCount);
}

void printHelp() {
  Serial.println("QPAD commands: help, status, stream on, stream off, threshold N");
}

void handleCommand(char *command) {
  if (!strcmp(command, "help")) {
    printHelp();
  } else if (!strcmp(command, "status")) {
    printStatus();
  } else if (!strcmp(command, "stream on")) {
    streamEnabled = true;
    Serial.println("ok,stream,on");
  } else if (!strcmp(command, "stream off")) {
    streamEnabled = false;
    Serial.println("ok,stream,off");
  } else if (!strncmp(command, "threshold ", 10)) {
    char *end = nullptr;
    long requested = strtol(command + 10, &end, 10);
    if (end != command + 10 && *end == '\0' && requested >= 0 &&
        requested <= kTouchMaximum) {
      touchThreshold = static_cast<uint16_t>(requested);
      Serial.print("ok,threshold,");
      Serial.println(touchThreshold);
    } else {
      Serial.println("error,threshold,expected 0..200");
    }
  } else if (command[0] != '\0') {
    Serial.print("error,unknown-command,");
    Serial.println(command);
  }
}

void readCommands() {
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\r') continue;
    if (c == '\n') {
      commandBuffer[commandLength] = '\0';
      handleCommand(commandBuffer);
      commandLength = 0;
    } else if (commandLength < kCommandBufferSize - 1) {
      commandBuffer[commandLength++] = c;
    } else {
      commandLength = 0;
      Serial.println("error,command-too-long");
    }
  }
}

void printStreamRow(unsigned long now) {
  Serial.print("touch,");
  Serial.print(now);
  for (uint8_t i = 0; i < kTouchCount; ++i) {
    Serial.print(',');
    Serial.print(touchValues[i]);
  }
  Serial.print(",0x");
  Serial.println(touchMask(), HEX);
}

void drawDisplay() {
  if (!displayReady) return;
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("QPAD v2 t=");
  display.print(touchThreshold);
  display.print(" m=");
  display.println(touchMask(), HEX);

  for (uint8_t i = 0; i < kTouchCount; ++i) {
    display.setCursor((i % 2) * 64, 14 + (i / 2) * 13);
    display.print(i + 1);
    display.print(touchedNow[i] ? '*' : ':');
    display.print(touchValues[i]);
  }

  display.setCursor(0, 54);
  display.print("events ");
  display.print(touchEventCount);
  display.print(streamEnabled ? " stream" : " paused");
  display.display();
}

}  // namespace

void setup() {
  Serial.begin(115200);
  pinMode(kLedRedPin, OUTPUT);
  pinMode(kLedGreenPin, OUTPUT);
  pinMode(kLedBluePin, OUTPUT);
  setRgb(false, false, false);

  Wire.setSDA(kOledSdaPin);
  Wire.setSCL(kOledSclPin);
  delay(50);
  displayReady = display.begin(SSD1306_SWITCHCAPVCC, kScreenAddress);
  if (displayReady) {
    display.clearDisplay();
    display.display();
  }

  Serial.println("QPAD diagnostic v2 ready");
  printHelp();
  if (!displayReady) Serial.println("warning,oled-not-found");
}

void loop() {
  unsigned long now = millis();
  readCommands();
  updateTouch();

  if (now - lastDisplayMs >= kDisplayPeriodMs) {
    lastDisplayMs = now;
    drawDisplay();
  }
  if (streamEnabled && now - lastStreamMs >= kStreamPeriodMs) {
    lastStreamMs = now;
    printStreamRow(now);
  }
}