% lineCaptureDocking.m
%clear;ca

% SB
%c = lrauv_load('/Users/Ben/tmp/201906110612_201906111454.mat');

% k=9
% matpath{k}
% c = lrauv_load(matpath{k});

c.lineCaptureHoming

hold_alt = c.lineCaptureHoming.HoldAltitude.value(end);
min_alt = c.lineCaptureHoming.MinAltitude.value(end);

%% Vertical control

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);
ylabel('Depth (m)')
legend('Depth','Depth cmd')
%reverseYDir
set(gca,'ydir','reverse')

ax1 = axes;
set(ax1,'Unit','normalized','Position',[0.05 0.375 0.9 0.275])
plot(c.RDI_Pathfinder.height_above_sea_floor.time,c.RDI_Pathfinder.height_above_sea_floor.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('Altitude','Min Altitude','Hold Altitude')

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))
ylabel('heading (deg)')

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
% ca

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

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

ax = gca;
ax.Position = [0.05, 0.75, 0.9, 0.2];
plot(c.RDI_Pathfinder.height_above_sea_floor.time,c.RDI_Pathfinder.height_above_sea_floor.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('altitude (m)')
legend('Altitude','Min Altitude','Hold Altitude')

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')
ylabel('range (m)')
legend('universal','TrackAcousticContact')

ax2 = axes; % delete(ax2)
ax2.Position = [0.05, 0.3, 0.9, 0.2];
plot(c.DUSBL_Hydroid.xAngle.time, c.DUSBL_Hydroid.xAngle.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(ax2,'xAngle (+ 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')
plot(c.HorizontalControl.headingCmd.time, rad2deg(c.HorizontalControl.headingCmd.value));
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, '~/Desktop/lineCaptureHoming.png','-r300')

%% Horizontal Guidance
% ca

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


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,'filled')
ylabel('range (m)')
legend('universal','TrackAcousticContact')

ax1 = axes; % delete(ax1)
ax1.Position = [0.05, 0.525, 0.9, 0.2];
plot(c.DUSBL_Hydroid.xAngle.time, c.DUSBL_Hydroid.xAngle.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')

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')
plot(c.HorizontalControl.headingCmd.time, rad2deg(c.HorizontalControl.headingCmd.value));
ylim(ax2, [-10 370])
ylabel('heading (deg)')
legend('heading','TrackAcousticContact', '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.HorizontalControl.rudderAngleAction.time, rad2deg(c.HorizontalControl.rudderAngleAction.value),'filled')
ylabel('rudder angle (deg)')
legend('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')

%% 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')

%% Ping request

el = (c.DUSBL_Hydroid.transponderCode.time - c.DUSBL_Hydroid.transponderCode.time(1)) * (3600*24);

ping_req_dt = diff(el);
ping_req_dt(ping_req_dt>15) = [];

figure;
plot(ping_req_dt); hold on
plot(1:length(ping_req_dt), ones(size(ping_req_dt))*mean(ping_req_dt));
ylabel('ping req dt (sec)')
set(gca,'FontSize',24)

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


wfig;
plot(el, c.DUSBL_Hydroid.transponderCode.value); hold on
scatter(el, c.DUSBL_Hydroid.transponderCode.value)

%% Time between pings
%clear 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, 500);
xlabel('contact range \Deltat (sec)')
ylabel('count')
% export_fig(gcf, '~/Desktop/lineCaptureHoming_RangeDeltaTHist.png','-r300')

return


%% Assemble data for full deployment
%   1. min ranges?
%   2. ping dt?

depth = struct();
depth.value = [];
depth.time = [];

line_cap = struct();
line_cap.HomeingActive.value = [];
line_cap.HomeingActive.time = [];

acoustic_contact_range = struct();
acoustic_contact_range.value = [];
acoustic_contact_range.time = [];

azimuth_to_contact_vehicle_frame = struct();
azimuth_to_contact_vehicle_frame.value = [];
azimuth_to_contact_vehicle_frame.time = [];

heading = struct();
heading.value = [];
heading.time = [];

for k=1:length(matpath)
    try
        c = lrauv_load(matpath{k}, 'depth', 'acoustic_contact_range', 'lineCaptureHoming', 'TrackAcousticContact', 'platform_orientation');

        depth.value = [depth.value, c.depth.value];
        depth.time = [depth.time, c.depth.time];

        acoustic_contact_range.value = [acoustic_contact_range.value, c.acoustic_contact_range.value];
        acoustic_contact_range.time = [acoustic_contact_range.time, c.acoustic_contact_range.time];

        line_cap.HomeingActive.value = [line_cap.HomeingActive.value, c.lineCaptureHoming.HomingActive.value];
        line_cap.HomeingActive.time = [line_cap.HomeingActive.time, c.lineCaptureHoming.HomingActive.time];

        azimuth_to_contact_vehicle_frame.value = [azimuth_to_contact_vehicle_frame.value, c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value];
        azimuth_to_contact_vehicle_frame.time = [azimuth_to_contact_vehicle_frame.time, c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.time];

        heading.value = [heading.value, c.platform_orientation.value];
        heading.time = [heading.time, c.platform_orientation.time];

    catch
        disp(['no ac data in: ' matpath{k}])
    end
end

azimuth_to_contact_vehicle_frame.value = rad2deg(azimuth_to_contact_vehicle_frame.value);
heading.value = rad2deg(heading.value);

srange = sort(acoustic_contact_range.value);

%% Time between pings
ca

dt = [0, diff(acoustic_contact_range.time - 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(acoustic_contact_range.time, acoustic_contact_range.value); hold on
scatter(acoustic_contact_range.time, acoustic_contact_range.value, 'filled');
ylabel(ax,'contact range (m)')

ax1 = axes;
ax1.Position = [0.05, 0.05, 0.9, 0.4];
plot(acoustic_contact_range.time, dt); hold on
scatter(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')

%% 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

%}
