;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Unused version of line sphere, touching list test
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;; it looks like there's two ways to collide a ray with the background.
;; the "old version" is slow, but simpler. So let's start with that.

;; The inputs here are an "origin" point, a "line", and a radius.
;; From this, we build an axis aligned bounding box, then find the collide-frag-meshes that are inside.
;; It's about the simplest possible implementation, but it's a nice check to see if we really understand what's going on.

(defmacro pabsw-hack (out in)
  `(let ((temp (new 'stack-no-clear 'array 'int32 4)))
     (set! (-> (the (pointer uint128) temp)) ,in)
     (set! (-> temp 0) (abs (-> temp 0)))
     (set! (-> temp 1) (abs (-> temp 1)))
     (set! (-> temp 2) (abs (-> temp 2)))
     (set! (-> temp 3) (abs (-> temp 3)))
     (set! ,out (-> (the (pointer uint128) temp)))
     )
  )

;; This is mostly stolen from collide-cache.gc. It builds the bounding box we'll use for later tests.
;; The bounding box is stored in *collide-work*.
(defun setup-collide-for-line-sphere ((origin vector) (line vector) (radius float))
  (local-vars (a0-2 uint128) (a0-3 uint128) (a0-7 float) (t0-1 uint128) (t0-2 uint128) (zero uint128))
  (rlet ((acc :class vf) (Q :class vf) (vf0 :class vf) (vf1 :class vf) (vf10 :class vf) (vf11 :class vf) (vf12 :class vf)
         (vf13 :class vf) (vf14 :class vf) (vf15 :class vf) (vf16 :class vf) (vf17 :class vf) (vf18 :class vf) (vf19 :class vf)
         (vf2 :class vf) (vf20 :class vf) (vf21 :class vf) (vf22 :class vf) (vf23 :class vf) (vf3 :class vf) (vf4 :class vf)
         (vf5 :class vf) (vf6 :class vf) (vf7 :class vf) (vf8 :class vf) (vf9 :class vf))
    (init-vf0-vector)
    (set! zero (the uint128 0))
    (let ((v1-0 (new 'static 'vector4w :x #x1000 :y #x1000 :z #x1000)))
      (.mov vf9 (the uint128 radius))  ;; vf9.x = radius
      (.lvf vf3 (&-> line quad))       ;; vf3 = line
      (let ((a0-1 (-> v1-0 quad)))     ;; a0-1 = 0x1000, 
        (.ftoi.vf vf21 vf3)            ;; vf21 is the line
        (.lvf vf1 (&-> origin quad))   ;; vf1 is the origin
        (.mul.vf vf8 vf3 vf3)          ;; 
        (let ((v1-1 *collide-work*))
          (.add.vf vf2 vf1 vf3)        ;; vf2 is the endpoint
          ;;(set! (-> obj ignore-mask) pat-ignore)
          (.mov t0-1 vf21)             ;; t0-1 is the line
          (pabsw-hack t0-2 t0-1)       ;; t0-2 is the line, absolute value
          (.pcgtw a0-2 t0-2 a0-1)
          (.ppach a0-3 zero a0-2)
          (let ((a0-4 (shl (the-as int a0-3) 16)))
            (b! (nonzero? a0-4) cfg-3)
            )
          (format 0 "BAD case in setup-collide-for-line-sphere")
          (b! #t cfg-13 :delay (nop!))
          
          (label cfg-3)
          (.add.y.vf vf8 vf8 vf8 :mask #b1)
          ;;(set! (-> obj num-tris) 0)
          (.min.vf vf4 vf1 vf2)
          ;;(set! (-> obj num-prims) 0)
          (.max.vf vf5 vf1 vf2)
          ;;(set! (-> obj collide-with) collide-with)
          (.sub.x.vf vf10 vf0 vf9 :mask #b111)
          ;;(set! (-> obj proc) proc)
          (.add.z.vf vf8 vf8 vf8 :mask #b1)
          (.sub.x.vf vf4 vf4 vf9 :mask #b111)
          (.add.x.vf vf5 vf5 vf9 :mask #b111)
          (.ftoi.vf vf15 vf10)
          (.isqrt.vf Q vf0 vf8 :fsf #b11 :ftf #b0)
          (.add.x.vf vf11 vf0 vf9 :mask #b111)
          (.svf (&-> v1-1 collide-box4w min quad) vf15)
          (.ftoi.vf vf6 vf4)
          ;;(.svf (&-> obj collide-box min quad) vf4)
          (.ftoi.vf vf7 vf5)
          ;;(.svf (&-> obj collide-box max quad) vf5)
          (.mov.vf vf13 vf0)
          ;;(.svf (&-> obj collide-box4w min quad) vf6)
          ;;(.svf (&-> obj collide-box4w max quad) vf7)
          (.wait.vf)
          (.add.vf vf8 vf0 Q :mask #b1)
          (.mul.x.vf vf12 vf3 vf8)
          (.div.vf Q vf0 vf8 :fsf #b11 :ftf #b0)
          (.mul.vf vf22 vf12 vf12)
          (.abs.vf vf23 vf12)
          (.add.y.vf vf22 vf22 vf22 :mask #b1)
          (.mov a0-7 vf23)
          (.wait.vf)
          (.add.vf vf8 vf0 Q :mask #b1)
          (b! (zero? a0-7) cfg-6 :likely-delay (.add.z.vf vf13 vf0 vf12 :mask #b1))
          (.sub.y.vf vf13 vf0 vf12 :mask #b1)
          (.isqrt.vf Q vf0 vf22 :fsf #b11 :ftf #b0)
          (.add.x.vf vf13 vf0 vf12 :mask #b10)
          (.wait.vf)
          (.mul.vf vf13 vf13 Q :mask #b11)
          (label cfg-6)
          (.outer.product.a.vf acc vf12 vf13)
          (.add.x.vf vf11 vf11 vf8 :mask #b1)
          (.outer.product.b.vf vf14 vf13 vf12 acc)
          (.ftoi.vf vf16 vf11)
          (.mov.vf vf17 vf12)
          (.mov.vf vf18 vf13)
          (.mov.vf vf19 vf14)
          (.mov.vf vf17 vf0 :mask #b1110)
          (.svf (&-> v1-1 collide-box4w max quad) vf16)
          (.mov.vf vf18 vf0 :mask #b1101)
          (.mov.vf vf19 vf0 :mask #b1011)
          (.add.x.vf vf17 vf17 vf13 :mask #b10)
          (.add.y.vf vf18 vf18 vf12 :mask #b1)
          (.add.z.vf vf19 vf19 vf12 :mask #b1)
          (.add.x.vf vf17 vf17 vf14 :mask #b100)
          (.add.y.vf vf18 vf18 vf14 :mask #b100)
          (.add.z.vf vf19 vf19 vf13 :mask #b10)
          (.mul.x.vf acc vf17 vf1)
          (.add.mul.y.vf acc vf18 vf1 acc)
          (.svf (&-> v1-1 inv-mat vector 0 quad) vf17)
          (.add.mul.z.vf vf20 vf19 vf1 acc)
          (.svf (&-> v1-1 inv-mat vector 1 quad) vf18)
          (.sub.vf vf20 vf0 vf20)
          (.svf (&-> v1-1 inv-mat vector 2 quad) vf19)
          (.svf (&-> v1-1 inv-mat vector 3 quad) vf20)
          (label cfg-13)
          )
        )
      )
  )
  )

;; convert vector of ints to floats.
(defmacro vector-as-floats (in)
  `(let ((vec (new 'stack-no-clear 'vector)))
     (rlet ((reg :class vf))
           (.lvf reg (&-> ,in quad))
           (.itof.vf reg reg)
           (.svf (&-> vec quad) reg)
           )
     vec
     )
  )

