@tool
extends Node3D

@export_tool_button("Generate", "Reload") var action: Callable = generate
@export var nav_point_offset: float = 0.5
@export var debug_duration: float = 50.0

@export var collider_shape: Shape3D = SphereShape3D.new()

@onready var grid_map: GridMap = get_parent() as GridMap

var grid: AStar3D = AStar3D.new()

var cell_nav_point_ids: Dictionary[Vector3i, int] = {}
var item_orient_nav_point_positions: Dictionary[int,Vector3] = {}
var item_orient_nav_point_normals: Dictionary[int, Vector3] = {}
@export var item_pair_traversability_map: Dictionary[int, bool] = {}

var cell_neighbours: Array[Vector3i] = []

func get_item_orientation_id(tile: int, orient: int) -> int:
	if tile == -1:
		return 0
	var ret: int = 0
	ret += (tile+1) & 0b11111111
	ret += (orient & 0b11111) << 8
	return ret

func get_pair_id(tile_a: int, orient_a: int, tile_b: int, orient_b: int, relative_pos: Vector3i) -> int:
	# tile_a: 8 bits
	# orient_a: 5 bits
	# tile_b: 8 bits
	# orient_b: 5 bits
	# relative_pos: 5 bits

	var ret: int = 0
	ret |= get_item_orientation_id(tile_a, orient_a)
	ret |= get_item_orientation_id(tile_b, orient_b) << 13
	
	relative_pos += Vector3i(1,1,1)
	var relative_pos_id: int = relative_pos.x + (relative_pos.y * 3) + (relative_pos.z * 9)
	
	ret |= (relative_pos_id & 0b11111) << 26
	
	return ret
	
func get_quad_id(item_orient_a: int, item_orient_b: int, item_orient_c: int, item_orient_d: int, relative_pos: Vector3i) -> int:
	relative_pos += Vector3i(1,1,1)
	var relative_pos_id: int = relative_pos.x + (relative_pos.y * 3) + (relative_pos.z * 9)
	
	var ret: int = 0
	ret |= item_orient_a
	ret |= item_orient_b << 13
	ret |= (relative_pos_id & 0b11111) << 26
	ret |= item_orient_c << 31
	ret |= item_orient_d << 44
	
	return ret
	
func find_nav_point(item: int, orientation: int, global_pos: Vector3) -> Array[Vector3]:
	var id: int = get_item_orientation_id(item, orientation)
	#print("Finding nav point for item %d orientation %d (ID:%d) at location %s" % [item, orientation, id, global_pos])
	if item_orient_nav_point_positions.has(id):
		var relative_pos: Vector3 = item_orient_nav_point_positions[id]
		var normal: Vector3 = item_orient_nav_point_normals[id]
		#print("cached nav point for item %d: %s" % [item, relative_pos])
		return [global_pos + relative_pos + (normal * nav_point_offset),normal]
	else:
		var params: PhysicsRayQueryParameters3D = PhysicsRayQueryParameters3D.new()
		params.from = global_pos + Vector3(0,2,0)
		params.to = global_pos - Vector3(0,2,0)
		#params.shape = collider_shape
		var result: Dictionary = get_world_3d().direct_space_state.intersect_ray(params)
		TowerUtil.draw_raycast_hit(params, result, debug_duration)
		if result.has("position"):
			var relative_pos: Vector3 = result["position"] - global_pos
			item_orient_nav_point_positions.set(id,relative_pos)
			item_orient_nav_point_normals.set(id,result["normal"])
			#print("new nav point for item %d: %s" % [id, relative_pos])
			return [result["position"] + (result["normal"] * nav_point_offset), result["normal"]]
	return []
	
func check_traversability(a: Vector3, b: Vector3) -> bool:
	#print("Testing traversability between %s and %s" % [a,b])
	var params: PhysicsShapeQueryParameters3D = PhysicsShapeQueryParameters3D.new()
	params.shape = collider_shape
	params.transform = Transform3D(Basis(),a)
	params.motion = b - a
	var result: PackedFloat32Array = get_world_3d().direct_space_state.cast_motion(params)
	
	var hit: bool = !is_equal_approx(1.0, result[0])
	
	DebugDraw3D.draw_line(a,b,Color.YELLOW if hit else Color.MAGENTA, debug_duration)
	return !hit
	#TowerUtil.draw_raycast_hit(params, result, debug_duration)
	#return !result.has("position")
	
