using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.VisualStudio.TestTools.UnitTesting;

namespace NavisworksTransport.UnitTests.Core
{
    #region 简单的几何类型（不依赖 Navisworks API）

    /// <summary>
    /// 简单的 3D 点结构体（用于测试）
    /// </summary>
    public struct TestPoint3D : IEquatable<TestPoint3D>
    {
        public double X { get; set; }
        public double Y { get; set; }
        public double Z { get; set; }

        public TestPoint3D(double x, double y, double z)
        {
            X = x;
            Y = y;
            Z = z;
        }

        public static TestPoint3D operator +(TestPoint3D p, TestVector3D v)
        {
            return new TestPoint3D(p.X + v.X, p.Y + v.Y, p.Z + v.Z);
        }

        public static TestPoint3D operator -(TestPoint3D p1, TestPoint3D p2)
        {
            return new TestPoint3D(p1.X - p2.X, p1.Y - p2.Y, p1.Z - p2.Z);
        }

        public static TestPoint3D operator *(TestPoint3D p, double scalar)
        {
            return new TestPoint3D(p.X * scalar, p.Y * scalar, p.Z * scalar);
        }

        public double Length
        {
            get { return Math.Sqrt(X * X + Y * Y + Z * Z); }
        }

        public bool Equals(TestPoint3D other)
        {
            return Math.Abs(X - other.X) < 1e-10 &&
                   Math.Abs(Y - other.Y) < 1e-10 &&
                   Math.Abs(Z - other.Z) < 1e-10;
        }

        public override bool Equals(object obj)
        {
            return obj is TestPoint3D && Equals((TestPoint3D)obj);
        }

        public override int GetHashCode()
        {
            return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
        }

        public static bool operator ==(TestPoint3D p1, TestPoint3D p2)
        {
            return p1.Equals(p2);
        }

        public static bool operator !=(TestPoint3D p1, TestPoint3D p2)
        {
            return !p1.Equals(p2);
        }
    }

    /// <summary>
    /// 简单的 3D 向量结构体（用于测试）
    /// </summary>
    public struct TestVector3D
    {
        public double X { get; set; }
        public double Y { get; set; }
        public double Z { get; set; }

        public TestVector3D(double x, double y, double z)
        {
            X = x;
            Y = y;
            Z = z;
        }

        public TestVector3D(TestPoint3D point)
        {
            X = point.X;
            Y = point.Y;
            Z = point.Z;
        }

