package physics

import "core:log"
import "core:math"
import lg "core:math/linalg"
import "game:debug"
import "game:halfedge"
import "libs:tracy"
import rl "vendor:raylib"
import "vendor:raylib/rlgl"

_ :: log
_ :: math
_ :: debug

draw_debug_shape :: proc(
	sim_state: ^Sim_State,
	shape: Collision_Shape,
	pos: Vec3,
	rot: Quat,
	color: rl.Color,
) {
	mat := lg.matrix4_from_trs(pos, rot, 1)

	rlgl.PushMatrix()
	defer rlgl.PopMatrix()

	rlgl.LoadIdentity()
	rlgl.MultMatrixf(cast([^]f32)&mat)

	switch s in shape {
	case Shape_Box:
		rl.DrawCubeV(0, s.size, color)
	case Internal_Shape_Convex:
		mesh := convex_container_get_mesh(&sim_state.convex_container, s.mesh)
		halfedge.debug_draw_mesh_wires(mesh, color)
	}
}

draw_debug_scene :: proc(scene: ^Scene) {
	tracy.Zone()

	sim_state := get_sim_state(scene)

	for _, i in sim_state.bodies {
		body := &sim_state.bodies_slice[i]
		if body.alive {
			pos := body.x

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

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

			draw_debug_shape(
				sim_state,
				body.shape,
				body_get_shape_pos(body),
				body.q,
				debug.int_to_color(i32(i + 2)),
			)
		}
	}

	for _, i in sim_state.suspension_constraints {
		wheel := &sim_state.suspension_constraints_slice[i]
		if wheel.alive {
			body := get_body(sim_state, 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(wheel_pos, wheel_pos + right * 10, rl.RED)

			if wheel.hit {
				// 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)
			}
		}
	}

	if true {
		for &contact, contact_idx in sim_state.contact_container.contacts {
			points_a := contact.manifold.points_a
			points_b := contact.manifold.points_b
			points_a_slice, points_b_slice :=
				points_a[:contact.manifold.points_len], points_b[:contact.manifold.points_len]
			debug_transform_points_local_to_world(get_body(sim_state, contact.a), points_a_slice)
			debug_transform_points_local_to_world(get_body(sim_state, contact.b), points_b_slice)
			debug_draw_manifold_points(
				contact,
				-1,
				points_a_slice,
				color = debug.int_to_color(i32(contact_idx * 2 + 0)),
			)
			debug_draw_manifold_points(
				contact,
				1,
				points_b_slice,
				color = debug.int_to_color(i32(contact_idx * 2 + 1)),
			)
		}
	}
}

debug_transform_points_local_to_world :: proc(body: Body_Ptr, points: []Vec3) {
	for i in 0 ..< len(points) {
		points[i] = body_local_to_world(body, points[i])
	}
}

debug_draw_manifold_points :: proc(
	contact: Contact,
	impulse_sign: f32,
	points: []Vec3,
	color: rl.Color,
) {
	if len(points) >= 3 {
		// Triangle or quad
		v1 := points[0]

		for i in 2 ..< len(points) {
			v2, v3 := points[i - 1], points[i]

			rl.DrawTriangle3D(v1, v2, v3, color)
		}
	} else if len(points) == 2 {
		// Line
		rl.DrawLine3D(points[0], points[1], color)
	}


	for point_idx in 0 ..< len(points) {
		p := points[point_idx]
		total_impulse :=
			contact.total_normal_impulse[point_idx] * contact.manifold.normal +
			contact.total_friction_impulse[point_idx].x * contact.manifold.tangent +
			contact.total_friction_impulse[point_idx].y * contact.manifold.bitangent
		rl.DrawLine3D(p, p + total_impulse * impulse_sign, color)

		// rl.DrawSphereWires(p, 0.1, 4, 4, color)
	}
}