% line_capture_analysis_DAT.m
clear;close all
setAxesDefaults

% This script expects the log file names as:
% - mat file: logname.mat (e.g., 20220615T212807.mat)
% - syslog: logname.log  (e.g., 20220615T212807.log)
% Feel free to mod the lines that follow to accommodate other paths/nameing
logname = '20220527T035028';

filename = ['/Users/byraanan/tmp/' logname  '.mat'];
syslog_path = ['/Users/byraanan/tmp/' logname '.log'];

c = lrauv_load(filename);
syslog = readsyslog( syslog_path  );

c.depth.z = c.depth.value;
[ mission ] = getmission(syslog, c.depth, 1);

%% lineCaptureHoming params

% hold_alt = 7;
% hold_alt = c.lineCaptureHoming.HoldAltitude.value(end);
% min_alt = 5;
% min_alt = c.lineCaptureDepth.LineCapture.MinAltitude.value(end);
% min_alt = c.lineCaptureHoming.LineCapture.MinAltitude.value(end);

% TrackingUpdatePeriod = c.lineCaptureDepth.TrackingUpdatePeriodMidcourse.value(end); % DefaultUnder

%% Extract line capture runs

[ L ] = extract_line_capture_runs( syslog );
T = array2table(L, 'VariableNames', {'start_time', 'dock_time', 'miss_time'});

start_range = c.acoustic_contact_range.value(closest(L(:,1), c.acoustic_contact_range.time));
finish_suc_range = c.acoustic_contact_range.value(closest(L(:,2), c.acoustic_contact_range.time));
finish_fail_range = c.acoustic_contact_range.value(closest(L(:,3), c.acoustic_contact_range.time));

success_rate = sum(~isnan(T.dock_time))/length(T.start_time);

approach_id = false(size(c.acoustic_contact_range.time));
for k=1:size(L,1)

    start_time = T.start_time(k);
    end_time = T.dock_time(k);
    if isnan(end_time)
        end_time = T.miss_time(k);
    end

    id = c.acoustic_contact_range.time >= start_time & c.acoustic_contact_range.time <= end_time;
    id = id & ([false, diff(c.acoustic_contact_range.value)] < 0);
    approach_id = (approach_id | id);
end


%% Vertical control

% c.RDI_Pathfinder.height_above_sea_floor.value(c.RDI_Pathfinder.height_above_sea_floor.value > 100) = NaN;

figure;
set(gcf,'Units','normalized','Position',[0 0 1 1])

ax=axes;
set(ax,'Unit','normalized','Position',[0.05 0.675 0.9 0.275])
plot(c.depth.time, c.depth.value); hold on;
% plot(c.depth.time, ones(size(c.depth.time))*c.altitudeServo.InitDepth.value);
plot(c.VerticalControl.depthCmd.time, c.VerticalControl.depthCmd.value);
scatter(c.VerticalControl.depthCmd.time, c.VerticalControl.depthCmd.value);
% plot(c.depth.time, ones(size(c.depth.time))*dock_depth, '--k');
ylabel('Depth (m)')
legend('Depth','Depth cmd')
title(ax, logname)
reverseYDir


% plot(c.SpeedControl.speedCmd.time, c.SpeedControl.speedCmd.value)
% scatter(c.LineCapture.Speed.time, c.LineCapture.Speed.value)
% scatter(c.LineCapture.GuidanceMode.time, c.LineCapture.GuidanceMode.value)

ax1 = axes;
set(ax1,'Unit','normalized','Position',[0.05 0.375 0.9 0.275])
plot(c.height_above_sea_floor.time,c.height_above_sea_floor.value - 8.5);hold on;
% plot(c.RDI_Pathfinder.height_above_sea_floor.time, ones(size(c.RDI_Pathfinder.height_above_sea_floor.time))*min_alt);
% plot(c.RDI_Pathfinder.height_above_sea_floor.time, ones(size(c.RDI_Pathfinder.height_above_sea_floor.time))*hold_alt);
ylabel('height above sea floor (m)')
legend('Altitude','Min Altitude','Hold Altitude')

% ax1 = axes;
% set(ax1,'Unit','normalized','Position',[0.05 0.375 0.9 0.275])
% plot(c.DVL_micro.Beam1Range.time,c.DVL_micro.Beam1Range.value);hold on;
% plot(c.DVL_micro.Beam2Range.time,c.DVL_micro.Beam2Range.value);hold on;
% plot(c.DVL_micro.Beam3Range.time,c.DVL_micro.Beam3Range.value);hold on;
% plot(c.DVL_micro.Beam4Range.time,c.DVL_micro.Beam4Range.value);hold on;
% % plot(c.RDI_Pathfinder.height_above_sea_floor.time, ones(size(c.RDI_Pathfinder.height_above_sea_floor.time))*min_alt);
% % plot(c.RDI_Pathfinder.height_above_sea_floor.time, ones(size(c.RDI_Pathfinder.height_above_sea_floor.time))*hold_alt);
% ylabel('height above sea floor (m)')
% legend('beam 1','beam 2','beam 3','beam 4')


% ax1 = axes;
% set(ax1,'Unit','normalized','Position',[0.05 0.375 0.9 0.275])
% plot(c.platform_mass_position.time,c.platform_mass_position.value*1000);hold on;


% ax1 = axes;
% set(ax1,'Unit','normalized','Position',[0.05 0.375 0.9 0.275])
% plot(c.platform_elevator_angle.time, rad2deg(c.platform_elevator_angle.value));hold on;

% ax1 = axes;
% set(ax1,'Unit','normalized','Position',[0.05 0.375 0.9 0.275])
% plot(c.platform_roll_angle.time, rad2deg(c.platform_roll_angle.value));
% ylabel('Roll (deg)')

