;;-*-Lisp-*-
(in-package goal)
(bundles "ENGINE.CGO" "GAME.CGO")
(require "engine/gfx/hw/display-h.gc")
(require "engine/math/trigonometry.gc")

;; DECOMP BEGINS

(defun vector-cross! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Compute the cross product. The w component is set to junk."
  (rlet ((acc :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 quad))
    (.outer.product.a.vf acc vf1 vf2)
    (.outer.product.b.vf vf3 vf2 vf1 acc)
    (.svf (&-> arg0 quad) vf3)
    arg0))

(defun vector+float! ((arg0 vector) (arg1 vector) (arg2 float))
  "Add float to each component of vector. The w component is set to 1"
  (rlet ((vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf))
    (init-vf0-vector)
    (.mov vf6 arg2)
    (.lvf vf4 (&-> arg1 quad))
    (.add.x.vf.w vf5 vf0 vf0)
    (.add.x.vf.xyz vf5 vf4 vf6)
    (.svf (&-> arg0 quad) vf5)
    arg0))

(defun vector*! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Elementwise product. Set w = 1"
  (rlet ((vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf))
    (init-vf0-vector)
    (.lvf vf4 (&-> arg1 quad))
    (.lvf vf5 (&-> arg2 quad))
    (.add.x.vf.w vf6 vf0 vf0)
    (.mul.vf.xyz vf6 vf4 vf5)
    (.svf (&-> arg0 quad) vf6)
    arg0))

(defun vector+*! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "set arg0 = arg1 + (arg3 * arg2). The w component will be set to 1"
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf))
    (init-vf0-vector)
    (.mov vf7 arg3)
    (.lvf vf5 (&-> arg2 quad))
    (.lvf vf4 (&-> arg1 quad))
    (.add.x.vf.w vf6 vf0 vf0)
    (.mul.x.vf.xyz acc vf5 vf7)
    ;; acts as just an add.
    (.add.mul.w.vf.xyz vf6 vf4 vf0 acc)
    (.svf (&-> arg0 quad) vf6)
    arg0))

(defun vector-*! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Set arg0 = arg1 - (arg3 * arg2). The w component will be set to 1."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf))
    (init-vf0-vector)
    (.mov vf7 arg3)
    (.lvf vf5 (&-> arg2 quad))
    (.lvf vf4 (&-> arg1 quad))
    (.add.x.vf.w vf6 vf0 vf0)
    (.mul.w.vf.xyz acc vf4 vf0)
    (.sub.mul.x.vf.xyz vf6 vf5 vf7 acc)
    (.svf (&-> arg0 quad) vf6)
    arg0))

(defun vector/! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Set arg0 = arg1 / arg2. The w component will be set to 1.
   The implementation is kind of crazy."
  (rlet ((Q :class vf)
         (vf0 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         (vf7 :class vf))
    (init-vf0-vector)
    (.lvf vf5 (&-> arg2 quad))
    ;; get started on the first divide ASAP.
    ;; do this before loading the second value.
    ;; q = 1 / arg2.y
    (.div.vf Q vf0 vf5 :fsf #b11 :ftf #b1)
    (.add.x.vf.w vf6 vf0 vf0)
    (.lvf vf4 (&-> arg1 quad))
    ;; use FPU to divide x while VU0 is dividing y.
    (let ((v1-0 (/ (-> arg1 data 0) (-> arg2 data 0))))
      (.wait.vf)
      (.mul.vf.y vf6 vf4 Q)
      (.nop.vf)
      (.nop.vf)
      (.div.vf Q vf0 vf5 :fsf #b11 :ftf #b10)
      (.mov vf7 v1-0))
    (.add.x.vf.x vf6 vf0 vf7)
    (.wait.vf)
    (.mul.vf.z vf6 vf4 Q)
    (.nop.vf)
    (.nop.vf)
    (.svf (&-> arg0 quad) vf6)
    arg0))

(defun vector-float*! ((arg0 vector) (arg1 vector) (arg2 float))
  "Multiply all values in a vector by arg2. Set w to 1."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (.mov vf2 arg2)
    (.add.x.vf.w vf1 vf0 vf0)
    (.mul.x.vf.xyz vf1 vf1 vf2)
    (.svf (&-> arg0 quad) vf1)
    arg0))

(defun vector-average! ((arg0 vector) (arg1 vector) (arg2 vector))
  "Set arg0 to the average of arg1 and arg2. Set w to 1."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (let ((v1-0 #x3f000000)) (.lvf vf1 (&-> arg1 quad)) (.lvf vf2 (&-> arg2 quad)) (.mov vf3 v1-0))
    (.add.x.vf.w vf4 vf0 vf0)
    (.mul.x.vf acc vf1 vf3)
    (.add.mul.x.vf.xyz vf4 vf2 vf3 acc)
    (.svf (&-> arg0 quad) vf4)
    arg0))

(defun vector+float*! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Set arg0 = arg1 + (arg2 * arg3). The w component will be set to 1.
   Is this different at all from vector+*! ? The implementation is slightly different
   but I think it comes out to the same thing."
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.lvf vf2 (&-> arg2 quad))
    (.lvf vf1 (&-> arg1 quad))
    (.mov vf3 arg3)
    (.add.x.vf.w vf4 vf0 vf0)
    (.mul.x.vf acc vf2 vf3)
    (.add.mul.w.vf.xyz vf4 vf1 vf0 acc)
    (.svf (&-> arg0 quad) vf4)
    arg0))

(defun vector--float*! ((arg0 vector) (arg1 vector) (arg2 vector) (arg3 float))
  "Set arg0 = arg1 - (arg2 * arg3). The w component will be set to 1
   Is this different from vector-*!"
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.lvf vf2 (&-> arg2 quad))
    (.lvf vf1 (&-> arg1 quad))
    (.mov vf3 arg3)
    (.add.x.vf.w vf4 vf0 vf0)
    (.mul.w.vf acc vf1 vf0)
    (.sub.mul.x.vf.xyz vf4 vf2 vf3 acc)
    (.svf (&-> arg0 quad) vf4)
    arg0))

