#include "World/DescentRoutes.h"
#include "World/DescentTerrainSubsystem.h"
#include "World/DescentWorldDef.h"
#include "Core/DescentMath.h"
#include "Core/Tuning/DescentTune_World.h"

namespace Descent::World
{
	namespace
	{
		FRouteWaypoint RoutePt(double X, double Y, double Z, double Speed, const TCHAR* Tag = TEXT(""))
		{
			FRouteWaypoint W;
			W.P = FVector(X, Y, Z); W.Speed = Speed; W.Tag = Tag;
			return W;
		}

		/** A waypoint with a heading; bHold = the carrier arrives at that heading and stops (v2 `hold`). */
		FRouteWaypoint RouteYaw(double X, double Y, double Z, double Speed, double YawDeg, bool bHold, const TCHAR* Tag = TEXT(""))
		{
			FRouteWaypoint W = RoutePt(X, Y, Z, Speed, Tag);
			W.bHasYaw = true; W.YawDeg = YawDeg; W.bHold = bHold;
			return W;
		}

		void RouteGalleryPoints(TArray<FRouteWaypoint>& Out, FName Tube, double Step, double Speed)
		{
			const FDescentWorldDef& Def = FDescentWorldDef::Get();
			const int32 T = Def.FindTube(Tube);
			for (double S = 0.0; S <= Def.Tubes[T].Len + 0.01; S += Step)
			{
				const FTubeSample Q = Def.TubeAtS(T, S);
				Out.Add(RoutePt(Q.P.X, Q.P.Y, Q.P.Z, Speed, *FString::Printf(TEXT("%s@%d"), *Tube.ToString(), FMath::RoundToInt(S))));
			}
		}

		/** The common tail T (section 2.10). */
		void RouteTail(TArray<FRouteWaypoint>& Out, bool bFromWC)
		{
			if (bFromWC)
			{
				Out.Add(RouteYaw(820, -330, -740, 0.9, -79.0, true, TEXT("cross")));
				Out.Add(RoutePt(820, -330, -990, 0.0, TEXT("Drop")));
				Out.Add(RouteYaw(820, -330, -990, 0.0, -12.0, true, TEXT("turn")));
			}
			Out.Add(RouteYaw(1060, -380, -1040, 0.9, -12.0, true, TEXT("Gullet")));
			Out.Add(RoutePt(1060, -380, -1310, 0.0, TEXT("GulletFoot")));
			Out.Add(RoutePt(1200, -320, -1325, 0.5, TEXT("GOAL")));
		}

		TArray<FRouteWaypoint> RouteBuild(int32 Route)
		{
			TArray<FRouteWaypoint> W;
			if (Route == 1 || Route == 3)
			{
				W.Add(RoutePt(0, 0, -30, 1.6, TEXT("start")));
				W.Add(RoutePt(600, -20, -60, 1.6, TEXT("canyon")));
				W.Add(RouteYaw(760, -25, -200, 0.9, 0.0, true, TEXT("WellA")));
				W.Add(RoutePt(760, -25, -640, 0.0, TEXT("Void")));
				if (Route == 1)
				{
					W.Add(RouteYaw(760, -25, -640, 0.0, -79.0, true, TEXT("turn")));
					RouteTail(W, true);
				}
				else
				{
					W.Add(RouteYaw(760, -25, -640, 0.0, -56.0, true, TEXT("turn")));
					W.Add(RouteYaw(900, -230, -765, 0.9, 0.0, true, TEXT("HairpinMouth")));
					RouteGalleryPoints(W, TEXT("S"), 30.0, 1.1);
					// The gallery ends under the Undercroft's roof: a 122 m hull cannot turn there. Carry on along the
					// heading into the chamber (DESIGN 2.10 turns at the gallery's last point) and turn in its middle.
					W.Add(RoutePt(900, -526, -995, 0.6, TEXT("UndercroftMouth")));
					W.Add(RouteYaw(900, -526, -995, 0.0, -12.0, true, TEXT("turn")));
					RouteTail(W, false);
				}
			}
			else
			{
				// DESIGN 2.10 starts at Z -105, but the canyon floor there is -116..-127 and the hull's keel and stern fins
				// reach 15 m under the pose: start 13 m higher, and turn on the canyon centreline (Y -8) where a 122 m hull fits, not at Y -30.
				W.Add(RoutePt(-20, -8, -92, 1.2, TEXT("canyon")));
				W.Add(RouteYaw(-20, -8, -92, 0.0, -75.0, true, TEXT("turn")));
				RouteGalleryPoints(W, TEXT("B"), 30.0, 1.1);
				W.Add(RouteYaw(430, -420, -200, 0.5, 0.0, true, TEXT("Honeycomb")));
				W.Add(RoutePt(430, -420, -680, 0.0, TEXT("BoreFoot")));
				W.Add(RouteYaw(430, -420, -680, 0.0, 13.0, true, TEXT("turn")));
				RouteTail(W, true);
			}
			return W;
		}
	}

