;;-*-Lisp-*-
(in-package goal)

;; name: chain-physics.gc
;; name in dgo: chain-physics
;; dgos: SEA, VOCA

;; DECOMP BEGINS

(defmethod gravity-update ((this chain-physics) (arg0 process-drawable))
  (vector-seek-3d-smooth! (-> this gravity) (-> this gravity-target) 0.05 0.1)
  0
  (none)
  )

(defmethod apply-gravity ((this chain-physics) (arg0 vector) (arg1 int) (arg2 process-drawable))
  (with-pp
    (vector-float*!
      arg0
      (-> this gravity)
      (* 4096.0 (-> pp clock time-adjust-ratio) (fmax 0.2 (lerp-scale 0.38 0.18 (the float arg1) 0.0 5.0)))
      )
    0
    (none)
    )
  )

(defmethod chain-physics-method-14 ((this chain-physics) (arg0 vector) (arg1 int))
  (vector-float*!
    arg0
    (the-as vector (+ (the-as uint (-> this chain-joints 0 velocity)) (* (+ arg1 1) 64)))
    (fmin 0.9 (lerp-scale 0.2 1.0 (the float arg1) 0.0 4.0))
    )
  0
  (none)
  )

(defmethod clamp-length ((this chain-physics) (arg0 vector) (arg1 vector) (arg2 object) (arg3 process-drawable))
  (let ((gp-0 (new 'stack-no-clear 'vector)))
    (vector-! gp-0 arg0 arg1)
    (vector-normalize! gp-0 (-> this joint-length))
    (vector+! arg0 gp-0 arg1)
    )
  )

(defmethod chain-physics-method-16 ((this chain-physics) (arg0 int))
  (lerp-scale 5461.3335 10922.667 (the float arg0) 0.0 8.0)
  )

(defmethod chain-physics-method-17 ((this chain-physics) (arg0 vector) (arg1 int))
  0
  (none)
  )

(defmethod initialize-chain-joints ((this chain-physics))
  (set! (-> this turn-off-start) 0)
  (dotimes (s5-0 (the-as int (-> this num-joints)))
    (let ((s4-0 (-> this chain-joints s5-0)))
      (vector<-cspace! (-> s4-0 position) (-> s4-0 joint-mod joint))
      (vector-reset! (-> s4-0 velocity))
      (mode-set! (-> s4-0 joint-mod) (joint-mod-mode foot-rot))
      )
    )
  #f
  )

(defmethod turn-off ((this chain-physics) (arg0 time-frame))
  (set-time! (-> this turn-off-start))
  (set! (-> this turn-off-duration) arg0)
  0
  (none)
  )

