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

// Inertia tensor in world space
Matrix3d ULink::GetMomentOfInertia() const {
	return GetRotationMatrix() * Inertia_Tensor_Local;
}

Matrix3d ULink::GetRotationMatrix() const{
	const FQuat Q = this->GetRelativeRotation().Quaternion();
	return Quaterniond(Q.W, Q.X, Q.Y, Q.Z).toRotationMatrix();
}

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

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

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;
	
	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::UpdateVisualTransform(){
	SetWorldLocation(FromEigen(Position));
	SetWorldRotation(FromEigen(Orientation));
}