#include <WiFi.h>
#include <PubSubClient.h>
#include <ESP32Servo.h>
#include <ArduinoJson.h>

// Configuración WiFi y MQTT
const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqtt_server = "xxxxxxxxx";   // Ejemplo de broker público
const int mqtt_port = 1883;
const char* mqtt_user = "mqtt-explorer-332xzxzx";
const char* topic = "controls/joy";

WiFiClient espClient;
PubSubClient client(espClient);

// config de los servo
Servo myservo1;
int servo1Pin = 18; // Pin de señal
Servo myservo2;
int servo2Pin = 19; // Pin de señal

//contdor de conexiones  mqtt
int cont = 0;

int currentPos1 = 90;
int currentPos2 = 90;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
     myservo1.attach(servo1Pin); // Conecta el servo
     myservo2.attach(servo2Pin); // Conecta el servo
     myservo1.write(15);  
     myservo2.write(15);  
     delay(1000);   
     myservo1.write(150);     
     myservo2.write(150);  
     delay(1000);
     myservo1.write(90);
     myservo2.write(90);  
     client.setServer(mqtt_server, mqtt_port);
     client.setCallback(callback);


  }

void reconnect() {
  while (!client.connected()) {
    if (client.connect(mqtt_user)) { // ID único del cliente
      Serial.print("Conectado a MQTT - ");
      cont++;
      Serial.println(cont);

    String payload = "ESP32 conectado";
    client.publish("esp32/datos", payload.c_str());
    client.subscribe(topic); 

    } else {
      delay(5000);
    }
  }
}

void callback(char *topico, byte *data, unsigned int length) {
    Serial.print("Message arrived in topic: ");
    Serial.println(topico);
    Serial.print("Message:");
    for (int i = 0; i < length; i++) {
        Serial.print((char) data[i]);
    }
    Serial.println();
    Serial.println("-----------------------");
    Serial.println("Des-serializando");
    StaticJsonDocument<256> doc;
    DeserializationError error = deserializeJson(doc, data, length);
    if (error) {
      Serial.print("Error al parsear JSON: ");
      Serial.println(error.c_str());
      return;
    }
    const char* button = doc["button"];

    //int currentPos1 = myservo1.read();
    //int currentPos2 = myservo2.read();

    if(strcmp(button, "enter") == 0 || strcmp(button, "enter1") == 0 || strcmp(button, "enterx10") == 0){
      currentPos1 = 90;
      currentPos2 = 90;

       myservo1.write(currentPos1);
       myservo2.write(currentPos2);
    }

    if( strcmp(button , "arriba") == 0 ){
       currentPos1 = 180;
       myservo1.write(currentPos1);
    }
    if(strcmp(button , "abajo") == 0 ){
       currentPos1 = 0;
       myservo1.write(currentPos1);
    }
    if(strcmp(button , "izquierda") == 0 ){
      currentPos2 = 0;
      myservo2.write(currentPos2);
    }
    if(strcmp(button , "derecha") == 0 ){
      currentPos2 = 180;
      myservo2.write(currentPos2);
    }

    if(strcmp(button , "up1") == 0 ){
      if(currentPos1 < 180){
        currentPos1++ ;
        myservo1.write(currentPos1);
      }      
    }
    if(strcmp(button , "down1") == 0 ){
      if(currentPos1 > 0){
        currentPos1-- ;
        myservo1.write(currentPos1);
      }      
    }
    if(strcmp(button , "left1") == 0 ){
      if(currentPos2 < 180){
        currentPos2++ ;
        myservo2.write(currentPos2);
      }      
    }
    if(strcmp(button , "right1") == 0 ){
      if(currentPos2 > 0){
       currentPos2-- ;
       myservo2.write(currentPos2);
      }      
    } 

    if(strcmp(button , "upx10") == 0 ){
      if(currentPos1 < 180){
        currentPos1 += 10 ;
        myservo1.write(currentPos1);
      }      
    }
    if(strcmp(button , "downx10") == 0 ){
      if(currentPos1 > 0){
        currentPos1 -= 10 ;
        myservo1.write(currentPos1);
      }      
    }
    if(strcmp(button , "leftx10") == 0 ){
      if(currentPos2 < 180){
        currentPos2 += 10 ;
        myservo2.write(currentPos2);
      }      
    }
    if(strcmp(button , "rightx10") == 0 ){
      if(currentPos2 > 0){ 
        currentPos2 -= 10 ;
        myservo2.write(currentPos2);
      }      
    }  

    delay(300);

    int newPos1 = myservo1.read();
    int newPos2 = myservo2.read();

    Serial.print("button presionado: "); 
    Serial.println(button);
    Serial.print(" Servo1: Current pos: "); 
    Serial.print(currentPos1); 
    Serial.print(" / newPos: ");
    Serial.print(newPos1); 
    Serial.print(" Servo2: CurrentPos: "); 
    Serial.print(currentPos2); 
    Serial.print(" / NewPos: ");
    Serial.print(newPos2); 

    // enviar mensaje al control
    String payload = "{\"PosServo1\":" + String(newPos1) + ", \"PosServo2\":" + String(newPos2) + "}";
    client.publish("sensor/joyPos", payload.c_str() );

}

void loop() {
  // put your main code here, to run repeatedly:
  if (!client.connected()) {
    reconnect();
  }
 client.loop();


}