(defmethod update ((this chain-physics) (arg0 process-drawable))
  (local-vars
    (f0-11 float)
    (sv-272 chain-physics-joint)
    (sv-288 vector)
    (sv-304 vector)
    (sv-320 (function vector float vector))
    (sv-336 vector)
    (sv-352 vector)
    (sv-368 vector)
    (sv-384 vector)
    (sv-400 vector)
    (sv-416 vector)
    (sv-432 vector)
    (sv-448 vector)
    (sv-464 vector)
    (sv-480 vector)
    (sv-496 vector)
    (sv-512 vector)
    (sv-528 vector)
    )
  (rlet ((acc :class vf)
         (vf0 :class vf)
         (vf1 :class vf)
         (vf2 :class vf)
         (vf3 :class vf)
         (vf4 :class vf)
         (vf5 :class vf)
         (vf6 :class vf)
         )
    (init-vf0-vector)
    (gravity-update this arg0)
    (let ((s4-0 (new 'stack-no-clear 'matrix))
          (s3-0 (vector<-cspace! (new 'stack-no-clear 'vector) (-> arg0 node-list data (-> this root-joint-index))))
          (s2-0 (vector-normalize-copy!
                  (new 'stack-no-clear 'vector)
                  (-> arg0 node-list data (-> this root-joint-index) bone transform fvec)
                  1.0
                  )
                )
          (s1-0 (vector-normalize-copy!
                  (new 'stack-no-clear 'vector)
                  (-> arg0 node-list data (-> this root-joint-index) bone transform uvec)
                  1.0
                  )
                )
          )
      (new 'stack-no-clear 'vector)
      (let ((f30-0 1.0))
        (if (nonzero? (-> this turn-off-start))
            (set! f30-0
                  (fmax
                    0.0
                    (fmin
                      1.0
                      (parameter-ease-sin-clamp
                        (- 1.0 (/ (the float (- (current-time) (-> this turn-off-start))) (the float (-> this turn-off-duration))))
                        )
                      )
                    )
                  )
            )
        (dotimes (s0-0 (the-as int (-> this num-joints)))
          (set! sv-272 (-> this chain-joints s0-0))
          (set! sv-528 (new 'stack-no-clear 'vector))
          (let ((v1-27 (-> sv-272 position quad)))
            (set! (-> sv-528 quad) v1-27)
            )
          (set! (-> sv-272 joint-mod flex-blend) f30-0)
          (if (-> this negate-y)
              (vector-negate! s1-0 s1-0)
              )
          (set! sv-288 (new 'stack-no-clear 'vector))
          (apply-gravity this sv-288 s0-0 arg0)
          (let ((v1-34 sv-528))
            (let ((a0-10 sv-528))
              (.mov.vf.w vf6 vf0)
              (.lvf vf4 (&-> a0-10 quad))
              )
            (.lvf vf5 (&-> sv-288 quad))
            (.add.vf.xyz vf6 vf4 vf5)
            (.svf (&-> v1-34 quad) vf6)
            )
          (when (< s0-0 (the-as int (+ (-> this num-joints) -1)))
            (set! sv-304 (new 'stack-no-clear 'vector))
            (chain-physics-method-14 this sv-304 s0-0)
            (let ((v1-40 sv-528))
              (let ((a0-13 sv-528))
                (.mov.vf.w vf6 vf0)
                (.lvf vf4 (&-> a0-13 quad))
                )
              (.lvf vf5 (&-> sv-304 quad))
              (.add.vf.xyz vf6 vf4 vf5)
              (.svf (&-> v1-40 quad) vf6)
              )
            )
          (clamp-length this sv-528 s3-0 s0-0 arg0)
          (set! sv-400 (new 'stack-no-clear 'vector))
          (let ((v1-43 sv-528)
                (a0-16 s3-0)
                )
            (.lvf vf4 (&-> v1-43 quad))
            (.lvf vf5 (&-> a0-16 quad))
            )
          (.mov.vf.w vf6 vf0)
          (.sub.vf.xyz vf6 vf4 vf5)
          (.svf (&-> sv-400 quad) vf6)
          (let ((f28-1 (vector-normalize-ret-len! sv-400 1.0))
                (f24-0 (vector-dot sv-400 s1-0))
                (f26-0 (chain-physics-method-16 this s0-0))
                )
            (when (< f24-0 (cos f26-0))
              (vector--float*! sv-400 sv-400 s1-0 f24-0)
              (set! sv-320 vector-normalize!)
              (set! sv-336 sv-400)
              (let ((a1-17 (sin f26-0)))
                (sv-320 sv-336 a1-17)
                )
              (set! sv-384 sv-400)
              (set! sv-352 sv-400)
              (set! sv-368 s1-0)
              (let ((f0-6 (cos f26-0)))
                (.lvf vf2 (&-> sv-368 quad))
                (.lvf vf1 (&-> sv-352 quad))
                (let ((v1-55 f0-6))
                  (.mov vf3 v1-55)
                  )
                )
              (.add.x.vf.w vf4 vf0 vf0)
              (.mul.x.vf acc vf2 vf3)
              (.add.mul.w.vf.xyz vf4 vf1 vf0 acc)
              (.svf (&-> sv-384 quad) vf4)
              (vector+float*! sv-528 s3-0 sv-400 f28-1)
              )
            )
          (chain-physics-method-17 this sv-528 s0-0)
          (set! sv-432 (new 'stack-no-clear 'vector))
          (let ((v1-61 s3-0)
                (a0-27 sv-528)
                )
            (.lvf vf4 (&-> v1-61 quad))
            (.lvf vf5 (&-> a0-27 quad))
            )
          (.mov.vf.w vf6 vf0)
          (.sub.vf.xyz vf6 vf4 vf5)
          (.svf (&-> sv-432 quad) vf6)
          (vector-normalize-ret-len! sv-432 1.0)
          (set! sv-416 (new 'stack-no-clear 'vector))
          (let ((v1-64 sv-528)
                (a0-30 (-> sv-272 position))
                )
            (.lvf vf4 (&-> v1-64 quad))
            (.lvf vf5 (&-> a0-30 quad))
            )
          (.mov.vf.w vf6 vf0)
          (.sub.vf.xyz vf6 vf4 vf5)
          (.svf (&-> sv-416 quad) vf6)
          (let ((f28-2 (vector-dot sv-432 sv-416)))
            (vector--float*! sv-416 sv-416 sv-432 f28-2)
            (cond
              ((< f28-2 0.0)
               (set! f0-11 (* f28-2 (-> this compress-vel-parallel)))
               (vector-float*! sv-416 sv-416 (-> this compress-vel))
               )
              (else
                (set! f0-11 (* f28-2 (-> this stretch-vel-parallel)))
                (vector-float*! sv-416 sv-416 (-> this stretch-vel))
                )
              )
            )
          (let ((v1-73 (-> sv-272 velocity)))
            (.lvf vf2 (&-> sv-432 quad))
            (.lvf vf1 (&-> sv-416 quad))
            (let ((a0-37 f0-11))
              (.mov vf3 a0-37)
              )
            (.add.x.vf.w vf4 vf0 vf0)
            (.mul.x.vf acc vf2 vf3)
            (.add.mul.w.vf.xyz vf4 vf1 vf0 acc)
            (.svf (&-> v1-73 quad) vf4)
            )
          (vector+! sv-528 (-> sv-272 position) (-> sv-272 velocity))
          (let* ((a2-9 (vector-! (new 'stack-no-clear 'vector) s3-0 sv-528))
                 (f0-13 (vector-length-squared a2-9))
                 (f1-6 (* (-> this maximum-stretch) (-> this joint-length)))
                 (f2-3 f1-6)
                 )
            (if (< (* f2-3 f2-3) f0-13)
                (vector--float*! sv-528 s3-0 a2-9 (/ f1-6 (sqrtf f0-13)))
                )
            )
          (vector-copy! (-> sv-272 position) sv-528)
          (if (-> sv-272 joint-mod)
              (vector-copy! (-> sv-272 joint-mod trans) sv-528)
              )
          (when (< s0-0 (the-as int (-> this num-joints)))
            (vector-! (-> s4-0 uvec) sv-528 s3-0)
            (vector-normalize! (-> s4-0 uvec) 1.0)
            (if (-> this negate-y)
                (vector-negate! (-> s4-0 uvec) (-> s4-0 uvec))
                )
            (vector-cross! (-> s4-0 rvec) (-> s4-0 uvec) s2-0)
            (vector-normalize! (-> s4-0 rvec) 1.0)
            (set! sv-496 (new 'stack-no-clear 'vector))
            (let ((v1-97 (-> sv-272 old-x))
                  (a0-51 (-> s4-0 rvec))
                  )
              (.lvf vf1 (&-> v1-97 quad))
              (.lvf vf2 (&-> a0-51 quad))
              )
            (.outer.product.a.vf acc vf1 vf2)
            (.outer.product.b.vf vf3 vf2 vf1 acc)
            (.svf (&-> sv-496 quad) vf3)
            (vector-flatten! sv-496 (-> sv-272 old-x) (-> s4-0 uvec))
            (vector-normalize! sv-496 1.0)
            (cond
              ((< (vector-dot (-> s4-0 rvec) sv-496) (cos (-> this axial-slop)))
               (vector-cross! sv-496 sv-496 (-> s4-0 rvec))
               (vector-cross! sv-496 (-> s4-0 rvec) sv-496)
               (vector-normalize! sv-496 1.0)
               (set! sv-464 (-> s4-0 rvec))
               (set! sv-448 (-> s4-0 rvec))
               (let ((f0-20 (cos (-> this axial-slop))))
                 (.lvf vf1 (&-> sv-448 quad))
                 (let ((v1-107 f0-20))
                   (.mov vf2 v1-107)
                   )
                 )
               (.add.x.vf.w vf1 vf0 vf0)
               (.mul.x.vf.xyz vf1 vf1 vf2)
               (.svf (&-> sv-464 quad) vf1)
               (set! sv-512 (-> s4-0 rvec))
               (set! sv-480 (-> s4-0 rvec))
               (let ((f0-22 (sin (-> this axial-slop))))
                 (.lvf vf2 (&-> sv-496 quad))
                 (.lvf vf1 (&-> sv-480 quad))
                 (let ((v1-113 f0-22))
                   (.mov vf3 v1-113)
                   )
                 )
               (.add.x.vf.w vf4 vf0 vf0)
               (.mul.x.vf acc vf2 vf3)
               (.add.mul.w.vf.xyz vf4 vf1 vf0 acc)
               (.svf (&-> sv-512 quad) vf4)
               (vector-copy! (-> sv-272 old-x) (-> s4-0 rvec))
               )
              (else
                (vector-copy! (-> s4-0 rvec) sv-496)
                (vector-copy! (-> sv-272 old-x) (-> s4-0 rvec))
                )
              )
            (vector-cross! (-> s4-0 fvec) (-> s4-0 rvec) (-> s4-0 uvec))
            (matrix->quaternion (-> sv-272 joint-mod quat) s4-0)
            (vector-copy! s1-0 (-> s4-0 uvec))
            (vector-copy! s2-0 (-> s4-0 fvec))
            0
            )
          (vector-copy! s3-0 sv-528)
          0
          )
        )
      )
    0
    (none)
    )
  )

