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


#include "Link.h"

#include <iostream>

#include "util.h"
#include "Eigen/Dense"
#include "Engine/StaticMeshSocket.h"

using namespace Eigen;

Matrix3d ULink::GetInertia() const {
	return Orientation.toRotationMatrix() * Inertia_Tensor_Local;
}

double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const{
	const double M = Mass;
	const Matrix3d I = GetInertia();
	return 1 / M + R.cross(N).transpose() * I.inverse() * R.cross(N);
}
double ULink::GetRotationalInverseMass() const{
	return 0;
}

void ULink::Setup(){
	SetupJoints();
	SetupProperties();
}

void ULink::SetupProperties(){
	Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
	Position = ToEigen(GetComponentLocation());
	Velocity = Vector3d::Zero();
	Mass = IsBase ? INFINITY : CalculateMass();
	Orientation = Quaterniond::Identity();
	AngularVelocity = Vector3d::Zero();
}

void ULink::SetupJoints() const {
	if (const UStaticMeshSocket* Socket = GetSocketByName("JointPrev")){
		if (PrevJoint){
			PrevJoint->SecondLocalPosition = ToEigen(Socket->RelativeLocation);
			PrevJoint->SecondRotateAxis = ToEigen(Socket->RelativeRotation.Vector());
		}
	}
	if (const UStaticMeshSocket* Socket = GetSocketByName("JointNext")){
		if (NextJoint){
			NextJoint->FirstLocalPosition = ToEigen(Socket->RelativeLocation);
			NextJoint->FirstRotateAxis = ToEigen(Socket->RelativeRotation.Vector());
		}
	}
}

void ULink::Update(const double H){
	const Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0;
	
	Last_Position = Position;
	Velocity += H * ExtTransForce / Mass;
	Position += H * Velocity;
	
	Last_Orientation = Orientation;
	// Angle Velocity has no external contributor
	Vector3d w = AngularVelocity;
	Orientation += Quaterniond(0, w.x(), w.y(), w.z()) * Orientation * 0.5 * H;
	Orientation.normalize();
}

void ULink::Integrate(const double H){
	Velocity = (Position - Last_Position) / H;
	Quaterniond DeltaOrientation = Orientation * Last_Orientation.inverse();
	AngularVelocity = 2 * DeltaOrientation.vec() / H;
	AngularVelocity = DeltaOrientation.w() >= 0 ? AngularVelocity : -AngularVelocity;
}

void ULink::UpdateInternalTransform(){
	SetWorldLocation(FromEigen(Position));
	SetWorldRotation(FromEigen(Orientation));
}