package physics

import "collision"
import lg "core:math/linalg"
import "game:halfedge"
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_Box:
		tensor = inertia_tensor_box(s.size)
	case Shape_Convex:
		// TODO: assuming precomputed
		tensor = s.inertia_tensor
	}

	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)
}

body_get_shape_offset_local :: proc(body: Body_Ptr) -> (offset: rl.Vector3) {
	#partial switch s in body.shape {
	case Shape_Convex:
		offset = -s.center_of_mass
	}
	return
}

// Returns the shape's world position
// Shape can be offset from COM
body_get_shape_pos :: proc(body: Body_Ptr) -> rl.Vector3 {
	offset := body_get_shape_offset_local(body)
	return body_local_to_world(body, offset)
}

body_get_convex_shape_world :: proc(
	body: Body_Ptr,
	allocator := context.temp_allocator,
) -> (
	mesh: collision.Convex,
) {
	switch s in body.shape {
	case Shape_Box:
		mesh = collision.box_to_convex(collision.Box{rad = s.size * 0.5}, allocator)
	case Shape_Convex:
		mesh = halfedge.copy_mesh(s.mesh, context.temp_allocator)
	}

	transform :=
		lg.matrix4_translate(body_get_shape_pos(body)) * lg.matrix4_from_quaternion(body.q)
	halfedge.transform_mesh(&mesh, transform)

	return
}