extends Node2D
class_name HumanPathFinder

var astar_grid: AStarGrid2D
var toFollow: Array[Vector2i]
@onready var world: World = get_parent().get_parent();
@onready var controled:Human = get_parent()
@export var can_walk_on_roads = false

var destination:Vector2 = Vector2.INF
var target_global_position:Vector2 = Vector2.INF
var HumanLayer = 0
var car_layer = 1

func _ready() -> void:
	astar_grid = AStarGrid2D.new()
	astar_grid.region = world.get_used_rect()
	astar_grid.cell_size = Vector2(48, 48)
	astar_grid.default_compute_heuristic = AStarGrid2D.HEURISTIC_CHEBYSHEV
	astar_grid.default_estimate_heuristic = AStarGrid2D.HEURISTIC_CHEBYSHEV
	astar_grid.update()

	# only take into account the tiles that are marked with a navigation layer for cars
	for x in world.get_used_rect().size.x:
		for y in world.get_used_rect().size.y:
			var tile_position = Vector2(
				x + world.get_used_rect().position.x,
				y + world.get_used_rect().position.y,
			)
			var tile_data = world.get_cell_tile_data(tile_position)
			if tile_data == null or tile_data.get_navigation_polygon(HumanLayer) == null:
				if can_walk_on_roads:
					if tile_data == null or tile_data.get_navigation_polygon(car_layer) == null:
						astar_grid.set_point_solid(tile_position)
				else:
					astar_grid.set_point_solid(tile_position)

			for obstacles in world.obstacles:
				if (obstacles.get_cell_tile_data(tile_position) != null and
				 	obstacles.get_cell_tile_data(tile_position).get_collision_polygons_count(0)):
					astar_grid.set_point_solid(tile_position)

func _process(delta: float) -> void:
	if !controled:
		return
	if controled.wants_to_interact_with:
		destination = Vector2.INF
	if controled.kill_path_finder:
		destination = Vector2.INF
		controled.kill_path_finder = false
	if destination != Vector2.INF:
		var my_global_position = controled.get_feet_global_position()
		
		if target_global_position != destination:
			target_global_position = destination

			# make the wanted position on the track move ahead by a certain amount

			# compute the new navigation points the car should follow
			var points = astar_grid.get_id_path(
				world.local_to_map(world.to_local(my_global_position)),
				world.local_to_map(world.to_local(target_global_position))
			).slice(1)
			if !points.is_empty():
				toFollow = points

		if $Label.visible:
			$Label.text = (
				"position "+str(world.local_to_map(world.to_local(my_global_position)))+" , "+str(my_global_position)+
				"\ntarget position "+ str(world.local_to_map(world.to_local(destination)))+" , "+str(destination)+
				"\npoint to follow "+str(toFollow)
				)

		if !toFollow.is_empty():
			if world.local_to_map(world.to_local(my_global_position)) == toFollow.front():
				toFollow.pop_front()
			
			if !toFollow.is_empty():
				controled.moveFeetTo(world.to_global(world.map_to_local(toFollow.front())));
		else:
			var distance_to_goal =  abs(destination - my_global_position)
			if distance_to_goal.x > 3 or distance_to_goal.y > 3:
				controled.moveFeetTo(destination)
			else:
				destination = Vector2.INF
				target_global_position = Vector2.INF