#|
  This file is a part of stoe project.
  Copyright (c) 2015 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|#

(uiop:define-package :stoe/maths/quaternion
    (:use :cl
          :stoe/maths/types
          :stoe/maths/vector
          :stoe/maths/matrix)
  (:export #:quat #:conjug #:q* #:quat-to-mat3 #:quat-to-mat4)
  (:import-from :stoe/maths/vector
                #:fill-vector))
(in-package :stoe/maths/quaternion)

(defun make-quaternion (&rest attribs)
  (let ((q (make-instance 'quaternion
                          :array (make-array '(4) :element-type 'single-float))))
    (loop with i = 0
       for attr in attribs
       do (setf i (fill-vector q attr i)))
    q))

(defun from-attribs (x y z w)
  (let ((q (make-quaternion x y z w)))
    (qnormalize q)))

(defun from-axis-and-angle (vec angle)
  (let ((v (normalize vec))
        (sin (coerce (sin (/ angle 2)) 'single-float))
        (cos (coerce (cos (/ angle 2)) 'single-float)))
    (qnormalize (make-quaternion (v* v sin) cos))))

(defmacro quat (&rest attribs)
  (let ((len (length attribs)))
    (assert (or (= len 0) (= len 2) (= len 4)))
    (case len
      (4 `(from-attribs ,@attribs))
      (2 `(from-axis-and-angle ,@attribs))
      (0 `(from-attribs 0.0 0.0 0.0 1.0)))))

(defun conjug (quat)
  (quat (- (x quat)) (- (y quat)) (- (z quat)) (w quat)))

(defun quat-to-mat3 (quat)
  (with-swizzle (x y z w) quat
    (let ((2xx (* 2 x x)) (2yy (* 2 y y)) (2zz (* 2 z z))
          (2xy (* 2 x y)) (2xz (* 2 x z)) (2xw (* 2 x w))
          (2yz (* 2 y z)) (2yw (* 2 y w))
          (2zw (* 2 z w)))
      (mat (- 1 2yy 2zz) (- 2xy 2zw) (+ 2xz 2yw)
           (+ 2xy 2zw) (- 1 2xx 2zz) (- 2yz 2xw)
           (- 2xz 2yw) (+ 2yz 2xw) (- 1 2xx 2yy)))))

(defun quat-to-mat4 (quat)
  (let ((mat (mat4 (quat-to-mat3 quat))))
    (setf (mref mat 3 3) 1.0)
    mat))

(defun qnormalize (q)
  (let ((len (vlength q))
        (quat (make-quaternion 0.0 0.0 0.0 0.0)))
    (loop for i from 0 below 4
       do (setf (vref quat i) (/ (vref q i) len)))
    quat))

(defun q* (&rest q-list)
  (qnormalize (reduce (lambda (q1 q2)
                        (with-swizzle ((ax x) (ay y) (az z) (aw w)) q1
                          (with-swizzle ((bx x) (by y) (bz z) (bw w)) q2
                            (quat (- (+ (* aw bx) (* ax bw) (* ay bz)) (* az by))
                                  (- (+ (* aw by) (* ay bw) (* az bx)) (* ax bz))
                                  (- (+ (* aw bz) (* az bw) (* ax by)) (* ay bx))
                                  (- (* aw bw) (* ax bx) (* ay by) (* az bz))))))
                      q-list)))