#define PIN_RED 17
#define PIN_GREEN 16
#define PIN_BLUE 25

#define N_TOUCH 6
#define THRESHOLD 30

int touch_pins[N_TOUCH] = {3, 4, 2, 27, 1, 26};
int touch_values[N_TOUCH] = {0};

bool pin_touched_now[N_TOUCH] = {false};
bool pin_touched_past[N_TOUCH] = {false};

void update_touch() {
  int t;
  int t_max = 200;

  for (int i = 0; i < N_TOUCH; i++) {
    int p = touch_pins[i];

    // discharge
    pinMode(p, OUTPUT);
    digitalWriteFast(p, LOW);
    delayMicroseconds(25);

    noInterrupts();
    pinMode(p, INPUT_PULLUP);

    t = 0;
    while (!digitalReadFast(p) && t < t_max) {
      t++;
    }
    interrupts();

    touch_values[i] = t;
    pin_touched_past[i] = pin_touched_now[i];
    pin_touched_now[i] = (t > THRESHOLD);
  }
}

void setup() {
  Serial.begin(115200);

  pinMode(PIN_RED, OUTPUT);
  pinMode(PIN_GREEN, OUTPUT);
  pinMode(PIN_BLUE, OUTPUT);

  digitalWrite(PIN_RED, HIGH);
  digitalWrite(PIN_GREEN, HIGH);
  digitalWrite(PIN_BLUE, HIGH);
}

void loop() {
  update_touch();

  for (int i = 0; i < N_TOUCH; i++) {
    // only trigger on the moment a button goes from "not touched" to "touched"
    if (pin_touched_now[i] && !pin_touched_past[i]) {
      Serial.print("Button ");
      Serial.print(i);
      Serial.println(" pressed!");

      // example: toggle LEDs depending on button
      if (i == 0) digitalWrite(PIN_GREEN, LOW);
      if (i == 1) digitalWrite(PIN_RED, LOW);
      if (i == 2) digitalWrite(PIN_BLUE, LOW);
    }

    // optional: handle release if you want LEDs to turn off again
    if (!pin_touched_now[i] && pin_touched_past[i]) {
      if (i == 0) digitalWrite(PIN_GREEN, HIGH);
      if (i == 1) digitalWrite(PIN_RED, HIGH);
      if (i == 2) digitalWrite(PIN_BLUE, HIGH);
    }
  }

  delay(50);
}