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

const int sensorRecep = 20;
const int sensorEmis = 17;

Servo myservo;
int pos; 

#define velmotorfrenesq 15
#define m1a 2
#define m1b 4

#define velmotorfrendir 26
#define m2a 27
#define m2b 14

#define velmotoratresq 18
#define m3a 19
#define m3b 3

#define velmotoratrdir 23
#define m4a 1
#define m4b 22

int vel=0; 

int leitura1;
int leitura2;
int leitura3;
int leitura4;
int leitura5;
int leitura6;
int leitura7;



bool funcionar = false;
const char* ssid = "IoT_Micro";
const char* password = "etec@1234";
const char* mqtt_server = "10.0.3.1";
 
WiFiClient espClient;
PubSubClient client(espClient);
long lastMsg = 0;
char msg[50];
int value = 0;

void setup() {

  Serial.begin(9600);
   
   
  setup_wifi();
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);
  pinMode(velmotorfrenesq,OUTPUT);
  pinMode(m1a,OUTPUT);
  pinMode(m1b,OUTPUT);
  digitalWrite(m1a, LOW);
  digitalWrite(m1b, LOW);
  analogWrite(velmotorfrenesq, vel);
  
  pinMode(velmotorfrendir,OUTPUT);
  pinMode(m2a,OUTPUT);
  pinMode(m2b,OUTPUT);
  digitalWrite(m2a, LOW);
  digitalWrite(m2b, LOW);
  analogWrite(velmotorfrendir, vel);  

  pinMode(velmotoratresq,OUTPUT);
  pinMode(m3a,OUTPUT);
  pinMode(m3b,OUTPUT);
  digitalWrite(m3a, LOW);
  digitalWrite(m3b, LOW);
  analogWrite(velmotoratresq, vel);
  
  pinMode(velmotoratrdir,OUTPUT);
  pinMode(m4a,OUTPUT);
  pinMode(m4b,OUTPUT);
  digitalWrite(m4a, LOW);
  digitalWrite(m4b, LOW);
  analogWrite(velmotoratrdir, vel);
  pinMode(sensorRecep, INPUT);
