% 
% Copyright (c) 2006-2013 by University of Washington.  All rights reserved.
%
% This file contains proprietary information and remains the 
% unpublished property of the University of Washington. Use, disclosure,
% or reproduction is prohibited.
% common display routines for the diveplot_X code

%
% KUTI - jfaust
% Made some slight modifications and reorganizations so that this code
% could work within the DiveData GUI framework.
%

%
% plot flight control variables vs. time
%
titlestring = underscore(sprintf('%s %s', mission_title, startstr));
basetitle = sprintf('%s: %s',base_file,titlestring);
cc_per_m3 = 1e6;

% kuti - jf - moved these two lines out of diveplot-specific code
ivb = find( vbddiff > 0.5 );
mass_vol_comment = sprintf('mass=%.3fkg, mass_c_o_m_p=%.3fkg, volmax=%.1fcc',mass,mass_comp,volmax);


timemin = 0;
timedel = 30;
timemax = (floor(sgtime_m(end)/timedel) + 1)*timedel;
maxd = max(vmdepth);
% use 1100 meters for SG so if we go a little over nomimal depth
% we don't jump to 2km scale
if maxd > 1100
  zscl = fix(maxd/100); % 1000 - 6000m
  % round up to the nearest 10
  zscl = (zscl - rem(zscl,10)) + 10;
elseif maxd > 500
  zscl = 10; % 500 - 1000m
elseif maxd > 300
  zscl = 5; % 300 - 500m
elseif maxd > 200
  zscl = 3; % 200 - 300m
elseif maxd > 100
  zscl = 2; % 100 - 200m
else
  zscl = 1; % 0 - 100m
end

dmin = 0;
dmax = zscl*100;
ddel = dmax/10;



% --------------------------------------------------------------------------------
% Plot 1 - Operational - vehicle attitude (diveplot)
% --------------------------------------------------------------------------------
try

%if mysettings.plotSelectionState(1) == 1
hFigPlot = figure('Name','Diveplot');
set(hFigPlot, 'UserData', 'Plot_Diveplot');

% Plot heading and right axis
%hdg_color = [0.5,0.5,0.5];  % gray
%hdg_color = [0, 0.4470, 0.7410];  % med blue
hdg_color = [1.0, .467, .109]; % orange

mag_hdg_color = hdg_color;

ax1 = axes('YAxisLocation', 'right');
set(gca, 'YLim', [0,360], 'YTick', [0:20:360], 'YColor', hdg_color, ...
         'Xlim', [timemin timemax], 'Xtick', [timemin:timedel:timemax]);
line(sgtime_m, hdg, 'Color', hdg_color, 'LineStyle', 'none', ...
     'Marker', '+', 'MarkerSize', 3);
y_label = 'Heading (\circM)';
ylabel(y_label, 'FontSize', 10); 
hold on;
plot([sgtime_m(1) sgtime_m(length(sgtime_m))], [mag_heading mag_heading], 'Color', mag_hdg_color, 'LineStyle', '--');

lg = legend('Heading (\circ M)', 'Desired Heading (\circ M)', ...
       'Location','NorthEast');
set(lg,'color','none'); % make transparent

% Plot left axis and curves

ax2 = axes('Position', get(ax1,'Position'),...
           'YAxisLocation', 'left', 'Color', 'none');
