%% Pro-Nav analysis
setAxesDefaults
if exist( 'position' ) ~= 1
  MonitorPositions = get(0,'MonitorPositions');
  position = MonitorPositions(end,:);
  npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
  npposition(3:4) = round([position(3)*.95 position(4)*.90]);
  position = npposition;
end

co = [0 0 1;
      0 0.5 0;
      1 0 0;
      0 0.75 0.75;
      0.75 0 0.75;
      0.75 0.75 0;
      0.25 0.25 0.25];
set(groot,'defaultAxesColorOrder',co)

whichRun = '1736';
whichRun = '2025';
whichRun = '2132';
whichRun = 'sim_anderson';
whichRun = 'select';
whichRun = 'sim';

if( strcmp( whichRun,'2132' ) )
  filename = ...
      '/Volumes/LRAUV/tethys/missionlogs/2022/20220712_20220715/20220714T200341/202207142003_202207142132.mat';
  logname = '202207142003_202207142132';
  syslog_path = '/Volumes/LRAUV/tethys/realtime/sbdlogs/2022/202207/20220714T200341/shore.log';
elseif( strcmp( whichRun,'1736' ) )
  filename = ...
      '/Volumes/LRAUV/tethys/missionlogs/2022/20220712_20220715/20220714T162217/202207141622_202207141736.mat';
  logname = '202207141622_202207141736';
  syslog_path = '/Volumes/LRAUV/tethys/realtime/sbdlogs/2022/202207/20220714T162217/shore.log';

elseif( strcmp( whichRun,'2025' ) )
  filename = ...
      '/Volumes/LRAUV/tethys/missionlogs/2022/20220712_20220715/20220714T213240/202207142132_202207152025.mat';
  logname = '202207142132_202207152025';
  syslog_path = '/Volumes/LRAUV/tethys/realtime/sbdlogs/2022/202207/20220714T213240/shore.log';

elseif( strcmp( whichRun,'sim' ) )
  %
  % This code assumes that Matlab is in the parent directory, 
  % and that the directory name is on the stack as a string "dirName".
  %
  if(~exist('dirName')) 
    fprintf('Please set dirName to the desired log directory.\n')
    return
  end
  tempName=dir(strcat(dirName, '/*.mat'));
  ss = size(tempName);
  if(ss(1) == 0 )
    fprintf('File %s not found.\n', dirName );
    return;
  elseif( ss(1) == 1 )
    if( isstruct(tempName) )
      tempName = tempName.name;
    else
      tempName = tempName(1:end-1);
      tempName = tempName(strfind(tempName,'/')+1:end);
    end
  elseif(ss(1) >= 2 )
    tempName = tempName(1).name;
  else
    fprintf('dirName parsing error.\n')
    return
  end
  logname  = tempName(1:end-4);   % drop off the .mat
  filename = sprintf('%s/%s/%s',pwd,dirName,tempName);
  syslog_path = sprintf('%s/%s/syslog',pwd,dirName);

elseif( strcmp( whichRun, 'sim_anderson' ) )
  filename = ...
      '/home/anderson/lrauv/lrauv-application/latest.mat';
  logname = 'sim';
  syslog_path = '/home/anderson/lrauv/lrauv-application/latest_syslog';
  dirName = '';

elseif( strcmp( whichRun, 'select') )
  filename = ...
      '/home/anderson/lrauv/logs/select.mat';
  fid = fopen('/home/anderson/lrauv/logs/logname.txt');
  lognamecell = textscan(fid, '%s');
  logname = string(lognamecell{1});
  syslog_path = '/home/anderson/lrauv/logs/select_syslog';
  dirName = '';
end
  
%filename = ['~/lr/lrauv-docking-analysis/' logname  '.mat'];
%syslog_path = ['~/lr/lrauv-docking-analysis/' logname '.log'];

c = lrauv_load(filename);

% Kill the outliers:
if( find(c.acoustic_contact_range.value > 1e5 ))
  fprintf('Removing outliers.\n');
end
c.acoustic_contact_range.value(find(c.acoustic_contact_range.value > 1e5)) = NaN;
c.TrackAcousticContact.range_to_contact.value(find(c.TrackAcousticContact.range_to_contact.value > 1e5)) = NaN;

syslog = readsyslog( syslog_path  );

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

%% Extract line capture runs from syslog

