; from project 3
; 8.351j
; bryan newbold

; from section 1.6.2

(define ((T-pend m l g ys) local)
  (let ((t (time local))
	(theta (coordinate local))
	(thetadot (velocity local)))
    (let ((vys (D ys)))
      (* 1/2 m
	      (+ (square (* 1 thetadot))
		 (square (vys t))
		 (* 2 l (vys t) thetadot (sin theta)))))))

(define ((V-pend m l g ys) local)
  (let ((t (time local))
	(theta (coordinate local)))
    (* m g (- (ys t) (* l (cos theta))))))

(define L-pend (- T-pend V-pend))

(define ((periodic-drive amplitude frequency phase) t)
  (* amplitude (cos (+ (* frequency t) phase))))

(define (L-periodically-driven-pendulum m l g a omega)
  (let ((ys (periodic-drive a omega 0)))
    (L-pend m l g ys)))

(define Hamiltonian->state-derivative
  phase-space-derivative)

;(se ((Lagrangian->Hamiltonian
;      (L-periodically-driven-pendulum 'm 'l 'g 'a 'omega))
;     (up 't 'theta 'p_theta)))

(define (H-pend-sysder m l g a omega)
  (Hamiltonian->state-derivative
   (Lagrangian->Hamiltonian
    (L-periodically-driven-pendulum m l g a omega))))

(define ((monitor-p-theta win) state)
  (let ((q ((principal-value :pi) (coordinate state)))
	(p (momentum state)))
    (plot-point win q p)))

(define window (frame :-pi :pi -100.0 100.0))

(let ((m 1.)
      (l 9.8)
      (g 9.8)
      (A 0.01)
      (omega 600))
  ((evolve H-pend-sysder m l g A omega)
   (up 0.
       1.5367
       0.)
   (monitor-p-theta window)
   .00001
   .6
   1.0e-12))

(define (driven-pendulum-map m l g A omega)
  (let ((advance (state-advancer H-pend-sysder m l g A omega))
	(map-period (/ :2pi omega)))
    (lambda (theta ptheta return fail)
      (let ((ns (advance
		 (up 0 theta ptheta)
		 map-period)))
	(return ((principal-value :pi) (coordinate ns))
		(momentum ns))))))

(graphics-close window)
(start-gnuplot "output")