/*
 * QPAD21 (SAMD21) Touch Pad Monitor
 * ---------------------------------
 * Shows real-time raw values from 6 capacitive pads on the OLED.
 * Uses the SAME touch style as test_touch_D21.ino (Adafruit_FreeTouch).
 *
 * Pins assumed: {2,3,4,5,6,7}. Edit touch_pins[] if yours differ.
 * Threshold is only used to draw a "*" indicator when a pad is considered "touched".
 *
 * Built for: Adafruit_SSD1306 128x64 I2C OLED @ 0x3C
 * Serial: Serial.begin(0) (as in your D21 examples)
 */

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

/* ---------- OLED ---------- */
#define SCREEN_WIDTH   128
#define SCREEN_HEIGHT  64
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1, 1700000UL, 1700000UL);

/* ---------- Touch (same style as test_touch_D21.ino) ---------- */
#define N_TOUCH   6
#define THRESHOLD 500   // Adjust live: 350–450 (more sensitive) or 700–900 (less sensitive)

// If your board uses different pad pins, change them here:
uint8_t touch_pins[N_TOUCH] = {2, 3, 4, 5, 6, 7};
Adafruit_FreeTouch* touch_devices[N_TOUCH];

int  touch_values[N_TOUCH]     = {0,0,0,0,0,0};
bool pin_touched_now[N_TOUCH]  = {0,0,0,0,0,0};
bool pin_touched_past[N_TOUCH] = {0,0,0,0,0,0};

bool init_touch(){
  bool ok = true;
  for (int i=0;i<N_TOUCH;i++){
    touch_devices[i] = new Adafruit_FreeTouch(
      touch_pins[i],
      OVERSAMPLE_4,
      RESISTOR_50K,
      FREQ_MODE_NONE
    );
    if (!touch_devices[i] || !touch_devices[i]->begin()) ok = false;
  }
  return ok;
}

void update_touch(){
  for (int i=0;i<N_TOUCH;i++){
    int v = touch_devices[i] ? touch_devices[i]->measure() : 0;
    touch_values[i] = v;
    pin_touched_past[i] = pin_touched_now[i];
    pin_touched_now[i]  = (v > THRESHOLD);
  }
}

inline bool justPressed(int idx){ return pin_touched_now[idx] && !pin_touched_past[idx]; }

/* ---------- UI helpers ---------- */
void drawHeader(){
  display.fillRect(0,0,SCREEN_WIDTH,10,SSD1306_BLACK);
  display.setCursor(0,0);
  display.print("Pad Monitor  THR=");
  display.print(THRESHOLD);
  display.display();
}

void drawValues(){
  // Clear body area
  display.fillRect(0, 12, SCREEN_WIDTH, SCREEN_HEIGHT-12, SSD1306_BLACK);

  // Show each pad on its own line
  // Format: "P0(2):  612  *"  -> "*" when above THRESHOLD
  for (int i=0;i<N_TOUCH;i++){
    int y = 12 + i*8;          // 8px per text row at textSize=1
    display.setCursor(0, y);
    display.print("P"); display.print(i);
    display.print("("); display.print(touch_pins[i]); display.print("): ");
    // Ensure fixed-ish width by padding values <1000 with spaces
    int v = touch_values[i];
    if (v < 1000) display.print(' ');
    if (v < 100)  display.print(' ');
    if (v < 10)   display.print(' ');
    display.print(v);
    display.print(' ');
    display.print(pin_touched_now[i] ? '*' : ' ');
  }

  display.display();
}

/* ---------- Setup / Loop ---------- */
void setup(){
  Serial.begin(0);     // as per your D21 serial example
  delay(20);

  Wire.begin();
  delay(80);
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)){
    // continue headless if OLED missing
  } else {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
  }

  (void)init_touch();
  drawHeader();
}

void loop(){
  update_touch();

  // Print to Serial too (optional; comment out if noisy)
  Serial.print("vals:");
  for (int i=0;i<N_TOUCH;i++){
    Serial.print(' '); Serial.print(i); Serial.print('='); Serial.print(touch_values[i]);
  }
  Serial.println();

  drawHeader();
  drawValues();

  delay(30); // ~33 FPS
}