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

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

	const TCHAR* const GArmPoseNames[] = { TEXT("hidden"), TEXT("idle"), TEXT("reach"), TEXT("grip"), TEXT("point"), TEXT("plug"), TEXT("hold_tool") };
	static_assert(UE_ARRAY_COUNT(GArmPoseNames) == static_cast<int32>(EDescentArmPose::Count), "pose names out of step with EDescentArmPose");

	FORCEINLINE double ArmRad(double Deg) { return FMath::DegreesToRadians(Deg); }

	FDescentArmPose BuildFromRecipe(const ArmTune::FPoseRecipe& R)
	{
		FDescentArmPose Base;
		for (int32 F = 0; F < 5; ++F)
		{
			Base.FingerCurl[F] = R.Curl[F];
		}
		return Descent::Arm::PoseFromHandAndElbow(FVector(R.HandX, R.HandY, R.HandZ), R.ElbowOutDeg, R.WristRollDeg, R.WristFlexDeg, Base);
	}
}

// ---------------------------------------------------------------------------------------------------------
// FDescentArmPose
// ---------------------------------------------------------------------------------------------------------

FDescentArmPose FDescentArmPose::Lerp(const FDescentArmPose& A, const FDescentArmPose& B, double Alpha)
{
	// Yaw and roll of the shoulder are full circles (the roll has no limit): go the short way round, and at the ends return
	// exactly A or B.
	auto Turn = [Alpha](double From, double To)
	{
		return Alpha <= 0.0 ? From : Alpha >= 1.0 ? To : FMath::UnwindDegrees(From + Alpha * FMath::UnwindDegrees(To - From));
	};
	FDescentArmPose R;
	R.ShoulderYawDeg = Turn(A.ShoulderYawDeg, B.ShoulderYawDeg);
	R.ShoulderPitchDeg = FMath::Lerp(A.ShoulderPitchDeg, B.ShoulderPitchDeg, Alpha);
	R.ShoulderRollDeg = Turn(A.ShoulderRollDeg, B.ShoulderRollDeg);
	R.ElbowFlexDeg = FMath::Lerp(A.ElbowFlexDeg, B.ElbowFlexDeg, Alpha);
	R.WristRollDeg = FMath::Lerp(A.WristRollDeg, B.WristRollDeg, Alpha);
	R.WristFlexDeg = FMath::Lerp(A.WristFlexDeg, B.WristFlexDeg, Alpha);
	for (int32 F = 0; F < 5; ++F)
	{
		R.FingerCurl[F] = FMath::Lerp(A.FingerCurl[F], B.FingerCurl[F], Alpha);
	}
	R.ShiftCm = FMath::Lerp(A.ShiftCm, B.ShiftCm, Alpha);
	return R;
}

double FDescentArmPose::MaxDifference(const FDescentArmPose& O) const
{
	// Yaw and roll are circles: 179 and -179 are 2 degrees apart.
	double M = FMath::Max3(FMath::Abs(FMath::UnwindDegrees(ShoulderYawDeg - O.ShoulderYawDeg)), FMath::Abs(ShoulderPitchDeg - O.ShoulderPitchDeg), FMath::Abs(FMath::UnwindDegrees(ShoulderRollDeg - O.ShoulderRollDeg)));
	M = FMath::Max3(M, FMath::Abs(ElbowFlexDeg - O.ElbowFlexDeg), FMath::Abs(WristRollDeg - O.WristRollDeg));
	M = FMath::Max(M, FMath::Abs(WristFlexDeg - O.WristFlexDeg));
	for (int32 F = 0; F < 5; ++F)
	{
		M = FMath::Max(M, 100.0 * FMath::Abs(FingerCurl[F] - O.FingerCurl[F]));
	}
	return FMath::Max(M, (ShiftCm - O.ShiftCm).GetAbsMax());
}

bool FDescentArmPose::Equals(const FDescentArmPose& O, double Tolerance) const
{
	return MaxDifference(O) <= Tolerance;
}

// ---------------------------------------------------------------------------------------------------------
// Joint maths and forward kinematics
// ---------------------------------------------------------------------------------------------------------

namespace Descent::Arm
{
	const TCHAR* PoseName(EDescentArmPose Pose)
	{
		const int32 I = static_cast<int32>(Pose);
		return I >= 0 && I < UE_ARRAY_COUNT(GArmPoseNames) ? GArmPoseNames[I] : TEXT("?");
	}

