package physics

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

inertia_tensor_sphere :: proc(radius: f32) -> (tensor: Matrix3) {
	tensor = radius * radius * (2.0 / 3.0)

	return
}

inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: Matrix3) {
	CONSTANT :: f32(1.0 / 12.0)

	tensor[0][0] = size.z * size.z + size.y * size.y
	tensor[1][1] = size.x * size.x + size.z * size.z
	tensor[2][2] = size.x * size.x + size.y * size.y

	tensor = tensor * Matrix3(CONSTANT)

	return
}

inertia_tensor_collision_shape :: proc(shape: Collision_Shape) -> (tensor: Matrix3) {
	switch s in shape {
	case Shape_Sphere:
		tensor = inertia_tensor_sphere(s.radius)
	case Shape_Box:
		tensor = inertia_tensor_box(s.size)
	}

	return
}

body_local_to_world :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
	return body.x + lg.quaternion_mul_vector3(body.q, pos)
}

body_local_to_world_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 {
	return lg.normalize0(lg.quaternion_mul_vector3(body.q, vec))
}

body_world_to_local :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
	inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(body.q))
	return lg.quaternion_mul_vector3(inv_q, pos - body.x)
}

body_world_to_local_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 {
	inv_q := lg.quaternion_inverse(body.q)
	return lg.normalize0(lg.quaternion_mul_vector3(inv_q, vec))
}

body_velocity_at_point :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
	return body.v + lg.cross(body.w, pos - body.x)
}

wheel_get_rel_wheel_pos :: #force_inline proc(
	body: Body_Ptr,
	wheel: Suspension_Constraint_Ptr,
) -> rl.Vector3 {
	t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest
	return wheel.rel_pos + wheel.rel_dir * (t - wheel.radius)
}

wheel_get_right_vec :: #force_inline proc(
	body: Body_Ptr,
	wheel: Suspension_Constraint_Ptr,
) -> rl.Vector3 {
	local_right := lg.quaternion_mul_vector3(
		lg.quaternion_angle_axis(wheel.turn_angle, rl.Vector3{0, 1, 0}),
		rl.Vector3{1, 0, 0},
	)
	return body_local_to_world_vec(body, local_right)
}