#pragma once

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

/** Which point of the hand is put on the target. */
enum class EDescentArmEffector : uint8
{
	HandPivot,   // the wrist flex axis (where the hand part's origin is)
	Palm,        // the middle of the palm
	Fingertip,   // the tip of the middle finger in the pose's current finger curl (the plug point of the jack-in)
};

/** A request to put a point of the hand on a target. All positions are in ARM SPACE: origin = the shoulder, +X along the arm at rest, centimetres. */
struct FDescentArmReachRequest
{
	FVector TargetCm = FVector::ZeroVector;
	/** The way the hand should point (its own X axis). Normalised by the solver. Ignored when bAimAtTarget. */
	FVector HandDirection = FVector::ForwardVector;
	/** Point the hand along the line shoulder -> target. */
	bool bAimAtTarget = false;
	EDescentArmEffector Effector = EDescentArmEffector::Fingertip;
	/** Of the (at most two) places the elbow can be, the one whose fold is nearer this side wins (arm space; default: Tune::Arm::ElbowHint). */
	FVector ElbowHint = Descent::Tune::Arm::ElbowHint;
	/** Twist of the palm about the hand direction: 0 = as close to palm-down as the direction allows, +90 = palm faces left (thumb up). */
	double PalmTwistDeg = 0.0;
};

struct FDescentArmReachResult
{
	/** The joints that put the effector there: shoulder, elbow, wrist. Finger curls and the shift are copied from the base pose. */
	FDescentArmPose Pose;
	/** The effector ended within Tune::Arm::ReachToleranceCm of the target. */
	bool bReachable = false;
	/** A joint limit (shoulder, elbow or wrist) held the solution back. */
	bool bLimited = false;
	/** Distance from the achieved effector point to the target, measured by the same forward kinematics that moves the meshes (cm). */
	double ErrorCm = 0.0;
	FVector AchievedCm = FVector::ZeroVector;
	/** Distance shoulder -> target, and the longest the arm can reach with this effector at the requested hand direction (cm). */
	double DistanceCm = 0.0;
	double MaxReachCm = 0.0;
	/** Angle between the requested hand direction and the achieved one (degrees). */
	double HandDirErrorDeg = 0.0;
	/** Angle between the requested palm twist and the achieved one (degrees); 0 when no twist was requested. */
	double TwistErrorDeg = 0.0;
};

namespace Descent::Arm
{
	/** The effector point in the HAND's own frame (cm) for the given finger curls. */
	THEDESCENT_API FVector EffectorOffsetInHand(const FDescentArmPose& Pose, EDescentArmEffector Effector);
	/** Where the effector is in arm space (cm) for a pose, by the forward kinematics that drives the meshes. */
	THEDESCENT_API FVector EffectorPosition(const FDescentArmPose& Pose, EDescentArmEffector Effector);
	/** The hand's X axis (the way the fingers point) in arm space for a pose. */
	THEDESCENT_API FVector HandDirection(const FDescentArmPose& Pose);

	/**
	 * Puts the effector on the target by turning the shoulder (yaw / pitch / roll), the elbow and the wrist (roll, flex): six joints
	 * for six numbers (where, which way the hand points, how the palm is twisted). The wanted hand orientation fixes the hand pivot;
	 * the two-bone triangle shoulder - elbow - pivot is solved in closed form, and the elbow is put on the one or two points of its
	 * circle where the forearm is square to the hand's hinge, which is what lets the wrist reach that orientation exactly.
	 * Where no such point exists (or a joint limit holds a joint back) it solves POSITION FIRST: with the wrist held, forearm and
	 * hand are one rigid piece, the elbow angle for the target's distance is closed-form again, and the one free turn of the arm
	 * about the line shoulder -> target is searched (10 degrees, then 1 degree) for the orientation nearest the wanted one; the wrist
	 * is then re-aimed and the step repeated (3 rounds). The hand is never taken off a target it can reach in order to point
	 * better. A target outside the arm's length makes the arm extend toward it as far as it can. The result always says what
	 * happened: bReachable, ErrorCm (measured by the forward kinematics that moves the meshes), HandDirErrorDeg, TwistErrorDeg,
	 * bLimited. Deterministic, no state.
	 */
	THEDESCENT_API FDescentArmReachResult SolveReach(const FDescentArmReachRequest& Request, const FDescentArmPose& Base);

	/**
	 * A pose described the way one would pose a real arm: where the hand pivot is (arm space, cm), where the elbow is (turned
	 * ElbowOutDeg about the line shoulder -> hand: 0 = hanging straight below that line, + = outward, the right of a right arm),
	 * and how the wrist is rolled and bent. Fingers and shift come from Base. The named poses are built with this, so each
	 * is exactly what its recipe says. bOutExact is false when the point is nearer or farther than the elbow allows or a limit held.
	 */
	THEDESCENT_API FDescentArmPose PoseFromHandAndElbow(const FVector& HandPivotCm, double ElbowOutDeg, double WristRollDeg, double WristFlexDeg, const FDescentArmPose& Base, bool* bOutExact = nullptr);

	/** Longest distance shoulder -> hand pivot (straight arm) and shortest (elbow at its limit), cm. */
	THEDESCENT_API double MaxHandPivotReachCm();
	THEDESCENT_API double MinHandPivotReachCm();
}