(defun vector-float/! ((arg0 vector) (arg1 vector) (arg2 float))
  "Divide all components by arg2. The w component will be set to 1."
  (rlet ((Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.mov vf3 arg2)
    (.div.vf Q vf0 vf3 :fsf #b11 :ftf #b0)
    (.lvf vf1 (&-> arg1 quad))
    (.add.x.vf.w vf4 vf0 vf0)
    (.wait.vf)
    (.mul.vf.xyz vf4 vf1 Q)
    (.nop.vf)
    (.nop.vf)
    (.svf (&-> arg0 quad) vf4)
    arg0))

(defun vector-negate! ((arg0 vector) (arg1 vector))
  "Negate xyz, set w to 1"
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (.sub.vf.xyz vf4 vf0 vf1)
    (.add.x.vf.w vf4 vf0 vf0)
    (.svf (&-> arg0 quad) vf4)
    arg0))

(defun vector-negate-in-place! ((arg0 vector))
  "Negate xyz. Doesn't touch w."
  (rlet ((vf0 :class vf)
         (vf1 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.sub.vf.xyz vf1 vf0 vf1)
    (.svf (&-> arg0 quad) vf1)
    arg0))

(defun vector= ((arg0 vector) (arg1 vector))
  "Are the two vectors equal? Does not compare the w component.
   The implementation is cool."
  ;; (label L91)
  ;;   (set! v0-0 #t)
  ;;   (set! v1-0 #xffff)
  (let* ((v1-0 #xffff)
         ;;   (set! a0-1 (l.q a0-0))
         (a0-1 (-> arg0 quad))
         ;;   (set! v1-1 (sll v1-0 48))
         (v1-1 (shl v1-0 48))
         ;;   (set! a1-1 (l.q a1-0))
         (a1-1 (-> arg1 quad))
         (a0-2 (the uint128 0))
         (r0 (the uint128 0)))
    ;;   (.pceqw a0-2 a0-1 a1-1)
    (.pceqw a0-2 a0-1 a1-1)
    ;;   (.ppach a0-3 r0-0 a0-2)
    (.ppach a0-2 r0 a0-2)
    ;;   (set! v1-2 (logior a0-3 v1-1))
    (set! v1-1 (logior (the int a0-2) v1-1))
    ;;   (set! v1-3 (+ v1-2 1))
    ;; will overflow the 64-bit integer if xyz is equal.
    (set! v1-1 (+ v1-1 1))
    (zero? v1-1))
  ;;   (b! (zero? v1-3) L92 (nop!))
  ;;   (set! v0-0 #f)
  ;; (label L92)
  ;;   (ret-value v0-0)
  )

(defun vector-delta ((arg0 vector) (arg1 vector))
  "Sum of the elementwise absolute value of differences"
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.lvf vf2 (&-> arg1 quad))
    (.sub.vf vf1 vf2 vf1)
    (.abs.vf vf1 vf1)
    ;; put abs.x in acc.w
    (.mul.x.vf.w acc vf0 vf1)
    ;; add abs.y
    (.add.mul.y.vf.w acc vf0 vf1 acc)
    ;; add abs.z
    (.add.mul.z.vf.w vf3 vf0 vf1 acc)
    ;; set acc.x = acc.w
    (.add.w.vf.x vf3 vf0 vf3)
    (.mov v0-0 vf3)
    v0-0))

(defun vector-seek! ((arg0 vector) (arg1 vector) (arg2 float))
  "Seek arg0 toward arg1. The arg0 is both read and written.
   arg2 is saturated to (0, 1)"
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf))
    (init-vf0-vector)
    (.mov vf4 arg2)
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg0 quad))
    (.add.x.vf.w vf1 vf0 vf0)
    (.sub.x.vf.x vf5 vf0 vf4)
    (.sub.vf.xyz vf3 vf1 vf2)
    (.min.x.vf.xyz vf3 vf3 vf4)
    (.max.x.vf.xyz vf3 vf3 vf5)
    (.add.vf.xyz vf1 vf2 vf3)
    (.svf (&-> arg0 quad) vf1)
    arg0))

