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

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

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

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

	const Matrix3d I = GetInertia().inverse();
	UE_LOG(LogTemp, Log, TEXT("%s"), *GetName());
	auto log = [this, I](int col){
		UE_LOG(LogTemp, Log, TEXT("%f %f %f"), I(col, 0), I(col, 1), I(col, 2));
	};
	log(0);log(1);log(2);
	
}

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){
	Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0;
	if (IsBase)
		ExtTransForce = Vector3d::Zero();
	
	Last_Position = Position;
	Velocity += H * ExtTransForce / Mass;
	Position += H * Velocity;
	
	Last_Orientation = Orientation;
	// Angle Velocity has no external contributor
	Vector3d 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));
}