;;-*-Lisp-*-
(in-package goal)
(bundles "ENGINE.CGO" "GAME.CGO")
(require "engine/math/vector-h.gc")
(require "engine/geometry/bounding-box-h.gc")

;; DECOMP BEGINS

(defun box-vector-enside? ((box bounding-box) (pt vector))
  "Is the point in the box? On the edge doesn't count"
  (and (< (-> box min x) (-> pt x))
       (< (-> box min y) (-> pt y))
       (< (-> box min z) (-> pt z))
       (< (-> pt x) (-> box max x))
       (< (-> pt y) (-> box max y))
       (< (-> pt z) (-> box max z))))

(defun box-vector-inside? ((box bounding-box) (pt vector))
  "Is the point in the box? On the edge counts."
  (and (>= (-> pt x) (-> box min x))
       (>= (-> pt y) (-> box min y))
       (>= (-> pt z) (-> box min z))
       (>= (-> box max x) (-> pt x))
       (>= (-> box max y) (-> pt y))
       (>= (-> box max z) (-> pt z))))

(defmethod set-from-point-offset! ((this bounding-box) (arg0 vector3s) (arg1 vector3s))
  "Set box to smallest containing the points arg0, (arg0 + arg1)"
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf))
    (init-vf0-vector)
    (.lvf vf3 arg1)
    (.lvf vf4 arg0)
    (.add.vf vf5 vf4 vf3)
    (.min.vf vf1 vf4 vf5)
    (.max.vf vf2 vf4 vf5)
    (.mov.vf.w vf1 vf0)
    (.mov.vf.w vf2 vf0)
    (.svf (&-> this min quad) vf1)
    (.svf (&-> this max quad) vf2)
    0))

(defmethod add-point! ((this bounding-box) (arg0 vector3s))
  "Expand the box if needed to contain the given point"
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (.lvf vf1 (&-> this min quad))
    (.lvf vf2 (&-> this max quad))
    (.lvf vf3 arg0)
    (.min.vf vf1 vf1 vf3)
    (.max.vf vf2 vf2 vf3)
    (.svf (&-> this min quad) vf1)
    (.svf (&-> this max quad) vf2)
    0))

(defmethod add-box! ((this bounding-box) (arg0 bounding-box))
  "Expand the box if needed to contain the given box"
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (.lvf vf1 (&-> this min quad))
    (.lvf vf2 (&-> this max quad))
    (.lvf vf3 (&-> arg0 min quad))
    (.lvf vf4 (&-> arg0 max quad))
    (.min.vf vf1 vf1 vf3)
    (.max.vf vf2 vf2 vf4)
    (.svf (&-> this min quad) vf1)
    (.svf (&-> this max quad) vf2)
    0))

(defmethod set-from-point-offset-pad! ((this bounding-box) (arg0 vector3s) (arg1 vector3s) (arg2 float))
  "Set the box size to contain pt, pt + offset, with some padding"
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf))
    (init-vf0-vector)
    (.lvf vf4 arg1)
    (.lvf vf5 arg0)
    (.mov vf1 arg2)
    (.add.vf vf6 vf5 vf4)
    (.min.vf vf2 vf5 vf6)
    (.max.vf vf3 vf5 vf6)
    (.add.x.vf.xyz vf3 vf3 vf1)
    (.sub.x.vf.xyz vf2 vf2 vf1)
    (.mov.vf.w vf2 vf0)
    (.mov.vf.w vf3 vf0)
    (.svf (&-> this min quad) vf2)
    (.svf (&-> this max quad) vf3)
    0))

(defmethod set-from-sphere! ((this bounding-box) (arg0 sphere))
  "Set the box size to contain the given sphere"
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (init-vf0-vector)
    (.lvf vf1 arg0)
    (.sub.w.vf.xyz vf2 vf1 vf1)
    (.add.w.vf.xyz vf3 vf1 vf1)
    (.mov.vf.w vf2 vf0)
    (.mov.vf.w vf3 vf0)
    (.svf this vf2)
    (.svf this vf3 :offset 16)
    0))

;;;;;;;;;;;;;;;;;;;;;;;;;
;; multi-sphere methods
;;;;;;;;;;;;;;;;;;;;;;;;;

;; these are used in the collision system to build bounding boxes around collision geometries, so they are quite optimized.

(defmethod add-spheres! ((this bounding-box) (spheres (inline-array sphere)) (count int))
  "Add count spheres."
  ;; the PS2 implementation is very optimized
  ;; It is unrolled and 'software pipelined' to do 4 at a time.
  ;; This is slightly less optimized.
  (rlet ((current-min :class vf)
         (current-max :class vf)
         (sph-min :class vf)
         (sph-max :class vf)
         (sph :class vf))
    (when (nonzero? count)
      ;; load these outside the loop
      (.lvf current-min (-> this min))
      (.lvf current-max (-> this max))
      (dotimes (i count)
        (.lvf sph (-> spheres i))
        (.sub.w.vf.xyz sph-min sph sph)
        (.add.w.vf.xyz sph-max sph sph)
        (.min.vf.xyz current-min current-min sph-min)
        (.max.vf.xyz current-max current-max sph-max))
      (.svf (-> this min) current-min)
      (.svf (-> this max) current-max)))
  0)

(defmethod set-from-spheres! ((this bounding-box) (spheres (inline-array sphere)) (count int))
  "Reset box to hold the given spheres. Note: this implementation could be optimized."
  ;; This is also unrolled, but does 7 at a time.
  (rlet ((vf0 :class vf)
         (current-min :class vf)
         (current-max :class vf)
         (sph-min :class vf)
         (sph-max :class vf)
         (sph :class vf))
    ;; init constant
    (init-vf0-vector)
    ;; init min/max. in the case we don't have any spheres, we should return (0,0,0,1) for min/max.
    (set! current-min vf0)
    (set! current-max vf0)
    (dotimes (i count)
      (.lvf sph (-> spheres i))
      (.sub.w.vf.xyz sph-min sph sph)
      (.add.w.vf.xyz sph-max sph sph)
      (cond
        ((zero? i) (set! current-min sph-min) (set! current-max sph-max))
        (else (.min.vf.xyz current-min current-min sph-min) (.max.vf.xyz current-max current-max sph-max))))
    (.svf (-> this min) current-min)
    (.svf (-> this max) current-max))
  0)