ax2 = axes;
set(ax2,'Unit','normalized','Position',[0.05 0.075 0.9 0.275])
plot(c.platform_pitch_angle.time, rad2deg(c.platform_pitch_angle.value)); hold on;
scatter(c.VerticalControl.pitchCmd.time, rad2deg(c.VerticalControl.pitchCmd.value))
ylabel('pitch (deg)')

% ax2 = axes;
% set(ax2,'Unit','normalized','Position',[0.05 0.075 0.9 0.275])
% plot(c.platform_orientation.time,rad2deg(c.platform_orientation.value));hold on;
% ylabel(ax2, 'heading (deg)')

% ax2 = axes;
% set(ax2,'Unit','normalized','Position',[0.05 0.075 0.9 0.275])
% plot(c.VerticalControl.verticalMode.time,c.VerticalControl.verticalMode.value);hold on;
% ylabel(ax2, 'verticalMode')

% ax2 = axes;
% set(ax2,'Unit','normalized','Position',[0.05 0.075 0.9 0.275])
% plot(c.platform_velocity_wrt_ground.time, c.platform_velocity_wrt_ground.value)
% ylabel('xyz vel wrt ground (m/s)')
% legend('x','y','z')

% ax2 = axes;
% set(ax2,'Unit','normalized','Position',[0.05 0.075 0.9 0.275])
% plot(c.height_above_sea_floor.time,c.height_above_sea_floor.value);hold on;
% plot(c.height_above_sea_floor.time,ones(size(c.height_above_sea_floor.value))*c.lineCaptureHoming.LineCapture.DockingAltitude.value(end), '--k')
% % plot(c.RDI_Pathfinder.height_above_sea_floor.time, ones(size(c.RDI_Pathfinder.height_above_sea_floor.time))*min_alt);
% % plot(c.RDI_Pathfinder.height_above_sea_floor.time, ones(size(c.RDI_Pathfinder.height_above_sea_floor.time))*hold_alt);
% ylabel('height above sea floor (m)')
% legend('Altitude','Min Altitude','Hold Altitude')

% ax2 = axes;
% set(ax2,'Unit','normalized','Position',[0.05 0.075 0.9 0.275])
% plot(c.SpeedControl.speedCmd.time, c.SpeedControl.speedCmd.value); hold on
% scatter(c.SpeedControl.speedCmd.time, c.SpeedControl.speedCmd.value)
% ylabel('speedCmd (m/s)')

linkaxes([ax ax1 ax2],'x')
dynamicDateTicks([ax ax1 ax2],'link')

set([ax,ax1],'XTickLabel',[])
% set([ax,ax1,ax2],'FontSize',24)

% export_fig(gcf, '~/Desktop/lineCaptureHoming_altitude.png','-r300')

%% Homing
% close all

f = figure;
f.Position = [0.6667    0.0563    0.3333    0.8788];
corder = colorOrder;

ax = gca;
ax.Position = [0.05, 0.75, 0.9, 0.2];
plot(c.depth.time, c.depth.value); hold on;
% plot(c.depth.time, ones(size(c.depth.time))*c.altitudeServo.InitDepth.value);
plot(c.VerticalControl.depthCmd.time, c.VerticalControl.depthCmd.value);
scatter(c.VerticalControl.depthCmd.time, c.VerticalControl.depthCmd.value, 5, 'filled');
ylabel(ax,'depth (m)')
legend('Depth','Depth cmd')
title(ax, logname)
reverseYDir

ax1 = axes; % delete(ax1)
ax1.Position = [0.05, 0.525, 0.9, 0.2];
plot(c.acoustic_contact_range.time, c.acoustic_contact_range.value); hold on;
scatter(c.TrackAcousticContact.range_to_contact.time, c.TrackAcousticContact.range_to_contact.value,'filled')
scatter(c.acoustic_contact_range.time(approach_id), c.acoustic_contact_range.value(approach_id),'filled');
ylabel('range (m)')
legend('universal','TrackAcousticContact','approach')

% DAT
ax2 = axes; % delete(ax2)
ax2.Position = [0.05, 0.3, 0.9, 0.2];
plot(c.DAT.azimuth_vehicleFrame.time, rad2deg(c.DAT.azimuth_vehicleFrame.value)); hold on;
scatter(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.time, rad2deg(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value),'filled')
% scatter(c.ExternalSim.homingSensorAzimSim.time, rad2deg(c.ExternalSim.homingSensorAzimSim.value));
plot(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.time, zeros(size(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value)), 'k--');
ylabel('azimuth (deg)')
legend(ax2,'Azimuth (+ to stbd)','TrackAcousticContact')

ax3 = axes; % delete(ax3)
ax3.Position = [0.05, 0.075, 0.9, 0.2];
plot(c.platform_orientation.time, rad2deg(c.platform_orientation.value)); hold on;
scatter(c.TrackAcousticContact.heading_to_contact.time, rad2deg(c.TrackAcousticContact.heading_to_contact.value),'filled')
scatter(c.HorizontalControl.headingCmd.time, rad2deg(c.HorizontalControl.headingCmd.value),'filled');
ylim(ax3, [-10 370])
ylabel('heading (deg)')
legend('heading','TrackAcousticContact', 'headingCmd')

% plot(c.platform_roll_angle.time, rad2deg(c.platform_roll_angle.value));

linkaxes([ax ax1 ax2 ax3], 'x')
dynamicDateTicks([ax ax1 ax2 ax3],'link')
set([ax,ax1,ax2],'XTickLabel',[])
% set([ax,ax1,ax2,ax3],'FontSize',24)

% export_fig(gcf, sprintf('~/Desktop/lineCapture_%s.png', logname), '-r300')

%% Horizontal Guidance
close all

f = figure;
f.Position = [0 0 1 1];
corder = colorOrder;

