#include "Player/Arm/DescentArmReach.h"

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

	FORCEINLINE double ReachRad(double Deg) { return FMath::DegreesToRadians(Deg); }
	FORCEINLINE double ReachDeg(double Rad) { return FMath::RadiansToDegrees(Rad); }
	/** Into (-180, 180]. */
	FORCEINLINE double ReachWrapDeg(double Deg) { return FMath::UnwindDegrees(Deg); }

	double UpperLenCm() { return Descent::Arm::ShoulderToElbowCm(); }
	double ForeLenCm() { return Descent::Arm::ElbowToHandPivotCm(); }

	/** Raw joint solution of one pass. */
	struct FReachCore
	{
		double Yaw = 0.0, Pitch = 0.0, Roll = 0.0, Elbow = 0.0, WRoll = 0.0, WFlex = 0.0;
		bool bLimited = false;
		FQuat HandRot = FQuat::Identity;
	};

	/** A vector perpendicular to V (any one, stable). */
	FVector ReachAnyPerpendicular(const FVector& V)
	{
		const FVector Axis = FMath::Abs(V.Z) < 0.9 ? FVector::UpVector : FVector::ForwardVector;
		return FVector::CrossProduct(V, Axis).GetSafeNormal();
	}

	/** The triangle shoulder - elbow - hand pivot for a hand pivot H (relative to the shoulder, cm). */
	struct FReachTriangle
	{
		FVector h = FVector::ForwardVector;   // unit, shoulder -> hand pivot
		double Dist = 0.0;                    // shoulder -> hand pivot, clamped to what the elbow allows
		double Alpha = 0.0;                   // angle upper arm / h (rad)
		double Theta = 0.0;                   // elbow flexion, 0 = straight (rad)
		FVector n0 = FVector::UpVector;       // unit, perpendicular to h: the fold direction at swivel 0 (the hint's side)
		bool bLimited = false;                // H was nearer or farther than the elbow allows
	};

	FReachTriangle ReachTriangle(const FVector& H, const FVector& Hint)
	{
		const double L1 = UpperLenCm(), L2 = ForeLenCm();
		const double DMax = L1 + L2;
		const double DMin = Descent::Arm::MinHandPivotReachCm();
		FReachTriangle Tri;
		Tri.Dist = H.Size();
		Tri.h = Tri.Dist > 1e-9 ? H / Tri.Dist : FVector::ForwardVector;
		Tri.bLimited = Tri.Dist > DMax + 1e-9 || Tri.Dist < DMin - 1e-9;
		Tri.Dist = FMath::Clamp(Tri.Dist, DMin, DMax);
		// Law of cosines.
		Tri.Theta = FMath::Acos(FMath::Clamp((Tri.Dist * Tri.Dist - L1 * L1 - L2 * L2) / (2.0 * L1 * L2), -1.0, 1.0));
		Tri.Alpha = FMath::Acos(FMath::Clamp((Tri.Dist * Tri.Dist + L1 * L1 - L2 * L2) / (2.0 * Tri.Dist * L1), -1.0, 1.0));
		const FVector n = Hint - Tri.h * FVector::DotProduct(Hint, Tri.h);
		Tri.n0 = n.SizeSquared() > 1e-10 ? n.GetSafeNormal() : ReachAnyPerpendicular(Tri.h);
		return Tri;
	}

	/**
	 * The swivel angles (degrees about h, 0 = the hint's plane) at which the forearm is perpendicular to the wanted hinge axis Yh
	 * of the hand. Only there can roll + flex of the wrist give the hand exactly the wanted orientation. With the fold direction
	 * n(phi) = n0 cos phi + m0 sin phi and the forearm f ~ h (Dist - L1 cos Alpha) + n L1 sin Alpha, f . Yh = 0 reads
	 * a cos phi + b sin phi = c. Two answers, or the single nearest one when there is none. Sorted: nearest the hint first.
	 */
	int32 ReachAnalyticSwivels(const FReachTriangle& Tri, const FVector& Yh, double Out[2])
	{
		const double L1 = UpperLenCm();
		const double Radius = L1 * FMath::Sin(Tri.Alpha);   // of the circle the elbow can be on
		Out[0] = Out[1] = 0.0;
		if (Radius < 1e-6)
		{
			return 1;   // straight arm: the swivel moves nothing
		}
		const FVector m0 = Tri.n0.RotateAngleAxis(90.0, Tri.h);
		const double a = FVector::DotProduct(Tri.n0, Yh), b = FVector::DotProduct(m0, Yh);
		const double R = FMath::Sqrt(a * a + b * b);
		if (R < 1e-9)
		{
			return 1;   // the hinge is along h: every swivel is as good (or as bad) as any other
		}
		const double c = -(Tri.Dist - L1 * FMath::Cos(Tri.Alpha)) * FVector::DotProduct(Tri.h, Yh) / Radius;
		const double Phi0 = ReachDeg(FMath::Atan2(b, a));
		if (FMath::Abs(c) >= R)
		{
			Out[0] = ReachWrapDeg(c > 0.0 ? Phi0 : Phi0 + 180.0);   // no exact answer: the nearest miss
			return 1;
		}
		const double Half = ReachDeg(FMath::Acos(c / R));
		Out[0] = ReachWrapDeg(Phi0 + Half);
		Out[1] = ReachWrapDeg(Phi0 - Half);
		if (FMath::Abs(Out[1]) < FMath::Abs(Out[0]))
		{
			Swap(Out[0], Out[1]);
		}
		return 2;
	}

	/**
	 * The wrist, for a forearm orientation QFore: roll and flex that bring the hand as near as they can to pointing along D
	 * with its hinge on Yh (both unit, arm space). In the forearm's frame a roll R puts the hand's hinge at (0, cos R, -sin R)
	 * and a flex F then points the hand along (cos F, -sin F sin R, -sin F cos R). Writes WRoll, WFlex, HandRot; ORs bLimited.
	 */
	void ReachSolveWrist(const FQuat& QFore, const FVector& D, const FVector& Yh, FReachCore& Out)
	{
		using namespace Descent::Arm;
		auto Limit = [&Out](double V, double Lo, double Hi)
		{
			if (V < Lo - 1e-9 || V > Hi + 1e-9) { Out.bLimited = true; }
			return FMath::Clamp(V, Lo, Hi);
		};
		const FVector Dl = QFore.UnrotateVector(D);
		const FVector Yl = QFore.UnrotateVector(Yh);
		// The roll the palm twist asks for.
		const double RollForTwist = (Yl.Y * Yl.Y + Yl.Z * Yl.Z) > 1e-12 ? ReachDeg(FMath::Atan2(-Yl.Z, Yl.Y)) : 0.0;
		double R = RollForTwist;
		const double Sideways = FMath::Sqrt(Dl.Y * Dl.Y + Dl.Z * Dl.Z);
		if (Sideways > 1e-9)
		{
			// The roll that points the hand exactly along D (or the same turned half round, with the flex reversed: take the
			// one nearer the wanted twist). Near the forearm's own axis the direction says nothing about roll, so it fades out.
			double RollForDir = ReachDeg(FMath::Atan2(-Dl.Y, -Dl.Z));
			if (FMath::Abs(ReachWrapDeg(RollForDir - RollForTwist)) > 90.0)
			{
				RollForDir += 180.0;
			}
			const double Fade = FMath::SmoothStep(0.0, FMath::Sin(ReachRad(ArmTune::WristSingularityDeg)), Sideways);
			R = RollForTwist + Fade * ReachWrapDeg(RollForDir - RollForTwist);
		}
		R = Limit(ReachWrapDeg(R), -ArmTune::WristRollLimitDeg, ArmTune::WristRollLimitDeg);
		const double Rr = ReachRad(R);
		const FVector FlexDir(0.0, -FMath::Sin(Rr), -FMath::Cos(Rr));   // where a flex of +90 would point the hand
		double F = ReachDeg(FMath::Atan2(FVector::DotProduct(Dl, FlexDir), Dl.X));
		F = Limit(F, ArmTune::WristFlexMinDeg, ArmTune::WristFlexMaxDeg);
		Out.WRoll = R;
		Out.WFlex = F;
		Out.HandRot = QFore * WristRollRotation(R) * WristFlexRotation(F);
	}

	/**
	 * The shoulder as yaw / pitch / roll for an upper arm pointing along u whose own "up" (the side the elbow folds toward) is w
	 * (unit, perpendicular, arm space). The ball joint's limit is on the direction (not into the cone straight back) and on the
	 * turn about the arm's own axis; a held-back joint ORs bLimited.
	 */
	void ReachShoulderFromAxes(FVector u, const FVector& w, FReachCore& Out)
	{
		using namespace Descent::Arm;
		const double MaxFromForward = ReachRad(ArmTune::ShoulderMaxFromForwardDeg);
		if (FMath::Acos(FMath::Clamp(u.X, -1.0, 1.0)) > MaxFromForward + 1e-9)
		{
			Out.bLimited = true;
			const FVector Side = FVector(0.0, u.Y, u.Z).GetSafeNormal(UE_SMALL_NUMBER, FVector(0.0, 0.0, -1.0));
			u = FVector(FMath::Cos(MaxFromForward), 0.0, 0.0) + Side * FMath::Sin(MaxFromForward);
		}
		const double Yaw = (FMath::Abs(u.X) + FMath::Abs(u.Y) < 1e-9) ? 0.0 : ReachDeg(FMath::Atan2(u.Y, u.X));
		const double Pitch = ReachDeg(FMath::Asin(FMath::Clamp(u.Z, -1.0, 1.0)));
		const FVector WLocal = ShoulderRotation(Yaw, Pitch, 0.0).UnrotateVector(w);
		double Roll = ReachDeg(FMath::Atan2(WLocal.Y, WLocal.Z));   // the roll axis (-1,0,0) takes +Z to (0, sin r, cos r)
		if (FMath::Abs(Roll) > ArmTune::ShoulderRollLimitDeg + 1e-9)
		{
			Out.bLimited = true;
			Roll = FMath::Clamp(Roll, -ArmTune::ShoulderRollLimitDeg, ArmTune::ShoulderRollLimitDeg);
		}
		Out.Yaw = Yaw;
		Out.Pitch = Pitch;
		Out.Roll = Roll;
	}

	/** Shoulder and elbow for a triangle with its elbow plane turned SwivelDeg about h. Sets bLimited afresh. */
	void ReachShoulderAndElbow(const FReachTriangle& Tri, double SwivelDeg, FReachCore& Out)
	{
		const double L1 = UpperLenCm();
		Out.bLimited = Tri.bLimited;
		// The plane of the bend contains h and n; the forearm folds toward +n, the upper arm bulges toward -n.
		const FVector n = SwivelDeg != 0.0 ? Tri.n0.RotateAngleAxis(SwivelDeg, Tri.h) : Tri.n0;
		const FVector Hc = Tri.h * Tri.Dist;
		const FVector u = (Tri.h * FMath::Cos(Tri.Alpha) - n * FMath::Sin(Tri.Alpha)).GetSafeNormal();
		const FVector f = (Hc - u * L1).GetSafeNormal();
		FVector w = f - u * FVector::DotProduct(f, u);
		if (w.SizeSquared() < 1e-12)
		{
			w = n - u * FVector::DotProduct(n, u);   // straight arm: the bend direction is the plane's
		}
		ReachShoulderFromAxes(u, w.GetSafeNormal(), Out);
		Out.Elbow = ReachDeg(Tri.Theta);
	}

	/** Joints for a triangle, a swivel, a hand direction D and a wanted hinge axis Yh of the hand (both unit, arm space). */
	void ReachSolveJoints(const FReachTriangle& Tri, const FVector& D, const FVector& Yh, double SwivelDeg, FReachCore& Out)
	{
		using namespace Descent::Arm;
		ReachShoulderAndElbow(Tri, SwivelDeg, Out);
		ReachSolveWrist(ShoulderRotation(Out.Yaw, Out.Pitch, Out.Roll) * ElbowRotation(Out.Elbow), D, Yh, Out);
	}

	/**
	 * Position first. With the wrist held at (WRoll, WFlex) the forearm and the hand are one rigid piece, so the effector is a
	 * fixed point P of the forearm's frame and the arm is a two-bone chain of lengths L1 and |P|: the elbow angle that gives the
	 * target's distance is closed-form, and every shoulder rotation that then carries the effector onto the target differs
	 * from the shortest one by a turn (SwivelDeg) about the line shoulder -> target. Exact in position whatever the hand's
	 * orientation, as long as the distance is within the chain's and the shoulder stays inside its limits.
	 */
	void ReachSolvePositionFirst(const FVector& T, const FVector& E, double WRoll, double WFlex, double SwivelDeg, FReachCore& Out)
	{
		using namespace Descent::Arm;
		const double L1 = UpperLenCm();
		Out.bLimited = false;
		auto Limit = [&Out](double V, double Lo, double Hi)
		{
			if (V < Lo - 1e-9 || V > Hi + 1e-9) { Out.bLimited = true; }
			return FMath::Clamp(V, Lo, Hi);
		};
		const FQuat QWrist = WristRollRotation(WRoll) * WristFlexRotation(WFlex);
		const FVector P = FVector(ForeLenCm(), 0.0, 0.0) + QWrist.RotateVector(E);
		// |(L1,0,0) + Elbow(theta) P|^2 = L1^2 + |P|^2 + 2 L1 rho cos(theta + phi0), rho and phi0 from P's part in the bend plane.
		const double Rho = FMath::Sqrt(P.X * P.X + P.Z * P.Z);
		double ThetaDeg = 0.0;
		if (Rho > 1e-9)
		{
			double C = (T.SizeSquared() - L1 * L1 - P.SizeSquared()) / (2.0 * L1 * Rho);
			if (C > 1.0 || C < -1.0)
			{
				Out.bLimited = true;   // farther than the straight chain, or nearer than the folded one
				C = FMath::Clamp(C, -1.0, 1.0);
			}
			ThetaDeg = ReachDeg(FMath::Acos(C) - FMath::Atan2(P.Z, P.X));
		}
		ThetaDeg = Limit(ThetaDeg, ArmTune::ElbowMinFlexDeg, ArmTune::ElbowMaxFlexDeg);
		const FQuat QElbow = ElbowRotation(ThetaDeg);
		const FVector Q = FVector(L1, 0.0, 0.0) + QElbow.RotateVector(P);   // the effector in the upper arm's frame
		const FVector TDir = T.GetSafeNormal(UE_SMALL_NUMBER, FVector::ForwardVector);
		const FQuat QShoulder = FQuat(TDir, ReachRad(SwivelDeg)) * FQuat::FindBetweenVectors(Q, T);

		ReachShoulderFromAxes(QShoulder.RotateVector(FVector::ForwardVector), QShoulder.RotateVector(FVector::UpVector), Out);
		Out.Elbow = ThetaDeg; Out.WRoll = WRoll; Out.WFlex = WFlex;
		Out.HandRot = ShoulderRotation(Out.Yaw, Out.Pitch, Out.Roll) * QElbow * QWrist;
	}

	void ReachApplyJoints(const FReachCore& C, FDescentArmPose& Pose)
	{
		Pose.ShoulderYawDeg = C.Yaw;
		Pose.ShoulderPitchDeg = C.Pitch;
		Pose.ShoulderRollDeg = C.Roll;
		Pose.ElbowFlexDeg = C.Elbow;
		Pose.WristRollDeg = C.WRoll;
		Pose.WristFlexDeg = C.WFlex;
	}

	FVector ReachPalmNormalOf(const FQuat& HandRot)
	{
		return HandRot.RotateVector(FVector(0.0, 0.0, -1.0));
	}

	/** The palm normal that "twist" degrees of supination about direction D gives (0 = as close to palm-down as D allows). */
	FVector ReachWantedPalmNormal(const FVector& D, double TwistDeg)
	{
		FVector P0 = FVector(0.0, 0.0, -1.0) - D * FVector::DotProduct(FVector(0.0, 0.0, -1.0), D);
		// Pointing straight up or down there is no "down" left: the palm then faces back toward the body.
		P0 = P0.SizeSquared() > 1e-6 ? P0.GetSafeNormal() : FVector(-1.0, 0.0, 0.0);
		return FQuat(-D, ReachRad(TwistDeg)).RotateVector(P0);
	}

	double ReachAngleDeg(const FVector& A, const FVector& B)
	{
		return ReachDeg(FMath::Acos(FMath::Clamp(FVector::DotProduct(A, B), -1.0, 1.0)));
	}
}

