using HalconDotNet;
using LB_VisionProcesses.Alogrithms.Halcon;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using OpenCvSharp;
using Size = OpenCvSharp.Size;
using Point = OpenCvSharp.Point;
using static LB_VisionProcesses.Alogrithms.OpenCvSharp.FindLineTool;
using System.Diagnostics;
using LB_VisionControl;

namespace LB_VisionProcesses.Alogrithms.OpenCvSharp
{
    public enum Transition { Positive, Negative, Ignore }

    public enum Selector { First, Last, Best }
    [Process("OpenCvSharp_找线工具", Category = "OpenCvSharp工具", Description = "创建OpenCvSharp_找线工具")]
    public class FindLineTool : TAlgorithm
    {
        public FindLineTool()
        {
            strProcessClass = "LB_VisionProcesses.Alogrithms.OpenCvSharp.FindLineTool";
            strProcessName = "OpenCvSharp_找线工具";

            Params.Inputs.Add("卡尺数量", 6);
            Params.Inputs.Add("卡尺长度", 30);
            Params.Inputs.Add("卡尺宽度", 10);
            Params.Inputs.Add("过滤一半像素", 2);
            Params.Inputs.Add("对比度阈值", 5);
            Params.Inputs.Add("极性", "Ignore");
            Params.Inputs.Add("边缘位置", "Best");
            Params.Inputs.Add("忽略点数", 0);

            Params.Outputs.Add("X", new List<double>());
            Params.Outputs.Add("Y", new List<double>());
            Params.Outputs.Add("CenterX", 0.0);
            Params.Outputs.Add("CenterY", 0.0);
            Params.Outputs.Add("Phi", 0.0);
            Params.Outputs.Add("Angle", 0.0);
            Params.Outputs.Add("Count", 0);
            Params.Outputs.Add("Segment", new HSegment());

            Params.ROI = new HSegment(0, 0, 250, 250);
        }

