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)
position = [0 0 .979 .935];


if exist( 'wbAirplanePtr' ) ~= 1
  wbAirplanePtr= figure;
  set( wbAirplanePtr, 'Name', 'LR Nav', 'DeleteFcn', 'clear wbAirplanePtr' );
  set(gcf,'units','normalized')
  set(gcf,'position',position)
  if(exist('fontSize') == 1) 
    wbAirplaneFontsize=fontSize;
  else
    wbAirplaneFontsize=20;
  end
  wbAirplaneFontweight='normal';
  set(gca,'fontsize',wbAirplaneFontsize,'fontweigh',wbAirplaneFontweight)
  set(gcf,'defaultaxesfontsize',wbAirplaneFontsize,'defaulttextfontsize', ...
      wbAirplaneFontsize)
  wbAirplaneAxesPtr = gca;
else
 figure(wbAirplanePtr);
end

subplot(2,1,1),
a=200,
n=.25,
x=[0:499]/499*a;
y = a/2*((x/a).^(1-n) - (x/a).^(n+1));
plot(x, y);
% f = y/x
f =  ((x/a).^(-n) - (x/a).^(n))/2;
theta = atan(f);
%theta = atan(a/2*((x/a).^(-n) - (x/a).^(n)));
hold on
ssample = 10;
xsub=x(10:ssample:end);
ysub=y(10:ssample:end);
thetasub = theta(10:ssample:end);
quiver(xsub,ysub,cos(thetasub-pi),sin(thetasub-pi),.1);
grid on
axis equal
ylabel('Meters');
thand=title(['\bf Pure Pursit Airplane in Cross Wind, $w/v$ = ', num2str(n)],...
    'interpreter','latex');
legend('\bf Trajectory','\bf Heading $\psi$','interpreter','latex','interpreter','latex');

sphand2=subplot(2,1,2);
thetaprime = 1./( 1 + f.^2)*(-n/2/a).*((x/a).^(-n-1) + (x/a).^(n-1));
phand2=plot(x, thetaprime*180/pi,'r');
grid on;
thetadiff = diff(theta)./diff(x);
hold on
%plot(x(2:end),thetadiff,'--r')
%set(gca,'ylim',[-5 0]);
phand3=plot(x,thetaprime*180/pi*.8/sqrt(2),'--r');
%ylabel('Degrees/Meter');
xlabel('Meters');
legend('Degrees/Meter','Approx. Degrees/Second');
set(gca,'yminortick','on','yminorgrid','on');
set(gca,'xminortick','on','xminorgrid','on');
thand2=title('\bf Rate of Change of Heading, $d\psi/dx$ and $\approx d\psi/dt$','interpreter','latex');

linkaxes(get(gcf,'children'),'x')
