using System.Numerics;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    /// <summary>
    /// 在 Canonical Space 中，将路径参考点解析为动画跟踪中心点。
    /// 统一表达“参考点位于物体法向哪一侧接触面”的语义。
    /// </summary>
    public static class CanonicalTrackedPositionResolver
    {
        /// <summary>
        /// 将任意法向上的参考点解析为几何中心。
        /// referenceContactFactor 的语义：
        /// 0 = 参考点位于底部/负法向接触面
        /// 1 = 参考点位于顶部/正法向接触面
        /// 中心点偏移 = normal * height * (0.5 - referenceContactFactor)
        /// </summary>
        public static Vector3 ResolveCenterFromContactReference(
            Vector3 referencePoint,
            Vector3 normal,
            double objectHeight,
            double referenceContactFactor)
        {
            return referencePoint + Vector3.Normalize(normal) * (float)(objectHeight * (0.5 - referenceContactFactor));
        }

        public static Vector3 ResolveGroundTrackedCenter(
            Vector3 groundContactPoint,
            Vector3 canonicalUp,
            double objectHeight)
        {
            return ResolveCenterFromContactReference(groundContactPoint, canonicalUp, objectHeight, 0.0);
        }

        public static Vector3 ResolveTopSuspensionTrackedCenter(
            Vector3 suspensionReferencePoint,
            Vector3 canonicalUp,
            double objectHeight)
        {
            return ResolveCenterFromContactReference(suspensionReferencePoint, canonicalUp, objectHeight, 1.0);
        }
    }
}
