function along_track = geodetic_to_along_track(lat,lon,elev,spacing)
% along_track = geodetic_to_along_track(lat,lon,elev,spacing)
%
% Converts geodetic (lat,lon,elev) in WGS-84 into along-track. The distance
% between each point is cumulated to create the along_track vector.
% Used by functions like csarp.m.
%
% lat,lon in degrees
% elev in meters
% spacing: optional scalar argument. When specified and not empty, the geodetic
%   coordinates are decimated to a minimum along track spacing specified
%   by "spacing" using the get_equal_alongtrack_spacing_idxs.m function
%   and then all the samples are filled in using these decimated indices.
%   This is better for SAR processing and helps deal with the case when
%   gps errors are on the order of the sample spacing (e.g. due to very
%   slow moving trajectory).
%
% Author: John Paden

% Load WGS84 ellipsoid
physical_constants;

if ~exist('spacing','var') || isempty(spacing)
  % Cumulative sum for each step of motion
  [x,y,z] = geodetic2ecef(lat/180*pi,lon/180*pi,elev,WGS84.ellipsoid);
  if size(x,1) > 1
    along_track = [0; cumsum(sqrt(diff(x).^2 + diff(y).^2 + diff(z).^2))];
  else
    along_track = [0 cumsum(sqrt(diff(x).^2 + diff(y).^2 + diff(z).^2))];
  end
else
  decim_idxs = get_equal_alongtrack_spacing_idxs( ...
    struct('lat',lat,'lon',lon,'elev',elev),spacing);
  physical_constants;
  [ecef(1,:),ecef(2,:),ecef(3,:)] = geodetic2ecef(lat/180*pi,lon/180*pi,elev,WGS84.ellipsoid);
  along_track = zeros(size(lat));
  along_track(decim_idxs) = geodetic_to_along_track(lat(decim_idxs),lon(decim_idxs),elev(decim_idxs));
  for decim_idxs_idx = 1:length(decim_idxs)-1
    start_idx = decim_idxs(decim_idxs_idx);
    stop_idx = decim_idxs(decim_idxs_idx+1);
    
    if decim_idxs_idx < length(decim_idxs)-1
      idxs = decim_idxs(decim_idxs_idx)+1:decim_idxs(decim_idxs_idx+1)-1;
    else
      idxs = decim_idxs(decim_idxs_idx)+1:length(lat);
    end
    
    % Find the closest point on the line joining the two ~closest reference points
    % to the current point
    %  See equation 21.4.16 from section "Lines in Three Dimensions", Numerical Recipes
    % ref_orig (a) is origin of line joining two closest reference points
    ref_orig = [ecef(1,start_idx); ...
      ecef(2,decim_idxs(decim_idxs_idx)); ecef(3,decim_idxs(decim_idxs_idx))];
    % ref_vector (v) is vector connecting two closest reference points
    ref_vector = [ecef(1,stop_idx)-ecef(1,start_idx); ...
      ecef(2,stop_idx)-ecef(2,start_idx); ecef(3,stop_idx)-ecef(3,start_idx)];
    ref_vector = ref_vector./sqrt(dot(ref_vector,ref_vector));
    
    offset_from_origin = dot([ecef(1,idxs)-ref_orig(1); ecef(2,idxs)-ref_orig(2); ecef(3,idxs)-ref_orig(3)], ...
      repmat(ref_vector,[1 length(idxs)]));
    along_track(idxs) = along_track(start_idx) + offset_from_origin;
  end
end

return;
