extends Module

var enabled := true

var pos_x_register_index:int
var pos_y_register_index:int
var pos_z_register_index:int
var index_register_index:int

var prev_index := 0
var target := Vector3i.ZERO
var path : Array[Vector3i]

func _ready() -> void:
	if Terrain.instance == null:
		print("No terrain to plan path on")
	if cpu.get_parent() is not Robot:
		print("Module not part of Robot")

func get_register_reservation_size()->int:
	return 4

func set_reserved_registers(index:int)->void:
	pos_x_register_index = index
	pos_y_register_index = index+1
	pos_z_register_index = index+2
	index_register_index = index+3

func get_register_types()->Dictionary[int, String]:
	return { 
		pos_x_register_index: "[X] Path", 
		pos_y_register_index: "[Y] Path",
		pos_z_register_index: "[Z] Path",
		index_register_index: "Idx Path"
		}

func early_tick()->void:
	var registers := cpu.cobor_vm.get_registers()
	prev_index = registers[index_register_index]

func late_tick()->void:
	var registers := cpu.cobor_vm.get_registers()
	var index := registers[index_register_index]
	if index < 0:
		target = Vector3i(
			registers[pos_x_register_index],
			registers[pos_y_register_index],
			registers[pos_z_register_index]
		)
		print("Target set: ", target)
	else:
		if index == prev_index:
			print("index stays same")
			return
		if prev_index < 0:
			var path_finder := VoxelAStarGrid3D.new()
			path_finder.set_terrain(Terrain.instance)
			path_finder.set_region(AABB(cpu.get_parent().global_position + Vector3.ONE * -100, Vector3.ONE * 200))
			path = path_finder.find_path(cpu.get_parent().global_position, target)
			if not path.is_empty():
				path.append(target)
			print("Recalculating path of size:", path.size(), " towards: ", target)
		if index >= path.size():
			index = path.size()-1
		var step:Vector3i
		if path.size() == 0: 
			step = Vector3i.ZERO
		else:
			step = path[index]
		print("next step: ", step)
		registers[pos_x_register_index] = step.x
		registers[pos_y_register_index] = step.y
		registers[pos_z_register_index] = step.z
		registers[index_register_index] = index
		cpu.cobor_vm.set_registers(cpu.register_size, registers)