	const TArray<FRouteWaypoint>& FDescentRoutes::Waypoints(int32 Route)
	{
		static TArray<FRouteWaypoint> Cache[NumRoutes];
		static bool bBuilt[NumRoutes] = { false, false, false };
		const int32 I = FMath::Clamp(Route, 1, NumRoutes) - 1;
		if (!bBuilt[I])
		{
			Cache[I] = RouteBuild(I + 1);
			bBuilt[I] = true;
		}
		return Cache[I];
	}

	FString FDescentRoutes::RouteName(int32 Route)
	{
		return Route == 1 ? TEXT("THROAT ROUTE") : (Route == 2 ? TEXT("LONG GALLERY ROUTE") : TEXT("HAIRPIN ROUTE"));
	}

	TArray<FRouteChoke> FDescentRoutes::Chokes(int32 Route)
	{
		const FDescentWorldDef& Def = FDescentWorldDef::Get();
		auto Pinch = [&](const TCHAR* Id)
		{
			for (const FPinch& P : Def.Pinches) { if (P.Id == Id) { return FRouteChoke{ P.Id, P.C, 50.0 }; } }
			return FRouteChoke();
		};
		TArray<FRouteChoke> Out;
		auto Shelf = [&](const TCHAR* Id)
		{
			for (const FShelf& S : Def.Shelves) { if (S.Id == Id) { return FRouteChoke{ S.Id, FVector(S.X, S.Y, S.Zc), 75.0 }; } }
			return FRouteChoke();
		};
		auto Ring = [&](const TCHAR* Id)
		{
			for (const FRing& R : Def.Rings) { if (R.Id == Id) { const FWell& W = Def.Wells[R.Well]; return FRouteChoke{ R.Id, FVector(W.X, W.Y, R.Zc), 55.0 }; } }
			return FRouteChoke();
		};
		if (Route == 1) { Out = { Shelf(TEXT("SA")), Shelf(TEXT("SC")), Ring(TEXT("DRING")) }; }
		else if (Route == 2) { Out = { Pinch(TEXT("NEEDLE")), Ring(TEXT("BRING")), Shelf(TEXT("SC")), Ring(TEXT("DRING")) }; }
		else { Out = { Shelf(TEXT("SA")), Pinch(TEXT("HTHROAT")), Ring(TEXT("DRING")) }; }
		return Out;
	}

	static double RouteWrapPi(double A)
	{
		while (A > UE_DOUBLE_PI) { A -= 2.0 * UE_DOUBLE_PI; }
		while (A < -UE_DOUBLE_PI) { A += 2.0 * UE_DOUBLE_PI; }
		return A;
	}