namespace Descent::Arm
{
	double MaxHandPivotReachCm() { return UpperLenCm() + ForeLenCm(); }

	double MinHandPivotReachCm()
	{
		const double L1 = UpperLenCm(), L2 = ForeLenCm();
		return FMath::Sqrt(L1 * L1 + L2 * L2 + 2.0 * L1 * L2 * FMath::Cos(ReachRad(ArmTune::ElbowMaxFlexDeg)));
	}

	FVector EffectorOffsetInHand(const FDescentArmPose& Pose, EDescentArmEffector Effector)
	{
		switch (Effector)
		{
		case EDescentArmEffector::HandPivot:
			return FVector::ZeroVector;
		case EDescentArmEffector::Palm:
			return ArmTune::PalmPointHandCm;
		case EDescentArmEffector::Fingertip:
		default:
		{
			// Only the fingers decide this, so take the arm joints at zero.
			FDescentArmPose Straight = Pose;
			Straight.ShoulderYawDeg = Straight.ShoulderPitchDeg = Straight.ShoulderRollDeg = 0.0;
			Straight.ElbowFlexDeg = Straight.WristRollDeg = Straight.WristFlexDeg = 0.0;
			Straight.ShiftCm = FVector::ZeroVector;
			FDescentArmTransforms W;
			ComputeWorld(Straight, W);
			const FVector Tip = W[static_cast<int32>(EDescentArmPart::Middle3)].TransformPosition(FVector(Def(EDescentArmPart::Middle3).LocalBoundsMaxCm.X, 0.0, 0.0));
			return W[static_cast<int32>(EDescentArmPart::Hand)].InverseTransformPosition(Tip);
		}
		}
	}

