// ============================================================================
// Copyright © 2026 Hexagon Technology Center GmbH. All Rights Reserved.
// 文件名: EdgeLineFitProcessor.cs
// 描述: 边缘查找拟合直线算子
// 功能:
//   - 沿用户定义的搜索线等间距放置多个卡尺（Caliper）
//   - 在每个卡尺内沿垂直方向提取灰度投影并求导，定位边缘点
//   - 支持亚像素精度（抛物线插值）
//   - 支持边缘极性选择（亮到暗/暗到亮/双向）
//   - 使用最小二乘或RANSAC算法拟合直线
//   - 输出拟合直线参数、边缘点、内点/外点、拟合误差
// 算法: 卡尺边缘检测 + 最小二乘/RANSAC直线拟合
// 作者: 李伟 wei.lw.li@hexagon.com
// ============================================================================

using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;
using Emgu.CV.Util;
using XP.ImageProcessing.Core;
using Serilog;
using System.Drawing;

namespace XP.ImageProcessing.Processors;

/// <summary>
/// 边缘点信息
/// </summary>
public class EdgePointInfo
{
    /// <summary>边缘点坐标（亚像素）</summary>
    public PointF Position { get; set; }

    /// <summary>边缘强度（梯度绝对值）</summary>
    public double Strength { get; set; }

    /// <summary>卡尺索引</summary>
    public int CaliperIndex { get; set; }

    /// <summary>是否为拟合内点</summary>
    public bool IsInlier { get; set; } = true;
}

/// <summary>
/// 直线拟合结果
/// </summary>
public class LineFitResult
{
    /// <summary>拟合是否成功</summary>
    public bool Success { get; set; }

    /// <summary>直线方向向量 (vx, vy)</summary>
    public PointF Direction { get; set; }

    /// <summary>直线上一点 (x0, y0)</summary>
    public PointF PointOnLine { get; set; }

    /// <summary>直线角度（度，相对于X轴）</summary>
    public double AngleDegrees { get; set; }

    /// <summary>直线端点1（用于绘制）</summary>
    public PointF Endpoint1 { get; set; }

    /// <summary>直线端点2（用于绘制）</summary>
    public PointF Endpoint2 { get; set; }

    /// <summary>所有检测到的边缘点</summary>
    public List<EdgePointInfo> EdgePoints { get; set; } = new();

    /// <summary>内点列表</summary>
    public List<PointF> Inliers { get; set; } = new();

    /// <summary>外点列表</summary>
    public List<PointF> Outliers { get; set; } = new();

    /// <summary>平均拟合误差（像素）</summary>
    public double FitError { get; set; }

    /// <summary>有效边缘点数</summary>
    public int EdgePointCount { get; set; }
}

/// <summary>
/// 边缘查找拟合直线算子 - 使用卡尺法检测边缘点并拟合直线
/// </summary>
public class EdgeLineFitProcessor : ImageProcessorBase
{
    private static readonly ILogger _logger = Log.ForContext<EdgeLineFitProcessor>();
    private static readonly Random _random = new();

    public EdgeLineFitProcessor()
    {
        Name = LocalizationHelper.GetString("EdgeLineFitProcessor_Name");
        Description = LocalizationHelper.GetString("EdgeLineFitProcessor_Description");
    }

