#include "CharacterState.h"
#include "LocationGameState.h"
#include "navigation/PathFinder.h"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdlib>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <memory>
#include <string>
#include <vector>

namespace {

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

std::filesystem::path writeGrid(
	const std::string& name, int width, int depth, bool corridor)
{
	const auto path = std::filesystem::temp_directory_path() / name;
	std::ofstream output(path);
	output << "cellSize 1\nagentRadius 0.1\nfloorY 0\n"
		   << "minX 0\nminZ 0\ngridWidth " << width
		   << "\ngridDepth " << depth << "\n";
	for (int z = 0; z < depth; ++z) {
		for (int x = 0; x < width; ++x)
			output << (!corridor || z == depth / 2 ? '1' : '0');
		output << '\n';
	}
	return path;
}

bool samePath(
	const std::vector<Eigen::Vector3f>& lhs,
	const std::vector<Eigen::Vector3f>& rhs)
{
	if (lhs.size() != rhs.size()) return false;
	for (size_t i = 0; i < lhs.size(); ++i)
		if (lhs[i] != rhs[i]) return false;
	return true;
}

float segmentDistanceSquared(
	const Eigen::Vector3f& point,
	const Eigen::Vector3f& start,
	const Eigen::Vector3f& end)
{
	const Eigen::Vector2f p(point.x(), point.z());
	const Eigen::Vector2f a(start.x(), start.z());
	const Eigen::Vector2f segment(end.x() - start.x(), end.z() - start.z());
	const float lengthSquared = segment.squaredNorm();
	const float t = lengthSquared > 1e-8f
		? (std::max)(0.f, (std::min)(1.f, (p - a).dot(segment) / lengthSquared))
		: 0.f;
	return (p - (a + segment * t)).squaredNorm();
}

void runBenchmark(FRG::PathFinder& pathFinder, int queries)
{
	std::vector<FRG::PathFinder::DynamicObstacle> obstacles;
	for (int i = 0; i < queries; ++i) {
		const float x = 4.5f + static_cast<float>((i * 7) % 70);
		const float z = 4.5f + static_cast<float>((i * 13) % 70);
		obstacles.push_back({Eigen::Vector3f(x, 0.f, z), 0.25f});
	}
	const auto start = std::chrono::steady_clock::now();
	size_t waypointCount = 0;
	for (int i = 0; i < queries; ++i) {
		const Eigen::Vector3f from(
			1.5f, 0.f, 1.5f + static_cast<float>(i % 30));
		const Eigen::Vector3f to(
			78.5f, 0.f, 78.5f - static_cast<float>(i % 30));
		waypointCount +=
			pathFinder.findPathToNearest(from, to, obstacles).size();
	}
	const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
		std::chrono::steady_clock::now() - start);
	std::cout << "PathFinder benchmark " << queries << " characters/queries: "
			  << elapsed.count() << " ms, " << waypointCount
			  << " waypoints" << std::endl;
}

} // namespace

