%% Key is pitch and elevator angle
time=data(:,m_present_time); pres = data(:,m_pressure); 
mdepth = data(:,m_depth); cdepth = data(:,c_depth);
mpitch = data(:,m_pitch)*(180/pi); cpitch = data(:,c_pitch)*(180/pi); 
melang = data(:,m_elevator_angle)*(180/pi); celang = data(:,c_elevator_angle)*(180/pi);

sub = [1 50 100:200:length(time)];
addpath '/u/ryjo/entropy/matlab'

figure(1); clf; paper 
%%------------------------
%%   Depth, pressure
%%------------------------
subplot(311);
p=plot(time(sub), pres(sub), 'go'); set(p,'color',[.3 .7 .3]);
set(gca,'YaxisLocation','right','box','off','YColor',[.3 .7 .3]);
set(gca,'fontweight','bold','fontsize',12);
set(gca,'Ydir','reverse','Ylim',[0 32]);
ylabel('Pressure (dB)'); 
title('DEPTH / PRESSURE');
AX = get(gca,'position');
hold on
axes('position',AX);
p=plot(time, mdepth, 'k'); set(p,'linewidth',2);
hold on
p=plot(time, cdepth, 'r'); set(p,'linewidth',2);
set(gca,'YaxisLocation','left','box','off','color','none');
set(gca,'fontweight','bold','fontsize',12);
set(gca,'Ydir','reverse','Ylim',[0 320]);
ll=legend('measured','commanded'); P=get(ll,'position'); set(ll,'position',P+[-.1 0 0 0]);
ylabel('Depth (m)'); 
axes('position',AX);
set(gca,'box','on','XTick',[],'YTick',[],'color','none');
t=text(.03,.12,['max attained : ' num2str(max(pres)) ' dB,  ' num2str(max(mdepth)) ' m']);
set(t,'fontweight','bold','fontsize',12);
t=text(.03,.2,['max commanded depth : ' num2str(max(cdepth)) ' m']);
set(t,'fontweight','bold','fontsize',12,'color','r');

%%------------------------
%%   Pitch 
%%------------------------
subplot(312);
p=plot(time, mpitch, 'k'); set(p,'linewidth',2);
hold on
% p=plot(time, mipitch, 'g.'); 
p=plot(time(sub), cpitch(sub), 'r+'); set(p,'linewidth',2);
set(gca,'fontweight','bold','fontsize',12);
set(gca,'Ylim',[-40 40]);
ylabel(['Pitch (' setstr(176) ')']); 
ll=legend('measured','commanded'); P=get(ll,'position'); set(ll,'position',P+[-.2 0 0 0]);
title('PITCH');

%%------------------------
%%   Elevator angle 
%%------------------------
subplot(313);
p=plot(time, melang, 'k'); set(p,'linewidth',2);
hold on
p=plot(time(sub), celang(sub), 'r+'); set(p,'linewidth',2);
set(gca,'fontweight','bold','fontsize',12);
set(gca,'Ylim',[-5 10]);
ylabel(['Elevator Angle (' setstr(176) ')']); xlabel('Mission time (seconds)');
title('ELEVATOR ANGLE');

% ll=legend('measured','commanded'); P=get(ll,'position'); set(ll,'position',P+[0 -.1 0 0]);
% idx = find(time > 100); 
% t=text(100,-3e4,'Elevator angle, after 100 s : '); set(t,'fontweight','bold','fontsize',12);
% str = ['measured : ' num2str(mean(melang(idx))) setstr(176) ' (' num2str(std(melang(idx))) setstr(176) ' stdev)'];
% t=text(100,-4e4,str); set(t,'fontweight','bold','fontsize',12);
% str = ['commanded : ' num2str(mean(celang(idx))) setstr(176) ' (' num2str(std(celang(idx))) setstr(176) ' stdev)'];
% t=text(100,-4.7e4,str); set(t,'fontweight','bold','fontsize',12);


%%%===========================================================
%% Other control variables
% myaw = data(:,m_yaw); mcompyaw = data(:,m_compass_yaw);
myaw = data(:,m_yaw)*(180/pi); % mcompyaw = data(:,m_compass_yaw)*(180/pi);
mroll = data(:,m_roll)*(180/pi); % miroll = data(:,m_inclino_roll)*(180/pi);
mrudang = data(:,m_rudder_angle)*(180/pi); crudang = data(:,c_rudder_angle)*(180/pi);
mtc = data(:,m_thruster_current); mps = data(:,m_propeller_speed);

figure(2); clf; paper 
%%------------------------
%%   VEHICLE THRUST 
%%------------------------
subplot(411);
p=plot(time, mps, 'k'); set(p,'color',[.3 .7 .3],'linewidth',2);
set(gca,'YaxisLocation','right','box','off','YColor',[.3 .7 .3]);
set(gca,'fontweight','bold','fontsize',12);
ylabel('measured propeller speed'); 
title('VEHICLE THRUST');
AX = get(gca,'position');
hold on
axes('position',AX);
p=plot(time, mtc, 'k.'); % set(p,'linewidth',2);
set(gca,'YaxisLocation','left','box','off','color','none');
set(gca,'fontweight','bold','fontsize',12);
ylabel('measured thruster current'); 
axes('position',AX);
set(gca,'box','on','XTick',[],'YTick',[],'color','none');

%%------------------------
%%   YAW 
%%------------------------
subplot(412);
p=plot(time, myaw, 'k'); set(p,'linewidth',2);
hold on
% p=plot(time(sub), mcompyaw(sub), 'b+'); 
set(gca,'fontweight','bold','fontsize',12);
set(gca,'Ylim',[0 100]);
ylabel(['Yaw (' setstr(176) ')']); 
ll=legend('m yaw','m compass yaw'); P=get(ll,'position'); set(ll,'position',P+[0 -.1 0 0]);
title('YAW');

%%------------------------
%%   Rudder angle 
%%------------------------
subplot(413);
p=plot(time, mrudang, 'k'); set(p,'linewidth',2);
hold on
p=plot(time(sub), crudang(sub), 'r+'); set(p,'linewidth',2);
set(gca,'fontweight','bold','fontsize',12);
set(gca,'Ylim',[-10 10]);
ll=legend('measured','commanded'); P=get(ll,'position'); set(ll,'position',P+[0 0 0 0]);
ylabel(['Rudder Angle (' setstr(176) ')']); 
title('RUDDER ANGLE');

%%------------------------
%%   Roll 
%%------------------------
subplot(414);
p=plot(time, mroll, 'k'); set(p,'linewidth',2);
hold on
% p=plot(time(sub), miroll(sub), 'b+'); set(p,'linewidth',2);
set(gca,'fontweight','bold','fontsize',12);
set(gca,'Ylim',[-10 10]);
ll=legend('m roll','m inclino roll'); P=get(ll,'position'); set(ll,'position',P+[0 0 0 0]);
ylabel(['Roll (' setstr(176) ')']); xlabel('Mission time (seconds)');
title('ROLL');