	FVector EffectorPosition(const FDescentArmPose& Pose, EDescentArmEffector Effector)
	{
		FDescentArmTransforms W;
		ComputeWorld(Pose, W);
		return W[static_cast<int32>(EDescentArmPart::Hand)].TransformPosition(EffectorOffsetInHand(Pose, Effector));
	}

	FVector HandDirection(const FDescentArmPose& Pose)
	{
		FDescentArmTransforms W;
		ComputeWorld(Pose, W);
		return W[static_cast<int32>(EDescentArmPart::Hand)].GetRotation().RotateVector(FVector::ForwardVector);
	}

	FDescentArmPose PoseFromHandAndElbow(const FVector& HandPivotCm, double ElbowOutDeg, double WristRollDeg, double WristFlexDeg, const FDescentArmPose& Base, bool* bOutExact)
	{
		// Swivel 0 is the plane through "up": the forearm folds up, the elbow hangs straight below the line shoulder -> hand.
		// A positive turn about that line then swings the elbow to +Y (checked by Descent.Arm.Reach).
		const FReachTriangle Tri = ReachTriangle(HandPivotCm - Base.ShiftCm, FVector::UpVector);
		FReachCore Core;
		ReachShoulderAndElbow(Tri, ElbowOutDeg, Core);
		FDescentArmPose Pose = Base;
		ReachApplyJoints(Core, Pose);
		Pose.WristRollDeg = FMath::Clamp(WristRollDeg, -ArmTune::WristRollLimitDeg, ArmTune::WristRollLimitDeg);
		Pose.WristFlexDeg = FMath::Clamp(WristFlexDeg, ArmTune::WristFlexMinDeg, ArmTune::WristFlexMaxDeg);
		if (bOutExact)
		{
			*bOutExact = !Core.bLimited && Pose.WristRollDeg == WristRollDeg && Pose.WristFlexDeg == WristFlexDeg;
		}
		return Pose;
	}

