package collision

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

_ :: 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}

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] = box.pos + 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)
	_, _ = edge_a, edge_b
	if edge_separation > 0 {
		return
	}
	face_query_a.separation += 0.1
	edge_separation -= 0.1

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

	log.infof(
		"face_a_sep: %v, face_b_sep: %v, edge_sep: %v",
		face_query_a.separation,
		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 {
		manifold = create_edge_contact_manifold(a, b, edge_separation, edge_a, edge_b)
	}

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

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
}

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

	step := 0

	separating_plane: Plane
	separating_plane_p: Vec3
	success_step: int

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

			axis := lg.normalize0(lg.cross(edge_a_dir, edge_b_dir))

			if axis == 0 {
				continue
			}


			edge_a_origin, _ := halfedge.get_edge_points(a, edge_a)
			if lg.dot(axis, edge_a_origin - a.center) < 0 {
				axis = -axis
			}
			plane_a := plane_from_point_normal(edge_a_origin, axis)
			vert_a, _, _ := find_support_point(a, plane_a.normal)
			vert_b, vert_b_idx, _ := find_support_point(b, -plane_a.normal)

			// We found the support vert on mesh b, but now we need to find the 
			// best edge that includes that point
			vert_b_edge: halfedge.Half_Edge
			vert_b_edge_idx: halfedge.Edge_Index = -1
			{
				min_b2_distance := max(f32)
				it := halfedge.iterator_vertex_edges(b, vert_b_idx)
				for edge, edge_idx in halfedge.iterate_next_vertex_edge(&it) {
					_, vert_b2 := halfedge.get_edge_points(b, edge)

					distance_b2 := signed_distance_plane(vert_b2, plane_a)
					if distance_b2 < min_b2_distance {
						min_b2_distance = distance_b2
						vert_b_edge = edge
						vert_b_edge_idx = edge_idx
					}
				}

				assert(vert_b_edge_idx >= 0, "couldn't find the edge on convex B")
			}

			distance_a := signed_distance_plane(vert_a.pos, plane_a)
			if distance_a > 0 {
				continue
			}
			distance_b := signed_distance_plane(vert_b.pos, plane_a)
			vert_b_projected := vert_b.pos + plane_a.normal * -distance_b

			if step == -1 {
				// a1, a2 := halfedge.get_edge_points(a, edge_a)
				// edge_a_center := (a1 + a2) * 0.5
				a1, a2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(a), edge_a)
				b1, b2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(b), vert_b_edge)

				rl.DrawLine3D(edge_a_origin, edge_a_origin + plane_a.normal, rl.BLUE)
				rl.DrawLine3D(a1 + 0.1, a2 + 0.1, rl.ORANGE)
				rl.DrawLine3D(b1 + 0.1, b2 + 0.1, rl.PURPLE)

				rl.DrawSphereWires(edge_a_origin, 0.1, 4, 4, rl.ORANGE)
				rl.DrawSphereWires(vert_b.pos, 0.05, 4, 4, rl.BLUE)
				rl.DrawSphereWires(vert_b_projected, 0.05, 4, 4, rl.BLUE)
				rl.DrawLine3D(vert_b.pos, vert_b_projected, rl.VIOLET)
				log.debugf("dist: %v", distance_b)

				{
					// rl.BeginBlendMode(.ALPHA)
					// defer rl.EndBlendMode()
					debug_draw_plane(edge_a_origin, plane_a, rl.Color{0, 228, 48, 100})
				}
			}
			if distance_b > separation {
				separation = distance_b
				a_edge = halfedge.Edge_Index(edge_a_idx)
				b_edge = vert_b_edge_idx
				separating_plane = plane_a
				separating_plane_p = edge_a_origin
				success_step = step
			}

			step += 1
		}
	}
	// log.debugf("step: %v", success_step)
	// debug_draw_plane(separating_plane_p, separating_plane, rl.Color{228, 0, 48, 100})

	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_convex := is_ref_a ? b : a

	ref_face := ref_convex.faces[ref_face_query.face]

	// incident face
	inc_face: halfedge.Face
	inc_face_idx: halfedge.Face_Index
	// Find the most anti parallel face
	{
		min_dot := f32(1.0)
		for face, face_idx in inc_convex.faces {
			dot := lg.dot(ref_face.normal, face.normal)
			if dot < min_dot {
				min_dot = dot
				inc_face = face
				inc_face_idx = halfedge.Face_Index(face_idx)
			}
		}
	}

	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 * 2, 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

	{
		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_plane := plane_from_point_normal(clipping_face_vert, clipping_face.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 < 0 && d2 < 0 {
						// Both points inside
						clipped_polygon[j] = src_polygon[k]
						j += 1
					} else if d1 >= 0 && d2 < 0 {
						// 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 < 0 && d2 >= 0 {
						// 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
		}
	}

	// Final clipping, remove verts above ref face
	{
		src_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)]
		clipped_polygon := clip_bufs[clip_buf_idx]

		ref_face_vert := ref_convex.vertices[ref_convex.edges[ref_face.edge].origin].pos
		ref_plane := plane_from_point_normal(ref_face_vert, ref_face.normal)

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

			if d <= 0 {
				clipped_polygon[j] = src_polygon[i]
				j += 1
			}
		}
		vert_count = j
		clip_buf_idx = get_other_clip_buf(clip_buf_idx)
	}

	manifold.normal = ref_face.normal
	manifold.points = clip_bufs[get_other_clip_buf(clip_buf_idx)][:vert_count]

	return
}

create_edge_contact_manifold :: proc(
	a, b: Convex,
	separation: f32,
	edge_a, edge_b: halfedge.Edge_Index,
) -> (
	manifold: Contact_Manifold,
) {
	a1, a2 := halfedge.get_edge_points(a, a.edges[edge_a])
	b1, b2 := halfedge.get_edge_points(b, b.edges[edge_b])

	rl.DrawLine3D(a1 + 0.1, a2 + 0.1, rl.ORANGE)
	rl.DrawLine3D(b1 + 0.1, b2 + 0.1, rl.BLUE)

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

	manifold.points = make([]Vec3, 1, context.temp_allocator)
	manifold.points[0] = (ps[0] + ps[1]) * 0.5

	return
}