#include <WiFi.h>
#include <PubSubClient.h>
#include <ESP32Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// ====== CONFIGURAÇÕES ======
const char* SSID = "Wokwi-GUEST";
const char* PASSWORD = "";

const char* BROKER_MQTT = "52.67.252.34";
const int BROKER_PORT = 1883;
const char* ID_MQTT = "smartlab_001";

// ====== TÓPICOS MQTT ======
const char* TOPICO_SLOT1 = "/iot/slot1";
const char* TOPICO_SLOT2 = "/iot/slot2";
const char* TOPICO_SLOT3 = "/iot/slot3";
const char* TOPICO_SLOT4 = "/iot/slot4";

// ====== OBJETOS ======
WiFiClient espClient;
PubSubClient MQTT(espClient);
LiquidCrystal_I2C lcd(0x27, 16, 2);

Servo servo1, servo2, servo3, servo4;

// ====== PINOS DOS SERVOS ======
const int pinoServo1 = 13;
const int pinoServo2 = 12;
const int pinoServo3 = 14;
const int pinoServo4 = 27;

// ====== PINOS I2C ======
#define SDA_PIN 21
#define SCL_PIN 22

// ====== ESTOQUE SIMULADO ======
int estoque[4] = {3, 2, 1, 4}; // Estoque inicial dos slots

// ====== CONTROLE DE TEMPO DOS SERVOS ======
static unsigned long startTime1 = 0;
static unsigned long startTime2 = 0;
static unsigned long startTime3 = 0;
static unsigned long startTime4 = 0;

const unsigned long TEMPO_RETORNO = 3000; // 3 segundos para retornar à posição inicial
const int POSICAO_INICIAL = 0;   // Posição inicial dos servos
const int POSICAO_DISPENSAR = 180;  // Posição para dispensar item

// ====== FUNÇÕES ======
void initWiFi() {
  delay(10);
  Serial.println();
  Serial.print("Conectando ao WiFi");
  WiFi.begin(SSID, PASSWORD);
  
  int tentativas = 0;
  while (WiFi.status() != WL_CONNECTED && tentativas < 20) {
    delay(500);
    Serial.print(".");
    tentativas++;
  }
  
  if (WiFi.status() == WL_CONNECTED) {
    Serial.println();
    Serial.println("WiFi conectado!");
    Serial.print("IP: ");
    Serial.println(WiFi.localIP());
  } else {
    Serial.println();
    Serial.println("Falha na conexão WiFi");
  }
}

void initMQTT() {
  MQTT.setServer(BROKER_MQTT, BROKER_PORT);
  MQTT.setCallback(mqtt_callback);
}

void atualizarDisplay() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("S1:");
  lcd.print(estoque[0]);
  lcd.print(" S2:");
  lcd.print(estoque[1]);
  lcd.print(" S3:");
  lcd.print(estoque[2]);
  
  lcd.setCursor(0, 1);
  lcd.print("S4:");
  lcd.print(estoque[3]);
  lcd.print(" SmartLab OK");
}

void liberarItem(int slot) {
  Serial.printf("=== ACIONANDO SLOT %d ===\n", slot + 1);
  
  if (estoque[slot] > 0) {
    estoque[slot]--;
    
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Liberando...");
    lcd.setCursor(0, 1);
    lcd.print("Slot ");
    lcd.print(slot + 1);
    
    Serial.printf("Movendo servo do slot ", slot + 1);
    Serial.printf("Estoque restante: ", estoque[slot]);

    // Aciona o servo correspondente baseado no exemplo funcional
    switch(slot) {
      case 0: // Slot 1
        servo1.write(POSICAO_DISPENSAR);
        startTime1 = millis();
        break;
      case 1: // Slot 2
        servo2.write(POSICAO_DISPENSAR);
        startTime2 = millis();
        break;
      case 2: // Slot 3
        servo3.write(POSICAO_DISPENSAR);
        startTime3 = millis();
        break;
      case 3: // Slot 4
        servo4.write(POSICAO_DISPENSAR);
        startTime4 = millis();
        break;
    }
    
    Serial.printf("Servo do slot acionado! Retornará em segundos\n", slot + 1, TEMPO_RETORNO/1000);
    
    // Atualiza display
    delay(1000);
    atualizarDisplay();
    
  } else {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("ERRO!");
    lcd.setCursor(0, 1);
    lcd.print("Slot ");
    lcd.print(slot + 1);
    lcd.print(" vazio");
    
    Serial.printf("ERRO: Slot sem estoque!\n", slot + 1);
    
    delay(2000);
    atualizarDisplay();
  }
}