	FDescentArmReachResult SolveReach(const FDescentArmReachRequest& Req, const FDescentArmPose& Base)
	{
		FDescentArmReachResult R;
		R.Pose = Base;
		// The shoulder sits at the pose's shift; the maths below is relative to the shoulder.
		const FVector T = Req.TargetCm - Base.ShiftCm;
		R.DistanceCm = T.Size();
		FVector D = Req.bAimAtTarget ? T.GetSafeNormal() : Req.HandDirection.GetSafeNormal();
		if (D.IsNearlyZero())
		{
			D = FVector::ForwardVector;
		}
		const FVector E = EffectorOffsetInHand(Base, Req.Effector);
		R.MaxReachCm = MaxHandPivotReachCm() + E.Size();

		// The hand orientation asked for: X along D, palm (-Z) on the twisted normal.
		const FVector WantPalm = ReachWantedPalmNormal(D, Req.PalmTwistDeg);
		const FQuat WantRot = FRotationMatrix::MakeFromXZ(D, -WantPalm).ToQuat();
		const FVector WantHinge = WantRot.RotateVector(FVector::RightVector);

		struct FCandidate
		{
			FReachCore Core;
			double PosErr = 0.0, DirErr = 0.0, TwistErr = 0.0, Score = TNumericLimits<double>::Max();
		};
		// Measures a candidate with the forward kinematics that moves the meshes. Position first, and strictly: a hundredth of a
		// millimetre off the target weighs more than the worst orientation (at most 360 degrees), so the hand is never taken
		// off a target it can reach to point better. Then the orientation.
		auto Rate = [&](FCandidate& C)
		{
			FDescentArmPose P = Base;
			ReachApplyJoints(C.Core, P);
			C.PosErr = (EffectorPosition(P, Req.Effector) - Req.TargetCm).Size();
			C.DirErr = ReachAngleDeg(C.Core.HandRot.RotateVector(FVector::ForwardVector), D);
			C.TwistErr = ReachAngleDeg(ReachPalmNormalOf(C.Core.HandRot), WantPalm);
			C.Score = 1.0e6 * C.PosErr + C.DirErr + C.TwistErr;
		};
		// The exact solution: the Which-th analytic swivel (the last one when there are fewer).
		auto Run = [&](int32 Which)
		{
			FCandidate C;
			FQuat HandRot = WantRot;
			double Swivel = 0.0;
			for (int32 It = 0; It < ArmTune::ReachIterations; ++It)
			{
				// The pivot that puts the effector on the target for the orientation reached so far (the wanted one at first:
				// when the joints can give it, this loop ends after one pass).
				const FVector H = T - HandRot.RotateVector(E);
				const FReachTriangle Tri = ReachTriangle(H, Req.ElbowHint);
				double S[2];
				const int32 N = ReachAnalyticSwivels(Tri, WantHinge, S);
				Swivel = S[FMath::Min(Which, N - 1)];
				ReachSolveJoints(Tri, D, WantHinge, Swivel, C.Core);
				const double Moved = ((T - C.Core.HandRot.RotateVector(E)) - H).Size();
				HandRot = C.Core.HandRot;
				if (Moved < 1e-10)
				{
					break;
				}
			}
			Rate(C);
			C.Score += 0.02 * FMath::Abs(Swivel);   // of two exact answers, the one on the hint's side
			return C;
		};

		FCandidate Best = Run(0);
		{
			const FCandidate Other = Run(1);
			if (Other.Score < Best.Score)
			{
				Best = Other;
			}
		}
		if (Best.PosErr > 1e-4)
		{
			// The wanted orientation cannot be had there (a joint limit, or no elbow position fits it), and chasing it has cost
			// the position. Position first, then: hold the wrist, put the effector on the target exactly, and search the one
			// free turn of the arm about the line shoulder -> target for the orientation nearest the wanted one, with the elbow
			// on the side away from the hint. Then give the wrist another go for the forearm that came out, and repeat.
			const FVector TDir = T.GetSafeNormal(UE_SMALL_NUMBER, FVector::ForwardVector);
			FVector HintSide = Req.ElbowHint - TDir * FVector::DotProduct(Req.ElbowHint, TDir);
			HintSide = HintSide.GetSafeNormal();
			double WRoll = Best.Core.WRoll, WFlex = Best.Core.WFlex;
			for (int32 Round = 0; Round < 3; ++Round)
			{
				FCandidate RoundBest;
				double RoundSwivel = 0.0;
				auto Try = [&](double S)
				{
					FCandidate C;
					ReachSolvePositionFirst(T, E, WRoll, WFlex, S, C.Core);
					Rate(C);
					const FVector Elbow = ShoulderRotation(C.Core.Yaw, C.Core.Pitch, C.Core.Roll).RotateVector(FVector::ForwardVector);
					const FVector ElbowSide = (Elbow - TDir * FVector::DotProduct(Elbow, TDir)).GetSafeNormal();
					C.Score += 5.0 * (1.0 + FVector::DotProduct(ElbowSide, HintSide));
					if (C.Score < RoundBest.Score)
					{
						RoundBest = C;
						RoundSwivel = S;
					}
				};
				for (double S = -180.0; S < 180.0; S += 10.0) { Try(S); }
				const double Centre = RoundSwivel;
				for (double S = Centre - 9.0; S <= Centre + 9.0; S += 1.0) { Try(S); }
				if (RoundBest.Score < Best.Score || Best.PosErr > RoundBest.PosErr + 1e-4)
				{
					Best = RoundBest;
				}
				FReachCore Wrist = RoundBest.Core;
				ReachSolveWrist(ShoulderRotation(Wrist.Yaw, Wrist.Pitch, Wrist.Roll) * ElbowRotation(Wrist.Elbow), D, WantHinge, Wrist);
				if (FMath::Abs(Wrist.WRoll - WRoll) + FMath::Abs(Wrist.WFlex - WFlex) < 0.01)
				{
					break;
				}
				WRoll = Wrist.WRoll;
				WFlex = Wrist.WFlex;
			}
		}

		ReachApplyJoints(Best.Core, R.Pose);
		R.bLimited = Best.Core.bLimited;
		R.AchievedCm = EffectorPosition(R.Pose, Req.Effector);
		R.ErrorCm = (R.AchievedCm - Req.TargetCm).Size();
		R.bReachable = R.ErrorCm <= ArmTune::ReachToleranceCm;
		R.HandDirErrorDeg = Best.DirErr;
		R.TwistErrorDeg = Best.TwistErr;
		return R;
	}
}