// Console commands of the player's cybernetic arm: Descent.Arm.*  (convention: Debug/DescentCommandUtil.h)
// They act on the character's arm, or on the debug rig's once `Descent.Arm.Rig showcase|first` has spawned that rig.
// (On the character only Idle and Hidden are the character's own choice: a pose set here stays until the mode leaves
// Foot / Swim, and `Descent.Arm.Pose hidden` on foot is answered by Idle at the next tick.)

#include "Debug/DescentCommandUtil.h"
#include "Core/Tuning/DescentTune_Arm.h"
#include "DescentUnits.h"
#include "Player/Arm/DescentArmDebugRig.h"
#include "Player/Arm/DescentArmViewmodelComponent.h"
#include "TheDescent.h"

#include "Engine/World.h"
#include "HAL/IConsoleManager.h"

namespace
{
	UDescentArmViewmodelComponent* ArmCmdFindArm(UWorld* World, const TCHAR* Command)
	{
		// The debug rig's arm when a rig exists (it was spawned to be driven), otherwise the first one: the character's.
		const ADescentArmDebugRig* Rig = ADescentArmDebugRig::Find(World);
		UDescentArmViewmodelComponent* Arm = Rig ? Rig->GetArm() : (World ? UDescentArmViewmodelComponent::FindFirst(World) : nullptr);
		if (!Arm)
		{
			Descent::Cmd::Fail(World, Command, TEXT("there is no arm in this world (Descent.Arm.Rig showcase|first spawns the debug rig)"));
		}
		return Arm;
	}

	void ArmCmdLogReach(const TCHAR* What, const FDescentArmReachResult& R)
	{
		UE_LOG(LogDescent, Display, TEXT("DESCENT_ARM_REACH %s reachable=%d error_cm=%.4f distance_cm=%.2f max_reach_cm=%.2f hand_dir_error_deg=%.2f twist_error_deg=%.2f limited=%d"),
			What, R.bReachable ? 1 : 0, R.ErrorCm, R.DistanceCm, R.MaxReachCm, R.HandDirErrorDeg, R.TwistErrorDeg, R.bLimited ? 1 : 0);
	}
}

static void ArmCmdRig(const TArray<FString>& Args, UWorld* World)
{
	const TCHAR* Cmd = TEXT("Descent.Arm.Rig");
	const FString Mode = Args.Num() > 0 ? Args[0].ToLower() : FString();
	const bool bFirst = Mode == TEXT("first") || Mode == TEXT("firstperson") || Mode == TEXT("fp");
	if (!World || (!bFirst && Mode != TEXT("showcase")))
	{
		Descent::Cmd::Fail(World, Cmd, TEXT("usage: Descent.Arm.Rig <showcase|first>"));
		return;
	}
	ADescentArmDebugRig* Rig = ADescentArmDebugRig::FindOrSpawn(World, bFirst ? EDescentArmRigMode::FirstPerson : EDescentArmRigMode::Showcase);
	if (!Rig)
	{
		Descent::Cmd::Fail(World, Cmd, TEXT("could not spawn the arm rig"));
		return;
	}
	if (bFirst)
	{
		Rig->TakeView();
	}
}
static FAutoConsoleCommandWithWorldAndArgs GArmCmdRig(TEXT("Descent.Arm.Rig"),
	TEXT("Descent.Arm.Rig <showcase|first>: spawns the arm's debug rig (or switches it). showcase = on a stand, lit, in the dark; first = mounted on an eye-height camera facing a wall with a nutrient port, and takes the view."),
	FConsoleCommandWithWorldAndArgsDelegate::CreateStatic(&ArmCmdRig));

