#include "rts_states.hpp"
#include "utils/util_functions.hpp"
#include <godot_cpp/core/math.hpp>
#include <godot_cpp/variant/utility_functions.hpp>
#include <cmath>

void MoveTo::_bind_methods() {}

void MoveTo::_ready() {
    this->parent_unit = Object::cast_to<Unit>(this->get_parent());
    this->agent = this->get_node<gd::NavigationAgent3D>("../NavigationAgent3D");
    if(this->target_node == nullptr) {
        this->end_state();
        gd::UtilityFunctions::push_warning("failed to start MoveTo state due to missing target");
    } else {
        this->agent->connect("velocity_computed", this->nav_velocity_computed);
        this->calculate_path();
    }
}

void MoveTo::_end_state() {
    this->agent->disconnect("velocity_computed", this->nav_velocity_computed);
    this->agent->set_target_position(this->parent_unit->get_global_position());
    this->agent->set_velocity({0.f, 0.f, 0.f});
    this->parent_unit->set_velocity({0.f, 0.f, 0.f});
}

void MoveTo::_process(double) {
    gd::Vector3 const pos = this->parent_unit->get_global_position();
    gd::Vector3 const target = this->agent->get_next_path_position();
    gd::Vector3 const direction = (target - pos).normalized();
    bool const navigation_finished{this->agent->is_navigation_finished()};
    if(this->is_action_done_interrupt() || navigation_finished) {
        this->end_state();
        return;
    }
    this->agent->set_velocity(direction * this->parent_unit->get_movement_speed());
    if((utils::time_seconds() - this->last_repath) > this->get_repath_interval())
        this->calculate_path();
}

void MoveTo::calculate_path() {
    gd::Vector3 const target_pos{this->target_node->get_global_position()};
    this->agent->set_target_position(target_pos);
    this->last_repath = utils::time_seconds();
    this->target_position_at_last = target_pos;
}

void MoveTo::on_velocity_computed(gd::Vector3 vel) {
    gd::Vector3 const pos{this->parent_unit->get_global_position()};
    gd::Vector3 const target_dir{(this->agent->get_next_path_position() - pos).normalized()};
    // recalculate if avoidance is causing the motion to lock up
    if(target_dir.dot(vel.normalized()) < 0.25f)
        this->calculate_path();
}

float MoveTo::target_delta_position() const {
    return this->target_position_at_last.distance_to(this->target_node->get_global_position());
}

float MoveTo::distance_to_target() const {
    return this->target_position_at_last.distance_to(this->parent_unit->get_global_position());
}

double MoveTo::get_repath_interval() const {
    float const target_delta_position{this->target_delta_position()};
    if(target_delta_position == 0.f)
        return INFINITY;
    else
        return gd::Math::max(gd::Math::min(double(this->distance_to_target()), 5.0) - target_delta_position, 0.2);
}

void Activate::_bind_methods() {}

void Activate::_ready() {
    this->anim = this->get_parent()->get_node<gd::AnimationPlayer>("%AnimationPlayer");
    if(!this->anim->has_animation(this->animation))
        this->end_state();
    else this->anim->play(this->animation);
}

void Activate::_process(double) {
    bool const animation_finished{!this->anim->is_playing() || this->anim->get_current_animation() != this->animation};
    if(this->is_action_done_interrupt() || animation_finished)
        this->end_state();
}

void Animate::_bind_methods() {}

void Animate::_ready() {
    this->anim = this->get_parent()->get_node<gd::AnimationPlayer>("%AnimationPlayer");
    if(!this->anim->has_animation(this->animation))
        this->end_state();
    else this->anim->play(this->animation);
}

void Animate::_process(double) {
    bool const animation_finished{!this->anim->is_playing() || this->anim->get_current_animation() != this->animation};
    if(this->is_action_done_interrupt() || animation_finished)
        this->end_state();
}

void Animate::_end_state() {
    this->anim->stop();
}