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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1  // No se utiliza reset

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Variables para almacenar los ángulos
int angle1 = 0;
int angle2 = 0;

void setup() {
  // Iniciar comunicación serie
  Serial.begin(9600);

  // Iniciar pantalla OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("Error al inicializar la pantalla OLED"));
    for (;;);
  }
  display.clearDisplay();
}

void loop() {
  // Simulación de los ángulos de los servos con valores aleatorios
  angle1 = random(0, 181);  // Ángulo entre 0 y 180 para el primer servo
  angle2 = random(0, 181);  // Ángulo entre 0 y 180 para el segundo servo

  // Limpiar el buffer de la pantalla
  display.clearDisplay();

  // Mostrar título en la pantalla
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(29, 0);
  display.print("Camera");

  // Mostrar los ángulos en la pantalla con etiquetas
  display.setTextSize(1);

  // Mostrar ángulo 1 (Yaw)
  display.setCursor(0, 25);
  display.print("Up and Down:");
  display.print(angle1);

  // Mostrar ángulo 2 (Pitch)
  display.setCursor(0, 40);
  display.print("Left and Rig.:");
  display.print(angle2);

  // Mostrar el texto "Battery C." centrado en la parte inferior
  display.setCursor((SCREEN_WIDTH - 50) / 2, 56);  
  display.print("C. Servos");

  // Mostrar los símbolos en las esquinas
  display.setCursor(0, 57); 
  display.print("<3");

  display.setCursor(116, 57); 
  display.print("4>");

  // Enviar el buffer a la pantalla
  display.display();

  delay(1000);  // Esperar un segundo antes de la próxima actualización
}