% ---------------------------------------------------
% This routine corrects navigation and computes currents, using the
% difference between DR and GPS updates.
%
% Creates a structure "pos" which contains a variety of useful parameters,
% including corrected lat and long (lat_c and long_c).
%
% Also creates a table with parameters for each underwater transit:
% "transit".
%
% J. G. Bellingham, June 1, 2013

% Assumes data loaded already....else "load shore.mat"

if (~exist('dbg'))
    dbg = 1; % Make this one if you want diagnostic plots.
end;

% ---------------------------------------------------
% First, find when the vehicle is on the surface by its GPS updates.
% Figure out the first surfacing for cases where there are multiple GPS
% updates per surface.

time_fix.first = time_fix.time*0;
time_fix.first(1) = 1;
time_fix.surface_number(1) = 1;
for i = 2:length(time_fix.time)
    t1 = time_fix.time(i-1);
    t2 = time_fix.time(i);
    
    ave_depth = mean(depth.value(find(depth.time > t1 & depth.time < t2)));
    if ave_depth > 5
        time_fix.first(i) = 1;
        time_fix.surface_number(i) = time_fix.surface_number(i-1)+1;
    else
        time_fix.surface_number(i) = time_fix.surface_number(i-1);
    end;
end;

% ---------------------------------------------------
% Construct vehicle track on depth timebase.
% Compute various derived values like  u and v

pos.lat = interp1(latitude.time, latitude.value, depth.time);
pos.long = interp1(longitude.time, longitude.value, depth.time);
pos.time = depth.time;

% Get rid of portions of the data which are NaNs (will break plots, etc.)
ok = find(~isnan(pos.lat) & ~isnan(pos.long) & ~isnan(pos.time));
pos.lat = pos.lat(ok);
pos.long = pos.long(ok);
pos.time = pos.time(ok);
pos.depth = depth.value(ok);

% Compute derivatives of values.
pos.d_lat = [0 diff(pos.lat)];
pos.d_long = [0 diff(pos.long)];
pos.d_t = [1 diff(pos.time)];  % Note:  first value set to one to avoid divide by zero.
pos.average_lat = mean(pos.lat);
pos.average_long = mean(pos.long);

% Compute vehicle velocity, heading, and speed.
pos.u = -pos.d_lat ./ pos.d_t * (pi* 12742000/360) / (24*3600) ; % Units of m/s
pos.v = -pos.d_long ./ pos.d_t * (pi* 12742000/360) *cos(pos.average_lat * pi/180) / (24*3600) ;
pos.heading = (180/pi)*atan2(pos.v, pos.u); % Measured clockwise from north!
pos.speed = (pos.u.^2 + pos.v.^2) .^(1/2);

pos.d_t(1) = 0; % Set to actual...

% If in debugging mode, generate a series of diagnostic plots
if dbg == 1
    t0 = pos.time(1);
    over = find(pos.depth < 2);
    under = find(pos.depth > 3);
    first = find(time_fix.first);
    first_not = find(~time_fix.first);
    
    figure;
    plot(pos.time(over) - t0, pos.lat(over), 'b*', pos.time(under) - t0, pos.lat(under), 'bo', latitude_fix.time(first)-t0, latitude_fix.value(first), 'rd', latitude_fix.time(first_not)-t0, latitude_fix.value(first_not), 'kd');
    legend('DR on surface', 'DR submerged', 'First GPS', 'Subsequent GPS', 'Location', 'SouthWest');
    title('Latitude');
    xlabel('Mission Time (days)');
    ylabel('Latitude (deg)');
    
    figure;
    plot(pos.time(over) - t0, pos.long(over), 'b*', pos.time(under) - t0, pos.long(under), 'bo', longitude_fix.time(first)-t0, longitude_fix.value(first), 'rd', longitude_fix.time(first_not)-t0, longitude_fix.value(first_not), 'kd');
    legend('DR on surface', 'DR submerged', 'First GPS', 'Subsequent GPS', 'Location', 'SouthWest');
    title('Longitude');
    xlabel('Mission Time (days)');
    ylabel('Longitude (deg)');
    
    figure;
    plot(pos.time(over) - t0, pos.u(over), 'b*', pos.time(under) - t0, pos.u(under), 'bo', pos.time(over) - t0, pos.v(over), 'r*',pos.time(under) - t0, pos.v(under), 'ro');
    legend('u on surface', 'u submerged', 'v on surface', 'v submerged');
    title('Velocities (m/s)')
    xlabel('Mission Time (days)');
    ylabel('Velocity (m/s)');
    
    figure;
    plot(pos.time(over) - t0, pos.speed(over), 'go', 'MarkerSize',7,'MarkerFaceColor','g'); hold on;
    plot(pos.time(under) - t0, pos.speed(under), 'bx', 'MarkerSize',7);
    xlabel('Mission Time (days)');
    ylabel('Speed (m/s)');
    
    figure;
    plot(pos.time(over) - t0, pos.heading(over), 'go', 'MarkerSize',7,'MarkerFaceColor','g'); hold on;
    plot(pos.time(under) - t0, pos.heading(under), 'bx', 'MarkerSize',7);
    xlabel('Mission Time (days)');
    ylabel('Heading (deg)');