% Does not make plots.  May need to update lat/long/depth, etc.
%
% hold_alt = 7; %
% hold_alt = c.lineCaptureHoming.HoldAltitude.value(end);
% min_alt = c.lineCaptureDepth.LineCapture.MinAltitude.value(end);
min_alt = c.lineCaptureHoming.LineCapture.MinAltitude.value(end);

dock_depth = 67.5;

if( ~isempty(c.Dock.lat.value))
  dock_lat = c.Dock.lat.value(end);
  dock_lon = c.Dock.lon.value(end);
else
  %
  % To plot a hard-coded dock location, enter the correct location here, 
  % then uncomment the dock_x and dock_y lines further below.
  %
  dock_lat =    36.777186;
  dock_lon =  -121.861619;
end

% TrackingUpdatePeriod =
% c.lineCaptureDepth.TrackingUpdatePeriodTerminal.value(end); % DefaultUnder
TrackingUpdatePeriod = c.lineCaptureHoming.TrackingUpdatePeriodTerminal.value(end); % DefaultUnder
% proNavGain = c.LineCapture.navigationGain.value(end);
proNavGain = 3;

% Extract line capture runs
[ L ] = extract_line_capture_runs( syslog );
% Construct a time series index for each run.
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);

if( 0 )
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

end
titletext = ['Proportional Navigation guidance, gain = ' num2str(proNavGain)];

%% Horizontal Guidance
% close all

