#include "navigation/PathFinder.h"

#include <Eigen/Core>
#include <cstdlib>
#include <iostream>

namespace {
int fail(const char* message)
{
    std::cout << "PathFinderTests: " << message << std::endl;
    return EXIT_FAILURE;
}
}

int main()
{
    FRG::PathFinder pathFinder;
    pathFinder.build();
    if (!pathFinder.isReady()) return fail("Navmesh was not built");

    const Eigen::Vector3f start(-50.f, 0.f, -50.f);
    const Eigen::Vector3f end(50.f, 0.f, 50.f);
    const auto path = pathFinder.findPathToNearest(start, end);
    if (path.size() < 2) return fail("Path query did not produce a route");
    if (!pathFinder.isWalkable(Eigen::Vector3f(0.f, 0.f, 50.f)))
        return fail("Walkability query failed");

    const auto clamped = pathFinder.clampToNavMesh(FRG::Vec2fp::fromFloat(150.f, 150.f));
    if (static_cast<float>(clamped.x) > 100.01f ||
        static_cast<float>(clamped.y) > 100.01f)
        return fail("Clamping returned a point outside the navmesh");

    std::cout << "PathFinderTests: OK" << std::endl;
    return EXIT_SUCCESS;
}