###
### graph class + algorithms
###
extends Object

class Edge:
	var idx0 : int
	var idx1 : int
	var weight : float
	
	func _init(idx0_ : int, idx1_ : int, weight_ : float):
		idx0 = idx0_
		idx1 = idx1_
		weight = weight_

class Graph:
	var vertices = []
	var edges = []
	
	func append_vertex(vertex):
		var idx = vertices.size()
		vertices.append(vertex)
		return idx
	
	func append_vertices(vertices_ : Array):
		for vertex in vertices_:
			vertices.append(vertex)
	
	func append_edge(idx0, idx1, weight := 1.0):
		for edge in edges:
			if (edge.idx0 == idx0 && edge.idx1 == idx1) || \
				(edge.idx0 == idx1 && edge.idx1 == idx0):
				return
		# TODO: why doesn't this work? bug?
		assert(0)
		# edges.append(Edge.new(idx0, idx1, weight))
	
	func remove_vertex(vertex):
		var idx = vertices.find(vertex)
		if idx > -1:
			remove_vertex_at(idx)
	
	func remove_vertex_at(idx):
		vertices.remove(idx)
		# fix edges
		var to_remove = []
		for edge in edges:
			if edge.idx0 == idx || edge.idx1 == idx:
				to_remove.append(edge)
			else:
				if edge.idx0 > idx:
					edge.idx0 -= 1
				if edge.idx1 > idx:
					edge.idx1 -= 1
		for edge in to_remove:
			edges.erase(edge)
	
	func remove_edge(idx0, idx1):
		var to_remove = []
		for edge in edges:
			if (edge.idx0 == idx0 && edge.idx1 == idx1) || \
				(edge.idx0 == idx1 && edge.idx1 == idx0):
				to_remove.append(edge)
		for edge in to_remove:
			edges.erase(edge)
	
	func duplicate(deep = false):
		var dupl = Graph.new()
		dupl.vertices = vertices.duplicate(deep)
		for edge in edges:
			dupl.append_edge(edge.idx0, edge.idx1, edge.weight)
		return dupl

################
# public stuff #
################
func _init():
	assert(0, "This class should not be instantiated.")

# generates a delaunay triangulation for the given point list
func gen_delaunay(point_list : Array) -> Graph:
	assert(point_list.size() > 2)
	var graph = Graph.new()
	var tris = []
	
	graph.append_vertices(point_list)
	__gen_super_triangle(graph, point_list)
	tris.append(Vector3(graph.vertices.size() - 3, graph.vertices.size() - 2, graph.vertices.size() - 1))
	
	for idx in range(point_list.size()):
		var point = point_list[idx]
		var bad_triangles = []
		for tri in tris:
			var p0 = graph.vertices[tri.x]
			var p1 = graph.vertices[tri.y]
			var p2 = graph.vertices[tri.z]
			
			if __point_in_circumcircle(point, p0, p1, p2):
				bad_triangles.append(tri)
		var polygon := []
		for bad_tri in bad_triangles:
			if !__edge_shared(bad_tri.x, bad_tri.y, bad_triangles):
				polygon.append(Vector2(bad_tri.x, bad_tri.y))
			if !__edge_shared(bad_tri.y, bad_tri.z, bad_triangles):
				polygon.append(Vector2(bad_tri.y, bad_tri.z))
			if !__edge_shared(bad_tri.z, bad_tri.x, bad_triangles):
				polygon.append(Vector2(bad_tri.z, bad_tri.x))
		for bad_tri in bad_triangles:
			tris.erase(bad_tri)
			graph.remove_edge(bad_tri.x, bad_tri.y)
			graph.remove_edge(bad_tri.y, bad_tri.z)
			graph.remove_edge(bad_tri.z, bad_tri.x)
		
		for edge in polygon:
			var tri = Vector3(edge.x, edge.y, idx)
			var p0 = graph.vertices[tri.x]
			var p1 = graph.vertices[tri.y]
			var p2 = graph.vertices[tri.z]
			
			tris.append(tri)
			graph.append_edge(tri.x, tri.y, p0.distance_to(p1))
			graph.append_edge(tri.y, tri.z, p1.distance_to(p2))
			graph.append_edge(tri.z, tri.x, p2.distance_to(p0))
	
	# remove super triangle
	for idx in range(graph.vertices.size() - 1, graph.vertices.size() - 4, -1):
		graph.remove_vertex_at(idx)
	
	return graph

# generate the minimum spanning tree
func gen_mst(graph : Graph) -> Graph:
	var mst = graph.duplicate()
	var possible_edges = mst.edges
	var forests = []
	mst.edges = []
	
	possible_edges.sort_custom(self, "__edge_weight_comp")
	for idx in range(mst.vertices.size()):
		forests.append([idx])
	
	while !possible_edges.empty():
		var edge = possible_edges.pop_front()
		# already connected?
		if forests[edge.idx0] == forests[edge.idx1]:
			continue
		# append edge
		mst.append_edge(edge.idx0, edge.idx1, edge.weight)
		# merge forests
		for idx in forests[edge.idx1]:
			forests[edge.idx0].append(idx)
			forests[idx] = forests[edge.idx0]
	
	return mst