ax = gca;
ax.Position = [0.05, 0.75, 0.9, 0.2];
plot(c.acoustic_contact_range.time, c.acoustic_contact_range.value); hold on;
scatter(c.TrackAcousticContact.range_to_contact.time, c.TrackAcousticContact.range_to_contact.value, 20, 'filled')
scatter(c.acoustic_contact_range.time(approach_id), c.acoustic_contact_range.value(approach_id),'filled');
scatter(ax, L(:,1), start_range, 75, 'g', 'filled')
scatter(ax, L(:,2), finish_suc_range, 75, 'm', 'filled')
scatter(ax, L(:,3), finish_fail_range, 75, 'r', 'filled')
ylabel('range (m)')
legend('universal','TrackAcousticContact', 'Approach', 'Homing start', 'Dock success', 'Missed')
title(ax, logname, 'Interpreter', 'none')

ax1 = axes; % delete(ax1)
ax1.Position = [0.05, 0.525, 0.9, 0.2];
plot(c.DAT.azimuth_vehicleFrame.time, rad2deg(c.DAT.azimuth_vehicleFrame.value)); hold on;
scatter(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.time, rad2deg(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value),'filled')
plot(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.time, zeros(size(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value)), 'k--');
ylabel('azimuth (deg)')
% legend(ax1,'xAngle (+ to stbd)','TrackAcousticContact')
legend(ax1,'xAngle to target (+ to stbd)')

ax2 = axes; % delete(ax2)
ax2.Position = [0.05, 0.3, 0.9, 0.2];
plot(c.platform_orientation.time, rad2deg(c.platform_orientation.value)); hold on;
scatter(c.TrackAcousticContact.heading_to_contact.time, rad2deg(c.TrackAcousticContact.heading_to_contact.value),'filled')
% scatter(c.HorizontalControl.headingCmd.time, rad2deg(c.HorizontalControl.headingCmd.value), 'filled');
ylim(ax2, [-10 370])
ylabel('heading (deg)')
% legend('heading','TrackAcousticContact', 'headingCmd')
legend('vehicle heading','bearing to target', 'headingCmd')

ax3 = axes; % delete(ax3)
ax3.Position = [0.05, 0.075, 0.9, 0.2];
plot(c.platform_rudder_angle.time, rad2deg(c.platform_rudder_angle.value)); hold on;
scatter(c.platform_rudder_angle.time, rad2deg(c.platform_rudder_angle.value)); hold on;
scatter(c.HorizontalControl.rudderAngleAction.time, rad2deg(c.HorizontalControl.rudderAngleAction.value),'filled')
ylabel('rudder angle (deg)')
legend('rudder angle','rudder angle','rudder angle action')
% plot(c.platform_roll_angle.time, rad2deg(c.platform_roll_angle.value));

linkaxes([ax ax1 ax2 ax3], 'x')
dynamicDateTicks([ax ax1 ax2 ax3],'link')
set([ax,ax1,ax2],'XTickLabel',[])
% set([ax,ax1,ax2,ax3],'FontSize',24)

% export_fig(gcf, '~/Desktop/lineCaptureHoming_horizontal_guidance.png','-r300')

%% DockingServo cable present

figure;
plot(c.DockingServo.cableValue.time,c.DockingServo.cableValue.value);hold on
scatter(c.DockingServo.cableValue.time,c.DockingServo.cableValue.value, 'filled');
ylabel('cable value (count)')
dynamicDateTicks

%% DockingServo state

ca
f = figure; % gcf;
% f =  gcf;
% f.Position = [0 0 1 1];
f.Position = [0    0.0500    0.3333    0.8900];
corder = colorOrder;

ax = gca;
ax.Position = [0.05, 0.75, 0.9, 0.2];
plot(c.Dock.docking_state.time, c.Dock.docking_state.value); hold on;
scatter(c.Dock.docking_state_cmd.time, c.Dock.docking_state_cmd.value, 'filled');
ylim([-0.5 3.5])
ylabel('docking state/cmd (enum)')
legend('docking state','docking state cmd')

ax1 = axes; % delete(ax1)
ax1.Position = [0.05, 0.525, 0.9, 0.2];
plot(c.DockingServo.armAngle.time, rad2deg(c.DockingServo.armAngle.value)); hold on;
ylabel('Arm Angle (deg)')

linkaxes([ax ax1], 'x')
dynamicDateTicks([ax ax1],'link')
set([ax],'XTickLabel',[])


%% Ping request

time = c.DAT.queryAddressRequested.time;
time_elapsed = (time - time(1)) * (3600*24);

t = time(2:end);
ping_req_dt = diff(time_elapsed);

%% Time between pings

% close all
time = c.DAT.acoustic_contact_range.time;

dt = [0, diff(time - time(1)) * (3600 * 24)];
% dt(dt>100)=100;

queryi = interp1( c.DAT.acoustic_contact_range.time, c.DAT.acoustic_contact_range.value, c.DAT.queryAddressRequested.time );
req_i_behav = interp1(time, dt, c.DAT.queryAddressRequested.time);
req_i_dat = interp1(time, dt, c.DAT.acoustic_transmit_time.time);

f = figure; % gcf;
% f =  gcf;
% f.Position = [0 0 1 1];
f.Position = [0    0.0500    0.3333    0.8900];
corder = colorOrder;

ax = gca;
ax.Position = [0.05, 0.75, 0.9, 0.2];
plot(c.DAT.acoustic_contact_range.time, c.DAT.acoustic_contact_range.value); hold on;
scatter(c.DAT.acoustic_contact_range.time, c.DAT.acoustic_contact_range.value, 'filled'); hold on;
% scatter(c.Tracking.range_to_contact.time, c.Tracking.range_to_contact.value,'filled');
scatter( c.DAT.queryAddressRequested.time, queryi, 'filled' );
ylabel(ax, 'range (m)')
% legend('DAT','Tracking', 'REQ')
legend(ax, 'DAT','DAT', 'REQ')