	TArray<FRoutePose> FDescentRoutes::Poses(int32 Route, double Step)
	{
		const TArray<FRouteWaypoint>& R = Waypoints(Route);
		TArray<FRoutePose> Out;
		double Yaw = 0.0;
		for (int32 I = 1; I < R.Num(); ++I)
		{
			const FRouteWaypoint &A = R[I - 1], &B = R[I];
			const double D = FVector::Dist(A.P, B.P);
			const double Hd = FMath::Sqrt(FMath::Square(B.P.X - A.P.X) + FMath::Square(B.P.Y - A.P.Y));
			const bool bYawDir = Hd > 3.0;
			const double YawDir = FMath::Atan2(B.P.Y - A.P.Y, B.P.X - A.P.X);
			const double Yb = B.bHasYaw ? FMath::DegreesToRadians(B.YawDeg) : (bYawDir ? YawDir : Yaw);
			if (D < 0.5)
			{
				const double Dy = RouteWrapPi(Yb - Yaw);
				const int32 M = FMath::Max(1, FMath::CeilToInt(FMath::Abs(Dy) / 0.087));
				for (int32 K = 1; K <= M; ++K) { Out.Add({ A.P, Yaw + Dy * K / M, 0.0 }); }
				Yaw = Yb;
				continue;
			}
			const int32 M = FMath::Max(1, FMath::CeilToInt(D / Step));
			const double Y0 = Yaw, Dy = RouteWrapPi(Yb - Y0);
			const double Pit = Hd > 3.0 ? FMath::Clamp(FMath::Atan2(B.P.Z - A.P.Z, Hd), -0.17, 0.17) : 0.0;
			for (int32 K = 1; K <= M; ++K)
			{
				const double T = static_cast<double>(K) / M;
				Out.Add({ FMath::Lerp(A.P, B.P, T), Y0 + Dy * FMath::Min(1.0, T * 2.2), Pit });
			}
			Yaw = Yb;
		}
		return Out;
	}

	double FDescentRoutes::HullRadius(double X)
	{
		if (X > 40.0) { const double T = (X - 40.0) / 20.0; return 12.0 * FMath::Sqrt(FMath::Max(0.0, 1.0 - T * T)); }
		if (X >= -40.0) { return 12.0; }
		return 12.0 - 5.0 * Descent::Smooth(40.0, 60.0, -X);
	}

	// v2 carrier-local (x lateral, y up, z long, bow at -z) -> (X = -z, Y = x, Z = y)
	const TArray<FVector>& FDescentRoutes::HullPoints()
	{
		static TArray<FVector> Pts;
		if (Pts.Num() == 0)
		{
			for (const double Zv : { -59.0, -54.0, -48.0, -40.0, -30.0, -18.0, -6.0, 6.0, 18.0, 30.0, 40.0, 50.0, 58.0 })
			{
				const double X = -Zv, R = HullRadius(X);
				const int32 N = FMath::Abs(Zv) > 50.0 ? 8 : 14;
				for (int32 I = 0; I < N; ++I)
				{
					const double A = static_cast<double>(I) / N * 2.0 * UE_DOUBLE_PI;
					Pts.Add(FVector(X, FMath::Cos(A) * R, FMath::Sin(A) * R));
				}
			}
			for (double Zv = -28.0; Zv <= 8.0; Zv += 6.0)
			{
				Pts.Add(FVector(-Zv, 0.0, 12.0 + 6.2));
				Pts.Add(FVector(-Zv, 2.6, 12.0 + 3.0));
				Pts.Add(FVector(-Zv, -2.6, 12.0 + 3.0));
			}
			for (const double S : { -1.0, 1.0 })
			{
				for (const double Zv : { 21.0, 30.0, 39.0 }) { Pts.Add(FVector(-Zv, S * 15.1, -3.5)); }
			}
			Pts.Add(FVector(30.0, 0.0, -12.9));
			Pts.Add(FVector(-10.0, 0.0, -12.9));
			Pts.Add(FVector(-55.0, 0.0, -15.0));
			Pts.Add(FVector(-55.0, 0.0, 16.0));
			Pts.Add(FVector(-55.0, 14.0, 0.0));
			Pts.Add(FVector(-55.0, -14.0, 0.0));
		}
		return Pts;
	}

