package collision

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

_ :: math
_ :: rl
_ :: rlgl
_ :: log

Convex :: halfedge.Half_Edge_Mesh

box_corners_norm := [8]Vec3 {
	{-1, 1, 1},
	{-1, -1, 1},
	{-1, 1, -1},
	{-1, -1, -1},
	{1, 1, 1},
	{1, -1, 1},
	{1, 1, -1},
	{1, -1, -1},
}

box_indices := [6 * 4]u16{0, 4, 6, 2, 3, 2, 6, 7, 7, 6, 4, 5, 5, 1, 3, 7, 1, 0, 2, 3, 5, 4, 0, 1}

@(private = "file")
box_mesh: Convex

@(init)
init_box_mesh :: proc() {
	box_mesh = Convex(halfedge.mesh_from_vertex_index_list(box_corners_norm[:], box_indices[:], 4))
}

@(fini)
deinit_box_mesh :: proc() {
	delete(box_mesh.vertices)
	delete(box_mesh.faces)
	delete(box_mesh.edges)
}

box_to_convex :: proc(box: Box, allocator := context.allocator) -> (convex: Convex) {
	convex = halfedge.copy_mesh(box_mesh, allocator)

	for &v in convex.vertices {
		v.pos = box.pos + v.pos * box.rad
	}

	return
}

double_sided_triangle_indices := [6]u16{0, 1, 2, 0, 2, 1}

double_sided_triangle_to_convex :: proc(
	tri: [3]Vec3,
	allocator := context.allocator,
) -> (
	convex: Convex,
) {
	tri := tri
	convex = Convex(
		halfedge.mesh_from_vertex_index_list(
			tri[:],
			double_sided_triangle_indices[:],
			3,
			allocator,
		),
	)
	return
}

Contact_Type_Face :: struct {
	face_idx_a: halfedge.Face_Index,
	face_idx_b: halfedge.Face_Index,
}

Contact_Type_Edge :: struct {
	edge_idx_a: halfedge.Edge_Index,
	edge_idx_b: halfedge.Edge_Index,
}

Contact_Type :: union {
	Contact_Type_Face,
	Contact_Type_Edge,
}

Contact_Manifold :: struct {
	type:       Contact_Type,
	normal:     Vec3,
	tangent:    Vec3,
	bitangent:  Vec3,
	separation: f32,
	points_a:   [4]Vec3,
	points_b:   [4]Vec3,
	points_len: int,
}

convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, collision: bool) {
	tracy.Zone()

	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, edge_separating_plane := query_separation_edges(a, b)
	_, _ = edge_a, edge_b
	if edge_separation > 0 {
		return
	}

	is_face_a_contact := (face_query_a.separation + 0.001) > edge_separation
	is_face_b_contact := (face_query_b.separation + 0.001) > edge_separation

	if is_face_a_contact || is_face_b_contact {
		manifold = create_face_contact_manifold(face_query_a, a, face_query_b, b)
	} else {
		manifold = create_edge_contact_manifold(
			a,
			b,
			edge_separating_plane,
			edge_separation,
			edge_a,
			edge_b,
		)
	}
	collision = manifold.points_len > 0
	if collision {
		manifold.bitangent = lg.normalize0(lg.cross(manifold.tangent, manifold.normal))
	}

	return
}

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

query_separation_face_directions :: proc(a, b: Convex) -> (result: Face_Query) {
	tracy.Zone()
	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.pos, plane)
		if distance > result.separation {
			result.face = halfedge.Face_Index(f)
			result.separation = distance
		}
	}

	return
}

find_support_point_from_slice :: proc(points: []Vec3, normal: Vec3) -> (p: Vec3) {
	max_proj := min(f32)
	for vert in points {
		proj := lg.dot(vert, normal)
		if proj > max_proj {
			max_proj = proj
			p = vert
		}
	}

	return
}

find_support_point :: proc(
	convex: Convex,
	normal: Vec3,
) -> (
	vert: halfedge.Vertex,
	idx: halfedge.Vertex_Index,
	ok: bool,
) {
	max_proj := min(f32)
	for v, i in convex.vertices {
		proj := lg.dot(v.pos, normal)
		if proj > max_proj {
			max_proj = proj
			vert = v
			idx = halfedge.Vertex_Index(i)
			ok = true
		}
	}

	return
}

find_furthest_point_from_point :: proc(points: []Vec3, ref: Vec3) -> (p: Vec3) {
	max_dist_sq := min(f32)
	for v in points {
		dist_sq := lg.length2(v - ref)
		if dist_sq > max_dist_sq {
			p = v
			max_dist_sq = dist_sq
		}
	}
	return
}

