%%
%%         SPRING.m
%%
%%    Function called: roc3d. 
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%    Spring: Solve the Pendulum Equations.
%    Both the Exact and the Approximate
%    forms are solved.
%
%       Solve the equations for the
%             Swinging Spring
%       for the 3 dimensional case.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
% global m=1  g = pi^2  k=4*pi^2  l=1 
% global l_0  epsilon  epssq  kappa
% global omega_Z  omega_R
% global elAAA  elBBB  elaaa  elbbb
global m  
global g 
global k 
global l
global l_0
global epsilon
global epssq
global kappa
global omega_Z
global omega_R
global elAAA
global elBBB
global elaaa
global elbbb
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
plotsout=1;
diary Diary
% diary on
% Define the basic (constant) parameters.
m=1; g = pi^2; k=4*pi^2; l=1;
epssq=(m*g)/(k*l);
epsilon=sqrt(epssq)
l_0=(1-epssq)*l;
omega_R = sqrt(g/l);  omega_Z = sqrt(k/m);
tau_R = 2*pi/omega_R, tau_Z = 2*pi/omega_Z
lambda = l_0*omega_Z^2/l^2
kappa = lambda/(2*omega_Z);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Set the Total Number of Timesteps   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
TMAX = 150   %     May be changed below.
skale=1.0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      Set the initial conditions       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ICtype = 5 % Star-shaped pattern in plan.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%   Vacillating Modes  %%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(ICtype==1)
  x_init=0.1;    y_init=0.0;
  z_init=-l+(lambda/(2*omega_Z^2))*x_init^2;  
  % Perturb z_init
  z_pert=1*x_init/10;   z_init=z_init+z_pert;
  
  xdot_init=0.0;    
  ydot_init=x_init*omega_R; 
  zdot_init=0.0;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Unmodulated (Elliptic/Parabolic) Modes%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(ICtype==2)
        aa = 0.05;  bb = aa/5; 
        aa = 0.01;  bb = aa/2; 
        sgn = +1;   %%  sgn=-1 is the other possibility
        dd=sqrt(aa^2+bb^2);
        cc=       +sgn*(aa^2-bb^2)/(2*sqrt(2)*dd);
        omega_1 = -sgn*(3*sqrt(2)*dd/(16*l))*omega_R;
        OMEGA_1 = -2*aa*bb*omega_1/dd^2;
        omega = omega_R+omega_1;
        %%%%%% Set timesteps for 90 degree precession.
        Rot100 = OMEGA_1*100*(180/pi)
        Rot200 = OMEGA_1*200*(180/pi)
        TMAX=abs(round(pi/(2*OMEGA_1)))
        aa, bb, cc, dd, 
        omega_1, OMEGA_1
        x_init=aa ; y_init=0 ; z_init=-l+cc ;
        xdot_init=0.0 ; ydot_init=omega*bb ; zdot_init=0.0;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%   Perturbed Conical Modes      %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(ICtype==3)
  TMAX = 100;
  x_init=0.04;   y_init=0.00; z_init=-l+3*x_init^2/(8*l);
  xdot_init=0.0; ydot_init=x_init*omega_R; zdot_init=0.0;
  % Perturb zdot_init
  zdot_pert = 0.5*ydot_init; zdot_init=zdot_init+zdot_pert;
  gnu = lambda*x_init/(2*omega_Z);
  Mod_period=pi/gnu
end
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Large Amplitude. Chaos       %%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(ICtype==4)
  TMAX = 500;
  x_init=0.40; y_init=0.00; z_init=-l
  xdot_init=0.0; ydot_init=x_init*omega_R; zdot_init=0.0;
end
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Aproximate versus exact solutions  %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(ICtype==5)
  ICmode=0
  if(ICmode==0)
     x_init=0.04*skale;
     y_init=0.00*skale;
     z_init=-l+0.08*skale; zprime_init=z_init+l;
     xdot_init=0.0*skale;
     ydot_init=0.25*x_init*omega_R;
     zdot_init=0.0*skale;
