!        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 noptp = 1
let q = 1
!let w_d = 1
let G = 0.02
let tstep = 0.001
let tmax = 750

!input prompt "Input 0 for print, 1 for plot:": noptp
!input prompt "Input amplitude: ": q
input prompt "Input driving frequency (w_d/w_0): ": w_d
!input prompt "Input driving amplitude (G/f_0): ": G
!input prompt "Input time step 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)
end if
if noptp = 0 then
     print "time","position","velocity"
end if
for t = 0 to tmax step tstep
     call rk4(q,w,t,qnew,wnew,tstep,w_d,G)

     ! pause .01
     if noptp = 1 then 
          plot q,w;
     else
          print t,q,w
     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

next t
end
!
def alpha(q,w,t, w_d, G)
     let Q_0 = 10
     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