extends Node3D
class_name PathfindingGridLoader

var loading: bool = false # when this is true the grid starts loading
var loading_done: bool = false # when the grid is loaded, this becomes true

@export var height: float= 0.2 ##Choose a height that is just above the floor
@export var lengthX: int = 50 ##How far the grid generates in the x direction, generates as a rectangle 
@export var lengthZ: int = 50 ##How far the grid generates in the z direction, generates as a rectangle 
@export var point_density: float = 1 ##How many points per unit of grid, cannot be = 0 
@export var minimum_connections_per_point: int = 3
@export var wallCollisionLayer: int = 0b100

var astar: AStar2D = AStar2D.new()

func _physics_process(_delta: float) -> void:
	if loading == false: return
	if loading_done == true: return
	generatePoints()
	connectPointsToNeighbors()
	killPointsWithoutFriends(minimum_connections_per_point)
	addWeightToPoints()
	loading = false
	loading_done = true

func loadGrid() -> void:
	loading = true

func castRay(startPoint:Vector3,endPoint: Vector3,collisionMask:int = 0xFFFFFFFF,hitFromInside: bool = true) -> bool: ##Returns true if the ray collides and false if it doesn't
	var space_state = get_world_3d().direct_space_state
	var rayQueryParam = PhysicsRayQueryParameters3D.create(startPoint,endPoint)
	rayQueryParam.collision_mask = collisionMask
	rayQueryParam.hit_from_inside = hitFromInside
	return space_state.intersect_ray(rayQueryParam).size()

func checkPoint(point: Vector2) -> bool: ## only ever call this from physics process as it uses physics for raycast and may not work on idle callback
	var floorDetected: bool = castRay(Vector3(point.x,height,point.y),Vector3(point.x,-(height*1.5),point.y)) 
	var wallDetected: bool = castRay(Vector3(point.x,height,point.y),Vector3(point.x,-(height*1.5),point.y),wallCollisionLayer) #Walls are on collision layer 3 (0b100)
	if !wallDetected && floorDetected:
		return true
	else:
		return false

func getPointsInRadius(origin: Vector2,radius: float, maxPoints: int = 9) -> Array[int]: #Origin point is always index 0 in the array
	var pointIDs: Array[int]
	for point in maxPoints:
		var id = astar.get_closest_point(origin)
		if (astar.get_point_position(id) - origin).length() <= radius:
			pointIDs.push_back(id)
			astar.set_point_disabled(id)
	for id in pointIDs:
		astar.set_point_disabled(id,false)
	return pointIDs

func connectIfValid(id1: int, id2: int):
	var point1 := astar.get_point_position(id1)
	var point2 := astar.get_point_position(id2)
	
	if !castRay(Vector3(point1.x,height,point1.y),Vector3(point2.x,height,point2.y)):
		astar.connect_points(id1,id2)

func generatePoints() -> void:
	var currentPoint: Vector2
	var currentID: int = 0
	for x in lengthX*point_density:
		for z in lengthZ*point_density:
			currentPoint = Vector2((x/point_density + self.global_position.x),z/point_density + self.global_position.z)
			if checkPoint(currentPoint): 
				astar.add_point(currentID,currentPoint)
				currentID += 1

func connectPointsToNeighbors() -> void:
	for id in astar.get_point_ids():
		var neighborPoints: Array[int]
		neighborPoints = getPointsInRadius(astar.get_point_position(id),(1/point_density)*1.5)
		for point in neighborPoints.size()-1:
			connectIfValid(neighborPoints[0],neighborPoints[point+1])

func killPointsWithoutFriends(minConnections: int) -> void: ##Really mean and evil and fucked up function
	var cullPoints: Array[int]
	for id in astar.get_point_ids():
		if astar.get_point_connections(id).size() < minConnections:
			cullPoints.push_back(id)
	for point in cullPoints:
		astar.remove_point(point)

func addWeightToPoints() -> void: #Add weight depending on number of connections
	for id in astar.get_point_ids():
		astar.set_point_weight_scale(id,8 - astar.get_point_connections(id).size())