% review.m - Odyssey II data review (~odyssey/matlab/review.m)

%----------------------------------------------------------------------
%                          Copyright (C) 1995
%                Massachussetts Institute of Technology
%               Sea Grant Underwater Vehicles Laboratory
%----------------------------------------------------------------------
% ----------------------------------------------------------------
%     $Revision: 1.4 $			$State: Exp $
%     $Author: jleonard $		$Date: 1996/05/13 21:58:17 $
% ---------------------------------------------------------------- */
% Created: 3-22-94 by jleonard (Note: different from bamoran version)

%%%%%%% get username to decide which version of review.m %%%%%%%%%%

jleonard=1;
make_timing_plot=0;
do_3d_plot=0;
recalc_dr=0;

if exist( 'jleonard' ) ~= 1                %% plot profile 4/18/96 lblsim
  if (strcmp('jleonard', getenv('USER')))
     jleonard=1; 
     sonar_freq=200;
     if (exist('make_timing_plot')==0)   
       make_timing_plot=0;
     end
     if (exist('do_3d_plot')==0) 
       do_3d_plot=0;
     end
     if (exist('make_lbl_plot')==0) 
       make_lbl_plot=1;
     end
     if (exist('make_real_plot')==0) 
       make_real_plot=0;
     end
     if (exist('make_sonar_plot')==0) 
       make_sonar_plot=0;
     end
     if (exist('make_trench_plot')==0) 
       make_trench_plot=0;
     end
  else 
     jleonard=0;
  end
end

if (exist('edc')==1)
  if (edc==1) 
    do_3d_plot=0;
    make_sonar_plot=0;
  end
else
  edc = 0;
end

if exist( 'hanu' ) ~= 1
  if (strcmp('hanu', getenv('USER')))
     hanu=1; 
     do_3d_plot=0;
     make_timing_plot=0;  
     make_lbl_plot=0;
     make_sonar_plot=0;
  else 
     hanu=0;
  end
end


if (exist('nrad')==1)
  if (nrad==1) 
    do_3d_plot=0;
    make_sonar_plot=0;
  end
else
  nrad = 0;
end

if (exist('run_name')==0) run_name = ' '; end
rad_to_deg = 180.0/pi;
n = length(data);
fig = 1;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% timing plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (exist('fig')==0) fig = 1; end 

if (exist('make_timing_plot')==0) make_timing_plot = 1; end 

if (make_timing_plot==1)
  figure(fig)
  fig = fig + 1;
  set( gcf, 'name', 'Control Cycle Timing Plot' );
  subplot(211)
  hold off
  title(['control cycle duration ',run_name]);
  ylabel('seconds');
  xlabel('control cycle count');
  tdiff = zeros(size(data(:,m_present_time)));
  for i = 2:(n-2)
    tdiff(i) = data(i,m_present_time) - data(i-1,m_present_time);
  end
  plot(tdiff)
  bin = zeros(100,2);
  for i = 1:100;
    bin(i,1) = i/100;
  end
  %
  for i = 1:(n-2)
    j = floor(((tdiff(i)+0.001)*100));
    if (j>100) 
      j = 100;
    elseif (j<1)
      j = 1;
    end
    bin(j,2) = bin(j,2) + 1;
  end
  subplot(212)
   hold off
  plot(bin(:,2:2))
  hold on
  plot(bin(:,2:2),'o')
  title(['Cycle duration histogram (mean = ',num2str(1000*mean(tdiff)),') ', num2str(bin(20,2)), ' cycles @ 200mSec']);
  ylabel('number');
  xlabel('control cycle time');
  v = axis;
  zoom on
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% depth plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (exist('make_depth_plot')==0) 
  make_depth_plot = 1;
end 
if (make_depth_plot==1)
  if (exist('c_depth')==1) 
    cdepth = data(:, c_depth);
  else 
    cdepth = data(:, cc_depth_0);
  end

  figure(fig)
  fig = fig + 1;
  set( gcf, 'name', 'Depth Plots' );
  subplot(211);
  hold off
  plot(data(:,m_present_time), data(:,m_depth))
  set(gca, 'Ydir', 'reverse');
  hold on
  grid on
  plot(data(:,m_present_time), cdepth, 'c')
  title(['meas depth and commanded depth vs time ',run_name]);
  ylabel('meters');
  xlabel('seconds');