    protected override void InitializeParameters()
    {
        // ── 搜索线起止点（由UI交互控件注入，不可见） ──
        Parameters.Add("StartX", new ProcessorParameter(
            "StartX", "StartX", typeof(int), 100, null, null, "") { IsVisible = false });
        Parameters.Add("StartY", new ProcessorParameter(
            "StartY", "StartY", typeof(int), 200, null, null, "") { IsVisible = false });
        Parameters.Add("EndX", new ProcessorParameter(
            "EndX", "EndX", typeof(int), 400, null, null, "") { IsVisible = false });
        Parameters.Add("EndY", new ProcessorParameter(
            "EndY", "EndY", typeof(int), 200, null, null, "") { IsVisible = false });

        // ── 卡尺参数 ──
        Parameters.Add("CaliperCount", new ProcessorParameter(
            "CaliperCount",
            LocalizationHelper.GetString("EdgeLineFitProcessor_CaliperCount"),
            typeof(int), 20, 3, 200,
            LocalizationHelper.GetString("EdgeLineFitProcessor_CaliperCount_Desc")));

        Parameters.Add("CaliperWidth", new ProcessorParameter(
            "CaliperWidth",
            LocalizationHelper.GetString("EdgeLineFitProcessor_CaliperWidth"),
            typeof(int), 40, 5, 500,
            LocalizationHelper.GetString("EdgeLineFitProcessor_CaliperWidth_Desc")));

        // ── 边缘检测参数 ──
        Parameters.Add("EdgePolarity", new ProcessorParameter(
            "EdgePolarity",
            LocalizationHelper.GetString("EdgeLineFitProcessor_EdgePolarity"),
            typeof(string), "Both", null, null,
            LocalizationHelper.GetString("EdgeLineFitProcessor_EdgePolarity_Desc"),
            new string[] { "BrightToDark", "DarkToBright", "Both" }));

        Parameters.Add("EdgeThreshold", new ProcessorParameter(
            "EdgeThreshold",
            LocalizationHelper.GetString("EdgeLineFitProcessor_EdgeThreshold"),
            typeof(int), 30, 1, 255,
            LocalizationHelper.GetString("EdgeLineFitProcessor_EdgeThreshold_Desc")));

        Parameters.Add("Sigma", new ProcessorParameter(
            "Sigma",
            LocalizationHelper.GetString("EdgeLineFitProcessor_Sigma"),
            typeof(double), 1.0, 0.1, 10.0,
            LocalizationHelper.GetString("EdgeLineFitProcessor_Sigma_Desc")));

        // ── 拟合参数 ──
        Parameters.Add("FitMethod", new ProcessorParameter(
            "FitMethod",
            LocalizationHelper.GetString("EdgeLineFitProcessor_FitMethod"),
            typeof(string), "RANSAC", null, null,
            LocalizationHelper.GetString("EdgeLineFitProcessor_FitMethod_Desc"),
            new string[] { "LeastSquares", "RANSAC" }));

        Parameters.Add("RansacThreshold", new ProcessorParameter(
            "RansacThreshold",
            LocalizationHelper.GetString("EdgeLineFitProcessor_RansacThreshold"),
            typeof(double), 2.0, 0.5, 20.0,
            LocalizationHelper.GetString("EdgeLineFitProcessor_RansacThreshold_Desc")));

        Parameters.Add("Thickness", new ProcessorParameter(
            "Thickness",
            LocalizationHelper.GetString("EdgeLineFitProcessor_Thickness"),
            typeof(int), 2, 1, 10,
            LocalizationHelper.GetString("EdgeLineFitProcessor_Thickness_Desc")));
    }

    public override Image<Gray, byte> Process(Image<Gray, byte> inputImage)
    {
        // 读取参数
        int startX = GetParameter<int>("StartX");
        int startY = GetParameter<int>("StartY");
        int endX = GetParameter<int>("EndX");
        int endY = GetParameter<int>("EndY");
        int caliperCount = GetParameter<int>("CaliperCount");
        int caliperWidth = GetParameter<int>("CaliperWidth");
        string edgePolarity = GetParameter<string>("EdgePolarity");
        int edgeThreshold = GetParameter<int>("EdgeThreshold");
        double sigma = GetParameter<double>("Sigma");
        string fitMethod = GetParameter<string>("FitMethod");
        double ransacThreshold = GetParameter<double>("RansacThreshold");
        int thickness = GetParameter<int>("Thickness");

        OutputData.Clear();

        _logger.Debug(
            "EdgeLineFit started: Search({StartX},{StartY})->({EndX},{EndY}), Calipers={Count}, Width={Width}, Polarity={Polarity}",
            startX, startY, endX, endY, caliperCount, caliperWidth, edgePolarity);

        // 计算搜索线方向和垂直方向
        double searchDx = endX - startX;
        double searchDy = endY - startY;
        double searchLen = Math.Sqrt(searchDx * searchDx + searchDy * searchDy);

        if (searchLen < 1.0)
        {
            _logger.Warning("Search line too short, cannot perform edge detection");
            OutputData["LineFitResult"] = new LineFitResult { Success = false };
            return inputImage.Clone();
        }

        // 搜索线单位方向
        double ux = searchDx / searchLen;
        double uy = searchDy / searchLen;

        // 垂直于搜索线的方向（卡尺搜索方向）
        double perpX = -uy;
        double perpY = ux;

        // 沿搜索线等间距放置卡尺
        var edgePoints = new List<EdgePointInfo>();
        double step = searchLen / (caliperCount + 1);

        for (int i = 0; i < caliperCount; i++)
        {
            // 卡尺中心点
            double cx = startX + ux * step * (i + 1);
            double cy = startY + uy * step * (i + 1);

            // 在卡尺内沿垂直方向提取灰度剖面
            var edgePoint = FindEdgeInCaliper(
                inputImage, cx, cy, perpX, perpY,
                caliperWidth, edgePolarity, edgeThreshold, sigma, i);

            if (edgePoint != null)
            {
                edgePoints.Add(edgePoint);
            }
        }

        _logger.Debug("Found {Count} edge points from {Total} calipers", edgePoints.Count, caliperCount);

        // 拟合直线
        var result = FitLine(edgePoints, fitMethod, ransacThreshold, inputImage.Size);

        // 存储输出数据
        OutputData["LineFitResult"] = result;
        OutputData["EdgePoints"] = edgePoints.Select(p => p.Position).ToArray();
        OutputData["EdgePointCount"] = edgePoints.Count;
        OutputData["Thickness"] = thickness;

        if (result.Success)
        {
            OutputData["FittedLineDirection"] = result.Direction;
            OutputData["FittedLinePoint"] = result.PointOnLine;
            OutputData["LineAngle"] = result.AngleDegrees;
            OutputData["LineEndpoint1"] = result.Endpoint1;
            OutputData["LineEndpoint2"] = result.Endpoint2;
            OutputData["InlierPoints"] = result.Inliers.ToArray();
            OutputData["OutlierPoints"] = result.Outliers.ToArray();
            OutputData["FitError"] = result.FitError;

            _logger.Information(
                "EdgeLineFit completed: Angle={Angle:F2}°, Inliers={Inliers}/{Total}, Error={Error:F3}px",
                result.AngleDegrees, result.Inliers.Count, edgePoints.Count, result.FitError);
        }
        else
        {
            _logger.Warning("EdgeLineFit failed: insufficient edge points for line fitting");
        }

        // 搜索区域信息（供UI绘制）
        OutputData["SearchStart"] = new PointF(startX, startY);
        OutputData["SearchEnd"] = new PointF(endX, endY);
        OutputData["CaliperWidth"] = caliperWidth;
        OutputData["CaliperCount"] = caliperCount;
        OutputData["PerpDirection"] = new PointF((float)perpX, (float)perpY);

        return inputImage.Clone();
    }

