package physics

import "collision"
import lg "core:math/linalg"
import "game:halfedge"

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

	return
}

inertia_tensor_box :: proc(size: Vec3) -> (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: Input_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: Vec3) -> Vec3 {
	return body.x + lg.quaternion_mul_vector3(body.q, pos)
}

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

body_world_to_local :: #force_inline proc(body: Body_Ptr, pos: Vec3) -> Vec3 {
	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: Vec3) -> Vec3 {
	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: Vec3) -> Vec3 {
	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,
) -> Vec3 {
	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,
) -> Vec3 {
	local_right := lg.quaternion_mul_vector3(
		lg.quaternion_angle_axis(wheel.turn_angle, Vec3{0, 1, 0}),
		Vec3{1, 0, 0},
	)
	return body_local_to_world_vec(body, local_right)
}

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

body_get_shape_offset_local :: proc(body: Body_Ptr) -> (offset: Vec3) {
	#partial switch s in body.shape {
	case Internal_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) -> Vec3 {
	offset := body_get_shape_offset_local(body)
	return body_local_to_world(body, offset)
}

body_get_convex_shape_world :: proc(
	sim_state: ^Sim_State,
	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 Internal_Shape_Convex:
		mesh = convex_container_get_mesh(&sim_state.convex_container, s.mesh)
		// TODO: make sure this works as intendent
		mesh = halfedge.copy_mesh(mesh, allocator)
	}

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

	return
}

shape_get_aabb :: proc(shape: Collision_Shape) -> (aabb: AABB) {
	switch s in shape {
	case Shape_Box:
		aabb.center = 0
		aabb.extent = s.size * 0.5
	case Internal_Shape_Convex:
		aabb.center = s.center
		aabb.extent = s.extent
	}

	return aabb
}

body_get_aabb :: proc(body: Body_Ptr) -> (aabb: AABB) {
	local_aabb := shape_get_aabb(body.shape)

	aabb_center_local_to_body := body_get_shape_offset_local(body) + local_aabb.center
	aabb.center = body_local_to_world(body, aabb_center_local_to_body)

	rotation_mat := lg.matrix3_from_quaternion(body.q)
	aabb.extent = lg.abs(local_aabb.extent.xxx * rotation_mat[0])
	aabb.extent += lg.abs(local_aabb.extent.yyy * rotation_mat[1])
	aabb.extent += lg.abs(local_aabb.extent.zzz * rotation_mat[2])

	return aabb
}