#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <driver/ledc.h>
#include <ESP32Servo.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Pines
const int potPin = 34;       // Potenciómetro
const int servoPin = 0;      // Servo motor
const int led1Pin = 15;      // LED 1
const int led2Pin = 2;       // LED 2
const int trigPin = 19;      // Sensor ultrasonido (Trigger)
const int echoPin = 18;      // Sensor ultrasonido (Echo)
const int buttonPins[4] = {26, 25, 33, 32}; // Teclado matricial (Filas)
const int keyPins[4] = {4, 16, 17, 5};     // Teclado matricial (Columnas)

int keys[4][4] = { {'1', '2', '3', 'A'},
                   {'4', '5', '6', 'B'},
                   {'7', '8', '9', 'C'},
                   {'*', '0', '#', 'D'} };

int MIN_DUTY = 1638;         // Valor mínimo de PWM para el servo
int MAX_DUTY = 8192;         // Valor máximo de PWM para el servo
int pwmChannel = 0;          // Canal PWM para el servo
int pwmFrequency = 50;       // Frecuencia PWM (50 Hz)
int pwmResolution = 16;      // Resolución PWM (16 bits)

// Contraseña secreta
String password = "1234";
String enteredPassword = "";

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

  // Configuración de la pantalla OLED
  if(!display.begin(SSD1306_I2C_ADDRESS, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  display.display();
  delay(2000);  // Pause for 2 seconds

  // Configuración del potenciómetro
  pinMode(potPin, INPUT);

  // Configuración del servo motor
  ledcSetup(pwmChannel, pwmFrequency, pwmResolution);
  ledcAttachPin(servoPin, pwmChannel);

  // Configuración de los LEDs
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);

  // Configuración del sensor de ultrasonido
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Configuración del teclado matricial
  for (int i = 0; i < 4; i++) {
    pinMode(buttonPins[i], OUTPUT);
    digitalWrite(buttonPins[i], LOW);
    pinMode(keyPins[i], INPUT_PULLDOWN);
  }
}

void loop() {
  mainMenu();
}

void mainMenu() {
  enteredPassword = "";
  showOLED("Ingrese clave:");

  while (enteredPassword.length() < 4) {
    char key = readKeypad();
    if (key != '\0' && isDigit(key)) {
      enteredPassword += key;
      showOLED("*" * enteredPassword.length());
      delay(300);
    }
  }

  if (enteredPassword == password) {
    showOLED("Bienvenido");
    delay(1000);
    while (true) {
      display.clearDisplay();
      display.setCursor(0,0);
      display.print("1: Servo");
      display.setCursor(0,10);
      display.print("2: LEDs");
      display.setCursor(0,20);
      display.print("3: Ultra");
      display.setCursor(0,30);
      display.print("5: Menu");
      display.display();

      char key = readKeypad();
      if (key == '1') {
        controlServoWithPot();
      } else if (key == '2') {
        controlLedsWithKeypad();
      } else if (key == '3') {
        measureDistance();
      } else if (key == '5') {
        break;
      }
      delay(200);
    }
  } else {
    showOLED("Incorrecto");
    delay(2000);
  }
}

void showOLED(String text, int line = 0) {
  display.clearDisplay();
  display.setCursor(0, line * 10);
  display.print(text);
  display.display();
}

char readKeypad() {
  for (int row = 0; row < 4; row++) {
    digitalWrite(buttonPins[row], HIGH);
    for (int col = 0; col < 4; col++) {
      if (digitalRead(keyPins[col]) == HIGH) {
        digitalWrite(buttonPins[row], LOW);
        return keys[row][col];
      }
    }
    digitalWrite(buttonPins[row], LOW);
  }
  return '\0';
}

void controlServoWithPot() {
  while (true) {
    int potValue = analogRead(potPin);
    int duty = MIN_DUTY + (potValue * (MAX_DUTY - MIN_DUTY)) / 4095;
    ledcWrite(pwmChannel, duty);
    float angle = (duty - MIN_DUTY) * 180.0 / (MAX_DUTY - MIN_DUTY);
    showOLED("Angle: " + String(angle, 2));
    display.setCursor(0, 10);
    display.print("5: Menu");
    display.display();
    char key = readKeypad();
    if (key == '5') {
      break;
    }
    delay(100);
  }
}

void controlLedsWithKeypad() {
  while (true) {
    char key = readKeypad();
    if (key) {
      if (key == '1') {
        digitalWrite(led1Pin, HIGH);
        showOLED("LED1 ON");
      } else if (key == '2') {
        digitalWrite(led1Pin, LOW);
        showOLED("LED1 OFF");
      } else if (key == '3') {
        digitalWrite(led2Pin, HIGH);
        showOLED("LED2 ON");
      } else if (key == '4') {
        digitalWrite(led2Pin, LOW);
        showOLED("LED2 OFF");
      }
      display.setCursor(0, 10);
      display.print("5: Menu");
      display.display();
      if (key == '5') {
        break;
      }
      delay(200);
    }
  }
}

void measureDistance() {
  while (true) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    long duration = pulseIn(echoPin, HIGH);
    float distance = (duration * 0.0343) / 2.0;
    showOLED("Dist: " + String(distance, 2) + " cm");
    display.setCursor(0, 10);
    display.print("5: Menu");
    display.display();
    char key = readKeypad();
    if (key == '5') {
      break;
    }
    delay(500);
  }
}