#include "rts_states.hpp"
#include "godot_cpp/variant/utility_functions.hpp"

void MoveTo::_bind_methods() {}

void MoveTo::_ready() {
    gd::UtilityFunctions::print("_ready", this->get_path());
    this->agent = this->get_node<gd::NavigationAgent3D>("../NavigationAgent3D");
    this->agent->set_target_position(this->target_node->get_global_position());
    this->parent_node3d = Object::cast_to<gd::Node3D>(this->get_parent());
}

void MoveTo::_end_state() {
    gd::UtilityFunctions::print("_end_state on ", this->get_path());
    this->agent->set_target_position(this->parent_node3d->get_global_position());
}

void MoveTo::_process(double delta_time) {
    gd::Vector3 const pos = this->parent_node3d->get_global_position();
    gd::Vector3 const target = this->agent->get_next_path_position();
    gd::Vector3 const direction = (target - pos).normalized();
    this->parent_node3d->set_global_position(pos + direction * delta_time);
    this->parent_node3d->look_at(pos - gd::Vector3{direction.x, 0.f, direction.z});
    if(this->agent->is_navigation_finished())
        this->end_state();
    State::_process(delta_time);
}

void Activate::_bind_methods() {}

void Animate::_bind_methods() {}