%% jleonard 2/7/96 need to fix this plot

  subplot(212);
  if (exist('show_both_fin_angles')==1) 
    if (show_both_fin_angles==1) 
      plot(data(:,m_present_time), data(:, c_elevator_angle)*rad_to_deg, 'c'); 
    end
  else
    hold off;
    plot(data(:,m_present_time), data(:, m_elevator_angle)*rad_to_deg);
    hold on
    grid on
    ii = find(data(:,cc_depth_mode)==0); %% jleonard - 2/7/96 
    plot(data(ii,m_present_time), data(ii, c_elevator_angle)*rad_to_deg, 'c');
  end

  title(['elevator_angle ',run_name]);
  ylabel('degrees');
  xlabel('seconds');
  zoom on
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% heading plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (exist('make_heading_plot')==0)  make_heading_plot = 1; end 
if (make_heading_plot==1) 

  yaw = data(:, m_yaw)*rad_to_deg;

  if (exist('c_depth')==1) 
    cyaw = data(:, c_heading)*rad_to_deg;
  else 
    cyaw = data(:, cc_heading_0)*rad_to_deg;
  end



  % jleonard doing a new mod360 sort of thing to fix plot ugliness
  ii = find(yaw>355);             
  yaw(ii) = yaw(ii) - 360.0;
  ii = find(cyaw>355);          
  cyaw(ii) = cyaw(ii) - 360.0;
  clear ii


  figure(fig)
  fig = fig + 1;
  set( gcf, 'name', 'Heading Plots' );
  subplot(211);
  hold off;
  plot(data(:,m_present_time), yaw,'.');
  hold on;
  grid on
  plot(data(:,m_present_time), cyaw, 'c');
  title(['magnetic_yaw and cmd_yaw vs time ',run_name]);
  ylabel('degrees');
  xlabel('seconds');
  
  subplot(212);
  hold off;
  plot(data(:,m_present_time), data(:, m_rudder_angle)*rad_to_deg);
  hold on;
  plot(data(:,m_present_time), data(:, c_rudder_angle)*rad_to_deg, 'c');
  grid on
  title(['rudder_angle ',run_name]);
  ylabel('degrees');
  xlabel('seconds');
  zoom on
end
  


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% attitude plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
if (exist('make_attitude_plot')==0)  make_attitude_plot = 1; end 
if (make_attitude_plot==1)
  figure(fig)
  fig = fig + 1;
  set( gcf, 'name', 'Attitude Plots' );
  subplot(211);
  hold off;
  plot(data(:,m_present_time), data(:,m_roll)*rad_to_deg);
  hold on;
  grid on
  title(['roll vs time ',run_name]);
  ylabel('degrees');
  xlabel('seconds');
  subplot(212);
  hold off;
  plot(data(:,m_present_time), data(:,m_pitch)*rad_to_deg);
  hold on;
  grid on
  title(['pitch vs time ',run_name]);
  ylabel('degrees');
  xlabel('seconds');
  zoom on
end
  


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% dr plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
if (exist('make_dr_plot')==0)  make_dr_plot = 1; end 

