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

position = [0 0 .979 .935];

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.  Unserialize -m Logs/...? \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);


if( ~isempty(c.Dock.lat.value))
  dock_lat = c.Dock.lat.value(end);
  dock_lon = c.Dock.lon.value(end);
  dock_depth = c.Dock.depth.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;
  dock_depth = 67.5;
end

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

% To make 20231115T180422 plot:
if( length(c.latitude.value) > length(c.longitude.value) ) 
  c.latitude.value = c.latitude.value(1:length(c.longitude.value));
end


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



%
% Remove NaNs:
lat = lat(~isnan(lat));
lon = lon(~isnan(lon));

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_median = rad2deg(c.TrackAcousticContact.contact_latitude_lowpass.value); % movmean(dock_lat_est, 11);
dock_lon_est_median = 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_median,dock_y_est_median] = deg2utm(dock_lat_est_median, dock_lon_est_median);

dock_x_est_median(dock_x_est_median==0) = NaN;
dock_y_est_median(dock_y_est_median==0) = NaN;


%% Plot 2D view

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

% [origin_x, origin_y, utm] = deg2utm(dock_lat, dock_lon);
% origin_x = mean(dock_x_est(rt<1800), 'omitnan');
% origin_y = mean(dock_y_est(rt<1800), 'omitnan');
origin_x = x_nav(1);
origin_y = y_nav(1);

[dock_x_est,dock_y_est] = deg2utm(rad2deg(c.TrackAcousticContact.contact_latitude.value), rad2deg(c.TrackAcousticContact.contact_longitude.value));
[dock_x_est_median,dock_y_est_median] = deg2utm(rad2deg(c.TrackAcousticContact.contact_latitude_lowpass.value), rad2deg(c.TrackAcousticContact.contact_longitude_lowpass.value));

% 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 - origin_x, y_nav - origin_y, 5, 'filled');
indx=c.platform_speed_wrt_ground.value<.5;
scatter(x_nav(indx) - origin_x, y_nav(indx) - origin_y, 5, 'm','filled');
scatter(dock_x_est - origin_x, dock_y_est - origin_y, 'filled' );
scatter(dock_x_est_median - origin_x, dock_y_est_median - origin_y, 'filled');
%scatter(dock_x - dock_x, dock_y - dock_y,75,'>k','filled');

legend('Nav vehicle pos','Propulsion off','Projected dock pos', 'Projected dock pos median') 
set(gca, 'DataAspectRatio',[1 1 1]);
axis equal %Same as above?
xlabel('+E (m)')
ylabel('+N (m)')
title(gca, dirName, 'Interpreter', 'none')


% 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

%
% Remove bad data points:
bad_data_pts = sum(c.acoustic_contact_range.time<=c.acoustic_contact_range.time(1));
if( bad_data_pts > 1 )
  fprintf('Removing %d/%d bad data points in c.acoustic_contact_range.time\n', ...
      bad_data_pts, length(c.acoustic_contact_range.time));
  indx = c.acoustic_contact_range.time>c.acoustic_contact_range.time(1);
  c.acoustic_contact_range.time = c.acoustic_contact_range.time(indx);
  c.acoustic_contact_range.value = c.acoustic_contact_range.value(indx);
end