func update_nav_point(id: int, cell: Vector3i) -> bool:
	var cell_above: Vector3i = cell + Vector3i(0,1,0)
	if grid_map.get_cell_item(cell_above) != -1:
		return false # skip cells with things on top
	var item: int = grid_map.get_cell_item(cell)
	var orientation: int = grid_map.get_cell_item_orientation(cell)
	var global_pos: Vector3 = grid_map.to_global(grid_map.map_to_local(cell))
	var nav_point: Array[Vector3] = find_nav_point(item, orientation, global_pos)
	grid.add_point(id, nav_point[0])
	cell_nav_point_ids.set(cell, id)
	#print("Nav ID for cell %s: %d" % [cell, id])
	DebugDraw3D.draw_sphere(nav_point[0], 0.1, Color.GREEN, debug_duration)
	DebugDraw3D.draw_arrow_ray(nav_point[0], nav_point[1], 1.0, Color.GREEN, 0.5, false, debug_duration)
	return true
	
func update_connections(cell: Vector3i) -> void:
	var nav_id: int = cell_nav_point_ids.get(cell, -1)
	#print("Nav ID for cell %s: %d" % [cell, nav_id])
	if nav_id == -1:
		return
	var global_pos: Vector3 = grid.get_point_position(nav_id)
	var item: int = grid_map.get_cell_item(cell)
	var orientation: int = grid_map.get_cell_item_orientation(cell)
	for rel_pos in cell_neighbours:
		var diagonal = abs(rel_pos.x + rel_pos.z) != 1
		
		var neighbour: Vector3i = cell + rel_pos
		#print("Testing traversability between %s and %s" % [cell, neighbour])
		var neighbour_item: int = grid_map.get_cell_item(neighbour)
		if neighbour_item == -1:
			continue
		var neighbour_orientation: int = grid_map.get_cell_item_orientation(neighbour)
		var neighbour_nav_id: int = cell_nav_point_ids.get(neighbour, -1)
		if neighbour_nav_id == -1:
			continue
		#print("\tNav IDs: %d and %d" % [nav_id, neighbour_nav_id])
		if grid.are_points_connected(nav_id, neighbour_nav_id, true):
			continue
		var neighbour_global_pos: Vector3 = grid.get_point_position(neighbour_nav_id)
		
		var quad_id: int = 0
		if diagonal:
			var diag_obstacle_y: int = neighbour.y + 1
			var cell_c: Vector3i = Vector3i(cell.x, diag_obstacle_y, neighbour.z)
			var cell_d: Vector3i = Vector3i(neighbour.x, diag_obstacle_y, cell.z)
			#print("Testing diagonal link from a:%s to b:%s; considering c:%s and d:%s" % [cell, neighbour, cell_c, cell_d])
			var tile_orient_c: int = get_item_orientation_id(grid_map.get_cell_item(cell_c),grid_map.get_cell_item_orientation(cell_c))
			var tile_orient_d: int = get_item_orientation_id(grid_map.get_cell_item(cell_d),grid_map.get_cell_item_orientation(cell_d))
			quad_id = get_quad_id(get_item_orientation_id(item,orientation),get_item_orientation_id(neighbour_item,neighbour_orientation),tile_orient_c,tile_orient_d,rel_pos)
		else:
			quad_id = get_pair_id(item, orientation, neighbour_item, neighbour_orientation, rel_pos)
			
		DebugDraw3D.draw_text(global_pos.lerp(neighbour_global_pos,0.5), "%x"%quad_id, 32, Color.RED, debug_duration)
			
		#var pair_id: int 
		var traversable: bool = true
		if item_pair_traversability_map.has(quad_id):
			traversable = item_pair_traversability_map.get(quad_id)
		else:
			traversable = check_traversability(global_pos, neighbour_global_pos)
			item_pair_traversability_map.set(quad_id, traversable)
			#print("New traversability pair: ID=%x Traversable=%s" % [quad_id, traversable])
		#print("\tTraversable: %s" % traversable)
		if traversable:
			grid.connect_points(nav_id, neighbour_nav_id)
			#print("Connected points %d and %d" % [nav_id, neighbour_nav_id])
			DebugDraw3D.draw_line(global_pos, neighbour_global_pos, Color.NAVY_BLUE, debug_duration)

func generate() -> void:
	cell_neighbours.clear()
	for x in range(-1,2):
		for y in range(-1,2):
			for z in range(-1,2):
				if x != 0 or z != 0:
					cell_neighbours.append(Vector3i(x,y,z))
	print(cell_neighbours)
	
	cell_nav_point_ids.clear()
	item_orient_nav_point_positions.clear()
	item_pair_traversability_map.clear()
	grid.clear()
	
	var id: int = 0

	for cell in grid_map.get_used_cells():
		var added: bool = update_nav_point(id, cell)
		if added:
			id += 1
		
	for cell in grid_map.get_used_cells():
		update_connections(cell)