#include <WiFi.h>
#include <PubSubClient.h>
#include <AccelStepper.h>
#include <ArduinoJson.h>
#include <FastLED.h>

// ====== WIFI & MQTT CONFIG ======
const char* ssid = "Oskar";
const char* password = "claritabonita";

const char* mqtt_server = "89.116.24.168";
const int mqtt_port = 31160;

const char* mqtt_user = "oskar";
const char* mqtt_password = "";

const char* topic_sub = "atom/motores";
const char* topic_pub = "atom/estado";
// =================================

// ====== PINES ATOM MATRIX ======
#define MOTOR1_STEP 26
#define MOTOR1_DIR  32

#define MOTOR2_STEP 19
#define MOTOR2_DIR  22
// ===============================

// ====== FINALES DE CARRERA ======
#define FINAL_MOTOR1 25
#define FINAL_MOTOR2 23
// ================================

// ====== LED MATRIX ATOM ======
#define LED_PIN 27
#define NUM_LEDS 25
#define BRIGHTNESS 20

CRGB leds[NUM_LEDS];
// ==============================

// ====== PARAMETROS MOTOR ======
#define DEFAULT_STEPS 20000
#define DEFAULT_SPEED 5000

#define MAX_SPEED 8000
#define ACCELERATION 500
#define MIN_PULSE_WIDTH 20
#define HOMING_SPEED 800
#define HOMING_DIR_M1 -1
#define HOMING_DIR_M2 -1
// ==============================

AccelStepper motor1(AccelStepper::DRIVER, MOTOR1_STEP, MOTOR1_DIR);
AccelStepper motor2(AccelStepper::DRIVER, MOTOR2_STEP, MOTOR2_DIR);

WiFiClient espClient;
PubSubClient client(espClient);

// ====== CORAZON PULSANTE ROJO/VERDE ======
void dibujarCorazonGrandeRojo() {
  uint8_t heart[25] = {
    0,1,0,1,0,
    1,1,1,1,1,
    1,1,1,1,1,
    0,1,1,1,0,
    0,0,1,0,0
  };

  for (int i = 0; i < NUM_LEDS; i++) {
    leds[i] = heart[i] ? CRGB::White : CRGB::Black;
  }

  FastLED.show();
}

void dibujarCorazonPequenoVerde() {
  uint8_t heartSmall[25] = {
    0,0,0,0,0,
    0,1,0,1,0,
    0,1,1,1,0,
    0,0,1,0,0,
    0,0,0,0,0
  };

  for (int i = 0; i < NUM_LEDS; i++) {
    leds[i] = heartSmall[i] ? CRGB::Green : CRGB::Black;
  }

  FastLED.show();
}

void corazonPulsante() {
  static unsigned long lastChange = 0;
  static bool grande = false;

  if (millis() - lastChange >= 500) {
    lastChange = millis();
    grande = !grande;

    if (grande) {
      dibujarCorazonGrandeRojo();
    } else {
      dibujarCorazonPequenoVerde();
    }
  }
}
// ==========================================

bool finalMotor1Activado() {
  return digitalRead(FINAL_MOTOR1) == HIGH;
}

bool finalMotor2Activado() {
  return digitalRead(FINAL_MOTOR2) == HIGH;
}

void configurarMotor(AccelStepper &motor, int speed) {
  if (speed < 100) speed = 100;
  if (speed > MAX_SPEED) speed = MAX_SPEED;

  motor.setMaxSpeed(speed);
  motor.setAcceleration(ACCELERATION);
  motor.setMinPulseWidth(MIN_PULSE_WIDTH);
}

void hacerHomingMotor1() {
  Serial.println("Homing motor 1...");

  motor1.setMaxSpeed(HOMING_SPEED);
  motor1.setAcceleration(200);
  motor1.setSpeed(HOMING_SPEED * HOMING_DIR_M1);

  while (!finalMotor1Activado()) {
    motor1.runSpeed();
    corazonPulsante();
  }

  motor1.stop();
  motor1.setCurrentPosition(0);

  Serial.println("Motor 1 en cero");
}

