#include "patrol_path.h"

void PatrolPath::_bind_methods() {}

void PatrolPath::child_added(Node *child) {
	if (Node3D * child3d{ cast_to<Node3D>(child) }) {
		this->path.push_back(child3d);
	}
}

void PatrolPath::child_removed(Node *child) {
	if (Node3D * child3d{ cast_to<Node3D>(child) }) {
		this->path.erase(child3d);
	}
}

void PatrolPath::enter_tree() {
	this->connect("child_entered_tree", callable_mp(this, &self_type::child_added));
	this->connect("child_exiting_tree", callable_mp(this, &self_type::child_removed));
}

void PatrolPath::_notification(int what) {
	if (Engine::get_singleton()->is_editor_hint()) {
		return;
	}
	switch (what) {
		default:
			return;
		case NOTIFICATION_ENTER_TREE:
			enter_tree();
			return;
	}
}

int PatrolPath::point_count() const {
	return this->path.size();
}

Vector3 PatrolPath::point_at(int &index) const {
	index = Math::abs(index) % point_count();
	return point_at_unchecked(index);
}

Vector3 PatrolPath::point_at_unchecked(int const index) const {
	return this->path.get(index)->get_global_position();
}

Vector3 PatrolPath::get_closest_point(Vector3 global_position, int *idx) {
	int best_choice_idx{ -1 };
	float best_choice_distance{ Math::INF };
	Vector3 point{ 0, 0, 0 };
	for (int i{ 0 }; i < point_count(); ++i) {
		int const next_idx{ (i + 1) % point_count() };
		Vector3 const current{ point_at(i) };
		Vector3 const next{ point_at_unchecked(next_idx) };
		Vector3 const path_direction{ (next - current).normalized() };
		Vector3 const difference{ global_position - current };
		Vector3 const closest{ current + path_direction.dot(difference) * path_direction };
		float const distance{ global_position.distance_squared_to(closest) };
		if (distance < best_choice_distance) {
			best_choice_idx = i;
			best_choice_distance = distance;
			point = closest;
		}
	}
	if (idx) {
		*idx = best_choice_idx;
	}
	return point;
}