set(gca, 'YLim', [-100,80], 'YTick', [-100:10:80], 'YColor', 'k');
hold on;
plot(sgtime_m, zpos/zscl, '-r'); 
x_label  =  'Time (minutes)';
xlabel(x_label, 'FontSize', 10); 
y_label  =  base_file;
ylabel(y_label, 'FontSize', 10); 
%title(titlestring, 'FontSize', 12);
title(basetitle, 'FontSize', 12);
hold on;
plot(sgtime_m, w, '-b'); 
%plot(sgtime_m, hspdg(ctd2sg_i), '-c'); 
%plot(sgtime_m(sg_ctd_valid_i), w_stdy(ctd2sg_valid_i), '--b'); 
%plot(sgtime_m(sg_ctd_valid_i), hspd_stdy(ctd2sg_valid_i), '--c'); 
plot(sgtime_m, pitch, '-g'); 
plot(sgtime_m, pitch_control*10, '--g');
plot(sgtime_m, roll, '-y'); 
plot(sgtime_m, roll_control, '--y');
%plot(sgtime_m(sg_ctd_valid_i), glideangle_stdy(ctd2sg_valid_i)*radpd, '--m'); % convert to degrees
% Ensure that these two use the same scaling factor so they can be compared visually
%plot(sgtime_m(sg_ctd_valid_i), buoy(ctd2sg_valid_i)/10, '-m'); 
plot(sgtime_m, vbd/10, '-k'); 
%
v = axis;
axis( [timemin timemax -100 80] );
set(gca, 'Ytick', [-100:10:80], 'Xtick', [timemin:timedel:timemax])
%
% Mark non-zero motor current 
%
% kuti - jf - ivb moved up - needed for more than just plot 1
%ivb = find( vbddiff > 0.5 );
plot(sgtime_m(ivb), zpos(ivb)/zscl, '.k');
%
% Mark negative (port) and positive (starboard) roll changes
%
irn = find( fix(rolldiff) - abs( fix(rolldiff) ) );
irp = find( fix(rolldiff) + abs( fix(rolldiff) ) );
plot(sgtime_m(irn), zpos(irn)/zscl, '<k');
plot(sgtime_m(irp), zpos(irp)/zscl, '>k');

%
% Mark negative (downward) and positive (upward) pitch changes
%
pitchc = fix(10.*pitchdiff);
ipn = find( fix(pitchc) - abs( fix(pitchc) ) );
ipp = find( fix(pitchc) + abs( fix(pitchc) ) );

if (~ isempty(ipn))
    plot(sgtime_m(ipn), zpos(ipn)/zscl, 'vk'); % pitch down
end
if (~ isempty(ipp))
    plot(sgtime_m(ipp), zpos(ipp)/zscl, '^k'); % pitch up
end

% Draw legend and time stamp

strs = {strcat('Height (',num2str(zscl), 'm)'), ...
       'W = dz/dt (cm/s)', ...
       'Pitch Up (deg)', ...
       'Pitch Control (mm)', ...
       'Vehicle Roll Stbd (deg)', ...
       'Roll Control (deg)', ...
       'VBD (10 cc)', ... 
       'Non-zero motor currentVBD', ... 
       'Roll to port', ...
       'Roll to stbd' };
legendEntries = length(strs);

if (~ isempty(ipn))
     legendEntries = legendEntries + 1;
    strs{legendEntries} = 'Pitch down';
end
if (~ isempty(ipp))
    legendEntries = legendEntries + 1;
    strs{legendEntries} = 'Pitch up';
end

lg = legend(strs, 'Location', 'SouthEast');
set(lg,'color','none'); % make transparent

%set(gca, 'YLim', [-100,80], 'YTick', [-100:10:80], 'YColor', 'k');
%hold on;
grid on;
pip1 = plot([sgtime_m(1) sgtime_m(mp)], [intended_pitch intended_pitch], ':g', 'DisplayName', 'intended pitch');
pip2 = plot([sgtime_m(1) sgtime_m(mp)], [-intended_pitch -intended_pitch], ':g', 'DisplayName', '-intended pitch');

piw1 = plot([sgtime_m(1) sgtime_m(mp)], [w_d w_d], ':b', 'DisplayName', 'w_d');
piw2 = plot([sgtime_m(1) sgtime_m(mp)], [-w_d -w_d], ':b', 'DisplayName', '-w_d');
%lgp = legend([pip1 pip2 piw1 piw2], 'Location','SouthWest');
%set(lgp,'color','none'); % make transparent

v = axis;
ytxt = v(3) -0.1*(v(4)-v(3));
xtxt = v(1);
text(xtxt , ytxt, timelabel);

%end
catch
end

