function[d]=spheredist(varargin)
%SPHEREDIST  Computes great circle distances on a sphere.
%
%   D=SPHEREDIST(LAT1,LON1,LAT2,LON2) computes arc length along a great
%   circle from the point with latitude and longitude coordinates LAT1 and
%   LON1 to the point LAT2 and LON2. 
%  
%   All input arguments should be arrays of the same size, or else either
%   LAT1,LON1 or LAT2,LON2 should be scalars.
%
%   D=SPHEREDIST(LAT,LON) with two input arguments LAT and LON, where these
%   are of arrays of length N, computes the great circle distance D between
%   adjacent points.  D is then of length N-1.
%
%   The Earth is approximated as a sphere of radius RADEARTH.
%
%   D=SPHEREDIST(...,R) instead uses a sphere of radius R, in kilometers.
%
%   'spheredist --t' runs a test.
%
%   Usage:  d=spheredist(lat,lon,lato,lono);   
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2006--2011 J.M. Lilly --- type 'help jlab_license' for details    

if strcmp(varargin{1}, '--t')
  spheredist_test,return
end

if isstr(varargin{end})
    flag=varargin{end};
    varargin=varargin(1:end-1);
else
    flag='haversine';
end

if length(varargin)==3
   R=varargin{3};
   varargin=varargin(1:end-1);
elseif length(varargin)==5
   R=varargin{5};
else
   R=radearth;
end

if length(varargin)==2
    if ~isvector(varargin{1})||~isvector(varargin{2})
        error('Sorry, with 2 or 3 input arguments LAT and LON must be vectors.')
    end
    lat1=varargin{1}(1:end-1);
    lon1=varargin{2}(1:end-1);
    lat2=varargin{1}(2:end);
    lon2=varargin{2}(2:end);
else
    lat1=varargin{1}; 
    lon1=varargin{2};
    lat2=varargin{3};
    lon2=varargin{4};
end
   
  

if ~aresame(size(lat1),size(lon1))
   error('LAT1 and LON1 must be the same size.')
end
if ~aresame(size(lat2),size(lon2))
   error('LAT2 and LON2 must be the same size.')
end
if (~aresame(size(lat1),size(lat2)) && (~isscalar(lat1) && ~isscalar(lat2)))
   error('LAT1 and LON1 must be the same size as LAT1 and LON1, or one pair must be scalars.')
end
if anyany(isfinite(lat1)&abs(lat1)>90)||anyany(isfinite(lat2)&abs(lat2)>90)
    error('Sorry, latitudes must be between -90 and +90')
end
%if anyany(abs(deg180(lon1))>180)||anyany(abs(deg180(lon2))>180)
%    error('Sorry, longitudes must be between -180 and +180, or 0 and 360.')
%end

if (~aresame(size(lat1),size(lat2)))
    if isscalar(lat1)
        lat1=lat1+zeros(size(lat2));
        lon1=lon1+zeros(size(lat2));
    elseif isscalar(lat2)
        lat2=lat2+zeros(size(lat1));
        lon2=lon2+zeros(size(lat1));
    end
end
        
d=[];
if ~isempty(lat1)&&~isempty(lat2)
    %This saves some unnecessary computations if we have lots of NANs
    d=nan.*zeros(size(lat1));
    %index=find(~isnan(lat1.*lat2.*lon1.*lon2));
    index=find(~isnan(lat1));
    %length(index)
    if ~isempty(index)
        d(index)=spheredist_one(lat1(index),lon1(index),lat2(index),lon2(index),R,flag);
    end
end


function [d]=spheredist_one(lat1,lon1,lat2,lon2,R,flag)

if strcmp(flag(1:3),'sph')
    %disp('Spherical law of cosines')
    %  From http://mathworld.wolfram.com/GreatCircle.html 
    %     Numerically bad small distances due to acos of numerical noise!!
    d=R.*acos(cosd(lat1).*cosd(lat2).*cosd(lon1-lon2)+sind(lat1).*sind(lat2));
else
    %disp('Haversine formula')
    lat1=jdeg2rad(lat1);
    lat2=jdeg2rad(lat2);
    lon1=jdeg2rad(lon1);
    lon2=jdeg2rad(lon2);

    %Haversin formula from http://www.movable-type.co.uk/scripts/latlong.html
    a=squared(sin(frac(lat2-lat1,2)))+cos(lat1).*cos(lat2).*squared(sin(frac(lon2-lon1,2)));
    d=R.*2.*atan2(sqrt(a),sqrt(1-a));
end

%Sometimes there is a very small complex portion
d=real(d);

function[]=spheredist_test

try
    s=which('sw_dist');
    
    N=1000;
    tol=1;  %1 km
    
    lat1=180*rand(N,1)-90;    
    lon1=360*rand(N,1);   
    lat2=lat1+randn(N,1)-.5;       
    lon2=lon1+randn(N,1)-.5;
    
    index=find(lat2>-90&lat2<90);
    vindex(lat1,lon1,lat2,lon2,index,1);
            
    %Note that SW_DIST compares badly for large displacements 
    %lat2=180*rand(N,1)-90;    
    %lon2=360*rand(N,1);

    d1=spheredist(lat1,lon1,lat2,lon2);
    d2=0*lat1;
    for i=1:length(lat1)
        d2(i)=sw_dist([lat1(i) lat2(i)],[lon1(i) lon2(i)],'km');
    end
    b=aresame(d1,d2,tol);
    reporttest('SPHEREDIST versus SW_DIST for small displacements',b)
    
catch
    disp('SPHEREDIST test not run because SW_DIST not found.')
end

reporttest('SPHEREDIST equals zero for zero displacement', spheredist( 23.876, -63.397, 23.876, -63.397)==0);
%reporttest('SPHEREDIST equals zero for zero displacement', spheredist( 23.876, -63.397, 23.876, -63.397,'sphere')==0);
reporttest('SPHEREDIST equals pi * radearth for opposing points to within one meters',aresame(spheredist( 23.876, 180-63.397, -23.876, -63.397),radearth*pi,1e-3));


N=10000;
lat1=180*rand(N,1)-90;    
lon1=360*rand(N,1);   
lat2=lat1+randn(N,1)-.5;       
lon2=lon1+randn(N,1)-.5;
index=find(lat2>-90&lat2<90);
vindex(lat1,lon1,lat2,lon2,index,1);
d1=spheredist(lat1,lon1,lat2,lon2);
d2=spheredist(lat1,lon1,lat2,lon2,'sphere');
bool=(d1>1);
reporttest('SPHEREDIST haversine formula and spherical law of cosines match to within 1 mm for > 1 km distance',aresame(d1(bool),d2(bool),1e-6));