(defmethod relocate ((this chain-physics) (offset int))
  (dotimes (v1-0 (the-as int (-> this num-joints)))
    (if (nonzero? (-> this chain-joints v1-0 joint-mod))
        (&+! (-> this chain-joints v1-0 joint-mod) offset)
        )
    )
  this
  )

(defun chain-physics-initialize ((arg0 process-drawable) (arg1 chain-physics) (arg2 int) (arg3 float) (arg4 (array chain-physics-setup)))
  (set! (-> arg1 num-joints) (the-as uint (min 20 (-> arg4 length))))
  (dotimes (s1-0 (the-as int (-> arg1 num-joints)))
    (set! (-> arg1 chain-joints s1-0 joint-mod)
          (new 'process 'joint-mod (joint-mod-mode flex-blend) arg0 (-> arg4 s1-0 joint-index))
          )
    (set! (-> arg1 chain-joints s1-0 joint-mod track-mode) (track-mode no-scale))
    )
  (set! (-> arg1 root-joint-index) (the-as uint arg2))
  (set! (-> arg1 joint-length) arg3)
  (set-vector! (-> arg1 gravity) 0.0 -1.0 0.0 1.0)
  (vector-copy! (-> arg1 gravity-target) (-> arg1 gravity))
  (set! (-> arg1 stretch-vel) 0.7)
  (set! (-> arg1 stretch-vel-parallel) 0.8)
  (set! (-> arg1 compress-vel) 0.85)
  (set! (-> arg1 compress-vel-parallel) 0.75)
  (set! (-> arg1 negate-y) #f)
  (set! (-> arg1 axial-slop) 3640.889)
  (set! (-> arg1 maximum-stretch) 1.5)
  (set! (-> arg1 turn-off-start) 0)
  0
  )
