using MathNet.Numerics.LinearAlgebra;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace TeamAAS_VP.Core
{
    /// <summary>
    /// 提供多种方法用于计算由四个角点定义的矩形中心点以及相关矩形参数的辅助类。
    /// 该类包含基于对角线优化、PCA（主成分分析）、RANSAC 与最小二乘拟合的不同策略，
    /// 以应对不同噪声和异常值情形。注意输入假定为三维向量（长度为 3 的向量）。
    /// </summary>
    public class RectangleCenterCalculator
    {
        /// <summary>
        /// 使用最小二乘法思想计算最优矩形中心点（对外统一入口）。
        /// 调用当前实现中更鲁棒的对角线优化方法来估计中心。
        /// </summary>
        /// <param name="corners">矩形的四个角点坐标，每个点为长度为 3 的 <see cref="Vector{Double}"/>。</param>
        /// <returns>返回估计的中心点坐标（长度为 3 的向量）。</returns>
        /// <exception cref="ArgumentException">当 <paramref name="corners"/> 为 null 或数量不等于 4 时抛出。</exception>
        public static Vector<double> CalculateOptimalCenter(List<Vector<double>> corners)
        {
            if (corners == null || corners.Count != 4)
                throw new ArgumentException("需要4个角点");

            // 方法1：利用矩形对角线相交于中心点的性质
            return CalculateByDiagonalOptimization(corners);
        }

        /// <summary>
        /// 方法1：通过对角线中点的组合优化来估计中心点。
        /// 由于角点测量可能存在噪声，函数尝试不同的对角线组合，并采用使得四个角点到候选中心距离方差最小的候选中心。
        /// </summary>
        /// <param name="corners">四个角点坐标。</param>
        /// <returns>估计的中心点向量（长度为 3）。</returns>
        private static Vector<double> CalculateByDiagonalOptimization(List<Vector<double>> corners)
        {
            // 由于点可能有噪声，尝试所有可能的对角线组合
            // 对角线应该是相对的点
            var bestCenter = Vector<double>.Build.Dense(3);
            double minError = double.MaxValue;

            // 常见的对角线组合（这里列出三种互不等价的配对）
            var diagonalPairs = new List<(int, int, int, int)>
        {
            (0, 2, 1, 3), // p1-p3, p2-p4
            (0, 1, 2, 3), // p1-p2, p3-p4
            (0, 3, 1, 2)  // p1-p4, p2-p3
        };

            foreach (var (i1, i2, i3, i4) in diagonalPairs)
            {
                // 计算两条对角线的中点
                var mid1 = (corners[i1] + corners[i2]) / 2.0;
                var mid2 = (corners[i3] + corners[i4]) / 2.0;

                // 计算两个中点的平均值作为候选中心
                var candidate = (mid1 + mid2) / 2.0;

                // 计算误差：中心到四个角点的距离方差
                double error = CalculateCenterError(candidate, corners);

                if (error < minError)
                {
                    minError = error;
                    bestCenter = candidate;
                }
            }

            return bestCenter;
        }

        /// <summary>
        /// 计算给定中心到四个角点距离的方差，作为中心优劣的度量（方差越小越优）。
        /// </summary>
        /// <param name="center">待评估的中心点向量（长度为 3）。</param>
        /// <param name="corners">四个角点坐标。</param>
        /// <returns>距离的方差值（double）。</returns>
        private static double CalculateCenterError(Vector<double> center, List<Vector<double>> corners)
        {
            // 计算中心到每个角点的距离
            var distances = corners.Select(c => Distance(center, c)).ToList();

            // 计算距离的方差 - 越小说明中心越优
            double mean = distances.Average();
            double variance = distances.Select(d => Math.Pow(d - mean, 2)).Average();

            return variance;
        }

        /// <summary>
        /// 计算两个向量之间的欧几里得距离（L2 范数）。
        /// </summary>
        /// <param name="a">向量 a（长度为 3）。</param>
        /// <param name="b">向量 b（长度为 3）。</param>
        /// <returns>返回 a 与 b 之间的 L2 距离。</returns>
        private static double Distance(Vector<double> a, Vector<double> b)
        {
            return (a - b).L2Norm();
        }

        /// <summary>
        /// 方法2：使用 PCA（主成分分析）找到矩形的主方向，然后根据在主轴上的最小/最大投影值计算中心位置。
        /// 适合点分布呈矩形、噪声较低的情况。
        /// </summary>
        /// <param name="corners">四个角点的坐标列表。</param>
        /// <returns>在原始坐标系中的估计中心点向量（长度为 3）。</returns>
        public static Vector<double> CalculateByPCA(List<Vector<double>> corners)
        {
            // 将点转换为矩阵
            var matrix = Matrix<double>.Build.DenseOfRows(corners);

            // 计算均值（初步中心估计）
            var mean = Vector<double>.Build.DenseOfEnumerable(
                Enumerable.Range(0, 3).Select(i => matrix.Column(i).Average()));

            // 中心化数据
            var centered = matrix.Clone();
            for (int i = 0; i < 4; i++)
            {
                centered.SetRow(i, centered.Row(i) - mean);
            }

            // 计算协方差矩阵（样本协方差：注意除以 n-1 或 n，当前实现使用除以 3）
            var covariance = centered.Transpose() * centered / 3.0;

            // 特征值分解（PCA）
            var evd = covariance.Evd();

            // 主方向（前两个特征向量）
            var axis1 = evd.EigenVectors.Column(0);
            var axis2 = evd.EigenVectors.Column(1);

            // 将点投影到两个主轴上
            var proj1 = corners.Select(p => (p - mean).DotProduct(axis1)).ToList();
            var proj2 = corners.Select(p => (p - mean).DotProduct(axis2)).ToList();

            // 在每个轴上找到最小和最大值
            double min1 = proj1.Min();
            double max1 = proj1.Max();
            double min2 = proj2.Min();
            double max2 = proj2.Max();

            // 计算在主轴上的中心位置（即区间中点）
            double center1 = (min1 + max1) / 2.0;
            double center2 = (min2 + max2) / 2.0;

            // 转换回原始坐标系得到中心点
            var center = mean + axis1 * center1 + axis2 * center2;

            return center;
        }

        /// <summary>
        /// 方法3：使用最小二乘法直接拟合矩形并返回参数（简化实现）。
        /// 这里返回的是基于 PCA 的初始估计以及默认轴向量，作为示例接口。
        /// </summary>
        /// <param name="corners">四个角点坐标。</param>
        /// <returns>
        /// 返回一个元组，包含：
        /// <see cref="Vector{Double}"/> Center - 估计的中心点；
        /// <see cref="Vector{Double}"/> Axis1 - 第一个轴（单位方向向量，当前示例返回笛卡尔 X 轴）;
        /// <see cref="Vector{Double}"/> Axis2 - 第二个轴（单位方向向量，当前示例返回笛卡尔 Y 轴）。
        /// </returns>
        public static (Vector<double> Center, Vector<double> Axis1, Vector<double> Axis2) FitRectangleByLeastSquares(List<Vector<double>> corners)
        {
            // 步骤1：计算初始质心
            var initialCenter = Vector<double>.Build.DenseOfEnumerable(
                Enumerable.Range(0, 3).Select(i => corners.Select(c => c[i]).Average()));

            // 步骤2：使用Levenberg-Marquardt优化
            // 定义优化问题：找到中心、两个轴方向、长度和宽度
            // 这里简化实现，使用PCA结果作为初始值

            var pcaCenter = CalculateByPCA(corners);

            // 计算平均偏差（用于诊断输出）
            var errors = corners.Select(c => (c - pcaCenter).L2Norm()).ToList();
            double avgError = errors.Average();

            Console.WriteLine($"PCA中心点: [{pcaCenter[0]:F4}, {pcaCenter[1]:F4}, {pcaCenter[2]:F4}]");
            Console.WriteLine($"平均误差: {avgError:F6}");

            // 返回简化的轴向量（占位，实际实现可替换为优化得到的方向）
            var axis1 = Vector<double>.Build.Dense(new[] { 1.0, 0.0, 0.0 });
            var axis2 = Vector<double>.Build.Dense(new[] { 0.0, 1.0, 0.0 });

            return (pcaCenter, axis1, axis2);
        }

        /// <summary>
        /// 方法4：使用 RANSAC（随机采样一致性）估计中心，适用于存在异常点的情况。
        /// 随机选择两点作为候选对角线端点，统计满足阈值的内点数量，选择内点最多的候选中心，并用内点重计算中心。
        /// </summary>
        /// <param name="corners">四个角点坐标。</param>
        /// <param name="iterations">RANSAC 迭代次数，越大越稳健但计算开销越高。默认 100。</param>
        /// <param name="threshold">判断内点的距离阈值（以与坐标单位一致的长度度量）。默认 0.01。</param>
        /// <returns>估计得到的中心点向量（长度为 3）。若没有足够内点则返回在迭代过程中最佳的候选中心（可能为零向量）。</returns>
        public static Vector<double> CalculateByRANSAC(List<Vector<double>> corners,int iterations = 100, double threshold = 0.01)
        {
            var bestCenter = Vector<double>.Build.Dense(3);
            int bestInliers = 0;

            var random = new Random();

            for (int i = 0; i < iterations; i++)
            {
                // 随机选择两个点作为对角线端点
                int idx1 = random.Next(4);
                int idx2;
                do
                {
                    idx2 = random.Next(4);
                } while (idx2 == idx1);

                // 计算对角线中点
                var candidateCenter = (corners[idx1] + corners[idx2]) / 2.0;

                // 统计内点（距离接近的点）
                int inliers = corners.Count(c =>
                    (c - candidateCenter).L2Norm() < threshold);

                if (inliers > bestInliers)
                {
                    bestInliers = inliers;
                    bestCenter = candidateCenter;
                }
            }

            // 使用所有内点重新计算中心（若存在内点）
            var inlierPoints = corners.Where(c =>
                (c - bestCenter).L2Norm() < threshold).ToList();

            if (inlierPoints.Count > 0)
            {
                bestCenter = Vector<double>.Build.DenseOfEnumerable(
                    Enumerable.Range(0, 3)
                        .Select(dim => inlierPoints.Select(p => p[dim]).Average()));
            }

            return bestCenter;
        }
    }
}