function [lat,lon,elev,time] = getGPS(dataTimeStamp,gpsFilename)
% [lat,lon,elev,time] = getGPS(data_time_stamp,gps_filename)
% data_time_stamp is in seconds

load(gpsFilename);

% ------------------------------------------------------------------------------
% Sort GPS data according to computer time-stamp
[compTime,sortedInd] = sort(compTime);
lat = lat(sortedInd);
lon = lon(sortedInd);
elev = elev(sortedInd);
gpsTime = gpsTime(sortedInd);

if isempty(dataTimeStamp)
  lat = [];
  lon = [];
  elev = [];
  time = [];
else

% Restrict GPS data to 10 seconds before and after data was collected
indexes = find(compTime > dataTimeStamp(1)-10 & compTime < dataTimeStamp(end)+10);
if isempty(indexes)
  fprintf('There is no GPS during the time this data was taken\n');
  return;
end
compTime = compTime(indexes);
lat = lat(indexes);
lon = lon(indexes);
elev = elev(indexes);
gpsTime = gpsTime(indexes);

%fprintf('Largest GPS time step: %f sec\n',max(diff(time_stamp)));

fprintf('Timestep Max %f s RMS %f s\n',max(diff(compTime)),sqrt(mean(abs(diff(compTime)).^2)));

lat = interp1(compTime,lat,dataTimeStamp,'spline',NaN);
lon = interp1(compTime,lon,dataTimeStamp,'spline',NaN);
elev = interp1(compTime,elev,dataTimeStamp,'spline',NaN);
% time = interp1(time_stamp,time_stamp,data_time_stamp,'nearest','extrap');
time = interp1(compTime,gpsTime,dataTimeStamp,'spline',NaN);
end

return;