/* Ejercicio entregable 011
 Deben de utilizar este archivo los alumnos con c<5, d>=5, u>=5
 siendo c,d,u las tres últimas cifras del DNI 22000cdu -W
 Veleta con encoder y motor CC con potenciometro
Para cambiar la direccion del viento haz click sobre la veleta durante la simulacion
 rellenar vuestro nombre y DNI
 NOMBRE ALUMNO: XXXXX
 DNI: XXXXX
 ENLACE WOKWI: XXXXXXXXXXX
*/

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

const char* mqttServer = "broker.mqttdashboard.com"; 
const int mqttPort = 1883; 
WiFiClient espClient; 
PubSubClient client(espClient); 

boolean Av, Bv, Cv, Dv;
int gray_encoder, binario_encoder;
float angulo_veleta,potenciometro_voltios, angulo_gondola, error, velocidad_motor, angulo_medio_veleta, vector_historico[5];  
unsigned long suma_potenciometro=0, muestras=0, potenciometro;
unsigned long suma_angulo_veleta=0, muestras_angulo_veleta=0;
unsigned long tiempo_lectura, tiempo_muestreo = 100;
unsigned long tiempo_publicacion_direccion, tiempo_muestreo_direccion = 1000;
unsigned long tiempo_publicacion_historico, tiempo_muestreo_historico = 3600000; //1 hora son 3.600.000 ms

void setup() {
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  // Conectar WiFi   
  WiFi.begin("Wokwi-GUEST", ""); // mombre de la wifi y contraseña   
  Serial.print("Conectando a la WiFi...");   
  while (WiFi.status() != WL_CONNECTED)      
  delay(500);

 // Conectar MQTT
 client.setServer(mqttServer, mqttPort);   
 client.setCallback(lectura);   
 while (!client.connected())   {    
 Serial.print("Connecting to MQTT...");       
 if (client.connect("User_SJA003_199" )) Serial.println("connected");       
 else {  
   Serial.print("failed with state "); 
   Serial.print(client.state()); 
   delay(2000);
  }   
 }   
  client.subscribe("veleta_199"); 

  // Salida PWM
  ledcAttach(26, 1000, 10); //frecuencia 1KHz y resolucion 2^10 para giro antihorario
  ledcAttach(27, 1000, 10); //frecuencia 1KHz y resolucion 2^10 para giro horario

  // Input: encoder veleta
  pinMode(17,INPUT);
  pinMode(5,INPUT);
  pinMode(18,INPUT);
  pinMode(19,INPUT);

  // Input: potenciómetro en la salida del motor
  pinMode(34,INPUT);
}

