#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>
#include "sensors.h"
#include "display.h"
#include "servo_control.h"
#include "battery.h"

// Definiciones de la pantalla OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Definiciones del servomotor
Servo myServo;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();
  
  myServo.attach(9);
  myServo.write(0);

  sensors_init();
  display_init(&display);
  battery_init();
}

void loop() {
  menu();
  delay(100);
}

void menu() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(F("Menu:"));
  display.println(F("1. Calibrate Sensors"));
  display.println(F("2. Start Servo Control"));
  display.println(F("3. Display Sensor Data"));
  display.println(F("4. Battery Status"));
  display.display();

  if (Serial.available() > 0) {
    char choice = Serial.read();
    switch (choice) {
      case '1':
        calibrateSensors();
        break;
      case '2':
        startServoControl(&myServo);
        break;
      case '3':
        displaySensorData(&display);
        break;
      case '4':
        battery_status(&display);
        break;
      default:
        Serial.println(F("Invalid choice"));
        break;
    }
  }
}