@tool
extends Node3D

@export var cell_grid_size: int = 1
@export var offset: Vector3 = Vector3(-1, 1, -1)

var astar_grid: AStar3D = AStar3D.new()
var timer: float = 0

func _ready() -> void:
	get_parent().connect("map_changed", generate)
	
func _process(delta: float) -> void:
	pass
	#timer += delta
	#if timer >= 1:
		#timer = 0
		##timer -= 1
		#generate()

func generate() -> void:
	astar_grid.clear()
	
	var cell_cast_positions: Array[Vector3] = get_cell_cast_positions()
	
	var next_point_id: int = 0
	var point_id_map: Dictionary[Vector3i, int]
	
	for cell in get_parent().get_used_cells():
		if get_parent().get_cell_item(cell + Vector3i(0,1,0)) != GridMap.INVALID_CELL_ITEM:
			continue # skip cells that are covered by something
			
		for cast_position in cell_cast_positions:
			var params: PhysicsRayQueryParameters3D = PhysicsRayQueryParameters3D.new()
			params.from = cast_position + get_parent().map_to_local(cell)
			params.to = params.from + Vector3.DOWN * 2
			params.collision_mask = get_parent().collision_layer
			#print("Raycasting from %s to %s" % [params.from, params.to])
			DebugDraw3D.draw_line(params.from, params.to, Color.RED, 1)
			var result: Dictionary = get_world_3d().direct_space_state.intersect_ray(params)
			if result.has("position"):
				var path_point: Vector3 = result["position"] + offset
				DebugDraw3D.draw_sphere(path_point, 0.5, Color.YELLOW, 1)
				astar_grid.add_point(next_point_id, path_point, 1.0)
				point_id_map[cell] = next_point_id
				next_point_id += 1
				
	for cell in point_id_map.keys():
		var point_id: int = point_id_map[cell]
		var point_pos: Vector3 = astar_grid.get_point_position(point_id)
		for x in range(-1, 2):
			for y in range(-1, 2):
				for z in range(-1, 2):
					var neighbour_cell: Vector3i = cell + Vector3i(x,y,z)
					if point_id_map.has(neighbour_cell):
						var neighbour_point_id: int = point_id_map[neighbour_cell]
						if astar_grid.are_points_connected(neighbour_point_id, point_id):
							continue
						var neighbour_point_pos: Vector3 = astar_grid.get_point_position(neighbour_point_id)
						var params: PhysicsRayQueryParameters3D = PhysicsRayQueryParameters3D.new()
						params.from = point_pos
						params.to = neighbour_point_pos
						params.collision_mask = get_parent().collision_layer
						var result: Dictionary = get_world_3d().direct_space_state.intersect_ray(params)
						if !result.has("position"):
							astar_grid.connect_points(point_id, neighbour_point_id)
							DebugDraw3D.draw_line(params.from, params.to, Color.YELLOW, 1)
			
			#print(result.keys())
		
func get_cell_cast_positions() -> Array[Vector3]:
	var ret: Array[Vector3] = []
	var frac: int = cell_grid_size * 2
	for x in range(1, frac, 2):
		var x_pos: float = (x / float(frac)) * get_parent().cell_size.x
		for y in range(1, frac, 2):
			var y_pos: float = (y / float(frac)) * get_parent().cell_size.z
			ret.append(Vector3(x_pos, 1, y_pos))
	return ret