;; incrementing counter to make the test input move around.
(define *debug-counter* 0)

(defun debug-draw-collide-work ((my-off float))
  (when (not (paused?))
  (+! *debug-counter* 1)
    
    )
  
  ;; create a test input that moves around sandover
  (let ((origin (new 'static 'vector :y (meters 7.0) :x (meters 5.) :z (meters 0.0)))
        (line (new 'static 'vector :y (meters 5.0) :x (meters -5.0)))
        (radius (meters 2.0))
        (val (+ my-off (* 0.003 (the float *debug-counter*))))
        )
    
    (set! (-> line z) (* (sin (degrees val)) (meters 35.0)))
    (set! (-> line x) (* (cos (degrees val)) (meters 35.0)))
    
    (let ((dbg2 (/ val 2)))
      (set! (-> origin x) (* (sin (degrees dbg2)) (meters 100.)))
      (set! (-> origin z) (* (cos (* 3.0 (degrees dbg2))) (meters 100.)))
      (+! (-> origin x) (meters -50.))
      )
#|
    ;;; PROBE (at the frag finding level)
    
    ;; setup the collide work
    (setup-collide-for-line-sphere origin line radius)
    

    
    ;; clear collide list
    (set! (-> *collide-list* num-items) 0)
    
    ;; loop over active levels
    (dotimes (lev-idx (-> *level* length))
      (let ((lev (-> *level* level lev-idx)))
        (when (= (-> lev status) 'active)
          ;; do the collision check!
          (collide-ray (-> lev bsp) 0 *collide-list*)
          )
        )
      )
    
    ;; print results
    (format *stdcon* "got ~d in list~%" (-> *collide-list* num-items))
    (format *stdcon* "cam at: ~`vector`P~%" (camera-pos))
    
    (let ((old-count 0))
      ;; visualize results
      (dotimes (i (-> *collide-list* num-items))
        (let ((mesh (the collide-frag-mesh (-> *collide-list* items i mesh)))
              (inst (the instance-tie (-> *collide-list* items i inst))))
          (when inst
            (+! old-count 1)
            (add-debug-sphere #t (bucket-id debug-draw0) (-> inst bsphere) (-> inst bsphere w) (new 'static 'rgba :b #x80 :a #x80))
            )
          
          (when (not inst)
            (+! old-count 1)
            ;;(format *stdcon* "NI: ~A~%" mesh)
            (add-debug-sphere #t (bucket-id debug-draw0) (vector-as-floats (-> mesh base-trans)) (meters 5.0) (new 'static 'rgba :g #x80 :a #x80))
            )
          ;
          )
        )
      
      ;; now test the other one
      (set! (-> *collide-list* num-items) 0)
      (collide-probe-make-list (-> *level* level 0) *collide-list*)
      
      ;; these should come out the same, if we did it right.
      (format *stdcon* "COMPARE: ~d ~d~%" (-> *collide-list* num-items) old-count)
      
      (dotimes (i (-> *collide-list* num-items))
        (let ((mesh (the collide-frag-mesh (-> *collide-list* items i mesh)))
              (inst (the instance-tie (-> *collide-list* items i inst))))
          (when inst
            (add-debug-sphere #t (bucket-id debug-draw0) (-> inst bsphere) (* 0.95 (-> inst bsphere w)) (new 'static 'rgba :b #x80 :r #x80 :a #x80))
            )
          
          (when (not inst)
            ;;(format *stdcon* "NI: ~A~%" mesh)
            (add-debug-sphere #t (bucket-id debug-draw0) (vector-as-floats (-> mesh base-trans)) (meters 6.0) (new 'static 'rgba :g #x30 :a #x80))
            )
          ;
          )
        )
      )
|#
    
    ;; CCACHE (fill from bounding box)
    (let ((bbox-fake (new 'stack 'bounding-box))
          (bbox (new 'stack 'bounding-box)))
      
      ;; make box
      (set! (-> bbox-fake min quad) (-> origin quad))
      (vector+! (-> bbox-fake max) origin line)
      
      (dotimes (i 3)
        (set! (-> bbox min data i) (fmin (-> bbox-fake min data i) (-> bbox-fake max data i)))
        (set! (-> bbox max data i) (fmax (-> bbox-fake min data i) (-> bbox-fake max data i)))
        )
      (set! (-> bbox min w) 1.0)
      (set! (-> bbox max w) 1.0)
      
      ;; BBOX
      ;; draw box
      ;(add-debug-box #t (bucket-id debug-draw0) (-> bbox min) (-> bbox max) (new 'static 'rgba :g #x30 :a #x80))
      ;; now fill!
      ; (fill-using-bounding-box *collide-cache*
      ;                          bbox
      ;                          (collide-kind background)
      ;                          (the process-drawable #f)
      ;                          (the pat-surface 0)
      ;                          )
      
      

      ;; Y PROBE
      ; (+! (-> bbox min y) (meters 8.0))
      ; (let ((vec (new 'stack-no-clear 'vector)))
      ;   (set! (-> vec quad) (-> bbox min quad))
      ;   (-! (-> vec y) (meters 20.0))
      ;   (add-debug-line 
      ;     #t 
      ;     (bucket-id debug-draw0)
      ;     (-> bbox min)
      ;     vec
      ;     (new 'static 'rgba :r #x80 :a #x80)
      ;     #f
      ;     (the rgba -1)
      ;     )
      ;   )
      ; (fill-using-y-probe *collide-cache* (-> bbox min) (meters 20.0) (collide-kind background) (the process-drawable #f) (the uint 0))
      
      ;; LINE SPHERE
      ; (fill-using-line-sphere *collide-cache* origin line radius (collide-kind background) (the process-drawable #f) 0)
      ; ;; draw the line we're going to check
      ; (add-debug-line 
      ;   #t 
      ;   (bucket-id debug-draw0)
      ;   origin
      ;   (vector+! (new 'stack-no-clear 'vector) origin line)
      ;   (new 'static 'rgba :r #x80)
      ;   #f
      ;   (the rgba -1)
      ;   )
      
      ; ;; debug draw spheres along the line
      ; (dotimes (i 10)
      ;   (add-debug-sphere
      ;     #t
      ;     (bucket-id debug-draw0)
      ;     (vector-lerp! (new 'stack-no-clear 'vector)
      ;                   origin
      ;                   (vector+! (new 'stack-no-clear 'vector) origin line)
      ;                   (/ (the float i) 9.0)
      ;                   )
      ;     radius
      ;     (new 'static 'rgba :g #x80 :r #x80 :b #x80 :a #x80)
      ;     )
      ;   )
      
      ;; Y probe (actual probe)
      (+! (-> bbox min y) (meters 8.0))
      (let ((vec (new 'stack-no-clear 'vector)))
        (set! (-> vec quad) (-> bbox min quad))
        (-! (-> vec y) (meters 20.0))
        ; (add-debug-line 
        ;   #t 
        ;   (bucket-id debug-draw0)
        ;   (-> bbox min)
        ;   vec
        ;   (new 'static 'rgba :r #x80 :a #x80)
        ;   #f
        ;   (the rgba -1)
        ;   )
        )
      
      (let ((result (new 'stack-no-clear 'collide-tri-result)))
        
        (let ((u (fill-and-probe-using-y-probe *collide-cache* (-> bbox min) (meters 20.0) (collide-kind background) (the process-drawable #f) result (the uint 0))))
          (-! (-> bbox min y) (* u (meters 20.0)))
          ;; (format *stdcon* "u = ~f : ~`vector`P ~%" u (-> result vertex 0))
          (add-debug-sphere
            #t
            (bucket-id debug-draw0)
            (-> bbox min)
            (meters 2.0)
            (new 'static 'rgba :r #x80 :a #x80)
            )
          )
         ;(add-debug-outline-triangle #t (bucket-id debug-draw1) (-> result vertex 0) (-> result vertex 1) (-> result vertex 2) (new 'static 'rgba :r #x80 :g #x80 :a #x80))
        
        
        
        
        )
      
      
      
      ;(debug-draw *collide-cache*)
      
      
      )
    
    #f
    )
  )

(defun start-debug-drawer ()
  ;;(kill-by-name 'debug-drawer *active-pool*)
  (let ((proc (get-process *nk-dead-pool* process 1024)))
    (activate proc *active-pool* 'debug-drawer *kernel-dram-stack*)
    (run-next-time-in-process 
      proc
      (lambda ()
        (let ((my-off (rand-vu-float-range 0.0 1000.0)))
          (while #t
            (debug-draw-collide-work my-off)
            (suspend)
            )
          
          
          )
        )
      )                      
    )
  )

(defun start-100-debug-drawer ()
  (dotimes (i 100)
    (start-debug-drawer)
    )
  )