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

#pragma once

#include "Eigen/Dense"
#include "CoreMinimal.h"
#include "Joint.generated.h"

using namespace Eigen;

class ULink;
/**
 * 
 */
UCLASS(BlueprintType, ClassGroup=(Custom))
class PBDROBOTICS_API UJoint : public UObject
{
	GENERATED_BODY()
public:
	
	UPROPERTY()
	ULink* FirstLink;

	UPROPERTY()
	ULink* SecondLink;

	// First Connection point in local space
	Vector3d FirstLocalPosition;

	// Second Connection point in local space
	Vector3d SecondLocalPosition;

	// Direction from CenterOfMass to First Connection point
	Vector3d GetFirstWorldDirection() const;

	// Direction from CenterOfMass to Second Connection point
	Vector3d GetSecondWorldDirection() const;

	// First - Second in World-Space 
	Vector3d GetConnection() const;
	
	Vector3d FirstRotateAxis;
	Vector3d SecondRotateAxis;

	// World Axis, around which the First Link wants to rotate
	Vector3d GetFirstWorldAxis() const;

	// World Axis, around which the Second Link wants to rotate
	Vector3d GetSecondWorldAxis() const;

	void SolvePosition(const double H) const;
	void SolveVelocity(const double H) const;
	
	UJoint();
};