ax1 = axes;
ax1.Position = [0.05, 0.525, 0.9, 0.2];
% plot(time, c.DUSBL_Hydroid.acoustic_contact_range.value); hold on
% scatter(time, c.DUSBL_Hydroid.acoustic_contact_range.value, 'filled');
% ylabel(ax,'contact range (m)')
plot(t, ping_req_dt); hold on
scatter(t, ping_req_dt, 'filled');
plot(t, ones(size(ping_req_dt))*mean(ping_req_dt));
ylabel(ax1,'DAT ping req. \Deltat (sec)')
lg = legend(ax1, '','DAT transponderCode', sprintf('Avg. %0.1f sec', mean(ping_req_dt)));
lg.FontSize = 20;

ax2 = axes;
ax2.Position = [0.05, 0.3, 0.9, 0.2];
plot(time, dt); hold on
scatter(time, dt, 80, 'filled');
scatter(c.DAT.queryAddressRequested.time, req_i_behav, 'filled')
scatter(c.DAT.acoustic_transmit_time.time, req_i_dat, 'filled')
ylabel(ax2,'range update \Deltat (sec)')
lg = legend(ax2, '','Range Hit', 'TrackAC Ping Request','DAT Ping Out');
lg.FontSize = 20;

linkaxes([ax ax1 ax2], 'x')
dynamicDateTicks([ax ax1 ax2],'link')
set([ax, ax1],'XTickLabel',[])
set([ax,ax1,ax2],'FontSize',20)

pingSucess = length(c.DAT.acoustic_contact_range.time)/length(c.DAT.acoustic_transmit_time.time)*100;

dt_req = [0, diff(c.DAT.queryAddressRequested.time - c.DAT.queryAddressRequested.time(1)) * (3600 * 24)];


title(ax,sprintf('%s - TrackingUpdatePeriod: %0.1f sec', logname, TrackingUpdatePeriod))


% export_fig(gcf, sprintf('~/Desktop/lineCapture_%s_RangeDeltaT.png', logname),'-r300')

% figure;
% histogram(dt_req, 500);
% xlabel('contact range \Deltat (sec)')
% ylabel('count')
% export_fig(gcf, '~/Desktop/lineCaptureHoming_RangeDeltaTHist.png','-r300')

%% Cmd heading: Azimuth vs Rotated

% get azimuth
t = c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.time;
az = rad2deg(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value);
% get rotated heading
cmd_hd_rot = rad2deg(c.TrackAcousticContact.heading_to_contact.value);

% get vehicle heading (interp to azimuth time grid)
hd = rad2deg(interp1(c.platform_orientation.time, c.platform_orientation.value, t));

% compute commanded heading directly from azimuth
cmd_hd_az = mod(hd + az, 360);

delta = cmd_hd_rot - cmd_hd_az;

figure;
plot(t, delta);hold on
scatter(t, delta, 7, 'filled');
title('cmd heading rot - cmd heading azimuth')
ylabel('cmd heading delta (deg)')
dynamicDateTicks

figure;
histogram(delta);
xlabel('cmd heading delta (deg)')
ylabel('freq (count)')

% export_fig(gcf, '~/Desktop/lineCaptureHoming_cmd_heading_az.png','-r300')

%% Course over ground
close all

[o.time, oi] = unique(c.platform_orientation.time, 'sorted');
o.value = c.platform_orientation.value(oi);

t = c.platform_x_velocity_wrt_ground.time;
h = rad2deg(interp1(o.time, o.value, t));
x = c.platform_x_velocity_wrt_ground.value;
y = c.platform_y_velocity_wrt_ground.value;

propPitch = 0.0318309;
xcmd = interp1(c.SpeedControl.propOmegaAction.time, c.SpeedControl.propOmegaAction.value, t);
xcmd = xcmd .* propPitch;

figure;
plot(t, x);hold on;
dynamicDateTicks

% calc the crab angle
crab = atan2(y, x);
mag = sqrt(y.^2 + x.^2);


figure
plot(t, mag)
dynamicDateTicks

% filter crab angle
crabf = crab;

n=10;
for k=(n+1):length(crab)
    crabf(k) = mean(crab(k-n:k));
end

crabf(~(abs(xcmd)>0) | mag < 0.1 | crabf > deg2rad(10)) = NaN;

wfig;
plot(t, rad2deg(crab), 'LineWidth', 0.5); hold on;
plot(t, rad2deg(crabf));
plot(t, zeros(size(crab)), 'k--', 'LineWidth', 0.5);
dynamicDateTicks


% add to heading to get COG
cog = mod(h + rad2deg(crabf), 360);


figure;
plot(y, t);hold on;
plot(rad2deg(crab), t);
plot(zeros(size(y)), t, 'k--');
datetick('y')

figure;
plot(h, t);hold on;
plot(cog, t);
plot(rad2deg(crab), t);
plot(zeros(size(y)), t, 'k--');
datetick('y')

wfig;
plot(t, h); hold on;
plot(t, cog);
legend('Heading', 'COG', 'FontSize', 26 )
dynamicDateTicks

%%

% et = (c.TrackAcousticContact.contact_longitude.time - c.TrackAcousticContact.contact_longitude.time(1)).*24*60;
guidanceMode =  interp1(c.LineCapture.guidanceMode.time, double(c.LineCapture.guidanceMode.value), c.TrackAcousticContact.range_to_contact.time, 'previous');
id_ = c.TrackAcousticContact.range_to_contact.value < 40 & (guidanceMode ~= 0 & guidanceMode ~= 4);

c_lon = double(c.TrackAcousticContact.contact_longitude.value(id_));
c_lat = double(c.TrackAcousticContact.contact_latitude.value(id_));
c_range = c.TrackAcousticContact.range_to_contact.value(id_);