        /// <summary>
        /// 算子逻辑
        /// </summary>
        public override void TAlgorithmMain()
        {
            #region 初始化变量
            HObject ho_Regions, ho_LineXld;
            HOperatorSet.GenEmptyObj(out ho_Regions);
            HOperatorSet.GenEmptyObj(out ho_LineXld);
            #endregion

            try
            {
                if (InputImage == null)
                {
                    Msg = "输入图片为空";
                    Result = false;
                    return;
                }

                #region 裁剪区域
                object DomainImage = null;
                if (!ReduceDomainImage(InputImage, ref DomainImage))
                {
                    Msg = "裁剪区域失败";
                    Result = false;
                    return;
                }

                Mat hoDomainImage = DomainImage as Mat;
                //判断是否为灰度图
                try
                {
                    if (hoDomainImage.Channels() != 1)
                        Cv2.CvtColor(hoDomainImage, hoDomainImage, ColorConversionCodes.RGB2GRAY);

                    //转换后再次检查是否为灰度图
                    if (hoDomainImage.Channels() != 1)
                    {
                        Msg = "输入图片不为灰度图";
                        Result = false;
                        return;
                    }
                }
                catch
                {
                    Msg = "输入图片不为灰度图且转换失败";
                    Result = false;
                    return;
                }
                #endregion


                #region 算子逻辑
                Record = new ObjectRecord();

                int hv_Elements = Convert.ToInt16(Params.Inputs["卡尺数量"]);
                double hv_DetectHeight = Convert.ToDouble(Params.Inputs["卡尺长度"]);
                double hv_DetectWidth = Convert.ToDouble(Params.Inputs["卡尺宽度"]);
                int hv_Sigma = Convert.ToInt16(Params.Inputs["过滤一半像素"]);
                int hv_Threshold = Convert.ToInt16(Params.Inputs["对比度阈值"]);
                int hv_IgnoreNum = Convert.ToInt16(Params.Inputs["忽略点数"]);

                Enum.TryParse(Params.Inputs["极性"]?.ToString(), out Transition transition);
                string hv_Transition = "ignore";
                switch (transition)
                {
                    case Transition.Positive:
                        hv_Transition = "positive";
                        break;
                    case Transition.Negative:
                        hv_Transition = "negative";
                        break;
                    case Transition.Ignore:
                    default:
                        hv_Transition = "ignore";
                        break;
                }
                double hv_Row1 = Convert.ToDouble(((HSegment)Params.ROI).BeginRow + Params.Fixture.Row);
                double hv_Column1 = Convert.ToDouble(((HSegment)Params.ROI).BeginColumn + Params.Fixture.Column);
                double hv_Row2 = Convert.ToDouble(((HSegment)Params.ROI).EndRow + Params.Fixture.Row);
                double hv_Column2 = Convert.ToDouble(((HSegment)Params.ROI).EndColumn + Params.Fixture.Column);

                // 执行检测
                var lines = DetectEdgePoints(hoDomainImage, new Point(hv_Column1, hv_Row1), new Point(hv_Column2, hv_Row2));

                if (lines.Count <= 0)
                {
                    Msg = "找线失败";
                    Result = false;
                    return;
                }
                #endregion

                #region 结果处理
                List<double> X = new List<double>();
                List<double> Y = new List<double>();
                double CenterX = 0;
                double CenterY = 0;
                double Phi = 0;
                HSegment HSegment = new HSegment();

                var contourArray = lines.ToArray();
                int count = contourArray.Length;

                double[] Xpoints = new double[count];
                double[] Ypoints = new double[count];

                Parallel.For(0, count, j =>
                {
                    var point = contourArray[j];
                    Xpoints[j] = Convert.ToDouble(point.X);
                    Ypoints[j] = Convert.ToDouble(point.Y);
                });

                X.AddRange(Xpoints);
                Y.AddRange(Ypoints);

                var resultSegment = FitLineToPoints(lines);

                pts_to_best_line(out ho_LineXld, new HTuple(Ypoints), new HTuple(Xpoints), hv_IgnoreNum
                        , out HTuple hv_Row11, out HTuple hv_Column11, out HTuple hv_Row21, out HTuple hv_Column21);
                ((ObjectRecord)Record).AddXld(ho_LineXld);
                CenterX = (hv_Column11.D + hv_Column21.D) / 2;
                CenterY = (hv_Row11.D + hv_Row21.D) / 2;
                //计算直线与x轴的夹角，逆时针方向为正向
                HOperatorSet.AngleLx(hv_Row11, hv_Column11, hv_Row21, hv_Column21, out HTuple hv_ATan);
                Phi = hv_ATan;

                if (X.Count >= 2)
                    HSegment = new HSegment(hv_Column11.D, hv_Row11.D, hv_Column21.D, hv_Row21.D);

                //HOperatorSet.GenRegionLine(out ho_LineXld, resultSegment.StartPoint.Y, resultSegment.StartPoint.X
                //    , resultSegment.EndPoint.Y, resultSegment.EndPoint.X);
                //((ObjectRecord)Record).AddXld(ho_LineXld);

                //CenterX = (resultSegment.StartPoint.X + resultSegment.EndPoint.X) / 2;
                //CenterY = (resultSegment.StartPoint.Y + resultSegment.EndPoint.Y) / 2;

                //Phi = resultSegment.Angle / 180 * Math.PI;

                //if (X.Count >= 2)
                //    HSegment = new HSegment(resultSegment.StartPoint.X, resultSegment.StartPoint.Y
                //        , resultSegment.EndPoint.X, resultSegment.EndPoint.Y);

                Params.Outputs["X"] = X;
                Params.Outputs["Y"] = Y;
                Params.Outputs["CenterX"] = CenterX;
                Params.Outputs["CenterY"] = CenterY;
                Params.Outputs["Phi"] = Phi;
                Params.Outputs["Angle"] = Phi * 180.0 / Math.PI;
                Params.Outputs["Count"] = X.Count;
                Params.Outputs["Segment"] = HSegment;
                #endregion

                #region 生成OutputImage给后续处理
                try
                {
                    OutputImage = hoDomainImage;
                }
                catch (Exception ex)
                {
                    Msg = "生成OutputImage失败,原因是:" + ex.ToString();
                    Result = false;
                    return;
                }
                #endregion

                if (Msg == "运行超时")
                {
                    Result = false;
                    return;
                }

                Msg = "运行成功";
                Result = true;
                return;
            }
            catch (Exception ex)
            {
                Msg = "运行失败,原因是:" + ex.ToString().TrimEnd();
                OutputImage = null;
                Result = false;
                return;
            }
            finally
            {
                if (!Result)
                {
                    Params.Outputs.Add("X", new List<double>());
                    Params.Outputs.Add("Y", new List<double>());
                    Params.Outputs.Add("CenterX", 0.0);
                    Params.Outputs.Add("CenterY", 0.0);
                    Params.Outputs.Add("Phi", 0.0);
                    Params.Outputs.Add("Angle", 0.0);
                    Params.Outputs.Add("Count", 0);
                    Params.Outputs.Add("Segment", new HSegment());
                }

                bCompleted = true;
                #region 内存释放
                ho_Regions.Dispose();
                #endregion
            }
        }

