clear
% Set parameters
%
beta=20;  % spring constant (larger = stiffer)
damp=0.0; % damping constant in du/dt equation
filt=0.1; % This is a filter used in the leap-frog time scheme. No need to change.
dt=0.01;  % nondimensional time step
npoints=100;  %number of points in time plots
pi=acos(-1);
%
% Initial conditions
%
r_initial=1.1;   % nondimensional initial radius
the_initial=0;   % initial angle from vertical, in degrees
u_initial=0;     % nondimensional initial radial velocity
v_initial=0;     % nondimensional initial tangential velocity
%
% Stop time
%
tstop=40.0;      % Time to stop integration
%
nt=floor(tstop/dt);
nint=ceil(nt./npoints);
%
% Array initialization
%
r(1:nt)=0;
the(1:nt)=0;
u(1:nt)=0;
v(1:nt)=0;
the_static=the;
v_static=v;
r_qs(1:nt)=0;
the_qs(1:nt)=0;
v_qs(1:nt)=0;
u_qs(1:nt)=0;
%
% Set initial conditions
%
for i=1:nt,
   time(i)=dt.*i;
   r_static(i)=1;
end   
the(1)=the_initial.*pi./180;
r(1)=r_initial;
u(1)=u_initial;
r_qs(1)=beta.*(r_initial-1);
%
%  Set initial azimuthal velocity to specified value;
%
%v(1)=v_initial;
%
%  or to "initialized" value that minimizes high frequency oscillations
%
v(1)=-sqrt((1+beta.*(r_initial-1)-cos(the(1))));
%
the_static(1)=the(1);
v_static(1)=v(1);
the_qs(1)=the(1);
v_qs(1)=v(1);
u_qs(1)=-3.*v_qs(1).*sin(the(1))./beta;
u(1)=u_qs(1);
%
% First time step
%
r(2)=r(1)+u(1).*dt;
the(2)=the(1)+dt.*v(1)./r(1);
u(2)=u(1)+dt.*(v(1).*v(1)./r(1)+cos(the(1))-1+beta.*(1-r(1))-damp.*u(1));
v(2)=v(1)+dt.*(-sin(the(1))-v(1).*u(1)./r(1));
the_static(2)=the_static(1)+dt.*v_static(1);
v_static(2)=v_static(1)-dt.*sin(the_static(1));
v_qs(2)=v_qs(1)+dt.*(-sin(the_qs(1))-v_qs(1).*u_qs(1)./(1+r_qs(1)./beta));
the_qs(2)=the_qs(1)+dt.*v_qs(1)./(1+r_qs(1)./beta);
u_qs(2)=-3.*sin(the_qs(2)).*v_qs(2)./beta;
r_qs(2)=v_qs(2).^2+cos(the_qs(2))-1;
%
% Leap frog in time
%
for i=3:nt;
   r(i)=r(i-2)+2.*dt.*u(i-1);
   the(i)=the(i-2)+2.*dt.*v(i-1)./r(i-1);
   u(i)=u(i-2)+2.*dt.*(v(i-1).^2./r(i-1)+cos(the(i-1))-1+beta.*(1-r(i-1))-damp.*u(i-2));
   v(i)=v(i-2)+2.*dt.*(-sin(the(i-1))-v(i-2).*u(i-1)./r(i-1));
   the_static(i)=the_static(i-2)+2.*dt.*v_static(i-1);
   v_static(i)=v_static(i-2)-2.*dt.*sin(the_static(i-1));
   v_qs(i)=v_qs(i-2)+2.*dt.*(-sin(the_qs(i-1))-v_qs(i-2).*u_qs(i-1)./(1.+r_qs(i-1)./beta));
   the_qs(i)=the_qs(i-2)+2.*dt.*v_qs(i-1)./(1+r_qs(i-1)./beta);
   u_qs(i)=-3.*sin(the_qs(i))*v_qs(i)./beta;
   r_qs(i)=v_qs(i).^2+cos(the_qs(i))-1;
   %u_qs(i)=-v_qs(i).*(1./beta).*sin(the_qs(i)).*(2.*(1+r_qs(i)./beta)+1)./(1+r_qs(i)./beta+2.*v_qs(i).^2);
   %
   % Apply filter
   %
   r(i-1)=r(i-1)+filt.*(r(i-2)+r(i)-2.*r(i-1));
   the(i-1)=the(i-1)+filt.*(the(i-2)+the(i)-2.*the(i-1));
   u(i-1)=u(i-1)+filt.*(u(i-2)+u(i)-2.*u(i-1));
   v(i-1)=v(i-1)+filt.*(v(i-2)+v(i)-2.*v(i-1));
   the_static(i-1)=the_static(i-1)+filt.*(the_static(i-2)+the_static(i)-2.*the_static(i-1));
   v_static(i-1)=v_static(i-1)+filt.*(v_static(i-2)+v_static(i)-2.*v_static(i-1));
   v_qs(i-1)=v_qs(i-1)+filt.*(v_qs(i-2)+v_qs(i)-2.*v_qs(i-1));
   the_qs(i-1)=the_qs(i-1)+filt.*(the_qs(i-2)+the_qs(i)-2.*the_qs(i-1));
end
%
% Resample
%
timeplot=time(1:nint:nt);
theplot=the(1:nint:nt).*180./pi;
rplot=r(1:nint:nt);
the_staticplot=the_static(1:nint:nt).*180./pi;
r_static_plot=r_static(1:nint:nt);
the_qsplot=the_qs(1:nint:nt).*180./pi;
r_qsplot=1+r_qs(1:nint:nt)/beta;
%
% Plot solution
%
h=plot(timeplot,theplot,timeplot,the_staticplot,timeplot,the_qsplot);
xlabel('Time','fontweight','bold')
ylabel('Angle (degrees)','fontweight','bold')
legend('Full solution','Static approximation','Quasi-static approximation')
pause
h=plot(timeplot,rplot,timeplot,r_static_plot,timeplot,r_qsplot);
xlabel('Time','fontweight','bold')
ylabel('Radius','fontweight','bold')
legend('Full solution','Static approximation','Quasi-static approximation')