ca
figure;
ax = gca; hold on;
s(1)  = plot(ax, lon, lat);
s(2) = scatter(ax, c_lon, c_lat, 24, -c_range, 'filled');
s(3) = scatter(ax, c.Dock.lon.value(end), c.Dock.lat.value(end), 76, 'filled');
cb=colorbar;
% cb.TickLabels = datestr(cb.Ticks, 'HH:MM:SS');


ax.DataAspectRatio = [111.32 cosd(mean(c.TrackAcousticContact.contact_latitude.value))*111.32 1];


%%

contact_time =  c.TrackAcousticContact.range_to_contact.time;
contact_lon = double(c.TrackAcousticContact.contact_longitude.value);
contact_lat = double(c.TrackAcousticContact.contact_latitude.value);
contact_range = double(c.TrackAcousticContact.range_to_contact.value);


win = 5;
contact_lon_mean = nan(size(contact_lon));
contact_lat_mean = nan(size(contact_lat));
contact_lon_median = nan(size(contact_lon));
contact_lat_median = nan(size(contact_lon));

for k=(win+1):length(contact_lon)

    clon = contact_lon(k+1-win:k);
    clat = contact_lat(k+1-win:k);

    contact_lon_mean(k) = mean(clon);
    contact_lat_mean(k) = mean(clat);
    contact_lon_median(k) = median(clon);
    contact_lat_median(k) = median(clat);
end

ca
figure;
ax = gca; hold on;
s(1)  = plot(ax, lon, lat);
s(2) = scatter(ax, contact_lon_mean,contact_lat_mean,24, 'filled');
s(3) = scatter(ax, contact_lon,contact_lat,24, 'filled');

% s(2) = scatter(ax, contact_lon_median,contact_lat_median,...
%     24, c.TrackAcousticContact.contact_latitude_lowpass.time, 'filled');


c.lineCaptureHoming.LineCapture.DockLat.value(end)

% s(4) = scatter(ax, c.Dock.lon.value(end), c.Dock.lat.value(end), 76, '^k','filled');
% cb=colorbar;
% cb.TickLabels = datestr(cb.Ticks, 'HH:MM:SS');

% ax.DataAspectRatio = [111.32 cosd(mean(lat))*111.32 1];



wfig;
plot(contact_time, contact_lat, '-o'); hold on;
plot(contact_time, contact_lat_mean, '-o');
plot(contact_time, contact_lat_median, '-o');
dynamicDateTicks;

%% Ping request

time = c.DAT.transponderAddress.time;
time_elapsed = (time - time(1)) * (3600*24);

t = time(2:end);
ping_req_dt = diff(time_elapsed);

gaps = ping_req_dt>15;
ping_req_dt(gaps) = [];
t(gaps) = [];

%% Time between pings
close all

dt = [0, diff(c.acoustic_contact_range.time - c.acoustic_contact_range.time(1)) * (3600 * 24)];
dt(dt>100)=100;

wfig;
ax = gca;
ax.Position = [0.05, 0.5, 0.9, 0.4];
plot(c.acoustic_contact_range.time, c.acoustic_contact_range.value); hold on
scatter(c.acoustic_contact_range.time, c.acoustic_contact_range.value, 'filled');
ylabel(ax,'contact range (m)')

ax1 = axes;
ax1.Position = [0.05, 0.05, 0.9, 0.4];
plot(c.acoustic_contact_range.time, dt); hold on
scatter(c.acoustic_contact_range.time, dt, 'filled');
ylabel(ax1,'contact range \Deltat (sec)')

linkaxes([ax ax1], 'x')
dynamicDateTicks([ax ax1],'link')
set([ax],'XTickLabel',[])
set([ax,ax1],'FontSize',20)

% export_fig(gcf, '~/Desktop/lineCaptureHoming_RangeDeltaT.png','-r300')

figure;
histogram(dt, 250);
xlabel('contact range \Deltat (sec)')
ylabel('count')
% export_fig(gcf, '~/Desktop/lineCaptureHoming_RangeDeltaTHist.png','-r300')

%% Range rate

time = c.TrackAcousticContact.range_to_contact.time;
time_el = datenum2elapsed_sec(time);
delta_t = [0, diff(time_el)];

slant = c.TrackAcousticContact.range_to_contact.value;
range_rate = [0, diff(slant)] ./ delta_t;

brng  = rad2deg(c.TrackAcousticContact.heading_to_contact.value);
brng_rate = [0, diff(brng)] ./ delta_t;

win = 4;
range_rate_smooth = range_rate;
brng_rate_smooth = brng_rate;
for k=(win+1):(length(range_rate_smooth))
    range_rate_smooth(k) = mean(range_rate(k+1-win:k));
    brng_rate_smooth(k) = mean(brng_rate(k+1-win:k));
end


dx = c.platform_x_velocity_wrt_ground;

[dx.time, ui] = unique(dx.time);
dx.value = dx.value(ui);
dx.val = interp1(dx.time, dx.value, time);

wfig;
plot(dx.time, dx.value); hold on
scatter(time, dx.val)
dynamicDateTicks


range = slant;
%{
% Convert slant range to horizontal range using simple pythagorean
beacon_depth = c.Dock.depth.value(end); % m

dz = interp1(c.depth.time, c.depth.value, time) - beacon_depth;
dz = min([slant;dz]);

range = sqrt(slant.^2 - dz.^2);
range_vel = [0, diff(range)] ./ delta_t;

figure
scatter(slant, slant-range, 'filled')
xlabel('slant range')
ylabel('\Deltaslant/rnage')


wfig;
plot(time, slant); hold on;
plot(time, range);
scatter(time(dz==slant), range(dz==slant));
legend('slant', 'horizontal', 'slant > \Deltadepth')
dynamicDateTicks

%}

min_range_vel = 0.2;
arm_range = 40.0;
lockout_range = 2.0;

ca

