package physics

import "bvh"
import "common:name"
import "core:fmt"
import "core:log"
import "core:math"
import lg "core:math/linalg"
import "core:mem"
import "core:slice"
import "core:strings"
import "game:debug"
import he "game:halfedge"
import "game:ui"
import rl "libs:raylib"
import "libs:raylib/rlgl"
import "libs:tracy"

_ :: name
_ :: mem
_ :: fmt
_ :: log
_ :: math
_ :: debug
_ :: he

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)
	shape_mat := lg.matrix4_translate_f32(shape.rel_x) * lg.matrix4_from_quaternion(shape.rel_q)
	mat *= auto_cast shape_mat

	rlgl.PushMatrix()
	defer rlgl.PopMatrix()

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

	switch s in shape.inner_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)
		he.debug_draw_mesh_wires(mesh, color)
	}
}

Debug_State :: struct {
	selected_contacts: map[int]struct {},
}

init_debug_state :: proc(debug_state: ^Debug_State) {
	debug_state.selected_contacts = make(map[int]struct {}, context.temp_allocator)
}

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

	sim_state := get_sim_state(scene)

	// Static_TLAS
	if true && sim_state.static_tlas.built {
		bvh.debug_draw_bvh_bounds(
			&sim_state.static_tlas.bvh_tree,
			sim_state.static_tlas.level_geom_aabbs,
			0,
		)
	}

	// Dynamic TLAS
	if false && sim_state.dynamic_tlas.built {
		bvh.debug_draw_bvh_bounds(
			&sim_state.dynamic_tlas.bvh_tree,
			sim_state.dynamic_tlas.body_aabbs,
			0,
		)
	}

	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)

			it := shapes_iterator(sim_state, body.shape)
			for shape in shapes_iterator_next(&it) {
				draw_debug_shape(
					sim_state,
					shape^,
					body_get_shape_pos(body),
					body.q,
					debug.int_to_color(i32(i + 2)),
				)

				shape_aabb := body_transform_shape_aabb(body, shape_get_aabb(shape^))
				rl.DrawBoundingBox(
					rl.BoundingBox {
						min = shape_aabb.center - shape_aabb.extent,
						max = shape_aabb.center + shape_aabb.extent,
					},
					debug.int_to_color(i32(i + 2)),
				)
			}
		}
	}

	// if true {
	// 	for &level_geom, geom_idx in sim_state.level_geoms {
	// 		if level_geom.alive {
	// 			vertices, indices := get_level_geom_data(sim_state, level_geom.geometry)

	// 			for i in 0 ..< len(indices) / 3 {
	// 				i1, i2, i3 := indices[i * 3 + 0], indices[i * 3 + 1], indices[i * 3 + 2]
	// 				v1, v2, v3 := vertices[i1], vertices[i2], vertices[i3]

	// 				rl.DrawTriangle3D(v1, v2, v3, debug.int_to_color(i32(geom_idx + 1)))
	// 			}
	// 		}
	// 	}
	// }

	for _, i in sim_state.suspension_constraints {
		wheel := &sim_state.suspension_constraints_slice[i]
		if wheel.alive {
			body := get_body(sim_state, wheel.body)

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

			wheel_world_pos := body_local_to_world(body, wheel.rel_pos + body.shape_offset)
			dir := body_local_to_world_vec(body, wheel.rel_dir)
			rl.DrawLine3D(wheel_world_pos, wheel_world_pos + dir, rl.BLUE)

			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)

			rotation_dir := body_local_to_world_vec(
				body,
				lg.matrix3_rotate(wheel.q, Vec3{-1, 0, 0}) * Vec3{0, 1, 0},
			)

			rl.DrawLine3D(
				wheel_pos - right * 0.1,
				wheel_pos - right * 0.1 + rotation_dir * wheel.radius,
				rl.ORANGE,
			)
			rl.DrawLine3D(
				wheel_pos + right * 0.1,
				wheel_pos + right * 0.1 + rotation_dir * wheel.radius,
				rl.ORANGE,
			)

			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 {
			if contact_idx in debug_state.selected_contacts ||
			   len(debug_state.selected_contacts) == 0 {
				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,
				)
				b_handle :=
					Body_Handle(contact.b) if contact.type == .Body_vs_Body else INVALID_BODY
				debug_transform_points_local_to_world(
					get_body(sim_state, b_handle),
					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)),
				)
			}
		}
	}
}