f = figure;
f.Position = position;
%set(gcf,'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; %This is slant range
%c.TrackAcousticContact.range_to_contact is horizontal range
%scatter(c.TrackAcousticContact.range_to_contact.time, c.TrackAcousticContact.range_to_contact.value, 40, 'filled')
scatter(c.TrackAcousticContact.horizontal_range_to_contact_lowpass.time, c.TrackAcousticContact.horizontal_range_to_contact_lowpass.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')

if(1)
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')
else
plot(ax, c.VerticalControl.verticalMode.time, 10*c.VerticalControl.verticalMode.value,'y')
plot(ax, c.LineCapture.guidanceMode.time, 10*c.LineCapture.guidanceMode.value, 'kx')
end

hold on
plot(c.height_above_sea_floor.time,c.height_above_sea_floor.value);
plot(c.VerticalControl.depthCmd.time,c.VerticalControl.depthCmd.value);
plot(c.depth.time, c.depth.value,'--m')
ylabel('range (m)')
legend('Slant Range','TrackAcousticContact Horizontal Range', 'Homing start', ...
    'Dock success', 'Missed', 'VerticalMode x 10', 'GuidanceMode x 10', ...
    'HeightAboveSeafloor', 'DepthCmd','Depth','Closest',...
    'location','best');

title(ax, dirName, 'Interpreter', 'none')

%Compute and plot miss distances

minLocs=islocalmin(c.acoustic_contact_range.value, 'MinSeparation',120);
plot(c.acoustic_contact_range.time(minLocs), c.acoustic_contact_range.value(minLocs),'mx');
minSlantRanges = round(c.acoustic_contact_range.value(minLocs)'*100);

depthAC    = interp1(c.depth.time,c.depth.value, c.acoustic_contact_range.time);
altitudeAC = interp1(c.height_above_sea_floor.time, ...
             c.height_above_sea_floor.value, c.acoustic_contact_range.time);
minDepths = round((depthAC(minLocs)' - 24)*100);
minAlts   = round((altitudeAC(minLocs)' - 34)*100);
fprintf('Miss distances in centimeters:\n');

fprintf('Slant Range  Horizontal Error   Depth Error\n');
fprintf('   %5d              %5d              %5d\n',...
    [minSlantRanges round(real(sqrt((minSlantRanges.^2-minDepths.^2)))), minDepths]');


%fprintf('Slant Range  Altitude Error Depth Error\n');
%fprintf('   %5d         %5d         %5d\n',[minSlantRanges, minAlts, minDepths]');

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

c.platform_speed_wrt_ground.value(find(c.platform_speed_wrt_ground.value > 1.5)) = NaN;
plot(c.platform_speed_wrt_ground.time, c.platform_speed_wrt_ground.value*100,'g--');

ylabel('azimuth (deg)')
legend(ax1,'Azimuth to target (+ to stbd)','TrackAcouticcontact: Azimuth (+ to stbd)','Zero','Commanded Speed cm/sec','Ground Speed cm/sec');

% 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')
scatter(c.TrackAcousticContact.heading_to_contact_lowpass.time, normalize_angle(rad2deg(c.TrackAcousticContact.heading_to_contact_lowpass.value),-180,180),'filled')
plot(c.HorizontalControl.headingCmd.time, normalize_angle(rad2deg(c.HorizontalControl.headingCmd.value),-180,180),'--o');
% ylim(ax2, [-10 370])
ylabel('heading (deg)')
% legend('heading','TrackAcousticContact', 'headingCmd')
legend('vehicle heading','heading to median-filtered 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),'-o'); hold on;
scatter(c.HorizontalControl.rudderAngleAction.time, ...
    rad2deg(c.HorizontalControl.rudderAngleAction.value),'filled')
plot(c.HorizontalControl.headingIntegralInternal.time, c.HorizontalControl.headingIntegralInternal.value*180/pi,'m--o')
ylabel('rudder angle (deg)')
legend(ax3,'Measured Rudder Angle','Commanded Rudder Angle','Integral Term')
% plot(c.platform_roll_angle.time, rad2deg(c.platform_roll_angle.value));


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

%dynamicDateTicks

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

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


%% Plot nav track and dock position estimates per approach

% Set approach number to plot:

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

if( APPROACH == 0 ) APPROACH = 1:height(T); end


for( i = APPROACH )

fprintf('Plotting approach # %d\n',i);
  
start_time = T.start_time(i);
end_time = T.dock_time(i);
if isnan(end_time)
    end_time = T.miss_time(i);
end

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

if( isnan(L(i,2)) && isnan(L(i,3)) )
  continue;
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;

idHC = c.HorizontalControl.headingCmd.time >= start_time & ...
    c.HorizontalControl.headingCmd.time <= end_time;



% Plot as offsets from the dock position
% ca
figure;
set(gcf,'position',position);
set(gcf,'Name',['Navplot for approach # ',num2str(i)]);
hold on;

hit_t    =  c.TrackAcousticContact.horizontal_range_to_contact_lowpass.time(idTACs);
range_t  =  c.TrackAcousticContact.horizontal_range_to_contact_lowpass.value(idTACs);


veh_at_hit_x =  interp1( c.longitude.time(idNav), x_nav(idNav), hit_t);
veh_at_hit_y =  interp1( c.latitude.time(idNav), y_nav(idNav),  hit_t);

hdg_at_hit   =  interp1( c.HorizontalControl.headingCmd.time(idHC),c.HorizontalControl.headingCmd.value(idHC) ,  hit_t);

dock_x_median = dock_x_est_median(idTACs);
dock_y_median = dock_y_est_median(idTACs);

mksize = 10^2;

%
% Select range subset:  ranges > 10 m and < 40 m

if( exist('rangeCut') ~= 1) 
  rangeCut = 10;
end

indx_tg   = range_t >= rangeCut & range_t <= 40;

hit_t_sub = hit_t(indx_tg);

if( isempty(hit_t_sub) ) 
  continue;
end
  
%idNav_tg  = c.latitude.time >= min(hit_t_sub) & c.latitude.time <= max(hit_t);
idNav_tg  = c.latitude.time >= min(hit_t_sub) & c.latitude.time <= end_time;

onlyTG = 1;

if( onlyTG )
  veh_x_sub         = x_nav(idNav_tg);
  veh_y_sub         = y_nav(idNav_tg);
  range_t_sub       = range_t(indx_tg);
  hit_t_sub         = hit_t(indx_tg);
  veh_at_hit_x_sub  = veh_at_hit_x(indx_tg);
  veh_at_hit_y_sub  = veh_at_hit_y(indx_tg);
  dock_x_median_sub = dock_x_median(indx_tg);
  dock_y_median_sub = dock_y_median(indx_tg);
  hdg_at_hit_sub    = hdg_at_hit(indx_tg);
  hdg_raw_at_hit_sub= c.TrackAcousticContact.heading_to_contact.value(idTAC);
  npts              = length(hit_t_sub);
else
  veh_x_sub         = x_nav(idNav);
  veh_y_sub         = y_nav(idNav);
  range_t_sub       = range_t;
  hit_t_sub         = hit_t;
  veh_at_hit_x_sub  = veh_at_hit_x;
  veh_at_hit_y_sub  = veh_at_hit_y;
  dock_x_median_sub = dock_x_median;
  dock_y_median_sub = dock_y_median;
  hdg_at_hit_sub    = hdg_at_hit;
  hdg_raw_at_hit_sub= c.TrackAcousticContact.heading_to_contact.value;
  npts              = length(hit_t);
end

%scatter(x_nav(idNav) - origin_x, y_nav(idNav) - origin_y, 15, 'filled');

if( isempty(veh_x_sub) ) 
  continue;
end

origin_x = veh_x_sub(1);
origin_y = veh_y_sub(1);

dock_x_final = veh_x_sub(end);
dock_y_final = veh_y_sub(end);

scatter(veh_x_sub - origin_x, veh_y_sub - origin_y, 15, 'filled');

axis equal

%scatter(dock_x_est(idTAC) - origin_x, dock_y_est(idTAC) - origin_y, mksize, c.TrackAcousticContact.range_to_contact.value(idTAC), 'filled','s');
%scatter(dock_x_est(idTAC) - origin_x, dock_y_est(idTAC) - origin_y, mksize, 'k', 'filled','s');
%scatter(dock_x_est_median(idTACs) - origin_x, dock_y_est_median(idTACs) - origin_y, mksize, jet(length(range_t)), 'filled','s');

scatter(dock_x_median_sub - origin_x, dock_y_median_sub - origin_y, mksize, jet(length(range_t_sub)), 'filled','s');
  
scatter(veh_at_hit_x_sub - origin_x, veh_at_hit_y_sub - origin_y, mksize, jet(length(range_t_sub)), 'filled','d');

axis equal

hold on
if( ~exist('dockLines')) dockLines = 1;end
for( ii=1:npts )
  if(dockLines)  
    plot([veh_at_hit_x_sub(ii) dock_x_median_sub(ii)] - origin_x,...
        [veh_at_hit_y_sub(ii) dock_y_median_sub(ii)] - origin_y, ...
        'k','linewidth',1);
  end
   qcolors = jet(npts);
   quiver(veh_at_hit_x_sub(ii)-origin_x, veh_at_hit_y_sub(ii)-origin_y,...
       2*sin(hdg_at_hit_sub(ii)), 2*cos(hdg_at_hit_sub(ii)), 'color',qcolors(ii,:));

   if( 0 )
   %sin(theta+pi) = -sin(theta); cos(theta+pi) = -cos(theta)
   plot(dock_x_final - origin_x - range_t_sub(ii)*sin(hdg_at_hit_sub(ii)),...
       dock_y_final - origin_y - range_t_sub(ii)*cos(hdg_at_hit_sub(ii)),'k--o');
   plot(dock_x_final - origin_x - range_t_sub(ii)*sin(hdg_raw_at_hit_sub(ii)),...
       dock_y_final - origin_y - ...
       range_t_sub(ii)*cos(hdg_raw_at_hit_sub(ii)),'--kd','MarkerFaceColor','k');
   end
 
end

axis equal

%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])
% set(gca, 'DataAspectRatio',[1 1 1]);
title([ dirName, ' Approach # ', num2str(i), '; ', num2str(npts),...
      ' Median-Filtered DAT Fixes Total; ', num2str(length(hit_t_sub)), ' here.' ]);
xlabel(['Eastings, Approach # ',num2str(i)]);
ylabel('Northings')
legend('Vehicle Position','Projected Dock Position', 'Corresponding Vehicle Position',...
    'Bearing to Dock');
%   'Bearing to Dock', 'Median Filtered Veh From Dock', 'Raw Veh From Dock');

axis(gca,'equal')

hold on


end

return



if( 0 )

%
% PURPOSE: Plot ahrs data.
%
% LOG:     30 June 2021 rsm.
%
if( 0 )
  lstr={};
  lrDatPtr=[];
  lrUsblPtr=[];
  %global titleStr;
  filepath = logname;
  titleStr = [' Data Set: ', replace(DirName2,'_','-'), ' / ', replace(filepath,'_','-')];
  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
  if exist( 'lrAhrsFigPtr' ) ~= 1
    lrAhrsFigPtr= figure;
    set( lrAhrsFigPtr, 'Name', 'LR Ahrs', 'DeleteFcn', 'clear lrAhrsFigPtr' );
    set(gcf,'position',position)
    if(exist('fontsize')) 
      lrNavFontsize=fontsize;
    else
      lrNavFontsize=20;
    end
    lrNavFontweight='normal';
    set(gca,'fontsize',lrNavFontsize,'fontweigh',lrNavFontweight)
    set(gcf,'defaultaxesfontsize',lrNavFontsize,'defaulttextfontsize', ...
        lrNavFontsize)
    lrNavAxesPtr = gca;
  else
    figure(lrAhrsFigPtr);
  end

end

figure;
set(gcf,'position',position);
set(gcf,'Name','Ahrs')
hold on;


vehicleDataAvail = 0;
%
% Check to see if we've got vehicle data.  If so, plot it.
if( exist( 'c' ) && isfield( c, 'AHRS_M2') )
  vehicleDataAvail = 1;
  fprintf('Plotting vehicle data.\n');
  subplot(3,1,1)
  plot(c.AHRS_M2.platform_roll_angle.time, c.AHRS_M2.platform_roll_angle.value*180/pi)
  grid on
  ylabel('Roll, Degrees')
  dynamicDateTicks
%  title(titleStr)
  

  subplot(3,1,2)
  plot(c.AHRS_M2.platform_pitch_angle.time, c.AHRS_M2.platform_pitch_angle.value*180/pi,'b');
  grid on
  ylabel('Pitch, Degrees')
  dynamicDateTicks
  
  subplot(3,1,3)
  plot(c.AHRS_M2.platform_orientation.time, c.AHRS_M2.platform_orientation.value*180/pi,'b');
  grid on
  ylabel('Yaw, Degrees')
  dynamicDateTicks
  
  linkaxes(get(gcf,'children'),'x')


else
  fprintf('No data found.  Assuming orignal vehicle data (not replay) is in struct ''data''.\n');
  return
end

end

%%
% ca
k = APPROACH(1);

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( 0 )
if isnan(end_time)
    start_time = start_time_.Position(1)
    end_time = end_time_.Position(1)
end
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 kk = 1:length(r)
    x = et(kk);
    y = r(kk);
    alpha = rb(kk)-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, ' Approach #', num2str(k)],'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

return


% 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(1)));
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(1)) ]);
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')
