!        PHSPshl
!
! program to plot phase space trajectories for the simple
!   harmonic oscillator.
! this program will either print out the values of position, x
!   and velocity, v, or plot them.
!
declare def alpha

let w_d = 2/3
let t = 0
let imax = 1000000
let eps = 1e-8
let noptp = 1
let q = 0
let tstep = 1
let tmax = 200
let G = 1.5
let w = 0

!input prompt "Input 0 for print, 1 for plot:": noptp
!input prompt "Input amplitude: ": q
!input prompt "Input w: ": w
!input prompt "Input driving frequency (w_d/w_0): ": w_d
!input prompt "Input driving amplitude (G/f_0): ": G
!input prompt "Input tstep and max. time:": tstep, tmax

! Setup the second solution
let q2 = q + 0.1
let w2 = w

if noptp = 1 then
     open #1: screen 0,.8,.2,1        ! Open state space window
     call axes(-4,4,-3,3)             ! Set axes in state space window
     plot text, at 2.5,2.5: "G ="     ! Write value of G on screen
     plot text, at 3,2.5: str$(G)
     open #2: screen 0,1,0,.19        ! Open window for s(t)
     call axes(-tmax/100,tmax,-0.1,5) ! Set axes in s window
     plot text, at tmax/50,4.5: "s"   ! Label s window
else
     print "G = ";G
     print "time"," angle ","ang. freq.","separation" 
end if
if noptp = 0 then
     print "time","position","velocity"
end if

for i = 0 to imax
     if(t < tmax) then
          call get_delta(q,w,t,qnew,wnew,tstep,w_d,G, delta)
          call get_delta(q2,w2,t,q2new,w2new,tstep,w_d,G, delta2)

          let delta = max(delta, delta2)

          if delta < eps then
               call rk4(q,w,t,qnew,wnew,tstep,w_d,G)
               call rk4(q2,w2,t,q2new,w2new,tstep,w_d,G)
               
               let s1 = sqr((q2 - q)^2 + (w2 - w)^2) ! Calculate separation modulo
               let s2 = sqr((q2 - q - 2*pi)^2 + (w2 - w)^2) ! any possible 2*pi’s
               let s3 = sqr((q2 - q + 2*pi)^2 + (w2 - w)^2)
               let smin = min(s2,s3) ! Choose the min value of the
               let s = min(smin,s1) ! three possible s’s

               ! pause .001
               if noptp = 1 then 
                    window #1
                    plot q,w; ! Plot state space step for traj 1
                    plot qnew,wnew
                    plot q2,w2; ! Plot state space step for traj 2
                    plot q2new,w2new
                    window #2
                    plot t,s;
                    pause 0.0001 ! This allows trajectories to be viewed on screen as they develop
               else
                    print t,q,w, tstep
               end if

               if wnew*w < 0 then ! This is code to determine the period
                    let qpnew = qnew
                    let wpnew = wnew
                    let tpstep = tstep*(0-w)/(wnew-w)
                    call rk4(q,w,t,qpnew,wpnew,tpstep,w_d,G)

                    let tzero = t+tstep*(0-w)/(wnew-w) ! Note that it prints every half period
                    ! print "w=0 at time = ";tzero;"amp = ";qpnew
               end if

               let q = qnew
               let w = wnew
               
               let q2 = q2new
               let w2 = w2new

               let t = t + tstep
               let tstep = tstep * 0.95 * (eps / delta)^(0.20)

               if q > pi then
                    let q = q - 2*pi
                    plot
               end if
               if q < -pi then
                    let q = q + 2*pi
                    plot
               end if
               
               if q2 > pi then
                    let q2 = q2 - 2*pi
                    plot
               end if
               if q2 < -pi then
                    let q2 = q2 + 2*pi
                    plot
               end if
          else
               let tstep = tstep * 0.95 * (eps / delta)^(0.25)
          end if
     else 
          let i = imax + 1
     end if
next i
end
!
def alpha(q,w,t, w_d, G)
     let Q_0 = 2
     let alpha = -sin(q) - (w / Q_0) + G * cos(w_d * t)
     ! let alpha = -sin(q)
end def
!
sub get_delta(q,w,t,qnew,wnew,tstep,w_d,G, delta)
     !!!!!!!!!!!!!!!!!!!!!
     !!!!! get delta !!!!!
     !!!!!!!!!!!!!!!!!!!!!

     ! get q w and t at the next step
     call rk4(q,w,t,qfnew,wfnew,tstep,w_d,G)

     ! get q w and t at the next two steps, half the size
     let tstep_half = tstep/2
     call rk4(q,w,t,qhnew,whnew,tstep_half,w_d,G)
     call rk4(qhnew,whnew,t+tstep_half,qhnew2,whnew2,tstep_half,w_d,G)

     ! get delta
     let q_delta = abs(qfnew-qhnew2)
     let w_delta = abs(wfnew-whnew2)
     let delta = max(q_delta,w_delta)

     ! if delta = 0, for debugging
     if delta = 0 then
          print "delta = 0"
     end if 
end sub
!
sub rk4(q,w,t,qnew,wnew,tstep,w_d,G)
     declare def alpha

     ! get k's
     let q_k1 = tstep*w
     let w_k1 = tstep*alpha(q,w,t, w_d, G)
     let q_k2 = tstep*(w + 0.5*w_k1)
     let w_k2 = tstep*alpha(q+0.5*q_k1,w+0.5*w_k1,t+0.5*tstep, w_d, G)
     let q_k3 = tstep*(w + 0.5*w_k2)
     let w_k3 = tstep*alpha(q+0.5*q_k2,w+0.5*w_k2,t+0.5*tstep, w_d, G)
     let q_k4 = tstep*(w + w_k3)
     let w_k4 = tstep*alpha(q+q_k3,w+w_k3,t+tstep, w_d, G)

     ! get new q and w
     let qnew = q + (q_k1 + 2*q_k2 + 2*q_k3 + q_k4)/6
     let wnew = w + (w_k1 + 2*w_k2 + 2*w_k3 + w_k4)/6

end sub
!
!
sub axes(xmin,xmax,ymin,ymax)
     let xdel=xmax-xmin
     let xb=xdel/10000
     let ydel=ymax-ymin
     let yb=ydel/10000
     set window xmin-xb,xmax+xb,ymin-yb,ymax+yb
     plot 0,0;
     plot 0,ymax;
     plot 0,ymin
     plot xmin,0;
     plot xmax,0
     let xtstep=xdel/8
     let xtsize=ydel/60
     for x=0 to xmax step xtstep            !tick marks on x-axis
          plot x,xtsize;
          plot x,0
     next x
     for x = 0 to xmin step -xtstep
          plot x,xtsize;
          plot x,0
     next x
     let ytstep=ydel/6
     let ytsize=xdel/100
     for y=0 to ymax step ytstep            !tick marks on y-axis
          plot 0,y;
          plot ytsize,y
     next y
     for y=0 to ymin step -ytstep
          plot 0,y;
          plot ytsize,y
     next y
end sub