#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SH1106.h> // Librería para OLED 1.3"

// --- AJUSTES DEL SISTEMA ---
#define SET_POINT 80      // 80 cm = 100%
#define PIN_SWITCH 27     // Interruptor de palanca
#define TRIG 13
#define ECHO 14
#define BUZZER 12
#define RGB_R 18
#define RGB_G 19
#define RGB_B 5

Adafruit_SH1106 oled(21, 22);

void setup() {
  pinMode(PIN_SWITCH, INPUT_PULLUP);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(BUZZER, OUTPUT);
  pinMode(RGB_R, OUTPUT);
  pinMode(RGB_G, OUTPUT);
  pinMode(RGB_B, OUTPUT);

  oled.begin(SH1106_SWITCHCAPVCC, 0x3C);
  oled.clearDisplay();
}

// Función para manejar el color del RGB de forma sencilla
void color(int r, int g, int b) {
  digitalWrite(RGB_R, r);
  digitalWrite(RGB_G, g);
  digitalWrite(RGB_B, b);
}

void loop() {
  // El switch en ON (Retro) dará un valor LOW por el PULLUP
  bool retroActivo = (digitalRead(PIN_SWITCH) == LOW);

  if (!retroActivo) {
    // ==========================
    // ESTADO: DRIVE (SISTEMA OFF)
    // ==========================
    noTone(BUZZER);
    color(0, 0, 0); // Apagar RGB
    
    oled.clearDisplay();
    oled.setTextSize(2);
    oled.setTextColor(WHITE);
    // Centrado aproximado para "DRIVE" en 128x64
    oled.setCursor(35, 25); 
    oled.print("DRIVE");
    oled.display();
  } 
  else {
    // ==========================
    // ESTADO: RETRO (SISTEMA ON)
    // ==========================
    long tiempo = dispararSensor();
    int dist = tiempo / 58;
    
    // Calcular el porcentaje de la distancia actual respecto al Set Point
    float porcentaje = ((float)dist / SET_POINT) * 100.0;

    oled.clearDisplay();
    oled.setTextColor(WHITE);
    oled.setTextSize(1);
    oled.setCursor(0, 0);
    oled.print("MODO: RETRO");
    oled.setCursor(80, 0);
    oled.print(dist); oled.print("cm");

    // LÓGICA DE ALERTAS POR PORCENTAJE
    
    if (dist > SET_POINT) {
      // a) > 100% del Set Point
      color(1, 0, 1); // MORADO (Rojo + Azul)
      noTone(BUZZER);
      mostrarEstado("SEGURO", 2);
    } 
    else if (porcentaje <= 99 && porcentaje >= 50) {
      // b) Entre 99% y 50%
      color(1, 1, 0); // AMARILLO (Rojo + Verde)
      tone(BUZZER, 262); // Nota DO
      delay(200);
      noTone(BUZZER); // Intermitencia
      mostrarEstado("AVISO", 2);
    }
    else if (porcentaje < 50 && porcentaje >= 5) {
      // c) Entre 49% y 5%
      // NARANJA: Rojo HIGH, Verde necesita PWM para ser naranja real, 
      // pero en digital usamos Rojo + Verde suave o solo Rojo para alerta.
      color(1, 1, 0); // En digital se ve similar al amarillo, para naranja real usar analogWrite
      tone(BUZZer, 330); // Nota MI (más agudo)
      mostrarEstado("ALERTA", 2);
    }
    else {
      // Menos del 5% (Peligro inminente)
      color(1, 0, 0); // ROJO FIJO
      tone(BUZZER, 500); // Pitido agudo constante
      mostrarEstado("STOP!", 3);
    }
    
    oled.display();
  }
}

long dispararSensor() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  return pulseIn(ECHO, HIGH);
}

void mostrarEstado(String texto, int tam) {
  oled.setTextSize(tam);
  oled.setCursor(25, 35);
  oled.print(texto);
}