    /// <summary>
    /// 在单个卡尺内查找边缘点
    /// </summary>
    private EdgePointInfo? FindEdgeInCaliper(
        Image<Gray, byte> image,
        double centerX, double centerY,
        double perpX, double perpY,
        int caliperWidth, string polarity,
        int threshold, double sigma, int caliperIndex)
    {
        int halfWidth = caliperWidth / 2;
        int profileLength = caliperWidth;

        // 提取灰度剖面
        var profile = new double[profileLength];
        int validCount = 0;

        for (int i = 0; i < profileLength; i++)
        {
            double offset = i - halfWidth;
            double px = centerX + perpX * offset;
            double py = centerY + perpY * offset;

            int ix = (int)Math.Round(px);
            int iy = (int)Math.Round(py);

            if (ix >= 0 && ix < image.Width && iy >= 0 && iy < image.Height)
            {
                profile[i] = image.Data[iy, ix, 0];
                validCount++;
            }
            else
            {
                profile[i] = 0;
            }
        }

        if (validCount < profileLength * 0.5)
            return null;

        // 高斯平滑
        if (sigma > 0.1)
        {
            profile = GaussianSmooth1D(profile, sigma);
        }

        // 求一阶导数
        var derivative = new double[profileLength];
        for (int i = 1; i < profileLength - 1; i++)
        {
            derivative[i] = (profile[i + 1] - profile[i - 1]) / 2.0;
        }

        // 根据极性查找最强边缘
        int bestIdx = -1;
        double bestStrength = 0;

        for (int i = 2; i < profileLength - 2; i++)
        {
            double strength = derivative[i];
            bool validPolarity = polarity switch
            {
                "BrightToDark" => strength < 0,   // 亮到暗：导数为负
                "DarkToBright" => strength > 0,    // 暗到亮：导数为正
                _ => true                          // Both：任意方向
            };

            if (!validPolarity) continue;

            double absStrength = Math.Abs(strength);
            if (absStrength >= threshold && absStrength > bestStrength)
            {
                bestStrength = absStrength;
                bestIdx = i;
            }
        }

        if (bestIdx < 0)
            return null;

        // 亚像素精度：抛物线插值
        double subPixelOffset = 0;
        if (bestIdx > 0 && bestIdx < profileLength - 1)
        {
            double left = Math.Abs(derivative[bestIdx - 1]);
            double center = Math.Abs(derivative[bestIdx]);
            double right = Math.Abs(derivative[bestIdx + 1]);
            double denom = 2.0 * (2.0 * center - left - right);
            if (Math.Abs(denom) > 1e-6)
            {
                subPixelOffset = (left - right) / denom;
                subPixelOffset = Math.Clamp(subPixelOffset, -0.5, 0.5);
            }
        }

        double edgeOffset = (bestIdx + subPixelOffset) - halfWidth;
        float edgeX = (float)(centerX + perpX * edgeOffset);
        float edgeY = (float)(centerY + perpY * edgeOffset);

        return new EdgePointInfo
        {
            Position = new PointF(edgeX, edgeY),
            Strength = bestStrength,
            CaliperIndex = caliperIndex,
            IsInlier = true
        };
    }

