Clean code.
User friendly with-ad.

* use labels to avoid funcall.
Compute generalized mass matrix.
Fix bug.