function [depth_yoyo, time_yoyo, number_yoyo, whichway_yoyo] = yoyo_classifier( varargin )

%[ depth, time, number, whichway ] = yoyo_classifier( depth, time, delta_t )
%   Takes depth as a function of time and returns an array in which upcast
%   and downcasts have been identified, numbered sequential, and the time
%   of start and end of each cast captured.
%
%   depth = depth vector
%   time = time vector
%   delta_t = optional argument determining interval over which to test
%   depth derivative.  Default is 50 units of time.
%
% The variables returned are:
%
%   depth_yoyo = depth vector (single valued)
%   time_yoyo = time vector (goes w single valued depth array
%   number_yoyo = optional argument determining interval over which to test
%   whichway_yoyo = going up (-) or down (+)
%
% Compute a depth rate across a long timebase so as to minimize
% susceptability to noise in the depth sensor or jitter in the time stamp.
%
% J. G. Bellingham, Sept 4, 2013
% MBARI

% Get arguments
depth = double(varargin{1});
time = double(varargin{2});
if nargin == 3
    delta_t = varargin{3};
else
    delta_t = 5*(time(length(depth))-time(1))/length(depth);
end

% Debugging:
debug = 1;

% -----------------------------------------------
% ----- FIGURE OUT WHEN VEHICLE GOING UP AND DOWN
% Compute depth rate
nn = length(depth);
dt = median(diff(time));
delta = round(delta_t/dt/2);  % Make sure this is an even number...

% Compute depth rate
drate = depth*0;
drate(2:nn(1)-1) = (depth(3:nn(1))-depth(1:nn(1)-2))./(time(3:nn(1))-time(1:nn(1)-2));
drate = smooth(drate, delta);

% Find portion of run going up, and portion going down
% Handle up and down separately, since the depth rate may not be the same
% going up as going down (different angles, buoyancy, etc.)
threshold_up = median(abs(drate(drate>0)))/5; % NOTE THRESHOLDING
threshold_down = median(abs(drate(drate<0)))/5; % NOTE THRESHOLDING

i_up = find(drate>threshold_up);
i_down = find(drate<-threshold_down);

% Create a vector that contains the direction of motion
whichway = depth * 0;
whichway(i_up)= 1;
whichway(i_down)= -1;

% -- Plot for inspection (debugging only)
if debug
    hold off;
    plot(time(i_up), depth(i_up), '.');
    hold on
    plot(time(i_down), depth(i_down), 'r.');
    plot(time, depth, 'g');
    hold off;
end;

% -----------------------------------------------
% ----- NOW FIND YOYO SEGMENTS BY RECOGNIZING INFLECTION POINTS
% Create an array marking the start and end of yoyos
last = 0;
jj = 1;

ok = find((whichway ~= 0) & (depth > 1)); % Creat new index basis

whichway_ok = whichway(ok);  % NEW WORKING VARIABLES in new index basis
depth_ok = depth(ok);
time_ok = time(ok);
inflection_ok = diff(whichway_ok);  % Begining and ends of yoyos

iturn = find(abs(inflection_ok) > 1);  % Inflection points in index basis

% Describe each yoyo segment...
for i = 1:(length(iturn)+1)
    if i == 1  % Handle exception of first decent...
        yoyo.i_start(i) = 1;
    else
        yoyo.i_start(i) = iturn(i-1)+1;
    end;
    
    if i == (length(iturn)+1)  % Handle exception of final climb...
        yoyo.i_end(i) = length(depth_ok);
    else
        yoyo.i_end(i) = iturn(i);
    end;
    
    yoyo.dir(i) = whichway_ok(yoyo.i_start(i)); % Dive direction...depth increasing.
    yoyo.extent(i) = depth_ok(yoyo.i_end(i))-depth_ok(yoyo.i_start(i));
    yoyo.duration(i) = time_ok(yoyo.i_end(i))-time_ok(yoyo.i_start(i));
end;

% -------------------------------------------
% Now take each yoyo segment and make sure it is really a full segment,
% for example remove 'bobbles' where the vehicle reverses depth direction
% briefly.
yoyo_store = yoyo;

i = 2;
while i < length(yoyo.i_start)
    
    % Bobble tests...is yo just a blip in a longer yo the opposite
    % direction?
    test1 = ((yoyo.extent(i-1)+yoyo.extent(i+1))/yoyo.extent(i))<-4;
    test2 = depth_ok(yoyo.i_end(i-1))*yoyo.dir(i-1) < depth_ok(yoyo.i_end(i+1))*yoyo.dir(i+1);
    
    % Extent test - if it is only a few meters, it is not a yoyo....
    test3 = abs(yoyo.extent(i)) < 3;
    
    % Now remove bobbles and brief excursions.
    if (test1 && test2) | test3
        nn = length(yoyo.i_start);
        if i == nn-1
            yoyo.i_start = yoyo.i_start(1:i-1);
            yoyo.i_end = yoyo.i_end([1:i-2 i+1]);
        else
            yoyo.i_start = yoyo.i_start([1:i-1 i+2:nn]);
            yoyo.i_end = yoyo.i_end([1:i-2 i+1:nn]);
        end;
        
        yoyo.dir = whichway_ok(yoyo.i_start); % Dive direction...depth increasing.
        yoyo.extent = depth_ok(yoyo.i_end)-depth_ok(yoyo.i_start);
        yoyo.duration = time_ok(yoyo.i_end)-time_ok(yoyo.i_start);
    else
        i = i+1;
    end;
end;

if debug
    hold off
    plot(time, depth, 'g');
    hold on;
    plot(time_ok(yoyo_store.i_start), depth_ok(yoyo_store.i_start), 'ok')
    plot(time_ok(yoyo.i_start), depth_ok(yoyo.i_start), 'or')
    plot(time_ok(yoyo_store.i_end), depth_ok(yoyo_store.i_end), '*k')
    plot(time_ok(yoyo.i_end), depth_ok(yoyo.i_end), '*r')
    hold off;
end;


% -------------------------------------------
% Make sure each yo single valued in depth...construct arrays to return
for i = 1:length(yoyo.i_start)
    okok(yoyo.i_start(i)) = 1;
    number(yoyo.i_start(i)) = i;
    d = depth_ok(yoyo.i_start(i));
    
    for j = yoyo.i_start(i)+1:yoyo.i_end(i)
        if depth_ok(j)*yoyo.dir(i) > d*yoyo.dir(i)
            okok(j) = 1;
            number(j) = i;
            d = depth_ok(j);
        else
            okok(j) = 0;
            number(j) = 0;
            whichway_ok(j) = 0;
        end;
    end;
end;

depth_yoyo = depth_ok(okok == 1);
time_yoyo = time_ok(okok == 1);
number_yoyo = number(okok == 1);
whichway_yoyo = whichway_ok(okok == 1);

% -- Plot for inspection (debugging only)
if debug
    hold off
    plot(time, depth, 'g');
    hold on;
    plot(time_yoyo(whichway_yoyo ==1), depth_yoyo(whichway_yoyo ==1), '.r');
    plot(time_yoyo(whichway_yoyo ==-1), depth_yoyo(whichway_yoyo ==-1), '.b');
    hold off;
end;

% -------------------------------------------
% DONE!!!