(defun vector-seek-2d-xz-smooth! ((vec vector) (target vector) (max-step float) (alpha float))
  "Smoothly seek vec's x and z components toward target.
   The step always points toward the target and has length (dist * alpha)
   If the step is longer than max-step, the step is projected onto a circle of radius max-step.
   Doesn't touch y or w."
  ;; how much we have to go to get to the target
  (let ((x-diff (- (-> target data 0) (-> vec data 0)))
        (z-diff (- (-> target data 2) (-> vec data 2))))
    ;; do we have to move?
    (if (or (!= x-diff 0.0) (!= z-diff 0.0))
      ;; if so, scale by alpha,
      (let* ((x-step (* x-diff alpha))
             (z-step (* z-diff alpha))
             ;; and get the length of this step
             (step-len (sqrtf (+ (* x-step x-step) (* z-step z-step)))))
        (cond
          ((>= max-step step-len)
           ;; step is within max-step, just do it.
           (+! (-> vec data 0) x-step)
           (+! (-> vec data 2) z-step))
          (else
           ;; not in range.
           (let ((f2-6 (/ max-step step-len))) (+! (-> vec data 0) (* f2-6 x-step)) (+! (-> vec data 2) (* f2-6 z-step))))))))
  vec)

(defun vector-seek-2d-yz-smooth! ((vec vector) (target vector) (max-step float) (alpha float))
  "Smoothly seek vec's y and z components toward target.
   The step always points toward the target and has length (dist * alpha)
   If the step is longer than max-step, the step is projected onto a circle of radius max-step.
   Doesn't touch x or w."
  (let ((y-diff (- (-> target data 1) (-> vec data 1)))
        (z-diff (- (-> target data 2) (-> vec data 2))))
    (if (or (!= y-diff 0.0) (!= z-diff 0.0))
      (let* ((y-step (* y-diff alpha))
             (z-step (* z-diff alpha))
             (step-len (sqrtf (+ (* y-step y-step) (* z-step z-step)))))
        (cond
          ((>= max-step step-len)
           (set! (-> vec data 1) (+ (-> vec data 1) y-step))
           (let ((f0-4 (+ (-> vec data 2) z-step))) (set! (-> vec data 2) f0-4)))
          (else
           (let ((step-scale (/ max-step step-len)))
             (set! (-> vec data 1) (+ (-> vec data 1) (* step-scale y-step)))
             (let ((f0-6 (+ (-> vec data 2) (* step-scale z-step)))) (set! (-> vec data 2) f0-6))))))))
  vec)

(defun vector-seek-3d-smooth! ((vec vector) (target vector) (max-step float) (alpha float))
  "Smoothly seek vec's x, y, and z components toward target.
   The step always points toward the target and has length (dist * alpha)
   If the step is longer than max-step, the step is projected onto a circle of radius max-step.
   Doesn't touch  w."
  (let ((x-diff (- (-> target data 0) (-> vec data 0)))
        (y-diff (- (-> target data 1) (-> vec data 1)))
        (z-diff (- (-> target data 2) (-> vec data 2))))
    (if (or (!= x-diff 0.0) (!= y-diff 0.0) (!= z-diff 0.0))
      (let* ((x-step (* x-diff alpha))
             (y-step (* y-diff alpha))
             (z-step (* z-diff alpha))
             (step-len (sqrtf (+ (+ (* x-step x-step) (* y-step y-step)) (* z-step z-step)))))
        (cond
          ((>= max-step step-len)
           (set! (-> vec data 0) (+ (-> vec data 0) x-step))
           (set! (-> vec data 1) (+ (-> vec data 1) y-step))
           (let ((f0-5 (+ (-> vec data 2) z-step))) (set! (-> vec data 2) f0-5)))
          (else
           (let ((step-scale (/ max-step step-len)))
             (set! (-> vec data 0) (+ (-> vec data 0) (* step-scale x-step)))
             (set! (-> vec data 1) (+ (-> vec data 1) (* step-scale y-step)))
             (let ((f0-7 (+ (-> vec data 2) (* step-scale z-step)))) (set! (-> vec data 2) f0-7))))))))
  vec)

(defun seek-with-smooth ((value float) (target float) (max-step float) (alpha float) (deadband float))
  "Move value closer to target.
   If we are within deadband, just go straight to target.
   If not, try to go alpha*err. If that is a larger step than max-step, limit to max-step"
  (let ((diff (- target value)))
    (if (>= deadband (fabs diff))
      target
      (let ((step (* diff alpha)))
        (let ((min-step (- max-step)))
          (cond
            ((< step min-step) (set! step min-step))
            ((< max-step step) (set! step max-step))))
        (+ step value)))))

(defun vector-identity! ((arg0 vector))
  "Set arg0 to 1, 1, 1, 1"
  (set! (-> arg0 data 0) 1.0)
  (set! (-> arg0 data 1) 1.0)
  (set! (-> arg0 data 2) 1.0)
  (set! (-> arg0 data 3) 1.0)
  arg0)

(defun vector-seconds ((arg0 vector) (arg1 vector))
  "Convert from actual seconds to the seconds unit."
  (set! (-> arg0 data 0) (fsec (-> arg1 data 0)))
  (set! (-> arg0 data 1) (fsec (-> arg1 data 1)))
  (set! (-> arg0 data 2) (fsec (-> arg1 data 2)))
  arg0)

(defun vector-seconds! ((arg0 vector))
  "Convert from actual seconds to seconds, in place"
  (set! (-> arg0 data 0) (fsec (-> arg0 data 0)))
  (set! (-> arg0 data 1) (fsec (-> arg0 data 1)))
  (set! (-> arg0 data 2) (fsec (-> arg0 data 2)))
  arg0)