// See: Physics for Game Programmers: The Separating Axis Test between Convex Polyhedra 
// https://gdcvault.com/play/1017646/Physics-for-Game-Programmers-The
is_minkowski_face :: #force_inline proc(bxa, dxc, a, b, c, d: Vec3) -> bool {
	cba := lg.dot(c, bxa)
	dba := lg.dot(d, bxa)
	adc := lg.dot(a, dxc)
	bdc := lg.dot(b, dxc)

	return cba * dba < 0 && adc * bdc < 0 && cba * bdc > 0
}

build_minkowski_face :: #force_inline proc(
	mesh_a, mesh_b: Convex,
	edge_a, edge_b: halfedge.Half_Edge,
) -> bool {
	a := mesh_a.faces[edge_a.face].normal
	b := mesh_a.faces[mesh_a.edges[edge_a.twin].face].normal
	c := mesh_b.faces[edge_b.face].normal
	d := mesh_b.faces[mesh_b.edges[edge_b.twin].face].normal

	bxa := halfedge.get_edge_direction(mesh_a, edge_a)
	dxc := halfedge.get_edge_direction(mesh_b, edge_b)

	// Negate normals c and d because we're interested in minkowski difference instead of sum
	return is_minkowski_face(bxa, dxc, a, b, -c, -d)
}

query_separation_edges :: proc(
	a, b: Convex,
) -> (
	separation: f32,
	a_edge: halfedge.Edge_Index,
	b_edge: halfedge.Edge_Index,
	separating_plane: Plane,
) {
	tracy.Zone()

	separation = min(f32)
	a_edge = -1
	b_edge = -1

	checked_pairs: bit_array.Bit_Array
	bit_array.init(&checked_pairs, len(a.edges) * len(b.edges), 0, context.temp_allocator)
	a_len := len(a.edges)

	calc_pair_index :: proc(a, b, a_len: int) -> int {
		return (b * a_len) + a
	}

	for edge_a, edge_a_idx in a.edges {
		for edge_b, edge_b_idx in b.edges {
			pair_idx := calc_pair_index(edge_a_idx, edge_b_idx, a_len)
			if bit_array.get(&checked_pairs, pair_idx) {
				continue
			}

			// TODO: sort edges so twins are next to each other, then can just iterate with step = 2 and skip this bitfield
			bit_array.set(&checked_pairs, pair_idx)
			bit_array.set(&checked_pairs, calc_pair_index(int(edge_a.twin), edge_b_idx, a_len))
			bit_array.set(&checked_pairs, calc_pair_index(edge_a_idx, int(edge_b.twin), a_len))
			bit_array.set(
				&checked_pairs,
				calc_pair_index(int(edge_a.twin), int(edge_b.twin), a_len),
			)

			if build_minkowski_face(a, b, edge_a, edge_b) {
				edge_a1, edge_a2 := halfedge.get_edge_points(a, edge_a)
				edge_b1, edge_b2 := halfedge.get_edge_points(b, edge_b)
				edge_a_dir := edge_a2 - edge_a1
				edge_b_dir := edge_b2 - edge_b1
				axis := lg.normalize0(lg.cross(edge_a_dir, edge_b_dir))
				if axis == 0 {
					continue
				}
				if lg.dot(axis, edge_a1 - a.center) < 0 {
					axis = -axis
				}

				plane_a := plane_from_point_normal(edge_a1, axis)

				distance := signed_distance_plane(edge_b1, plane_a)
				if distance > separation {
					separation = distance
					a_edge = halfedge.Edge_Index(edge_a_idx)
					b_edge = halfedge.Edge_Index(edge_b_idx)
					separating_plane = plane_a
				}
			}
		}
	}

	return
}

