// Copyright IHY.
#include "OptimizerReconciler.h"

namespace OptimizerReconciler
{
	bool ComputeCorrectedWorld(
		const FMatrix& CanonToMember,
		const FTransform& W,
		const FBox& CanonLocalBounds,
		double ShearTol,
		FTransform& OutWprime,
		double& OutMaxCornerDev)
	{
		// Exact corrected world matrix: apply D first (canon-local -> sibling-local), then W.
		const FMatrix Exact = CanonToMember * W.ToMatrixWithScale();
		OutWprime.SetFromMatrix(Exact);

		// Compare the FTransform reconstruction against the exact matrix at the canonical bbox corners.
		// Any gap means the exact matrix had shear that FTransform discarded -> not representable.
		const FVector Mn = CanonLocalBounds.Min;
		const FVector Mx = CanonLocalBounds.Max;
		const FVector Corners[8] =
		{
			FVector(Mn.X, Mn.Y, Mn.Z), FVector(Mx.X, Mn.Y, Mn.Z),
			FVector(Mn.X, Mx.Y, Mn.Z), FVector(Mx.X, Mx.Y, Mn.Z),
			FVector(Mn.X, Mn.Y, Mx.Z), FVector(Mx.X, Mn.Y, Mx.Z),
			FVector(Mn.X, Mx.Y, Mx.Z), FVector(Mx.X, Mx.Y, Mx.Z)
		};

		double MaxDev = 0.0;
		for (const FVector& C : Corners)
		{
			const FVector ExactPt = Exact.TransformPosition(C);
			const FVector ViaTransform = OutWprime.TransformPosition(C);
			MaxDev = FMath::Max(MaxDev, FVector::Dist(ExactPt, ViaTransform));
		}
		OutMaxCornerDev = MaxDev;
		return MaxDev <= ShearTol;
	}
}