// ============================================================================
// Copyright © 2026 Hexagon Technology Center GmbH. All Rights Reserved.
// 文件名: EdgeCircleFitProcessor.cs
// 描述: 边缘查找拟合圆算子
// 功能:
//   - 沿预估圆周等角度放置卡尺，每个卡尺沿径向搜索边缘点
//   - 支持亚像素精度（抛物线插值）
//   - 支持边缘极性选择和搜索方向（向内/向外）
//   - 使用最小二乘或RANSAC算法拟合圆
//   - 输出拟合圆参数、边缘点、内点/外点、拟合误差
// 算法: 卡尺边缘检测 + 最小二乘/RANSAC圆拟合
// 作者: 李伟 wei.lw.li@hexagon.com
// ============================================================================

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

namespace XP.ImageProcessing.Processors;

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

    /// <summary>拟合圆心X</summary>
    public double CenterX { get; set; }

    /// <summary>拟合圆心Y</summary>
    public double CenterY { get; set; }

    /// <summary>拟合半径</summary>
    public double Radius { 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 EdgeCircleFitProcessor : ImageProcessorBase
{
    private static readonly ILogger _logger = Log.ForContext<EdgeCircleFitProcessor>();
    private static readonly Random _random = new();

    public EdgeCircleFitProcessor()
    {
        Name = LocalizationHelper.GetString("EdgeCircleFitProcessor_Name");
        Description = LocalizationHelper.GetString("EdgeCircleFitProcessor_Description");
    }

    protected override void InitializeParameters()
    {
        // ── 预估圆参数（由UI交互注入，不可见） ──
        Parameters.Add("CenterX", new ProcessorParameter(
            "CenterX", "CenterX", typeof(int), 200, null, null, "") { IsVisible = false });
        Parameters.Add("CenterY", new ProcessorParameter(
            "CenterY", "CenterY", typeof(int), 200, null, null, "") { IsVisible = false });
        Parameters.Add("Radius", new ProcessorParameter(
            "Radius", "Radius", typeof(int), 100, null, null, "") { IsVisible = false });

        // ── 卡尺参数 ──
        Parameters.Add("CaliperCount", new ProcessorParameter(
            "CaliperCount",
            LocalizationHelper.GetString("EdgeCircleFitProcessor_CaliperCount"),
            typeof(int), 36, 3, 360,
            LocalizationHelper.GetString("EdgeCircleFitProcessor_CaliperCount_Desc")));

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

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

        Parameters.Add("EdgeThreshold", new ProcessorParameter(
            "EdgeThreshold",
            LocalizationHelper.GetString("EdgeCircleFitProcessor_EdgeThreshold"),
            typeof(int), 20, 1, 255,
            LocalizationHelper.GetString("EdgeCircleFitProcessor_EdgeThreshold_Desc")));

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

        Parameters.Add("SearchDirection", new ProcessorParameter(
            "SearchDirection",
            LocalizationHelper.GetString("EdgeCircleFitProcessor_SearchDirection"),
            typeof(string), "Both", null, null,
            LocalizationHelper.GetString("EdgeCircleFitProcessor_SearchDirection_Desc"),
            new string[] { "Inward", "Outward", "Both" }));

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

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

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

    public override Image<Gray, byte> Process(Image<Gray, byte> inputImage)
    {
        int centerX = GetParameter<int>("CenterX");
        int centerY = GetParameter<int>("CenterY");
        int radius = GetParameter<int>("Radius");
        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 searchDirection = GetParameter<string>("SearchDirection");
        string fitMethod = GetParameter<string>("FitMethod");
        double ransacThreshold = GetParameter<double>("RansacThreshold");

        OutputData.Clear();

        _logger.Debug(
            "EdgeCircleFit started: Center=({CX},{CY}), R={R}, Calipers={Count}, Width={Width}",
            centerX, centerY, radius, caliperCount, caliperWidth);

        if (radius < 5)
        {
            _logger.Warning("Radius too small for circle fitting");
            OutputData["CircleFitResult"] = new CircleFitResult { Success = false };
            return inputImage.Clone();
        }

        // 沿圆周等角度放置卡尺
        var edgePoints = new List<EdgePointInfo>();
        double angleStep = 2.0 * Math.PI / caliperCount;

        for (int i = 0; i < caliperCount; i++)
        {
            double angle = angleStep * i;
            // 圆周上的采样点
            double sampleX = centerX + radius * Math.Cos(angle);
            double sampleY = centerY + radius * Math.Sin(angle);

            // 径向方向（从圆心指向外）
            double dirX = Math.Cos(angle);
            double dirY = Math.Sin(angle);

            // 根据搜索方向确定卡尺搜索方向
            double searchDirX, searchDirY;
            if (searchDirection == "Inward")
            {
                searchDirX = -dirX;
                searchDirY = -dirY;
            }
            else if (searchDirection == "Outward")
            {
                searchDirX = dirX;
                searchDirY = dirY;
            }
            else // Both: 搜索方向为径向（从内到外），卡尺中心在圆周上
            {
                searchDirX = dirX;
                searchDirY = dirY;
            }

            var edgePoint = FindEdgeInCaliper(
                inputImage, sampleX, sampleY, searchDirX, searchDirY,
                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 = FitCircle(edgePoints, fitMethod, ransacThreshold);

        // 存储输出
        OutputData["CircleFitResult"] = result;
        OutputData["EdgePoints"] = edgePoints.Select(p => p.Position).ToArray();
        OutputData["EdgePointCount"] = edgePoints.Count;
        OutputData["Thickness"] = GetParameter<int>("Thickness");

        if (result.Success)
        {
            OutputData["FittedCenterX"] = result.CenterX;
            OutputData["FittedCenterY"] = result.CenterY;
            OutputData["FittedRadius"] = result.Radius;
            OutputData["InlierPoints"] = result.Inliers.ToArray();
            OutputData["OutlierPoints"] = result.Outliers.ToArray();
            OutputData["FitError"] = result.FitError;

            _logger.Information(
                "EdgeCircleFit completed: Center=({CX:F2},{CY:F2}), R={R:F2}, Inliers={Inliers}/{Total}, Error={Error:F3}px",
                result.CenterX, result.CenterY, result.Radius,
                result.Inliers.Count, edgePoints.Count, result.FitError);
        }
        else
        {
            _logger.Warning("EdgeCircleFit failed: insufficient edge points");
        }

        return inputImage.Clone();
    }

    // ══════════════════════════════════════════════════════════════
    // 卡尺边缘检测（复用直线拟合中的逻辑）
    // ══════════════════════════════════════════════════════════════

    private EdgePointInfo? FindEdgeInCaliper(
        Image<Gray, byte> image,
        double centerX, double centerY,
        double dirX, double dirY,
        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 + dirX * offset;
            double py = centerY + dirY * 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
            };

            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 + dirX * edgeOffset);
        float edgeY = (float)(centerY + dirY * edgeOffset);

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

    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, 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;
    }

    // ══════════════════════════════════════════════════════════════
    // 圆拟合
    // ══════════════════════════════════════════════════════════════

    private CircleFitResult FitCircle(List<EdgePointInfo> edgePoints, string method, double ransacThreshold)
    {
        var result = new CircleFitResult();

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

        if (method == "RANSAC" && edgePoints.Count >= 4)
            return FitCircleRANSAC(edgePoints, ransacThreshold);
        else
            return FitCircleLeastSquares(edgePoints);
    }

    /// <summary>
    /// 最小二乘拟合圆（Kasa方法）
    /// 将 (x-a)² + (y-b)² = r² 展开为: x² + y² = 2ax + 2by + (r²-a²-b²)
    /// 令 c = r²-a²-b², 线性方程: 2ax + 2by + c = x² + y²
    /// </summary>
    private CircleFitResult FitCircleLeastSquares(List<EdgePointInfo> edgePoints)
    {
        var points = edgePoints.Select(p => p.Position).ToArray();
        var (cx, cy, r) = KasaFit(points);

        var result = new CircleFitResult
        {
            Success = true,
            CenterX = cx,
            CenterY = cy,
            Radius = r,
            Inliers = points.ToList(),
            Outliers = new List<PointF>(),
            EdgePointCount = edgePoints.Count,
            EdgePoints = edgePoints
        };

        foreach (var ep in edgePoints)
            ep.IsInlier = true;

        result.FitError = ComputeCircleFitError(points, cx, cy, r);
        return result;
    }

    /// <summary>
    /// RANSAC 圆拟合
    /// </summary>
    private CircleFitResult FitCircleRANSAC(List<EdgePointInfo> edgePoints, double threshold)
    {
        var result = new CircleFitResult();
        var points = edgePoints.Select(p => p.Position).ToArray();
        int n = points.Length;

        int maxIterations = Math.Min(2000, n * (n - 1) * (n - 2) / 6);
        int bestInlierCount = 0;
        double bestCx = 0, bestCy = 0, bestR = 0;
        List<int> bestInlierIndices = new();

        for (int iter = 0; iter < maxIterations; iter++)
        {
            // 随机选3个点
            int i1 = _random.Next(n), i2 = _random.Next(n), i3 = _random.Next(n);
            if (i1 == i2 || i1 == i3 || i2 == i3) continue;

            var (cx, cy, r) = FitCircleFrom3Points(points[i1], points[i2], points[i3]);
            if (r <= 0 || double.IsNaN(r)) continue;

            // 统计内点
            var inlierIndices = new List<int>();
            for (int i = 0; i < n; i++)
            {
                double dist = Math.Abs(Distance(points[i], cx, cy) - r);
                if (dist <= threshold)
                    inlierIndices.Add(i);
            }

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

                // 用所有内点重新拟合
                var inlierPoints = inlierIndices.Select(i => points[i]).ToArray();
                (bestCx, bestCy, bestR) = KasaFit(inlierPoints);
            }

            if (bestInlierCount > n * 0.95)
                break;
        }

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

        result.Success = true;
        result.CenterX = bestCx;
        result.CenterY = bestCy;
        result.Radius = bestR;

        var inlierSet = new HashSet<int>(bestInlierIndices);
        for (int i = 0; i < n; i++)
        {
            if (inlierSet.Contains(i))
            {
                result.Inliers.Add(points[i]);
                edgePoints[i].IsInlier = true;
            }
            else
            {
                result.Outliers.Add(points[i]);
                edgePoints[i].IsInlier = false;
            }
        }

        result.FitError = ComputeCircleFitError(result.Inliers.ToArray(), bestCx, bestCy, bestR);
        result.EdgePointCount = edgePoints.Count;
        result.EdgePoints = edgePoints;

        return result;
    }

    /// <summary>
    /// Kasa 最小二乘圆拟合
    /// </summary>
    private static (double cx, double cy, double r) KasaFit(PointF[] points)
    {
        int n = points.Length;
        if (n < 3) return (0, 0, 0);

        // 构建线性方程组: A * [a, b, c]^T = B
        // 其中 2*a*xi + 2*b*yi + c = xi² + yi²
        double sumX = 0, sumY = 0, sumX2 = 0, sumY2 = 0;
        double sumXY = 0, sumX3 = 0, sumY3 = 0, sumX2Y = 0, sumXY2 = 0;

        for (int i = 0; i < n; i++)
        {
            double x = points[i].X, y = points[i].Y;
            double x2 = x * x, y2 = y * y;
            sumX += x; sumY += y;
            sumX2 += x2; sumY2 += y2;
            sumXY += x * y;
            sumX3 += x2 * x; sumY3 += y2 * y;
            sumX2Y += x2 * y; sumXY2 += x * y2;
        }

        double A = n * sumX2 - sumX * sumX;
        double B = n * sumXY - sumX * sumY;
        double C = n * sumY2 - sumY * sumY;
        double D = 0.5 * (n * (sumX3 + sumXY2) - sumX * (sumX2 + sumY2));
        double E = 0.5 * (n * (sumX2Y + sumY3) - sumY * (sumX2 + sumY2));

        double denom = A * C - B * B;
        if (Math.Abs(denom) < 1e-10)
            return (0, 0, 0);

        double cx = (D * C - B * E) / denom;
        double cy = (A * E - B * D) / denom;
        double r = Math.Sqrt((sumX2 + sumY2 - 2 * cx * sumX - 2 * cy * sumY) / n + cx * cx + cy * cy);

        return (cx, cy, r);
    }

    /// <summary>
    /// 3点拟合圆
    /// </summary>
    private static (double cx, double cy, double r) FitCircleFrom3Points(PointF p1, PointF p2, PointF p3)
    {
        double ax = p1.X, ay = p1.Y;
        double bx = p2.X, by = p2.Y;
        double cx = p3.X, cy = p3.Y;

        double d = 2 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by));
        if (Math.Abs(d) < 1e-10)
            return (0, 0, -1);

        double ux = ((ax * ax + ay * ay) * (by - cy) + (bx * bx + by * by) * (cy - ay) + (cx * cx + cy * cy) * (ay - by)) / d;
        double uy = ((ax * ax + ay * ay) * (cx - bx) + (bx * bx + by * by) * (ax - cx) + (cx * cx + cy * cy) * (bx - ax)) / d;
        double r = Math.Sqrt((ax - ux) * (ax - ux) + (ay - uy) * (ay - uy));

        return (ux, uy, r);
    }

    private static double Distance(PointF p, double cx, double cy)
    {
        double dx = p.X - cx, dy = p.Y - cy;
        return Math.Sqrt(dx * dx + dy * dy);
    }

    private static double ComputeCircleFitError(PointF[] points, double cx, double cy, double r)
    {
        if (points.Length == 0) return 0;
        double total = 0;
        foreach (var p in points)
            total += Math.Abs(Distance(p, cx, cy) - r);
        return total / points.Length;
    }
}