#include "Player/Arm/DescentArmJack.h"
#include "Player/Arm/DescentArmReach.h"
#include "Core/Tuning/DescentTune_Arm.h"

namespace
{
	namespace ArmTune = Descent::Tune::Arm;

	FORCEINLINE double JackSmooth(double X)
	{
		const double T = FMath::Clamp(X, 0.0, 1.0);
		return T * T * (3.0 - 2.0 * T);
	}

	FORCEINLINE double JackSpan(double T, double A, double B)
	{
		return B > A ? FMath::Clamp((T - A) / (B - A), 0.0, 1.0) : 1.0;
	}

	/** Solves with the hand as it is in Base at a fingertip target and direction. */
	FDescentArmReachResult JackSolveFingertip(const FVector& Target, const FVector& Dir, const FDescentArmPose& Base)
	{
		FDescentArmReachRequest Req;
		Req.TargetCm = Target;
		Req.HandDirection = Dir;
		Req.Effector = EDescentArmEffector::Fingertip;
		Req.PalmTwistDeg = ArmTune::JackPalmTwistDeg;
		return Descent::Arm::SolveReach(Req, Base);
	}

	void JackCopyFingers(const FDescentArmPose& From, FDescentArmPose& To)
	{
		for (int32 I = 0; I < 5; ++I)
		{
			To.FingerCurl[I] = From.FingerCurl[I];
		}
	}
}

namespace Descent::Arm
{
	const TCHAR* JackPhaseName(EDescentArmJackPhase Phase)
	{
		switch (Phase)
		{
		case EDescentArmJackPhase::Inactive: return TEXT("inactive");
		case EDescentArmJackPhase::Raise:    return TEXT("raise");
		case EDescentArmJackPhase::Reach:    return TEXT("reach");
		case EDescentArmJackPhase::Plug:     return TEXT("plug");
		case EDescentArmJackPhase::Feed:     return TEXT("feed");
		case EDescentArmJackPhase::Withdraw: return TEXT("withdraw");
		case EDescentArmJackPhase::Lower:    return TEXT("lower");
		case EDescentArmJackPhase::Done:     return TEXT("done");
		}
		return TEXT("?");
	}

	EDescentArmJackPhase JackPhaseAt(double T)
	{
		if (T < 0.0) { return EDescentArmJackPhase::Inactive; }
		if (T < ArmTune::JackRaiseEnd) { return EDescentArmJackPhase::Raise; }
		if (T < ArmTune::JackReachEnd) { return EDescentArmJackPhase::Reach; }
		if (T < ArmTune::JackPlugEnd) { return EDescentArmJackPhase::Plug; }
		if (T < ArmTune::JackFeedEnd) { return EDescentArmJackPhase::Feed; }
		if (T < ArmTune::JackWithdrawEnd) { return EDescentArmJackPhase::Withdraw; }
		if (T < ArmTune::JackTotal) { return EDescentArmJackPhase::Lower; }
		return EDescentArmJackPhase::Done;
	}