wfig;
ax = gca;
ax.Position = [0.05, 0.5, 0.9, 0.4];
hold on;
plot( time, range );
scatter(time, range);
plot([time(1) time(end)], [arm_range arm_range], ':');
plot([time(1) time(end)], [lockout_range lockout_range], ':');
title(logname)
legend('','range','arm', 'lockout')
ylabel('Range (m)')

ax1 = axes;
ax1.Position = [0.05, 0.05, 0.9, 0.4];
plot(c.platform_x_velocity_wrt_ground.time, c.platform_x_velocity_wrt_ground.value); hold on
plot(time, range_rate, '-o');
plot(time, range_rate_smooth, '-o');
% scatter(time, range_vel);
plot([time(1) time(end)], [0 0], 'k--');
ylabel('vel (m/s)')
plot([time(1) time(end)], [min_range_vel min_range_vel], 'k:');
plot([time(1) time(end)], [-min_range_vel -min_range_vel], 'k:');
legend('x vel wrt ground', 'range rate', 'range rate smooth')

linkaxes([ax ax1], 'x')
dynamicDateTicks([ax ax1],'link')
set([ax],'XTickLabel',[])
set([ax,ax1],'FontSize',20)

% export_fig(gcf, '~/Desktop/lineCaptureHoming_RangeRate.png','-r300')

wfig;
ax = gca;
ax.Position = [0.05, 0.5, 0.9, 0.4];
hold on;
plot( time, range );
scatter(time, range);
plot([time(1) time(end)], [arm_range arm_range], ':');
plot([time(1) time(end)], [lockout_range lockout_range], ':');
title(logname)
legend('','range','arm', 'lockout')
ylabel('Range (m)')

ax1 = axes; % delete(ax1)
ax1.Position = [0.05, 0.05, 0.9, 0.4];
% plot(c.platform_o.time, c.platform_x_velocity_wrt_ground.value); hold on
plot(time, brng_rate, '-o'); hold on
plot(time, brng_rate_smooth, '-o');
% scatter(time, range_vel);
plot([time(1) time(end)], [0 0], 'k--');
ylabel('bearing rate (deg/s)')
% plot([time(1) time(end)], [min_range_vel min_range_vel], 'k:');
% plot([time(1) time(end)], [-min_range_vel -min_range_vel], 'k:');
% legend('x vel wrt ground', 'range rate', 'range rate smooth')

linkaxes([ax ax1], 'x')
dynamicDateTicks([ax ax1],'link')
set([ax],'XTickLabel',[])
set([ax,ax1],'FontSize',20)


% id = time >= 738479.031334249 & time <= 738479.035736142
% figure;boxplot(range_rate(id));boxplotlegend


%% Noise in range
win = 4;
half_win = floor(win/2);
range_std = nan(size(range));
range_mean = nan(size(range));
brng_std = nan(size(brng));
brng_mean = nan(size(brng));
for k=(1+half_win):(length(range_std)-half_win)

    r_ = detrend(range(k-half_win:k+half_win));
    range_mean(k) = mean(r_);
    range_std(k) = std(r_);

    b_ = detrend(brng(k-half_win:k+half_win));
    brng_mean(k) = mean(b_);
    brng_std(k) = std(b_);

end

ca


ca

wfig;
ax = gca;
ax.Position = [0.05, 0.5, 0.9, 0.4];
hold on;
plot( time, range );
scatter(time, range);
plot([time(1) time(end)], [arm_range arm_range], ':');
plot([time(1) time(end)], [lockout_range lockout_range], ':');
title(logname)
legend('','range','arm', 'lockout')
ylabel('Range (m)')

ax1 = axes;
ax1.Position = [0.05, 0.05, 0.9, 0.4];
plot(time, range_std); hold on;
scatter(time, range_std);
ylabel('Range std (m)')

linkaxes([ax ax1], 'x')
dynamicDateTicks([ax ax1],'link')
set([ax],'XTickLabel',[])
set([ax,ax1],'FontSize',20)

% export_fig(gcf, '~/Desktop/lineCaptureHoming_RangeStd.png','-r300')

wfig;
ax = gca;
ax.Position = [0.05, 0.5, 0.9, 0.4];
hold on;
plot( time, range );
scatter(time, range);
plot([time(1) time(end)], [arm_range arm_range], ':');
plot([time(1) time(end)], [lockout_range lockout_range], ':');
title(logname)
legend('','range','arm', 'lockout')
ylabel('Range (m)')

ax1 = axes;
ax1.Position = [0.05, 0.05, 0.9, 0.4];
plot(time, brng_std); hold on;
scatter(time, brng_std);
ylabel('Bearing std (deg)')

linkaxes([ax ax1], 'x')
dynamicDateTicks([ax ax1],'link')
set([ax],'XTickLabel',[])
set([ax,ax1],'FontSize',20)

% export_fig(gcf, '~/Desktop/lineCaptureHoming_BearingStd.png','-r300')


%% Current
ca

[t_, ti_] = unique(c.platform_orientation.time);
head = interp1(t_, c.platform_orientation.value(ti_), c.RDI_Pathfinder.platform_x_velocity_wrt_ground.time);
speed = interp1(c.SpeedCalculator.platform_x_velocity_wrt_sea_water.time, c.SpeedCalculator.platform_x_velocity_wrt_sea_water.value, c.RDI_Pathfinder.platform_x_velocity_wrt_ground.time);

t = c.RDI_Pathfinder.platform_x_velocity_wrt_ground.time;
% t = mean(reshape(t, 20, length(t)/20));
x = c.RDI_Pathfinder.platform_x_velocity_wrt_ground.value;
% x = c.RDI_Pathfinder.platform_x_velocity_wrt_ground.value;
% x = mean(reshape(x, 20, length(x)/20));
y = c.RDI_Pathfinder.platform_y_velocity_wrt_ground.value;
% y = mean(reshape(y, 20, length(y)/20));
% head = mean(reshape(head, 20, length(head)/20));

