#include "Vehicles/CarrierHelm.h"
#include "Core/DescentMath.h"
#include "World/DescentRoutes.h"
#include "World/DescentTerrainSubsystem.h"

namespace HT = Descent::Tune::Helm;

FVector FCarrierHelmState::VelocityMps() const
{
	const FRotator Aim(PitchDeg, YawDeg, 0.0);
	return Aim.Vector() * VFwd + FRotator(0.0, YawDeg, 0.0).RotateVector(FVector(0, 1, 0)) * VLat + FVector(0.0, 0.0, VBal);
}

bool FCarrierHelmState::IsAtRest() const
{
	return FMath::Abs(VFwd) < 1e-3 && FMath::Abs(VLat) < 1e-3 && FMath::Abs(VBal) < 1e-3 && FMath::Abs(YawRate) < 1e-5 && FMath::Abs(PitchDeg) < 0.01
		&& FMath::Abs(Throttle) < 1e-4 && FMath::Abs(Ballast) < 1e-4 && FMath::Abs(Strafe) < 1e-4 && FMath::Abs(FRotator::NormalizeAxis(AimYawDeg - YawDeg)) < 0.01;
}

void FCarrierHelmState::AllStop()
{
	Throttle = 0.0;
	Ballast = 0.0;
	Strafe = 0.0;
	AimYawDeg = YawDeg;
	AimPitchDeg = 0.0;
}

void FCarrierHelmState::AddAim(double DeltaYawDeg, double DeltaPitchDeg)
{
	const double Off = FMath::Clamp(FRotator::NormalizeAxis(AimYawDeg + DeltaYawDeg - YawDeg), -HT::AimYawLimitDeg, HT::AimYawLimitDeg);
	AimYawDeg = YawDeg + Off;
	AimPitchDeg = FMath::Clamp(AimPitchDeg + DeltaPitchDeg, -HT::AimPitchLimitDeg, HT::AimPitchLimitDeg);
}

