
clear
clc

load data64
ErrorPsiIC=0;

% Crosswind (ft/sec) in NED components
% x = north, y = east, z = down
Vwx=sin(45/180*pi)*0;
Vwy=-sin(45/180*pi)*0;

% Straight Ahead
xwp=[0 2000];
ywp=[0 0];

% Turn
dist=750;
turn=80/180*pi;
xwp=[0 200 (dist/2)+200 dist+200];
ywp=[0 0 (tan(turn)*dist/2) tan(turn)*dist];

% % Sharp Turn
% xwp=[0 200 200];
% ywp=[0 0 2000];

% % Oval
% xwp=[0 500 1000 500 -500 -1000 -500 0];
% ywp=[0 0 500 1000 1000 500 0 0];


% % Double Turn
% xwp=[0 200 1200 3000];
% ywp=[0 0 1732 1732];

% %Circle
% r=500;
% n=8;
% eta=2*pi;
% xwp(1)=0; ywp(1)=0;
% for i=0:n
%     theta=eta/n*i;
%     xwp(i+2)=r*sin(theta)+300;
%     ywp(i+2)=r-r*cos(theta);
% end

% % Figure 8, need circle
% r=-r;
% eta=-eta;
% for i=n+1:2*n
%     theta=eta/n*i;
%     xwp(i+1)=r*sin(theta)+100;
%     ywp(i+1)=r-r*cos(theta);
% end

% xwp=[0 200 1200 3000];
% ywp=[0 0 1732 1732];

plot(ywp,xwp,'o--')
hold on
axis equal
grid

if sqrt(Vwx^2+Vwy^2)>0
    plot_wind
end