f = figure;
f.Position = position;
f.Name = 'Range,Az,Heading,Rudder';
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, '-o'); hold on;
scatter(c.TrackAcousticContact.range_to_contact.time, c.TrackAcousticContact.range_to_contact.value, 40, '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(:,2), finish_suc_range, 240, 'gd','filled')
scatter(ax, L(:,3), finish_fail_range,200, 'r', 'filled')
scatter(ax, c.VerticalControl.verticalMode.time, 10*c.VerticalControl.verticalMode.value, 10, 'y', 'filled')
scatter(ax, c.LineCapture.guidanceMode.time, 10*c.LineCapture.guidanceMode.value, 75, 'k', 'filled')
hold on
plot(c.height_above_sea_floor.time,c.height_above_sea_floor.value);
plot(c.VerticalControl.depthCmd.time,c.VerticalControl.depthCmd.value);
ylabel('range (m)')
%legend('universal','TrackAcousticContact', 'Approach', 'Homing start', ...
legend('Universal Range','TrackAcousticContact Range', 'Homing start', ...
    'Dock success', 'Missed', 'VerticalMode x 10', 'GuidanceMode x 10', ...
    'HeightAboveSeafloor', 'DepthCmd',...
    'location','best');
title(ax, dirName, 'Interpreter', 'none')

% DAT
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--');
plot(c.SpeedControl.speedCmd.time, c.SpeedControl.speedCmd.value*100,'g');
ylabel('azimuth (deg)')
legend(ax1,'xAngle to target (+ to stbd)','TrackAcouticcontact: xAngle (+ to stbd)','Zero','Commanded Speed x100');
%legend(ax1,'xAngle to target (+ to stbd)')


% DUSBL
% 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')
% 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, normalize_angle(rad2deg(c.platform_orientation.value), -180, 180)); hold on;
scatter(c.TrackAcousticContact.heading_to_contact.time, rad2deg(c.TrackAcousticContact.heading_to_contact.value),'filled')
plot(c.HorizontalControl.headingCmd.time, normalize_angle(rad2deg(c.HorizontalControl.headingCmd.value),-180,180));
% ylim(ax2, [-10 370])
ylabel('heading (deg)')
% legend('heading','TrackAcousticContact', 'headingCmd')
legend('vehicle heading','heading 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));
scatter(c.HorizontalControl.rudderAngleAction.time, rad2deg(c.HorizontalControl.rudderAngleAction.value),'filled')
ylabel('rudder angle (deg)')
legend(ax3,'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')
%}



lat = rad2deg(c.latitude.value);
lon = rad2deg(c.longitude.value);

dock_lat_est = rad2deg(c.TrackAcousticContact.contact_latitude.value);
dock_lon_est = rad2deg(c.TrackAcousticContact.contact_longitude.value);
dock_depth_est = c.TrackAcousticContact.contact_depth.value;

dock_lat_est_smooth = rad2deg(c.TrackAcousticContact.contact_latitude_lowpass.value); % movmean(dock_lat_est, 11);
dock_lon_est_smooth = rad2deg(c.TrackAcousticContact.contact_longitude_lowpass.value); % movmean(dock_lon_est, 11);


[x_nav, y_nav] = deg2utm(lat, lon);
z_nav = c.depth.value;

[dock_x_est,dock_y_est] = deg2utm(dock_lat_est, dock_lon_est);
[dock_x_est_smooth,dock_y_est_smooth] = deg2utm(dock_lat_est_smooth, dock_lon_est_smooth);
[dock_x,dock_y, utm] = deg2utm(dock_lat, dock_lon);

dock_x_est_smooth(dock_x_est_smooth==0) = NaN;
dock_y_est_smooth(dock_y_est_smooth==0) = NaN;


% dock_x = mean(dock_x_est(rt<1800), 'omitnan');
% dock_y = mean(dock_y_est(rt<1800), 'omitnan');


%% Plot 2D view

[x_nav, y_nav] = deg2utm(rad2deg(c.latitude.value) , rad2deg(c.longitude.value));
z_nav = c.depth.value;

[dock_x_est,dock_y_est] = deg2utm(rad2deg(c.TrackAcousticContact.contact_latitude.value), rad2deg(c.TrackAcousticContact.contact_longitude.value));
[dock_x_est_smooth,dock_y_est_smooth] = deg2utm(rad2deg(c.TrackAcousticContact.contact_latitude_lowpass.value), rad2deg(c.TrackAcousticContact.contact_longitude_lowpass.value));
%[dock_x,dock_y, utmz] = deg2utm(dock_lat, dock_lon);


% Plot as offsets from the dock position
% ca
figure;
set(gcf,'position',position)
set(gcf,'Name','Map View with Dock Location');
hold on;
scatter(x_nav - dock_x, y_nav - dock_y, 5, 'filled');
scatter(dock_x_est - dock_x, dock_y_est - dock_y, 'filled' );
scatter(dock_x_est_smooth - dock_x, dock_y_est_smooth - dock_y, 'filled');
%scatter(dock_x - dock_x, dock_y - dock_y,75,'>k','filled');

legend('Nav vehicle pos','Projected dock pos', 'Projected dock pos smooth', 'Dock pos') % 'Projected dock pos smooth',
set(gca, 'DataAspectRatio',[1 1 1]);
xlabel('+E (m)')
ylabel('+N (m)')
title(gca, dirName, 'Interpreter', 'none')


%% Plot nav track and dock position estimates per approach

% Set approach number to plot:
%APPROACH = 6;
if( height(T) > 1 )
  [table([1:height(T)]') T]
  APPROACH=input(sprintf('Please enter approach number 1 - %d.\n', height(T)));
else
  APPROACH=1;
end

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

if isnan(end_time)
    start_time = start_time_.Position(1)
    end_time = end_time_.Position(1)
end

idNav = c.latitude.time >= start_time & c.latitude.time <= end_time;
idTAC = c.TrackAcousticContact.range_to_contact.time >= start_time & c.TrackAcousticContact.range_to_contact.time <= end_time;
idTACs = c.TrackAcousticContact.contact_latitude_lowpass.time >= start_time & c.TrackAcousticContact.contact_latitude_lowpass.time <= end_time;

% Plot as offsets from the dock position
% ca
figure;
set(gcf,'position',position);
set(gcf,'Name',['Navplot for approach # ',num2str(APPROACH)]);
hold on;
scatter(x_nav(idNav) - dock_x, y_nav(idNav) - dock_y, 15, 'filled');
scatter(dock_x_est(idTAC) - dock_x, dock_y_est(idTAC) - dock_y, 34, c.TrackAcousticContact.range_to_contact.value(idTAC), 'filled' );
scatter(dock_x_est_smooth(idTACs) - dock_x, dock_y_est_smooth(idTACs) - dock_y, 34, c.TrackAcousticContact.range_to_contact.value(idTAC), 'filled','d');
%scatter(dock_x - dock_x, dock_y - dock_y,75,'>k','filled');
colormap(brewermap(128,'*RdPu'))
cb = colorbar;
cb.Label.String = 'Range (m)'; %'Elapsed time (sec)';
caxis([0 100])
title(['Approach # ' num2str(APPROACH) ])
% set(gca, 'DataAspectRatio',[1 1 1]);
xlabel(['+E (m), Approach # ',num2str(APPROACH)])
ylabel('+N (m)')
legend('Nav vehicle pos','Projected dock pos', 'Projected dock pos smooth', 'Dock pos') % 'Projected dock pos smooth',
axis equal
title(gca, dirName, 'Interpreter', 'none')

%%
% ca
k = APPROACH;

MAX_RANGE = 100; % m
MIN_RANGE = 2; % m

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

if isnan(end_time)
    start_time = start_time_.Position(1)
    end_time = end_time_.Position(1)
end

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


r = c.acoustic_contact_range.value(id);
t = c.acoustic_contact_range.time(id);
b = interp1(c.TrackAcousticContact.heading_to_contact.time, rad2deg(c.TrackAcousticContact.heading_to_contact.value), t);

et = (t - t(1)).*24*3600;
rb = -(b - b(1));

f = figure;
f.Position = position;
f.Name='Range,Time,LOS Angle';
ax = axes;
scatter(et,r,'k','filled'); hold on;
for k = 1:length(r)
    x = et(k);
    y = r(k);
    alpha = rb(k)-90;
    x2 = x + y*cosd(alpha);
    y2 = y + y*sind(alpha);
    plot([x x2],[y y2], 'k', 'LineWidth', 1);
end

ylabel('Range (m)');
xlabel('Elapsed time (s)');
legend('Range (m)', 'Relative LOS angle (deg)','Location','se');
title(ax,titletext,'Interpreter','none')
reverseYDir;
axis square
axis tight

% export_fig(gcf, ['~/Desktop/lineCaptureHoming_line_of_sight_angle_' logname '.png'],'-r300')

figure;
set(gcf,'position',position);
set(gcf,'Name','LOS Angle vs. Time')
plot(r, rb); hold on;
scatter(r, rb, 'filled');
plot(r,zeros(size(r)), 'k--')
set(gca, 'XDir', 'reverse')
xlabel('Range (m)')
ylabel('Relative LOS angle (deg)')
title(titletext,'Interpreter','none')
axis square
axis tight

% export_fig(gcf, ['~/Desktop/lineCaptureHoming_relative_angle_' logname '.png'],'-r300')

%% Raw/filtered bearing rate

decay = c.lineCaptureHoming.LineCapture.losRateIIRFilterDecay.value(end);
% decay = 0.0;

[c.LineCapture.rawBearingRate.time,si] = unique(c.LineCapture.rawBearingRate.time);
c.LineCapture.rawBearingRate.value = c.LineCapture.rawBearingRate.value(si);
bri = interp1(c.LineCapture.rawBearingRate.time, c.LineCapture.rawBearingRate.value, c.TrackAcousticContact.contact_latitude_lowpass.time);

% ca
figure;
set(gcf,'position',position);
set(gcf,'Name','Single-Pole IIR Filter, LOS Rate and Accel')
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')
%legend('universal','TrackAcousticContact', 'Approach', 'Homing start', 'Dock success', 'Missed')
legend('universal','TrackAcousticContact', 'Homing start', 'Dock success', 'Missed')
title(ax, dirName, 'Interpreter', 'none')
title(['Single-pole IIR filter decay = ' num2str(decay, 2) ])
ylabel('range (m)')

ax1 = axes; % delete(ax1)
ax1.Position = [0.05, 0.525, 0.9, 0.2];
hold on;
scatter(c.LineCapture.rawBearingRate.time, rad2deg(c.LineCapture.rawBearingRate.value), 'filled')
scatter(c.LineCapture.bearingRate.time, rad2deg(c.LineCapture.bearingRate.value));
% scatter(c.TrackAcousticContact.contact_latitude_lowpass.time, rad2deg(bri), 'filled')
legend(ax1,'raw LOS-rate', 'filtered LOS-rate') % 'LOS-rate external update'
ylabel('LOS-rate (deg/s)')

ax2 = axes; % delete(ax2)
ax2.Position = [0.05, 0.3, 0.9, 0.2];
plot(c.LineCapture.proNavCmd.time, rad2deg(c.LineCapture.proNavCmd.value)); hold on;
scatter(c.LineCapture.proNavCmd.time, rad2deg(c.LineCapture.proNavCmd.value));
ylabel('PN accel cmd (deg/s^2)')

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

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



return




%%

rawLosRate = rad2deg(c.LineCapture.rawBearingRate.value);
rawLosRate(abs(rawLosRate)>10) = [];

losRate = rad2deg(c.LineCapture.bearingRate.value);
losRate(abs(losRate)>10) = [];

f=figure;
f.Position = position;
f.Name = 'LOS Rate Histogram';
histogram( rawLosRate , 100); hold on;
histogram( losRate , 100);
xlabel('deg/s')


%% Plot smoothing
% ca

ti = c.TrackAcousticContact.contact_latitude_lowpass.time;
lat_lp  = rad2deg(c.TrackAcousticContact.contact_latitude_lowpass.value);
lon_lp  = rad2deg(c.TrackAcousticContact.contact_longitude_lowpass.value);

figure;
set(gcf,'position',position)
set(gcf,'Name','Range, Lat/Lon, Filt&Raw Heading to Dock')
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, dirName, 'Interpreter', 'none')

ax1 = axes; % delete(ax1)
ax1.Position = [0.05, 0.525, 0.9, 0.2];
scatter(c.TrackAcousticContact.contact_latitude.time, rad2deg(c.TrackAcousticContact.contact_latitude.value), 'filled'); hold on;
scatter(ti, lat_lp);

ax2 = axes; % delete(ax2)
ax2.Position = [0.05, 0.3, 0.9, 0.2];
scatter(c.TrackAcousticContact.contact_longitude.time, rad2deg(c.TrackAcousticContact.contact_longitude.value), 'filled'); hold on;
scatter(ti, lon_lp);


ax3 = axes; % delete(ax3)
ax3.Position = [0.05, 0.075, 0.9, 0.2];
scatter(c.TrackAcousticContact.heading_to_contact.time, normalize_angle(rad2deg(c.TrackAcousticContact.heading_to_contact.value)), 'filled'); hold on;
scatter(c.TrackAcousticContact.heading_to_contact_lowpass.time, normalize_angle(rad2deg(c.TrackAcousticContact.heading_to_contact_lowpass.value)));


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



%%

figure;
set(gcf,'position',position)
set(gcf,'Name',sprintf('Az, El and Depth Error for Approach %d.',APPROACH));
corder = colorOrder;

ax = gca;
ax.Position = [0.05, 0.75, 0.9, 0.2];
plot(c.TrackAcousticContact.range_to_contact.value(idTAC), rad2deg(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value(idTAC))); hold on
scatter(c.TrackAcousticContact.range_to_contact.value(idTAC), rad2deg(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value(idTAC)), 'filled')
plot([0 max(c.TrackAcousticContact.range_to_contact.value(idTAC))], [0 0], '--k')
title(['Approach # ' num2str(APPROACH) ]);
ylabel('Azimuth (deg)')
% xlabel('Range (m)')

ax1 = axes; % delete(ax1)
ax1.Position = [0.05, 0.525, 0.9, 0.2];
plot(c.TrackAcousticContact.range_to_contact.value(idTAC), rad2deg(c.TrackAcousticContact.elevation_to_contact_vehicle_frame.value(idTAC))); hold on
scatter(c.TrackAcousticContact.range_to_contact.value(idTAC), rad2deg(c.TrackAcousticContact.elevation_to_contact_vehicle_frame.value(idTAC)), 'filled')
plot([0 max(c.TrackAcousticContact.range_to_contact.value(idTAC))], [0 0], '--k')
ylabel('Elevation (deg)')

ax2 = axes; % delete(ax2)
ax2.Position = [0.05, 0.3, 0.9, 0.2];
plot(c.TrackAcousticContact.range_to_contact.value(idTAC), c.TrackAcousticContact.contact_depth.value(idTAC)); hold on
scatter(c.TrackAcousticContact.range_to_contact.value(idTAC), c.TrackAcousticContact.contact_depth.value(idTAC), 'filled')
plot([0 max(c.TrackAcousticContact.range_to_contact.value(idTAC))], [dock_depth dock_depth], '--k')
ylabel('Contact depth (m)')
xlabel('Range (m)')

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

%%

xponder_alt = 8.5;

time   = c.TrackAcousticContact.range_to_contact.time(idTAC);
range  = c.TrackAcousticContact.range_to_contact.value(idTAC);
elev   = c.TrackAcousticContact.elevation_to_contact_vehicle_frame.value(idTAC);
rolli  = interp1(c.platform_roll_angle.time, c.platform_roll_angle.value, time);
pitchi = interp1(c.platform_pitch_angle.time, c.platform_pitch_angle.value, time);
yawi   = interp1(c.platform_orientation.time, c.platform_orientation.value, time);
alti   = interp1(c.height_above_sea_floor.time, c.height_above_sea_floor.value, time);

roll_ = deg2rad(-6);

r = 100:-1:eps;
dz = xponder_alt - alti;

el = real(asin(dz./range));

R_ = Rzyx(roll_, 0.0, 0.0);

el_ = nan(size(range)); 

az_ = nan(size(range));

for i=1:length(el)

    R = Rzyx(rolli(i), pitchi(i), yawi(i));

    u = azimuth_and_elevation_to_unit_vector(0, el(i));

    u = R*u;

    [az_(i), el_(i)] = unit_vector_to_azimuth_elevation(u);
    
end

%close all;


f=figure;
f.Name='Elevation vs. Range';
f.Position = position;
plot(range, rad2deg(el));hold on
scatter(range, rad2deg(el));
plot(range, rad2deg(el_));hold on
scatter(range, rad2deg(el_));

plot(range, rad2deg(elev));hold on
scatter(range, rad2deg(elev));

ylabel('Elevation (deg)')
xlabel('Range (m)')

if( 0 )
figure;
plot(r, rad2deg(el));hold on
scatter(r, rad2deg(el));
plot(r, rad2deg(el_));hold on
scatter(r, rad2deg(el_));
ylabel('Elevation (deg)')
xlabel('Range (m)')
end

f=figure;
plot(range, rad2deg(az_)); hold on;
scatter(range, rad2deg(az_));
ylabel('Azimuth (deg)')
xlabel('Range (m)')
f.Name='Azimuth vs. Range';
f.Position = position;


%real(asin(dz./range))

figure
% contact_depth = interp1(c.TrackAcousticContact.contact_depth.time, c.TrackAcousticContact.contact_depth.value, time);
% plot(range, contact_depth);hold on
vehicle_depth = interp1(c.depth.time, c.depth.value, time);
plot(range, vehicle_depth); hold on
plot(range, rad2deg(elev));
ylabel('Inclination (deg)')
xlabel('Range (m)')
set(gcf,'position',position)
set(gcf,'Name','Inclination Versus Range');

%%
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(L(:,1), start_range, 75, 'g', 'filled')
scatter(L(:,2), finish_suc_range, 75, 'm', 'filled')
scatter(L(:,3), finish_fail_range, 75, 'r', 'filled')
ylabel('range (m)')
legend('universal','TrackAcousticContact', 'Approach', 'Homing start', 'Dock success', 'Missed')
title( dirName, 'Interpreter', 'none')
ylabel('Range (m)')
set(gcf,'position',position)
set(gcf,'Name','Parameters Versus Range');
%dynamicDateTicks

figure
set(gcf,'Name','Elevation, Range, Azimuth v.s. Time');
p1=subplot(2,1,1)
% contact_depth = interp1(c.TrackAcousticContact.contact_depth.time, c.TrackAcousticContact.contact_depth.value, time);
% plot(range, contact_depth);hold on
% vehicle_depth = interp1(c.depth.time, c.depth.value, time);
plot(c.depth.time, c.depth.value); hold on
plot(c.TrackAcousticContact.elevation_to_contact_vehicle_frame.time, ...
     rad2deg(c.TrackAcousticContact.elevation_to_contact_vehicle_frame.value)); hold on

% plot(c.DAT.elevation_vehicleFrame.time, rad2deg(c.DAT.elevation_vehicleFrame.value), 'o'); hold on
% plot(c.ExternalSim.homingSensorElevSim.time, rad2deg(c.ExternalSim.homingSensorElevSim.value))
ylabel('Elevation (deg) & Vehicle Depth (m)')
xlabel('Time')
legend('vehicle depth', 'TrackAcousticContact Elevation')

p2=subplot(2,1,2)
plot(c.TrackAcousticContact.range_to_contact.time, c.TrackAcousticContact.range_to_contact.value); hold on
plot(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.time, rad2deg(c.TrackAcousticContact.azimuth_to_contact_vehicle_frame.value))
ylabel('Azimuth (deg) & Range to Contact (m)')
xlabel('Time')
legend('TrackAcousticContact Range', 'TrackAcousticContact Azimuth')
set(gcf,'position',position)
dynamicDateTicks
linkaxes([p1 p2], 'x')
dynamicDateTicks([p1 p2],'link')