void FCarrierHelmState::Step(double Dt, UDescentTerrainSubsystem* Terrain)
{
	if (Dt <= 0.0) { return; }
	// power: at 0 the lever falls to 0
	Power = FMath::Clamp(Power + (HT::PowerRegen - HT::PowerDrainPerThrottle * FMath::Abs(Throttle)) * Dt, 0.0, 100.0);
	if (Power <= 0.0) { Throttle = 0.0; }
	double Thr = FMath::Clamp(Throttle, HT::ThrottleMin, HT::ThrottleMax);
	double Bal = FMath::Clamp(Ballast, -1.0, 1.0);
	if (IsCrippled()) { Thr = FMath::Clamp(Thr, -HT::CrippledThrottle, HT::CrippledThrottle); Bal = FMath::Clamp(Bal, -HT::CrippledBallast, HT::CrippledBallast); }
	// U9: 2.0 m/s, reached within seconds
	VFwd += (Thr * HT::TopSpeedMps - VFwd) / HT::SpeedTau * Dt;
	VLat += (FMath::Clamp(Strafe, -1.0, 1.0) * HT::LateralMps - VLat) / HT::LateralTau * Dt;
	// the Shear takes the ballast-up authority away near the top (section 2.11)
	const double UpAuthority = Bal > 0.0 ? 1.0 - Descent::Smooth(-25.0, -5.0, PosM.Z) : 1.0;
	VBal += (Bal * HT::BallastMps * UpAuthority - VBal) / HT::BallastTau * Dt;
	// U4: the hull turns toward the aim, heavily
	const double Err = FMath::DegreesToRadians(FRotator::NormalizeAxis(AimYawDeg - YawDeg));
	const double WTarget = FMath::Clamp(HT::YawGain * Err, -HT::YawRateMax, HT::YawRateMax);
	const double Alpha = HT::YawAccelBase + HT::YawAccelPerMps * FMath::Abs(VFwd);
	YawRate += FMath::Clamp((WTarget - YawRate) / HT::YawTau, -Alpha, Alpha) * Dt;
	YawDeg = FRotator::NormalizeAxis(YawDeg + FMath::RadiansToDegrees(YawRate) * Dt);
	const double PitchTarget = FMath::Abs(VFwd) > HT::PitchMinSpeed ? AimPitchDeg : 0.0;
	PitchDeg = FMath::Clamp(PitchDeg + (PitchTarget - PitchDeg) / HT::PitchTau * Dt, -HT::AimPitchLimitDeg, HT::AimPitchLimitDeg);

	FVector Vel = VelocityMps();
	PosM += Vel * Dt;   // no clamp (U7)

	// collision: the hull sample points against the density field
	Contacts = 0;
	SumPen = 0.0;
	if (Terrain)
	{
		const FRotator Rot(PitchDeg, YawDeg, 0.0);
		FVector Push = FVector::ZeroVector;
		double Torque = 0.0;
		for (const FVector& L : Descent::World::FDescentRoutes::HullPoints())
		{
			const FVector R = Rot.RotateVector(L);
			const FVector W = PosM + R;
			const double D = Terrain->Density(W);
			if (D > -HT::ContactBand)
			{
				const double Pen = D + HT::ContactBand;
				const FVector N = -Terrain->Gradient(W);
				Push += N * Pen;
				Torque += FVector::CrossProduct(R, N).Z * Pen;
				SumPen += Pen;
				++Contacts;
			}
		}
		if (Contacts > 0)
		{
			PosM += Push * (HT::PushScale / FMath::Max(1.0, HT::PushPerContact * Contacts));
			const FVector N = Push.GetSafeNormal();
			const double Vn = FVector::DotProduct(Vel, N);
			if (Vn < 0.0) { Vel -= N * (Vn * HT::NormalRemove); }
			Vel *= FMath::Exp(-HT::ContactDrag * Dt);
			YawRate += HT::YawTorque * Torque;
			// back into the hull's own axes
			const FVector Fwd = Rot.Vector(), Right = FRotator(0.0, YawDeg, 0.0).RotateVector(FVector(0, 1, 0));
			VFwd = FVector::DotProduct(Vel, Fwd);
			VLat = FVector::DotProduct(Vel, Right);
			VBal = Vel.Z - Fwd.Z * VFwd;
			Hull = FMath::Max(0.0, Hull - SumPen * (HT::DamageBase + HT::DamagePerMps * Vel.Size()) * Dt);
		}
	}
	Scrape = Descent::Damp(Scrape, Contacts > 0 ? FMath::Min(1.0, 0.3 * SumPen + 0.4 * Vel.Size()) : 0.0, 6.0, Dt);
	const double Over = Descent::DepthOfZ(PosM.Z) - HT::RatingM;
	if (Over > 0.0) { Hull = FMath::Max(0.0, Hull - (0.6 + Over / 25.0) * Dt); }
}

void FCarrierAutopilot::Start(int32 InRoute)
{
	bOn = true;
	Route = FMath::Clamp(InRoute, 0, Descent::World::FDescentRoutes::NumRoutes - 1);
	Index = 0;
	bStalled = bDone = false;
	StallTimer = 0.0;
	BestDist = 1e9;
}