static void ArmCmdPose(const TArray<FString>& Args, UWorld* World)
{
	const TCHAR* Cmd = TEXT("Descent.Arm.Pose");
	const TCHAR* Usage = TEXT("usage: Descent.Arm.Pose <hidden|idle|reach|grip|point|plug|hold_tool|zero> [blend seconds]");
	if (Args.Num() < 1)
	{
		Descent::Cmd::Fail(World, Cmd, Usage);
		return;
	}
	double Blend = -1.0;
	TArray<double> N;
	if (Args.Num() > 1)
	{
		if (!Descent::Cmd::ParseNumbers(Args, 1, 1, N) || N[0] < 0.0)
		{
			Descent::Cmd::Fail(World, Cmd, Usage);
			return;
		}
		Blend = N[0];
	}
	UDescentArmViewmodelComponent* Arm = ArmCmdFindArm(World, Cmd);
	if (!Arm)
	{
		return;
	}
	if (Arm->IsJackActive())
	{
		Descent::Cmd::Fail(World, Cmd, TEXT("a jack-in is running; it owns the arm until it ends"));
		return;
	}
	EDescentArmPose Pose;
	if (Args[0].ToLower() == TEXT("zero"))
	{
		// Every joint at zero: the arm straight ahead, palm down, fingers straight. Not a game pose; for looking at the model.
		Arm->BlendToPose(FDescentArmPose(), Blend < 0.0 ? Descent::Tune::Arm::PoseBlendSeconds : Blend);
	}
	else if (Descent::Arm::ParsePose(Args[0], Pose))
	{
		Arm->SetPose(Pose, Blend);
	}
	else
	{
		Descent::Cmd::Fail(World, Cmd, FString::Printf(TEXT("unknown pose '%s'. %s"), *Args[0], Usage));
	}
}
static FAutoConsoleCommandWithWorldAndArgs GArmCmdPose(TEXT("Descent.Arm.Pose"),
	TEXT("Descent.Arm.Pose <hidden|idle|reach|grip|point|plug|hold_tool|zero> [blend seconds]: blends the arm to a named pose (0 snaps)."),
	FConsoleCommandWithWorldAndArgsDelegate::CreateStatic(&ArmCmdPose));

static void ArmCmdRecipe(const TArray<FString>& Args, UWorld* World)
{
	const TCHAR* Cmd = TEXT("Descent.Arm.Recipe");
	TArray<double> N;
	if (Args.Num() != 6 || !Descent::Cmd::ParseNumbers(Args, 0, 6, N))
	{
		Descent::Cmd::Fail(World, Cmd, TEXT("usage: Descent.Arm.Recipe handX handY handZ elbowOutDeg wristRollDeg wristFlexDeg  (hand pivot in arm space, cm; the numbers of a pose recipe in DescentTune_Arm.h)"));
		return;
	}
	UDescentArmViewmodelComponent* Arm = ArmCmdFindArm(World, Cmd);
	if (!Arm || Arm->IsJackActive())
	{
		return;
	}
	bool bExact = false;
	const FDescentArmPose Pose = Descent::Arm::PoseFromHandAndElbow(FVector(N[0], N[1], N[2]), N[3], N[4], N[5], Arm->GetTargetPose(), &bExact);
	Arm->BlendToPose(Pose, 0.0);
	UE_LOG(LogDescent, Display, TEXT("DESCENT_ARM_RECIPE exact=%d joints: yaw %.1f pitch %.1f roll %.1f elbow %.1f wrist roll %.1f flex %.1f"), bExact ? 1 : 0,
		Pose.ShoulderYawDeg, Pose.ShoulderPitchDeg, Pose.ShoulderRollDeg, Pose.ElbowFlexDeg, Pose.WristRollDeg, Pose.WristFlexDeg);
}
static FAutoConsoleCommandWithWorldAndArgs GArmCmdRecipe(TEXT("Descent.Arm.Recipe"),
	TEXT("Descent.Arm.Recipe handX handY handZ elbowOutDeg wristRollDeg wristFlexDeg: poses the arm from the numbers of a pose recipe (for tuning the named poses while looking at them); the fingers stay as they are."),
	FConsoleCommandWithWorldAndArgsDelegate::CreateStatic(&ArmCmdRecipe));

