// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#include "CoreMinimal.h"
#include "Joint.h"
#include "Components/StaticMeshComponent.h"
#include "Eigen/Dense"
#include "Link.generated.h"

using namespace Eigen;

/**
 * 
 */
UCLASS(ClassGroup=(Custom), meta=(BlueprintSpawnableComponent))
class PBDROBOTICS_API ULink : public UStaticMeshComponent
{
	GENERATED_BODY()
public:
	
	// Properties in World space
	
	Vector3d Position;
	Vector3d Velocity;
	double Mass;
	Quaterniond Orientation;
	Vector3d AngularVelocity;

	Vector3d Last_Position;
	Quaterniond Last_Orientation;
	
	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	UJoint* PrevJoint;

	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	UJoint* NextJoint;

	UFUNCTION(BlueprintCallable)
	FVector GetPosition() const;

	UFUNCTION(BlueprintCallable)
	FQuat GetOrientation() const;

	
	void Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce);
	void Integrate(const double H);
	void UpdateInternalTransform();

	// Inertia tensor in world space
	Matrix3d GetInertia() const;
	
	double GetPositionalInverseMass(const Vector3d& R, const Vector3d& N) const;
	double GetRotationalInverseMass(const Vector3d& N) const;
	
	void Setup();

	UPROPERTY(EditAnywhere)
	bool IsEffector = false;

	Vector3d GetGravityForce() const;
	Matrix3d Inertia_Tensor_Local;
private:
	UPROPERTY(EditAnywhere)
	bool IsBase = false;
	
	void SetupJoints() const;
	void SetupProperties();
};