void FCarrierAutopilot::Step(FCarrierHelmState& Helm, double Dt)
{
	if (!bOn || bDone) { return; }
	const TArray<Descent::World::FRouteWaypoint>& Way = Descent::World::FDescentRoutes::Waypoints(Route + 1);   // the routes API counts from 1
	if (Index >= Way.Num()) { bDone = true; bOn = false; Helm.AllStop(); return; }
	const FVector To = Way[Index].P - Helm.PosM;
	const double Dist = To.Size();
	if (Dist < HT::WaypointReachM)
	{
		++Index;
		BestDist = 1e9;
		StallTimer = 0.0;
		LineUpSec = 0.0;
		return;
	}
	// stalled: no progress toward the waypoint for 40 s (an undug choke holds the hull)
	if (Dist < BestDist - 1.0) { BestDist = Dist; StallTimer = 0.0; }
	else { StallTimer += Dt; }
	bStalled = StallTimer > HT::StallAfterSec;
	// the same commands a player gives: aim, lever, ballast
	const double Horizontal = FVector2D(To.X, To.Y).Size();
	// in a well (a move that is mostly vertical) the hull keeps the heading the well was dug for
	const bool bWell = Way[Index].bHasYaw && (Horizontal <= 3.0 || FMath::Abs(To.Z) > 3.0 * Horizontal);
	const double WantYaw = bWell ? Way[Index].YawDeg : (Horizontal > 3.0 ? FMath::RadiansToDegrees(FMath::Atan2(To.Y, To.X)) : Helm.YawDeg);
	Helm.AimYawDeg = Helm.YawDeg + FMath::Clamp(FRotator::NormalizeAxis(WantYaw - Helm.YawDeg), -HT::AimYawLimitDeg, HT::AimYawLimitDeg);
	const double HeadingErr = FMath::Abs(FRotator::NormalizeAxis(WantYaw - Helm.YawDeg));
	const double Climb = FMath::RadiansToDegrees(FMath::Atan2(To.Z, FMath::Max(1.0, Horizontal)));
	Helm.AimPitchDeg = FMath::Clamp(Climb, -HT::AimPitchLimitDeg, HT::AimPitchLimitDeg);
	Helm.Throttle = Horizontal > 6.0 ? FMath::Clamp(Way[Index].Speed, 0.2, 1.0) * (HeadingErr > 25.0 ? 0.25 : 1.0) : 0.0;
	// a well is a vertical move: what the pitch cannot do, the ballast does
	Helm.Ballast = FMath::Clamp((To.Z - Helm.VFwd * FMath::Sin(FMath::DegreesToRadians(Helm.PitchDeg)) * 4.0) / 12.0, -1.0, 1.0);
	// the lateral thrusters take out what the heading leaves beside the line
	const FVector Right = FRotator(0.0, Helm.YawDeg, 0.0).RotateVector(FVector(0, 1, 0));
	Helm.Strafe = FMath::Clamp(FVector::DotProduct(To, Right) / 8.0, -1.0, 1.0);
	// A well (a leg straight down or up): it was dug for the hull on the well's axis with the heading of the leg before it
	// (FDescentRoutes::Poses). So: hold that heading, creep onto the axis, and only then use the ballast.
	const FVector Leg = Index > 0 ? Way[Index].P - Way[Index - 1].P : To;
	if (Index > 0 && FVector2D(Leg.X, Leg.Y).Size() <= 3.0 && FMath::Abs(Leg.Z) > 10.0)
	{
		double LegYaw = Helm.YawDeg;
		for (int32 J = Index - 1; J > 0; --J)
		{
			const FVector L2 = Way[J].P - Way[J - 1].P;
			if (Way[J].bHasYaw) { LegYaw = Way[J].YawDeg; break; }
			if (FVector2D(L2.X, L2.Y).Size() > 3.0) { LegYaw = FMath::RadiansToDegrees(FMath::Atan2(L2.Y, L2.X)); break; }
		}
		const double WellYaw = Way[Index].bHasYaw ? Way[Index].YawDeg : LegYaw;
		const double YawErr = FRotator::NormalizeAxis(WellYaw - Helm.YawDeg);
		Helm.AimYawDeg = Helm.YawDeg + FMath::Clamp(YawErr, -HT::AimYawLimitDeg, HT::AimYawLimitDeg);
		Helm.AimPitchDeg = 0.0;
		Helm.Throttle = FMath::Clamp(FVector::DotProduct(To, FRotator(0.0, Helm.YawDeg, 0.0).Vector()) / 12.0, -0.3, 0.3);
		const bool bLinedUp = Horizontal < 2.0 && FMath::Abs(YawErr) < 2.0;
		if (!bLinedUp && LineUpSec < 180.0)
		{
			LineUpSec += Dt;
			StallTimer = 0.0;   // lining up is progress
			Helm.Ballast = 0.0;
		}
	}
}