#include <WiFi.h>
#include <WiFiClient.h>
#include <PubSubClient.h>

// Conexiones del Motor A
#define enA 12 // Pin que admite PWM
#define in1 13
#define in2 15

// Conexiones del Motor B
#define enB 32 // Pin que admite PWM
#define in3 33
#define in4 25

#define WIFI_SSID "Wokwi-GUEST"
#define WIFI_PASSWORD ""
// Defining the WiFi channel speeds up the connection:
#define WIFI_CHANNEL 6

// constantes del MQTT
// direccion broker, puerto, y nombre cliente
const char *MQTT_BROKER_ADRESS = "test.mosquitto.org";
const uint16_t MQTT_PORT = 1883;
const char *MQTT_CLIENT_NAME = "McQueen";

// instanciar objetos
WiFiClient M5_Client;
PubSubClient mqttClient(M5_Client);

String payload;
String content = "";

void ConnectWiFi()
{
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD, WIFI_CHANNEL);
  Serial.print("Connecting to WiFi ");
  Serial.print(WIFI_SSID);
  // Wait for connection
  while (WiFi.status() != WL_CONNECTED)
  {
    delay(100);
    Serial.print(".");
  }
  Serial.println(" Connected!");

  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
}

void InitMqtt()
{
  mqttClient.setServer(MQTT_BROKER_ADRESS, MQTT_PORT);
  SuscribeMqtt();
  mqttClient.setCallback(OnMqttReceived);
}

void ConnectMqtt()
{
  while (!mqttClient.connected())
  {
    Serial.print("Starting MQTT connection...");
    if (mqttClient.connect(MQTT_CLIENT_NAME))
    {
      SuscribeMqtt();
    }
    else
    {
      Serial.print("Failed MQTT connection, rc=");
      Serial.print(mqttClient.state());
      Serial.println(" try again in 5 seconds");

      delay(5000);
    }
  }
}

void HandleMqtt()
{
  if (!mqttClient.connected())
  {
    ConnectMqtt();
  }
  mqttClient.loop();
}

void SuscribeMqtt()
{
  mqttClient.subscribe("/McQueen");
}

void OnMqttReceived(char *topic, byte *payload, unsigned int length)
{
  Serial.print("Received on ");
  Serial.print(topic);
  Serial.print(": ");

  content = "";
  for (size_t i = 0; i < length; i++)
  {
    content.concat((char)payload[i]);
  }
  Serial.print(content);
  Serial.println();

if (content.length() > 0){
// Avanza Motor A y B
if( content == "a" )
{ 
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(enA, 90);
  analogWrite(enB, 90);
}

  // Retroceder Motor A y B
else if( content == "e" )
{ 
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enA, 90);
  analogWrite(enB, 90);
}


  //  Girar Izquierda
else if( content == "b" )
{ 
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(enA, 60);
  analogWrite(enB, 90);
}

  //  Girar Derercha
else if( content == "d" )
{ 
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(enA, 90);
  analogWrite(enB, 60);
}
}

}

void setup(void)
{
  Serial.begin(115200);

  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);

  ConnectWiFi();

  InitMqtt();
}

void loop()
{
  HandleMqtt();

  delay(10);
}