void loop() {
  suma_potenciometro +=analogRead(34);
  muestras++;
  // Oversampling en el control
  if (millis()>tiempo_lectura) {
    tiempo_lectura += tiempo_muestreo;
   // Pasar codigo gray de la veleta a grados
   Av = digitalRead(17);
   Bv = digitalRead(5);
   Cv = digitalRead(18);
   Dv = digitalRead(19);
   gray_encoder = ((Dv << 3) | (Cv << 2) | (Bv << 1) | Av); // en gray
   binario_encoder = grayToBinary (gray_encoder); // en binario
   angulo_veleta=360.0/16.0*binario_encoder; // en grados
   if (angulo_veleta >= 337.5) { // ajuste para que cuando llegue a 360º se conviertan en 0º
     angulo_veleta = 0.0;
   } 
   // Serial.print("Ángulo de la veleta:");
   // Serial.print(angulo_veleta);
   // Serial.print("        ");

   // Pasar voltaje del potenciometro a grados
   potenciometro = suma_potenciometro/muestras;
   suma_potenciometro=0;
   muestras=0;
   potenciometro_voltios = potenciometro*3.3/2045; // de valores (de 0 a 2045) a voltios (de 0V a 3.3V)
    angulo_gondola = 360.0/3.3 *potenciometro_voltios; // en grados
   // Serial.print("Ángulo de la góndola:");
   // Serial.print(angulo_gondola);
   // Serial.print("        ");

   // Control proporcional en la PWM que le llega al motor. 
   // Hay cuatro opciones si queremos siempre mantener el camino más corto para aplicar el control proporcional. 
   // El motor girará en sentido antihorario en dos opciones: el ángulo de la veleta es mayor al de la góndola
   // en no más de 180º y el ángulo de la veleta es menor al de la góndola en más de 180º. 
   // En cambio, el motor girará en sentido horario en dos opciones: el ángulo de la veleta es mayor al de la góndola
   // en más de 180º y el ángulo de la veleta es menor al de la góndola en no más de 180º. 
   // Si la diferencia es nula, no se aplicará ningún control (motor apagado) 

   error=angulo_veleta-angulo_gondola;
   velocidad_motor = abs(error)*1023/360; // resolucion PWM 2^10=1024

   if( error < 0 && error > -180 ){       // Sentido antihorario
     ledcWrite(26,velocidad_motor);
     delay(10);  // para evitar errores de corrientes
   } 
   else if( error > 0 && error > 180 ){   // Sentido antihorario
     ledcWrite(26,velocidad_motor);
     delay(10);  // para evitar errores de corrientes
   }
   else if( error < 0 && error < -180 ){  // Sentido horario
     ledcWrite(27,velocidad_motor);
     delay(10);  // para evitar errores de corrientes
   }
   else if( error > 0 && error < 180 ){   // Sentido horario
     ledcWrite(27,velocidad_motor);
     delay(10);  // para evitar errores de corrientes
   }
   else{     // Motor apagado        
     ledcWrite(26,0);
   } 
   // Serial.print("Velocidad del motor:");
   // Serial.print(velocidad_motor);
   // Serial.print("        ");
  }

  // Publicar la dirección del viento cada x segundos (tiempo_muestreo_direccion)
  if (millis()>tiempo_publicacion_direccion) {
    tiempo_publicacion_direccion += tiempo_muestreo_direccion;
    PubMQTT_direccion(angulo_veleta);
    Serial.print("Ángulo de la góndola:");
    Serial.print(angulo_gondola);
  }

 // Oversampling en el vector histórico
  suma_angulo_veleta += angulo_veleta;
  muestras_angulo_veleta++;

  // Publicar el vector histórico cada hora (tiempo_muestreo_historico)
  if (millis()>tiempo_publicacion_historico) {
    tiempo_publicacion_historico += tiempo_muestreo_historico;
    angulo_medio_veleta= suma_angulo_veleta / muestras_angulo_veleta;
    suma_angulo_veleta = 0;
    muestras_angulo_veleta = 0;

    // Desplazar el array a la derecha para hacer hueco para la nueva media
    for (int i = 4; i > 0; --i) {
      vector_historico[i] =  vector_historico[i - 1];
    }
    // Insertamos la nueva media en la última posición
    vector_historico[0] = angulo_medio_veleta;

    PubMQTT_historico(vector_historico);
  }
}

// Función para pasar de código gray a binario
int grayToBinary(int gray) {
    int binary = gray; // Copiar el Gray al binario inicial
    while (gray >>= 1) { // Mientras haya bits para procesar
       binary ^= gray;  // Aplicar XOR con el Gray desplazado
    }
    return binary;
}

// Función lectura (no se usa, pero si quisiera ver lo que estoy publicando la puedo usar)
void lectura(char* topic, byte* payload, unsigned int length)  
{ 
 Serial.print(topic);   
 Serial.print(" : ");   
 char message[length+1]={0x00};    
 for(int i=0;i<length;i++)   
 message[i]=(char)payload[i];    
 message[length]=0x00;   
 Serial.println(message); 
} 

// Función para publicar la dirección del viento
void PubMQTT_direccion(float angulo) 
{  
 char str[10]; 
 sprintf(str, "%.2f",angulo);
 int len = strlen(str);   
 client.publish("veleta_199/direccion",(uint8_t*) str,len,true); 
 Serial.println("Dirección publicada:"); 
 Serial.println(str); 
}

// Función para publicar el vector histórico de las últimas 5 horas
void PubMQTT_historico(float angulos_medios[5]) 
{  
 char str[50]; 
 int len = snprintf(str,50, "[%0.2f, %0.2f, %0.2f, %0.2f, %0.2f]", angulos_medios[0], angulos_medios[1], angulos_medios[2], angulos_medios[3], angulos_medios[4]);  
 client.publish("veleta_199/histórico", (uint8_t*)str, len, true);
 Serial.println("Vector histórico publicado:"); 
 Serial.println(str); 
}



}