# finds the shortest path through the given graph
# returns a dictionary with a "distance" and a "path"
func find_path(graph : Graph, start_idx : int, end_idx : int) -> Dictionary:
	var dist := []
	var prev := []
	var queue := []
	for idx in range(graph.vertices.size()):
		dist.append(INF)
		prev.append(-1)
		queue.append(idx)
	dist[start_idx] = 0.0
	
	while !queue.is_empty():
		var shortest_dist = INF
		var shortest_idx : int
		for idx in queue:
			if dist[idx] < shortest_dist:
				shortest_dist = dist[idx]
				shortest_idx = idx
		if shortest_idx == end_idx: # found it
			if shortest_dist == INF:
				return {
					"distance": INF,
					"path": []
				}
			var path = [end_idx]
			var idx = end_idx
			while prev[idx] != -1:
				path.append(prev[idx])
				idx = prev[idx]
			assert(idx == start_idx)
			return {
				"distance": dist[end_idx],
				"path": path
			}
		queue.erase(shortest_idx)
		for edge in graph.edges:
			var target_idx
			if edge.idx0 == shortest_idx:
				target_idx = edge.idx1
			elif edge.idx1 == shortest_idx:
				target_idx = edge.idx0
			else:
				continue
			var new_dist = shortest_dist + edge.weight
			if new_dist < dist[target_idx]:
				dist[target_idx] = new_dist
				prev[target_idx] = shortest_idx
	return {
		"distance": INF,
		"path": []
	}

# draws a graph onto a control
func dump_graph_2d(graph : Graph, control : Control) -> void:
	var normalized_graph = graph.duplicate()
	var min_xy = Vector2(INF, INF)
	var max_xy = -Vector2(INF, INF)
	
	# normalize graph
	for vertex in normalized_graph.vertices:
		if vertex.x < min_xy.x:
			min_xy.x = vertex.x
		if vertex.x > max_xy.x:
			max_xy.x = vertex.x
		if vertex.y < min_xy.y:
			min_xy.y = vertex.y
		if vertex.y > max_xy.y:
			max_xy.y = vertex.y
	
	for idx in range(normalized_graph.vertices.size()):
		var normalized_vertex = normalized_graph.vertices[idx]
		normalized_vertex.x = inverse_lerp(min_xy.x, max_xy.x, normalized_vertex.x)
		normalized_vertex.y = inverse_lerp(min_xy.y, max_xy.y, normalized_vertex.y)
		normalized_graph.vertices[idx] = normalized_vertex
	
	var img_size = control.rect_size
	
	for edge in normalized_graph.edges:
		var v0 = normalized_graph.vertices[edge.idx0]
		var v1 = normalized_graph.vertices[edge.idx1]
		var coord0 = __img_vertex_coord(v0, img_size)
		var coord1 = __img_vertex_coord(v1, img_size)
		
		control.draw_line(coord0, coord1, Color.YELLOW, 2.0)
	
	for vertex in normalized_graph.vertices:
		var coord = __img_vertex_coord(vertex, img_size)
		control.draw_circle(coord, 5.0, Color.RED)

#################
# private stuff #
#################
func __gen_super_triangle(graph : Graph, point_list : Array) -> void:
	var min_xy = Vector2(INF, INF)
	var max_xy = -Vector2(INF, INF)
	
	for point in point_list:
		if point.x < min_xy.x:
			min_xy.x = point.x
		if point.y < min_xy.y:
			min_xy.y = point.y
		if point.x > max_xy.x:
			max_xy.x = point.x
		if point.y > max_xy.y:
			max_xy.y = point.y
	
	min_xy -= Vector2(1.0, 1.0)
	
	var p1 = Vector2(2.0 * max_xy.x, min_xy.y)
	var p2 = Vector2(min_xy.x, 2.0 * max_xy.y)
	
	var idx0 = graph.append_vertex(min_xy)
	var idx1 = graph.append_vertex(p1)
	var idx2 = graph.append_vertex(p2)
	graph.append_edge(idx0, idx1, min_xy.distance_to(p1))
	graph.append_edge(idx1, idx2, p1.distance_to(p2))
	graph.append_edge(idx2, idx0, p2.distance_to(min_xy))

func __edge_shared(p0 : int, p1 : int, tris : Array) -> bool:
	var edge_count = 0
	for tri in tris:
		if (p0 == tri.x || p0 == tri.y || p0 == tri.z) && \
			(p1 == tri.x || p1 == tri.y || p1 == tri.z):
			edge_count += 1
	return edge_count > 1

func __point_in_circumcircle(point : Vector2, p0 : Vector2, p1 : Vector2, p2 : Vector2) -> bool:
	var p0x_ = p0.x - point.x
	var p0y_ = p0.y - point.y
	var p1x_ = p1.x - point.x
	var p1y_ = p1.y - point.y
	var p2x_ = p2.x - point.x
	var p2y_ = p2.y - point.y
	return (
		(p0x_*p0x_ + p0y_*p0y_) * (p1x_*p2y_-p2x_*p1y_) - \
		(p1x_*p1x_ + p1y_*p1y_) * (p0x_*p2y_-p2x_*p0y_) + \
		(p2x_*p2x_ + p2y_*p2y_) * (p0x_*p1y_-p1x_*p0y_) \
	) > 0;

func __check_poly(polygon) -> void:
	for edge0 in polygon:
		for edge1 in polygon:
			if edge0 == edge1:
				continue
			assert(edge0.x != edge1.x || edge0.y != edge1.y)
			assert(edge0.x != edge1.y || edge0.y != edge1.x)

func __edge_weight_comp(e0 : Edge, e1 : Edge):
	return e0.weight < e1.weight

func __img_vertex_coord(vertex : Vector2, img_size : Vector2):
	return Vector2(5.0, 5.0) + vertex * (img_size - Vector2(10.0, 10.0))