// Librerías
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>
#include <WiFi.h>
#include <ESP32Servo.h>
#include <WiFiClientSecure.h>
#include <UniversalTelegramBot.h>

// Credenciales WiFi
const char* ssid = "Wokwi-GUEST";  // Cambiar por la red deseada
const char* password = "";  // Cambiar por la contraseña de la red

WiFiClientSecure cliente;
const char* telegramBotToken = "7681717759:AAFS1wSWWH0BqruEWz56_Vx28ZXpDOd7sj4";  // Cambiar por el token de tu bot

// Bot de Telegram
UniversalTelegramBot bot(telegramBotToken, cliente);
unsigned long botLastScan = 0;
const unsigned long botScanDelay = 1000;

// Pines de los servos
const int PIN_SERVO_BASE = 4;    // Servo Base
const int PIN_SERVO_ARM1 = 15;   // Servo Brazo 1
const int PIN_SERVO_ARM2 = 13;   // Servo Brazo 2

// Instancias de los servos
Servo servoBase;
Servo servoArm1;
Servo servoArm2;

// Ángulos actuales de los servos
int angles[3] = {90, 90, 90};  // Inicia con todos los servos a 90 grados
int selectedServo = -1;  // Variable para seleccionar el servo activo

void setup() {
  Serial.begin(115200);
  Serial.println("Conectando al WiFi...");
  WiFi.begin(ssid, password);
  
  // Configuración de cliente seguro para Telegram
  cliente.setCACert(TELEGRAM_CERTIFICATE_ROOT);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("Conectado a WiFi!");

  // Inicializar los servos
  servoBase.attach(PIN_SERVO_BASE);
  servoArm1.attach(PIN_SERVO_ARM1);
  servoArm2.attach(PIN_SERVO_ARM2);
  
  // Configuración inicial de ángulos
  servoBase.write(angles[0]);
  servoArm1.write(angles[1]);
  servoArm2.write(angles[2]);
}

void loop() {
  // Verifica si hay nuevos mensajes en el bot de Telegram
  if (millis() - botLastScan > botScanDelay) {
    int nuevosMensajes = bot.getUpdates(bot.last_message_received + 1);
    while (nuevosMensajes) {
      Serial.println("Nuevo mensaje recibido");
      procesarMensaje(nuevosMensajes);
      nuevosMensajes = bot.getUpdates(bot.last_message_received + 1);
    }
    botLastScan = millis();
  }
}

// Función para procesar los mensajes del bot
void procesarMensaje(int nuevosMensajes) {
  for (int i = 0; i < nuevosMensajes; i++) {
    String chat_id = bot.messages[i].chat_id;
    String texto = bot.messages[i].text;
    texto.toLowerCase();

    if (texto == "/start") {
      String mensaje = "Bienvenido al control de servos!\n";
      mensaje += "Comandos disponibles:\n";
      mensaje += "/servo1 para seleccionar el servo de la base\n";
      mensaje += "/servo2 para seleccionar el primer brazo\n";
      mensaje += "/servo3 para seleccionar el segundo brazo\n";
      mensaje += "Luego de seleccionar un servo, envía un ángulo entre 0 y 180 para moverlo.\n";
      bot.sendMessage(chat_id, mensaje, "Markdown");
    }
    else if (texto == "/servo1") {
      selectedServo = 0;
      bot.sendMessage(chat_id, "Servo base seleccionado. Envía un ángulo entre 0 y 180.");
    }
    else if (texto == "/servo2") {
      selectedServo = 1;
      bot.sendMessage(chat_id, "Primer brazo seleccionado. Envía un ángulo entre 0 y 180.");
    }
    else if (texto == "/servo3") {
      selectedServo = 2;
      bot.sendMessage(chat_id, "Segundo brazo seleccionado. Envía un ángulo entre 0 y 180.");
    }
    else if (isNumeric(texto) && selectedServo != -1) {
      int angulo = texto.toInt();
      if (angulo >= 0 && angulo <= 180) {
        angles[selectedServo] = angulo;
        moverServo(selectedServo, angulo);
        bot.sendMessage(chat_id, "Servo movido a " + String(angulo) + " grados.");
      } else {
        bot.sendMessage(chat_id, "Ángulo fuera de rango. Por favor envía un valor entre 0 y 180.");
      }
    } else {
      bot.sendMessage(chat_id, "Comando no reconocido o sin servo seleccionado. Usa /start para ver los comandos.");
    }
  }
}

// Función para verificar si un mensaje es numérico
bool isNumeric(String str) {
  for (byte i = 0; i < str.length(); i++) {
    if (!isDigit(str[i])) {
      return false;
    }
  }
  return true;
}

// Función para mover el servo según el índice
void moverServo(int servoIndex, int angulo) {
  switch (servoIndex) {
    case 0:
      servoBase.write(angulo);
      break;
    case 1:
      servoArm1.write(angulo);
      break;
    case 2:
      servoArm2.write(angulo);
      break;
  }
}