;;; An unconstrained Lagrangian

(define Lu (literal-function 'Lu (Lagrangian)))

#|
(((Lagrange-equations Lu)
  (literal-function 'q))
 't)
#|
(+
 (* (((partial 2) ((partial 2) Lu)) (up t (q t) ((D q) t)))
    (((expt D 2) q) t))
 (* (((partial 1) ((partial 2) Lu)) (up t (q t) ((D q) t)))
    ((D q) t))
 (((partial 0) ((partial 2) Lu)) (up t (q t) ((D q) t)))
 (* -1 (((partial 1) Lu) (up t (q t) ((D q) t)))))
|#
|#

;;; A function of time and coordinates 

(define (phi state)
  (let ((t (time state))
	(q (coordinate state))
	(v (velocity state)))
    ((literal-function 'phi
		       (-> (UP Real Real) Real))
     (up t q))))


;;; A Lagrangian that incorporates the constraint phi=0

(define (Lp estate)
  (let ((t (time estate))
	(qlam (coordinate estate))
	(qdlamd (velocity estate)))
    (let ((q (ref qlam 0))
	  (lam (ref qlam 1))
	  (qd (ref qdlamd 0))
	  (lamd (ref qdlamd 1)))
      (let ((state (up t q qd)))
	(+ (Lu state)
	   (* lam (phi state)))))))

#|
(((Lagrange-equations Lp)
  (up (literal-function 'q)
      (literal-function 'lambda)))
 't)
#|
(down
 (+
  (* (((partial 2) ((partial 2) Lu)) (up t (q t) ((D q) t)))
     (((expt D 2) q) t))
  (* (((partial 1) ((partial 2) Lu)) (up t (q t) ((D q) t)))
     ((D q) t))
  (((partial 0) ((partial 2) Lu)) (up t (q t) ((D q) t)))
  (* -1 (((partial 1) Lu) (up t (q t) ((D q) t))))
  (* -1 (((partial 1) phi) (up t (q t))) (lambda t)))
 (* -1 (phi (up t (q t)))))
|#
|#
;;; Note that the Lagrange equations for q are the same as the 
;;; ones for the unconstrained Lagrangian, except for the added
;;; term (* -1 (lambda t) (((partial 1) phi) (up t (q t)))),

;;; The case of a constraint that is a total time derivative of a
;;; function of time and position only.

(define psi (Dt phi))

#|
(psi (up 't 'q 'v))
#|
(+ (((partial 0) phi) (up t q))
   (* (((partial 1) phi) (up t q)) v))
|#
|#


(define (Lpp estate)
  (let ((t (time estate))
	(qlam (coordinate estate))
	(qdlamd (velocity estate)))
    (let ((q (ref qlam 0))
	  (lamp (ref qlam 1))
	  (qd (ref qdlamd 0))
	  (lamd (ref qdlamd 1)))
      (let ((state (up t q qd)))
	(+ (Lu state)
	   (* lamp (psi state)))))))

(((Lagrange-equations Lpp)
  (up (literal-function 'q)
      (literal-function 'lambda-prime)))
 't)
#|
(down
 (+
  (* (((partial 2) ((partial 2) Lu)) (up t (q t) ((D q) t)))
     (((expt D 2) q) t))
  (* (((partial 1) ((partial 2) Lu)) (up t (q t) ((D q) t)))
     ((D q) t))
  (((partial 0) ((partial 2) Lu)) (up t (q t) ((D q) t)))
  (* -1 (((partial 1) Lu) (up t (q t) ((D q) t))))
  (* ((D lambda-prime) t) (((partial 1) phi) (up t (q t)))))
 (+ (* -1 ((D q) t) (((partial 1) phi) (up t (q t))))
    (* -1 (((partial 0) phi) (up t (q t))))))
|#

;;; Note that the Lagrange equations for q are the same as the 
;;; ones for the unconstrained Lagrangian, except for the added
;;; term (* ((D lambda-prime) t) (((partial 1) phi) (up t (q t)))),
;;; which is the same as:
;;; (* ((D lambda-prime) t) (((partial 2) psi) (up t (q t))))

;;; The case of a non-integrable constraint that is linear in velocities.

(define (zeta state)
  (let ((t (time state))
	(q (coordinate state))
	(v (velocity state)))
    (+ ((literal-function 'G_2 (-> (UP Real Real) Real))
	(up t q))
       (* v
	  ((literal-function 'G_1 (-> (UP Real Real) Real))
	   (up t q))))))

#|
(zeta (up 't 'q 'v))
#|
(+ (* v (G_1 (up t q))) (G_2 (up t q)))
|#
|#

(define (Lppp estate)
  (let ((t (time estate))
	(qlam (coordinate estate))
	(qdlamd (velocity estate)))
    (let ((q (ref qlam 0))
	  (lamp (ref qlam 1))
	  (qd (ref qdlamd 0))
	  (lamd (ref qdlamd 1)))
      (let ((state (up t q qd)))
	(+ (Lu state)
	   (* lamp (zeta state)))))))

(((Lagrange-equations Lppp)
  (up (literal-function 'q)
      (literal-function 'lambda-prime)))
 't)
#|
(down
 (+
  (* (((partial 2) ((partial 2) Lu)) (up t (q t) ((D q) t)))
     (((expt D 2) q) t))
  (* (((partial 1) ((partial 2) Lu)) (up t (q t) ((D q) t)))
     ((D q) t))
  (((partial 0) ((partial 2) Lu)) (up t (q t) ((D q) t)))
  (* -1 (((partial 1) Lu) (up t (q t) ((D q) t))))
  (* ((D lambda-prime) t) (((partial 1) phi) (up t (q t)))))
 (+ (* -1 ((D q) t) (((partial 1) phi) (up t (q t))))
    (* -1 (((partial 0) phi) (up t (q t))))))
|#

;;; Note that the Lagrange equations for q are the same as the 
;;; ones for the unconstrained Lagrangian, except for the added
;;; terms
;;;  (* ((D lambda-prime) t) (G_1 (up t (q t))))
;;;  (* (lambda-prime t) (((partial 0) G_1) (up t (q t))))
;;;  (* -1 (lambda-prime t) (((partial 1) G_2) (up t (q t))))
;;; So the traditional stuff about non-holonomic constraints
;;; is equivalent to the augmented Lagrangian only if
;;;  (((partial 0) G_1) (up t (q t)))
;;;      = (((partial 1) G_2) (up t (q t)))
;;; Ugh!  This is exactly the condition on G for psi being
;;; a total time derivative.