	const TArray<FVector>& FDescentRoutes::SweepPoints()
	{
		static TArray<FVector> Pts;
		if (Pts.Num() == 0)
		{
			for (double Zv = -60.0; Zv <= 60.0; Zv += 6.0)
			{
				const double X = -Zv, R = HullRadius(X) + 1.6;
				const int32 N = R < 3.0 ? 1 : 12;
				for (int32 I = 0; I < N; ++I)
				{
					const double A = static_cast<double>(I) / N * 2.0 * UE_DOUBLE_PI;
					Pts.Add(FVector(X, FMath::Cos(A) * R, FMath::Sin(A) * R));
				}
			}
			for (const double S : { -1.0, 1.0 })
			{
				for (const double Zv : { 21.0, 30.0, 40.0 })
				{
					Pts.Add(FVector(-Zv, S * 16.2, -3.5));
					Pts.Add(FVector(-Zv, S * 16.2, -1.5));
					Pts.Add(FVector(-Zv, S * 16.2, -5.5));
				}
			}
			for (double Zv = -28.0; Zv <= 8.0; Zv += 6.0)
			{
				Pts.Add(FVector(-Zv, 0.0, 12.0 + 7.5));
				Pts.Add(FVector(-Zv, 3.6, 12.0 + 4.0));
				Pts.Add(FVector(-Zv, -3.6, 12.0 + 4.0));
			}
			Pts.Add(FVector(30.0, 0.0, -14.5));
			Pts.Add(FVector(-10.0, 0.0, -14.5));
			Pts.Add(FVector(-55.0, 0.0, -16.5));
			Pts.Add(FVector(-55.0, 0.0, 17.5));
			Pts.Add(FVector(-55.0, 15.5, 0.0));
			Pts.Add(FVector(-55.0, -15.5, 0.0));
		}
		return Pts;
	}

	FVector FDescentRoutes::LocalToWorld(const FRoutePose& Pose, const FVector& Local)
	{
		const FQuat Q = FRotator(FMath::RadiansToDegrees(Pose.PitchRad), FMath::RadiansToDegrees(Pose.YawRad), 0.0).Quaternion();
		return Pose.P + Q.RotateVector(Local);
	}

	double FDescentRoutes::MinClearance(UDescentTerrainSubsystem& Terrain, TConstArrayView<FRoutePose> Poses, FVector* OutWhere)
	{
		double Best = 1e9;
		for (const FRoutePose& Pose : Poses)
		{
			for (const FVector& Lp : HullPoints())
			{
				const FVector W = LocalToWorld(Pose, Lp);
				const double C = -Terrain.Density(W);
				if (C < Best)
				{
					Best = C;
					if (OutWhere) { *OutWhere = W; }
				}
			}
		}
		return Best;
	}

	int32 FDescentRoutes::ClearRoute(UDescentTerrainSubsystem& Terrain, int32 Route)
	{
		TSet<FIntVector> Seen;
		int32 Calls = 0;
		for (const FRoutePose& Pose : Poses(Route, 5.0))
		{
			for (const FVector& Lp : SweepPoints())
			{
				const FVector W = LocalToWorld(Pose, Lp);
				if (Terrain.Density(W) > -(Descent::Tune::World::ClampD - 0.5))   // the stored field is clamped at -4: 'rock within a few metres'
				{
					const FIntVector Key(FMath::RoundToInt(W.X / 2.0), FMath::RoundToInt(W.Y / 2.0), FMath::RoundToInt(W.Z / 2.0));
					if (Seen.Contains(Key)) { continue; }
					Seen.Add(Key);
					Terrain.Carve(W, 6.5, 999.0, 1.0, true);
					++Calls;
				}
			}
		}
		return Calls;
	}
}