if (make_dr_plot==1) 
  figure(fig)
  fig = fig + 1;
  clf;
  set( gcf, 'name', 'DR Plot' );


  speed = 1.35;        % just a guess, really. (m/sec)

  % check if m_dr_north and m_dr_east variables are defined in the 
  % data file, if not, re-calculate dr trajectory. 

  if (exist('recalc_dr')==0) recalc_dr = 0; end;

  if (recalc_dr == 1)
    dr_time = data(:, m_present_time);
    dr_depth = data(:, m_depth);

    dt = zeros(n,1);
    dr_north = zeros(n,1);
    dr_east = zeros(n,1);
    dr_pitch = data(:,m_pitch);
    dr_yaw = data(:,m_yaw);
  
  
    if exist('value_u_veh_init_no') 
       disp('review: using value_u_veh_init_no for to init dead-reckoning');
       dr_north(1) = value_u_veh_init_no;
       dr_north(2) = value_u_veh_init_no;
    elseif exist('real_x')
       disp('review: using real_x to init dead-reckoning');
       dr_north(1) = data(1, real_x);
       dr_north(2) = data(1, real_x);
    end
    if exist('value_u_veh_init_ea') 
      disp('review: using value_u_veh_init_ea for to init dead-reckoning');
      dr_east(1) = value_u_veh_init_ea;
      dr_east(2) = value_u_veh_init_ea;
    elseif exist('real_y')
       disp('review: using real_y to init dead-reckoning');
       dr_east(1) = data(1, real_y);
      dr_east(2) = data(1, real_y);
    end
    
    for i=3:n
      dt(i) = dr_time(i) - dr_time(i-1);
      horz_speed = speed * cos(dr_pitch(i));
      dr_north(i) = dr_north(i-1) + (dt(i) * horz_speed * cos(dr_yaw(i)));
      dr_east(i) = dr_east(i-1) + (dt(i) * horz_speed * sin(dr_yaw(i)));
    end
  end  


  if exist( 'do_3d_plot' ) ~= 1
    do_3d_plot=1;
  end
  %
  if (do_3d_plot == 1)
    zoom off
    if (recalc_dr == 1 )
      plot3(dr_east, dr_north, data(:,m_depth))
    else 
      plot3(data(:,m_dr_east), data(:,m_dr_north), data(:,m_depth))
    end
    set(gca, 'zdir', 'reverse', 'zlim', [0 inf] )
    grid on
    axis('equal')
    title(['dead-reckoned vehicle trajectory ', run_name]);
  else  
    subplot(111)
    hold off;
    nn = length(data);
    if (recalc_dr == 1 )
      plot(dr_east, dr_north, '-');
      hold on
      plot(dr_east(1), dr_north(1), 'o');
      plot(dr_east(1:50:nn,:), dr_north(1:50:nn), '+');
    else
      index = find(data(:,m_dr_north));
      plot(data(index,m_dr_east), data(index,m_dr_north), '-');
      hold on
      plot(data(index(1),m_dr_east), data(index(1),m_dr_north), 'o');
      nn = length(index);
      plot(data(index(1:50:nn),m_dr_east), data(index(1:50:nn),m_dr_north), '+');
    end
    if (exist('real_x')==1) 
      plot(data(:,real_y), data(:,real_x), 'm');
      plot(data(1:50:nn,real_y), data(1:50:nn,real_x), 'm+');
    end
    if (( exist('m_lbl_est_north')==1) & (exist('m_lbl_est_east')==1))
      plot(data(:,m_lbl_est_east), data(:,m_lbl_est_north), 'w.')
    end
    if (( exist('m_lbl_northfix')==1) & (exist('m_lbl_eastfix')==1))
      index = find(data(:,m_lbl_eastfix));
      plot(data(index,m_lbl_eastfix), data(index,m_lbl_northfix), 'co')
    end

    if exist('swillcox')
      if (swillcox==1)
         lblbeac('haro96');
      end
    end

%     if exist('jleonard')
%       if (jleonard==1)
%          lblbeac('haro96');
%      end
%     end

    ylabel('north (meters)');
    xlabel('east (meters)');
    grid on
    zoom on
    axis('equal')
    title(['dead-reckoned vehicle trajectory ', run_name]);
  end
end

