// 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::GetFirstWorldDirection() const{
	return FirstLink->Orientation * FirstLocalPosition;
}

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

Vector3d UJoint::GetConnection() const{
	return FirstLink->Position + GetFirstWorldDirection() - (SecondLink->Position + GetSecondWorldDirection());
}

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 = GetFirstWorldDirection();
		const Vector3d R2 = GetSecondWorldDirection();
		const Vector3d D = GetConnection();
		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 = 0; //1. / 100000000; // Compliance (inverse of stiffness)
		const double Denominator = W1 + W2 + A / (H * H);
		Vector3d P;
		if (Denominator == 0)
			P = Vector3d::Zero();
		else
			P = N * -C / Denominator;
	
		L1->Position += P / M1;
		L2->Position -= P / M2;
		
		Vector3d T1 = I1.inverse() * R1.cross(P);
		Vector3d T2 = I2.inverse() * R2.cross(P);
		const Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
		const Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
		L1->Orientation = (L1->Orientation + Add1).normalized();
		L2->Orientation = (L2->Orientation - Add2).normalized();
	}
	
	// Rotational Constraints
	{
		Vector3d Q;
		if (L2->IsEffector){
			const Quaterniond Tq = L1->Orientation * L2->Orientation.inverse();
			Q = -2 * Tq.vec();
		} else{
			const Vector3d A1 = GetFirstWorldAxis();
			const Vector3d A2 = GetSecondWorldAxis();
			assert(A1.norm() == A2.norm() == 1);
			Q = A1.cross(A2);
		}
		const Vector3d N = Q.normalized();
		const double Theta = UKismetMathLibrary::Asin(Q.norm());// * 180. / PI;

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

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

		constexpr double A = 0; //1. / 100000000;
		const double Denominator = W1 + W2 + A / (H * H);
		Vector3d P;
		if (Denominator == 0)
			P = Vector3d::Zero();
		else
			P = N * Theta / Denominator;
		
		Vector3d T1 = I1.inverse() * P;
		Vector3d T2 = I2.inverse() * P;
		const Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
		const Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
		L1->Orientation = (L1->Orientation + Add1).normalized();
		L2->Orientation = (L2->Orientation - Add2).normalized();
	}
	
}

void UJoint::SolveVelocity(const double H) const{
	ULink* L1 = FirstLink;
	ULink* L2 = SecondLink;
	
	// Positional Damping
	{
		const Vector3d V1 = L1->Velocity;
		const Vector3d V2 = L2->Velocity;

		const Vector3d R1 = GetFirstWorldDirection();
		const Vector3d R2 = GetSecondWorldDirection();
	
		constexpr double MuLin = 1000; // Damping strength
		const Vector3d DeltaV = (V2 - V1) * (MuLin * H < 1 ? MuLin * H : 1);
		
		const Vector3d N = GetConnection().normalized();
	
		const double W1 = L1->GetPositionalInverseMass(R1, N);
		const double W2 = L2->GetPositionalInverseMass(R2, N);
		
		const Vector3d P = DeltaV / (W1 + W2);
	
		//L1->Velocity += P / L1->Mass;
		//L2->Velocity -= P / L2->Mass;

		const Matrix3d I1 = L1->GetInertia();
		const Matrix3d I2 = L2->GetInertia();
		
		//L1->AngularVelocity += I1.inverse() * R1.cross(P);
		//L2->AngularVelocity -= I2.inverse() * R2.cross(P);

		constexpr double A = 7;
		const double Damp = 1 - A * H;

		L1->Velocity *= Damp;
		L2->Velocity *= Damp;
		L1->AngularVelocity *= Damp;
		L2->AngularVelocity *= Damp;
	}
	
	// Rotational Damping
	{
		//constexpr double MuAng = 10000;
		//const Vector3d DeltaWV = (WV2 - WV1) * (MuAng * H < 1 ? MuAng * H : 1);

		
	}
}

UJoint::UJoint()
{
}