using System.Collections.Generic;
using System.Numerics;
using Autodesk.Navisworks.Api;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    /// <summary>
    /// 统一的路径目标框架解析入口。
    /// 对象姿态解释层统一后，Ground/Hoisting/Rail 只在这里区分目标框架。
    /// </summary>
    public static class PathTargetFrameResolver
    {
        public static bool TryResolvePlanarStartHostForward(
            NavisworksTransport.PathType pathType,
            IReadOnlyList<Point3D> pathPoints,
            out Vector3 hostForward)
        {
            if (pathPoints == null)
            {
                hostForward = Vector3.Zero;
                return false;
            }

            var hostPoints = new List<Vector3>(pathPoints.Count);
            for (int i = 0; i < pathPoints.Count; i++)
            {
                hostPoints.Add(new Vector3(
                    (float)pathPoints[i].X,
                    (float)pathPoints[i].Y,
                    (float)pathPoints[i].Z));
            }

            return TryResolvePlanarStartHostForward(pathType, hostPoints, out hostForward);
        }

        public static bool TryResolvePlanarStartHostForward(
            NavisworksTransport.PathType pathType,
            IReadOnlyList<Vector3> pathPoints,
            out Vector3 hostForward)
        {
            hostForward = Vector3.Zero;

            if (pathPoints == null)
            {
                return false;
            }

            if (pathType == NavisworksTransport.PathType.Hoisting)
            {
                if (pathPoints.Count < 2)
                {
                    return false;
                }

                // 优先使用第一个水平段方向（点2-点1），2点路径使用唯一线段方向
                if (pathPoints.Count >= 3)
                {
                    hostForward = new Vector3(
                        (float)(pathPoints[2].X - pathPoints[1].X),
                        (float)(pathPoints[2].Y - pathPoints[1].Y),
                        (float)(pathPoints[2].Z - pathPoints[1].Z));
                }
                else
                {
                    hostForward = new Vector3(
                        (float)(pathPoints[1].X - pathPoints[0].X),
                        (float)(pathPoints[1].Y - pathPoints[0].Y),
                        (float)(pathPoints[1].Z - pathPoints[0].Z));
                }
                return hostForward.LengthSquared() >= 1e-9f;
            }

            if (pathPoints.Count < 2)
            {
                return false;
            }

            hostForward = new Vector3(
                (float)(pathPoints[1].X - pathPoints[0].X),
                (float)(pathPoints[1].Y - pathPoints[0].Y),
                (float)(pathPoints[1].Z - pathPoints[0].Z));
            return hostForward.LengthSquared() >= 1e-9f;
        }

        public static bool TryCreatePlanarStartHostFrame(
            NavisworksTransport.PathType pathType,
            IReadOnlyList<Point3D> pathPoints,
            CoordinateSystemType hostType,
            out PathTargetFrame frame)
        {
            if (pathPoints == null)
            {
                frame = null;
                return false;
            }

            var hostPoints = new List<Vector3>(pathPoints.Count);
            for (int i = 0; i < pathPoints.Count; i++)
            {
                hostPoints.Add(new Vector3(
                    (float)pathPoints[i].X,
                    (float)pathPoints[i].Y,
                    (float)pathPoints[i].Z));
            }

            return TryCreatePlanarStartHostFrame(pathType, hostPoints, hostType, out frame);
        }

        public static bool TryCreatePlanarStartHostFrame(
            NavisworksTransport.PathType pathType,
            IReadOnlyList<Vector3> pathPoints,
            CoordinateSystemType hostType,
            out PathTargetFrame frame)
        {
            frame = null;

            if (!TryResolvePlanarStartHostForward(pathType, pathPoints, out var hostForward))
            {
                return false;
            }

            return TryCreatePlanarHostFrame(hostForward, hostType, out frame);
        }

        public static bool TryCreatePlanarHostFrame(
            Vector3 hostForward,
            CoordinateSystemType hostType,
            out PathTargetFrame frame)
        {
            frame = null;

            if (hostForward.LengthSquared() < 1e-9f)
            {
                return false;
            }

            Vector3 hostUp = hostType == CoordinateSystemType.YUp
                ? Vector3.UnitY
                : Vector3.UnitZ;
            frame = new PathTargetFrame(hostForward, hostUp);
            return true;
        }

        public static bool TryResolvePlanarHostYaw(
            Vector3 hostForward,
            CoordinateSystemType hostType,
            out double yawRadians)
        {
            yawRadians = 0.0;

            if (hostForward.LengthSquared() < 1e-9f)
            {
                return false;
            }

            var adapter = new HostCoordinateAdapter(hostType);
            Vector3 canonicalForward = adapter.ToCanonicalVector3(hostForward);
            canonicalForward.Z = 0f;
            if (canonicalForward.LengthSquared() < 1e-9f)
            {
                return false;
            }

            canonicalForward = Vector3.Normalize(canonicalForward);
            yawRadians = System.Math.Atan2(canonicalForward.Y, canonicalForward.X);
            return true;
        }

        public static bool TryResolvePlanarStartHostYaw(
            NavisworksTransport.PathType pathType,
            IReadOnlyList<Vector3> pathPoints,
            CoordinateSystemType hostType,
            out double yawRadians)
        {
            yawRadians = 0.0;
            return TryResolvePlanarStartHostForward(pathType, pathPoints, out var hostForward) &&
                   TryResolvePlanarHostYaw(hostForward, hostType, out yawRadians);
        }

        public static bool TryCreateRailHostFrame(
            PathRoute route,
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            out PathTargetFrame frame)
        {
            frame = null;

            if (!RailPathPoseHelper.TryCreateHostLocalFrame(route, previousPoint, currentPoint, nextPoint, out var localFrame))
            {
                return false;
            }

            frame = new PathTargetFrame(localFrame.Forward, localFrame.Normal);
            return true;
        }
    }
}
