#include <WiFi.h>
#include <WiFiUdp.h>
#include <ESP32Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// Definición de las credenciales de la red WiFi
const char* ssid = "HUAWEIY9a";
const char* password = "Fime2500";

// Definición de la configuración UDP
WiFiUDP udp;
const unsigned int localUdpPort = 4210;  // Puerto en el que escucha el ESP32
char incomingPacket[255];  // Buffer para recibir datos

// Definición de los pines para los servos
const int servoPin1 = 4;
const int servoPin2 = 16;
const int servoPin3 = 17;
const int servoPin4 = 5;
const int servoPin5 = 18;

Servo servo1;  // Crear instancias de Servo
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;

int pos1 = 90;  // Posiciones iniciales de los servos
int pos2 = 90;
int pos3 = 90;
int pos4 = 90;
int pos5 = 0;
//Poaición máximo 
//base no importa
//150 brazo2
//brazo 1 40

//+ brazo2 en unity baja 
//- br<azo 2 sube en unity pero en arduino va bien al presionar menos baja el robot de verdad

//brazo 1 en unity con el más baja y en arduino tambien
//brazo 1 en unity con el menos sube y en arduino sube también

//Base Gripper cambiar el orden unity 
///--->Orden Botones 

//Gripper todo bien 

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

void setup() {
  // Inicializar la pantalla OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }

  // Limpiar la pantalla OLED
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  // Configura cada servo en su pin correspondiente
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  servo4.attach(servoPin4);
  servo5.attach(servoPin5);

  servo1.write(pos1);  // Mueve los servos a la posición inicial
  servo2.write(pos2);
  servo3.write(pos3);
  servo4.write(pos4);
  servo5.write(pos5);

  Serial.begin(115200);
  WiFi.begin(ssid, password);

  // Intento de conexión a la red WiFi
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
    display.setCursor(0, 0);
    display.println("Connecting to WiFi");
    display.display();
  }

  Serial.println("Connected to WiFi");
  Serial.print("IP Address: ");
  IPAddress ip = WiFi.localIP();  // Obtener la dirección IP del ESP32
  Serial.println(ip);  // Imprimir la dirección IP en el monitor serial

  // Mostrar la dirección IP en la pantalla OLED
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("IP Address:");
  display.println(ip);
  display.display();

  udp.begin(localUdpPort);  // Inicia la escucha en el puerto UDP especificado
  Serial.printf("Now listening at IP %s, UDP port %d\n", ip.toString().c_str(), localUdpPort);
}

void loop() {
  int packetSize = udp.parsePacket();  // Verifica si hay un paquete UDP recibido
  if (packetSize) {
    int len = udp.read(incomingPacket, 255);  // Lee los datos del paquete recibido en el buffer
    if (len > 0) {
      incomingPacket[len] = 0;  // Agrega un terminador nulo al final de los datos recibidos
    }
    Serial.printf("Received packet of size %d from %s:%d\n", packetSize, udp.remoteIP().toString().c_str(), udp.remotePort());
    Serial.printf("UDP packet contents: %s\n", incomingPacket);

    // Verificar el contenido del paquete recibido y mover el servo correspondiente
    if (strcmp(incomingPacket, "FORWARD1") == 0) {
      pos1 += 10;  // Incrementa la posición del servo
      if (pos1 > 180) pos1 = 180;  // Limita la posición a 180 grados
      servo1.write(pos1);
    } else if (strcmp(incomingPacket, "BACKWARD1") == 0) {
      pos1 -= 10;  // Decrementa la posición del servo
      if (pos1 < 0) pos1 = 0;  // Limita la posición a 0 grados
      servo1.write(pos1);
    } else if (strcmp(incomingPacket, "FORWARD2") == 0) {
      pos2 += 10;
      if (pos2 > 180) pos2 = 180;
      servo2.write(pos2);
    } else if (strcmp(incomingPacket, "BACKWARD2") == 0) {
      pos2 -= 10;
      if (pos2 < 40) pos2 = 40;
      servo2.write(pos2);
    } else if (strcmp(incomingPacket, "BACKWARD3") == 0) {
      pos3 += 10;
      if (pos3 > 140) pos3 = 140;
      servo3.write(pos3);
    } else if (strcmp(incomingPacket, "FORWARD3") == 0) {
      pos3 -= 10;
      if (pos3 < 0) pos3 = 0;
      servo3.write(pos3);
    } else if (strcmp(incomingPacket, "FORWARD4") == 0) {
      pos4 += 10;
      if (pos4 > 180) pos4 = 180;
      servo4.write(pos4);
    } else if (strcmp(incomingPacket, "BACKWARD4") == 0) {
      pos4 -= 10;
      if (pos4 < 0) pos4 = 0;
      servo4.write(pos4);
    } else if (strcmp(incomingPacket, "FORWARD5") == 0) {
      pos5 += 10;
      if (pos5 > 90) pos5 = 90;
      servo5.write(pos5);
    } else if (strcmp(incomingPacket, "BACKWARD5") == 0) {
      pos5 -= 10;
      if (pos5 < 0) pos5 = 0;
      servo5.write(pos5);
    }
    // Enviar la posición actual del servo de vuelta al remitente
    udp.beginPacket(udp.remoteIP(), udp.remotePort());
    udp.printf("POS1:%d POS2:%d POS3:%d POS4:%d POS5:%d", pos1, pos2, pos3, pos4, pos5);
    udp.endPacket();
  }
}

//BASE Posicion inicial rotación en Y 0°. Limites +-90
//PIVOTE 2 Gira en X. Rotación en X -45° (Unity). Unity MAX +45 y MIN -95
//PIVOTE 3 Rota en X. X -70° (Unity). MIN -130° y MAX 20° (Unity) 
//PIVOTE 4 -+90°
//PIVOTE 5 0 a 90 °. Gira en Z (Unity)
//PINZA 0 a -90° Gira en Z (Unity)