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


#include "Joint.h"

#include "Link.h"
#include "util.h"
#include "Kismet/KismetMathLibrary.h"

Vector3d UJoint::GetFirstWorldPosition() const{
	return FirstLink->Position + FirstLink->Orientation * FirstLocalPosition;
}

Vector3d UJoint::GetSecondWorldPosition() const{
	return SecondLink->Position + SecondLink->Orientation * SecondLocalPosition;
}

void UJoint::SolvePosition(const double H) const{
	ULink* L1 = FirstLink;
	ULink* L2 = SecondLink;

	// Positional Constraints
	const Vector3d R1 = GetFirstWorldPosition();
	const Vector3d R2 = GetSecondWorldPosition();
	const Vector3d D = R1 - R2; // Original PBD (not R2 - R1), because Impulse P is negated
	const Vector3d N = D.normalized();
	const double C = D.norm();
	const Matrix3d I1 = L1->GetInertia();
	const Matrix3d I2 = L2->GetInertia();
	const double M1 = L1->Mass;
	const double M2 = L2->Mass;
	
	const double W1 = L1->GetPositionalInverseMass(R1, N);
	const double W2 = L2->GetPositionalInverseMass(R2, N);

	const double A = 0.000001 / (H * H);
	const Vector3d P = -C / (W1 + W2 + A) * N;
	
	L1->Position += P / M1;
	L2->Position -= P / M2;
		
	Vector3d T1 = I1.inverse() * R1.cross(P);
	Vector3d T2 = I2.inverse() * R2.cross(P);
	L1->Orientation += Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
	L2->Orientation -= Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;

	// Rotational Constraints
}

void UJoint::SolveVelocity(const double H) const{
	ULink* L1 = FirstLink;
	ULink* L2 = SecondLink;

	// Damping
	const Vector3d V1 = L1->Velocity;
	const Vector3d V2 = L2->Velocity;

	constexpr double MuLin = 100000;
	const Vector3d DeltaV = (V2 - V1) * UKismetMathLibrary::Min(MuLin * H, 1);

	const Vector3d R1 = GetFirstWorldPosition();
	const Vector3d R2 = GetSecondWorldPosition();
	const Vector3d N = (R2 - R1).normalized();

	const Matrix3d I1 = L1->GetInertia();
	const Matrix3d I2 = L2->GetInertia();
	
	const double W1 = L1->GetPositionalInverseMass(R1, N);
	const double W2 = L2->GetPositionalInverseMass(R2, N);
	
	const Vector3d P = DeltaV / (W1 + W2);

	// UE_LOG(LogTemp, Log, TEXT("%f | %f"), (V2 - V1).norm(), MuLin * H);
	
	L1->Velocity += P / L1->Mass;
	L2->Velocity -= P / L2->Mass;
	L1->AngularVelocity += I1.inverse() * R1.cross(P);
	L2->AngularVelocity -= I2.inverse() * R2.cross(P);
}

UJoint::UJoint()
{
}