	FDescentArmJackFrame EvaluateJack(double T, const FDescentArmJackInput& In)
	{
		FDescentArmJackFrame F;
		F.Phase = JackPhaseAt(T);
		if (F.Phase == EDescentArmJackPhase::Done || F.Phase == EDescentArmJackPhase::Inactive)
		{
			F.Pose = F.Phase == EDescentArmJackPhase::Done ? In.End : In.Start;
			F.PhaseProgress = 1.0;
			return F;
		}

		const FVector PlugDir = In.PlugDirection.GetSafeNormal(UE_SMALL_NUMBER, FVector::ForwardVector);
		const FVector ReadyDir = FVector(FMath::Cos(FMath::DegreesToRadians(4.0)), 0.0, FMath::Sin(FMath::DegreesToRadians(4.0)));
		// The point on the port's axis the hand lines up at before it goes in, and comes back to on the way out.
		const FVector Standoff = In.PortCm - PlugDir * ArmTune::JackStandoffCm;

		// The fingers: from whatever the hand was doing to the plug shape by the time it is lined up, and to the end pose's
		// shape while the arm is lowered. Shape is the base of every solve below (only its fingers matter).
		const double LowerA = JackSpan(T, ArmTune::JackWithdrawEnd, ArmTune::JackTotal);
		FDescentArmPose Shape = FDescentArmPose::Lerp(In.Start, NamedPose(EDescentArmPose::Plug), JackSmooth(JackSpan(T, 0.0, ArmTune::JackReachEnd)));
		Shape.ShiftCm = FVector::ZeroVector;
		for (int32 I = 0; I < 5; ++I)
		{
			Shape.FingerCurl[I] = FMath::Lerp(Shape.FingerCurl[I], In.End.FingerCurl[I], JackSmooth(LowerA));
		}

		FDescentArmReachResult Solved;
		switch (F.Phase)
		{
		case EDescentArmJackPhase::Raise:
		{
			// Joint space: a hanging arm swings up to the ready position. Ease in only, so the reach carries the movement on.
			F.PhaseProgress = JackSpan(T, 0.0, ArmTune::JackRaiseEnd);
			const FDescentArmReachResult Ready = JackSolveFingertip(ArmTune::JackReadyHandCm, ReadyDir, Shape);
			F.Pose = FDescentArmPose::Lerp(In.Start, Ready.Pose, F.PhaseProgress * F.PhaseProgress);
			JackCopyFingers(Shape, F.Pose);
			return F;
		}
		case EDescentArmJackPhase::Reach:
		{
			// Fingertips on a straight line from the ready position to the standoff, turning into the port's direction. Ease out.
			F.PhaseProgress = JackSpan(T, ArmTune::JackRaiseEnd, ArmTune::JackReachEnd);
			const double S = 1.0 - FMath::Square(1.0 - F.PhaseProgress);
			const FVector Dir = FQuat::Slerp(FQuat::FindBetweenNormals(FVector::ForwardVector, ReadyDir), FQuat::FindBetweenNormals(FVector::ForwardVector, PlugDir), S).RotateVector(FVector::ForwardVector);
			Solved = JackSolveFingertip(FMath::Lerp(static_cast<FVector>(ArmTune::JackReadyHandCm), Standoff, S), Dir, Shape);
			break;
		}
		case EDescentArmJackPhase::Plug:
		{
			F.PhaseProgress = JackSpan(T, ArmTune::JackReachEnd, ArmTune::JackPlugEnd);
			Solved = JackSolveFingertip(FMath::Lerp(Standoff, In.PortCm, JackSmooth(F.PhaseProgress)), PlugDir, Shape);
			break;
		}
		case EDescentArmJackPhase::Feed:
		{
			F.PhaseProgress = JackSpan(T, ArmTune::JackPlugEnd, ArmTune::JackFeedEnd);
			F.bFeeding = true;
			Solved = JackSolveFingertip(In.PortCm, PlugDir, Shape);
			break;
		}
		case EDescentArmJackPhase::Withdraw:
		{
			F.PhaseProgress = JackSpan(T, ArmTune::JackFeedEnd, ArmTune::JackWithdrawEnd);
			Solved = JackSolveFingertip(FMath::Lerp(In.PortCm, Standoff, JackSmooth(F.PhaseProgress)), PlugDir, Shape);
			break;
		}
		case EDescentArmJackPhase::Lower:
		default:
		{
			// Joint space again: from the standoff down to the end pose.
			F.PhaseProgress = LowerA;
			FDescentArmPose PlugShape = Shape;
			JackCopyFingers(NamedPose(EDescentArmPose::Plug), PlugShape);   // the standoff pose must not move with the fingers reopening
			const FDescentArmPose Back = JackSolveFingertip(Standoff, PlugDir, PlugShape).Pose;
			F.Pose = FDescentArmPose::Lerp(Back, In.End, JackSmooth(F.PhaseProgress));
			JackCopyFingers(Shape, F.Pose);
			return F;
		}
		}
		F.Pose = Solved.Pose;
		F.ReachErrorCm = Solved.ErrorCm;
		F.bReachable = Solved.bReachable;
		F.HandDirErrorDeg = Solved.HandDirErrorDeg;
		return F;
	}
}