        /// <summary>
        /// 直线段数据
        /// </summary>
        public class LineSegment
        {
            public Point2f StartPoint { get; set; }
            public Point2f EndPoint { get; set; }

            public List<Point2f> EdgePoints { get; set; }

            public float Vx { get; set; }  // 方向向量X
            public float Vy { get; set; }  // 方向向量Y
            public float Score { get; set; } // 拟合分数 0-100
            public int RegionIndex { get; set; } // 区域索引

            public double Length => Math.Sqrt(
                Math.Pow(EndPoint.X - StartPoint.X, 2) +
                Math.Pow(EndPoint.Y - StartPoint.Y, 2));

            public double Angle => Math.Atan2(Vy, Vx) * 180 / Math.PI;

            public LineSegment() { }

            public LineSegment(Point2f start, Point2f end, float vx, float vy, List<Point2f> edgePoints)
            {
                StartPoint = start;
                EndPoint = end;
                Vx = vx;
                Vy = vy;
                EdgePoints = edgePoints;
            }
        }

        /// <summary>
        /// 过滤无效点
        /// </summary>
        private List<Point2f> FilterValidPoints(List<Point2f> points)
        {
            if (points == null) return new List<Point2f>();

            return points.Where(p =>
                !float.IsNaN(p.X) && !float.IsNaN(p.Y) &&
                !float.IsInfinity(p.X) && !float.IsInfinity(p.Y) &&
                p.X >= 0 && p.Y >= 0).ToList();
        }

        /// <summary>
        /// 检测边缘点（使用卡尺检测方式）
        /// </summary>
        private List<Point2f> DetectEdgePoints(Mat regionImage, Point startPoint, Point endPoint)
        {
            var edgePoints = new List<Point2f>();

            try
            {
                // 转换为灰度
                var gray = new Mat();
                if (regionImage.Channels() != 1)
                    Cv2.CvtColor(regionImage, gray, ColorConversionCodes.RGB2GRAY);
                else
                    gray = regionImage.Clone();

                // 获取参数
                int caliperCount = Params.Inputs.ContainsKey("卡尺数量") ?
                    Convert.ToInt32(Params.Inputs["卡尺数量"]) : 6;
                int caliperLength = Params.Inputs.ContainsKey("卡尺长度") ?
                    Convert.ToInt32(Params.Inputs["卡尺长度"]) : 30;
                int caliperWidth = Params.Inputs.ContainsKey("卡尺宽度") ?
                    Convert.ToInt32(Params.Inputs["卡尺宽度"]) : 10;
                int filterHalfPixels = Params.Inputs.ContainsKey("过滤一半像素") ?
                    Convert.ToInt32(Params.Inputs["过滤一半像素"]) : 2;
                int contrastThreshold = Params.Inputs.ContainsKey("对比度阈值") ?
                    Convert.ToInt32(Params.Inputs["对比度阈值"]) : 5;
                string polarity = Params.Inputs.ContainsKey("极性") ?
                    Params.Inputs["极性"]?.ToString() : "Ignore";
                string edgePosition = Params.Inputs.ContainsKey("边缘位置") ?
                    Params.Inputs["边缘位置"]?.ToString() : "Best";

                // 使用卡尺方式检测边缘点，传入起始点和终点
                edgePoints = DetectEdgesAlongLineWithCaliper(
                    gray,
                    startPoint,
                    endPoint,
                    caliperCount,
                    caliperLength,
                    caliperWidth,
                    contrastThreshold,
                    polarity,
                    edgePosition);

                // 过滤掉无效的点
                edgePoints = FilterValidPoints(edgePoints);
            }
            catch (Exception ex)
            {
                Debug.WriteLine($"边缘点检测错误: {ex.Message}");
            }

            return edgePoints ?? new List<Point2f>();
        }