	bool ParsePose(const FString& Name, EDescentArmPose& Out)
	{
		const FString Key = Name.ToLower().Replace(TEXT("_"), TEXT("")).Replace(TEXT("-"), TEXT(""));
		for (int32 I = 0; I < UE_ARRAY_COUNT(GArmPoseNames); ++I)
		{
			if (Key == FString(GArmPoseNames[I]).Replace(TEXT("_"), TEXT("")))
			{
				Out = static_cast<EDescentArmPose>(I);
				return true;
			}
		}
		return false;
	}

	const FDescentArmPose& NamedPose(EDescentArmPose Pose)
	{
		static const TStaticArray<FDescentArmPose, static_cast<int32>(EDescentArmPose::Count)> Poses = []()
		{
			TStaticArray<FDescentArmPose, static_cast<int32>(EDescentArmPose::Count)> P;
			FDescentArmPose Hidden;
			Hidden.ShoulderYawDeg = ArmTune::HiddenShoulderYawDeg;
			Hidden.ShoulderPitchDeg = ArmTune::HiddenShoulderPitchDeg;
			Hidden.ElbowFlexDeg = ArmTune::HiddenElbowFlexDeg;
			Hidden.ShiftCm = ArmTune::HiddenShiftCm;
			for (int32 F = 0; F < 5; ++F)
			{
				Hidden.FingerCurl[F] = ArmTune::HiddenCurl;
			}
			P[static_cast<int32>(EDescentArmPose::Hidden)] = Hidden;
			P[static_cast<int32>(EDescentArmPose::Idle)] = BuildFromRecipe(ArmTune::IdleRecipe);
			P[static_cast<int32>(EDescentArmPose::Reach)] = BuildFromRecipe(ArmTune::ReachRecipe);
			P[static_cast<int32>(EDescentArmPose::Grip)] = BuildFromRecipe(ArmTune::GripRecipe);
			P[static_cast<int32>(EDescentArmPose::Point)] = BuildFromRecipe(ArmTune::PointRecipe);
			P[static_cast<int32>(EDescentArmPose::Plug)] = BuildFromRecipe(ArmTune::PlugRecipe);
			P[static_cast<int32>(EDescentArmPose::HoldTool)] = BuildFromRecipe(ArmTune::HoldRecipe);
			return P;
		}();
		return Poses[FMath::Clamp(static_cast<int32>(Pose), 0, static_cast<int32>(EDescentArmPose::Count) - 1)];
	}

	FQuat ShoulderRotation(double YawDeg, double PitchDeg, double RollDeg)
	{
		// Yaw about up, then pitch about the (yawed) right axis with + = raise, then roll about the arm itself.
		// FQuat A * B applies B first: roll, then pitch, then yaw.
		const FQuat Yaw(FVector::UpVector, ArmRad(YawDeg));
		const FQuat Pitch(FVector(0.0, -1.0, 0.0), ArmRad(PitchDeg));
		const FQuat Roll(Def(EDescentArmPart::Wrist).JointAxis, ArmRad(RollDeg));
		return Yaw * Pitch * Roll;
	}

	FQuat ElbowRotation(double FlexDeg)
	{
		// The model's positive hinge rotation takes the forearm DOWN; a human elbow folds the other way (toward the back of
		// the hand when the palm faces down), so flexion is the negative of the model's angle.
		return FQuat(Def(EDescentArmPart::Forearm).JointAxis, -ArmRad(FlexDeg));
	}

	FQuat WristRollRotation(double RollDeg)
	{
		return FQuat(Def(EDescentArmPart::Wrist).JointAxis, ArmRad(RollDeg));
	}

	FQuat WristFlexRotation(double FlexDeg)
	{
		return FQuat(Def(EDescentArmPart::Hand).JointAxis, ArmRad(FlexDeg));
	}

	double FingerJointAbsoluteDeg(int32 Finger, int32 Joint, double Curl)
	{
		const double C = FMath::Clamp(Curl, 0.0, 1.0);
		if (Finger == 0)
		{
			return C * ArmTune::ThumbClosedDeg[Joint];
		}
		return C * ArmTune::FingerClosedDeg[Finger - 1][Joint];
	}

