#include <Wire.h>
#include <ESP32Servo.h>
#include <Arduino.h>
#include <math.h>
#define PI 3.141592654

using namespace std; 
const int endereco_sensor=0x68;

Servo myservo;
Servo myservo2;
Servo servoprofundor;
Servo servoleme;


double resultado;
double cosseno;
double x;
double seno; 
double deltaL; 
double converter;
double y;
double angulolat1; 
double angulolat2;
double angulolon1;
double angulolon2; 
double angulofinal;
double lat1; 
double lon1; 
double lat2;
double lon2; 
double distancia;
double distAntiga;
double distInicial;

double profundor;
double anguloservo;
double bussola;
double leme;
char op;
int contagem;


double lat1A;
double lat2A;
double velocidade;
double distPercorrida;
int conclusao;



//Atualização de posição//


    double distancia_atual;
    double lat_alvo;
    double long_alvo;
    double lat_atualizada;
    double long_atualizada;
    double R = 6371;
    double Ad;
    double angulo_atualizado;
    double brng;
    double radianos;
    double distancia_atual_convertida;

    
//Giroscópio//


int girx, girz, giry, acelx, acelz, acely, temp;





//TinyGPSPlus gps;

//void getgps(TinyGPSPlus &gps);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);


  //giroscopio

Wire.begin();
Wire.beginTransmission(endereco_sensor);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);