void controlarServos() {
  // Controla retorno do servo1 baseado no exemplo funcional
  if(startTime1 > 0 && millis() - startTime1 >= TEMPO_RETORNO) {
    servo1.write(POSICAO_INICIAL);
    startTime1 = 0; // Reset do cronômetro
    Serial.println("Servo 1 retornou à posição inicial");
  }
  
  // Controla retorno do servo2
  if(startTime2 > 0 && millis() - startTime2 >= TEMPO_RETORNO) {
    servo2.write(POSICAO_INICIAL);
    startTime2 = 0;
    Serial.println("Servo 2 retornou à posição inicial");
  }
  
  // Controla retorno do servo3
  if(startTime3 > 0 && millis() - startTime3 >= TEMPO_RETORNO) {
    servo3.write(POSICAO_INICIAL);
    startTime3 = 0;
    Serial.println("Servo 3 retornou à posição inicial");
  }
  
  // Controla retorno do servo4
  if(startTime4 > 0 && millis() - startTime4 >= TEMPO_RETORNO) {
    servo4.write(POSICAO_INICIAL);
    startTime4 = 0;
    Serial.println("Servo 4 retornou à posição inicial");
  }
}

void mqtt_callback(char* topic, byte* payload, unsigned int length) {
  String msg = "";
  for (int i = 0; i < length; i++) {
    msg += (char)payload[i];
  }

  Serial.printf(">>> MQTT RECEBIDO <<<\n");
  Serial.printf("Tópico: %s\n", topic);
  Serial.printf("Mensagem: %s\n", msg.c_str());

  if (msg == "dispense") {
    String topico = String(topic);
    
    if (topico == TOPICO_SLOT1) {
      Serial.println("Acionando SERVO 1 (Pino 13)");
      liberarItem(0);
    } else if (topico == TOPICO_SLOT2) {
      Serial.println("Acionando SERVO 2 (Pino 12)");
      liberarItem(1);
    } else if (topico == TOPICO_SLOT3) {
      Serial.println("Acionando SERVO 3 (Pino 14)");
      liberarItem(2);
    } else if (topico == TOPICO_SLOT4) {
      Serial.println("Acionando SERVO 4 (Pino 27)");
      liberarItem(3);
    } else {
      Serial.printf("ERRO: Tópico não reconhecido: %s\n", topic);
    }
  } else {
    Serial.printf("ERRO: Mensagem não reconhecida: %s\n", msg.c_str());
  }
  Serial.println(">>> FIM CALLBACK <<<\n");
}

void verificaConexoesWiFiEMQTT() {
  // Verifica WiFi
  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("Reconectando WiFi...");
    initWiFi();
  }

  // Verifica MQTT
  if (!MQTT.connected()) {
    Serial.print("Conectando ao broker MQTT...");
    
    if (MQTT.connect(ID_MQTT)) {
      Serial.println(" Conectado!");
      
      // Subscribe nos tópicos
      MQTT.subscribe(TOPICO_SLOT1);
      MQTT.subscribe(TOPICO_SLOT2);
      MQTT.subscribe(TOPICO_SLOT3);
      MQTT.subscribe(TOPICO_SLOT4);
      
      Serial.println("Inscrito nos tópicos MQTT");
      
    } else {
      Serial.print(" Falha! Estado: ");
      Serial.println(MQTT.state());
      delay(2000);
    }
  }
}

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  Serial.println("=== SMARTLAB INICIANDO ===");
  
  // Inicializa I2C
  Wire.begin(SDA_PIN, SCL_PIN);
  
  // Inicializa LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("SmartLab");
  lcd.setCursor(0, 1);
  lcd.print("Iniciando...");
  
  // Inicializa servos baseado no exemplo funcional
  Serial.println("Inicializando servos...");
  servo1.attach(pinoServo1);
  servo2.attach(pinoServo2);
  servo3.attach(pinoServo3);
  servo4.attach(pinoServo4);
  
  // Posição inicial dos servos (90º como no exemplo)
  servo1.write(POSICAO_INICIAL);
  servo2.write(POSICAO_INICIAL);
  servo3.write(POSICAO_INICIAL);
  servo4.write(POSICAO_INICIAL);
  
  // Inicializa os cronômetros
  startTime1 = 0;
  startTime2 = 0;
  startTime3 = 0;
  startTime4 = 0;
  
  delay(2000); // Aguarda servos se posicionarem
  
  // Conecta WiFi e MQTT
  initWiFi();
  initMQTT();
  
  // Atualiza display inicial
  atualizarDisplay();
  
  Serial.println("=== SETUP COMPLETO ===");
}

void loop() {
  verificaConexoesWiFiEMQTT();
  MQTT.loop();
  
  // Controla o retorno automático dos servos (baseado no exemplo)
  controlarServos();
  
  delay(100);
}