    /// <summary>
    /// 一维高斯平滑
    /// </summary>
    private static double[] GaussianSmooth1D(double[] data, double sigma)
    {
        int kernelRadius = (int)Math.Ceiling(sigma * 3);
        int kernelSize = kernelRadius * 2 + 1;
        var kernel = new double[kernelSize];
        double sum = 0;

        for (int i = 0; i < kernelSize; i++)
        {
            double x = i - kernelRadius;
            kernel[i] = Math.Exp(-x * x / (2.0 * sigma * sigma));
            sum += kernel[i];
        }
        for (int i = 0; i < kernelSize; i++)
            kernel[i] /= sum;

        var result = new double[data.Length];
        for (int i = 0; i < data.Length; i++)
        {
            double val = 0;
            double wSum = 0;
            for (int k = 0; k < kernelSize; k++)
            {
                int idx = i + k - kernelRadius;
                if (idx >= 0 && idx < data.Length)
                {
                    val += data[idx] * kernel[k];
                    wSum += kernel[k];
                }
            }
            result[i] = wSum > 0 ? val / wSum : data[i];
        }
        return result;
    }

    /// <summary>
    /// 拟合直线
    /// </summary>
    private LineFitResult FitLine(List<EdgePointInfo> edgePoints, string method,
        double ransacThreshold, Size imageSize)
    {
        var result = new LineFitResult();

        if (edgePoints.Count < 2)
        {
            result.Success = false;
            return result;
        }

        if (method == "RANSAC" && edgePoints.Count >= 3)
        {
            return FitLineRANSAC(edgePoints, ransacThreshold, imageSize);
        }
        else
        {
            return FitLineLeastSquares(edgePoints, imageSize);
        }
    }

    /// <summary>
    /// 最小二乘直线拟合（使用OpenCV FitLine）
    /// </summary>
    private LineFitResult FitLineLeastSquares(List<EdgePointInfo> edgePoints, Size imageSize)
    {
        var result = new LineFitResult();
        var points = edgePoints.Select(p => p.Position).ToArray();

        using var pointVector = new VectorOfPointF(points);
        using var lineMat = new Mat();
        CvInvoke.FitLine(pointVector, lineMat, DistType.L2, 0, 0.01, 0.01);
        var lineParams = new float[4];
        System.Runtime.InteropServices.Marshal.Copy(lineMat.DataPointer, lineParams, 0, 4);

        float vx = lineParams[0], vy = lineParams[1];
        float x0 = lineParams[2], y0 = lineParams[3];

        result.Success = true;
        result.Direction = new PointF(vx, vy);
        result.PointOnLine = new PointF(x0, y0);
        result.AngleDegrees = Math.Atan2(vy, vx) * 180.0 / Math.PI;

        // 计算端点（延伸到图像边界或搜索范围）
        ComputeLineEndpoints(result, points, imageSize);

        // 所有点都是内点
        result.Inliers = points.ToList();
        result.Outliers = new List<PointF>();
        foreach (var ep in edgePoints)
            ep.IsInlier = true;

        // 计算拟合误差
        result.FitError = ComputeFitError(points, vx, vy, x0, y0);
        result.EdgePointCount = edgePoints.Count;
        result.EdgePoints = edgePoints;

        return result;
    }