        /// <summary>
        /// 沿指定线段使用多个卡尺检测边缘点
        /// </summary>
        private List<Point2f> DetectEdgesAlongLineWithCaliper(
            Mat grayImage,
            Point startPoint,
            Point endPoint,
            int caliperCount,
            int caliperLength,
            int caliperWidth,
            int contrastThreshold,
            string polarity,
            string edgePosition)
        {
            var edgePoints = new List<Point2f>();

            // 计算主线段方向
            double dx = endPoint.X - startPoint.X;
            double dy = endPoint.Y - startPoint.Y;
            double mainLength = Math.Sqrt(dx * dx + dy * dy);

            if (mainLength < 1) return edgePoints;

            // 主线段单位方向向量
            double mainUx = dx / mainLength;
            double mainUy = dy / mainLength;

            // 主线段垂直方向向量（卡尺方向）
            double caliperUx = -mainUy;
            double caliperUy = mainUx;

            // 在主线段上均匀分布卡尺
            for (int i = 0; i < caliperCount; i++)
            {
                double t = (i + 1.0) / (caliperCount + 1); // 在0-1之间均匀分布
                double centerX = startPoint.X + t * mainLength * mainUx;
                double centerY = startPoint.Y + t * mainLength * mainUy;

                // 计算卡尺的起点和终点（垂直于主线段方向）
                Point caliperStart = new Point(
                    (int)Math.Round(centerX - caliperLength / 2.0 * caliperUx),
                    (int)Math.Round(centerY - caliperLength / 2.0 * caliperUy));

                Point caliperEnd = new Point(
                    (int)Math.Round(centerX + caliperLength / 2.0 * caliperUx),
                    (int)Math.Round(centerY + caliperLength / 2.0 * caliperUy));

                // 检测单个卡尺上的边缘点
                var edges = DetectEdgesAlongLine(
                    grayImage,
                    caliperStart.X, caliperStart.Y,
                    caliperEnd.X, caliperEnd.Y,
                    caliperWidth,
                    contrastThreshold,
                    polarity,
                    edgePosition);
                edgePoints.AddRange(edges);
            }

            return edgePoints;
        }