        public static TestVector3D operator +(TestVector3D v1, TestVector3D v2)
        {
            return new TestVector3D(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
        }

        public static TestVector3D operator -(TestVector3D v1, TestVector3D v2)
        {
            return new TestVector3D(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
        }

        public static TestVector3D operator *(TestVector3D v, double scalar)
        {
            return new TestVector3D(v.X * scalar, v.Y * scalar, v.Z * scalar);
        }

        public static TestVector3D operator *(double scalar, TestVector3D v)
        {
            return v * scalar;
        }

        public double Length
        {
            get { return Math.Sqrt(X * X + Y * Y + Z * Z); }
        }

        public TestVector3D Normalize()
        {
            double len = Length;
            if (len < 1e-10)
                return new TestVector3D(0, 0, 0);
            return new TestVector3D(X / len, Y / len, Z / len);
        }

        public static double DotProduct(TestVector3D v1, TestVector3D v2)
        {
            return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z;
        }

        public static TestVector3D CrossProduct(TestVector3D v1, TestVector3D v2)
        {
            return new TestVector3D(
                v1.Y * v2.Z - v1.Z * v2.Y,
                v1.Z * v2.X - v1.X * v2.Z,
                v1.X * v2.Y - v1.Y * v2.X
            );
        }
    }

    /// <summary>
    /// 测试用圆弧轨迹数据
    /// </summary>
    public class TestArcTrajectory
    {
        public TestPoint3D Ts { get; set; }           // 进入切点
        public TestPoint3D Te { get; set; }           // 退出切点
        public TestPoint3D ArcCenter { get; set; }    // 圆心
        public double RequestedRadius { get; set; }   // 请求半径
        public double ActualRadius { get; set; }      // 实际半径
        public double DeflectionAngle { get; set; }   // 偏转角（弧度）
        public double ArcLength { get; set; }         // 圆弧长度
    }

    #endregion

    /// <summary>
    /// PathCurveEngine 独立测试版本（不依赖 Navisworks API）
    /// 用于测试核心算法逻辑的正确性
    /// </summary>
    public static class TestPathCurveEngine
    {
        private static double Clamp(double value, double min, double max)
        {
            if (value < min) return min;
            if (value > max) return max;
            return value;
        }

        /// <summary>
        /// 计算圆弧切点和轨迹参数
        /// </summary>
        public static TestArcTrajectory CalculateFillet(
            TestPoint3D pPrev,
            TestPoint3D pCurr,
            TestPoint3D pNext,
            double turnRadius)
        {
            // 1. 计算单位向量
            TestVector3D v1 = new TestVector3D(pPrev - pCurr).Normalize();
            TestVector3D v2 = new TestVector3D(pNext - pCurr).Normalize();

            // 2. 计算夹角 α
            double cosAlpha = TestVector3D.DotProduct(v1, v2);
            double angleRad = Math.Acos(Clamp(cosAlpha, -1.0, 1.0));

            // 如果几乎共线，返回无效轨迹
            if (angleRad < 0.01 || angleRad > Math.PI - 0.01)
            {
                return new TestArcTrajectory
                {
                    Ts = pCurr,
                    Te = pCurr,
                    ArcCenter = pCurr,
                    RequestedRadius = turnRadius,
                    ActualRadius = 0,
                    DeflectionAngle = angleRad,
                    ArcLength = 0
                };
            }

            // 3. 计算切线长 Lt = R / tan(α/2)
            double Lt = turnRadius / Math.Tan(angleRad / 2.0);

            // 4. 安全截断检查（限制为边长的45%）
            double seg1Length = (pPrev - pCurr).Length;
            double seg2Length = (pNext - pCurr).Length;
            double maxAllowedLt = Math.Min(seg1Length, seg2Length) * 0.45;

            double actualRadius = turnRadius;
            if (Lt > maxAllowedLt)
            {
                Lt = maxAllowedLt;
                actualRadius = Lt * Math.Tan(angleRad / 2.0);
            }

            // 5. 计算切点
            TestPoint3D ts = pCurr + v1 * Lt;
            TestPoint3D te = pCurr + v2 * Lt;

            // 6. 计算圆心（使用角平分线）
            TestVector3D bisector = (v1 + v2).Normalize();
            double distToCenter = actualRadius / Math.Sin(angleRad / 2.0);
            TestPoint3D arcCenter = pCurr + bisector * distToCenter;

            // 7. 计算圆弧长度
            double arcLength = actualRadius * angleRad;

            return new TestArcTrajectory
            {
                Ts = ts,
                Te = te,
                ArcCenter = arcCenter,
                RequestedRadius = turnRadius,
                ActualRadius = actualRadius,
                DeflectionAngle = angleRad,
                ArcLength = arcLength
            };
        }

        /// <summary>
        /// 采样圆弧为离散点序列
        /// </summary>
        public static List<TestPoint3D> SampleArc(TestArcTrajectory trajectory, double samplingStep)
        {
            var points = new List<TestPoint3D>();

            if (trajectory.ActualRadius < 1e-10 || trajectory.ArcLength < 1e-10)
            {
                points.Add(trajectory.Ts);
                points.Add(trajectory.Te);
                return points;
            }

            // 计算采样点数量
            int numSamples = Math.Max(2, (int)Math.Ceiling(trajectory.ArcLength / samplingStep));

            // 计算起始和结束角度
            TestVector3D vStart = new TestVector3D(trajectory.Ts - trajectory.ArcCenter).Normalize();
            TestVector3D vEnd = new TestVector3D(trajectory.Te - trajectory.ArcCenter).Normalize();

            // 计算旋转轴（使用叉积）
            TestVector3D axis = TestVector3D.CrossProduct(vStart, vEnd).Normalize();

            // 生成采样点
            for (int i = 0; i <= numSamples; i++)
            {
                double t = (double)i / numSamples;
                double angle = t * trajectory.DeflectionAngle;
                TestPoint3D p = RotatePointAroundAxis(trajectory.Ts, trajectory.ArcCenter, axis, angle);
                points.Add(p);
            }

            // 确保最后一个点是退出切点
            points[points.Count - 1] = trajectory.Te;

            return points;
        }

        /// <summary>
        /// 绕轴旋转点（罗德里格斯旋转公式）
        /// </summary>
        private static TestPoint3D RotatePointAroundAxis(
            TestPoint3D point,
            TestPoint3D center,
            TestVector3D axis,
            double angle)
        {
            TestVector3D v = new TestVector3D(point - center);
            TestVector3D kxv = TestVector3D.CrossProduct(axis, v);
            double kdv = TestVector3D.DotProduct(axis, v);

            TestVector3D vRot = v * Math.Cos(angle) + kxv * Math.Sin(angle) + axis * kdv * (1 - Math.Cos(angle));
            return center + vRot;
        }
    }

    /// <summary>
    /// PathCurveEngine 核心算法独立测试
    /// 测试路径曲线化算法的正确性（不依赖 Navisworks API）
    /// </summary>
    [TestClass]
    public class PathCurveEngineStandaloneTests
    {
        #region CalculateFillet 测试

        [TestMethod]
        public void Standalone_CalculateFillet_RightAngleTurn_ReturnsValidArc()
        {
            // Arrange - 创建90度直角转弯
            var pPrev = new TestPoint3D(0, 10, 0);  // 上方点
            var pCurr = new TestPoint3D(0, 0, 0);   // 转弯点
            var pNext = new TestPoint3D(10, 0, 0);  // 右方点
            double turnRadius = 2.0;

            // Act
            var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.AreEqual(turnRadius, trajectory.RequestedRadius, 0.01, "请求半径应该正确");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
            Assert.IsTrue(trajectory.DeflectionAngle > 0, "偏转角应该大于0");
            Assert.IsTrue(trajectory.ArcLength > 0, "圆弧长度应该大于0");

            // 验证切点位置
            Assert.IsTrue(trajectory.Ts.Y > 0, "进入切点应该在转弯点上方");
            Assert.IsTrue(Math.Abs(trajectory.Ts.X) < 0.01, "进入切点X坐标应该接近0");
            Assert.IsTrue(trajectory.Te.X > 0, "退出切点应该在转弯点右侧");
            Assert.IsTrue(Math.Abs(trajectory.Te.Y) < 0.01, "退出切点Y坐标应该接近0");
        }

        [TestMethod]
        public void Standalone_CalculateFillet_AcuteAngleTurn_ReturnsValidArc()
        {
            // Arrange - 创建锐角转弯（约60度）
            var pPrev = new TestPoint3D(0, 10, 0);
            var pCurr = new TestPoint3D(0, 0, 0);
            var pNext = new TestPoint3D(8.66, 5, 0);  // 60度方向
            double turnRadius = 2.0;

            // Act
            var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
            Assert.IsTrue(trajectory.DeflectionAngle > 0.5 && trajectory.DeflectionAngle < 1.5, "偏转角应该在60度左右");
        }

        [TestMethod]
        public void Standalone_CalculateFillet_ObtuseAngleTurn_ReturnsValidArc()
        {
            // Arrange - 创建钝角转弯（约120度）
            var pPrev = new TestPoint3D(0, 10, 0);
            var pCurr = new TestPoint3D(0, 0, 0);
            var pNext = new TestPoint3D(8.66, -5, 0);  // 120度方向
            double turnRadius = 2.0;

            // Act
            var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
            Assert.IsTrue(trajectory.DeflectionAngle > 1.5 && trajectory.DeflectionAngle < 2.5, "偏转角应该在120度左右");
        }

        [TestMethod]
        public void Standalone_CalculateFillet_CollinearPoints_ReturnsInvalidArc()
        {
            // Arrange - 创建共线点（几乎直线）
            var pPrev = new TestPoint3D(0, 10, 0);
            var pCurr = new TestPoint3D(0, 0, 0);
            var pNext = new TestPoint3D(0, -10, 0);  // 同一直线
            double turnRadius = 2.0;

            // Act
            var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.AreEqual(0, trajectory.ActualRadius, 0.01, "实际半径应该为0");
            Assert.AreEqual(0, trajectory.ArcLength, 0.01, "圆弧长度应该为0");
            Assert.AreEqual(pCurr, trajectory.Ts, "切点应该与转弯点相同");
        }

        [TestMethod]
        public void Standalone_CalculateFillet_SafetyTruncation_AdjustsRadius()
        {
            // Arrange - 创建短边场景，需要安全截断
            var pPrev = new TestPoint3D(0, 1, 0);   // 短边
            var pCurr = new TestPoint3D(0, 0, 0);
            var pNext = new TestPoint3D(1, 0, 0);   // 短边
            double turnRadius = 2.0;  // 半径大于边长

            // Act
            var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.IsTrue(trajectory.ActualRadius < turnRadius, "实际半径应该小于请求半径");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
        }

        [TestMethod]
        public void Standalone_CalculateFillet_3DTurn_ReturnsValidArc()
        {
            // Arrange - 创建3D空间中的转弯
            var pPrev = new TestPoint3D(0, 10, 0);
            var pCurr = new TestPoint3D(0, 0, 0);
            var pNext = new TestPoint3D(10, 0, 5);  // 有Z轴变化
            double turnRadius = 2.0;

            // Act
            var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
            Assert.IsTrue(Math.Abs(trajectory.ArcCenter.Z) > 0.01, "圆心Z坐标应该不为0");
        }

        #endregion

        #region SampleArc 测试

        [TestMethod]
        public void Standalone_SampleArc_WithValidTrajectory_ReturnsSampledPoints()
        {
            // Arrange
            var trajectory = new TestArcTrajectory
            {
                Ts = new TestPoint3D(0, 2, 0),
                Te = new TestPoint3D(2, 0, 0),
                ArcCenter = new TestPoint3D(0, 0, 0),
                ActualRadius = 2.0,
                DeflectionAngle = Math.PI / 2,  // 90度
                ArcLength = Math.PI  // 半圆周长
            };
            double samplingStep = 0.5;

            // Act
            var sampledPoints = TestPathCurveEngine.SampleArc(trajectory, samplingStep);

            // Assert
            Assert.IsNotNull(sampledPoints, "采样点列表不应该为null");
            Assert.IsTrue(sampledPoints.Count >= 2, "采样点数量应该至少为2");
            Assert.AreEqual(trajectory.Ts, sampledPoints.First(), "第一个点应该是进入切点");
            Assert.AreEqual(trajectory.Te, sampledPoints.Last(), "最后一个点应该是退出切点");
        }

        [TestMethod]
        public void Standalone_SampleArc_SmallArcLength_ReturnsThreePoints()
        {
            // Arrange
            var trajectory = new TestArcTrajectory
            {
                Ts = new TestPoint3D(0, 0.1, 0),
                Te = new TestPoint3D(0.1, 0, 0),
                ArcCenter = new TestPoint3D(0, 0, 0),
                ActualRadius = 0.1,
                DeflectionAngle = 0.1,
                ArcLength = 0.01  // 非常小的圆弧
            };
            double samplingStep = 0.05;

            // Act
            var sampledPoints = TestPathCurveEngine.SampleArc(trajectory, samplingStep);

            // Assert
            Assert.IsNotNull(sampledPoints, "采样点列表不应该为null");
            Assert.AreEqual(3, sampledPoints.Count, "小圆弧应该返回3个点（起点、中间点、终点）");
        }

        [TestMethod]
        public void Standalone_SampleArc_SamplingStep0_05_ReturnsCorrectCount()
        {
            // Arrange
            var trajectory = new TestArcTrajectory
            {
                Ts = new TestPoint3D(0, 2, 0),
                Te = new TestPoint3D(2, 0, 0),
                ArcCenter = new TestPoint3D(0, 0, 0),
                ActualRadius = 2.0,
                DeflectionAngle = Math.PI / 2,
                ArcLength = Math.PI
            };
            double samplingStep = 0.05;

            // Act
            var sampledPoints = TestPathCurveEngine.SampleArc(trajectory, samplingStep);

            // Assert
            int expectedCount = (int)Math.Ceiling(Math.PI / 0.05) + 1;
            Assert.IsTrue(sampledPoints.Count >= expectedCount - 2 && sampledPoints.Count <= expectedCount + 2,
                $"采样点数量应该在 {expectedCount - 2} 到 {expectedCount + 2} 之间");
        }

        #endregion

        #region 集成测试

        [TestMethod]
        public void FullWorkflow_RightAngleTurn_CalculatesCorrectTrajectory()
        {
            // Arrange - 创建一个90度转弯
            var pPrev = new TestPoint3D(0, 10, 0);
            var pCurr = new TestPoint3D(0, 0, 0);
            var pNext = new TestPoint3D(10, 0, 0);
            double turnRadius = 2.0;

            // Act - 计算轨迹
            var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // 采样圆弧
            var sampledPoints = TestPathCurveEngine.SampleArc(trajectory, 0.5);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");

            // 验证圆弧长度
            Assert.IsTrue(trajectory.ArcLength > 0 && trajectory.ArcLength < 10, "圆弧长度应该合理");

            // 验证采样点
            Assert.IsNotNull(sampledPoints, "采样点不应该为null");
            Assert.IsTrue(sampledPoints.Count > 2, "采样点数量应该大于2");

            // 验证所有采样点到圆心的距离应该接近半径
            foreach (var point in sampledPoints)
            {
                double dist = (point - trajectory.ArcCenter).Length;
                Assert.IsTrue(Math.Abs(dist - trajectory.ActualRadius) < 0.1, 
                    $"采样点到圆心的距离应该接近半径: {dist} vs {trajectory.ActualRadius}");
            }
        }

        [TestMethod]
        public void FullWorkflow_AcuteAngleTurn_CalculatesCorrectTrajectory()
        {
            // Arrange - 创建一个60度锐角转弯
            var pPrev = new TestPoint3D(0, 10, 0);
            var pCurr = new TestPoint3D(0, 0, 0);
            var pNext = new TestPoint3D(8.66, 5, 0);
            double turnRadius = 2.0;

            // Act
            var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
            var sampledPoints = TestPathCurveEngine.SampleArc(trajectory, 0.5);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.IsTrue(trajectory.DeflectionAngle > 0.5 && trajectory.DeflectionAngle < 1.5, 
                "偏转角应该在60度左右");
            Assert.IsTrue(sampledPoints.Count > 2, "采样点数量应该大于2");
        }

        #endregion
    }
}