#include "navigation/PathFinder.h"

#include <Eigen/Core>
#include <cstdlib>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <stdexcept>

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

std::filesystem::path writeNavigationFixture()
{
    const auto path = std::filesystem::temp_directory_path() /
        "shadow_over_bishkek_pathfinder_test.json";
    std::ofstream file(path);
    file << R"json({
        "areas": [{
            "available": true,
            "name": "test_area",
            "polygon": [[-10.0, 10.0], [10.0, 10.0], [10.0, -10.0], [-10.0, -10.0]]
        }],
        "floorY": 2.5,
        "obstacles": [{
            "name": "test_obstacle",
            "polygon": [[-1.0, 1.0], [1.0, 1.0], [1.0, -1.0], [-1.0, -1.0]]
        }]
    })json";
    if (!file)
        throw std::runtime_error("Failed to create navigation test fixture");
    return path;
}
}

int main()
{
    const auto configPath = writeNavigationFixture();
    FRG::PathFinder pathFinder;
    pathFinder.build(configPath.string());
    if (!pathFinder.isReady()) return fail("Navmesh was not built");

    if (pathFinder.getArea().name != "test_area")
        return fail("First navigation area was not loaded");
    if (pathFinder.getArea().polygon.size() != 4)
        return fail("Navigation area polygon was not loaded");
    if (pathFinder.getObstaclePolygons().size() != 1 ||
        pathFinder.getObstaclePolygons().front().name != "test_obstacle")
        return fail("Navigation obstacles were not loaded");
    if (static_cast<float>(pathFinder.getFloorY()) != 2.5f)
        return fail("Navigation floorY was not loaded");

    const Eigen::Vector3f start(-5.f, 0.f, -5.f);
    const Eigen::Vector3f end(5.f, 0.f, 5.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, 5.f)))
        return fail("Walkability query failed");
    if (path.back().y() != 2.5f)
        return fail("Path waypoint did not use floorY");

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

    pathFinder.build(configPath.string());
    if (pathFinder.getObstaclePolygons().size() != 1)
        return fail("Repeated build accumulated navigation obstacles");

    bool rejectedNonJson = false;
    try {
        pathFinder.build((configPath.parent_path() / "navigation.txt").string());
    } catch (const std::runtime_error&) {
        rejectedNonJson = true;
    }
    if (!rejectedNonJson)
        return fail("Non-JSON navigation config was accepted");

    std::filesystem::remove(configPath);
    std::cout << "PathFinderTests: OK" << std::endl;
    return EXIT_SUCCESS;
}