
function [nav, uv] = adcp_nav(fixes, profs, arg3)
%  [nav, uvabs] = adcp_nav(fixes, profs, cfg)
%  -- or --
%  [nav, uvabs] = adcp_nav(fixes, profs, yearbase)
%
% From a set of averaged ADCP profiles and GPS fixes,
% calculate smoothed velocities and adjusted fix positions.
% This is the main part of a replacement for refabs.c and
% smoothr.c; although it does not do exactly what they do,
% it can produce similar results.  Its intended advantage
% is that it can work with an all-inclusive reference layer;
% it is not necessary to pick a single reference layer that
% has valid data throughout a cruise.  Instead, one might
% choose bins 1 through 30, for example, so that there is
% information available even in shallow water, but a large
% depth range is used in deep water.
%
% All input arguments and outputs are structures (unless using all cfg
% defaults and yearbase is the only element being specified)
%
% Called from refsm_core (do 'help refsm_core' see  cfg defaults)
%
%
%     profs.dd                 % 1 x nt vector of decimal days
%     profs.u, profs.v         % nd x nt arrays of velocity in m/s
%     profs.pg, profs.pf       % nd x nt arrays of percent good and
%                              %   profile flags
% *   profs.uship, profs.vship % OPTIONAL ship velocity estimates
%                              %   corresponding to the actual ensemble
%                              %   of pings remaining after editing
%                              %   1 x nt vectors
%
%     fixes.dd                 % fix times in decimal days
%     fixes.x, fixes.y         % fix longitude, latitude in degrees
%
%
%     nav                    % structure with navigation information
%     nav.dd                 % fix times (decimal day)
%     nav.x, nav.y           % values from fixes corresponding to
%                            %   times in profs.dd; may be interpolated
%     nav.u, nav.v         % raw ship velocity from these fixes
%     nav.u_adj, nav.v_adj % ship vel adjusted based on smoothing
%     nav.x_adj, nav.y_adj % adjusted fix positions
% *   nav.u_adj_uvship, ...% ship vel from profs.uship adj by smoothing
%
%     uvabs                    % structure with velocities rendered
%                              %   absolute (i.e. relative to earth)
%     uvabs.raw                % nd x nt array of complex velocities
%                              %   based on unadjusted fixes
%     uvabs.adj                % same, but using adjusted fixes
% *   uvabs.raw_uvship         % based on profs.uship, profs.vship
% *   uvabs.adj_uvship
%
%  [nav, uvabs] = adcp_nav(fixes, profs, cfg)
%  -- or --
%  [nav, uvabs] = adcp_nav(fixes, profs, yearbase)
%


%
% 2001/11/01 EF
% 2001/11/26 JH changed "fix" to "nav"
% 2001/01/15 JH made all cfg optional (except yearbase)
% 2002/05/05 JH adjustment of positions still uses profile flags, but
%               velocities returned no longer reflect profile flags
% 2002/05/24 JH


%!!! These should be the same as in refsm_core.  The are only repeated
%    here in case someone wants to run adcp_nav without running refsm_core.

defcfg.yearbase                = [];
defcfg.min_pg                  = 30;
defcfg.min_maxpg               = 50;
defcfg.bl_half_width           = 3;
defcfg.xy_bl_half_width        = 6;
defcfg.ref_top                 = 1;
defcfg.ref_mid                 = 20;
defcfg.ref_bot                 = 20;
defcfg.ensemble_secs           = 300;
defcfg.fix_t_mismatch          = 10;
defcfg.min_xy_frac             = 0.05;
defcfg.min_uv_frac             = 0.5;
defcfg.max_dt                  = 310;
defcfg.lon_split               = [];
%% these are not used here, but are are needed for fillstruct to be happy.
defcfg.cleanup = [];
defcfg.dbname  = [];
defcfg.dbpath  = [];
defcfg.fixfile = [];
defcfg.prefix  = [];
defcfg.shipuv  = [];

if (~isstruct(arg3))  % then it should be yearbase, an integer
   cfg.yearbase = arg3;
else
   cfg = arg3;
end


cfg = fillstruct(defcfg, cfg, 0);
if (isempty(cfg.yearbase))
   error('must set cfg.yearbase')
end

have_uvship = 0;
if isfield(profs, 'uship'),
   have_uvship = 1;
end


