#include "goal_marker.hpp"
#include "godot_cpp/core/math.hpp"
#include "planner.hpp"
#include "utils/godot_macros.h"
#include <godot_cpp/classes/global_constants.hpp>

namespace godot {
void GoalMarker::_bind_methods() {
#define CLASSNAME GoalMarker
    GDPROPERTY_HINTED(goal, Variant::OBJECT, PROPERTY_HINT_RESOURCE_TYPE, "Goal");
}

bool GoalMarker::is_point_on(Vector3 point) {
    return point.distance_squared_to(this->get_global_position()) <= .5f * .5f;
}

Vector3 GoalMarker::nearest_point_on(Vector3 nearest_to) {
    return this->get_global_position();
}

void GoalMarker::set_goal(Ref<goap::Goal> goal) {
    this->goal = goal;
}

Ref<goap::Goal> GoalMarker::get_goal() const {
    return this->goal;
}

void GoalMarker::set_radius(float radius) {
    this->radius = radius;
}

float GoalMarker::get_radius() const {
    return this->radius;
}

#undef CLASSNAME

void LineGoalMarker::_bind_methods() {
#define CLASSNAME LineGoalMarker
    GDPROPERTY(extent, Variant::FLOAT);
}

bool LineGoalMarker::is_point_on(Vector3 point) {
    return this->nearest_point_on(point).distance_to(point) < this->radius * this->radius;
}

Vector3 LineGoalMarker::nearest_point_on(Vector3 point) {
    Basis const &basis{this->get_global_basis()};
    Vector3 const left_unit{basis.get_column(0).normalized()};
    Vector3 const right_vec{this->get_global_position() + left_unit * -extent};
    float const length{extent * 2.f};
    return right_vec + left_unit * Math::clamp(left_unit.dot(point - right_vec), 0.f, length);
}

void LineGoalMarker::set_extent(float extent) {
    this->extent = extent;
}

float LineGoalMarker::get_extent() const {
    return this->extent;
}
}
