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


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;


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



%% 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_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');
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','Projected dock pos', 'Projected dock pos median', 'Dock pos') % 'Projected dock pos median',
set(gca, 'DataAspectRatio',[1 1 1]);
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;
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')

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

if(1)
plot(ax, c.VerticalControl.verticalMode.time, 10*c.VerticalControl.verticalMode.value, 'mx');
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);
ylabel('range (m)')
legend('Range','TrackAcousticContact Range', 'Homing start', ...
    'Dock success', 'Missed', 'VerticalMode x 10', 'GuidanceMode x 10', ...
    'HeightAboveSeafloor', 'DepthCmd',...
    'location','best');

%   'Dock success', 'Missed', ...


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

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



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


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


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

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

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

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

indx_tg   = range_t >= 10 & 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);

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

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


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

if( 0 )

  fprintf(' Number of good hits:  %d\n', npts)
  %cax=[min(posxUsbl) max(posxUsbl)]; 
  beaconColor = 'hitNumber';
  beaconColor = 'range';
  if( strcmp( beaconColor, 'range' ) )
    cax=[min(range_t) max(range_t)]; 
    cmp = colormap(jet(128));
    rng=linspace(cax(1),cax(2),length(cmp));
    rng=linspace(cax(1),cax(2),length(hit_t));
  elseif( strcmp( beaconColor, 'hitNumber' ) )
    cax=[1 npts]; 
    cmp = colormap(jet(npts));
  end
  hold on;
  for i=1:npts
    %[mn kk]=min(abs(rng-posxUsbl(i)));
    if( strcmp( beaconColor, 'range' ) ) [mn kk]=min(abs(rng-range_t(i)));
    elseif( strcmp( beaconColor, 'hitNumber' ) ) kk=npts-i+1;
    end
    plot(veh_at_hit_x - origin_x, veh_at_hit_y - origin_y,...
        'd','color',cmp(round(rng(i)),:),'markersize',7);
%        'd','color',cmp(kk,:),'markersize',7);
    % plot(usblE(i)*distUnits,usblN(i)*distUnits,...
    %     'k+','color',cmp(kk,:),'markersize',7);
  end
  %
  % Plotting in a loop kills the labeling. So, 
  % THE ABOVE LOOP HAS TO COME LAST OR IT SCRAMBLES THE LEGENDS.

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

end

hold on
for( ii=1:npts )
  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


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


end

fprintf('Before ''return''\n');

return