static void ArmCmdCurl(const TArray<FString>& Args, UWorld* World)
{
	const TCHAR* Cmd = TEXT("Descent.Arm.Curl");
	const TCHAR* Usage = TEXT("usage: Descent.Arm.Curl <thumb|index|middle|ring|pinky|all> <0..1> [blend seconds]");
	static const TCHAR* const Names[] = { TEXT("thumb"), TEXT("index"), TEXT("middle"), TEXT("ring"), TEXT("pinky") };
	TArray<double> N;
	if (Args.Num() < 2 || !Descent::Cmd::ParseNumbers(Args, 1, Args.Num() > 2 ? 2 : 1, N) || N[0] < 0.0 || N[0] > 1.0)
	{
		Descent::Cmd::Fail(World, Cmd, Usage);
		return;
	}
	const FString Which = Args[0].ToLower();
	int32 Finger = Which == TEXT("all") ? 5 : INDEX_NONE;
	for (int32 I = 0; I < 5; ++I)
	{
		if (Which == Names[I]) { Finger = I; }
	}
	if (Finger == INDEX_NONE)
	{
		Descent::Cmd::Fail(World, Cmd, Usage);
		return;
	}
	UDescentArmViewmodelComponent* Arm = ArmCmdFindArm(World, Cmd);
	if (!Arm)
	{
		return;
	}
	const double Blend = N.Num() > 1 ? N[1] : 0.15;
	for (int32 I = 0; I < 5; ++I)
	{
		if (Finger == 5 || Finger == I)
		{
			Arm->SetFingerCurl(I, N[0], Blend);
		}
	}
}
static FAutoConsoleCommandWithWorldAndArgs GArmCmdCurl(TEXT("Descent.Arm.Curl"),
	TEXT("Descent.Arm.Curl <thumb|index|middle|ring|pinky|all> <0..1> [blend seconds]: curls one finger (0 straight, 1 closed) on top of the current pose."),
	FConsoleCommandWithWorldAndArgsDelegate::CreateStatic(&ArmCmdCurl));

static void ArmCmdReach(const TArray<FString>& Args, UWorld* World)
{
	const TCHAR* Cmd = TEXT("Descent.Arm.Reach");
	const TCHAR* Usage = TEXT("usage: Descent.Arm.Reach x y z [dx dy dz [palm twist deg]]  (arm space, centimetres from the shoulder: +x ahead, +y right, +z up)");
	TArray<double> N;
	const int32 Count = Args.Num();
	if ((Count != 3 && Count != 6 && Count != 7) || !Descent::Cmd::ParseNumbers(Args, 0, Count, N))
	{
		Descent::Cmd::Fail(World, Cmd, Usage);
		return;
	}
	UDescentArmViewmodelComponent* Arm = ArmCmdFindArm(World, Cmd);
	if (!Arm)
	{
		return;
	}
	if (Arm->IsJackActive())
	{
		Descent::Cmd::Fail(World, Cmd, TEXT("a jack-in is running; it owns the arm until it ends"));
		return;
	}
	FDescentArmReachRequest Req;
	Req.TargetCm = FVector(N[0], N[1], N[2]);
	Req.Effector = EDescentArmEffector::Fingertip;
	if (Count >= 6)
	{
		Req.HandDirection = FVector(N[3], N[4], N[5]);
		if (Req.HandDirection.IsNearlyZero())
		{
			Descent::Cmd::Fail(World, Cmd, TEXT("the hand direction is zero"));
			return;
		}
	}
	else
	{
		Req.bAimAtTarget = true;
	}
	if (Count == 7)
	{
		Req.PalmTwistDeg = N[6];
	}
	// An unreachable target is not a failed command: the arm stretches toward it and the result says so (state section `arm`).
	ArmCmdLogReach(TEXT("arm_space"), Arm->ReachTo(Req));
}
static FAutoConsoleCommandWithWorldAndArgs GArmCmdReach(TEXT("Descent.Arm.Reach"),
	TEXT("Descent.Arm.Reach x y z [dx dy dz [palm twist deg]]: puts the middle fingertip on a point given in arm space (cm from the shoulder). Without a direction the hand aims along the line of sight. The result is logged and in the state section `arm`."),
	FConsoleCommandWithWorldAndArgsDelegate::CreateStatic(&ArmCmdReach));