pinMode(sensorEmis, OUTPUT);

  myservo.attach(12);

  
}
void setup_wifi() 
{
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}
void callback(char* topic, byte* payload, unsigned int length) {
   String string;
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");
  for (int i = 0; i < length; i++) {
     string+=((char)payload[i]);  
  }
    Serial.print(string);
    int status = string.toInt();  
 sensor = digitalRead(20);
 Serial.prinln(digitalRead(sensorRecep));
 if (digitalRead (sensorRecep =< 10)) 
 {
  funcionar = false;
  String temp = String(10);
  client.publish("carro07/Perdeu", temp.c_str());
   digitalWrite(m1a, LOW);//frente
  		digitalWrite(m1b, LOW);
 		analogWrite(velmotorfrenesq, 255);
 
 		digitalWrite(m2a, LOW);
  		digitalWrite(m2b, LOW);
  		analogWrite(velmotorfrendir, 255);
  	   


    digitalWrite(m3a, LOW);
  		digitalWrite(m3b, LOW);
 		analogWrite(velmotoratresq, 255);
 
 		digitalWrite(m4a, LOW);
  		digitalWrite(m4b, LOW);
  		analogWrite(velmotoratrdir, 255);
     
 }
 else 
 {
  String temp2 = String(100);
  client.publish("carro07/Perdeu", temp2.c_str());
 }
if (topic ="carro07/Frente" and funcionar == true) { /// 
    
    if(string == "22")
    {
      digitalWrite(m1a, HIGH);//frente
  		digitalWrite(m1b, LOW);
 		analogWrite(velmotorfrenesq, 255);
 
 		digitalWrite(m2a, HIGH);
  		digitalWrite(m2b, LOW);
  		analogWrite(velmotorfrendir, 255);
  	   


    digitalWrite(m3a, HIGH);
  		digitalWrite(m3b, LOW);
 		analogWrite(velmotoratresq, 255);
 
 		digitalWrite(m4a, HIGH);
  		digitalWrite(m4b, LOW);
  		analogWrite(velmotoratrdir, 255);
  		delay(1000);    

     digitalWrite(m1a, LOW);//frente
  		digitalWrite(m1b, LOW);
 		analogWrite(velmotorfrenesq, 255);
 
 		digitalWrite(m2a, LOW);
  		digitalWrite(m2b, LOW);
  		analogWrite(velmotorfrendir, 255);
  	   


    digitalWrite(m3a, LOW);
  		digitalWrite(m3b, LOW);
 		analogWrite(velmotoratresq, 255);
 
 		digitalWrite(m4a, LOW);
  		digitalWrite(m4b, LOW);
  		analogWrite(velmotoratrdir, 255);

    }
    }
    delay(15);  
if (topic ="carro07/Tras" and funcionar == true) { /// 
    
    if(string == "1")
    { digitalWrite(m1b, HIGH);
 		analogWrite(velmotorfrenesq, 255);
 
 		digitalWrite(m2a, LOW);
  		digitalWrite(m2b, HIGH);
  		analogWrite(velmotorfrendir, 255);   


digitalWrite(m3a, LOW);
  		digitalWrite(m3b, HIGH);
 		analogWrite(velmotoratresq, 255);
 
 		digitalWrite(m4a, LOW);
  		digitalWrite(m4b, HIGH);
  		analogWrite(velmotoratrdir, 255);
  		delay(1000);    

       digitalWrite(m1a, LOW);//frente
  		digitalWrite(m1b, LOW);
 		analogWrite(velmotorfrenesq, 255);
 
 		digitalWrite(m2a, LOW);
  		digitalWrite(m2b, LOW);
  		analogWrite(velmotorfrendir, 255);
  	   


    digitalWrite(m3a, LOW);
  		digitalWrite(m3b, LOW);
 		analogWrite(velmotoratresq, 255);
 
 		digitalWrite(m4a, LOW);
  		digitalWrite(m4b, LOW);
  		analogWrite(velmotoratrdir, 255);

    }
    
    }
    if (topic ="carro07/Esquerda" and funcionar == true) { /// 
    
    if(string == "2")
    {
      digitalWrite(m1a, LOW);//direita
  		digitalWrite(m1b, HIGH);
 		analogWrite(velmotorfrenesq, 255);
 
 		digitalWrite(m2a, HIGH);
  		digitalWrite(m2b, LOW);
  		analogWrite(velmotorfrendir, 255);   


digitalWrite(m3a, LOW);
  		digitalWrite(m3b, HIGH);
 		analogWrite(velmotoratresq, 255);
 
 		digitalWrite(m4a, HIGH);
  		digitalWrite(m4b, LOW);
  		analogWrite(velmotoratrdir, 255);
  		delay(1000);   
       digitalWrite(m1a, LOW);//frente
  		digitalWrite(m1b, LOW);
 		analogWrite(velmotorfrenesq, 255);
 
 		digitalWrite(m2a, LOW);
  		digitalWrite(m2b, LOW);
  		analogWrite(velmotorfrendir, 255);
  	   


    digitalWrite(m3a, LOW);
  		digitalWrite(m3b, LOW);
 		analogWrite(velmotoratresq, 255);
 
 		digitalWrite(m4a, LOW);
  		digitalWrite(m4b, LOW);
  		analogWrite(velmotoratrdir, 255);
 
    }
    
    }
    if (topic ="carro07/Direita" and funcionar == true) { /// 
    
    if(string == "3")
    {
     digitalWrite(m1a, HIGH);//direita
  		digitalWrite(m1b, LOW);
 		analogWrite(velmotorfrenesq, 255);
 
 		digitalWrite(m2a, LOW);
  		digitalWrite(m2b, HIGH);
  		analogWrite(velmotorfrendir, 255);   


digitalWrite(m3a, HIGH);
  		digitalWrite(m3b, LOW);
 		analogWrite(velmotoratresq, 255);
 
 		digitalWrite(m4a, LOW);
  		digitalWrite(m4b, HIGH);
  		analogWrite(velmotoratrdir, 255);
  		delay(1000);    

      digitalWrite(m1a, LOW);//frente
  		digitalWrite(m1b, LOW);
 		analogWrite(velmotorfrenesq, 255);
 
 		digitalWrite(m2a, LOW);
  		digitalWrite(m2b, LOW);
  		analogWrite(velmotorfrendir, 255);
  	   


    digitalWrite(m3a, LOW);
  		digitalWrite(m3b, LOW);
 		analogWrite(velmotoratresq, 255);
 
 		digitalWrite(m4a, LOW);
  		digitalWrite(m4b, LOW);
  		analogWrite(velmotoratrdir, 255);

    }
    
    }
    if (topic ="carro07/Baixa" and funcionar == true) { /// 
   
    if(string == "180")
    {
     int status = string.toInt();   
     pos = map(status, 1, 100, 0, 180);
     Serial.println(pos);
     s.write(180);
    delay(500);
    s.write(0);
    }
    
    }
    delay(15); 
     /*if (funcionar == true)
     { 
      
      String temp = String(10);
     client.publish("carro07/Perdeu", temp.c_str());
     }
     if (funcionar == false)
     {
      String temp2 = String(100);
     client.publish("carro07/Perdeu", temp2.c_str());
     }*/
  }
  
 
void reconnect() {
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    if (client.connect("ESPClient")) {
      Serial.println("connected");
      client.subscribe("carro07/Frente");
      client.subscribe("carro07/Tras");
      client.subscribe("carro07/Esquerda");
      client.subscribe("carro07/Direita");
      client.subscribe("carro07/Baixa");
      client.subscribe("carro07/Perdeu");
      
      
      
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds"); 
      delay(5000);
   }}}
void loop() {
  if (!client.connected()) {
    reconnect();
  }
  client.loop(); 
 delay(100);
 
}