#include "character_actor.hpp"
#include "goal_marker.hpp"
#include "planner.hpp"
#include "projectile_pool.hpp"
#include "state.hpp"
#include "tunnels_game_mode.hpp"
#include "utils/game_root.hpp"
#include "utils/godot_macros.h"
#include <cmath>
#include <godot_cpp/classes/navigation_agent3d.hpp>
#include <godot_cpp/classes/time.hpp>
#include <godot_cpp/variant/callable.hpp>
#include <godot_cpp/variant/utility_functions.hpp>

namespace godot {
void CharacterActor::_bind_methods() {
#define CLASSNAME CharacterActor
    GDPROPERTY_HINTED(rotation_speed_curve, Variant::OBJECT, PROPERTY_HINT_RESOURCE_TYPE, "Curve");
    GDPROPERTY_HINTED(target, Variant::OBJECT, PROPERTY_HINT_NODE_TYPE, "Node");
    GDPROPERTY(acceleration, Variant::FLOAT);
    GDPROPERTY(walk_speed, Variant::FLOAT);
    GDPROPERTY(sprint_speed, Variant::FLOAT);
    GDPROPERTY(rotation_speed, Variant::FLOAT);
    GDFUNCTION(get_is_near_player);
}

void CharacterActor::_enter_tree() { GDGAMEONLY();
    this->nav_agent = this->get_node<NavigationAgent3D>("NavigationAgent3D");
    this->nav_agent->connect("velocity_computed", callable_mp(this, &CharacterActor::set_velocity_target));
    this->target_rotation = this->get_global_transform().get_basis().get_quaternion();
    this->health = this->get_node<Health>("Health");
    this->primary_weapon_pool = this->get_node<ProjectilePool>("ProjectilePool");
    this->planner = this->get_node<goap::Planner>("Planner");
    Ref<TunnelsGameMode> game_mode = GameRoot::get_singleton()->get_game_mode();
    game_mode->register_player_character(this);
}

void CharacterActor::_process(double delta_time) { GDGAMEONLY();
    if(!this->mode_manual) {
        this->process_behaviour(delta_time);
        this->process_navigation(delta_time);
        if(!this->velocity_target.is_zero_approx())
            this->aim_direction(this->velocity_target.normalized());
    }
    if(this->firing) {
        this->try_fire_weapon();
    }
    this->process_rotation(delta_time);
}

void CharacterActor::_physics_process(double delta_time) { GDGAMEONLY();
    // accelerate towards velocity target
    Vector3 const new_velocity = this->get_velocity().move_toward(this->velocity_target, delta_time * CharacterActor::acceleration);
    Vector3 const gravity{Vector3{0.f, this->get_velocity().y - 9.8f, 0.f}};
    // apply either gravity or walking velocity depending on results
    this->set_velocity(this->is_on_floor() ? new_velocity : this->get_velocity() + gravity);
    // update position
    this->move_and_slide();
}

void CharacterActor::move(Vector3 world_vector) {
    this->velocity_target = world_vector * CharacterActor::walk_speed;
}

void CharacterActor::aim(Vector3 at) {
    // calculate the forward vector by normalized difference between player character and the target on the XZ plane
    Vector3 const position{this->weapon_muzzle->get_global_position()};
    Vector3 const forward{Vector3{at.x, 0.f, at.z} - Vector3{position.x, 0.f, position.z}};
    this->aim_direction(forward.normalized());
}

void CharacterActor::aim_direction(Vector3 direction) {
    if(!direction.is_normalized()) {
        UtilityFunctions::push_error("'", this->get_path(), "' Cannot aim along a non-normalized direction vector", direction.length_squared());
        return;
    }
    // we always want up to be the global unit up
    Vector3 const up{0.f, 1.f, 0.f};
    // left is the cross product of the two
    this->target_rotation = Basis{up.cross(direction), up, direction};
}

void CharacterActor::move_to(Vector3 to, float target_distance) {
    this->nav_agent->set_target_desired_distance(target_distance);
    this->nav_agent->set_target_position(to);
}

void CharacterActor::shoot_at(Vector3 at) {
    this->aim(at);
    this->set_firing(true);
}

void CharacterActor::force_update_action() {
    this->set_state(this->planner->get_next_state());
}

void CharacterActor::set_firing(bool firing) {
    this->firing = firing;
}

void CharacterActor::set_manual_mode(bool value) {
    this->mode_manual = value;
    ProcessMode const mode = value ?  ProcessMode::PROCESS_MODE_DISABLED : ProcessMode::PROCESS_MODE_PAUSABLE;
    //this->nav_agent->set_process_mode(mode);
    this->nav_agent->set_avoidance_priority(value ? 1.f : 0.9f);
    this->set_state(goap::State::new_invalid());
}

void CharacterActor::set_rotation_speed_curve(Ref<Curve> curve) {
    this->rotation_speed_curve = curve;
}

Ref<Curve> CharacterActor::get_rotation_speed_curve() const {
    return this->rotation_speed_curve;
}

Health *CharacterActor::get_health() {
    return this->health;
}

Health const *CharacterActor::get_health() const {
    return this->health;
}

void CharacterActor::set_character_data(Ref<CharacterData> data) {
    this->data = data;
    this->fire_interval = 1.f / this->data->get_weapon()->get_rounds_per_second();
    this->primary_weapon_pool->set_data(this->data->get_weapon());
}

void CharacterActor::set_weapon_muzzle(Node3D *node) {
    this->weapon_muzzle = node;
}

void CharacterActor::set_velocity_target(Vector3 value) {
    this->velocity_target = value;
}

Vector3 CharacterActor::get_velocity_target() const {
    return this->velocity_target;
}

bool CharacterActor::get_is_near_player() const {
    return Ref<TunnelsGameMode>(GameRoot::get_singleton()->get_game_mode())
        ->get_player_instance()
        ->get_character()
        ->get_global_position().distance_to(this->get_global_position()) < 5.f;
}

bool CharacterActor::get_is_near_target() const {
    GoalMarker *target_marker = Object::cast_to<GoalMarker>(this->target);
    Node3D *target_node3d = Object::cast_to<Node3D>(this->target);
    return target_marker
        ? target_marker->is_point_on(this->get_global_position())
        : (target_node3d && target_node3d->get_global_position().distance_to(this->get_global_position()) < 5.f);
}

Vector3 CharacterActor::get_move_target() const {
    GoalMarker *as_marker = Object::cast_to<GoalMarker>(this->current_state.move_to);
    if(as_marker)
        return as_marker->nearest_point_on(this->get_global_position());
    Node3D *as_node3d = Object::cast_to<Node3D>(this->current_state.move_to);
    if(as_node3d)
        return as_node3d->get_global_position();
    return this->get_global_position();
}

void CharacterActor::set_target(Node *target) {
    this->target = target;
}

Node *CharacterActor::get_target() const {
    return this->target;
}

goap::Planner *CharacterActor::get_planner() const {
    return this->planner;
}

void CharacterActor::set_state(goap::State state) {
    switch(this->current_state.type) {
    default:
        break;
    case goap::State::STATE_MOVE_TO:
        this->nav_agent->set_target_position(this->get_global_position());
        break;
    }
    this->current_state = state;
    switch(state.type) {
    default:
        break;
    case goap::State::STATE_MOVE_TO:
        this->move_to(this->get_move_target());
        break;
    }
}

void CharacterActor::process_behaviour(double delta_time) {
    if(this->current_state.is_complete(this) || this->planner->is_action_complete())
        this->set_state(this->planner->get_next_state());
    switch(this->current_state.type) {
    default:
        break;
    case goap::State::STATE_MOVE_TO:
        if(this->nav_agent->get_target_position().distance_to(this->get_move_target()) > 2.f)
            this->nav_agent->set_target_position(this->get_move_target());
        break;
    case goap::State::STATE_ACTIVATE:
        break;
    case goap::State::STATE_ANIMATE:
        break;
    }
}

void CharacterActor::process_navigation(double delta_time) {
    float const distance_sqr = this->nav_agent->get_target_position().distance_squared_to(this->get_global_position());
    float const distance_target_sqr = std::pow(this->nav_agent->get_target_desired_distance(), 2.f);
    if(!this->nav_agent->is_navigation_finished() && distance_sqr >= distance_target_sqr) {
        Vector3 const target_position = this->nav_agent->get_next_path_position();
        Vector3 const direction = (target_position - this->get_global_position()).normalized();
        if(this->nav_agent->get_avoidance_enabled())
            this->nav_agent->set_velocity(direction * CharacterActor::walk_speed);
        else
            this->move(direction);
    }
}

void CharacterActor::process_rotation(double delta_time) {
    // copy the current transform and basis matrix
    Transform3D trans{this->get_global_transform()};
    Basis basis = trans.get_basis();
    // construct the current rotation ..
    Quaternion const current_quaternion = basis.get_rotation_quaternion();
    // .. and the target rotation from their respective bases
    Quaternion const target_quaternion = this->target_rotation.get_rotation_quaternion();
    // calculate the angle that still needs to be traveled
    float const angle = current_quaternion.angle_to(target_quaternion);
    // calculate the angle amount that can be moved this frame
    float const angle_step{float(this->rotation_speed_curve->sample(angle) * CharacterActor::rotation_speed * delta_time)};
    // update this object's global transform with the new rotation
    basis.set_quaternion(angle < angle_step ? target_quaternion // to avoid overshooting, check if the max step is smaller than the angle distance
                         : current_quaternion.slerp(target_quaternion, angle_step / angle)); // convert the angle step to a lerp t value between current and target rotations
    trans.set_basis(basis);
    this->set_global_transform(trans);
}

void CharacterActor::try_fire_weapon() {
    if(float(Time::get_singleton()->get_ticks_msec()) / 1000.f < this->fire_timer)
        return;
    if(!this->data->get_weapon()->get_allow_automatic())
        this->set_firing(false);
    this->fire_timer = float(Time::get_singleton()->get_ticks_msec()) / 1000.f + this->fire_interval;
    Node3D *node = this->primary_weapon_pool->claim_projectile();
    if(node != nullptr)
        node->set_global_transform(this->weapon_muzzle->get_global_transform());
}

void CharacterActor::set_acceleration(float acceleration) {
    this->acceleration = acceleration;
}

float CharacterActor::get_acceleration() const {
    return this->acceleration;
}

void CharacterActor::set_walk_speed(float walk_speed) {
    this->walk_speed = walk_speed;
}

float CharacterActor::get_walk_speed() const {
    return this->walk_speed;
}

void CharacterActor::set_sprint_speed(float sprint_speed) {
    this->sprint_speed = sprint_speed;
}

float CharacterActor::get_sprint_speed() const {
    return this->sprint_speed;
}

void CharacterActor::set_rotation_speed(float rotation_speed) {
    this->rotation_speed = rotation_speed;
}

float CharacterActor::get_rotation_speed() const {
    return this->rotation_speed;
}
}
