#include <Arduino.h>

// ====== Adjustable parameters ======
#define N_TOUCH 6
int touch_pins[N_TOUCH] = {3, 4, 2, 27, 1, 26};  // Your 6 touch pins
const uint32_t SAFE_WINDOW_MS = 3000;           // Power-up safety window
const uint16_t T_MAX_US       = 5000;           // Maximum time per measurement (microseconds)
const uint32_t PRINT_INTERVAL_MS = 50;          // Print once every 50 ms
const uint16_t HEADER_EVERY_LINES = 40;         // Reprint header every 40 lines

// ====== Global state ======
unsigned long t_boot = 0;
uint16_t touch_us[N_TOUCH] = {0};
uint32_t last_print_ms = 0;
uint16_t lines_since_header = 0;

// Let the USB stack take a breath
static inline void usb_friendly_yield() {
  yield();  // Philhower RP2040 core schedules background tasks (including USB) here
}

// Measure the time (in microseconds) for a given touch pin
// to discharge and then be pulled high
static inline uint16_t measure_touch_us(int pin) {
  // Discharge
  pinMode(pin, OUTPUT);
  digitalWriteFast(pin, LOW);
  delayMicroseconds(25);

  // Enable pull-up and start timing
  pinMode(pin, INPUT_PULLUP);
  unsigned long t0 = micros();

  while (!digitalReadFast(pin)) {
    if ((uint16_t)(micros() - t0) >= T_MAX_US) break;  // Timeout protection
    usb_friendly_yield();                              // Do not block USB
  }
  return (uint16_t)(micros() - t0);
}

void print_header() {
  Serial.println("t1_us,t2_us,t3_us,t4_us,t5_us,t6_us");
}

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(115200);    // For faster speed, you can use 921600 or 2000000 (make sure monitor matches)
  t_boot = millis();
}

void loop() {
  // 1) Power-up safety window: only run USB/heartbeat, so you can connect serial first
  if (millis() - t_boot < SAFE_WINDOW_MS) {
    digitalWrite(LED_BUILTIN, (millis()/250) % 2);
    delay(50);
    return;
  }

  // 2) Sample six channels
  for (int i = 0; i < N_TOUCH; i++) {
    touch_us[i] = measure_touch_us(touch_pins[i]);
  }

  // 3) Print at intervals (once every 50 ms)
  uint32_t now = millis();
  if (now - last_print_ms >= PRINT_INTERVAL_MS) {
    last_print_ms = now;

    // Output one CSV line (minimize formatting calls, just print items one by one)
    for (int i = 0; i < N_TOUCH; i++) {
      Serial.print(touch_us[i]);
      if (i < N_TOUCH - 1) Serial.write(',');
    }
    Serial.write('\n');

    lines_since_header++;
    if (lines_since_header >= HEADER_EVERY_LINES) lines_since_header = 0;

    // Light heartbeat
    digitalWrite(LED_BUILTIN, HIGH); delay(3);
    digitalWrite(LED_BUILTIN, LOW);
  }

  usb_friendly_yield();
}