// Headless tests of milestone M6 (DESIGN_UE.md section 10): the helm's dynamics (U9, U4) and the three carrier routes under the autopilot.

#include "Tests/DescentTestCommon.h"
#include "Vehicles/CarrierHelm.h"
#include "World/DescentRoutes.h"
#include "World/DescentTerrainSubsystem.h"

#if WITH_DEV_AUTOMATION_TESTS

IMPLEMENT_SIMPLE_AUTOMATION_TEST(FDescentHelmDynamicsTest, "Descent.Helm.Dynamics", DESCENT_TEST_FLAGS)
bool FDescentHelmDynamicsTest::RunTest(const FString& Parameters)
{
	// U9: 2.0 m/s top speed, reached within seconds. Open water (no terrain).
	FCarrierHelmState H;
	H.Throttle = 1.0;
	double At4 = 0.0, Max = 0.0;
	for (int32 I = 1; I <= 300; ++I)
	{
		H.Step(1.0 / 30.0, nullptr);
		Max = FMath::Max(Max, H.VelocityMps().Size());
		if (I == 120) { At4 = H.VelocityMps().Size(); }
	}
	AddInfo(FString::Printf(TEXT("throttle 1 from rest: %.3f m/s at 4 s, %.3f at 10 s, max %.3f"), At4, H.VelocityMps().Size(), Max));
	TestTrue(TEXT("at least 1.80 m/s at 4.0 s"), At4 >= 1.80);
	TestTrue(TEXT("1.95 - 2.05 m/s at 10 s"), H.VelocityMps().Size() >= 1.95 && H.VelocityMps().Size() <= 2.05);
	TestTrue(TEXT("never above 2.05"), Max <= 2.05);
	H.AllStop();
	for (int32 I = 0; I < 120; ++I) { H.Step(1.0 / 30.0, nullptr); }
	TestTrue(TEXT("all stop: at most 0.2 m/s within 4 s"), H.VelocityMps().Size() <= 0.2);
	// U4: the aim is held within 40 degrees of the heading and 10 degrees of level; the hull follows at 0.10 rad/s at most.
	FCarrierHelmState A;
	A.AddAim(90.0, -30.0);
	TestEqual(TEXT("aim yaw clamps to +40"), A.AimYawDeg, 40.0, 1e-9);
	TestEqual(TEXT("aim pitch clamps to -10"), A.AimPitchDeg, -10.0, 1e-9);
	A.AimYawDeg = 30.0;
	A.AimPitchDeg = 0.0;
	double MaxRate = 0.0, YawAt3 = 0.0, Reached = -1.0;
	for (int32 I = 1; I <= 600; ++I)
	{
		A.Step(1.0 / 30.0, nullptr);
		MaxRate = FMath::Max(MaxRate, FMath::Abs(FMath::RadiansToDegrees(A.YawRate)));
		if (I == 90) { YawAt3 = A.YawDeg; }
		if (Reached < 0.0 && FMath::Abs(A.YawDeg - 30.0) <= 1.0) { Reached = I / 30.0; }
	}
	AddInfo(FString::Printf(TEXT("aim +30: yaw %.2f after 3 s, within 1 degree after %.1f s, largest yaw rate %.3f deg/s"), YawAt3, Reached, MaxRate));
	TestTrue(TEXT("heavy: 5 - 20 degrees after 3 s"), YawAt3 >= 5.0 && YawAt3 <= 20.0);
	TestTrue(TEXT("30 +- 1 degrees within 14 s"), Reached > 0.0 && Reached <= 14.0);
	TestTrue(TEXT("yaw rate never above 5.8 deg/s"), MaxRate <= 5.8);
	return true;
}

namespace
{
	/** Flies a route with the autopilot in a throw-away world. Returns the seconds of sim it took. */
	double HelmTestFly(UDescentTerrainSubsystem* Terrain, int32 Route, FCarrierHelmState& H, FCarrierAutopilot& Auto, double MaxSeconds, int32* OutContactStepsWhileHeld = nullptr)
	{
		const TArray<Descent::World::FRouteWaypoint>& Way = Descent::World::FDescentRoutes::Waypoints(Route + 1);   // the routes API counts from 1
		H = FCarrierHelmState();
		H.PosM = Way.Num() ? Way[0].P : FVector(0.0, 0.0, -30.0);
		H.YawDeg = H.AimYawDeg = (Way.Num() && Way[0].bHasYaw) ? Way[0].YawDeg : 0.0;
		Auto.Start(Route);
		double T = 0.0;
		int32 MaxContacts = 0;
		while (T < MaxSeconds && !Auto.bDone && !Auto.bStalled)
		{
			Auto.Step(H, 0.05);
			H.Step(0.05, Terrain);
			if (H.Contacts > 0 && Auto.StallTimer > 0.0) { ++MaxContacts; }
			T += 0.05;
		}
		if (OutContactStepsWhileHeld) { *OutContactStepsWhileHeld = MaxContacts; }
		return T;
	}
}