int main(int argc, char** argv)
{
	const bool benchmark = argc > 1 && std::string(argv[1]) == "--benchmark";
	const auto openGrid = writeGrid("frg_pathfinder_open.txt", 80, 80, false);
	FRG::PathFinder pathFinder;
	pathFinder.build(openGrid.string());
	if (!pathFinder.isReady()) return fail("Open test grid did not load");

	const FRG::PathFinder::DynamicObstacle first{
		Eigen::Vector3f(8.5f, 0.f, 8.5f), 0.75f};
	const FRG::PathFinder::DynamicObstacle second{
		Eigen::Vector3f(12.5f, 0.f, 10.5f), 0.75f};
	const Eigen::Vector3f start(2.5f, 0.f, 8.5f);
	const Eigen::Vector3f end(18.5f, 0.f, 8.5f);
	const auto ordered = pathFinder.findPathToNearest(
		start, end, {first, second});
	const auto reversed = pathFinder.findPathToNearest(
		start, end, {second, first});
	if (ordered.empty() || !samePath(ordered, reversed))
		return fail("Obstacle insertion order changed the route");
	Eigen::Vector3f previous = start;
	for (const Eigen::Vector3f& point : ordered) {
		if (segmentDistanceSquared(
				first.position, previous, point) < 0.85f * 0.85f)
			return fail("Smoothed path crossed a dynamic obstacle");
		previous = point;
	}

	const auto occupiedDestination =
		pathFinder.findPathToNearest(start, first.position, {first});
	if (occupiedDestination.empty())
		return fail("Occupied destination did not produce a nearest route");
	const Eigen::Vector3f occupiedEnd = occupiedDestination.back();
	const Eigen::Vector2f occupiedDelta(
		occupiedEnd.x() - first.position.x(),
		occupiedEnd.z() - first.position.z());
	if (occupiedDelta.squaredNorm() < 0.85f * 0.85f)
		return fail("Occupied destination ended inside the obstacle");

	const auto corridorGrid =
		writeGrid("frg_pathfinder_corridor.txt", 20, 3, true);
	FRG::PathFinder corridor;
	corridor.build(corridorGrid.string());
	const FRG::PathFinder::DynamicObstacle blocker{
		Eigen::Vector3f(10.5f, 0.f, 1.5f), 0.45f};
	const auto nearest = corridor.findPathToNearest(
		Eigen::Vector3f(1.5f, 0.f, 1.5f),
		Eigen::Vector3f(18.5f, 0.f, 1.5f), {blocker});
	if (nearest.empty() || nearest.back().x() >= blocker.position.x())
		return fail("Nearest fallback ignored a dynamic blocker");

	FRG::CharacterState guardedMover;
	guardedMover.configureAsPlayer();
	guardedMover.enabled = true;
	guardedMover.walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f);
	guardedMover.requestedWalkTarget = guardedMover.walkTarget;
	guardedMover.pathWaypoints = {guardedMover.walkTarget};
	const std::vector<FRG::CharacterMovementObstacle> movementObstacles{
		{"mover", Eigen::Vector3f::Zero(), 0.45f},
		{"blocker", Eigen::Vector3f(0.8f, 0.f, 0.f), 0.45f}};
	FRG::CharacterUpdateContext guardedContext;
	guardedContext.characterId = "mover";
	guardedContext.movementObstacles = &movementObstacles;
	FRG::CharacterUpdateEvents blockedEvents;
	for (int i = 0; i < 3; ++i)
		blockedEvents = guardedMover.update(100, guardedContext);
	if (!guardedMover.position.isZero() ||
		blockedEvents.forceNavigationReplan ||
		!guardedMover.pathWaypoints.empty() ||
		guardedMover.requestedWalkTarget != guardedMover.position ||
		guardedMover.currentState != FRG::AnimationState::STAND)
		return fail("Blocked player action was not cancelled");

	FRG::CharacterState retryingNpc;
	retryingNpc.enabled = true;
	retryingNpc.configureNavigationRetryCooldowns(
		"retrying_npc", {2, 3, 4, 5, 6, 7, 8, 9, 10, 11});
	FRG::CharacterState otherNpc;
	otherNpc.configureNavigationRetryCooldowns(
		"other_npc", {2, 3, 4, 5, 6, 7, 8, 9, 10, 11});
	if (otherNpc.navigationRetryCooldownTicks ==
		retryingNpc.navigationRetryCooldownTicks)
		return fail("Stable NPC IDs produced the same retry sequence");
	const int firstCooldown =
		retryingNpc.navigationRetryCooldownTicks.front();
	retryingNpc.walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f);
	retryingNpc.requestedWalkTarget = retryingNpc.walkTarget;
	retryingNpc.pathWaypoints = {retryingNpc.walkTarget};
	const std::vector<FRG::CharacterMovementObstacle> movingBlocker{
		{"b", Eigen::Vector3f(0.8f, 0.f, 0.f), 0.45f, true,
		 Eigen::Vector3f(-1.f, 0.f, 0.f)}};
	FRG::CharacterUpdateContext retryContext;
	retryContext.characterId = "retrying_npc";
	retryContext.movementObstacles = &movingBlocker;
	FRG::CharacterUpdateEvents retryEvents;
	for (int i = 0; i < 3; ++i)
		retryEvents = retryingNpc.update(100, retryContext);
	if (retryEvents.forceNavigationReplan ||
		!retryingNpc.pathWaypoints.empty() ||
		retryingNpc.navigationRetryTicks != firstCooldown ||
		retryingNpc.requestedWalkTarget != Eigen::Vector3f(2.f, 0.f, 0.f))
		return fail("Blocked NPC did not stop with a deterministic cooldown");
	for (int i = 0; i < firstCooldown; ++i)
		retryEvents = retryingNpc.update(100, retryContext);
	if (!retryEvents.forceNavigationReplan ||
		retryEvents.blockingCharacterId != "b")
		return fail("Blocked NPC did not retry after its cooldown");

	retryingNpc.movedLastTick = true;
	nlohmann::json blockedState;
	retryingNpc.save(blockedState);
	FRG::CharacterState restoredMover;
	restoredMover.load(blockedState);
	if (restoredMover.movementBlockedTicks !=
			retryingNpc.movementBlockedTicks ||
		restoredMover.navigationRetryTicks !=
			retryingNpc.navigationRetryTicks ||
		restoredMover.navigationRetryCooldownTicks !=
			retryingNpc.navigationRetryCooldownTicks ||
		restoredMover.navigationRetryCooldownIndex !=
			retryingNpc.navigationRetryCooldownIndex ||
		!restoredMover.movedLastTick)
		return fail("Blocked movement state did not round trip");

	FRG::LocationGameState occupancyState;
	auto stationary = std::make_shared<FRG::CharacterState>();
	stationary->enabled = true;
	stationary->hp = 10.f;
	auto moving = std::make_shared<FRG::CharacterState>();
	moving->enabled = true;
	moving->hp = 10.f;
	moving->pathWaypoints = {Eigen::Vector3f(4.f, 0.f, 0.f)};
	moving->movedLastTick = true;
	FRG::PlayerStateMap occupancyPlayers{
		{"stationary", stationary}, {"moving", moving}};
	if (occupancyState.collectPathObstacles(
			"requester", occupancyPlayers).size() != 1 ||
		occupancyState.collectPathObstacles(
			"requester", occupancyPlayers, "moving").size() != 2 ||
		!occupancyState.collectPathObstacles(
			"stationary", occupancyPlayers).empty())
		return fail("Stationary occupancy policy is incorrect");
	moving->movedLastTick = false;
	if (occupancyState.collectPathObstacles(
			"requester", occupancyPlayers).size() != 2)
		return fail("Physically stalled character was omitted from planning");

	FRG::LocationGameState approachingState;
	auto left = std::make_shared<FRG::CharacterState>();
	left->configureAsPlayer();
	left->enabled = true;
	left->position = Eigen::Vector3f(0.f, 0.f, 0.f);
	left->walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f);
	left->requestedWalkTarget = left->walkTarget;
	left->pathWaypoints = {left->walkTarget};
	auto right = std::make_shared<FRG::CharacterState>();
	right->configureAsPlayer();
	right->enabled = true;
	right->position = Eigen::Vector3f(1.4f, 0.f, 0.f);
	right->walkTarget = Eigen::Vector3f(-1.f, 0.f, 0.f);
	right->requestedWalkTarget = right->walkTarget;
	right->pathWaypoints = {right->walkTarget};
	FRG::PlayerStateMap approachingPlayers{{"a", left}, {"b", right}};
	for (int i = 0; i < 5; ++i)
		approachingState.update(100, approachingPlayers, false);
	const float separation = (right->position - left->position).norm();
	if (separation + 1e-5f <
			left->movementContactRadius + right->movementContactRadius ||
		!left->pathWaypoints.empty() || !right->pathWaypoints.empty())
		return fail("Colliding players did not stop without overlap");

	if (benchmark) {
		runBenchmark(pathFinder, 100);
		runBenchmark(pathFinder, 1000);
	}
	std::filesystem::remove(openGrid);
	std::filesystem::remove(corridorGrid);
	return EXIT_SUCCESS;
}