(require :unittest "lib/llib/unittest.l")

;;(setq sys::*gc-hook* #'(lambda (a b) (format t "GC! free:~A  total:~A~%" a b)))

(init-unit-test)

(setq i-max 100000)

(deftest test-makecoords-rpy
  (setq vmrss-orig (elt (unix::getrusage 0) 2))
  (dotimes (j 10)
    (dotimes (i i-max)
      (setq ret #f(0 0 0))
      (make-cascoords :pos (float-vector 0 0 0)
                      :rpy (float-vector 0 0 0)))
    ;;
    (setq vmrss (elt (unix::getrusage 0) 2))
    (format *error-output* "~A gc:~A, vmrss:~A -> ~A~%" j (sys::gc) vmrss-orig vmrss)
    (assert (< vmrss (* 2 vmrss-orig)) "make-coords pos/rpy")))

(deftest test-makecoords-rot-1
  (setq vmrss-orig (elt (unix::getrusage 0) 2))
  (dotimes (j 10)
    (dotimes (i i-max)
      (setq ret #f(0 0 0))
      (make-cascoords :pos (float-vector 0 0 0)
                      :rot (unit-matrix 3)))
    ;;
    (setq vmrss (elt (unix::getrusage 0) 2))
    (format *error-output* "~A gc:~A, vmrss:~A -> ~A~%" j (sys::gc) vmrss-orig vmrss)
    (assert (< vmrss (* 2 vmrss-orig)) "make-coords pos/rot (unit-matrix)")))

(deftest test-makecoords-rot-2
  (setq vmrss-orig (elt (unix::getrusage 0) 2))
  (dotimes (j 10)
    (dotimes (i i-max)
      (setq ret #f(0 0 0))
      (make-cascoords :pos (float-vector 0 0 0)
                      :rot (rpy-matrix 0 0 0)))
    ;;
    (setq vmrss (elt (unix::getrusage 0) 2))
    (format *error-output* "~A gc:~A, vmrss:~A -> ~A~%" j (sys::gc) vmrss-orig vmrss)
    (assert (< vmrss (* 2 vmrss-orig)) "make-coords pos/rot (rpy-matrix)")))

(warning-message 1 "test-makecoords-quaternion is not working on eusgl ...
-- undefined function quaternion2matrix , exitting...
-- See jskeus/irteus/coords.l~%")

#-:word-size=64
(defvar *current-eps* *epsilon*)

#+:word-size=64
(defvar *current-eps* (* *epsilon* 1e-8))

(deftest test-make-new-coords
  (let ((pos (float-vector 100 200 300))
        (rot (rpy-matrix 0.2 0.3 0.5))
        (cds (make-coords))
        cds-new0 cds-new1)

    (setq cds-new0 (make-coords))
    (send cds-new0 :newcoords cds)

    (setq cds-new1 (make-coords))
    (send cds-new1 :newcoords rot pos)

    (assert (eq (send cds :pos) (send cds-new0 :pos)))
    (assert (eq (send cds :rot) (send cds-new0 :rot)))

    (assert (eq pos (send cds-new1 :pos)))
    (assert (eq rot (send cds-new1 :rot)))
    ))

(deftest test-convert-vector
  (let* ((vec0 (float-vector 100 200 300))
         (vec1 (float-vector 50 100 500))
         (mat (rpy-matrix 0.1 0.2 0.3))
         (pos (float-vector 10 20 30))
         (cds (make-coords :pos pos :rot mat))
         )
    (assert (eps-v= (send cds :rotate-vector vec0)
                    (transform mat vec0) *current-eps*))
    (assert (eps-v= (send cds :rotate-vector vec1)
                    (transform mat vec1) *current-eps*))
    #| ;; :inverse-rotate-vector is a method in irteus
    (assert (eps-v= (send cds :inverse-rotate-vector vec0)
                    (transform vec0 mat) *current-eps*))
    (assert (eps-v= (send cds :inverse-rotate-vector vec1)
                    (transform vec1 mat) *current-eps*))
    |#
    (assert (eps-v= (send cds :transform-vector vec0)
                    (v+ (transform mat vec0) pos) *current-eps*))
    (assert (eps-v= (send cds :transform-vector vec1)
                    (v+ (transform mat vec1) pos) *current-eps*))

    (assert (eps-v= (send cds :inverse-transform-vector vec0)
                    (transform (transpose mat) (v- vec0 pos)) *current-eps*))
    (assert (eps-v= (send cds :inverse-transform-vector vec1)
                    (transform (transpose mat) (v- vec1 pos)) *current-eps*))
    ))

(deftest test-create-target-coordinates
  (let* ((pos0 (float-vector 100 200 300))
         (pos1 (float-vector 50 100 500))
         (pos2 (float-vector 10 -10 20))
         (mat0 (rpy-matrix 0.1 0.2 0.3))
         (mat1 (rpy-matrix 0.05 0.1 0.4))
         (mat2 (rpy-matrix -0.1 0.2 -0.05))
         (cds  (make-coords :pos pos0 :rot mat0))
         (cds1 (make-coords :pos pos1 :rot mat1))
         (cds2 (make-coords :pos pos2 :rot mat2))
         )
    (let ((res (send cds :inverse-transformation)))
      (assert (eps-v= (array-entity (send res :rot))
                      (array-entity (transpose mat0)) *current-eps*))
      (assert (eps-v= (send res :pos)
                      (transform (transpose mat0) (v- pos0)) *current-eps*))
      )
    (let ((res0 (send cds :transformation cds1))
          (res1 (send (send cds :inverse-transformation) :transform cds1)))
      (assert (eps-v= (array-entity (send res0 :rot)) (array-entity (send res1 :rot)) *current-eps*))
      (assert (eps-v= (send res0 :pos) (send res1 :pos) *current-eps*))
      )
    (let ((res0 (send cds :transformation cds1 :world))
          (res1 (send (send cds1 :copy-coords)
                      :transform (send cds :inverse-transformation))))
      (assert (eps-v= (array-entity (send res0 :rot)) (array-entity (send res1 :rot)) *current-eps*))
      (assert (eps-v= (send res0 :pos) (send res1 :pos) *current-eps*))
      )
    (let ((res0 (send cds :transformation cds1 cds2))
          (res1 (send (send (send cds2 :inverse-transformation) :transform cds1)
                      :transform
                      (send (send cds  :inverse-transformation) :transform cds2))))
      (assert (eps-v= (array-entity (send res0 :rot)) (array-entity (send res1 :rot)) *current-eps*))
      (assert (eps-v= (send res0 :pos) (send res1 :pos) *current-eps*))
      )
    ))

(deftest test-move-to
  (let* ((pos0 (float-vector 100 200 300))
         (pos1 (float-vector 50 100 500))
         (pos2 (float-vector 10 -10 20))
         (mat0 (rpy-matrix 0.1 0.2 0.3))
         (mat1 (rpy-matrix 0.05 0.1 0.4))
         (mat2 (rpy-matrix -0.1 0.2 -0.05))
         (cds  (make-coords :pos pos0 :rot mat0))
         (cds1 (make-coords :pos pos1 :rot mat1))
         (cds2 (make-coords :pos pos2 :rot mat2))
         (tmp-l (send cds :copy-coords))
         (tmp-w (send cds :copy-coords))
         (tmp-o (send cds :copy-coords))
         )
    (send tmp-l :move-to cds1)
    (let ((res0 tmp-l)
          (res1 (send (send cds :copy-coords) :transform cds1)))
      (assert (eps-v= (array-entity (send res0 :rot)) (array-entity (send res1 :rot)) *current-eps*))
      (assert (eps-v= (send res0 :pos) (send res1 :pos) *current-eps*))
      )
    ;;
    (send tmp-w :move-to cds1 :world)
    (assert (eq (send tmp-w :pos) (send cds1 :pos)))
    (assert (eq (send tmp-w :rot) (send cds1 :rot)))
    ;;
    (send tmp-o :move-to cds1 cds2)
    (let ((res0 tmp-o)
          (res1 (send (send cds2 :copy-coords) :transform cds1)))
      (assert (eps-v= (array-entity (send res0 :rot)) (array-entity (send res1 :rot)) *current-eps*))
      (assert (eps-v= (send res0 :pos) (send res1 :pos) *current-eps*))
      )
    ))

(deftest test-translate
  (let* ((pos0 (float-vector 100 200 300))
         (pos1 (float-vector 50 100 500))
         (pos2 (float-vector 10 -10 20))
         (mat0 (rpy-matrix 0.1 0.2 0.3))
         (mat1 (rpy-matrix 0.05 0.1 0.4))
         (mat2 (rpy-matrix -0.1 0.2 -0.05))
         (cds  (make-coords :pos pos0 :rot mat0))
         (cds1 (make-coords :pos pos1 :rot mat1))
         (cds2 (make-coords :pos pos2 :rot mat2))
         (tmp-l (send cds :copy-coords))
         (tmp-w (send cds :copy-coords))
         (tmp-o (send cds :copy-coords))
         )
    (send tmp-l :translate pos1)
    (assert (eps-v= (send tmp-l :pos)
                    (v+ pos0 (transform mat0 pos1)) *current-eps*))
    ;;
    (send tmp-w :translate pos1 :world)
    (assert (eps-v= (send tmp-w :pos) (v+ pos0 pos1) *current-eps*))
    ;;
    (send tmp-o :translate pos1 cds2)
    (assert (eps-v= (send tmp-o :pos)
                    (v+ pos0 (transform mat2 pos1)) *current-eps*))
    ))

(deftest test-locate
  (let* ((pos0 (float-vector 100 200 300))
         (pos1 (float-vector 50 100 500))
         (pos2 (float-vector 10 -10 20))
         (mat0 (rpy-matrix 0.1 0.2 0.3))
         (mat1 (rpy-matrix 0.05 0.1 0.4))
         (mat2 (rpy-matrix -0.1 0.2 -0.05))
         (cds  (make-coords :pos pos0 :rot mat0))
         (cds1 (make-coords :pos pos1 :rot mat1))
         (cds2 (make-coords :pos pos2 :rot mat2))
         (tmp-l (send cds :copy-coords))
         (tmp-w (send cds :copy-coords))
         (tmp-o (send cds :copy-coords))
         )
    (send tmp-l :locate pos1)
    (assert (eps-v= (send tmp-l :pos)
                    (v+ pos0 (transform mat0 pos1)) *current-eps*))
    ;;
    (send tmp-w :locate pos1 :world)
    (assert (eps-v= (send tmp-w :pos) pos1 *current-eps*))
    ;;
    (send tmp-o :locate pos1 cds2)
    (assert (eps-v= (send tmp-o :pos)
                    (v+ pos2 (transform mat2 pos1)) *current-eps*))
    ))

(deftest test-transform
  (let* ((pos0 (float-vector 100 200 300))
         (pos1 (float-vector 50 100 500))
         (pos2 (float-vector 10 -10 20))
         (mat0 (rpy-matrix 0.1 0.2 0.3))
         (mat1 (rpy-matrix 0.05 0.1 0.4))
         (mat2 (rpy-matrix -0.1 0.2 -0.05))
         (cds  (make-coords :pos pos0 :rot mat0))
         (cds1 (make-coords :pos pos1 :rot mat1))
         (cds2 (make-coords :pos pos2 :rot mat2))
         (tmp-l (send cds :copy-coords))
         (tmp-w (send cds :copy-coords))
         (tmp-o (send cds :copy-coords))
         )
    (send tmp-l :transform cds1)
    (let ((rot (m* mat0 mat1))
          (pos (v+ pos0 (transform mat0 pos1))))
      (assert (eps-v= (array-entity (send tmp-l :rot)) (array-entity rot) *current-eps*))
      (assert (eps-v= (send tmp-l :pos) pos *current-eps*))
      )
    (send tmp-w :transform cds1 :world)
    (let ((rot (m* mat1 mat0))
          (pos (v+ pos1 (transform mat1 pos0))))
      (assert (eps-v= (array-entity (send tmp-w :rot)) (array-entity rot) *current-eps*))
      (assert (eps-v= (send tmp-w :pos) pos *current-eps*))
      )
    (send tmp-o :transform cds1 cds2)
    (let ((trs (make-coords)))
      (send trs :transform cds2)
      (send trs :transform cds1)
      (send trs :transform (send cds2 :inverse-transformation))
      (send trs :transform cds)
      (assert (eps-v= (array-entity (send tmp-o :rot)) (array-entity (send trs :rot)) *current-eps*))
      (assert (eps-v= (send tmp-o :pos) (send trs :pos) *current-eps*))
      )
    ))

(eval-when (load eval)
  (run-all-tests)
  (exit))
