/* Ejercicio entregable 000
 Deben de utilizar este archivo los alumnos con a<5, b<5, c<5
 siendo a,b,c las tres últimas cifras del DNI 22000cba -W
 Veleta con encoder y motor paso a paso con encoder
Para cambiar la direccion del viento haz click sobre la veleta durante la simulacion
 rellenar vuestro nombre y DNI
 NOMBRE ALUMNO: Nuria Nevot Montolio
 DNI: 26600031W
 ENLACE WOKWI: https://wokwi.com/projects/417730026534763521
*/

// ----------------------------------------------------------------------------------------------------

/* ------- TEORÍA: Encoder Absoluto Codificado en Código Gray de 4 bits
-Nos da la posición exacta del eje en todo momento
  usando un código específico (código Gray).
-Cada posición tiene un valor único, por lo que no es necesario
  un punto de referencia (diferencia con el encoder incremental)
-Proporciona un conjunto de salidas digitales simultáneamente (4 bits: A, B, C, D).
-Código Gray: representación binaria en el que dos valores consecutivos difieren en solo un bit. 
  4 bits puede representar 2^4=16 posiciones por vuelta.*/

/* ------- TEORÍA: Motor paso a paso
-No gira continuamente como un motor DC.
-Divide cada vuelta completa en un número específico de pasos (200 pasos/vuelta).
-Cada pulso enviado al motor avanza el rotor un paso (un pequeño incremento angular).
-Cada paso corresponde a un ángulo: ang_paso=360/200=1.8 grados.*/

// ----------------------------------------------------------------------------------------------------

// ------- LIBRERÍAS
#include <Stepper.h> // Librería para el motor paso a paso
#include <WiFi.h>   // Librería para conectividad WiFi
#include <PubSubClient.h> // Librería para MQTT

// ----------------------------------------------------------------------------------------------------

// ------- VARIABLES

const int veleta_pin_a = 17;
const int veleta_pin_b = 5;
const int veleta_pin_c = 18;
const int veleta_pin_d = 19;

const int posicion_pin_a = 23;
const int posicion_pin_b = 22;
const int posicion_pin_c = 16;
const int posicion_pin_d = 15;

const int motor_pin_1 = 25;
const int motor_pin_2 = 26;
const int motor_pin_3 = 27;
const int motor_pin_4 = 14;

int x;
int xtot;
float promedio;

int tiempo;
int tiempo_inicial;
int tiempo_total;   // Contador de tiempo para añadir el nuevo valor promedio en el histórico (1 hora)

char vector_historico[100];   // Vector histórico (cadena de caracteres)
char vector_direccion[10];    // Dirección de la veleta (cadena de caracteres)

const int paso_motor = 200; // Pasos del motor para una vuelta completa
Stepper myStepper(paso_motor, motor_pin_4, motor_pin_3, motor_pin_2, motor_pin_1);

// ------- WiFi y MQTT. 
const char* mqtt_server = "broker.emqx.io";
const int mqttPort = 1883;
// Los dos topics:
const char* topic_direccion = "veleta_031/direccion"; 
const char* topic_historico = "veleta_031/historico";
const char* client_name = "veleta_client_031";

WiFiClient espClient;
PubSubClient client(espClient);

float historico[5] = {0, 0, 0, 0, 0};
unsigned long lastPublish = 0;
// const int intervalo=60*60*1000; // 1 hora en ms. 
const int intervalo=1000; // NOTA: Para poder probar el histórico he cambiado el valor del intervalo 
const unsigned long publishInterval = intervalo; 

// ----------------------------------------------------------------------------------------------------

// ------- FUNCIÓN PARA CONVERTIR DE GRAY A BINARIO
int GrayEnBinario(int gray) {
    int binario = gray;
    while (gray >>= 1) {
        binario ^= gray;
    }
    return binario;
}

// ----------------------------------------------------------------------------------------------------

// ------- PUBLICAR EN EL SERVIDOR
void PubMQTT(const char* historico, const char* direccion)
{     
  // Publicamos la dirección y el histórico
  client.publish("veleta_031/direccion", direccion);
  client.publish("veleta_031/historico", historico);
}

