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)
%
%
%     cfg.yearbase = 2000;     (required)
%     cfg.min_pg = 50;         % Don't use velocity if pg is less than this.
%     cfg.min_maxpg = 70;      % Don't use profile unless part has greater pg.
%     cfg.bl_half_width = 3;   % For filtering velocity.
%     cfg.xy_bl_half_width = 6;% For high-pass filtering position adjustments.
%     cfg.ref_top = 1;         % first bin of reference layer
%     cfg.ref_mid = 20;        % not used at present
%     cfg.ref_bot = 20;        % last bin of reference layer
%     cfg.fix_t_mismatch = 10; % seconds between ensemble time and fix time
%     cfg.min_xy_frac = 0.05;  % data fraction of filter weight integral,
%     cfg.min_uv_frac = 0.5;   %   _xy_ for position, _uv_ for velocity
%     cfg.max_dt = 310;        % If a velocity profile time exceeds the
%                              %   previous one by more than this many
%                              %   seconds, there is a gap; don't use it.
%
%     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
%
%     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
%
%
%     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
%
%
%  usage:
%  [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)


defcfg.yearbase = [];     
defcfg.min_pg = 50;         
defcfg.min_maxpg = 70;      
defcfg.bl_half_width = 3;   
defcfg.xy_bl_half_width = 6;
defcfg.ref_top = 1;         
defcfg.ref_mid = 20;        
defcfg.ref_bot = 20;        
defcfg.fix_t_mismatch = 10; 
defcfg.min_xy_frac = 0.05;  
defcfg.min_uv_frac = 0.5;   
defcfg.max_dt = 310;        


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


[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
for ii = 1:nt,
   absdt = abs(fixes.dd - profs.dd(ii));
   i_closest = find(absdt == min(absdt));
   ifix(ii) = i_closest(1);
   abs_min(ii) = absdt(i_closest);
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);
ibad = i_missing_fix;  % Shorter name for temp use.
nav.dd(ibad) = profs.dd(ibad);   % Assign profile time to time of
nav.x(ibad) = NaN;                %   missing nav.
nav.y(ibad) = NaN;

if 1 & (sum(ibad) > 0),  % interpolate missing fixes.    Dangerous!
   nav.x(ibad) = table1([nav.dd(~ibad).', nav.x(~ibad).'], ...
                          nav.dd(ibad)).';
   nav.y(ibad) = table1([nav.dd(~ibad).', nav.y(~ibad).'], ...
                          nav.dd(ibad)).';
   disp(['Interpolated ', num2str(sum(ibad)), ' missing fixes.']);
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.
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;

%%%


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;

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

