#pragma once

#include "Eigen/Dense"

inline Vector3d ToEigen(const FVector V){
	return Vector3d(V.X, V.Y, V.Z);
}

inline Quaterniond ToEigen(const FQuat Q){
	return Quaterniond(Q.W, Q.X, Q.Y, Q.Z);
}

inline FVector FromEigen(const Vector3d& V){
	return FVector(V.x(), V.y(), V.z());
}

inline FQuat FromEigen(const Quaterniond& Q){
	return FQuat(Q.x(), Q.y(), Q.z(), Q.w());
}

inline Quaterniond operator* (const Quaterniond& Q, const double D){
	return Quaterniond(Q.w() * D, Q.x() * D, Q.y() * D, Q.z() * D);
}

inline Quaterniond operator+ (const Quaterniond& Q1, const Quaterniond& Q2){
	return Quaterniond(Q1.w() + Q2.w(), Q1.x() + Q2.x(), Q1.y() + Q2.y(), Q1.z() + Q2.z());
}

inline Quaterniond operator- (const Quaterniond& Q1, const Quaterniond& Q2){
	return Quaterniond(Q1.w() - Q2.w(), Q1.x() - Q2.x(), Q1.y() - Q2.y(), Q1.z() - Q2.z());
}

inline void Log(const MatrixXd& M){
	assert(M.rows() == 3);
	if (M.cols() == 3){
		UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(0, 0), M(0, 1), M(0, 2));
		UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(1, 0), M(1, 1), M(1, 2));
		UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(2, 0), M(2, 1), M(2, 2));
	}
	if (M.cols() == 1){
		UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(0), M(1), M(2));
	}
}

inline void Log(const double V){
	UE_LOG(LogTemp, Log, TEXT("%f"), V);
}

inline void Log(const char* S){
	UE_LOG(LogTemp, Log, TEXT("%s"), *FString(S));
}

inline void Log(const Quaterniond& Q){
	UE_LOG(LogTemp, Log, TEXT("%f %f %f | %f"), Q.x(), Q.y(), Q.z(), Q.w());
}