    /// <summary>
    /// RANSAC直线拟合
    /// </summary>
    private LineFitResult FitLineRANSAC(List<EdgePointInfo> edgePoints, double threshold, Size imageSize)
    {
        var result = new LineFitResult();
        var points = edgePoints.Select(p => p.Position).ToArray();
        int n = points.Length;

        // RANSAC参数
        int maxIterations = Math.Min(1000, n * (n - 1) / 2);
        int bestInlierCount = 0;
        float bestVx = 0, bestVy = 0, bestX0 = 0, bestY0 = 0;
        List<int> bestInlierIndices = new();

        for (int iter = 0; iter < maxIterations; iter++)
        {
            // 随机选择2个点
            int idx1 = _random.Next(n);
            int idx2 = _random.Next(n);
            if (idx1 == idx2) continue;

            PointF p1 = points[idx1], p2 = points[idx2];
            float dx = p2.X - p1.X, dy = p2.Y - p1.Y;
            float len = (float)Math.Sqrt(dx * dx + dy * dy);
            if (len < 1e-6f) continue;

            float vx = dx / len, vy = dy / len;

            // 统计内点
            var inlierIndices = new List<int>();
            for (int i = 0; i < n; i++)
            {
                double dist = PointToLineDistance(points[i], p1, vx, vy);
                if (dist <= threshold)
                {
                    inlierIndices.Add(i);
                }
            }

            if (inlierIndices.Count > bestInlierCount)
            {
                bestInlierCount = inlierIndices.Count;
                bestInlierIndices = inlierIndices;

                // 用所有内点重新拟合
                var inlierPoints = inlierIndices.Select(i => points[i]).ToArray();
                using var pv = new VectorOfPointF(inlierPoints);
                using var lpMat = new Mat();
                CvInvoke.FitLine(pv, lpMat, DistType.L2, 0, 0.01, 0.01);
                var lp = new float[4];
                System.Runtime.InteropServices.Marshal.Copy(lpMat.DataPointer, lp, 0, 4);
                bestVx = lp[0]; bestVy = lp[1]; bestX0 = lp[2]; bestY0 = lp[3];
            }

            // 如果内点比例已经很高，提前退出
            if (bestInlierCount > n * 0.95)
                break;
        }

        if (bestInlierCount < 2)
        {
            result.Success = false;
            return result;
        }

        result.Success = true;
        result.Direction = new PointF(bestVx, bestVy);
        result.PointOnLine = new PointF(bestX0, bestY0);
        result.AngleDegrees = Math.Atan2(bestVy, bestVx) * 180.0 / Math.PI;

        // 分类内点/外点
        var inliers = new List<PointF>();
        var outliers = new List<PointF>();
        var inlierSet = new HashSet<int>(bestInlierIndices);

        for (int i = 0; i < n; i++)
        {
            if (inlierSet.Contains(i))
            {
                inliers.Add(points[i]);
                edgePoints[i].IsInlier = true;
            }
            else
            {
                outliers.Add(points[i]);
                edgePoints[i].IsInlier = false;
            }
        }

        result.Inliers = inliers;
        result.Outliers = outliers;

        // 计算端点
        ComputeLineEndpoints(result, inliers.ToArray(), imageSize);

        // 计算拟合误差（仅内点）
        result.FitError = ComputeFitError(inliers.ToArray(), bestVx, bestVy, bestX0, bestY0);
        result.EdgePointCount = edgePoints.Count;
        result.EdgePoints = edgePoints;

        return result;
    }

    /// <summary>
    /// 计算点到直线的距离
    /// </summary>
    private static double PointToLineDistance(PointF point, PointF linePoint, float vx, float vy)
    {
        // 直线法向量 (-vy, vx)
        double dx = point.X - linePoint.X;
        double dy = point.Y - linePoint.Y;
        return Math.Abs(-vy * dx + vx * dy);
    }

    /// <summary>
    /// 计算直线端点（基于边缘点的投影范围）
    /// </summary>
    private static void ComputeLineEndpoints(LineFitResult result, PointF[] points, Size imageSize)
    {
        float vx = result.Direction.X, vy = result.Direction.Y;
        float x0 = result.PointOnLine.X, y0 = result.PointOnLine.Y;

        // 将所有点投影到直线方向上，找最小和最大投影值
        double minT = double.MaxValue, maxT = double.MinValue;
        foreach (var p in points)
        {
            double t = (p.X - x0) * vx + (p.Y - y0) * vy;
            if (t < minT) minT = t;
            if (t > maxT) maxT = t;
        }

        // 稍微延伸一点
        double extend = (maxT - minT) * 0.05;
        minT -= extend;
        maxT += extend;

        result.Endpoint1 = new PointF(
            (float)(x0 + vx * minT),
            (float)(y0 + vy * minT));
        result.Endpoint2 = new PointF(
            (float)(x0 + vx * maxT),
            (float)(y0 + vy * maxT));
    }

    /// <summary>
    /// 计算平均拟合误差
    /// </summary>
    private static double ComputeFitError(PointF[] points, float vx, float vy, float x0, float y0)
    {
        if (points.Length == 0) return 0;

        double totalError = 0;
        foreach (var p in points)
        {
            double dx = p.X - x0;
            double dy = p.Y - y0;
            double dist = Math.Abs(-vy * dx + vx * dy);
            totalError += dist;
        }
        return totalError / points.Length;
    }
}