IMPLEMENT_SIMPLE_AUTOMATION_TEST(FDescentRoutesBlockedTest, "Descent.Routes.BlockedBeforeDig", DESCENT_TEST_FLAGS)
bool FDescentRoutesBlockedTest::RunTest(const FString& Parameters)
{
	// On each route the autopilot stalls at the first choke nobody has dug.
	for (int32 Route = 0; Route < Descent::World::FDescentRoutes::NumRoutes; ++Route)
	{
		FDescentTestWorld TW;
		UDescentTerrainSubsystem* Terrain = TW.Sub<UDescentTerrainSubsystem>();
		if (!TestNotNull(TEXT("terrain subsystem"), Terrain)) { return false; }
		FCarrierHelmState H;
		FCarrierAutopilot Auto;
		int32 Held = 0;
		const double T = HelmTestFly(Terrain, Route, H, Auto, 2400.0, &Held);
		AddInfo(FString::Printf(TEXT("route %d undug: %.0f s, waypoint %d, stalled %d, done %d, steps in contact while held %d, hull %.1f, at (%.0f, %.0f, %.0f)"), Route + 1, T, Auto.Index, Auto.bStalled, Auto.bDone, Held, H.Hull, H.PosM.X, H.PosM.Y, H.PosM.Z));
		TestTrue(*FString::Printf(TEXT("route %d: the autopilot stalls before the goal"), Route + 1), Auto.bStalled && !Auto.bDone);
		TestTrue(*FString::Printf(TEXT("route %d: the hull is against rock while it is held (contacts > 0)"), Route + 1), Held > 0);
	}
	return true;
}

#define DESCENT_ROUTE_GOAL_TEST(ClassName, TestName, RouteIndex) \
	IMPLEMENT_SIMPLE_AUTOMATION_TEST(ClassName, TestName, DESCENT_TEST_FLAGS) \
	bool ClassName::RunTest(const FString& Parameters) \
	{ \
		FDescentTestWorld TW; \
		UDescentTerrainSubsystem* Terrain = TW.Sub<UDescentTerrainSubsystem>(); \
		if (!TestNotNull(TEXT("terrain subsystem"), Terrain)) { return false; } \
		Descent::World::FDescentRoutes::ClearRoute(*Terrain, RouteIndex + 1); \
		FCarrierHelmState H; \
		FCarrierAutopilot Auto; \
		const double T = HelmTestFly(Terrain, RouteIndex, H, Auto, 6000.0); \
		AddInfo(FString::Printf(TEXT("route %d dug: %.0f s, waypoint %d of %d, done %d, stalled %d, hull %.1f, at (%.0f, %.0f, %.0f)"), RouteIndex + 1, T, Auto.Index, \
			Descent::World::FDescentRoutes::Waypoints(RouteIndex + 1).Num(), Auto.bDone, Auto.bStalled, H.Hull, H.PosM.X, H.PosM.Y, H.PosM.Z)); \
		if (!Auto.bDone) \
		{ \
			const TArray<Descent::World::FRouteWaypoint>& Way = Descent::World::FDescentRoutes::Waypoints(RouteIndex + 1); \
			for (int32 I = FMath::Max(0, Auto.Index - 2); I < FMath::Min(Way.Num(), Auto.Index + 2); ++I) \
			{ \
				AddInfo(FString::Printf(TEXT("waypoint %d at (%.0f, %.0f, %.0f) speed %.2f, density there %.2f, %.1f m from the hull"), I, Way[I].P.X, Way[I].P.Y, Way[I].P.Z, Way[I].Speed, Terrain->Density(Way[I].P), FVector::Dist(Way[I].P, H.PosM))); \
			} \
			AddInfo(FString::Printf(TEXT("hull: yaw %.1f, pitch %.1f, contacts %d, sum pen %.2f, v fwd %.2f, throttle %.2f, ballast %.2f, aim yaw %.1f"), H.YawDeg, H.PitchDeg, H.Contacts, H.SumPen, H.VFwd, H.Throttle, H.Ballast, H.AimYawDeg)); \
		} \
		TestTrue(TEXT("the autopilot reaches the goal"), Auto.bDone); \
		TestTrue(TEXT("with more than 50 % of the hull"), H.Hull > 50.0); \
		return true; \
	}

DESCENT_ROUTE_GOAL_TEST(FDescentRoutesReachGoal1Test, "Descent.Routes.ReachGoal_1", 0)
DESCENT_ROUTE_GOAL_TEST(FDescentRoutesReachGoal2Test, "Descent.Routes.ReachGoal_2", 1)
DESCENT_ROUTE_GOAL_TEST(FDescentRoutesReachGoal3Test, "Descent.Routes.ReachGoal_3", 2)

#endif // WITH_DEV_AUTOMATION_TESTS