extends CharacterBody2D

signal finished_movement()

const MAX_SPEED: float            = 300.0
const ACCELERATION: int           = 4800
const SNAP_DISTANCE: float        = 2.0
var frozen: bool                  = false
var target: Vector2               = Vector2(-1, -1)
var path: Array[MPNavigationNode] = []
var current_path_index: int       = 0

@onready var navigation_graph = $"../NavigationGraph"


func move_to(target: Vector2) -> void:
    var using_position: Vector2 = (global_position - Vector2(16, 16)) / 32
    var pathfinding_result: MPPathfindingResult = navigation_graph.determine_next_position(using_position, target)
    navigation_graph.draw_pathfinding_result(pathfinding_result)
    path           = pathfinding_result.path
    current_path_index = 0
    print("Moving to ", target)
    for node in path:
        print(" - ", node.position)


func _physics_process(delta: float) -> void:
    if Input.is_action_just_pressed("ui_select"):
        frozen = !frozen

    if frozen or path.size() == 0:
        return

    var movement: Vector2 = Vector2()

    if current_path_index < path.size():
        var next_position: Vector2 = path[current_path_index].global_position * 32 + Vector2(16, 16)

        var distance_to_next: float = global_position.distance_to(next_position)
        if distance_to_next > SNAP_DISTANCE:
            movement = (next_position - global_position).normalized() * ACCELERATION
        else:
            # snap to the next node if close enough
            global_position = next_position
            current_path_index += 1

            # if the end of the path is reached, stop movement
            if current_path_index >= path.size():
                path.clear()
                target = Vector2(-1, -1)
                finished_movement.emit()
                navigation_graph.clear_pathfinding_result()

    # Immediate deceleration for tight control
    velocity = movement
    if velocity.length() > MAX_SPEED:
        velocity = velocity.normalized() * MAX_SPEED

    move_and_slide()