package physics

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

_ :: log
_ :: math

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 _, i in scene.suspension_constraints {
		wheel := &scene.suspension_constraints_slice[i]
		if wheel.alive {
			body := get_body(scene, wheel.body)
			t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest

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

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

			rel_wheel_pos := wheel_get_rel_wheel_pos(body, wheel)
			wheel_pos := body_local_to_world(body, rel_wheel_pos)
			right := wheel_get_right_vec(body, wheel)

			rl.DrawCylinderWiresEx(
				wheel_pos - right * 0.1,
				wheel_pos + right * 0.1,
				wheel.radius,
				wheel.radius,
				16,
				rl.RED,
			)

			rl.DrawLine3D(
				pos + t * dir,
				pos + t * dir + wheel.applied_impulse.x * right * 10,
				rl.RED,
			)

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