x(abs(x)>5) = NaN;
y(abs(y)>5) = NaN;


wfig;
ax = gca;
ax.Position = [0.05, 0.55, 0.9, 0.4];
plot(c.acoustic_contact_range.time, c.acoustic_contact_range.value); hold on;
scatter(c.TrackAcousticContact.range_to_contact.time, c.TrackAcousticContact.range_to_contact.value,'filled')
scatter(ax, L(:,1), start_range, 'g', 'filled')
scatter(ax, L(:,2), finish_suc_range, 'm', 'filled')
scatter(ax, L(:,3), finish_fail_range, 'r', 'filled')
ylabel('range (m)')
legend('universal','TrackAcousticContact', 'Homing start', 'Dock', 'Missed')

ax1 = axes;
ax1.Position = [0.05, 0.1, 0.9, 0.4];
p(1) = plot(t, speed, '--');plot(t, zeros(size(t)), 'k:'); hold on;

p(2) = plot(t, x);
p(3) = plot(t, y);

ylabel('Velocity wrt ground (m/s)')
legend(p,'cmd X','X','Y')

linkaxes([ax ax1], 'x')
dynamicDateTicks([ax ax1],'link')
set([ax],'XTickLabel',[])
set([ax,ax1],'FontSize',20)

theta = atand(y./x);

mag = sqrt(x.^2 + y.^2);


offset = round(2.5*120);
T.current_mag_avg = NaN(size(T.start_time));
T.current_mag_std = NaN(size(T.start_time));
T.current_azimuth_avg = NaN(size(T.start_time));
T.current_azimuth_std = NaN(size(T.start_time));

for k=1:length(T.start_time)

    si = closest(T.start_time(k), t) + offset;
    if( isnan(T.dock_time(k)) )
        ei = closest(T.miss_time(k), t);
    else
        ei = closest(T.dock_time(k), t) - offset;
    end


    T.cross_current_mag_avg(k) = mean(y(si:ei), 'omitnan');
    T.cross_current_mag_std(k) = std(x(si:ei), 'omitnan');

    T.current_mag_avg(k) = mean(mag(si:ei), 'omitnan');
    T.current_mag_std(k) = std(mag(si:ei), 'omitnan');
    T.current_azimuth_avg(k) = mean(theta(si:ei), 'omitnan');
    T.current_azimuth_std(k) = std(theta(si:ei), 'omitnan');

end

run = 1:length(T.start_time);

figure
scatter( T.current_mag_avg(~isnan(T.dock_time)), T.current_azimuth_avg(~isnan(T.dock_time)),75, 'filled'); hold on
scatter(T.current_mag_avg(isnan(T.dock_time)), T.current_azimuth_avg(isnan(T.dock_time)),'filled');
xlabel('Current magnitude (m/s)')
ylabel('Current azimuth (deg)')
legend('Dock','Miss')



figure;
errorbar(run(~isnan(T.dock_time)), T.cross_current_mag_avg(~isnan(T.dock_time)), T.cross_current_mag_std(~isnan(T.dock_time)), 'o');hold on;
s1 = scatter(run(~isnan(T.dock_time)), T.cross_current_mag_avg(~isnan(T.dock_time)), 75, 'filled');
errorbar(run(isnan(T.dock_time)), T.cross_current_mag_avg(isnan(T.dock_time)), T.cross_current_mag_std(isnan(T.dock_time)), 'o');hold on;
s2 = scatter(run(isnan(T.dock_time)), T.cross_current_mag_avg(isnan(T.dock_time)), 75,'filled');
plot([0 length(T.start_time)], [0 0], 'k:')
xlabel('Run #')
ylabel('Platform y velocity wrt ground \pm std (m/s)')
lg = legend([s1 s2], 'Dock','Miss');
lg.FontSize = 22;

curr_head = theta + head;


figure
% feather(x,y)
plot(t, curr_head)
dynamicDateTicks

%% Random plots

wfig;
plot(c.depth.time, c.depth.value); hold on
plot(c.VerticalControl.depthCmd.time, c.VerticalControl.depthCmd.value);
scatter(c.VerticalControl.depthCmd.time, c.VerticalControl.depthCmd.value);
% scatter(c.DefaultUnder.DefaultMode.time, c.DefaultUnder.DefaultMode.value)
wfig('z');
dynamicDateTicks

wfig;
plot(c.SpeedControl.speedCmd.time, c.SpeedControl.speedCmd.value); hold on
ylabel('SpeedControl.speedCmd')
dynamicDateTicks

wfig;
plot(c.platform_propeller_rotation_rate.time, c.platform_propeller_rotation_rate.value); hold on
scatter(c.SpeedControl.propOmegaAction.time, c.SpeedControl.propOmegaAction.value)
legend('platform_propeller_rotation_rate','propOmegaAction', 'Interpreter', 'none')
ylabel('SpeedControl.propOmegaAction')
dynamicDateTicks

wfig;
plot(c.platform_buoyancy_position.time, c.platform_buoyancy_position.value); hold on
plot(c.VerticalControl.buoyancyAction.time, c.VerticalControl.buoyancyAction.value);
scatter(c.VerticalControl.buoyancyAction.time, c.VerticalControl.buoyancyAction.value);
ylabel('platform buoyancy position')
dynamicDateTicks

wfig;
plot(c.platform_x_velocity_wrt_ground.time, c.platform_x_velocity_wrt_ground.value); hold on
scatter(c.platform_x_velocity_wrt_ground.time, c.platform_x_velocity_wrt_ground.value);
ylabel('platform x velocity')
dynamicDateTicks

wfig
plot(c.HorizontalControl.headingCmd.time, c.HorizontalControl.headingCmd.value); hold on
scatter(c.HorizontalControl.headingCmd.time, c.HorizontalControl.headingCmd.value)
dynamicDateTicks

