package physics

import "core:log"
import lg "core:math/linalg"
import rl "vendor:raylib"

_ :: log

draw_debug_scene :: proc(scene: ^Scene) {
	for &body in scene.bodies {
		if body.alive {
			pos := body.x

			q := body.q
			x := lg.quaternion_mul_vector3(q, rl.Vector3{1, 0, 0})
			y := lg.quaternion_mul_vector3(q, rl.Vector3{0, 1, 0})
			z := lg.quaternion_mul_vector3(q, rl.Vector3{0, 0, 1})

			rl.DrawLine3D(pos, pos + x, rl.RED)
			rl.DrawLine3D(pos, pos + y, rl.GREEN)
			rl.DrawLine3D(pos, pos + z, rl.BLUE)
		}
	}

	for &constraint in scene.suspension_constraints {
		if constraint.alive {
			body := get_body(scene, constraint.body)
			t := constraint.hit_t > 0 ? constraint.hit_t : constraint.rest

			pos := body.x
			rot := body.q
			pos += lg.quaternion_mul_vector3(rot, constraint.rel_pos)
			dir := lg.quaternion_mul_vector3(rot, constraint.rel_dir)

			rl.DrawLine3D(pos, pos + dir * t, rl.ORANGE)

			if constraint.hit {
				rl.DrawSphereWires(constraint.hit_point, 0.1, 4, 4, rl.RED)
			}
		}
	}
}