!        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
let phi = 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 phi: ": phi
!input prompt "Input tstep and max. time:": tstep, tmax

if noptp = 1 then
     clear
     let xmin = -1.33333*q
     let xmax = 1.33333*q
     let ymin = -1.1*q
     let ymax = 1.1*q
     ! let xmin = -0.4
     ! let xmax = 0.4
     ! let ymin = -0.3
     ! let ymax = 0.3
  !call axes(xmin,xmax,ymin,ymax)
  call axes(-4,4,-3,3)
  plot text, at 2.5,2.5: "G ="
  plot text, at 3.0,2.5: str$(G)
end if
if noptp = 0 then
     print "time","position","velocity"
end if

for i = 0 to imax
     if(t < tmax) then
          !!!!!!!!!!!!!!!!!!!!!
          !!!!! 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 

          if delta < eps then
               call rk4(q,w,t,qnew,wnew,tstep,w_d,G)

               ! pause .001
               if noptp = 1 then 
                    plot q,w;
               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 wt1 = mod(2*t/3 - phi,2*pi)
               let wt2 = mod(2*(t + tstep)/3 - phi,2*pi)
               
               if wt2 < wt1 then
                    let tpstep = tstep * ( (2 * pi - wt1) / (2 * pi - wt1 + wt2) )
                    call rk4(q,w,t,qpnew,wpnew,tpstep, w_d,G)
                    if noptp = 1 then
                         set color "black"
                         plot qpnew,wpnew
                    else
                         print t,qpnew,wpnew
                    end if
               end if

               let q = qnew
               let w = wnew
               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
          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 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