%% hard-wired axis limits from jleonard 
%% axis([13992 14150 36870 37020])



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% sonar plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
if (exist('make_sonar_plot')==0)  make_sonar_plot = 0; end 
if (make_sonar_plot==1) 
  if (exist('m_sonar_oa2')==1)
    figure(fig)
    fig = fig + 1;
    clf
    set( gcf, 'name', 'Sonar Plot' );

    if (exist('sonar_freq')==0)
      sonar_freq=200
    end      
    if (sonar_freq==500) 
      calt = data(:, m_sonar_oa2).*cos(data(:,m_pitch));
    else 
      calt = data(:, m_sonar_alt).*cos(data(:,m_pitch));
    end

    % end
    if (abennett==1)
      if (exist('real_alt')==1) 
        plot(data(:,m_present_time), data(:,m_depth), 'g.',...
	 data(:,m_present_time), cdepth, 'c', data(:,m_present_time),...
	 data(:,m_depth)+calt+0.3, 'r.', data(:,m_present_time),...
	 data(:,real_alt)+data(:,real_z),'w')
        plot(data(:,m_present_time), data(:,m_depth), 'g.',...
	 data(:,m_present_time), cdepth, 'c', data(:,m_present_time),...
	 data(:,m_depth)+calt+0.3, 'r.')
      else
        plot(data(:,m_present_time), data(:,m_depth), 'g.', data(:,m_present_time),...
          cdepth, 'c', data(:,m_present_time), data(:,m_depth)+calt, 'r.')
      end
    else 
      plot(data(:,m_present_time), data(:,m_depth), 'g.', data(:,m_present_time), cdepth, 'c', data(:,m_present_time), data(:,m_depth)+calt, 'r.')
    end
    set(gca, 'ydir', 'reverse')
    grid on
    zoom on
    title(['measured and command depth and calculated bottom ', run_name]);
    xlabel('time (sec)');
    ylabel('depth (m)');
  end 
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% lbl plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (exist('make_lbl_plot')==0)  make_lbl_plot = 1; end 
if (make_lbl_plot==1) 
  if ((exist('m_lbl_1t')==1) & (exist('m_lbl_2t')==1)) 
      figure(fig)
      fig = fig + 1;

      %% npings = length(find(finite(data(:,m_lbl_ping_time))));
      npings = length(find(data(:,m_lbl_ping_time)>0));
      disp('NOTE: For SWB95 ping numbers should be divided by 2');
      disp(['Total number of LBL pings: ', num2str(npings)]);

      %% ind = find(data(:,m_lbl_1t)>0);    % taken out jleonard 10/18/95 
      ind = find(data(:,m_lbl_1t)>0);
      disp(['LBL beacon 1: ',num2str(length(ind)), ' returns ' ...
           num2str(100*length(ind)/npings), ' percent detection rate']);
      clf
      set( gcf, 'name', 'LBL Times Plot' );
      plot(data(ind,m_present_time), data(ind, m_lbl_1t), 'ro')
      hold on
      grid on
      zoom on

      ind = find(data(:,m_lbl_2t)>0);
      disp(['LBL beacon 2: ',num2str(length(ind)), ' returns ' ...
           num2str(100*length(ind)/npings), ' percent detection rate']);
      plot(data(ind,m_present_time), data(ind, m_lbl_2t), 'go')

      ind = find(data(:,m_lbl_3t)>0);
      disp(['LBL beacon 3: ',num2str(length(ind)), ' returns ' ...
           num2str(100*length(ind)/npings), ' percent detection rate']);
      plot(data(ind,m_present_time), data(ind, m_lbl_3t), 'bo')

      ind = find(data(:,m_lbl_4t)>0);
      disp(['LBL beacon 4: ',num2str(length(ind)), ' returns ' ...
           num2str(100*length(ind)/npings), ' percent detection rate']);
      plot(data(ind,m_present_time), data(ind, m_lbl_4t), 'yo')

	title(['LBL travel times ' run_name]);


      if ((exist('m_lbl_t1_valid')==1) & (exist('m_lbl_t2_valid')==1) & ...
        (exist('m_lbl_t3_valid')==1) & (exist('m_lbl_t4_valid')==1))

      

 
        i1 = find(data(:,m_lbl_t1_valid)==1);    
        i2 = find(data(:,m_lbl_t2_valid)==1);    
        i3 = find(data(:,m_lbl_t3_valid)==1);    
        i4 = find(data(:,m_lbl_t4_valid)==1);    

       disp(' '); 
       disp(['LBL beacon 1: ',num2str(length(i1)), ' validated returns ' ...
           num2str(100*length(i1)/npings), ' percent ']);
       disp(['LBL beacon 2: ',num2str(length(i2)), ' validated returns ' ...
           num2str(100*length(i2)/npings), ' percent']);
       disp(['LBL beacon 3: ',num2str(length(i3)), ' validated returns ' ...
           num2str(100*length(i3)/npings), ' percent']);
       disp(['LBL beacon 4: ',num2str(length(i4)), ' validated returns ' ...
           num2str(100*length(i4)/npings), ' percent']);


        figure(fig)
        fig = fig + 1;
        clf
        plot(data(:,1), data(:,m_lbl_t1_valid), 'ro');
        hold on
        plot(data(:,1), data(:,m_lbl_t2_valid), 'go');
        plot(data(:,1), data(:,m_lbl_t3_valid), 'bo');
        plot(data(:,1), data(:,m_lbl_t4_valid), 'yo');
        title('indicator variables for t1, t2, t3, t4 (1=validated)');
        xlabel('time (sec)');
        grid on
        zoom on
     end
  else 
    disp('review: skipping lbl plot because of undefined variable');
  end


end
  