% --------------------------------------------------------------------------------
% Plot 4 - Operational - scatter of pitch with pitch_control
% --------------------------------------------------------------------------------
try
	
%if mysettings.plotSelectionState(4) == 1

  %
  % find linear fit between pitch and pitch_control
  %
  
  ymax = 60; % PARAMETER max positive measured pitch (degrees)
  ymin = -60; % PARAMETER min positive measured pitch (degrees)
  ytic = 10;
  xmax = 3.5; % PARAMETER max positive pitch control position (cm)
  xmin = -3.5; % PARAMETER max megative pitch control position (cm)
  xtic = 0.5;
  
  ip = find(pitch_control > xmin & pitch_control < xmax);
  X = [ones(size(pitch_control(ip))) pitch_control(ip)];
  A = X\pitch(ip);
  inv_pitch_cnv = 1/loginfo.PITCH_CNV; % 217.39 % AD counts per cm
  c_pitch_imp = c_pitch - ( A(1)/A(2) )*inv_pitch_cnv;
  
  pitch_control_max = (pitch_max_cnts - c_pitch)/inv_pitch_cnv;
  pitch_control_min = (pitch_min_cnts - c_pitch)/inv_pitch_cnv;
  pitch_control_max_imp = ...
      (pitch_max_cnts - c_pitch_imp)/inv_pitch_cnv;
  pitch_control_min_imp = ...
      (pitch_min_cnts - c_pitch_imp)/inv_pitch_cnv;
  
  hFigPlot = figure('Name','Pitch');
  set(hFigPlot, 'UserData', 'Plot_Pitch');  

  plot(pitch_control, pitch, '.b');
  hold on;
  grid on;
  plot(pitch_control_intended, pitch_intended, '.r')
  plot([0], [0], '+r', 'MarkerSize', 10)
  axis([xmin xmax ymin ymax]);
  set(gca, 'XTick', [xmin:xtic:xmax], 'YTick', [ymin:ytic:ymax]);
  xlabel('Pitch Control [cm]');
  ylabel('Pitch [\circ]');
  title(basetitle, 'FontSize', 12);
  
  ix = [xmin:0.01*(xmax-xmin):xmax];
  plot(ix, A(1) + A(2)*ix, '-k');
  
  xtxt = xmin + 0.1*(xmax-xmin);
  ytxt = ymin + 0.95*(ymax-ymin);
  comment = strcat( 'C_{PITCH} =', num2str(c_pitch) );
  text(xtxt, ytxt, comment);
  ytxt = ymin + 0.9*(ymax-ymin);
  comment = strcat( 'Pitch control range =', ...
                    num2str(pitch_control_min), ' to ', ...
                    num2str(pitch_control_max), ' cm'); 
  text(xtxt, ytxt, comment);
  ytxt = ymin + 0.85*(ymax-ymin);
  comment = strcat( 'Implied Pitch Center =', num2str(c_pitch_imp) );
  text(xtxt, ytxt, comment);
  ytxt = ymin + 0.8*(ymax-ymin);
  comment = strcat( 'Implied pitch control range =', ...
                    num2str(pitch_control_min_imp), ' to ', ...
                    num2str(pitch_control_max_imp), ' cm'); 
  text(xtxt, ytxt, comment);
  ytxt = ymin + 0.75*(ymax-ymin);
  comment = strcat( 'Assumed pitch control gain =', ...
                    num2str(inv_pitch_cnv), ' [counts/cm aft]' );
  text(xtxt, ytxt, comment);
  
  xtxt = xmin + 0.3*(xmax-xmin);
  ytxt = ymin + 0.14*(ymax-ymin);
  comment = strcat( 'Pitch [\circ] =', num2str(A(1)), '\circ +', ...
                    'Pitch Control [cm] *', num2str(A(2)), '\circ/cm' );
  text(xtxt, ytxt, comment);
  
  v = axis;
  ytxt = v(3) -0.1*(v(4)-v(3));
  xtxt = v(1);
  text(xtxt , ytxt, timelabel);

%end
catch
end

return;

