// 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 {
	const Matrix3d R = Orientation.toRotationMatrix();
	return R * Inertia_Tensor_Local * R.transpose();
}

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 Vector3d N) const{
	const Matrix3d I = GetInertia();
	return N.transpose() * I.inverse() * N;
}

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

void ULink::SetupProperties(){
	
	Mass = CalculateMass();
	Position = ToEigen(GetComponentLocation());
	Velocity = Vector3d::Zero();
	Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
	//Inertia_Tensor_Local = Matrix3d::Identity() * Mass;
	Orientation = ToEigen(GetComponentQuat());
	AngularVelocity = Vector3d::Zero();

	if (IsBase){
		Mass = 1e50;
		Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e50;
	}
}

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){
	
	// Translation
	Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 1;
	if (IsBase)
		ExtTransForce = Vector3d::Zero();
	
	Last_Position = Position;
	Velocity += H * ExtTransForce / Mass;
	Position += H * Velocity;

	// Rotation
	const Vector3d ExtRotForce = Vector3d::Zero();
	
	Last_Orientation = Orientation;
	Vector3d w = AngularVelocity;
	AngularVelocity += H * GetInertia().inverse() * (ExtRotForce - w.cross(GetInertia() * w));
	w = AngularVelocity;
	Orientation = 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));
}