path_manager2.gd

# TODO: Make sure it can handle multiple spawn locations
# TODO: Add the ability to trace back along player nav_path to find furthest point to use as distance traveled
# TODO: Add ability to get a point from a percentage along the nav_path
# TODO: Add ability to look forward n distance from where the intersect or player is
# TODO: Add ability to check if player can travel to path without obstacles. if not find another near segment to check from.
# TODO: Add ability to get path ahead from vector3 to distance
extends Node

var director:Node

export(Color) var path_color = Color.red
export(Color) var path_color2 = Color.blue
export(Color) var path_color3 = Color.green

var path_material = SpatialMaterial.new()
var path_material2 = SpatialMaterial.new()
var path_material3 = SpatialMaterial.new()

var path_geometry = ImmediateGeometry.new()
var path_geometry2 = ImmediateGeometry.new()
var path_geometry3 = ImmediateGeometry.new()
var path_geometry4 = ImmediateGeometry.new()



func _ready() -> void:

	#create an immediate geometry node
	add_child(path_geometry)
	add_child(path_geometry2)
	add_child(path_geometry3)
	add_child(path_geometry4)

	path_material.albedo_color = path_color
#	path_material.vertex_color_use_as_albedo = false
	path_material.params_line_width = 1
	path_material.params_point_size = 2
	path_material.flags_unshaded = true
	path_material.flags_no_depth_test = true
	path_material.params_grow = true
	path_material.params_grow_amount = 2

	path_material2.albedo_color = path_color2
#	path_material2.vertex_color_use_as_albedo = false
	path_material2.params_line_width = 1
	path_material2.params_point_size = 2
	path_material2.flags_unshaded = true
	path_material2.flags_no_depth_test = true
	path_material2.params_grow = true
	path_material2.params_grow_amount = 2

	path_material3.albedo_color = path_color3
#	path_material3.vertex_color_use_as_albedo = false
	path_material3.params_line_width = 1
	path_material3.params_point_size = 2
	path_material3.flags_unshaded = true
	path_material3.flags_no_depth_test = true
	path_material3.params_grow = true
	path_material3.params_grow_amount = 2

	#needs more robust checking
	director = get_node("/root").get_child(0).get_node("_director")
	if director == null:
		dprint("Path Manager:: Director not found, disabled.")
	pass

# Starting point. Called by director
func initialize(nav_node:Navigation, path_hint_node:Spatial,path_zone_node:Spatial) -> Array:
	#get waypoints
	var waypoints = validate_waypoints(get_waypoints_from_node(path_hint_node))
	var output_path = generate_path(nav_node, waypoints)
	dprint(["Is path valid?",validate_path(output_path,waypoints,path_zone_node)])
	_draw_path(output_path,path_geometry,path_geometry2,path_material,path_material2)
#	_draw_path2(build_path(nav_node, waypoints),path_geometry,path_material)
	return output_path
	pass

# takes a parent node with child waypoint nodes. Finds all waypoints
# Gives and array of waypoint nodes
func get_waypoints_from_node(path_hint_node:Spatial) -> Array:
	var child_nodes = path_hint_node.get_children()
	var path_nodes = []
	for node in child_nodes:
		if node is Waypoint:
			path_nodes.append(node)
	return path_nodes
	pass

func validate_path(path:Array, waypoints:Array,path_zone_node:Spatial) -> bool:
	var aa:Array = validate_waypoints(waypoints)
	if aa.size() <= 0:
		#It is missing at least 1 of the required waypoints
		return false
	var start_node:Waypoint
	var end_node:Waypoint
	for waypoint in aa:
		if waypoint.name == "_player_spawn":
			start_node = waypoint
		if waypoint.name == "_safe_room":
			end_node = waypoint

	var bb = get_path_zones(path_zone_node)
	if is_valid_path_zones(bb) == false:
		#It is missing at least 1 of the required zones
		return false

	var start_zone:Area
	var end_zone:Area

	for zone in bb:
		if zone.name == "_player_spawn":
			start_zone = zone
		if zone.name == "_safe_room":
			end_zone = zone
	if point_in_area(start_node.global_transform.origin,start_zone) == false:
		return false
	if point_in_area(end_node.global_transform.origin,end_zone) == false:
		return false

	#check if first and last points are in safe zones
	if path.size()>1:
		if point_in_area(path[0],start_zone) == false:
			return false
		if point_in_area(path[path.size()-1],end_zone) == false:
			return false
	else:
		return false
	return true

