;; Bouncing ball in Scheme
;; Base on Lagrangian Mechanics
;; By Piotr Mitros
;; Distributed under GPL

(define w1 (frame -50 50 -pi pi))
(define w2 (frame 0 50 -pi pi))
(graphics-enable-buffering w1)

(define (clear) 
  (graphics-clear w1)
  (graphics-clear w2))

(define (close) 
  (graphics-close w1)
  (graphics-close w2))

; We define a half-spring. It behaves as kx on one side, and 
; provides almost no force on the other. For the benefit of 
; analysis, we use a function of arctan to make the transition 
; smooth. As el->infinity, we get the exact function. 
(define (wall k el z) 
  (* (/ k 2) (square z)
     (/ (- pi/2 (atan (* 1000 z))) pi)))

; Now, we write the Lagrangian for the particle
(define ((L-particle m1 k1 k2 g) local)
  (let ((q (coordinate local))
	 (v (velocity local)))
    (let ((z (ref q 0)))
      (- (* 1/2 m1 (+ (square (ref v 0)) (square (ref v 1))))
	 (+ ; (* k1 (square (ref q 1)))
	    (+ (* m1 g z)
	       (wall k2 1000 z)))))))

; We pretty-print the Lagrangian. Use 'se' for LaTeX output. 
(pe ((L-particle 'm1 'k1 'k2 'g)
     (up 't (up 'x 'y) (up 'xdot 'ydot))))

; We computer the equations of motion from the Lagrangian
(define (sysder m1 k1 k2 g)
  (Lagrangian->state-derivative
   (L-particle m1 k1 k2 g)))

; And we simplify and print those
(pe ((sysder 'm1 'k1 'k2 'g)
     (up 't (up 'x 'y) (up 'xdot 'ydot))))

; We omit this, since apparently 'evolve' will now compile 
; automatically
; (define double-sysder-compiled
;   (compile-parametric 5 double-sysder))

(clear)

(define ((monitor-coords w1 w2) state)
  (let ((theta1 ((principal-value pi) (ref (coordinate state) 0)))
	(theta2 ((principal-value pi) (ref (coordinate state) 1))))
    (graphics-clear w1)
    ;(graphics-operation w1 'fill-circle (* .01 theta2) (* .01 theta1) 5)
    (plot-point w1 theta1 theta2)
    (plot-point w2 (time state) theta1)
    (graphics-flush w1)
    state))

(clear)

((evolve sysder  1. .5 100 .1)
 (up 0. (up pi/2 pi) (up 0. 0.)) 
 (monitor-coords w1 w2)
 .01
 50
 1.e-13)