;;-*-Lisp-*-
(in-package goal)
(bundles "ENGINE.CGO" "GAME.CGO")
(require "engine/math/matrix.gc")
(require "engine/math/transform-h.gc")

;; Quaternion-backed transformq and trsq are used more often than these Euler forms.

;; DECOMP BEGINS

(defmethod print ((this transform))
  (format #t "#<transform @ #x~X~%" this)
  (format #t "~T~Ttrans:~F ~F ~F ~F ~%" (-> this trans x) (-> this trans y) (-> this trans z) (-> this trans w))
  (format #t "~T~Trot:  ~F ~F ~F ~F ~%" (-> this rot x) (-> this rot y) (-> this rot z) (-> this rot w))
  (format #t "~T~Tscale:~F ~F ~F ~F>" (-> this scale x) (-> this scale y) (-> this scale z) (-> this scale w))
  this)

(defmethod new trs ((allocation symbol) (type-to-make type))
  "Allocate an identity trs."
  (let ((this (object-new allocation type-to-make (the-as int (-> type-to-make size)))))
    (set! (-> this trans w) 1.0)
    (set! (-> this rot w) 1.0)
    (vector-identity! (-> this scale))
    this))

(defun transform-matrix-calc! ((tf transform) (dst-mat matrix))
  "Build dst-mat as S * Rz * Rx * Ry * T from tf. This routine favors simple composition over speed."
  (let ((step-mat (new-stack-matrix0))
        (accum-mat (new-stack-matrix0)))
    (matrix-identity! dst-mat)
    (matrix-translate! dst-mat (-> tf trans))
    ;; Y is multiplied nearest translation, keeping yaw world-aligned.
    (matrix-rotate-y! step-mat (-> tf rot y))
    (matrix*! accum-mat step-mat dst-mat)
    (matrix-rotate-x! step-mat (-> tf rot x))
    (matrix*! dst-mat step-mat accum-mat)
    (matrix-rotate-z! step-mat (-> tf rot z))
    (matrix*! accum-mat step-mat dst-mat)
    (matrix-scale! step-mat (-> tf scale))
    (matrix*! dst-mat step-mat accum-mat)))

(defun transform-matrix-parent-calc! ((tf transform) (dst-mat matrix) (inv-scale vector))
  "Build dst-mat as S * Rz * Rx * Ry * inverse-scale * T for parent-relative composition."
  (let ((step-mat (new-stack-matrix0))
        (accum-mat (new-stack-matrix0)))
    (matrix-identity! accum-mat)
    (matrix-translate! accum-mat (-> tf trans))
    (matrix-inv-scale! step-mat inv-scale)
    (matrix*! dst-mat step-mat accum-mat)
    (matrix-rotate-y! step-mat (-> tf rot y))
    (matrix*! accum-mat step-mat dst-mat)
    (matrix-rotate-x! step-mat (-> tf rot x))
    (matrix*! dst-mat step-mat accum-mat)
    (matrix-rotate-z! step-mat (-> tf rot z))
    (matrix*! accum-mat step-mat dst-mat)
    (matrix-scale! step-mat (-> tf scale))
    (matrix*! dst-mat step-mat accum-mat)))

(defun trs-matrix-calc! ((tf trs) (dst-mat matrix))
  "Build a matrix from a basic trs, which shares transform's trans/rot/scale layout."
  (transform-matrix-calc! (the-as transform (-> tf trans)) dst-mat))