void hacerHomingMotor2() {
  Serial.println("Homing motor 2...");

  motor2.setMaxSpeed(HOMING_SPEED);
  motor2.setAcceleration(200);
  motor2.setSpeed(HOMING_SPEED * HOMING_DIR_M2);

  while (!finalMotor2Activado()) {
    motor2.runSpeed();
    corazonPulsante();
  }

  motor2.stop();
  motor2.setCurrentPosition(0);

  Serial.println("Motor 2 en cero");
}

void hacerHomingInicial() {
  Serial.println("Iniciando cero inicial...");

  hacerHomingMotor1();
  delay(300);

  hacerHomingMotor2();
  delay(300);

  configurarMotor(motor1, DEFAULT_SPEED);
  configurarMotor(motor2, DEFAULT_SPEED);

  Serial.println("Cero ok");
}

void connectWiFi() {
  Serial.print("Conectando a WiFi: ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
    corazonPulsante();
  }

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

void publicarEstado(String estado) {
  client.publish(topic_pub, estado.c_str());
  Serial.println(estado);
}

void callback(char* topic, byte* payload, unsigned int length) {
  String mensaje;

  for (unsigned int i = 0; i < length; i++) {
    mensaje += (char)payload[i];
  }

  Serial.print("MQTT recibido: ");
  Serial.println(mensaje);

  StaticJsonDocument<256> doc;
  DeserializationError error = deserializeJson(doc, mensaje);

  if (error) {
    publicarEstado("ERROR JSON");
    return;
  }

  int motor = doc["motor"] | 0;
  long steps = doc["steps"] | DEFAULT_STEPS;
  int speed = doc["speed"] | DEFAULT_SPEED;

  configurarMotor(motor1, speed);
  configurarMotor(motor2, speed);

  if (motor == 1) {
    motor1.move(steps);
    publicarEstado("Motor 1 moviendo");
  }

  else if (motor == 2) {
    motor2.move(steps);
    publicarEstado("Motor 2 moviendo");
  }

  else if (motor == 0) {
    motor1.move(steps);
    motor2.move(steps);
    publicarEstado("Motores 1 y 2 moviendo");
  }

  else {
    publicarEstado("Motor no valido");
  }
}

void reconnectMQTT() {
  while (!client.connected()) {
    Serial.print("Conectando a MQTT... ");

    if (client.connect("ATOM_MATRIX_MOTORES", mqtt_user, mqtt_password)) {
      Serial.println("conectado");
      client.subscribe(topic_sub);
      Serial.print("Suscrito a: ");
      Serial.println(topic_sub);
    } else {
      Serial.print("fallo, rc=");
      Serial.print(client.state());
      Serial.println(" reintentando en 5 segundos");
      delay(5000);
    }
  }
}

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

  FastLED.addLeds<WS2812B, LED_PIN, GRB>(leds, NUM_LEDS);
  FastLED.setBrightness(BRIGHTNESS);
  FastLED.clear();
  FastLED.show();

  pinMode(MOTOR1_STEP, OUTPUT);
  pinMode(MOTOR1_DIR, OUTPUT);
  pinMode(MOTOR2_STEP, OUTPUT);
  pinMode(MOTOR2_DIR, OUTPUT);

  pinMode(FINAL_MOTOR1, INPUT_PULLUP);
  pinMode(FINAL_MOTOR2, INPUT_PULLUP);

  configurarMotor(motor1, DEFAULT_SPEED);
  configurarMotor(motor2, DEFAULT_SPEED);

  hacerHomingInicial();

  connectWiFi();

  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(callback);

  Serial.println("ATOM Matrix listo para controlar motores por MQTT");
}

void loop() {
  if (!client.connected()) {
    reconnectMQTT();
  }

  client.loop();

  motor1.run();
  motor2.run();

  corazonPulsante();
}