(defun vector-v! ((arg0 vector))
  "Convert a velocity to a displacement per frame. The velocity should be in X/actual_second"
  (vector-float*! arg0 arg0 (-> *display* seconds-per-frame))
  arg0)

(defun vector-v+! ((result vector) (position vector) (velocity vector))
  "Euler forward step, using the current display time settings"
  (vector+float*! result position velocity (-> *display* seconds-per-frame))
  result)

(defun vector-v*float+! ((result vector) (position vector) (velocity vector) (velocity-scale float))
  "Euler forward step, scaling velocity by velocity-scale"
  (vector+float*! result position velocity (* velocity-scale (-> *display* seconds-per-frame)))
  result)

(defun vector-v++! ((position vector) (velocity vector))
  "Update position in place, using display's current timing"
  (vector+float*! position position velocity (-> *display* seconds-per-frame))
  position)

(defun vector-v*float! ((delta-p vector) (velocity vector) (scale float))
  "Go from velocity to delta-p per frame, scaling by scale"
  (vector-float*! delta-p velocity (* scale (-> *display* seconds-per-frame))))

(defun vector-v*float++! ((position vector) (velocity vector) (scale float))
  "update position with given velocity, scaled by scale."
  (vector+float*! position position velocity (* scale (-> *display* seconds-per-frame)))
  position)

(defun vector-to-ups! ((arg0 vector) (arg1 vector))
  "Go from units per frame to units per second?"
  (local-vars (at-0 int))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (let ((f0-0 (-> *display* frames-per-second))) (.mov at-0 f0-0))
    (.mov vf2 at-0)
    (.mov.vf.w vf1 vf0)
    (.mul.x.vf.xyz vf1 vf1 vf2)
    (.svf (&-> arg0 quad) vf1)
    arg0))

(defun vector-from-ups! ((arg0 vector) (arg1 vector))
  "Go from units per second to units per frame?"
  (local-vars (at-0 int))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> arg1 quad))
    (let ((f0-0 (-> *display* seconds-per-frame))) (.mov at-0 f0-0))
    (.mov vf2 at-0)
    (.mov.vf.w vf1 vf0)
    (.mul.x.vf.xyz vf1 vf1 vf2)
    (.svf (&-> arg0 quad) vf1)
    arg0))

(defun vector-length ((arg0 vector))
  "Get the length of the xyz part."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.mul.vf vf1 vf1 vf1)
    (.mul.x.vf.w acc vf0 vf1)
    (.add.mul.y.vf.w acc vf0 vf1 acc)
    (.add.mul.z.vf.w vf1 vf0 vf1 acc)
    (.sqrt.vf Q vf1 :ftf #b11)
    (.add.w.vf.x vf1 vf0 vf0)
    (.wait.vf)
    (.mul.vf.x vf1 vf1 Q)
    (.nop.vf)
    (.nop.vf)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-length-squared ((arg0 vector))
  "Get the squared length of the xyz part."
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.add.w.vf.x vf2 vf0 vf0)
    (.mul.vf vf1 vf1 vf1)
    (.mul.x.vf.x acc vf2 vf1)
    (.add.mul.y.vf.x acc vf2 vf1 acc)
    (.add.mul.z.vf.x vf1 vf2 vf1 acc)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-xz-length-squared ((arg0 vector))
  "Get the length of the xz part, squared."
  (+ (* (-> arg0 data 0) (-> arg0 data 0)) (* (-> arg0 data 2) (-> arg0 data 2))))

(defun vector-xz-length ((arg0 vector))
  "Get the length of the xz part"
  (sqrtf (+ (* (-> arg0 data 0) (-> arg0 data 0)) (* (-> arg0 data 2) (-> arg0 data 2)))))

