/* probado en Wokwi 
radio de la tierra segun latitud  43°22'16.9'' Norte de coruña 6368096.92 en metros
  WGS84 43°22'16.9'' N	8°23'45.6'' O
HaverSine(latitud origen, longitud origen, latitud destino, longitud destino)
 devuelve distancia en metros entre origen y destino

CalcBearing(latitud origen, longitud origen, latitud destino, longitud destino)
 devuelve rumbo en grados entre origen y destino

ComputeDestPoint(latitud origen, longitud origen, rumbo en grados, distancia en metros) 
 "devuelve" por ahora solo imprime : latitud destino, longitud destino

*/


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

  float x = HaverSine(43.35966498798669, -8.432635889371722, 43.35720002698969, -8.424643387422178);
  Serial.println(x, 2);
  int y = CalcBearing(43.35720002698969, -8.424643387422178, 43.35966498798669, -8.432635889371722);
  Serial.println(y);
  int e = CalcBearing(43.35966498798669, -8.432635889371722, 43.35720002698969, -8.424643387422178);
  Serial.println(e);
  // ComputeDestPoint(43.35966498798669, -8.432635889371722, 113, 2302.78871 ); //el ultimo parametro esta en pies 
   ComputeDestPoint(43.35966498798669, -8.432635889371722, 113, 701.89 );    // 0.70189
  }

void loop()
{
  
} 

float HaverSine(float lat1, float lon1, float lat2, float lon2)
{
  float ToRad = PI / 180.0;
  //float R = 6371;   // radius earth in Km
  //float R = 6378140; // radio tierra en metros
  float R = 6368096.92; // radio tierra en latitud coruña
  float dLat = (lat2-lat1) * ToRad;
  float dLon = (lon2-lon1) * ToRad; 
  
  float a = sin(dLat/2) * sin(dLat/2) +
        cos(lat1 * ToRad) * cos(lat2 * ToRad) * 
        sin(dLon/2) * sin(dLon/2); 
        
  float c = 2 * atan2(sqrt(a), sqrt(1-a)); 
  
  float d = R * c;
  return d;
}



//convert degrees to radians
double dtor(double fdegrees)
{
return(fdegrees * PI / 180);
}

//Convert radians to degrees
double rtod(double fradians)
{
return(fradians * 180.0 / PI);
}


//Calculate bearing from lat1/lon1 to lat2/lon2
//Note lat1/lon1/lat2/lon2 must be in radians
//Returns bearing in degrees
int CalcBearing(double lat1, double lon1, double lat2, double lon2)
{
lat1 = dtor(lat1);
lon1 = dtor(lon1);
lat2 = dtor(lat2);
lon2 = dtor(lon2);

//determine angle
double bearing = atan2(sin(lon2-lon1)*cos(lat2), (cos(lat1)*sin(lat2))-(sin(lat1)*cos(lat2)*cos(lon2-lon1)));
//convert to degrees
bearing = rtod(bearing);
//use mod to turn -90 = 270
bearing = fmod((bearing + 360.0), 360);
return (int) bearing + 0.5;
}

void ComputeDestPoint(float lat1, float lon1, float iBear, float iDist)
{
float bearing = dtor((float) iBear);
//float dist = (float) iDist / 20925656.2;   // hay que enviale pies
//float dist = (float) iDist / 6378140;        // hay que enviarle metros
float dist = (float) iDist / 6368096.92; // radio tierra en latitud coruña
lat1 = dtor(lat1);
lon1 = dtor(lon1);
float *lat2, *lon2;
*lat2 = asin(sin(lat1)* cos(dist)+ cos(lat1)* sin(dist)*cos(bearing));
*lon2 = lon1 + atan2(sin(bearing)*sin(dist)*cos(lat1), cos(dist)-sin(lat1)*sin(*lat2));
*lon2 = fmod( *lon2 + 3 * PI, 2*PI )- PI;
*lon2 = rtod( *lon2);
*lat2 = rtod( *lat2);
 float LAT2, LON2;
   LAT2 = *lat2;
   LON2 = *lon2;
Serial.println(LAT2,15);
Serial.println(LON2,15);
}