#include "player_states.hpp"
#include "player_character.hpp"
#include "tunnels_game_mode.hpp"
#include "utils/game_root.hpp"
#include <godot_cpp/variant/utility_functions.hpp>

namespace godot {
void PlayerState::_bind_methods() {
#define CLASSNAME PlayerState
}

void PlayerState::init(Node *target, StateMachine *machine) {
    this->state_machine = machine;
    this->parent = Object::cast_to<PlayerCharacter>(target);
    if(!this->parent)
        UtilityFunctions::push_error("Attempt to apply player state to non-PlayerCharacter node of type '", target->get_class(), "'");
    Ref<TunnelsGameMode> game_mode = GameRoot::get_singleton()->get_game_mode();
    this->player = game_mode->get_player_instance();
    if(!this->player)
        UtilityFunctions::push_error("Failed to find valid TunnelsPlayer instance in game mode");
}

StringName PlayerState::get_next() const {
    return this->get_class();
}

float PlayerState::get_distance_from_player(bool sqr) const {
    Vector3 const target_position = this->get_manual_character()->get_global_position();
    return sqr
        ? target_position.distance_squared_to(this->parent->get_global_position())
        : target_position.distance_to(this->parent->get_global_position());
}

PlayerCharacter *PlayerState::get_manual_character() const {
    return this->player->get_character();
}

#undef CLASSNAME // PlayerState

void FollowingPlayer::_bind_methods() {
#define CLASSNAME FollowingPlayer
}

void FollowingPlayer::_process(double delta_time) {
    this->parent->move(this->get_manual_character()->get_velocity_target().normalized());
}

StringName FollowingPlayer::get_next() const {
    if(this->get_distance_from_player(true) > std::pow(4.f, 2.f))
        return "Regroup";
    return this->get_class();
}

#undef CLASSNAME // FollowingPlayer

void Regroup::_bind_methods() {
#define CLASSNAME Regroup
}

void Regroup::_process(double delta_time) {
    Basis const player_basis = this->get_manual_character()->get_global_transform().basis;
    Vector3 const parent_position = this->parent->get_global_position();
    Vector3 const player_position = this->get_manual_character()->get_global_position();
    Vector3 const left = player_basis.get_column(2);
    Vector3 const offset = left.dot(parent_position - player_position) * left;
    this->target = player_position - player_basis.get_column(2) + offset;
    this->parent->move_to(this->target, 1.f);
}

void Regroup::_exit_tree() {
    this->parent->move_to(this->parent->get_global_position(), 0.5f);
}

StringName Regroup::get_next() const {
    if(this->target.distance_squared_to(this->parent->get_global_position()) < std::pow(1.f, 2.f))
        return "FollowingPlayer";
    return this->get_class();
}

#undef CLASSNAME // Regroup
}