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

void MoveTo::_bind_methods() {}

void MoveTo::_ready() {
    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() {
    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->is_action_done())
        this->state_finished();
    else if(this->agent->is_navigation_finished())
        this->state_failed();
}

void Activate::_bind_methods() {}

void Animate::_bind_methods() {}

void Animate::_ready() {
    this->anim = this->get_node<gd::AnimationPlayer>("../AnimationPlayer");
    this->anim->queue(this->animation);
}

void Animate::_process(double delta_time) {
    if(this->is_action_done()) {
        this->state_finished();
    } else if(!this->anim->is_playing() || this->anim->get_current_animation() != this->animation) {
        this->state_failed();
    }
}

void Animate::_end_state() {
    if(this->anim->get_current_animation() == this->animation)
        this->anim->stop();
}