(defun vector-vector-distance ((arg0 vector) (arg1 vector))
  "Subtract the xyz parts and get the norm"
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (init-vf0-vector)
    (.lvf vf2 (&-> arg0 quad))
    (.lvf vf3 (&-> arg1 quad))
    (.sub.vf vf1 vf3 vf2)
    (.mul.vf vf1 vf1 vf1)
    (.mul.x.vf.w acc vf0 vf1)
    (.add.mul.y.vf.w acc vf0 vf1 acc)
    (.add.mul.z.vf.w vf1 vf0 vf1 acc)
    (.sqrt.vf Q vf1 :ftf #b11)
    (.add.w.vf.x vf1 vf0 vf0)
    (.wait.vf)
    (.mul.vf.x vf1 vf1 Q)
    (.nop.vf)
    (.nop.vf)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-vector-distance-squared ((arg0 vector) (arg1 vector))
  "Squared norm of the difference of the xyz parts"
  (local-vars (v0-0 float))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (.lvf vf2 (&-> arg0 quad))
    (.lvf vf3 (&-> arg1 quad))
    (.sub.vf vf1 vf3 vf2)
    (.mul.vf vf1 vf1 vf1)
    (.add.y.vf.x vf1 vf1 vf1)
    (.add.z.vf.x vf1 vf1 vf1)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-vector-xz-distance ((arg0 vector) (arg1 vector))
  "Distance on the xz plane"
  (local-vars (v0-0 float))
  (rlet ((acc :class vf)
         (Q :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (init-vf0-vector)
    (.lvf vf2 (&-> arg0 quad))
    (.lvf vf3 (&-> arg1 quad))
    (.sub.vf vf1 vf3 vf2)
    (.mul.vf vf1 vf1 vf1)
    (.mul.x.vf.w acc vf0 vf1)
    (.add.mul.z.vf.w vf1 vf0 vf1 acc)
    (.sqrt.vf Q vf1 :ftf #b11)
    (.add.w.vf.x vf1 vf0 vf0)
    (.wait.vf)
    (.mul.vf.x vf1 vf1 Q)
    (.nop.vf)
    (.nop.vf)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-vector-xz-distance-squared ((arg0 vector) (arg1 vector))
  "Squared distance on the xz plane"
  (local-vars (v0-0 float))
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf))
    (.lvf vf2 (&-> arg0 quad))
    (.lvf vf3 (&-> arg1 quad))
    (.sub.vf vf1 vf3 vf2)
    (.mul.vf vf1 vf1 vf1)
    (.add.z.vf.x vf1 vf1 vf1)
    (.mov v0-0 vf1)
    v0-0))

(defun vector-normalize! ((arg0 vector) (arg1 float))
  "Modify arg0 in place to have length arg1 for its xyz components. The w part is not changed."
  (let ((f0-0 (vector-length arg0)))
    (let ((v1-1 (/ arg1 f0-0)))
      (set! (-> arg0 data 0) (* (-> arg0 data 0) v1-1))
      (set! (-> arg0 data 1) (* (-> arg0 data 1) v1-1))
      (set! (-> arg0 data 2) (* (-> arg0 data 2) v1-1))))
  arg0
  ; (rlet ((acc :class vf)
  ;        (Q :class vf)
  ;        (vf0 :class vf)
  ;        (vf1 :class vf)
  ;        (vf2 :class vf)
  ;        (vf3 :class vf)
  ;        )
  ;  (init-vf0-vector)
  ;  (.lvf vf1 (&-> arg0 quad))
  ;  (.mul.vf.xyz vf2 vf1 vf1)
  ;  (let ((v1-0 arg1))
  ;   (.mov vf3 v1-0)
  ;   )
  ;  (.mul.x.vf.w acc vf0 vf2)
  ;  (.add.mul.y.vf.w acc vf0 vf2 acc)
  ;  (.add.mul.z.vf.w vf2 vf0 vf2 acc)
  ;  (.isqrt.vf Q vf3 vf2 :fsf #b0 :ftf #b11)
  ;  (.wait.vf)
  ;  (.mul.vf.xyz vf1 vf1 Q)
  ;  (.nop.vf)
  ;  (.nop.vf)
  ;  (.nop.vf)
  ;  (.svf (&-> arg0 quad) vf1)
  ;  arg0
  ;  )
  )

(defun vector-normalize-ret-len! ((arg0 vector) (arg1 float))
  "Modify arg0 in place to have length arg1 for its xyz components.
   The w part isn't changed and the _original_ length is returned."
  (let ((f0-0 (vector-length arg0)))
    (let ((v1-1 (/ arg1 f0-0)))
      (set! (-> arg0 data 0) (* (-> arg0 data 0) v1-1))
      (set! (-> arg0 data 1) (* (-> arg0 data 1) v1-1))
      (set! (-> arg0 data 2) (* (-> arg0 data 2) v1-1)))
    f0-0)
  ; (local-vars (v1-1 float))
  ; (rlet ((acc :class vf)
  ;        (Q :class vf)
  ;        (vf0 :class vf)
  ;        (vf1 :class vf)
  ;        (vf2 :class vf)
  ;        (vf3 :class vf)
  ;        )
  ;  (init-vf0-vector)
  ;  (.lvf vf1 (&-> arg0 quad))
  ;  (.mul.vf.xyz vf2 vf1 vf1)
  ;  (let ((v1-0 arg1))
  ;   (.mov vf3 v1-0)
  ;   )
  ;  (.mul.x.vf.w acc vf0 vf2)
  ;  (.add.mul.y.vf.w acc vf0 vf2 acc)
  ;  (.add.mul.z.vf.w vf2 vf0 vf2 acc)
  ;  (.isqrt.vf Q vf3 vf2 :fsf #b0 :ftf #b11)
  ;  (.add.w.vf.x vf2 vf0 vf2)
  ;  (.mov v1-1 vf2)
  ;  (let ((v0-0 (sqrtf v1-1)))
  ;   (.wait.vf)
  ;   (.mul.vf.xyz vf1 vf1 Q)
  ;   (.nop.vf)
  ;   (.nop.vf)
  ;   (.nop.vf)
  ;   (.svf (&-> arg0 quad) vf1)
  ;   v0-0
  ;   )
  ;  )
  )

(defun vector-normalize-copy! ((arg0 vector) (arg1 vector) (arg2 float))
  "Normalize, but not in place.
   This implementation is very good compared to the vector-normalize! one.
   The w component is set to 1."
  (let ((f0-0 (vector-length arg1)))
    (if (= f0-0 0.0)
      (set! (-> arg0 quad) (-> arg1 quad))
      (let ((v1-1 (/ arg2 f0-0)))
        (set! (-> arg0 data 0) (* (-> arg1 data 0) v1-1))
        (set! (-> arg0 data 1) (* (-> arg1 data 1) v1-1))
        (let ((f0-7 (* (-> arg1 data 2) v1-1))) (set! (-> arg0 data 2) f0-7)))))
  (set! (-> arg0 data 3) 1.0)
  arg0)

(defun vector-xz-normalize! ((arg0 vector) (arg1 float))
  "Normalize, xz components only"
  (let ((f0-0 (vector-xz-length arg0)))
    (if (!= f0-0 0.0)
      (let ((v1-1 (/ arg1 f0-0)))
        (set! (-> arg0 data 0) (* (-> arg0 data 0) v1-1))
        (set! (-> arg0 data 2) (* (-> arg0 data 2) v1-1)))))
  arg0)

(defun vector-length-max! ((arg0 vector) (arg1 float))
  "Make vector at most arg1 length (xyz only).
   If it is larger, project onto sphere.
   Doesn't touch w"
  (let ((f0-0 (vector-length arg0)))
    (when (not (or (= f0-0 0.0) (< f0-0 arg1)))
      (set! f0-0 (/ f0-0 arg1))
      (when (!= f0-0 0.0)
        (set! (-> arg0 data 0) (/ (-> arg0 data 0) f0-0))
        (set! (-> arg0 data 1) (/ (-> arg0 data 1) f0-0))
        (set! (-> arg0 data 2) (/ (-> arg0 data 2) f0-0)))))
  arg0)

(defun vector-xz-length-max! ((arg0 vector) (arg1 float))
  "Make vector at most arg1 length (xz only).
   It it is larger, project onto circle.
   Doesn't touch w or y"
  (let ((f0-0 (vector-xz-length arg0)))
    (when (not (or (= f0-0 0.0) (< f0-0 arg1)))
      (set! f0-0 (/ f0-0 arg1))
      (when (!= f0-0 0.0)
        (set! (-> arg0 data 0) (/ (-> arg0 data 0) f0-0))
        (let ((f0-1 (/ (-> arg0 data 2) f0-0))) (set! (-> arg0 data 2) f0-1)))))
  arg0)

(defun vector-rotate-around-y! ((arg0 vector) (arg1 vector) (arg2 float))
  "Rotate a vector around the y axis"
  (let ((f26-0 (-> arg1 data 2))
        (f30-0 (-> arg1 data 0))
        (f28-0 (cos arg2))
        (f0-0 (sin arg2)))
    (set! (-> arg0 quad) (-> arg1 quad))
    (set! (-> arg0 data 2) (- (* f26-0 f28-0) (* f30-0 f0-0)))
    (set! (-> arg0 data 0) (+ (* f26-0 f0-0) (* f30-0 f28-0))))
  arg0)

(defun rotate-y<-vector+vector ((arg0 vector) (arg1 vector))
  "Get the y rotation between vectors. These should have the same length."
  (atan (- (-> arg1 data 0) (-> arg0 data 0)) (- (-> arg1 data 2) (-> arg0 data 2))))

(defun vector-cvt.w.s! ((arg0 vector) (arg1 vector))
  "Convert float to int32. Truncate."
  (rlet ((vf1 :class vf)) (.lvf vf1 (&-> arg1 quad)) (.ftoi.vf vf1 vf1) (.svf (&-> arg0 quad) vf1) arg0))

(defun vector-cvt.s.w! ((arg0 vector) (arg1 vector))
  "Convert float to int32."
  (rlet ((vf1 :class vf)) (.lvf vf1 (&-> arg1 quad)) (.itof.vf vf1 vf1) (.svf (&-> arg0 quad) vf1) arg0))

(defun rot-zxy-from-vector! ((arg0 vector) (arg1 vector))
  "I think this gives you a vector of euler angles to rotate some unit vector
   to arg1."
  (let* ((f28-0 (-> arg1 data 2))
         (f30-0 (-> arg1 data 0))
         (f0-0 (atan f30-0 f28-0)))
    (set! (-> arg0 data 1) f0-0)
    (let* ((f26-0 (- f0-0))
           (f0-4 (- (* f28-0 (cos f26-0)) (* f30-0 (sin f26-0))))
           (f0-5 (atan (- (-> arg1 data 1)) f0-4)))
      (set! (-> arg0 data 0) f0-5)))
  (set! (-> arg0 data 2) 0.0)
  arg0)

(defun rot-zyx-from-vector! ((arg0 vector) (arg1 vector))
  "I think this gives you a vector of euler angles to rotate some unit vector
   to arg1."
  (let* ((f28-0 (-> arg1 data 2))
         (f30-0 (- (-> arg1 data 1)))
         (f0-1 (atan f30-0 f28-0)))
    (set! (-> arg0 data 0) f0-1)
    (let* ((f26-0 (- f0-1))
           (f0-5 (- (* f28-0 (cos f26-0)) (* f30-0 (sin f26-0))))
           (f0-6 (atan (-> arg1 data 0) f0-5)))
      (set! (-> arg0 data 1) f0-6)))
  (set! (-> arg0 data 2) 0.0)
  arg0)

(defun vector-lerp! ((out vector) (a vector) (b vector) (alpha float))
  "Linearly interpolate between two vectors. Alpha isn't clamped.
   w will be set to 1."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> a quad))
    (.lvf vf2 (&-> b quad))
    (.mov vf4 alpha)
    (.add.x.vf.w vf3 vf0 vf0)
    (.sub.vf vf2 vf2 vf1)
    (.mul.x.vf vf2 vf2 vf4)
    (.add.vf.xyz vf3 vf1 vf2)
    (.svf (&-> out quad) vf3)
    out))

(defun vector-lerp-clamp! ((out vector) (a vector) (b vector) (alpha float))
  "Linearly interpolate between two vectors, clamping alpha to 0, 1
   w will be set to 1."
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (cond
      ((>= 0.0 alpha) (set! (-> out quad) (-> a quad)))
      ((>= alpha 1.0) (set! (-> out quad) (-> b quad)))
      (else
       (let ((v1-2 out))
         (let ((f0-2 alpha)) (.lvf vf1 (&-> a quad)) (.lvf vf2 (&-> b quad)) (let ((a1-1 f0-2)) (.mov vf4 a1-1)))
         (.add.x.vf.w vf3 vf0 vf0)
         (.sub.vf vf2 vf2 vf1)
         (.mul.x.vf vf2 vf2 vf4)
         (.add.vf.xyz vf3 vf1 vf2)
         (.svf (&-> v1-2 quad) vf3))))
    out))

(defun vector4-lerp! ((out vector) (a vector) (b vector) (alpha float))
  "Interpolate all 4 elements of a vector. Alpha is not clamped"
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (.lvf vf1 (&-> a quad))
    (.lvf vf2 (&-> b quad))
    (.mov vf4 alpha)
    (.sub.vf vf2 vf2 vf1)
    (.mul.x.vf vf2 vf2 vf4)
    (.add.vf vf3 vf1 vf2)
    (.svf (&-> out quad) vf3)
    out))

(defun vector4-lerp-clamp! ((out vector) (a vector) (b vector) (alpha float))
  "Interpolate all 4 elements of a vector. Alpha is clamped to [0, 1]"
  (rlet ((vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (cond
      ((>= 0.0 alpha) (set! (-> out quad) (-> a quad)))
      ((>= alpha 1.0) (set! (-> out quad) (-> b quad)))
      (else
       (let ((v1-2 out))
         (let ((f0-2 alpha)) (.lvf vf1 (&-> a quad)) (.lvf vf2 (&-> b quad)) (let ((a1-1 f0-2)) (.mov vf4 a1-1)))
         (.sub.vf vf2 vf2 vf1)
         (.mul.x.vf vf2 vf2 vf4)
         (.add.vf vf3 vf1 vf2)
         (.svf (&-> v1-2 quad) vf3))))
    out))

(defun vector-degi ((arg0 vector) (arg1 vector))
  "Convert a vector (in _rotations_) to degrees units, stored in an int.
   Truncates to the nearest _rotation_.
   Neither the input or output is a commonly used form.
   Unsurprisingly, this strange function is never used."
  (local-vars (v1-0 uint128) (v1-1 uint128))
  (rlet ((vf1 :class vf))
    (.lvf vf1 (&-> arg1 quad))
    (.ftoi.vf vf1 vf1)
    (.mov v1-0 vf1)
    (.pw.sll v1-1 v1-0 16)
    (set! (-> arg0 quad) (the-as uint128 v1-1))
    arg0))

(defun vector-degf ((arg0 vector) (arg1 vector))
  "Convert a vector (in integer degree units) to floating point rotations.
   Truncates to the nearest _rotation_.
   Like the previous function, this is stupid and unused"
  (local-vars (v1-1 uint128))
  (rlet ((vf1 :class vf))
    (let ((v1-0 (-> arg1 quad))) (.pw.sra v1-1 v1-0 16))
    (.mov vf1 v1-1)
    (.itof.vf vf1 vf1)
    (.svf (&-> arg0 quad) vf1)
    arg0))

(defun vector-degmod ((arg0 vector) (arg1 vector))
  "This one is actually right. Wraps degrees units (in floats, like they should be)
  to +/- half a rotation."
  (local-vars (v1-0 uint128) (v1-1 uint128) (v1-2 uint128))
  (rlet ((vf1 :class vf))
    (.lvf vf1 (&-> arg1 quad))
    (.ftoi.vf vf1 vf1)
    (.mov v1-0 vf1)
    (.pw.sll v1-1 v1-0 16)
    (.pw.sra v1-2 v1-1 16)
    (.mov vf1 v1-2)
    (.itof.vf vf1 vf1)
    (.svf (&-> arg0 quad) vf1)
    arg0))

(defun vector-deg-diff ((arg0 vector) (arg1 vector) (arg2 vector))
  "Wrapped difference, degrees units. Will have the usual 16-bit accuracy issue"
  (local-vars (v0-0 float) (v1-0 uint128) (v1-1 uint128) (v1-2 uint128) (v1-3 uint128) (a1-1 uint128) (a1-2 uint128))
  (rlet ((vf1 :class vf)
         (vf2 :class vf))
    (.lvf vf1 (&-> arg1 quad))
    (.lvf vf2 (&-> arg2 quad))
    (.ftoi.vf vf1 vf1)
    (.ftoi.vf vf2 vf2)
    (.mov a1-1 vf1)
    (.mov v1-0 vf2)
    (.pw.sll a1-2 a1-1 16)
    (.pw.sll v1-1 v1-0 16)
    (.psubw v1-2 a1-2 v1-1)
    (.pw.sra v1-3 v1-2 16)
    (.mov vf1 v1-3)
    (.itof.vf vf1 vf1)
    (.svf (&-> arg0 quad) vf1)
    (.mov v0-0 vf1)
    (none)))

(defun vector-deg-lerp-clamp! ((out vector) (min-val vector) (max-val vector) (in float))
  "Apply deg-lerp-clamp to the xyz components of a vector. Sets w = 1."
  (cond
    ((>= 0.0 in)
     ;; there is actually something weird here where it is not possible
     ;; this was written like you would expect. Perhaps this is from a macro?
     (let ((v1-0 out)) (set! (-> v1-0 quad) (-> min-val quad))))
    ((>= in 1.0) (let ((v1-1 out)) (set! (-> v1-1 quad) (-> max-val quad))))
    (else
     (set! (-> out data 0) (deg-lerp-clamp (-> min-val data 0) (-> max-val data 0) in))
     (set! (-> out data 1) (deg-lerp-clamp (-> min-val data 1) (-> max-val data 1) in))
     (set! (-> out data 2) (deg-lerp-clamp (-> min-val data 2) (-> max-val data 2) in))
     (set! (-> out data 3) 1.0)))
  out)

;; The weird docstrings for the next 4 functions were left behind in the game.
;; We suspect they accidentally put something before the docstring, turning it
;; into a string constant. GOAL doesn't eliminate dead code and
;; loads into registers greedily, so it decompiles into a variable assignment.

(defun vector3s-copy! ((arg0 vector) (arg1 vector))
  (let ((v1-0 "Copy a vector3s")))
  (set! (-> arg0 data 0) (-> arg1 data 0))
  (set! (-> arg0 data 1) (-> arg1 data 1))
  (set! (-> arg0 data 2) (-> arg1 data 2))
  arg0)

(defun vector3s+! ((arg0 vector) (arg1 vector) (arg2 vector))
  (let ((v1-0 "Add 2 vectors3.")))
  (set! (-> arg0 data 0) (+ (-> arg1 data 0) (-> arg2 data 0)))
  (set! (-> arg0 data 1) (+ (-> arg1 data 1) (-> arg2 data 1)))
  (set! (-> arg0 data 2) (+ (-> arg1 data 2) (-> arg2 data 2)))
  arg0)

(defun vector3s*float! ((arg0 vector) (arg1 vector) (arg2 float))
  (let ((v1-0 "mult vectors3 by float")))
  (set! (-> arg0 data 0) (* (-> arg1 data 0) arg2))
  (set! (-> arg0 data 1) (* (-> arg1 data 1) arg2))
  (set! (-> arg0 data 2) (* (-> arg1 data 2) arg2))
  arg0)

(defun vector3s-! ((arg0 vector) (arg1 vector) (arg2 vector))
  (let ((v1-0 "Subtract 2 vectors3: c = (a - b).")))
  (set! (-> arg0 data 0) (- (-> arg1 data 0) (-> arg2 data 0)))
  (set! (-> arg0 data 1) (- (-> arg1 data 1) (-> arg2 data 1)))
  (set! (-> arg0 data 2) (- (-> arg1 data 2) (-> arg2 data 2)))
  arg0)

(defun spheres-overlap? ((arg0 sphere) (arg1 sphere))
  "Do the spheres overlap?"
  (local-vars (v1-0 float) (a0-1 float))
  (rlet ((vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf))
    (init-vf0-vector)
    (.lvf vf1 (&-> arg0 quad))
    (.lvf vf2 (&-> arg1 quad))
    (.sub.vf.xyz vf3 vf1 vf2)
    (.mul.vf.xyz vf3 vf3 vf3)
    (.add.w.vf.w vf4 vf1 vf2)
    (.mul.w.vf.w vf4 vf4 vf4)
    (.add.y.vf.x vf3 vf3 vf3)
    (.add.z.vf.x vf3 vf3 vf3)
    (.add.w.vf.x vf4 vf0 vf4)
    (.mov a0-1 vf4)
    (.mov v1-0 vf3)
    (>= a0-1 v1-0)))

(defun sphere<-vector! ((arg0 sphere) (arg1 vector))
  "Set the position of the sphere to arg1. Does not change the radius"
  (let ((f0-0 (-> arg0 data 3))) (set! (-> arg0 quad) (-> arg1 quad)) (set! (-> arg0 data 3) f0-0))
  arg0)

(defun sphere<-vector+r! ((arg0 sphere) (arg1 vector) (arg2 float))
  "Set the position of the sphere from arg1 and the radius from arg2"
  (set! (-> arg0 quad) (-> arg1 quad))
  (set! (-> arg0 data 3) arg2)
  arg0)

(defun rand-vu-sphere-point! ((arg0 vector) (arg1 float))
  "Get a random point on the sphere at the origin with radius arg1.
   The point is on the surface of the sphere."
  (set-vector! arg0 (rand-vu-float-range -1.0 1.0) (rand-vu-float-range -1.0 1.0) (rand-vu-float-range -1.0 1.0) 1.0)
  (vector-normalize! arg0 (rand-vu-float-range 0.0 arg1)))
