LuckyWorld/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h

#pragma once
#include "CoreMinimal.h"
#include "Robot/PilotComponent/RobotPilotComponent.h"
#include "RobotPilotSO100Component.generated.h"

USTRUCT(BlueprintType)
struct FSo100Actuators
{
	GENERATED_BODY()
	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	double Rotation = 0.;
	
	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	double Pitch = 0.;

	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	double Elbow = 0.;

	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	double WristPitch = 0.;

	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	double WristRoll = 0.;
	
	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	double Jaw = 0.;
};

UCLASS(Blueprintable)
class LUCKYWORLDV2_API URobotPilotSO100Component : public URobotPilotComponent
{
	GENERATED_BODY()

public:
	URobotPilotSO100Component();
	
	virtual void BeginPlay() override;
	virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;

	UFUNCTION(BlueprintCallable)
	void SetTarget(const FTransform& TargetTransformIn);
private:
	FTransform TargetTransform;
	
public:
	UFUNCTION(BlueprintCallable)
	void PrintActuators() const;

private:
	// SO100 Controls by name
	FString Ctrl_Rotation = FString("Rotation");
	FString Ctrl_Pitch = FString("Pitch");
	FString Ctrl_Elbow = FString("Elbow");
	FString Ctrl_WristPitch = FString("Wrist_Pitch");
	FString Ctrl_WristRoll = FString("Wrist_Roll");
	FString Ctrl_Jaw = FString("Jaw");

	// SO100 Static Variables
	FVector PivotOffset = FVector{-0.000030, 4.520021, 1.650041};
	
	/**
	 * Query the physic proxy on the RobotOwner to get the SO100 actuators values
	 * @return 
	 */
	FSo100Actuators GetCurrentActuatorsFromPhysicScene() const;
	
	// Called after every physic step
	virtual void PostPhysicStepUpdate(const float SimulationTime) override;
	bool AnimateActuators(float SimulationTime) const; // Bound to the PhysicProxy post-update delegate
	FSo100Actuators CurrentRobotActuators; // This will be updated by the post-physic delegate 
	FSo100Actuators AnimStartRobotActuators;
	FSo100Actuators AnimTargetRobotActuators;

	// Quick and dirty sequence of moves
	// -1 -> Start Game, extended
	// 0 -> Retract - base pose
	// 1 -> Rotate towards goal
	// 2 -> Move to target
	// 3 -> Close the jaw
	// 4 -> Go to drop zone
	// 5 -> open jaw
	int32 CurrentAnimationState = -1;
	int32 MaxAnimationState = 5;
	void NextAnimationState();

	void BasePose();
	void RotateToTarget();
	void CloseJaw();
	void MoveToDropZone();
	void OpenJaw();
	
	
	
	// Here let's write the code trying to match with Constantin class
	// After both classes have been designed around specific needs, see what can be migrated in the parent class and update both children

	FSo100Actuators ActuatorsRestPosition {
		0.,
		-1.54,
		3.105,
		-1.5,
		1.47,
		-1.39
	};

	FSo100Actuators ActuatorsMaxExtendPosition {
		0.,
		0.,
		0.,
		0.,
		-1.56,
		-1.045
	};
	
	FSo100Actuators ActuatorsLeftDropZone {
		0.,
		0.,
		0.,
		0.,
		-1.56,
		-1.045
	};

	FSo100Actuators ActuatorsRightDropZone {
		0.,
		0.,
		0.,
		0.,
		-1.56,
		-1.045
	};
	
	// Tick where it can have targets
	// Open Claw (ClawIndex)
	// Presets to move certain joints into certain positions -> HardCode
	// Move the arm myself JB + Capture LOG

	// Set the target in world absolute <- get actor of class with tag
	// Compute target relative
	// Compute distance to target
	// compute rotation - direction to target
	// lerp values to go grab
	// close jaw
	// select drop zone
	// move to drop zone (pick bool)
	// open jaw
	// assess success or failure
};