% Plot trajectories and waypoints
figure(1)
plot(yout,xout)
hold on; plot(ywp,xwp,'r+'); 
plot(ywp,xwp,'r'); 
plot(ywp(firstwp),xwp(firstwp),'ro')
plot(yout(1),xout(1),'bo')
xlabel('y-east(ft)')
ylabel('x-north (ft)')
lastwp=length(ywp);
plot(ywp(lastwp),xwp(lastwp),'rs')
last=length(yout);
plot(yout(last),xout(last),'bs')

hold off


figure(2)
subplot(411);
plot(time,xout)
ylabel('x-North(ft)')

subplot(412);
plot(time,yout)
ylabel('y-East(ft)')

subplot(413);
plot(time,cout)
ylabel('Xtrack error(ft)')
xlabel('time')

subplot(414);
plot(time,nextout)
ylabel('next')
xlabel('time')

figure(3)
subplot(321);
plot(time,xout)
ylabel('x-North(ft)')

subplot(323);
plot(time,yout)
ylabel('y-East(ft)')

subplot(322);
plot(time,cout)
ylabel('c(ft)')
xlabel('time')
grid on

subplot(324);
plot(time,cdotout)
ylabel('cdot(ft/s)')
xlabel('time')
grid on

subplot(325);
plot(time,nextout)
ylabel('next')
xlabel('time')

subplot(326);
plot(time,PhiComLimited)
ylabel('PhiComLimited')
xlabel('time')