% now we should have the same values in cfg if run with defaults refsm_core
% or adcp_nav

[nd, nt] = size(profs.u);


% Time is the column index, so make sure these are row vectors.
fixes.dd = fixes.dd(:).';
fixes.x = fixes.x(:).';
fixes.y = fixes.y(:).';
profs.dd = profs.dd(:).';


% For each profile time, find the fix with the closest time.
ifix = zeros(nt, 1);      % indices into fixes.*
abs_min = ifix;           % corresponding magnitude of time mismatch
tenpercent = fix(nt/10);

for ii = 1:nt,
   if ((nt > 1500) & rem(ii, tenpercent) == 0)
      fprintf('%d percent done...\n', round(100*ii/nt))
   end
   absdt = abs(fixes.dd - profs.dd(ii));
   [minjunk, i_closest] = min(absdt);
   if (length(i_closest) > 1)
      fprintf('duplicate closest matches found for index=%d,\n',ii)
      fprintf(' profile time: %10.7f, matches found at:\n',profs.dd(ii))
      for matchi = 1:length(i_closest)
         fprintf('fix time %10.7f\n', fixes.dd(i_closest(matchi)))
      end
   end
   ifix(ii) = i_closest(1);
   abs_min(ii) = absdt(i_closest(1));
end


% All vectors in fix structure match the columns in profs.
nav.dd = fixes.dd(ifix);
nav.x = fixes.x(ifix);
nav.y = fixes.y(ifix);


% A fix is "missing" if the time mismatch exceeds a threshold.
i_missing_fix = abs_min > (cfg.fix_t_mismatch / 86400);
% A fix is "extra" if there are two of them
i_extra_fix =  [(diff(ifix) == 0);0];
iinterp = i_missing_fix | i_extra_fix;  % Shorter name for temp use.
if (find(i_missing_fix));
   fprintf('%d missing fixes\n', length(find(i_missing_fix)))
end
if (find(i_extra_fix))
   fprintf('%d duplicate fix indices\n', length(find(i_extra_fix)))
end



nav.dd(iinterp) = profs.dd(iinterp);   % Assign profile time to time of
nav.x(iinterp) = NaN;                %   missing nav.
nav.y(iinterp) = NaN;


