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


/****************************************************************** 
 * This function calculates the distance between 
 * (lat1, lng1) and (lat2, lng2) in Km.
 * latitude and longitude are given in degree.
 ******************************************************************/ 
 

double calcDist(float  lat1, float lng1, float lat2, float lng2) 
{ 
     /**********************************************************
       International Standard Messures
       a = 6378.388 Km  (Radius Equator)
       b = 6356.912 Km  (Radius Pol)
       (a-b)/a = 1/297  

       r = 6371.221 Km  (Mean Radius)
       m =10002.288 Km  (Length Meridian)
     **********************************************************/
     const double k = 5.369;    /* a/k = 1188 */
     const double a = 6378.388; /* (Hayford)  */
     const double d2r = M_PI / 180;
     const double eps = 1.0e-6; /* ca. 9.0 km */
     double phi1, phi2, lambda1, lambda2, sinp1, sinp2, cosp1, cosp2; 
     double sigma, deltalam, cosdl, capa, capb, cossigma, p, q, s; 
 
     phi1 = d2r * lat1; 
     phi2 = d2r * lat2; 
     lambda1 = d2r * lng1; 
     lambda2 = d2r * lng2; 
 
     sinp1 = sin(phi1); 
     sinp2 = sin(phi2); 
     deltalam = lambda2 - lambda1; 
     cosp1 = cos(phi1); 
     cosp2 = cos(phi2); 
     cosdl = cos(deltalam); 
     cossigma = (sinp1 * sinp2) + (cosp1 * cosp2 * cosdl); 

     if (cossigma < 1-eps && cossigma > -(1-eps))
     {
        sigma = acos(cossigma); 
        s = a*sigma; 

        capa = (sinp1 + sinp2) * (sinp1 + sinp2); 
        capb = (sinp1 - sinp2) * (sinp1 - sinp2); 
        p = k * (3 * sin(sigma) - sigma) / (1 + cossigma); 
        q = k * (3 * sin(sigma) + sigma) / (1 - cossigma); 

        s += (p*capa) + (q*capb); 
     }
     else
     {
        if (cossigma >= 1)
        {
           s = 0;
        }
	else if (cossigma <= -1)
	{
	   s = a * M_PI;
	}
	else
	{
	   /* No correction made */
           sigma = acos(cossigma); 
           s = a*sigma; 
	}
     }

     return s; 
} 


/** Conversion degree.minutes to degree **/

static double decimalize (double degmin)
{
   double degree = degmin < 0 ? ceil(degmin) : floor(degmin);
   return degree + (degmin - degree)/0.6;
}