% -----------------------------------------------------------------------------
% process depth  1) remove updates which are not really updates...
%                2) compute best estimate of time of depth measurement.
%                3) compute depth rate...
%                4) classify yoyos
%
% Depth measurement actually initiated on previous vehicle time cycle, complete
% after about 0.1 seconds.
%
% Yoyos require depth rate > yoyo_discriminator m/s (typically 0.1 m/s)

d = data(:, m_depth);
n = length(d);
d(1:(n-1)) = d(2:n);

tmplt = d*NaN;
yoyo_discriminator = 0.1;

% Condition depth data by removing data points in which temperature
% of Parosci is being updated rather than depth.  This occurs every
% ten cycles, and is evident by an almost unchanged depth reading.
% Need turn measurements during which depth not actually measured into NaNs.

cyc = find(abs(diff(d)) < 0.0015);      % Find sequential unchanged depths
tens = find(abs(diff(cyc)-10) < 0.003); % Find unchanged pairs ten cycles apart
offset = mod(cyc(tens), 10);

% If offset consistent, then make non-updates into NaNs...
if (std(offset) < 0.2) & (length(offset) > 10) 
   d_off = round(mean(offset));
   bad = 10*(0:(floor(length(d)/10)-1))+d_off+1;
   d(bad) = NaN;
end;

% Figure out which depths are okay....
ok = find(~isnan(d));

% Find depth rates...
d_r = diff(d(ok))./diff(time(ok));
d_r(length(ok)) = 0;
d_r = smooth(d_r, 11);

d_rate =  tmplt;
d_rate(ok) = d_r;

% -------------------------------------------------------------------
% Classify yoyos...but first find begining and end of yoyos....

ind10f = min(find(d > 10));   % First time at 10 m
ind10l = max(find(d > 10));   % Last time at 10 m

earliest = max(find(d_rate(1:ind10f) < yoyo_discriminator)); 
latest = min(find(d_rate(ind10l:length(d_rate)) > (-yoyo_discriminator)))+ind10l-1; 
if length(latest) == 0
   latest = length(d_rate);
end;

ok1 = earliest:latest;  % data set

dwn = find(diff(d_rate(ok1) > yoyo_discriminator) == 1);

% Make sure that yoyos are long enough to really be full yoyos
okok = find(diff(dwn) > 100);
okok(2:(length(okok)+1)) = okok+1;
okok(1) = 1;
dwn = ok1(dwn(okok));
 
% Make an array with yoyo numbers...
yoyo = tmplt;
if length(dwn) > 2
   yoyo(1:dwn(1)) = 1;
   for i = 2:length(dwn)
      yoyo(dwn(i-1):dwn(i)) = i;
   end;
   yoyo(dwn(length(dwn)):length(data(:, m_present_time))) = i+1;
end;

% Classify going up or down
going_up = find(d_rate < -yoyo_discriminator);
going_down = find(d_rate > yoyo_discriminator);

clear d_r;  clear okok; clear cyc; clear tens; clear offset;
clear temp; clear ok; clear dwn; clear tmplt; clear ok1; 
clear earliest; clear latest;

