#include <math.h>
#include<stdio.h>

#define pi 3.14159265358979323846

/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
/*::  This function converts decimal degrees to radians             :*/
/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
double deg2rad(double deg) {
  return (deg * pi / 180);
}

/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
/*::  This function converts radians to decimal degrees             :*/
/*:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::*/
double rad2deg(double rad) {
  return (rad * 180 / pi);
}

double distance(double pos1[2], double pos2[2]) {
  double d_lambda, d_phi;
  double a,c,dist;
  double R = 6371000.0;
  double lat1,lon1,lat2,lon2;

  lat1 = deg2rad(pos1[0]);
  lon1 = deg2rad(pos1[1]);
  lat2 = deg2rad(pos2[0]);
  lon2 = deg2rad(pos2[1]);
  d_lambda = lon2 - lon1;
  d_phi = lat2 - lat1;

  a = sin(d_phi/2)*sin(d_phi/2)+cos(lat1)*cos(lat2)*sin(d_lambda/2)*sin(d_lambda/2);
  c = 2*atan2(sqrt(a),sqrt(1-a));
  dist = R*c;
  return dist;
}

double bearing(double pos1[2], double pos2[2]) {
  double bearing;
  double d_lambda;
  double lat1,lon1,lat2,lon2;
  lat1 = deg2rad(pos1[0]);
  lon1 = deg2rad(pos1[1]);
  lat2 = deg2rad(pos2[0]);
  lon2 = deg2rad(pos2[1]);
d_lambda = lon2-lon1;
bearing = atan2(sin(d_lambda)*cos(lat2),cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(d_lambda));
return rad2deg(bearing);
}


void NewLatLon(double pos1[2],double dist, double bearing,double pos2[2])
{
  double lat1,lon1,lat2,lon2;
  double R = 6371000.0;

  lat1 = deg2rad(pos1[0]);
  lon1 = deg2rad(pos1[1]);
  bearing = deg2rad(bearing);
  lat2 = asin(sin(lat1)*cos(dist/R)+cos(lat1)*sin(dist/R)*cos(bearing));
  lon2 = lon1+atan2(sin(bearing)*sin(dist/R)*cos(lat1),cos(dist/R)-sin(lat1)*sin(lat2));
  pos2[0] = rad2deg(lat2);
  pos2[1] = rad2deg(lon2);
  return;
}