# Takes a list of waypoints, finds required waypoints, then traverses them to see if they go from begginging
# to the end.
# Gives an array of Waypoint Nodes
func validate_waypoints(waypoints:Array) -> Array :
	var start_node:Spatial = null
	for waypoint in waypoints:
		if waypoint.name == "_player_spawn":
			start_node = waypoint
			break
	if start_node == null:
		dprint("_player_spawn NOT found!")
		return []

	var visited_nodes: Array = []
	var next_node:Waypoint = start_node
	while true:
		if (visited_nodes.find(next_node)) < 0:
			visited_nodes.append(next_node)
			if next_node.next_node() != null:
				next_node = next_node.next_node()
			else:
				dprint("Node ordering Complete")
				break
		else:
			dprint("Looping, Halting")
			break
	if next_node.name != "_safe_room":
		dprint("_safe_room NOT found!")
		return []
	return visited_nodes

# Takes validated waypoints and connects them together using the navigation mesh.
# Gives an array of Vector3
func generate_path(nav_node:Navigation, waypoints:Array) -> Array:
	var points = validate_waypoints(waypoints)
	var path = []
	for i in range(points.size()-1):
		var start = points[i].global_transform.origin
		var end = points[i+1].global_transform.origin
		var local_path = nav_node.get_simple_path(start - nav_node.global_transform.origin ,end - nav_node.global_transform.origin)
		if local_path.size() > 0:
#			path.append_array(local_path)
			for point in local_path:
				path.append(point)
		else:
			path.append(end)
#		print(local_path.size())
	path = optimize_path(path)
	return path

# Checks an Area node to see if a point is inside it
func point_in_area(point:Vector3,area:Area, collide_with_bodies:bool = false, collide_with_areas: bool = true) -> bool:
	var _shape = SphereShape.new()
	var _temp_spatial = Spatial.new()
	add_child(_temp_spatial)
	var _space_state = _temp_spatial.get_world().direct_space_state
	var _query = PhysicsShapeQueryParameters.new()
	_shape.radius = .00001
	_query.shape_rid = _shape.get_rid()
	_query.transform.origin = point
	_query.collide_with_bodies = collide_with_bodies
	_query.collide_with_areas = collide_with_areas
	_query.exclude = [_shape.get_rid()]
	var _result = _space_state.intersect_shape(_query)
	remove_child(_temp_spatial)
	_temp_spatial.queue_free()

	for item in _result:
		if item.collider == area:
			return true
	return false

# Optimize Path
# Removes consecutive linear points (repeated vector3's in a row), and merges line segments with the similar normals
# path:Array-> An array of Vector3
# remove_duplicates:bool = true	-> Should the repeated Vector3's be removed?
# margin:float-> The level of floating point precision when joining the line segments.
func optimize_path(path:Array,remove_duplicates:bool = true, margin = .01):
#	return optimize_path2(path)
	#fail early
	if path.size() < 1:
		return path

	var uniq = []
	#remove linear duplicates
	if remove_duplicates == true:
		uniq.append(path[0])
		for _point in path:
			if _point !=  uniq[uniq.size()-1]:
				uniq.append(_point)
	else:
		uniq = path

	#join straight lines
	var last_normal:Vector3
	var optimized_list = []
	var current_normal:Vector3

	for i in range(1,uniq.size()):
		var start:Vector3 = uniq[i-1]
		var end:Vector3 = uniq[i]
		current_normal = stepify_vector(start.direction_to(end),margin)
		if (last_normal != current_normal):
			optimized_list.append(start)
		last_normal = current_normal
	optimized_list.append(uniq[uniq.size()-1])

	return optimized_list

# Takes a Vector3 and a float and changes the decimal precision
# Gives a Vector3
func stepify_vector(Vector:Vector3, step) -> Vector3:
	return Vector.snapped(Vector3.ONE *step)

# Print function that links to director, tells where text is comming from
func dprint(args=[]) -> void:
	if director != null:
		director.dprint(str("Path Generator::",args))
	else:
		print(str("Path Generator::",args))

func _draw_path2(path: Array,geometry:ImmediateGeometry,material:SpatialMaterial) -> void:
	geometry.clear()
	geometry.set_material_override(material)
	geometry.begin(Mesh.PRIMITIVE_LINES, null)
	for i in range(0, path.size()-1):
		geometry.add_vertex(path[i])
		geometry.add_vertex(path[i+1])
	geometry.end()

func _draw_path(path: Array,geometry0:ImmediateGeometry,geometry1:ImmediateGeometry,material0:SpatialMaterial,material1:SpatialMaterial) -> void:
	geometry0.clear()
	geometry1.clear()

	var aa = true
	geometry0.set_material_override(material0)
	geometry0.begin(Mesh.PRIMITIVE_LINES, null)

	geometry1.set_material_override(material1)
	geometry1.begin(Mesh.PRIMITIVE_LINES, null)

	for i in range(0, path.size()-1 ):
		if aa== true:
			geometry0.add_vertex(path[i])
			geometry0.add_vertex(path[i+1])
		else:
			geometry1.add_vertex(path[i])
			geometry1.add_vertex(path[i+1])
		aa = !aa
	geometry0.end()
	geometry1.end()