wfig
plot(c.HorizontalControl.kpHeadingOverride.time, c.HorizontalControl.kpHeadingOverride.value); hold on
scatter(c.HorizontalControl.kpHeadingOverride.time, c.HorizontalControl.kpHeadingOverride.value)
dynamicDateTicks

wfig
plot(c.HorizontalControl.kiHeadingOverride.time, c.HorizontalControl.kiHeadingOverride.value); hold on
scatter(c.HorizontalControl.kiHeadingOverride.time, c.HorizontalControl.kiHeadingOverride.value)
dynamicDateTicks

wfig
plot(c.SpeedControl.speedCmd.time, c.SpeedControl.speedCmd.value); hold on
scatter(c.SpeedControl.speedCmd.time, c.SpeedControl.speedCmd.value)
dynamicDateTicks


wfig
plot(c.BuoyancyServo.platform_buoyancy_position.time, c.BuoyancyServo.platform_buoyancy_position.value); hold on
scatter(c.BuoyancyServo.platform_buoyancy_position.time, c.BuoyancyServo.platform_buoyancy_position.value)
dynamicDateTicks

%%

wfig
plot(c.PowerOnly.samplePowerOnly.time, c.PowerOnly.samplePowerOnly.value); hold on
scatter(c.PowerOnly.samplePowerOnly.time, c.PowerOnly.samplePowerOnly.value, 5, 'filled');
ylabel('PowerOnly.samplePowerOnly')
dynamicDateTicks

wfig
plot(c.LineCapture.guidanceMode.time, c.LineCapture.guidanceMode.value); hold on
scatter(c.LineCapture.guidanceMode.time, c.LineCapture.guidanceMode.value, 'filled');
legend('guidanceMode', ['0: UNINITIALIZED' 10 '1: TERMINAL GUIDANCE' 10 '2: FINAL APPROACH' 10 '3: INTERCEPT LOCKOUT' 10 '4: ROLLOUT']);
ylabel('LineCapture.guidanceMode');
set(gca, 'YTick', 0:4);
ylim([-1 5])
dynamicDateTicks


%% Extra

%{

figure;
hist3([acoustic_contact_range.value', dt'], 'nbins', [100 100], 'CDataMode','auto','FaceColor','interp', 'EdgeColor',[105,105,105]/255)
% histogram2(acoustic_contact_range.value, dt, [100 100])

figure;
hist3([azimuth_to_contact_vehicle_frame.value', dt'], 'nbins', [100 100], 'CDataMode','auto','FaceColor','interp', 'EdgeColor',[105,105,105]/255)
% histogram2(azimuth_to_contact_vehicle_frame.value, dt, [100 100],'FaceColor','flat')
% view(2)


figure;
scatter3(acoustic_contact_range.value', azimuth_to_contact_vehicle_frame.value', dt')

figure;
scatter3(acoustic_contact_range.time', acoustic_contact_range.value', azimuth_to_contact_vehicle_frame.value')


figure;
scatter(acoustic_contact_range.time', azimuth_to_contact_vehicle_frame.value');
dynamicDateTicks


%%

homing = zeros(size(depth.time));
for k=1:length(line_cap.HomeingActive.time)

    homing(depth.time >= line_cap.HomeingActive.time(k)) = (line_cap.HomeingActive.value(k)==1);

end

% homing = interp1(line_cap.HomeingActive.time, single(line_cap.HomeingActive.value), acoustic_contact_range.time, 'previous');

figure
plot(depth.time, homing);hold on;
scatter(line_cap.HomeingActive.time, line_cap.HomeingActive.value);
% scatter(acoustic_contact_range.time, homing);
dynamicDateTicks

%%

wfig;
ax = gca;
ax.Position = [0.05, 0.65, 0.9, 0.25];
scatter(acoustic_contact_range.time, acoustic_contact_range.value);

ax1 = axes;
ax1.Position = [0.05, 0.35, 0.9, 0.25];
scatter(acoustic_contact_range.time, azimuth_to_contact_vehicle_frame.value);

ax2 = axes;
ax2.Position = [0.05, 0.05, 0.9, 0.25];
scatter(heading.time, heading.value);

linkaxes([ax ax1 ax2], 'x')
dynamicDateTicks([ax ax1 ax2],'link')

%%


wfig;
plot(c.depth.time, c.depth.value); hold on;
ylabel('Depth (m)')
reverseYDir
dynamicDateTicks

wfig;
plot(c.platform_pitch_angle.time, rad2deg(c.platform_pitch_angle.value))
ylabel('pitch angle (deg)')
dynamicDateTicks

wfig;
plot(c.platform_orientation.time, rad2deg(c.platform_orientation.value))
ylabel('heading (deg)')
dynamicDateTicks

wfig;
plot(c.platform_elevator_angle.time, rad2deg(c.platform_elevator_angle.value)); hold on
scatter(c.platform_elevator_angle.time, rad2deg(c.platform_elevator_angle.value))
ylabel('ele angle (deg)')
dynamicDateTicks

wfig;
plot(c.VerticalControl.verticalMode.time, c.VerticalControl.verticalMode.value)
ylabel('verticalMode (enum)')
dynamicDateTicks

wfig;
plot(c.platform_mass_position.time, c.platform_mass_position.value); hold on
scatter(c.platform_mass_position.time, c.platform_mass_position.value)
plot(c.VerticalControl.massPositionAction.time, c.VerticalControl.massPositionAction.value,'k--');
ylabel('mass position (m)')
dynamicDateTicks

wfig;
plot(c.platform_propeller_rotation_rate.time, c.platform_propeller_rotation_rate.value)
ylabel('propeller rotation rate (rad/s)')
dynamicDateTicks

wfig;
plot(c.platform_buoyancy_position.time, c.platform_buoyancy_position.value)
ylabel('buoyancy position (m)')
dynamicDateTicks

%}