%%%%%%%%%%%%%%%%%%%%%%%%%%%%% real values plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (exist('make_real_plot')==0)  make_real_plot = 1; end 
if (make_real_plot==1) 
  if (exist('real_u')==1) 
      figure(fig)
      fig = fig + 1;
      clf
      set( gcf, 'name', 'Real Values Plot' );      
      if (exist('real_u')) 
        plot(data(:,m_present_time), data(:,real_u))
        grid on
        zoom on
        title(['real_u vs. time ', run_name]);
	xlabel('time (sec)')
	ylabel('real u (m/sec)');
      end
    end
  end
end

%%% 6/19/96 use brad's ctd plots 

[temp,cond]=ctdplot( data(:,m_depth), data(:,m_temp)/1000, data(:,m_cond),'xan0895');


%% ss = salinity( cond, temp, data(:,m_depth));

  figure(fig)
  fig = fig + 1;
  clf

%%  subplot(2,1,1);
  hold off
  plot(data(:,m_present_time), temp);
  grid on
  title('Temperature vs. time');
  xlabel('time (sec)');
  ylabel('temp (deg C)');

%%  subplot(2,1,2);
%%  plot(data(:,m_present_time), ss);
%%  grid on
%%  title('Salinity vs. time');
%%  xlabel('time (sec)');
%%  ylabel('Salinity (psu)');

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CTD Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% cond_temp.m  script file to apply conversion from raw freq
% to conductivity and temperature values 

%%   figure(fig)
%%   fig = fig + 1;
%% 
%%   clf
%%   subplot(211)
%%   plot(data(find(data(:,m_cond)>0), m_present_time), ...
%%        data(find(data(:,m_cond)>0),m_cond));
%%   hold on
%%   grid on
%%   zoom on
%%   title(['raw temp vs. time' run_name]);   %% Backwards !!! 5/96
%%   xlabel('time (s)')
%%   subplot(212)
%%   plot(data(find(data(:,m_temp)>0), m_present_time), ...
%%        data(find(data(:,m_temp)>0),m_temp));
%%   hold on
%%   grid on
%%   zoom on
%%   title(['raw cond vs. time' run_name]);   %% Backwards !!! 5/96
%%   xlabel('time (s)' );

  figure(fig)
  fig = fig + 1;

  dsize = size(data);
  n = dsize(1);
  if (exist('run_name')==0)
    run_name = '';
  end

  time = data(:, m_present_time);
  cond = data(:, m_temp);
  temp = data(:, m_cond);
  depth = data(:, m_depth);

  temp_a = 3.68098579e-03
  temp_b = 6.00005234e-04
  temp_c = 1.65211796e-05
  temp_d = 3.21668085e-06
  temp_f0 = 5789.49

  cond_a = 3.30988162e-06
  cond_b = 3.79709715e-01
  cond_c = -3.75226531e+00
  cond_d = -6.35985296e-05
  cond_m = 4.6

  s = size(temp)
  ff = zeros(s);
  temp1 = zeros(s);
  temp2 = zeros(s);
  cond1 = zeros(s);
  cond2 = zeros(s);
  for i=1:n
    %
    % First do temp conversion
    %
    if (temp(i)~=0) 
      ff(i) = log(temp_f0/temp(i));
      dummy = temp_a + temp_b*ff(i) + temp_c*ff(i)*ff(i) + temp_d*ff(i)*ff(i)*ff(i);
      temp2(i) = 1/(dummy) - 273.15;
    else
      temp2(i) = nan;
    end
    %
    % Now do the conductivity conversion (JL/JWB 4-8-94)
    %
    cond2(i) = (cond_a * (cond(i)/1000)^cond_m + cond_b * (cond(i)/1000)^2 + cond_c + cond_d * temp2(i))/(10);
  end

  subplot(211);
  hold off
  plot(time,temp2)
  hold on
  grid
  title(['temperature (deg C) vs time ',run_name]);
  ylabel('degrees C');
  xlabel('cycle count');
  subplot(212);
  hold off
  plot(time,cond2)
  hold on
  grid
  title(['conductivity (Siemens/meter) vs time ',run_name]);
  ylabel('Siemens/meter');
  xlabel('cycle count');
end


%%%%%%%%%%%%%%%%%%%%%%%%%%% EDC PLots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


if ((exist('edc')==1) & (edc==1))
  review_edc;
end

