extends Node

class_name Entity

var position: Vector3
var position2D: Vector2i
var speed = 3.0 # blocs per second
var id = -1

var moving_counter = 0

signal moving

func _init(id: int, position: Vector2i):
	self.id = id
	self.position.x = position.x
	self.position.y = Global.world.get_height(Vector2i(position.x, position.y))
	self.position.z = position.y
	position2D = position
	
func get_data():
	var data = {
		"id": self.id,
		"position": self.position
	}
	
	return data

func move(new_position: Vector2i):
	var new_position_3d = Vector3(new_position.x, Global.world.get_height(Vector2i(new_position.x, new_position.y)), new_position.y)
	emit_signal("moving", new_position_3d, speed)
	Global.world.blocs[position.x][position.y].entity = -1
	self.position = new_position_3d
	position2D = new_position
	Global.world.blocs[new_position.x][new_position.y].entity = id

func follow_path(path: Array):
	moving_counter += 1
	var current_moving_counter = moving_counter
	for next in path:
		
		if moving_counter != current_moving_counter:
			return
			
		move(next)
		var t = Timer.new()
		t.wait_time =  1.0 / speed
		t.set_one_shot(true)
		t.autostart = true
		Engine.get_main_loop().get_root().add_child(t)
		await t.timeout
		t.queue_free()

func heuristic(a: Vector2i, b: Vector2i) -> float:
	return abs(a.x - b.x) + abs(a.y - b.y)

func pathfinding(goal: Vector2i):
	var frontier = []
	var priorities = {}
	var came_from = {}
	var cost_so_far = {}
	came_from[position2D] = null
	cost_so_far[position2D] = 0
	frontier.append(position2D)
	
	while frontier.size():
		var current = frontier.pop_front()
		
		if current == goal:
			break
		
		for next in Global.world.neighbours(current):
			var new_cost = cost_so_far[current] + Global.world.cost(current, next)
			if next not in cost_so_far or new_cost < cost_so_far[next]:
				cost_so_far[next] = new_cost
				var priority = new_cost + heuristic(goal, next)
				frontier.append(next)
				priorities[next] = priority
				came_from[next] = current
		frontier.sort_custom(
			func(a, b):
				if priorities[a] < priorities[b]:
					return true
				return false
		)
		
	var current = goal
	var path = []
	while current != position2D:
		path.append(current)
		current = came_from[current]
	path.reverse()
	return path