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

// Configurações de Wi-Fi e MQTT
const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqtt_server = "test.mosquitto.org";

WiFiClient espClient;
PubSubClient client(espClient);

// Pinos do ESP32 conectados ao L298N
const int IN1 = 18;
const int IN2 = 19;
const int IN3 = 21;
const int IN4 = 22;

int stepsPerRevolution = 200;  // Número de passos por revolução
int currentStep = 0;
float angle = 0;

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

  // Conectar ao Wi-Fi
  setup_wifi();
  
  // Configurar MQTT
  client.setServer(mqtt_server, 1883);

  // Configurar os pinos do motor como saída
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
}

void loop() {
  // Verificar se o ESP32 está conectado ao MQTT
  if (!client.connected()) {
    reconnect();
  }
  client.loop();

  // Controlar o motor para dar um passo
  stepMotor(1);  // Um passo no sentido horário
  currentStep++;
  
  // Calcular o ângulo do motor
  calculateAngle();

  // Enviar o ângulo para o Node-RED via MQTT
  sendAngleToNodeRed();

  delay(1000);  // Atraso para controlar a velocidade do motor e a frequência do loop
}

// Função para mover o motor um passo
void stepMotor(int step) {
  switch (step % 4) {
    case 0:  // Aumentar corrente em A+
      digitalWrite(IN1, HIGH);
      digitalWrite(IN2, LOW);
      digitalWrite(IN3, HIGH);
      digitalWrite(IN4, LOW);
      break;
    case 1:  // Aumentar corrente em A-
      digitalWrite(IN1, LOW);
      digitalWrite(IN2, HIGH);
      digitalWrite(IN3, HIGH);
      digitalWrite(IN4, LOW);
      break;
    case 2:  // Aumentar corrente em B+
      digitalWrite(IN1, LOW);
      digitalWrite(IN2, HIGH);
      digitalWrite(IN3, LOW);
      digitalWrite(IN4, HIGH);
      break;
    case 3:  // Aumentar corrente em B-
      digitalWrite(IN1, HIGH);
      digitalWrite(IN2, LOW);
      digitalWrite(IN3, LOW);
      digitalWrite(IN4, HIGH);
      break;
  }
}

void calculateAngle() {
  // Calcula o ângulo baseado no número de passos dados
  angle = (currentStep % stepsPerRevolution) * (360.0 / stepsPerRevolution);
  Serial.print("Ângulo atual: ");
  Serial.println(angle);  // Mostra o ângulo no monitor serial
}

void sendAngleToNodeRed() {
  // Envia o ângulo do motor via MQTT para o Node-RED
  String payload = "Ângulo do Motor: " + String(angle) + " graus";
  client.publish("TOPICO_BRACO_MOTOR2", payload.c_str());
}

void setup_wifi() {
  delay(10);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("WiFi conectado.");
}

void reconnect() {
  while (!client.connected()) {
    if (client.connect("ESP32Client")) {
      client.subscribe("TOPICO_BRACO_MOTOR2");
    } else {
      delay(5000);
    }
  }
}