static void ArmCmdReachWorld(const TArray<FString>& Args, UWorld* World)
{
	const TCHAR* Cmd = TEXT("Descent.Arm.ReachWorld");
	TArray<double> N;
	const int32 Count = Args.Num();
	if ((Count != 3 && Count != 6) || !Descent::Cmd::ParseNumbers(Args, 0, Count, N))
	{
		Descent::Cmd::Fail(World, Cmd, TEXT("usage: Descent.Arm.ReachWorld x y z [dx dy dz]  (world, metres)"));
		return;
	}
	UDescentArmViewmodelComponent* Arm = ArmCmdFindArm(World, Cmd);
	if (!Arm)
	{
		return;
	}
	if (Arm->IsJackActive())
	{
		Descent::Cmd::Fail(World, Cmd, TEXT("a jack-in is running; it owns the arm until it ends"));
		return;
	}
	const FVector Dir = Count == 6 ? FVector(N[3], N[4], N[5]) : FVector::ZeroVector;   // zero: aim along the line of sight
	ArmCmdLogReach(TEXT("world"), Arm->ReachToWorld(Descent::MetresToUU(FVector(N[0], N[1], N[2])), Dir));
}
static FAutoConsoleCommandWithWorldAndArgs GArmCmdReachWorld(TEXT("Descent.Arm.ReachWorld"),
	TEXT("Descent.Arm.ReachWorld x y z [dx dy dz]: the same for a point of the world, in metres."),
	FConsoleCommandWithWorldAndArgsDelegate::CreateStatic(&ArmCmdReachWorld));

static void ArmCmdJack(const TArray<FString>& Args, UWorld* World)
{
	const TCHAR* Cmd = TEXT("Descent.Arm.Jack");
	const TCHAR* Usage = TEXT("usage: Descent.Arm.Jack  |  Descent.Arm.Jack x y z dx dy dz  (socket in world metres, and the way the hand goes into it)  |  Descent.Arm.Jack abort");
	UDescentArmViewmodelComponent* Arm = ArmCmdFindArm(World, Cmd);
	if (!Arm)
	{
		return;
	}
	if (Args.Num() == 1 && Args[0].ToLower() == TEXT("abort"))
	{
		Arm->AbortJackIn();
		return;
	}
	FVector PortCm, Dir;
	if (Args.Num() == 0)
	{
		const ADescentArmDebugRig* Rig = ADescentArmDebugRig::Find(World);
		if (!Rig)
		{
			Descent::Cmd::Fail(World, Cmd, FString::Printf(TEXT("no arm rig to take the port from. %s"), Usage));
			return;
		}
		PortCm = Rig->GetPortWorldCm();
		Dir = Rig->GetPlugDirectionWorld();
	}
	else
	{
		TArray<double> N;
		if (Args.Num() != 6 || !Descent::Cmd::ParseNumbers(Args, 0, 6, N) || FVector(N[3], N[4], N[5]).IsNearlyZero())
		{
			Descent::Cmd::Fail(World, Cmd, Usage);
			return;
		}
		PortCm = Descent::MetresToUU(FVector(N[0], N[1], N[2]));
		Dir = FVector(N[3], N[4], N[5]);
	}
	if (!Arm->StartJackIn(PortCm, Dir))
	{
		Descent::Cmd::Fail(World, Cmd, TEXT("a jack-in is already running"));
	}
}
static FAutoConsoleCommandWithWorldAndArgs GArmCmdJack(TEXT("Descent.Arm.Jack"),
	TEXT("Descent.Arm.Jack [x y z dx dy dz | abort]: starts the 4.8 s jack-in (raise, reach, plug, feed, withdraw, lower) against a socket of the world (metres) or, with no arguments, the debug rig's port."),
	FConsoleCommandWithWorldAndArgsDelegate::CreateStatic(&ArmCmdJack));