%    Tweak y for tuning Delta-Theta 
     %%  nudge=15/13.75  ; ydot_init=nudge*ydot_init; %%  for skale = 1.
     nudge=1.038     ; ydot_init=nudge*ydot_init; %%  for skale = 0.15.
     alf0 = atan2(-xdot_init,(omega_R*x_init));
     bet0 = atan2(-ydot_init,(omega_R*y_init));
     gam0 = atan2(-zdot_init,(2*omega_R*zprime_init));
     if(abs(cos(alf0))>eps) AA0 = x_init/cos(alf0);
        else AA0 = xdot_init/(-omega_R*sin(alf0));
     end
     if(abs(cos(bet0))>eps) BB0 = y_init/cos(bet0);
        else BB0 = ydot_init/(-omega_R*sin(bet0));
     end
     if(abs(cos(gam0))>eps) CC0 = zprime_init/cos(gam0);
        else CC0 = zdot_init/(-2*omega_R*sin(gam0));
     end
     ICS_xyz = [ x_init xdot_init; y_init ydot_init; zprime_init zdot_init ]
     ICS_ABC = [ AA0 BB0 CC0; alf0 bet0 gam0 ]
  end
  if(ICmode==1)
    %%%     AA0      BB0     CC0    alf0   bet0   gam0 
    ICS = [ 0.04 , 0.0105 , 0.10,  0.000, -pi/2, 0.000 ];
    AA0 = ICS(1); BB0 = ICS(2); CC0 = ICS(3);
    alf0 = ICS(4); bet0 = ICS(5); gam0 = ICS(6); 
    x_init=AA0*cos(alf0);
    y_init=BB0*cos(bet0);
    z_init=-l+CC0*cos(gam0);   zprime_init=z_init+l;
    xdot_init=-omega_R*AA0*sin(alf0);
    ydot_init=-omega_R*BB0*sin(bet0);
    zdot_init=-2*omega_R*CC0*sin(gam0);
    ICS_xyz = [ x_init xdot_init; y_init ydot_init; zprime_init zdot_init ]
    ICS_ABC = [ AA0 BB0 CC0; alf0 bet0 gam0 ]
  end
  
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(ICtype==6)
  printf('ICtype = 6 not permitted. \n');
  return
end
if(ICtype>6)
  printf('ICtype > 6 not permitted. \n');
  return
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial POSITIONS and MOMENTA
x(1) = x_init;    x(3) = y_init;    x(5) = z_init;
x(2) = xdot_init; x(4) = ydot_init; x(6) = zdot_init;
% Initial condition vector
x0 = x;
xyz0=x0(1:2:5)+[0,0,l]; xyzdot0=x0(2:2:6);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h_0=(x_init*ydot_init - y_init*xdot_init)
R_init=sqrt(x_init.^2+y_init.^2)
R_1=sqrt(h_0/omega_R)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    tmax=TMAX
    tmax=round(tmax/skale)
    sps=10;  %  Steps per second.
    numt=sps*tmax+1
    t = linspace(0,tmax,numt)';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      Solve the Exact Equations    %%%%%
[time,xvec] = ode45('roc3d',0,tmax,x0);
t1=t;
xx=xvec(:,1); xxdot=xvec(:,2);
yy=xvec(:,3); yydot=xvec(:,4);
zz=xvec(:,5); zzdot=xvec(:,6);
rr=sqrt(xx.^2+yy.^2+zz.^2); RR=sqrt(xx.^2+yy.^2);
zzdash=l+zz;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  TOTAL ENERGY
Energy=0.5*m*(xxdot.^2+yydot.^2+zzdot.^2)+0.5*k*(rr-l_0).^2+m*g*zz;
Energy0=+0.5*k*(l-l_0).^2+m*g*(-l); Energy=Energy-Energy0;
   E_start  = Energy(1); E_finish = Energy(numt);
   E_ratio = (E_finish/E_start)*100  %  Per cent
   T_Spring = 0.5*m*(zzdot.^2);
   V_Spring = 0.5*k*zzdash.^2;
   X_Spring = 0.5*lambda*RR.^2.*zzdash;
