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

WiFiClient espClient;
PubSubClient client(espClient);

const char* mqttServer = "instrumentacion-uji.dynv6.net";
const int mqttPort = 1883;
const char* client_name = "alejilla";

int tactual = 0;
int tanterior = 0;
int pulsos = 0;

bool estadoact;
bool estadoant;

long velocidad = 0;

const int Pulsosporvuelta = 200;
const int EncoderPulsosporvuelta = 4;
Stepper miStepper(Pulsosporvuelta, 25, 26, 27, 14);

float orientaciongr;
float orientacionsteps;

int pasosact;
int pasosant;
int pasosdef;
int potenciometro;

int encoderBits[4] = {0, 0, 0, 0};

float historico[5] = {0, 0, 0, 0, 0};

void PubMQTTVelocidad(char out1[128])
{
  client.publish("anemometro_804/velocidad", out1);
}

void PubMQTThistorico(char out2[128])
{
  client.publish("anemometro_804/histórico", out2);
}

void setup()
{
  Serial.begin(115200);
  WiFi.begin("Wokwi-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED)
  {
    delay(100);
    Serial.print(".");
  }
  Serial.println(" Conectado a WiFi!");

  client.setServer(mqttServer, mqttPort);
  while (!client.connected())
  {
    Serial.print("Conectando a MQTT...");
    if (client.connect(client_name))
      Serial.println("Conectado a MQTT");
    else
    {
      Serial.print("Fallo con el servidor ");
      Serial.print(client.state());
      delay(2000);
    }
  }

  pinMode(17, INPUT_PULLUP);
  pinMode(34, INPUT_PULLUP);

  miStepper.setSpeed(15);
}

void loop()
{
  tactual = millis();
  estadoact = digitalRead(17);
  potenciometro = analogRead(34);

  if (estadoact == 1 && estadoact != estadoant)
  {
    pulsos = pulsos + 1;
  }
  estadoant = estadoact;

  if (tactual - tanterior >= 1000)
  {
    velocidad = map(pulsos, 0, 50, 0, 100);
    Serial.println(velocidad);
    tanterior = tactual;
    pulsos = 0;

    orientaciongr = map(velocidad, 0, 100, 10, 90);
    orientacionsteps = 0.55555555 * orientaciongr;

    ActualizacionEncoder();

    pasosact = orientacionsteps + (encoderGrados() * Pulsosporvuelta / 180);
    if (pasosact != pasosant)
    {
      pasosdef = pasosact - pasosant;
      miStepper.step(pasosdef);
    }

    pasosant = pasosact;

    ActualizacionHistorico(orientaciongr);

    char velocidadStr[128];
    sprintf(velocidadStr, "%f", orientaciongr);
    PubMQTTVelocidad(velocidadStr);

    char historicoStr[128];
    sprintf(historicoStr, "[%f, %f, %f, %f, %f]", historico[0], historico[1], historico[2], historico[3], historico[4]);
    PubMQTThistorico(historicoStr);
  }
}

void ActualizacionEncoder()
{
  for (int i = 0; i < 4; i++)
  {
    encoderBits[i] = digitalRead(i + 10);
  }
}

int encoderGrados()
{
  int encoderValue = 0;
  for (int i = 0; i < 4; i++)
  {
    encoderValue += encoderBits[i] * pow(2, i);
  }
  return map(encoderValue, 0, 15, 0, 180);
}

void ActualizacionHistorico(float nuevaVelocidad)
{
  for (int i = 4; i > 0; i--)
  {
    historico[i] = historico[i - 1];
  }
  historico[0] = nuevaVelocidad;
}