create_face_contact_manifold :: proc(
	face_query_a: Face_Query,
	a: Convex,
	face_query_b: Face_Query,
	b: Convex,
) -> (
	manifold: Contact_Manifold,
) {
	tracy.Zone()

	is_ref_a := (face_query_a.separation + 0.01) > face_query_b.separation
	ref_face_query := is_ref_a ? face_query_a : face_query_b
	ref_convex := is_ref_a ? a : b
	inc_convex := is_ref_a ? b : a

	ref_face_idx := ref_face_query.face
	ref_face := ref_convex.faces[ref_face_idx]

	// incident face
	inc_face: halfedge.Face
	inc_face_idx: halfedge.Face_Index
	// Find the most anti parallel face
	{
		_, support_idx, _ := find_support_point(inc_convex, -ref_face.normal)

		it := halfedge.iterator_vertex_edges(inc_convex, support_idx)

		min_dot := max(f32)

		for edge in halfedge.iterate_next_vertex_edge(&it) {
			face := inc_convex.faces[edge.face]
			dot := lg.dot(ref_face.normal, face.normal)
			if dot < min_dot {
				min_dot = dot
				inc_face = face
				inc_face_idx = halfedge.Face_Index(edge.face)
			}
		}
	}

	inc_polygon: []Vec3
	original_vert_count := 0

	// Get incident face vertices
	{
		it := halfedge.iterator_face_edges(halfedge.Half_Edge_Mesh(inc_convex), inc_face_idx)

		for _ in halfedge.iterate_next_edge(&it) {
			original_vert_count += 1
		}

		inc_polygon = make([]Vec3, original_vert_count * 4, context.temp_allocator)

		halfedge.iterator_reset_edges(&it)

		i := 0
		for edge in halfedge.iterate_next_edge(&it) {
			inc_polygon[i] = inc_convex.vertices[edge.origin].pos
			i += 1
		}
	}


	assert(len(inc_polygon) > 0)

	// Set up ping pong buffers
	inc_polygon2 := make([]Vec3, len(inc_polygon), context.temp_allocator)

	// Buffer 0 is the original incident polygon, start with index 1
	clip_buf_idx := 1
	clip_bufs := [2][]Vec3{inc_polygon, inc_polygon2}

	get_other_clip_buf :: proc(idx: int) -> int {
		return (idx + 1) % 2
	}

	step := 0
	vert_count := original_vert_count


	// {
	// 	polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)]
	// 	for i in 2 ..< vert_count {
	// 		rl.DrawTriangle3D(polygon[0], polygon[i - 1], polygon[i], rl.RED)
	// 	}
	// }

	EPS :: 1e-6

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

		for edge in halfedge.iterate_next_edge(&it) {
			src_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)]
			clipped_polygon := clip_bufs[clip_buf_idx]

			clipping_face, clipping_face_idx, _ := halfedge.get_adjacent_face(
				halfedge.Half_Edge_Mesh(ref_convex),
				edge,
			)
			clipping_face_vert :=
				ref_convex.vertices[ref_convex.edges[clipping_face.edge].origin].pos

			clipping_plane_center: Vec3
			{
				clipping_plane_it := halfedge.iterator_face_edges(
					halfedge.Half_Edge_Mesh(ref_convex),
					clipping_face_idx,
				)

				num := 0
				for clip_edge in halfedge.iterate_next_edge(&clipping_plane_it) {
					vert := ref_convex.vertices[clip_edge.origin].pos
					clipping_plane_center += vert
					num += 1
				}

				clipping_plane_center /= f32(num)
			}

			clipping_normal := clipping_face.normal

			// If adjacent face is facing backwards from us (like a double sided triangle)
			// use the edge normal
			if lg.dot(ref_face.normal, clipping_face.normal) < -0.99 {
				e1, e2 := halfedge.get_edge_points(ref_convex, edge)

				clipping_normal = lg.cross(e2 - e1, clipping_face_vert - clipping_plane_center)
				clipping_normal = -lg.normalize0(lg.cross(e2 - e1, clipping_normal))
			}

			clipping_plane := plane_from_point_normal(clipping_face_vert, clipping_normal)

			// Actual clipping
			{
				j := 0
				for i in 0 ..< vert_count {
					k := (i + 1) % vert_count
					d1 := signed_distance_plane(src_polygon[i], clipping_plane)
					d2 := signed_distance_plane(src_polygon[k], clipping_plane)

					if d1 < -EPS && d2 < -EPS {
						// Both points inside
						clipped_polygon[j] = src_polygon[k]
						j += 1
					} else if d1 > -EPS && d2 < -EPS {
						// First point is outside
						_, clipped_polygon[j], _ = intersect_segment_plane(
							{src_polygon[i], src_polygon[k]},
							clipping_plane,
						)
						j += 1
						clipped_polygon[j] = src_polygon[k]
						j += 1
					} else if d1 < -EPS && d2 > -EPS {
						// Second point outside
						_, clipped_polygon[j], _ = intersect_segment_plane(
							{src_polygon[i], src_polygon[k]},
							clipping_plane,
						)
						j += 1
					}
				}
				vert_count = j
			}

			clip_buf_idx = get_other_clip_buf(clip_buf_idx)
			step += 1
		}
	}

	ref_face_vert, ref_face_vert2 := halfedge.get_edge_points(
		ref_convex,
		ref_convex.edges[ref_face.edge],
	)
	ref_plane := plane_from_point_normal(ref_face_vert, ref_face.normal)
	ref_tangent := lg.normalize0(ref_face_vert2 - ref_face_vert)

	// Final clipping, remove verts above ref face and project those left onto the reference plane
	{
		src_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)]
		clipped_polygon := clip_bufs[clip_buf_idx]

		j := 0
		for i in 0 ..< vert_count {
			d := signed_distance_plane(src_polygon[i], ref_plane)

			if d <= 1e-02 {
				clipped_polygon[j] = src_polygon[i] - d * ref_plane.normal
				j += 1
			}
		}
		vert_count = j
		clip_buf_idx = get_other_clip_buf(clip_buf_idx)
	}

	// Resulting polygon before contact optimization
	full_clipped_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)][:vert_count]

	ref_points: [4]Vec3

	// Contact optimization
	if len(full_clipped_polygon) > 4 {
		start_dir := lg.cross(ref_plane.normal, ref_face_vert)

		first_point := find_support_point_from_slice(full_clipped_polygon, start_dir)
		second_point := find_furthest_point_from_point(full_clipped_polygon, first_point)

		third_point: Vec3
		{
			max_area := min(f32)
			for v in full_clipped_polygon {
				area := 0.5 * lg.dot(ref_plane.normal, lg.cross(first_point - v, second_point - v))
				if area > max_area {
					max_area = area
					third_point = v
				}
			}
		}

		existing_edges := [][2]Vec3 {
			{first_point, second_point},
			{second_point, third_point},
			{third_point, first_point},
		}

		fourth_point: Vec3
		{
			// We're looking for max negative area actually
			min_area := max(f32)
			for v in full_clipped_polygon {
				for edge in existing_edges {
					area := 0.5 * lg.dot(ref_plane.normal, lg.cross(edge[0] - v, edge[1] - v))
					if area < min_area {
						min_area = area
						fourth_point = v
					}
				}
			}
		}

		ref_points[0] = first_point
		ref_points[1] = second_point
		ref_points[2] = third_point
		ref_points[3] = fourth_point
		manifold.points_len = 4
	} else {
		copy(ref_points[:], full_clipped_polygon)
		manifold.points_len = len(full_clipped_polygon)
		assert(len(full_clipped_polygon) <= 4)
	}

	inc_face_vert := inc_convex.vertices[inc_convex.edges[inc_face.edge].origin].pos
	inc_plane := plane_from_point_normal(inc_face_vert, inc_face.normal)

	inc_points: [4]Vec3
	for p, i in ref_points[:manifold.points_len] {
		inc_points[i] = project_point_on_plane(p, inc_plane)
	}

	if is_ref_a {
		manifold.type = Contact_Type_Face {
			face_idx_a = ref_face_idx,
			face_idx_b = inc_face_idx,
		}
		manifold.points_a = ref_points
		manifold.points_b = inc_points
	} else {
		manifold.type = Contact_Type_Face {
			face_idx_a = inc_face_idx,
			face_idx_b = ref_face_idx,
		}
		manifold.points_b = ref_points
		manifold.points_a = inc_points
	}

	manifold.separation = ref_face_query.separation
	// Normal is always pointing from a to b
	manifold.normal = is_ref_a ? ref_face.normal : -ref_face.normal
	manifold.tangent = ref_tangent

	return
}