//giroscópio

  myservo.attach(12);  myservo.write(90);
  servoprofundor.attach(4);  servoprofundor.write(90);
  servoleme.attach(13);   servoleme.write(90);
  
    lat1 = -27.6673400; 
    lon1 = -52.3059850;
  
    lat2 = -27.6654000;         
    lon2 = -52.3024300;  
  
   
      
    lat_alvo=lat1;
    long_alvo=lon1;
  
    
   delay (5000);
        
        double dLat = (lat2 - lat1) * M_PI / 180.0;
        double dLon = (lon2 - lon1) * M_PI / 180.0;
 
        // conversão em raio
        lat1A = (lat1) * M_PI / 180.0;
        lat2A = (lat2) * M_PI / 180.0;
 
        // aplicando a fórmula
        double a = pow(sin(dLat / 2), 2) + pow(sin(dLon / 2), 2) * cos(lat1A) * cos(lat2A);
        double rad = 6371;
        double c = 2 * asin(sqrt(a));
        distancia = rad * c;
    //  distancia = distancia*1000;
        distInicial = distancia;
        Serial.print("Distancia Inicial: ");
        Serial.println(distancia*1000);

        
               //DEFINE O X//
    
    deltaL=lon1-lon2;
    cosseno=(lat2* PI) / 180;
    resultado= cos (cosseno);
    converter=(deltaL*PI) / 180;
    x=sin(converter);
    x=x*resultado;
    
    //-- FIM DEFINE O X--//
    
    
    //-- FIM DEFINE O X--//
    
    //--define o Y--//
   
    angulolat1=(lat1*PI)/180;
    angulolat2=(lat2*PI)/180;
    angulolon1=(lon1*PI)/180;
    angulolon2=(lon2*PI)/180;
    y=cos (angulolat1) * sin (angulolat2) - sin (angulolat1) * cos (angulolat2)* cos (converter);
  
    //FIM define o Y//
    
    //ANGULO DE ATAQUE FINAL//
    
    angulofinal = atan2(x, y);
    angulofinal = angulofinal * 180.0/PI;
    

      //Transforma -180 em 0 e 180 em 360°//  
     if (angulofinal<0)
    {
       angulofinal=abs(angulofinal);
       angulofinal= angulofinal;
    }
   
     else 
    {
      angulofinal = 360-angulofinal;    
    }
  
  
      
    
      angulo_atualizado=angulofinal;  
      Ad = distancia_atual/R;
      radianos= (angulo_atualizado*PI)/180;
}
   

  
    void loop() {
  
     velocidade = distancia/60; //define a velocidade por segundo   
  //  velocidade = 2; //       2 m/s
    distancia_atual=0;
      
    for (distancia_atual=0; distancia_atual<distancia;)
    {


      //Giroscópio angulo que o drone está//
    
Wire.beginTransmission(endereco_sensor);

Wire.write(0x3B);

Wire.endTransmission(false);

Wire.requestFrom(endereco_sensor, 14, true);

acelx = Wire.read()<<8|Wire.read();
acelz = Wire.read()<<8|Wire.read();
acely = Wire.read()<<8|Wire.read();

temp = Wire.read()<<8|Wire.read();

girx = Wire.read()<<8|Wire.read();
girx = map(girx, -32750, 32750, -180, 180);
bussola = girx;
girz = Wire.read()<<8|Wire.read();
girz = map(girz, -32750, 32750, -180, 180);

giry= Wire.read()<<8|Wire.read();
giry = map(giry, -32750, 32750, -180, 180);


//Giroscópio//




      
      lat_alvo = (lat_alvo) * M_PI / 180;
      long_alvo = (long_alvo) * M_PI / 180;
      
      distancia_atual+=velocidade;
      
      Ad = distancia_atual/R;
      radianos = (angulo_atualizado*PI)/180; 
      
      lat_atualizada = asin(sin(lat_alvo)*cos(Ad) + cos(lat_alvo)*sin(Ad)*cos(radianos));
      long_atualizada = long_alvo + atan2(sin(radianos)*sin(Ad)*cos(lat_alvo), cos(Ad)-sin(lat_alvo)*sin(lat_atualizada));
      
      
      lat_atualizada = (lat_atualizada)/ M_PI * 180;
      long_atualizada = (long_atualizada)/ M_PI * 180;
   
      lat_alvo = (lat_alvo) / M_PI * 180;
      long_alvo = (long_alvo) / M_PI * 180;
      
      Serial.print("Latitude atualizada: ");
      Serial.println(lat_atualizada,6);
      Serial.print("Longitude atualizada: ");
      Serial.println(long_atualizada,6);
      Serial.print("Distancia percorrida: ");
      Serial.println(distancia_atual*1000);
       Serial.print("Angulo atualizado: ");
      Serial.println(angulo_atualizado);
      
       deltaL=long_alvo-long_atualizada;
       cosseno=(lat_atualizada* PI) / 180;
       resultado= cos (cosseno);
       converter=(deltaL*PI) / 180;
       x=sin(converter);
       x=x*resultado;
    //-- FIM DEFINE O X--//
    
    //--define o Y--//
    angulolat1=(lat_alvo*PI)/180;
    angulolat2=(lat_atualizada*PI)/180;
    angulolon1=(long_alvo*PI)/180;
    angulolon2=(long_atualizada*PI)/180;
       
    y=cos (angulolat1) * sin (angulolat2) - sin (angulolat1) * cos (angulolat2)* cos (converter);


    //DEFINE A DIREÇÃO do servo
    angulofinal = atan2(x, y);
    angulofinal = angulofinal * 180.0/PI;
    
      
     //Transforma -180 em 0 e 180 em 360°//  
     if (angulofinal<0)
    {
       angulofinal=abs(angulofinal);
       angulofinal= angulofinal;
    }
   
     else 
    {
      angulofinal = 360-angulofinal;    
    }      
      
      
    angulo_atualizado = angulofinal;
    
      
    Serial.print("Angulo Real: "); Serial.println(angulofinal);
     
     bussola = bussola * 0.5;
     angulofinal = angulofinal*0.5;
     angulofinal = angulofinal-bussola;
      
     

    if (angulofinal>=0 and angulofinal<=45)
    {
      
      myservo.write(angulofinal);
      Serial.print("Angulo de ataque: ");
      Serial.println(angulofinal);
      Serial.print("Angulo servo a: ");
      Serial.println(angulofinal);
      
      leme = 90+angulofinal;
      servoleme.write(leme);
      Serial.print("Leme: ");
      Serial.println(leme);
      Serial.println("Direita");
    }

    if (angulofinal>45 and angulofinal<=90)
    {
      myservo.write(45);
      Serial.print("Angulo de ataque: ");
      Serial.println(angulofinal);
      Serial.println("Servo a 45");
      Serial.println("Angulo servo a: 45 F");
      
      leme = 90;
      servoleme.write(leme);
      Serial.print("Leme: ");
      Serial.println(leme); 
      Serial.println("Direita");
    }
    if (angulofinal>90 and angulofinal<=157.5)
    { 
      myservo.write(180);
      Serial.print("Angulo de ataque: ");
      Serial.println(angulofinal);
      Serial.println("Servo a 180");
      Serial.println("Angulo servo a: 180 F");
      
 
      leme = 0;
      servoleme.write(leme);
      Serial.print("Leme: ");
      Serial.println(leme); 
      Serial.println("Esquerda");
    }
    if (angulofinal>157.5 and angulofinal<=180)
    { 
      myservo.write(angulofinal);
      Serial.print("Angulo de ataque: ");
      Serial.println(angulofinal);
      Serial.print("Angulo final: ");
      Serial.println(angulofinal);
      
      leme = 180-angulofinal;
      servoleme.write(leme);
      Serial.print("Leme: ");
      Serial.println(leme);
      Serial.println("Esquerda");

    }      
                     

//DEFINE A DISTANCIA

        double dLat = (lat2 - lat_atualizada) *
                      M_PI / 180.0;
        double dLon = (lon2 - long_atualizada) *
                      M_PI / 180.0;
 
        // conversão em raio
        lat1A = (lat_atualizada) * M_PI / 180.0;
        lat2A = (lat2) * M_PI / 180.0;
 
        // aplicando a fórmula
        double AB = pow(sin(dLat / 2), 2) +
                   pow(sin(dLon / 2), 2) *
                   cos(lat1A) * cos(lat2A);
        double rad = 6371;
        double c = 2 * asin(sqrt(AB));
      
        distancia = rad * c;
        distancia = distancia*1000;
        
        Serial.print("Velocidade m/s: "); 
        Serial.println (velocidade);
         
        
       
        Serial.print("Distancia restante: "); 
        Serial.println (distancia,2);
        
      
                 


// define o angulo do profundor
      

      //135 angulo de decolagem 


Serial.print("Angulo aeronave: ");
Serial.println(giry);

     if(distancia_atual<0.020)
     { 
        if(giry>=0 && giry<90)
        {
         profundor = 135;
         servoprofundor.write(profundor);
        }

        if (giry>=90 && giry<=135) 
        {
          profundor = map(giry, 90 , 135, 135 , 90); 
          servoprofundor.write(profundor);
        }

        if (giry>135 && giry<=180)
        {
        profundor = map(giry, 135 , 180, 90 , 45); 
        servoprofundor.write(profundor);       
        }

        if (giry>180)
        {
          profundor = 45;
         servoprofundor.write(profundor);
        }
        Serial.println("Decolagem ");

     }
         
     else if(distancia_atual>=0.020 && distancia_atual<=distInicial-0.040)
       
     {  
      //angulo de cruzeiro
        if(giry>=0 && giry<45)
        {
         profundor = 135; 
         servoprofundor.write(profundor);
        }
 
        if(giry>=45 && giry<90)
        {
         profundor = map(giry, 45 , 90, 135 , 90); 
         servoprofundor.write(profundor);
        }

        if (giry>=90 && giry<=135) 
        {
          profundor = map(giry, 90 , 135, 90 , 45); 
          servoprofundor.write(profundor);
        }  
        if (giry>135)
        {
        profundor = 45; 
        servoprofundor.write(profundor);       
        }
                Serial.print("Cruzeiro ");
     }
     
       
     else if(distancia_atual>distInicial-0.040 && distancia_atual<=distInicial-0.020)
       
     {
      //45 angulo de ataque final
  
       if(giry>=0 && giry<45)
        {
         profundor = map(giry, 0 , 45, 135 , 90); 
         servoprofundor.write(profundor);
        }

        if (giry>=45 && giry<=90) 
        {
          profundor = map(giry, 45 , 90, 90, 45); 
          servoprofundor.write(profundor);
        }  

        if (giry>90)
        {
        profundor = 45;
        servoprofundor.write(profundor);       
        }

     }  
     
       else if(distancia_atual>distInicial-0.020)
       
     {

       if(giry>=0 && giry<90)
        {
         profundor = 135;
         servoprofundor.write(profundor);
        }

        if (giry>=90 && giry<=135) 
        {
          profundor = map(giry, 90 , 135, 135 , 90); 
          servoprofundor.write(profundor);
        }

        if (giry>135 && giry<=180)
        {
        profundor = map(giry, 135 , 180, 90 , 45); 
        servoprofundor.write(profundor);       
        }

        if (giry>180)
        {
          profundor = 45;
         servoprofundor.write(profundor);
        }
        
     }   
Serial.print("Profundor: ");
Serial.println(profundor);
      distAntiga = distancia;


     Serial.println ("########################################################");   
    //Serial.println(gps.location.lng(), 6);
    //Serial.print("Altitude: ");
    //Serial.println(gps.altitude.meters());
     delay(5000);
      
    }

      if(distancia<10)
      {
        Serial.println("@@ Misseis disparados @@");
        delay(500000);
      }



    
    
    //fim do for
     }