package collision

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

Convex :: distinct halfedge.Half_Edge_Mesh

BOX_CORNERS_NORM :: [8]Vec3 {
	Vec3{-1, -1, -1},
	Vec3{-1, -1, 1},
	Vec3{-1, 1, -1},
	Vec3{-1, 1, 1},
	Vec3{1, -1, -1},
	Vec3{1, -1, 1},
	Vec3{1, 1, -1},
	Vec3{1, 1, 1},
}

box_indices := [6 * 4]u16 {
	// near
	0,
	4,
	6,
	2,
	// right
	4,
	5,
	7,
	6,
	// far
	5,
	1,
	3,
	7,
	// left
	1,
	0,
	2,
	3,
	// top
	2,
	6,
	7,
	3,
	// bottom
	0,
	4,
	5,
	1,
}

box_to_convex :: proc(box: Box, allocator := context.allocator) -> (convex: Convex) {
	vertices := make([]Vec3, 8, context.temp_allocator)

	for corner, i in BOX_CORNERS_NORM {
		vertices[i] = corner * box.rad
	}

	convex = Convex(halfedge.mesh_from_vertex_index_list(vertices, box_indices[:], 4, allocator))

	return
}

Contact_Manifold :: struct {
	points: []Vec3,
	normal: Vec3,
}

convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, collision: bool) {
	face_query_a := query_separation_face_directions(a, b)
	if face_query_a.separation > 0 {
		return
	}
	face_query_b := query_separation_face_directions(b, a)
	if face_query_b.separation > 0 {
		return
	}

	edge_separation, edge_a, edge_b := query_separation_edges(a, b)
	if edge_separation > 0 {
		return
	}

	is_face_a_contact := face_query_a.separation > edge_separation
	is_face_b_contact := face_query_b.separation > edge_separation

	collision = true
	if is_face_a_contact && is_face_b_contact {
		manifold = create_face_contact_manifold(face_query_a, a, face_query_b, b)
	} else {

	}

	return
}

Face_Query :: struct {
	separation: f32,
	face:       halfedge.Face_Index,
}

query_separation_face_directions :: proc(a, b: Convex) -> (result: Face_Query) {
	result.separation = min(f32)
	for face, f in a.faces {
		index := a.edges[face.edge].origin

		pos := a.vertices[index].pos
		normal := face.normal

		support_point := find_support_point(b, -normal)

		plane := plane_from_point_normal(pos, normal)

		distance := signed_distance_plane(support_point, plane)
		if distance > result.separation {
			result.face = halfedge.Face_Index(f)
			result.separation = distance
		}
	}

	return
}

find_support_point :: proc(convex: Convex, normal: Vec3) -> Vec3 {
	p: Vec3
	max_proj := min(f32)
	for vert in convex.vertices {
		proj := lg.dot(vert.pos, normal)
		if proj > max_proj {
			max_proj = proj
			p = vert.pos
		}
	}

	return p
}

query_separation_edges :: proc(
	a, b: Convex,
) -> (
	separation: f32,
	a_edge: halfedge.Edge_Index,
	b_edge: halfedge.Edge_Index,
) {
	separation = min(f32)
	a_edge = -1
	b_edge = -1

	for edge_a, edge_a_idx in a.edges {
		for edge_b, edge_b_idx in b.edges {
			edge_a_dir := halfedge.get_edge_direction_normalized(
				halfedge.Half_Edge_Mesh(a),
				edge_a,
			)
			edge_b_dir := halfedge.get_edge_direction_normalized(
				halfedge.Half_Edge_Mesh(b),
				edge_b,
			)

			axis := lg.cross(edge_a_dir, edge_b_dir)

			edge_a_origin := a.vertices[edge_a.origin].pos
			if lg.dot(axis, edge_a_origin - a.center) < 0 {
				axis = -axis
			}

			plane_a := plane_from_point_normal(edge_a_origin, axis)
			vert_b := find_support_point(b, -plane_a.normal)
			distance := signed_distance_plane(vert_b, plane_a)
			if distance > separation {
				separation = distance
				a_edge = halfedge.Edge_Index(edge_a_idx)
				b_edge = halfedge.Edge_Index(edge_b_idx)
			}
		}
	}

	return
}

create_face_contact_manifold :: proc(
	face_query_a: Face_Query,
	a: Convex,
	face_query_b: Face_Query,
	b: Convex,
) -> (
	manifold: Contact_Manifold,
) {
	is_ref_a := face_query_a.separation > face_query_b.separation
	ref_face_query := is_ref_a ? face_query_a : face_query_b
	ref_convex := is_ref_a ? a : b

	inc_face_query := is_ref_a ? face_query_b : face_query_a
	inc_convex := is_ref_a ? b : a

	it := halfedge.iterator_face_edges(halfedge.Half_Edge_Mesh(ref_convex), ref_face_query.face)

	for edge in halfedge.iterate_next_edge(&it) {
		clipping_face := halfedge.get_adjacent_face(halfedge.Half_Edge_Mesh(ref_convex), edge)
	}

	return
}