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

// ================== LED PINS ==================
// Pines del LED RGB / RGB LED pins
#define PIN_RED   17
#define PIN_GREEN 16
#define PIN_BLUE  25

// ================== TOUCH SETTINGS ==================
// Número de sensores táctiles / Number of touch sensors
#define N_TOUCH 6
// Umbral para detectar toque / Threshold to detect touch
#define THRESHOLD 6

// Pines táctiles / Touch pins
int touch_pins[N_TOUCH] = {3, 4, 2, 27, 1, 26};
// Valores medidos / Measured values
int touch_values[N_TOUCH] = {0, 0, 0, 0, 0, 0};

// Estados actuales y anteriores / Current and previous states
bool pin_touched_now[N_TOUCH]  = {false, false, false, false, false, false};
bool pin_touched_past[N_TOUCH] = {false, false, false, false, false, false};

// ================== OLED SETTINGS ==================
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1, 1700000UL, 1700000UL);


// ====================================================
// Function to update touch sensors
// Función para actualizar los sensores táctiles
// ====================================================
void update_touch() {

  int t;
  int t_max = 200;
  int p;

  for (int i = 0; i < N_TOUCH; i++) {

    p = touch_pins[i];

    // Discharge pin (set LOW)
    // Descargar el pin (poner a LOW)
    pinMode(p, OUTPUT);
    digitalWriteFast(p, LOW);

    delayMicroseconds(25); // Stabilization delay / Tiempo de estabilización

    // Enable internal pull-up
    // Activar resistencia pull-up interna
    pinMode(p, INPUT_PULLUP);

    // Measure time to rise
    // Medir tiempo hasta subir a HIGH
    t = 0;
    while (!digitalReadFast(p) && t < t_max) {
      t++;
    }

    touch_values[i] = t;

    // Save previous state
    // Guardar estado anterior
    pin_touched_past[i] = pin_touched_now[i];

    // Compare with threshold
    // Comparar con el umbral
    pin_touched_now[i] = (touch_values[i] > THRESHOLD);
  }
}


// ====================================================
// SETUP
// ====================================================
void setup() {

  // Initialize Serial communication
  // Inicializar comunicación Serial
  Serial.begin(115200);
  delay(50);

  Serial.println("Touch system initialized");
  Serial.println("Sistema tactil iniciado");

  // Initialize OLED display
  // Inicializar pantalla OLED
  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  display.clearDisplay();
  display.display();

  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(2); // Larger text / Texto más grande

  // Initialize RGB LED (active LOW)
  // Inicializar LED RGB (activo en LOW)
  pinMode(PIN_RED, OUTPUT);
  pinMode(PIN_GREEN, OUTPUT);
  pinMode(PIN_BLUE, OUTPUT);

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


// ====================================================
// LOOP
// ====================================================
void loop() {

  update_touch();

  int touchedIndex = -1;

  // Check which button is pressed
  // Comprobar qué botón está pulsado
  for (int i = 0; i < N_TOUCH; i++) {
    if (pin_touched_now[i]) {
      touchedIndex = i;
      break;
    }
  }

  // ===== SERIAL MONITOR OUTPUT =====
  // Print raw sensor values
  // Mostrar valores crudos de los sensores
  Serial.print("Values: ");
  for (int i = 0; i < N_TOUCH; i++) {
    Serial.print(touch_values[i]);
    Serial.print(" ");
  }

  if (touchedIndex >= 0) {
    Serial.print("  --> Button pressed: ");
    Serial.println(touchedIndex);
  } else {
    Serial.println("  --> No button pressed");
  }

  // ===== LED + DISPLAY CONTROL =====
  if (touchedIndex >= 0) {

    // Turn ON green LED
    // Encender LED verde
    digitalWrite(PIN_GREEN, LOW);

    // Show pressed button on display
    // Mostrar botón pulsado en pantalla
    display.clearDisplay();
    display.setCursor(15, 25);
    display.print("Touch: ");
    display.print(touchedIndex);
    display.display();
  }
  else {

    // Turn OFF LED
    // Apagar LED
    digitalWrite(PIN_GREEN, HIGH);

    // Clear screen
    // Limpiar pantalla
    display.clearDisplay();
    display.display();
  }

  delay(50); // Slow down Serial output / Reducir velocidad de impresión
}