if 1 & (sum(iinterp) > 0),  % interpolate missing fixes.    Dangerous!

   nav.x(iinterp) = table1([nav.dd(~iinterp).', nav.x(~iinterp).'], ...
                          nav.dd(iinterp)).';
   nav.y(iinterp) = table1([nav.dd(~iinterp).', nav.y(~iinterp).'], ...
                          nav.dd(iinterp)).';
   badtxys = [nav.dd(iinterp)', nav.x(iinterp)',  nav.y(iinterp)', ...
             floor([0; 86400*diff(nav.dd(iinterp)')])];

   disp(['Interpolated ', num2str(sum(iinterp)), ' missing fixes:']);
   fprintf('    dday           x                 y        dt (seconds)\n');
   fprintf('%10.7f %15.7f %15.7f %7d\n', badtxys');

   fid_badxy = fopen('refsm_interp.log','w');
   fprintf(fid_badxy, ...
           '    dday           x                 y        dt (seconds)\n');
   fprintf(fid_badxy, '%10.7f %15.7f %15.7f %7d\n', badtxys');
   fclose(fid_badxy)
end


nav.dt = diff(nav.dd) * 86400;
nav.y_av = 0.5 * (nav.y(1:(end-1)) + nav.y(2:end));
nav.dx = lon_to_m(diff(nav.x), nav.y_av);
nav.dy = lat_to_m(diff(nav.y), nav.y_av);

% Raw ship velocity estimates:
nav.u = [NaN nav.dx ./ nav.dt];
nav.v = [NaN nav.dy ./ nav.dt];

% Now look at velocities.
%     save original (unflagged) data for use at the end;
origprofs = profs;
%     flag bad data for calculation
goodmask = ((profs.pg > cfg.min_pg) & (profs.pf == 0));
profs.u(~goodmask) = NaN;
profs.v(~goodmask) = NaN;
i_low_maxpg = (max(profs.pg) < cfg.min_maxpg);
dt = [0, diff(profs.dd) * 86400];
i_longtime = (dt > cfg.max_dt);

profs.u(:, i_longtime | i_low_maxpg) = NaN;
profs.v(:, i_longtime | i_low_maxpg) = NaN;

%%%



if have_uvship,
   uvf = zeros(nd, nt);
   filt_frac = uvf;
   uv.raw_uvship = (profs.u + repmat(profs.uship, nd, 1) ) ...
              + i * (profs.v + repmat(profs.vship, nd, 1));

   for ii = 1:nd,
      [uvf(ii, :), filt_frac(ii, :)] = bl_filt(uv.raw_uvship(ii, :), cfg.bl_half_width);
   end

   ibad = (filt_frac < cfg.min_uv_frac);
   uvf(ibad) = NaN;

   ii = cfg.ref_top:cfg.ref_bot;
   uv_mean_adj = mstdgap(uvf(ii,:) - uv.raw_uvship(ii, :));
   uv.adj_uvship = uv.raw_uvship + repmat(uv_mean_adj, nd, 1);
   nav.u_adj_uvship = real(uv_mean_adj) + profs.uship;
   nav.v_adj_uvship = imag(uv_mean_adj) + profs.vship;
end




uvf = zeros(nd, nt);
filt_frac = uvf;
uv.raw = (profs.u + repmat(nav.u, nd, 1) ) ...
           + i * (profs.v + repmat(nav.v, nd, 1));
for ii = 1:nd,
   [uvf(ii, :), filt_frac(ii, :)] = bl_filt(uv.raw(ii, :), cfg.bl_half_width);
end

ibad = (filt_frac < cfg.min_uv_frac);
uvf(ibad) = NaN;

ii = cfg.ref_top:cfg.ref_bot;
[uvres.mean, uvres.std, uvres.n] = mstdgap(uv.raw(ii, :) - uvf(ii, :));






% NaNs where there is no data.
%dt = diff(profs.dd) * 86400;
dt = diff(nav.dd) * 86400;
dt = [ (dt(1) - diff(dt(1:2))), dt];

uvdt = uvres.mean .* dt;
nan_mask = isnan(uvdt);
uvdt(nan_mask) = 0;                  % Zero where there is no data.
xy_adjust = cumsum(uvdt, 2);
xy_adjust(nan_mask) = NaN;
%% May need to put this in a loop to break it into segments at NaN.

if 0
   [xy_lp, xy_frac] = bl_filt(xy_adjust, cfg.xy_bl_half_width);
else
   xy_lp = NaN * zeros(1, nt);
   xy_frac = zeros(1, nt);
   i0 = find(diff([1, nan_mask]) == -1);
   i1 = find(diff([nan_mask, 1]) == 1);
   nsegs = length(i0);
   for iseg = 1:nsegs,
      ii = i0(iseg):i1(iseg);
      [xy_lp(ii), xy_frac(ii)] = bl_filt(xy_adjust(ii), cfg.xy_bl_half_width);
   end
end
xy_hp = xy_adjust - xy_lp;

tmp_mask = (xy_frac < cfg.min_xy_frac);
xy_hp(tmp_mask) = NaN + i * NaN;

xy_adj.mean = xy_hp;

nav.x_adj = nav.x - m_to_lon(real(xy_adj.mean), nav.y);
nav.y_adj = nav.y - m_to_lat(imag(xy_adj.mean), nav.y);
nav.dx_adj = lon_to_m(diff(nav.x_adj), nav.y_av);
nav.dy_adj = lat_to_m(diff(nav.y_adj), nav.y_av);
nav.u_adj = [NaN nav.dx_adj ./ nav.dt];
nav.v_adj = [NaN nav.dy_adj ./ nav.dt];


% If we don't have a valid adjustment for a fix, leave
% the original fixes.
ibad = isnan(nav.x_adj);
nav.x_adj(ibad) = nav.x(ibad);
nav.y_adj(ibad) = nav.y(ibad);
nav.u_adj(ibad) = nav.u(ibad);
nav.v_adj(ibad) = nav.v(ibad);
nav.was_adjusted = ones(size(nav.x));
nav.was_adjusted(ibad) = 0;
nav.was_interpolated = zeros(size(nav.x));
nav.was_interpolated(iinterp) = 1;


uv.adj = (origprofs.u + repmat(nav.u_adj, nd, 1) ) ...
          + i * (origprofs.v + repmat(nav.v_adj, nd, 1));