// ----------------------------------------------------------------------------------------------------
// ------- SETUP

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

    myStepper.setSpeed(10); // Velocidad inicial del motor paso a paso (en RPM)

    // Configuración de pines deL encoder de la veleta
    pinMode(veleta_pin_a, INPUT);
    pinMode(veleta_pin_b, INPUT);
    pinMode(veleta_pin_c, INPUT);
    pinMode(veleta_pin_d, INPUT);

    // Configuración de pines del encoder de comprobación de posición del aerogenerador
    pinMode(posicion_pin_a, INPUT);
    pinMode(posicion_pin_b, INPUT);
    pinMode(posicion_pin_c, INPUT);
    pinMode(posicion_pin_d, INPUT);

    Serial.print("Connecting to WiFi");
    WiFi.begin("Wokwi-GUEST", "", 6);
    while (WiFi.status() != WL_CONNECTED) {
        delay(100);
        Serial.print(".");
    }
    Serial.println(" Connected!");

    client.setServer(mqtt_server, mqttPort);
    
    while (!client.connected()) {
        Serial.print("Connecting to MQTT...");
        if (client.connect(client_name)) {
            Serial.println("connected");
        } else {
            Serial.print("failed with state ");
            Serial.println(client.state());
            delay(2000);
        }
    }
}

// ----------------------------------------------------------------------------------------------------
// ------- LOOP

void loop() {
    // Obtenemos la posición de la veleta
    int gray_veleta = (digitalRead(veleta_pin_d) << 3) |
    (digitalRead(veleta_pin_c) << 2) |
    (digitalRead(veleta_pin_b) << 1) |
    digitalRead(veleta_pin_a);

    int posicion_veleta_binario = GrayEnBinario(gray_veleta);
    float posicion_veleta_grados = (posicion_veleta_binario / 16.0) * 360.0;

    // Obtenemos la posición del aerogenerador
    int gray_posicion = (digitalRead(posicion_pin_d) << 3) |
    (digitalRead(posicion_pin_c) << 2) |
    (digitalRead(posicion_pin_b) << 1) |
    digitalRead(posicion_pin_a);

    int posicion_aerogenerador_binario = GrayEnBinario(gray_posicion);
    float posicion_aerogenerador_grados = (posicion_aerogenerador_binario / 16.0) * 360.0;

    Serial.print("Posición de la veleta (grados): ");
    Serial.println(posicion_veleta_grados);

    Serial.print("Posición del aerogenerador (grados): ");
    Serial.println(posicion_aerogenerador_grados);

    // Calculamos la diferencia 
    float diferencia = posicion_veleta_grados - posicion_aerogenerador_grados;

    // Determinamos el menor recorrido
    if (diferencia > 180) {
        diferencia -= 360;
    } else if (diferencia < -180) {
        diferencia += 360;
    }

    Serial.print("Diferencia angular: ");
    Serial.println(diferencia);

    // Si la diferencia es significativa, movemos el motor
    if (abs(diferencia) > 1) { // Margen de error de 1 grado
        int pasos_a_mover = (diferencia * paso_motor) / 360; // Convertir diferencia angular a pasos
        
        Serial.println("Posición incorrecta: Alineando aerogenerador...");
        Serial.print("Moviendo motor pasos: ");
        Serial.println(pasos_a_mover);

        myStepper.step(-pasos_a_mover);
        
    } else {
        Serial.println("Posición correcta: Aerogenerador alineado con la dirección del viento.");
    }

  // Calculamos la dirección promedia del viento de la última hora
  x+=posicion_veleta_grados;  // Sumatorio de la posición
  xtot++;                     // Contador del número de valores incluidos en el sumatorio
  promedio = x/xtot;          // Cálculo del promedio

  tiempo = millis();        
  tiempo_total = tiempo - tiempo_inicial;

  // Actualizamos el histórico
  if (tiempo_total >= intervalo) { // 
    // Desplazamos todos los valores a la izquierda
    for (int i = 0; i < 4; i++){
      historico[i] = historico[i + 1];
    }
    // Añadimos el nuevo valor al final del vector
    historico[4] = promedio;

    // Reiniciamos sumatorio, promedio y contador
    x = 0;
    xtot = 0;
    tiempo_inicial = tiempo;
  }

  // Convertimos el vector histórico y dirección en cadena de caracteres 
  snprintf(vector_historico, sizeof(vector_historico), "[%f, %f, %f, %f, %f]", historico[0], historico[1], historico[2], historico[3], historico[4]);
  snprintf(vector_direccion, sizeof(vector_direccion), "%.2f", posicion_veleta_grados);

  // Lo publicamos en el servidor
  PubMQTT(vector_historico, vector_direccion);

  Serial.print("Promedio: ");
  Serial.println(promedio);
  Serial.print("Histórico: ");
  Serial.println(vector_historico);

  if (!client.loop()) {
      Serial.println("Desconnectado");
      while (!client.connected())
      {    Serial.print("Connecting to MQTT...");
          if (client.connect(client_name)) 
              Serial.println("connected");
          else
            {  Serial.print("failed with state ");
              Serial.print(client.state());
              delay(2000);
            }
      }
  }

    Serial.println("---------------------------------");
    delay(1000);
}

/* NOTA: Los ángulos que se obtienen de este código vienen comprendidos 
en rangos de 22.5º (ya que 360/16=22.5).*/