draw_debug_ui :: proc(
	debug_state: ^Debug_State,
	ctx: ^ui.Context,
	scene: ^Scene,
	config: Solver_Config,
) {
	tracy.Zone()

	sim_state := get_sim_state(scene)

	if .ACTIVE in ui.treenode(ctx, "Bodies") {
		for _, i in sim_state.bodies_slice {
			body := &sim_state.bodies_slice[i]
			if body.alive {
				if .ACTIVE in
				   ui.treenode(ctx, fmt.tprintf("%v (%v)", name.to_string(body.name), i)) {
					ui.keyval(ctx, "Pos", body.x)
					ui.keyval(ctx, "Shape Offset", body.shape_offset)
					aabb := body_get_aabb(body)
					ui.keyval(ctx, "AABB", aabb)
					ui.keyval(ctx, "Inv Inertia", body.inv_inertia_tensor)
				}
			}
		}
	}

	if .ACTIVE in ui.treenode(ctx, "Contacts") {
		@(static) search_buf: [1024]u8
		@(static) search_len: int
		ui.textbox(ctx, search_buf[:], &search_len)
		search_str := string(search_buf[0:search_len])
		Contact_And_Index :: struct {
			contact: Contact,
			idx:     int,
		}
		sorted_contacts := make(
			[]Contact_And_Index,
			len(sim_state.contact_container.contacts),
			context.temp_allocator,
		)
		for &contact, i in sim_state.contact_container.contacts {
			sorted_contacts[i] = Contact_And_Index {
				contact = contact,
				idx     = i,
			}
		}
		slice.reverse_sort_by_key(
			sorted_contacts,
			#force_inline proc(contact_and_index: Contact_And_Index) -> f32 {
				return lg.max(contact_and_index.contact.total_normal_impulse)
			},
		)
		for &contact_and_index, i in sorted_contacts {
			contact := contact_and_index.contact
			title: string

			ui.push_id(ctx, mem.any_to_bytes(i))
			defer ui.pop_id(ctx)

			if contact.type == .Body_vs_Body {
				title = fmt.tprintf(
					"%d %v v %v",
					contact_and_index.idx,
					name.to_string(get_body(sim_state, contact.a).name),
					name.to_string(get_body(sim_state, Body_Handle(contact.b)).name),
				)
			} else {
				title = fmt.tprintf(
					"%d %v v lvl",
					contact_and_index.idx,
					name.to_string(get_body(sim_state, contact.a).name),
				)
			}

			if strings.contains(title, search_str) {
				if ui.inspect_value(ctx, title, contact) {
					debug_state.selected_contacts[contact_and_index.idx] = {}
				}
			}
		}
	}

	if .ACTIVE in ui.treenode(ctx, "Static TLAS") {
		if sim_state.static_tlas.built {
			bvh.debug_ui_bvh_node_recursive(
				ctx,
				sim_state.static_tlas.bvh_tree,
				sim_state.static_tlas.level_geom_aabbs,
				0,
			)
		}
	}

	if .ACTIVE in ui.treenode(ctx, "Dynamic TLAS") {
		if sim_state.dynamic_tlas.built {
			bvh.debug_ui_bvh_node_recursive(
				ctx,
				sim_state.dynamic_tlas.bvh_tree,
				sim_state.dynamic_tlas.body_aabbs,
				0,
			)
		}
	}

	// active_wheels := []int{0, 1}

	// w, h: i32 = 500, 500

	// window_x: i32 = 0

	// for i in 0 ..< len(sim_state.suspension_constraints_slice) {
	// 	s := &sim_state.suspension_constraints_slice[i]

	// 	if s.alive {
	// 		for idx in active_wheels {
	// 			if i == idx {
	// 				if ui.window(
	// 					ctx,
	// 					fmt.tprintf("Wheel %v", i),
	// 					ui.Rect{x = window_x, y = 0, w = w, h = h},
	// 					ui.Options{},
	// 				) {
	// 					NUM_SAMPLES :: 100

	// 					dt := f32(config.timestep) / f32(config.substreps_minus_one + 1)
	// 					inv_dt := 1.0 / dt

	// 					{
	// 						ui.layout_row(ctx, {-1}, 300)
	// 						{
	// 							ui.begin_line(ctx, ui.Color{255, 0, 0, 255})
	// 							defer ui.end_line(ctx)

	// 							for j in 0 ..< NUM_SAMPLES {
	// 								alpha := f32(j) / f32(NUM_SAMPLES - 1)
	// 								x := alpha * 200.0 - 100.0

	// 								long_friction := abs(
	// 									pacejka_94_longitudinal(
	// 										s.pacejka_long,
	// 										x,
	// 										max(abs(s.spring_impulse), 0.001) * inv_dt * 0.001,
	// 									),
	// 								)

	// 								ui.push_line_point(
	// 									ctx,
	// 									ui.Vec2f{alpha, long_friction * -0.5 + 1},
	// 								)
	// 							}

	// 							long_friction := abs(
	// 								pacejka_94_longitudinal(
	// 									s.pacejka_long,
	// 									s.slip_ratio,
	// 									max(abs(s.spring_impulse), 0.001) * inv_dt * 0.001,
	// 								),
	// 							)

	// 							rect := ui.get_line(ctx).rect

	// 							cur_point :=
	// 								Vec2 {
	// 										(s.slip_ratio + 100.0) / 200.0,
	// 										long_friction * -0.5 + 1,
	// 									} *
	// 									Vec2{f32(rect.w), f32(rect.h)} +
	// 								Vec2{f32(rect.x), f32(rect.y)}
	// 							ui.draw_rect(
	// 								ctx,
	// 								ui.rect_from_point_extent(
	// 									ui.Vec2{i32(cur_point.x), i32(cur_point.y)},
	// 									2,
	// 								),
	// 								ui.Color{255, 255, 0, 255},
	// 							)
	// 						}
	// 					}

	// 					{
	// 						ui.layout_row(ctx, {-1}, 300)
	// 						ui.begin_line(ctx, ui.Color{0, 255, 0, 255})
	// 						defer ui.end_line(ctx)

	// 						for j in 0 ..< NUM_SAMPLES {
	// 							alpha := f32(j) / f32(NUM_SAMPLES - 1)
	// 							x := alpha * 180.0 - 90.0

	// 							lat_friction := abs(
	// 								pacejka_94_lateral(
	// 									s.pacejka_lat,
	// 									x,
	// 									max(abs(s.spring_impulse), 0.001) * inv_dt * 0.001,
	// 									0.0,
	// 								),
	// 							)

	// 							ui.push_line_point(ctx, ui.Vec2f{alpha, lat_friction * -0.5 + 1})
	// 						}

	// 						lat_friction := abs(
	// 							pacejka_94_lateral(
	// 								s.pacejka_lat,
	// 								s.slip_angle,
	// 								max(abs(s.spring_impulse), 0.001) * inv_dt * 0.001,
	// 								0.0,
	// 							),
	// 						)

	// 						rect := ui.get_line(ctx).rect

	// 						cur_point :=
	// 							Vec2{(s.slip_angle + 100.0) / 200.0, lat_friction * -0.5 + 1} *
	// 								Vec2{f32(rect.w), f32(rect.h)} +
	// 							Vec2{f32(rect.x), f32(rect.y)}
	// 						ui.draw_rect(
	// 							ctx,
	// 							ui.rect_from_point_extent(
	// 								ui.Vec2{i32(cur_point.x), i32(cur_point.y)},
	// 								2,
	// 							),
	// 							ui.Color{255, 255, 0, 255},
	// 						)
	// 					}

	// 					window_x += w
	// 				}
	// 			}
	// 		}
	// 	}
	// }
}

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