E_Spring = T_Spring+V_Spring-X_Spring;
E_Swing = Energy-E_Spring;
%  ANGULAR MOMENTUM (per unit mass)
h = (xx.*yydot-yy.*xxdot);
h_ratio = (h(numt)/h(1))*100  %  Per cent
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Get the parameters of the instantaneous ellipse
%
numtime=length(time)
for n=2:numtime-1
  Maz = [ xx(n-1)^2 , 2*xx(n-1)*yy(n-1) , yy(n-1)^2 ;
          xx(n  )^2 , 2*xx(n  )*yy(n  ) , yy(n  )^2 ;
          xx(n+1)^2 , 2*xx(n+1)*yy(n+1) , yy(n+1)^2 ] ;
  pqr = Maz \ [1;1;1];
  ppp = pqr(1); qqq = pqr(2); rrr = pqr(3);
  azim = 0.5*atan(2*qqq/(ppp-rrr));
  azi(n) = azim;
  denom1= ppp.*cos(azim).^2+qqq.*sin(2*azim)+rrr.*sin(azim).^2;
  denom2= ppp.*sin(azim).^2-qqq.*sin(2*azim)+rrr.*cos(azim).^2;
  Ax_1 = 1/sqrt(denom1); Ax_2 = 1/sqrt(denom2);
  SAmaj = max(Ax_1,Ax_2); SAmin = min(Ax_1,Ax_2);
  Amaj(n) = SAmaj;
  ecc(n)=sqrt(1-(SAmin./SAmaj).^2); SAprod = SAmaj.*SAmin;
end
azi(1) = azi(2) ; azi(numtime) = azi(numtime-1);
ecc(1) = ecc(2) ; ecc(numtime) = ecc(numtime-1);
Amaj(1)=Amaj(2) ; Amaj(numtime)=Amaj(numtime-1);
% Take away jumps of pi/2 in azi. Make it monotone.
dazi=diff(azi);
for n=1:numtime-1
  if(dazi(n)>(+pi/4)) 
    azi(n+1:numtime)=azi(n+1:numtime)-pi/2;
   end
  if(dazi(n)<(-pi/4)) 
    azi(n+1:numtime)=azi(n+1:numtime)+pi/2;
   end
end
%%  Change azimuth to Degrees.
r2d=(180/pi);
azi = azi*(180/pi);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       Plot the solutions.
if(plotsout==1)
%  Plot the horizontal projection.
   whitebg([0,0,0.75]);
   plot(xx,yy,'y');
   axis('equal'); axis('square')
   print -depsc SSstar.eps;
   pause
%  Plot the components and the energy.
   subplot(2,2,1);  plot(time,xx,'y'); title('X');
   subplot(2,2,2);  plot(time,yy,'y'); title('Y');
   subplot(2,2,3);  plot(time,zzdash,'g'); title('Z'); 
   subplot(2,2,4);  plot(time,E_Swing,'y',time,E_Spring,'g',time,Energy,'c'); 
   title('Energy')
   print -depsc SStime.eps;
   pause
% Plot the azimuth of exact and modulation equations.
%  subplot(1,1,1); plot(time,azi,time,Amaj*1000,'g');
%  title('Azimuth and Major Axis (*10^3)');
%  print -depsc SSazim.eps;
%  pause
% Plot the azimuth and horizontal energy.
  E_horiz = 180*E_Swing/max(E_Swing);
  subplot(1,1,1); plot(time,azi,time,E_horiz,'c');
  axis([0,tmax,0,200]);
  title('Azimuth and Horizontal Energy');
  print -depsc SSazen.eps;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
diary off
return