end;
% ---------------------------------------------------
% Find begining and finish of underwater segments using GPS....

j = 1;
for i=1:length(time_fix.time)-1;
    if time_fix.first(i+1)
        transit.begin(j) = time_fix.time(i);
        transit.finish(j) = time_fix.time(i+1);
        transit.length(j) = transit.finish(j) - transit.begin(j);
        transit.GPS_i(j) = i+1;
        j=j+1;
    end;
end;
    
% ---------------------------------------------------
% Now create corrected lat, long track and compute average current.

% Find lat/longs between transits...

pos.lat_c = pos.lat;
pos.long_c = pos.long;
for i = 1:length(transit.begin)
    % Find subset of data corresponding to this transit, which is
    % underwater.
    ok = find((pos.time > transit.begin(i)) & (pos.time < transit.finish(i)) & pos.depth > 2);
    t0 = pos.time(ok(1));
    last = length(ok);
    
    % Compute difference between DR suface position and GPS surface
    transit.ave_depth(i) = mean(pos.depth(ok));
    
    % Compute difference between DR suface position and GPS surface
    % position.  Also compute length of time of transit.  This is so
    % average current experienced can be computed.
    d_t = pos.time(ok(last)) - pos.time(ok(1));
    d_lat = latitude_fix.value(transit.GPS_i(i)) - pos.lat(ok(last));
    d_long = longitude_fix.value(transit.GPS_i(i)) - pos.long(ok(last));
    
    % Compute corrected position assuming starting point ok, and position
    % motified by uniform current (current = d_lat/d_t, d_long/d_t).
    pos.lat_c(ok) = pos.lat(ok) + d_lat/d_t * (pos.time(ok)-pos.time(ok(1)));
    pos.long_c(ok) = pos.long(ok) + d_long/d_t * (pos.time(ok)-pos.time(ok(1)));
    
    % Tabulate average current experienced during transit.  For the
    % purposes of plotting, find the midpoint of the transit.
    midpoint_t = (mean(pos.time(ok)));  % Find the point closes to halfway through the transit
    transit.lat_mid(i) = interp1(pos.time(ok), pos.lat(ok), midpoint_t);  % Find center of transit in corrected nav position
    transit.long_mid(i) = interp1(pos.time(ok), pos.long(ok), midpoint_t);
    transit.lat_mid_c(i) = interp1(pos.time(ok), pos.lat_c(ok), midpoint_t);  % Find center of transit in corrected nav position
    transit.long_mid_c(i) = interp1(pos.time(ok), pos.long_c(ok), midpoint_t);
    transit.current_u(i) = d_lat / d_t * (pi* 12742000/360) / (24*3600) ; % Units of m/s
    transit.current_v(i) = d_long / d_t * (pi* 12742000/360) *cos(transit.lat_mid(i) * pi/180) / (24*3600) ;
end;

% If debugging, generate a diagnostic plot of length of vehicle transists
% and average depth.
if dbg == 1
    figure;
    [haxes,hline1,hline2] = plotyy(transit.begin-transit.begin(1), transit.length, transit.begin-transit.begin(1), transit.ave_depth);
    % Put labels on the respective y axes, and change the line styles to
    % show data points...
    axes(haxes(1)); 
    ylabel('Transit Length (days)');
    set(hline1,'Marker','o')
    axes(haxes(2)); 
    ylabel('Average Transit Depth (m)');
    set(hline2,'Marker','o')
    xlabel('Mission Time (days)');
end;

clear('i', 'terminate', 'dbg', 'ok', 't0', 'last', 'd_t', 'd_lat', 'd_long', 't1', 't2', 'over', 'under');
clear('first', 'first_not', 'haxes','hline1','hline2');
