%                           geodes.m
%  Scope:   This MATLAB macro computes geodetic data for a specified 
%           departure-destination pair; WGS-84 earth model is considered.
%  Usage:   [range,bearing1,bearing2] = geodes(lat1,lon1,lat2,lon2)
%  Description of parameters:
%           lat1      - input, departure point latitude, in degrees
%           lon1      - input, departure point longitude, in degrees
%           lat2      - input, departure point latitude, in degrees
%           lon2      - input, departure point longitude, in degrees
%           range     - output, range, in meters
%           bearing1  - output, departure bearing, in degrees
%           bearing2  - output, destination bearing, in degrees
%  Remark:  The macros wgs84con and convcon should be called before using the
%           macro geodes in order to initialized the global variables used.
%  Reference:
%           [1] Anon., Minimum operation performance standards for airborne
%               supplemental navigation equipment using global positioning
%               system (GPS). RTCA/DO-208, July 1991, Appendix B.
%  Last update:  05/28/00
%  Copyright (C) 1998-00 by LL Consulting. All Rights Reserved.

function [range,bearing1,bearing2] = geodes(lat1,lon1,lat2,lon2)

global  flatness b_smaxis eprime2
global  deg_rad 

lat1 = deg_rad * lat1;
lon1 = deg_rad * lon1;
lat2 = deg_rad * lat2;
lon2 = deg_rad * lon2;
  
%  Compute the difference in longitude

dlon = lon2 - lon1;

%  Compute the reduced latitudes

beta1 = atan( (1. - flatness)*tan(lat1) );
beta2 = atan( (1. - flatness)*tan(lat2) );
sin_beta1 = sin(beta1);
sin_beta2 = sin(beta2);
cos_beta1 = cos(beta1);
cos_beta2 = cos(beta2);

%  Initialization and execution of the iterative process

lambda = dlon + 1.;
lambda_new = dlon;
iter = 0;

while (abs(lambda - lambda_new) > 1.e-12)
   iter = iter + 1;
   if (iter >= 50)
      disp('Error 1 : the algorithm does not converge ');
      return
   end   
   lambda = lambda_new;
   sin_lambda = sin(lambda);
   cos_lambda = cos(lambda);

   sin_sigma = sqrt( (cos_beta2 * sin_lambda)^2 + ...
      (cos_beta1*sin_beta2 - sin_beta1*cos_beta2*cos_lambda)^2); 
   
   cos_sigma = sin_beta1*sin_beta2 + cos_beta1*cos_beta2*cos_lambda;
   
   sigma = atan2(sin_sigma,cos_sigma);
   
   sin_alpha = cos_beta1*cos_beta2*sin_lambda/sin_sigma;
   
   cos2_alpha = 1. - sin_alpha*sin_alpha;
   
   if (cos2_alpha ~= 0)
      cos_2sigmam = cos_sigma - 2.*sin_beta1*sin_beta2/cos2_alpha;
   else
      cos_2sigmam = 0.;
   end
   
   c = (flatness/16)*cos2_alpha*(4. + flatness*(4. - 3.*cos2_alpha));
    
   lambda_new = dlon + (1. - c)*flatness*sin_alpha*(sigma + c*sin_sigma* ...
      (cos_2sigmam + c*cos_sigma*(-1. + 2.*cos_2sigmam*cos_2sigmam)));
   
end   

%  Computation of range and bearings at departure point and destination point

u2 = eprime2*cos2_alpha;

a = 1. + (u2/16384.)*(4096. + u2*(-768. + u2*(320. - 175.*u2)));

b = (u2/1024.)*(256. + u2*(-128. + u2*(74. - 47.*u2)));

dsigma = b*sin_sigma*(cos_2sigmam + 0.25*b*((-1. + 2.*cos_2sigmam*cos_2sigmam)*...
   cos_sigma - (b/6.)*(-3. + 4.*sin_sigma*sin_sigma)*(-3. + ...
   4.*cos_2sigmam*cos_2sigmam)*cos_2sigmam));

range = b_smaxis * a * (sigma - dsigma);

sin_lamdba = sin(lambda_new);
cos_lamdba = cos(lambda_new);

bearing1 = (180./pi)* atan2((cos_beta2*sin_lambda),(cos_beta1*sin_beta2 - ...
            sin_beta1*cos_beta2*cos_lambda));

bearing2 = (180./pi)* atan2((cos_beta1*sin_lambda),(-sin_beta1*cos_beta2 + ...
            cos_beta1*sin_beta2*cos_lambda));