	double FingerJointDeltaDeg(int32 Finger, int32 Joint, double Curl)
	{
		// The model's rest rotation already contains RestCurlDeg of curl for the four fingers (a relaxed hand); the thumb is
		// measured from its modelled pose (RestCurlDeg is 0 there by construction).
		return FingerJointAbsoluteDeg(Finger, Joint, Curl) - Def(FingerPart(Finger, Joint)).RestCurlDeg;
	}

	double RodSlideCm(double ElbowFlexDeg)
	{
		const double T = FMath::Clamp((ElbowFlexDeg - ArmTune::ElbowMinFlexDeg) / (ArmTune::ElbowMaxFlexDeg - ArmTune::ElbowMinFlexDeg), 0.0, 1.0);
		return -ArmTune::RodStrokeCm * T;
	}

	void ComputeLocal(const FDescentArmPose& P, FDescentArmTransforms& Out)
	{
		Out.Reset();
		Out.SetNum(PartCount);
		for (int32 I = 0; I < PartCount; ++I)
		{
			const FDescentArmPartDef& D = Def(static_cast<EDescentArmPart>(I));
			FQuat Joint = FQuat::Identity;
			FVector Offset = D.LocalOffsetCm;
			switch (static_cast<EDescentArmPart>(I))
			{
			case EDescentArmPart::Root:      Offset += P.ShiftCm; break;
			case EDescentArmPart::UpperArm:  Joint = ShoulderRotation(P.ShoulderYawDeg, P.ShoulderPitchDeg, P.ShoulderRollDeg); break;
			case EDescentArmPart::Forearm:   Joint = ElbowRotation(P.ElbowFlexDeg); break;
			case EDescentArmPart::PistonRod: Offset += D.RestRotation.RotateVector(D.JointAxis * RodSlideCm(P.ElbowFlexDeg)); break;
			case EDescentArmPart::Wrist:     Joint = WristRollRotation(P.WristRollDeg); break;
			case EDescentArmPart::Hand:      Joint = WristFlexRotation(P.WristFlexDeg); break;
			default:
				if (D.Joint == EDescentArmJoint::Finger)
				{
					const int32 Finger = (I - static_cast<int32>(EDescentArmPart::Thumb1)) / 3;
					const int32 Phalanx = (I - static_cast<int32>(EDescentArmPart::Thumb1)) % 3;
					Joint = FQuat(D.JointAxis, ArmRad(FingerJointDeltaDeg(Finger, Phalanx, P.FingerCurl[Finger])));
				}
				break;
			}
			// Rest orientation first in the parent's frame, the joint turns the part about its own axis: R_rest * R_joint.
			// The table carries 7 decimals: keep the product a unit quaternion.
			Out[I] = FTransform((D.RestRotation * Joint).GetNormalized(), Offset);
		}
	}

	void ComputeWorld(const FDescentArmPose& P, FDescentArmTransforms& Out)
	{
		FDescentArmTransforms Local;
		ComputeLocal(P, Local);
		Out.Reset();
		Out.SetNum(PartCount);
		for (int32 I = 0; I < PartCount; ++I)
		{
			const int32 Parent = Def(static_cast<EDescentArmPart>(I)).ParentIndex;
			Out[I] = Parent < 0 ? Local[I] : Local[I] * Out[Parent];   // FTransform A * B = A first, then B
		}
	}
}

// ---------------------------------------------------------------------------------------------------------
// FDescentArmBlender
// ---------------------------------------------------------------------------------------------------------

void FDescentArmBlender::Reset(const FDescentArmPose& Pose)
{
	From = To = Cur = Pose;
	Duration = 0.0;
	Elapsed = 0.0;
}

void FDescentArmBlender::BlendTo(const FDescentArmPose& Target, double DurationSeconds)
{
	From = Cur;
	To = Target;
	Elapsed = 0.0;
	Duration = DurationSeconds;
	if (DurationSeconds <= 0.0)
	{
		Duration = 0.0;
		Cur = Target;
	}
}

double FDescentArmBlender::Weight() const
{
	const double T = Progress();
	return T * T * (3.0 - 2.0 * T);
}

void FDescentArmBlender::Advance(double DeltaSeconds)
{
	if (Duration <= 0.0 || Elapsed >= Duration)
	{
		return;
	}
	Elapsed = FMath::Min(Elapsed + FMath::Max(DeltaSeconds, 0.0), Duration);
	Cur = Elapsed >= Duration ? To : FDescentArmPose::Lerp(From, To, Weight());
}