/* Ejercicio entregable 010
 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 encoder
 rellenar vuestro nombre y DNI
 Para cambiar la direccion del viento haz click sobre la veleta durante la simulacion
 NOMBRE ALUMNO: David Porras Casanova
 DNI: 
 ENLACE WOKWI: https://wokwi.com/projects/385009707640677377
*/
#include <PubSubClient.h>
#include <WiFi.h>

bool Veleta_D, Veleta_C, Veleta_B, Veleta_A;
bool Gondola_D, Gondola_C, Gondola_B, Gondola_A;
int Lectura_veleta;
int Posicion_veleta;
int Lectura_gondola;
int Lectura_veleta_bin;
int Lectura_gondola_bin;
int Posicion_gondola;
int diferencia; 
int velocidad; 
int hist[5];
int t1;
int t2;
int i=0;

// Función pasar de gray a binario 
unsigned int grayBinario(unsigned int num) {
  unsigned int bin = 0;
  while (num) {
    bin ^= num;
    num >>= 1;
  }
  return bin;
}
// VariblesMQTT
const char* mqttServer = "instrumentacion-uji.dynv6.net";
const int mqttPort = 1883;
const char* client_name = "Veleta_David";

// FUNCIONES MQTT
WiFiClient espClient;
PubSubClient client(espClient);

// Publicar dirección del viento en grados
void PubMQTT_direct(float direccion)
{   
  char str[10];
  int len = snprintf(str,10, "%.2f",direccion);  
  client.publish("veleta_453/direccion",(uint8_t*) str,len,true);
  Serial.println(str);
}

// Publicar histórico

void PubMQTT_hist(float historico)
{   
  char str[10];
  int len = snprintf(str,10, "%.2f",historico);  
  client.publish("veleta_453/histórico",(uint8_t*) str,len,true);
  Serial.println(str);
}


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);
}



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

  client.setServer(mqttServer, mqttPort);
  client.setCallback(lectura);
  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);
        }
  }
  //client.subscribe("veleta_453/direccion");
  //client.subscribe("veleta_453/histórico");
 


// PINES
  Serial.begin(115200);
  ledcAttachPin(26, 0);
  ledcAttachPin(27, 1);
  ledcSetup(0, 1000, 10); //frecuencia 1KHz y resolucion 2^10
  pinMode(19, INPUT);
  pinMode(18, INPUT);
  pinMode(5, INPUT);
  pinMode(17, INPUT);

  pinMode(15, INPUT);
  pinMode(16, INPUT);
  pinMode(22, INPUT);
  pinMode(23, INPUT);

}

void loop() {
    
// LECTURA ENCODER VELETA 
  Veleta_D =digitalRead(19);
  Veleta_C =digitalRead(18);
  Veleta_B =digitalRead(5); 
  Veleta_A =digitalRead(17); 

  Lectura_veleta=(Veleta_D<<3)|(Veleta_C<<2)|(Veleta_B<<1)|(Veleta_A); // Ordenar código
  Lectura_veleta_bin = grayBinario(Lectura_veleta); // Pasar a binario
  
  // 16 posibles posiciones, lo pasamos a ángulos
  Posicion_veleta= Lectura_veleta_bin*360/16;

  /*Serial.print("Veleta: ");
  Serial.println(Lectura_veleta_bin);
  Serial.print("---");*/

  // Histórico direcciones viento
  t1=millis();
  if(t1!=18000000){ // Esté 5 horas leyendo 
  t2=millis();
  if(t2>3600000 ){ //Cada hora
      hist[i]=Lectura_veleta_bin;
      i++;
      t2=0;
    }
  }
  else t1=0;
  

  // LECTURA ENCODER GÓNDOLA // Pregunar pines 
  Gondola_D =digitalRead(15);//1
  Gondola_C =digitalRead(16);//0
  Gondola_B =digitalRead(22); //0
  Gondola_A =digitalRead(23); // 1

  Lectura_gondola=(Gondola_D<<3)|(Gondola_C<<2)|(Gondola_B<<1)|(Gondola_A); // Crear codigo binario DCBA
  Lectura_gondola_bin=grayBinario(Lectura_gondola); // Pasar a binario

  /*Serial.print("Gondola:  ");
  Serial.println(Lectura_gondola_bin);*/

  // 16 posibles posiciones, lo pasamos a ángulo
  Posicion_gondola=Lectura_gondola_bin*360/16;
 
// DESPLAZAR MOTOR según diferencia 
  diferencia= abs(Posicion_veleta-Posicion_gondola);
  velocidad = map(diferencia, 0, 180, 0, 1023);

// Determinar la dirección del motor
  if(Posicion_veleta>Posicion_gondola){
    if(Posicion_veleta-Posicion_gondola>=180) {
      ledcWrite(1,velocidad); // Decrementar ángulo
      ledcWrite(0,0);
    } 
    else {
      ledcWrite(0,velocidad); // Aumentar angulo
      ledcWrite(1,0);
    }
  }
   if (Posicion_veleta < Posicion_gondola){
    if(Posicion_gondola-Posicion_veleta>=180){
         ledcWrite(0,velocidad); // Aumentar angulo
         ledcWrite(1,0);
    }
    else {
      ledcWrite(1,velocidad); // Decrementar ángulo
      ledcWrite(0,0);
      }
  } 
   if(Posicion_veleta==Posicion_gondola){
    ledcWrite(0,0);
    ledcWrite(1,0);
   }



  // Publicar MQTT
   PubMQTT_direct(Posicion_veleta);
   PubMQTT_hist(hist[5]);
   
      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);
            }
      }

    }
   
}