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

const int buttonPin1 = 2;    // Pin del primer pulsador
const int buttonPin2 = 3;    // Pin del segundo pulsador
const int ledPin = 13;       // Pin del LED
const int buzzerPin = 12;    // Pin del buzzer
const int servoPin = 9;      // Pin del servomotor

Servo myServo;
int angle = 0;                // Ángulo del servomotor
const int stallAngle = 30;    // Ángulo crítico para la pérdida de sustentación

// Configuración del display OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Configuración del giroscopio
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);

  myServo.attach(servoPin);
  myServo.write(angle);

  Serial.begin(9600);

  // Inicializar el display OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.display();
  delay(2000);
  display.clearDisplay();
  
  // Inicializar el giroscopio
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println(F("MPU6050 connection failed"));
    for (;;);
  }
}

void loop() {
  // Leer el estado de los pulsadores
  int buttonState1 = digitalRead(buttonPin1);
  int buttonState2 = digitalRead(buttonPin2);

  // Incrementar el ángulo con el primer pulsador
  if (buttonState1 == LOW) {
    angle += 5;
    if (angle > 180) angle = 180;
    myServo.write(angle);
    delay(200);  // Pequeño retardo para evitar incrementos rápidos
  }

  // Decrementar el ángulo con el segundo pulsador
  if (buttonState2 == LOW) {
    angle -= 5;
    if (angle < 0) angle = 0;
    myServo.write(angle);
    delay(200);  // Pequeño retardo para evitar decrementos rápidos
  }

  // Verificar si el ángulo está en la zona de pérdida de sustentación
  if (angle >= stallAngle) {
    digitalWrite(ledPin, HIGH);
    digitalWrite(buzzerPin, HIGH);
  } else {
    digitalWrite(ledPin, LOW);
    digitalWrite(buzzerPin, LOW);
  }

  // Imprimir el ángulo en el monitor serial
  Serial.println(angle);

  // Leer datos del giroscopio
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Convertir los datos a float para mostrarlos en el display (opcional)
  float fax = ax / 16384.0;
  float fay = ay / 16384.0;
  float faz = az / 16384.0;
  float fgx = gx / 131.0;
  float fgy = gy / 131.0;
  float fgz = gz / 131.0;

  // Mostrar los datos en el display OLED
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("Servo Angle: ");
  display.println(angle);
  display.print("Gyro X: ");
  display.println(fgx);
  display.print("Gyro Y: ");
  display.println(fgy);
  display.print("Gyro Z: ");
  display.println(fgz);
  display.display();

  delay(50);  // Pequeño retardo para evitar lectura continua
}