// 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;
}

Vector3d UJoint::GetFirstWorldAxis() const{
	return FirstLink->Orientation * FirstRotateAxis;
}
Vector3d UJoint::GetSecondWorldAxis() const{
	return SecondLink->Orientation * SecondRotateAxis;
}

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);

		constexpr double A = 1. / 1000000; // Compliance (inverse of stiffness)
		const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N;
	
		L1->Position += P / M1;
		L2->Position -= P / M2;
		
		Vector3d T1 = I1.inverse() * R1.cross(P);
		Vector3d T2 = I2.inverse() * R2.cross(P);
		Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
		Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
		//L1->Orientation = L1->Orientation + Add1;
		//L2->Orientation = L2->Orientation- Add2;
		//UE_LOG(LogTemp, Log, TEXT("%f %f"), (T1).norm(), T2.norm());
	}
	
	// Rotational Constraints
	{
		const Vector3d A1 = GetFirstWorldAxis();
		const Vector3d A2 = GetSecondWorldAxis();
		const Vector3d Q = A1.cross(A2);
		const Vector3d N = Q.normalized();
		const double Theta = Q.norm();

		const Matrix3d I1 = L1->GetInertia();
		const Matrix3d I2 = L2->GetInertia();

		const double W1 = L1->GetRotationalInverseMass(N);
		const double W2 = L2->GetRotationalInverseMass(N);

		const double A = 0.000001 / (H * H);
		const Vector3d P = -Theta / (W1 + W2 + A) * N;
		
		Vector3d T1 = I1.inverse() * P;
		Vector3d T2 = I2.inverse() * P;
		T1 = T2 = 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;

		// UE_LOG(LogTemp, Log, TEXT("%f | %f"), W1, W2);
	}
	
}

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; // Damping strength
	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()
{
}