func optimize_path2(path:Array,remove_duplicates = true) -> Array:
	#fail early
	if path.size() <= 1:
		return path

	var uniq = []

	#remove linear duplicates
	if remove_duplicates == true:
		uniq.append(path[0])
		for _point in path:
			if _point !=  uniq[uniq.size()-1]:
				uniq.append(_point)
	else:
		uniq = path

	#join straight lines
	var current_normal:Vector3
	var last_normal:Vector3
	var optimized_list = []

#	print(uniq.size())
	for i in range(1,uniq.size()):
		var start:Vector3 = uniq[i-1]
		var end:Vector3 = uniq[i]
		current_normal = stepify_vector(start.direction_to(end),.01)
#		print(current_normal , last_normal == current_normal,  [start, end] )
		if (last_normal != current_normal):
			optimized_list.append(start)
		last_normal = current_normal
	optimized_list.append(uniq[uniq.size()-1])

	return optimized_list

func get_path_zones(_path_zone_node:Spatial) -> Array:
	var child_nodes = _path_zone_node.get_children()
	var zone_nodes = []
	for node in child_nodes:
		if node is Area:
			zone_nodes.append(node)
	return zone_nodes

func is_valid_path_zones(_path_zones:Array) -> bool:
	var found_nodes = []
	for node in _path_zones:
		match node.name:
			"_player_spawn":
#				dprint("spawn zone found")
				found_nodes.append(node)
			"_safe_room":
				found_nodes.append(node)
#				dprint("safe zone found")
	if found_nodes.size() != 2:
		return false
#	dprint("Start and end zones found")
	return true

func path_length(path:Array) -> float:
	var distance = 0.0
	for i in range(0, path.size()-1):
		distance += path[i].distance_to(path[i+1])
	return distance

func get_point_from_distance(path_array:Array,distance:float) -> Vector3:
	var distance_clamped = clamp(distance,0,path_length(path_array))
	var working_distance:float = 0
	var normal:Vector3
	var start_point:Vector3
	for i in range(0, path_array.size()-1):
		var temp_distance = path_array[i].distance_to(path_array[i+1])
		if working_distance + temp_distance >= distance_clamped:
			normal = path_array[i].direction_to(path_array[i+1])
			start_point = path_array[i]
			break
		else:
			working_distance += temp_distance
	return  start_point + ((distance_clamped - working_distance) * normal)

func get_point_ahead(path_array:Array, start_distance:float, distance_ahead:float) -> Vector3:
	return get_point_from_distance(path_array,start_distance+distance_ahead)
	pass

# Gets distance strictly along the path
func get_distance_from_point(path_array:Array, global_point:Vector3) -> float:
	#get the distance from each segment
	var seg_list = direct_distance_from_each_segment( path_array, global_point)
	#find the segment with the shortest distance
	var current_segment = find_nearest_euclidean_segment(seg_list)
	var intersect = get_nearest_point_on_line_segment(path_array[current_segment],path_array[current_segment+1],global_point)

	var completed_distance = 0.0
	if current_segment>0:
		for i in range(0, current_segment):
			completed_distance += path_array[i].distance_to(path_array[i+1])
		completed_distance += path_array[current_segment].distance_to(intersect)
	else:
		completed_distance =  path_array[current_segment].distance_to(intersect)
	return (completed_distance)

	# Measures distance of global_point to closest point in each segment
func direct_distance_from_each_segment(path_flow:Array, global_point:Vector3):
	var seg_list = []
	for i in range(0, path_flow.size()-1):
		var intersect:Vector3 = get_nearest_point_on_line_segment(path_flow[i],path_flow[i+1],global_point)
		seg_list.append(intersect.distance_to(global_point))
	return seg_list

func find_nearest_euclidean_segment(segment_distances:Array):
	var min_dist
	var current_seg = 0
	min_dist = segment_distances[0]
	for i in range(0, segment_distances.size()):
		if (segment_distances[i] <min_dist):
			current_seg = i
			min_dist = segment_distances[i]
	return current_seg
	pass
# Measures distance of global_point to closest point in each segment

func get_nearest_point_on_line_segment(start:Vector3, end:Vector3, point:Vector3):
	#https://forum.unity.com/threads/how-do-i-find-the-closest-point-on-a-line.340058/#post-5839648
	var linedir = start.direction_to(end)
	var v = point - start
	var d = v.dot(linedir)
	d = clamp(d, 0.0, start.distance_to(end))
	return start + linedir * d
	pass