// Copyright 2022, Algoryx Simulation AB.

#pragma once

// Yumi includes.
#include "RobotSequence.h"

// Unreal Engine includes.
#include "Components/ActorComponent.h"
#include "CoreMinimal.h"

#include "YumiArmController.generated.h"

class UAGX_Constraint1DofComponent;

struct SequenceExecutionData
{
	FRobotSequence Sequence;
	bool IsExecuting = false;
	int TargetIndex = 0;
	double TargetElapsedTime = 0.0;
	TArray<double> TargetStartArmJoints;
	TArray<double> TargetStartGripperJoints;
};

UCLASS(ClassGroup = "Controller", Category = "Controller", Meta = (BlueprintSpawnableComponent))
class YUMI_API UYumiArmController : public UActorComponent
{
	GENERATED_BODY()

public:
	UYumiArmController();
	~UYumiArmController() = default;

	// Important: This function must be called prior to using this controller.
	UFUNCTION(BlueprintCallable, Category = "Controller")
	void Init(
		TArray<UAGX_Constraint1DofComponent*> InJointsArm,
		TArray<UAGX_Constraint1DofComponent*> InJointsGripper);

	UFUNCTION(BlueprintCallable, Category = "Controller")
	void StartRunSequence(const FRobotSequence& Sequence);

	UPROPERTY(BlueprintReadOnly, Category = "Controller")
	bool IsSequenceRunning = false;

private:

	UFUNCTION()
	void TickController(double SimTime);

	virtual void BeginPlay() override;

	void RunCurrentSequence(double DeltaTime);
	void SetSequenceFinished();
	void SetTargetReached();
	TArray<double> GetCurrentJointPos(const TArray<UAGX_Constraint1DofComponent*>& Joints);


	TArray<UAGX_Constraint1DofComponent*> JointsArm;
	TArray<UAGX_Constraint1DofComponent*> JointsGripper;
	SequenceExecutionData CurrentSequence;
	double PreviousSimTime = 0.0;
};
