#include "Vehicles/CarrierHullShape.h"
#include "Vehicles/CarrierLayout.h"
#include "Core/Tuning/DescentTune_Carrier.h"

namespace Descent::Carrier
{
	namespace HullTune = Descent::Tune::Carrier;

	bool InPoolShaft(const FVector& P)
	{
		return P.X >= HullTune::PoolX0 && P.X <= HullTune::PoolX1 && P.Y >= HullTune::PoolY0 && P.Y <= HullTune::PoolY1
			&& P.Z < HullTune::PoolWaterZ && P.Z > -HullTune::HullRadiusM - 1.0;
	}

	double HullDistance(const FVector& P, bool bOpenPool)
	{
		// The shaft: free water from the belly up to the deck (the shaft walls are real collision, the swimmer's sweep meets them).
		if (bOpenPool && P.X >= HullTune::PoolX0 && P.X <= HullTune::PoolX1 && P.Y >= HullTune::PoolY0 && P.Y <= HullTune::PoolY1 && P.Z < HullTune::DeckFloorZ1 + 2.0)
		{
			return 1e3;
		}
		const double Xc = FMath::Clamp(P.X, -HullTune::HullHalfLengthM, HullTune::HullHalfLengthM);
		const double Radial = FMath::Sqrt(P.Y * P.Y + P.Z * P.Z) - HullRadius(Xc);
		const double Axial = FMath::Abs(P.X) - HullTune::HullHalfLengthM;
		if (Axial > 0.0 && Radial > 0.0)
		{
			return FMath::Sqrt(Axial * Axial + Radial * Radial);
		}
		return FMath::Max(Radial, Axial);
	}

	bool HullPushOut(FVector& P, double RadiusM, bool bOpenPool, FVector* OutNormal)
	{
		const double D = HullDistance(P, bOpenPool);
		if (D >= RadiusM)
		{
			return false;
		}
		// Outward normal by central differences of the distance (the pool opening is left out so the normal never points into the shaft).
		const double H = 0.25;
		FVector N(
			HullDistance(P + FVector(H, 0, 0), false) - HullDistance(P - FVector(H, 0, 0), false),
			HullDistance(P + FVector(0, H, 0), false) - HullDistance(P - FVector(0, H, 0), false),
			HullDistance(P + FVector(0, 0, H), false) - HullDistance(P - FVector(0, 0, H), false));
		if (!N.Normalize())
		{
			N = FVector(0.0, 0.0, -1.0);
		}
		P += N * (RadiusM - D);
		if (OutNormal)
		{
			*OutNormal = N;
		}
		return true;
	}
}