        /// <summary>
        /// 沿直线检测边缘点
        /// </summary>
        private List<Point2f> DetectEdgesAlongLine(
            Mat grayImage,
            int startX, int startY,
            int endX, int endY,
            int lineWidth,
            int contrastThreshold,
            string polarity, string edgePosition)
        {
            var edgePoints = new List<Point2f>();

            // 计算直线方向
            double dx = endX - startX;
            double dy = endY - startY;
            double length = Math.Sqrt(dx * dx + dy * dy);

            if (length == 0) return edgePoints;

            // 单位方向向量
            double ux = dx / length;
            double uy = dy / length;

            // 垂直方向向量
            double vx = -uy;
            double vy = ux;

            // 沿线采样
            for (double t = 0; t <= length; t += 1.0)
            {
                double centerX = startX + t * ux;
                double centerY = startY + t * uy;

                // 在垂直方向上进行采样
                List<double> profile = new List<double>();
                List<Point> profilePoints = new List<Point>();

                for (int w = -lineWidth / 2; w <= lineWidth / 2; w++)
                {
                    int sampleX = (int)Math.Round(centerX + w * vx);
                    int sampleY = (int)Math.Round(centerY + w * vy);

                    if (sampleX >= 0 && sampleX < grayImage.Width &&
                        sampleY >= 0 && sampleY < grayImage.Height)
                    {
                        byte pixelValue = grayImage.At<byte>(sampleY, sampleX);
                        profile.Add(pixelValue);
                        profilePoints.Add(new Point(sampleX, sampleY));
                    }
                }

                if (profile.Count > 0)
                {
                    // 分析灰度剖面，检测边缘
                    if (Enum.TryParse(polarity, out Transition transition))
                    {
                        var edgeCandidates = FindEdgesInProfile(profile, profilePoints, contrastThreshold, transition, edgePosition);
                        edgePoints.AddRange(edgeCandidates);
                    }
                    else
                    {
                        var edgeCandidates = FindEdgesInProfile(profile, profilePoints, contrastThreshold, Transition.Ignore, edgePosition);
                        edgePoints.AddRange(edgeCandidates);
                    }
                }
            }

            return edgePoints;
        }

        /// <summary>
        /// 在灰度剖面中查找边缘点
        /// </summary>
        private List<Point2f> FindEdgesInProfile(
            List<double> profile,
            List<Point> profilePoints,
            int contrastThreshold,
            Transition polarity, string edgePosition)
        {
            var edges = new List<Point2f>();

            if (profile.Count < 3) return edges;

            // 计算梯度
            List<double> gradients = new List<double>();
            for (int i = 1; i < profile.Count - 1; i++)
            {
                double gradient = (profile[i + 1] - profile[i - 1]) / 2.0;
                gradients.Add(gradient);
            }

            // 根据极性过滤边缘
            var edgeCandidates = new List<(int index, double strength)>();

            for (int i = 0; i < gradients.Count; i++)
            {
                double gradient = gradients[i];
                bool isEdge = false;

                if (polarity == Transition.Positive && gradient > contrastThreshold)
                {
                    isEdge = true; // 从暗到亮的边缘
                }
                else if (polarity == Transition.Negative && gradient < -contrastThreshold)
                {
                    isEdge = true; // 从亮到暗的边缘
                }
                else if (polarity == Transition.Ignore && Math.Abs(gradient) > contrastThreshold)
                {
                    isEdge = true; // 忽略极性，只关注对比度
                }

                if (isEdge)
                {
                    edgeCandidates.Add((i + 1, Math.Abs(gradient)));
                }
            }

            // 根据边缘位置参数选择边缘点
            if (edgeCandidates.Count > 0)
            {
                if (Enum.TryParse(edgePosition, out Selector selector))
                {
                    var selectedEdge = SelectEdgeByPosition(edgeCandidates, profile, selector);
                    if (selectedEdge.index >= 0)
                    {
                        Point2f edgePoint = RefineEdgePosition(profile, profilePoints, selectedEdge.index);
                        edges.Add(edgePoint);
                    }
                }
                else
                {
                    var selectedEdge = SelectEdgeByPosition(edgeCandidates, profile, Selector.Best);
                    if (selectedEdge.index >= 0)
                    {
                        Point2f edgePoint = RefineEdgePosition(profile, profilePoints, selectedEdge.index);
                        edges.Add(edgePoint);
                    }
                }
            }

            return edges;
        }

        /// <summary>
        /// 根据边缘位置参数选择边缘点
        /// </summary>
        private (int index, double strength) SelectEdgeByPosition(
            List<(int index, double strength)> edgeCandidates,
            List<double> profile,
            Selector edgePosition)
        {
            if (edgeCandidates.Count == 0) return (-1, 0);

            switch (edgePosition)
            {
                case Selector.Best: // 最强边缘
                    return edgeCandidates.OrderByDescending(e => e.strength).First();

                case Selector.First: // 第一个边缘
                    return edgeCandidates.OrderBy(e => e.index).First();

                case Selector.Last: // 最后一个边缘
                    return edgeCandidates.OrderByDescending(e => e.index).First();

                default: // 默认使用最强边缘
                    return edgeCandidates.OrderByDescending(e => e.strength).First();
            }
        }