project_point_on_plane :: proc(point: Vec3, plane: Plane) -> Vec3 {
	dist := signed_distance_plane(point, plane)
	return point + plane.normal * -dist
}

create_edge_contact_manifold :: proc(
	a, b: Convex,
	separating_plane: Plane,
	separation: f32,
	edge_a, edge_b: halfedge.Edge_Index,
) -> (
	manifold: Contact_Manifold,
) {
	tracy.Zone()

	a1, a2 := halfedge.get_edge_points(a, a.edges[edge_a])
	b1, b2 := halfedge.get_edge_points(b, b.edges[edge_b])

	_, ps := closest_point_between_segments(a1, a2, b1, b2)

	manifold.type = Contact_Type_Edge {
		edge_idx_a = edge_a,
		edge_idx_b = edge_b,
	}
	manifold.normal = separating_plane.normal
	manifold.separation = lg.dot(ps[0] - ps[1], separating_plane.normal)
	manifold.points_a[0] = ps[0]
	manifold.points_b[0] = ps[1]
	manifold.points_len = 1
	manifold.tangent = lg.normalize0(lg.cross(a2 - a1, manifold.normal))

	return
}

ray_vs_convex :: proc(
	c: Convex,
	origin, dir: Vec3,
	min_t: f32,
) -> (
	t: f32,
	face_idx: halfedge.Face_Index,
	normal: Vec3,
	bary: [3]f32,
	hit: bool,
) {
	t = min_t
	for i in 0 ..< len(c.faces) {
		it := halfedge.iterator_face_triangles(halfedge.Half_Edge_Mesh(c), halfedge.Face_Index(i))

		for tri in halfedge.iterate_next_triangle(&it) {
			hit_t, tmp_normal, tmp_bary, ok := intersect_ray_triangle(
				[2]Vec3{origin, origin + dir},
				tri,
			)

			if ok && hit_t < t {
				t = hit_t
				normal = tmp_normal
				bary = tmp_bary
				face_idx = halfedge.Face_Index(i)
				hit = true
			}
		}
	}

	if hit {
		normal = lg.normalize0(normal)
	}

	return
}