        /// <summary>
        /// 亚像素精度边缘定位
        /// </summary>
        private Point2f RefineEdgePosition(List<double> profile, List<Point> profilePoints, int edgeIndex)
        {
            if (edgeIndex <= 0 || edgeIndex >= profile.Count - 1)
                return new Point2f(profilePoints[edgeIndex].X, profilePoints[edgeIndex].Y);

            // 简单的二次插值
            double y0 = profile[edgeIndex - 1];
            double y1 = profile[edgeIndex];
            double y2 = profile[edgeIndex + 1];

            // 计算亚像素偏移
            double offset = (y0 - y2) / (2 * (y0 - 2 * y1 + y2));

            // 线性插值计算亚像素位置
            Point p0 = profilePoints[edgeIndex - 1];
            Point p1 = profilePoints[edgeIndex];
            Point p2 = profilePoints[edgeIndex + 1];

            double subPixelX = p1.X + offset * (p2.X - p0.X) / 2.0;
            double subPixelY = p1.Y + offset * (p2.Y - p0.Y) / 2.0;

            return new Point2f((float)subPixelX, (float)subPixelY);
        }


        /// <summary>
        /// 拟合直线
        /// </summary>
        private LineSegment FitLineToPoints(List<Point2f> points, float LineExtension = 5.0f)
        {
            if (points.Count < 2) return null;

            try
            {
                // 使用OpenCV的FitLine
                var lineOutput = new Mat();
                using (var pointsArray = InputArray.Create(points))
                {
                    Cv2.FitLine(pointsArray, lineOutput, DistanceTypes.L2, 0, 0.01, 0.01);
                }

                // 解析拟合结果: [vx, vy, x0, y0]
                float vx = lineOutput.At<float>(0);
                float vy = lineOutput.At<float>(1);
                float x0 = lineOutput.At<float>(2);
                float y0 = lineOutput.At<float>(3);

                // 检查拟合结果是否有效
                if (float.IsNaN(vx) || float.IsNaN(vy) || float.IsNaN(x0) || float.IsNaN(y0) ||
                    float.IsInfinity(vx) || float.IsInfinity(vy) || float.IsInfinity(x0) || float.IsInfinity(y0))
                {
                    Debug.WriteLine("直线拟合结果包含NaN或Infinity值");
                    return FitLineFallback(points, LineExtension);
                }

                // 检查方向向量是否为零向量
                float dirLength = (float)Math.Sqrt(vx * vx + vy * vy);
                if (dirLength < 1e-6f)
                {
                    Debug.WriteLine("方向向量长度接近零，使用备用方法");
                    return FitLineFallback(points, LineExtension);
                }

                // 归一化方向向量
                vx /= dirLength;
                vy /= dirLength;

                // 计算直线的起点和终点（基于点集的范围）
                float minT = float.MaxValue;
                float maxT = float.MinValue;

                foreach (var point in points)
                {
                    float t = (point.X - x0) * vx + (point.Y - y0) * vy;
                    if (float.IsNaN(t) || float.IsInfinity(t))
                        continue;

                    minT = Math.Min(minT, t);
                    maxT = Math.Max(maxT, t);
                }

                // 检查参数范围是否有效
                if (minT > maxT || float.IsNaN(minT) || float.IsNaN(maxT) ||
                    float.IsInfinity(minT) || float.IsInfinity(maxT))
                {
                    Debug.WriteLine("参数范围无效，使用备用方法");
                    return FitLineFallback(points, LineExtension);
                }

                // 扩展线段
                minT -= LineExtension;
                maxT += LineExtension;

                var startPoint = new Point2f(x0 + minT * vx, y0 + minT * vy);
                var endPoint = new Point2f(x0 + maxT * vx, y0 + maxT * vy);

                // 检查最终坐标是否有效
                if (float.IsNaN(startPoint.X) || float.IsNaN(startPoint.Y) ||
                    float.IsNaN(endPoint.X) || float.IsNaN(endPoint.Y))
                {
                    Debug.WriteLine("最终坐标包含NaN，使用备用方法");
                    return FitLineFallback(points, LineExtension);
                }

                return new LineSegment(startPoint, endPoint, vx, vy, points);
            }
            catch (Exception ex)
            {
                Debug.WriteLine($"直线拟合错误: {ex.Message}，使用备用方法");
                return FitLineFallback(points, LineExtension);
            }
        }

        /// <summary>
        /// 备用直线拟合方法（最小二乘法）
        /// </summary>
        private LineSegment FitLineFallback(List<Point2f> points, float LineExtension = 5.0f)
        {
            if (points.Count < 2) return null;

            try
            {
                // 计算点的均值
                float meanX = points.Average(p => p.X);
                float meanY = points.Average(p => p.Y);

                // 计算方差和协方差
                float varX = 0, varY = 0, covXY = 0;
                foreach (var point in points)
                {
                    float dx = point.X - meanX;
                    float dy = point.Y - meanY;
                    varX += dx * dx;
                    varY += dy * dy;
                    covXY += dx * dy;
                }

                // 检查数据是否有效
                if (varX < 1e-6f && varY < 1e-6f)
                {
                    // 所有点几乎重合，创建水平线
                    float minX = points.Min(p => p.X);
                    float maxX = points.Max(p => p.X);
                    float centerY = points.Average(p => p.Y);

                    return new LineSegment(new Point2f(minX - LineExtension, centerY)
                        , new Point2f(maxX + LineExtension, centerY), 1, 0, points);
                }

                // 计算主方向
                float angle;
                if (varX > varY)
                {
                    // 主要沿X轴方向
                    angle = 0;
                }
                else
                {
                    // 主要沿Y轴方向
                    angle = (float)(Math.PI / 2);
                }

                // 计算方向向量
                float vx = (float)Math.Cos(angle);
                float vy = (float)Math.Sin(angle);

                // 计算点在主方向上的投影范围
                float minT = float.MaxValue;
                float maxT = float.MinValue;

                foreach (var point in points)
                {
                    float t = (point.X - meanX) * vx + (point.Y - meanY) * vy;
                    minT = Math.Min(minT, t);
                    maxT = Math.Max(maxT, t);
                }

                // 扩展线段
                minT -= LineExtension;
                maxT += LineExtension;

                var startPoint = new Point2f(meanX + minT * vx, meanY + minT * vy);
                var endPoint = new Point2f(meanX + maxT * vx, meanY + maxT * vy);

                return new LineSegment(startPoint, endPoint, vx, vy, points);
            }
            catch (Exception ex)
            {
                Debug.WriteLine($"备用直线拟合也失败: {ex.Message}");
                return null;
            }
        }

        /// <summary>
        /// 数据预处理，移除异常点
        /// </summary>
        private List<Point2f> PreprocessPoints(List<Point2f> points)
        {
            if (points.Count < 3) return points;

            // 计算点的中心
            float meanX = points.Average(p => p.X);
            float meanY = points.Average(p => p.Y);

            // 计算距离中心的平均距离和标准差
            var distances = points.Select(p =>
                Math.Sqrt(Math.Pow(p.X - meanX, 2) + Math.Pow(p.Y - meanY, 2))).ToList();

            double meanDistance = distances.Average();
            double stdDistance = Math.Sqrt(distances.Average(d => Math.Pow(d - meanDistance, 2)));

            // 移除距离中心过远的异常点（超过3倍标准差）
            var filteredPoints = points.Where((p, i) =>
                distances[i] <= meanDistance + 3 * stdDistance).ToList();

            if (filteredPoints.Count < 2)
            {
                // 如果过滤后点数太少，返回原始点集
                return points;
            }

            Debug.WriteLine($"过滤了 {points.Count - filteredPoints.Count} 个异常点");
            return filteredPoints;
        }
    }
}