using OpenCvSharp;
using OpenCvSharp.Aruco;
using TraceCad.Core.Geometry;

namespace TraceCad.Vision.Calibration;

public sealed class SheetCalibrator
{
    private static readonly PredefinedDictionaryName[] CandidateDictionaries =
    {
        PredefinedDictionaryName.Dict5X5_1000,
        PredefinedDictionaryName.Dict5X5_250,
        PredefinedDictionaryName.Dict5X5_100,
        PredefinedDictionaryName.Dict5X5_50
    };

    public SheetCalibrationResult Calibrate(
        string sourceImagePath,
        string outputDirectory,
        ReferenceSheetTemplate? template = null,
        SheetCalibrationOptions? options = null)
    {
        ArgumentException.ThrowIfNullOrWhiteSpace(sourceImagePath);
        ArgumentException.ThrowIfNullOrWhiteSpace(outputDirectory);

        template ??= ReferenceSheetTemplate.DefaultA4();
        options ??= SheetCalibrationOptions.Default;
        Directory.CreateDirectory(outputDirectory);

        using var source = Cv2.ImRead(sourceImagePath, ImreadModes.Color);
        if (source.Empty())
        {
            return Failure("Could not load calibration image.", template, options);
        }

        var detection = DetectBestDictionary(source, template, options);
        if (detection is null)
        {
            return Failure("No matching 5x5 ArUco markers with IDs 0..7 were detected.", template, options);
        }

        if (detection.MatchedMarkers < 4)
        {
            return new SheetCalibrationResult(
                false,
                $"Detected {detection.MatchedMarkers} matching markers; at least 4 are required.",
                null,
                detection.DictionaryName,
                detection.DetectedMarkers,
                detection.MatchedMarkers,
                template.WidthMm,
                template.HeightMm,
                1.0 / options.PixelsPerMillimetre,
                0.0,
                0.0,
                "none",
                null);
        }

        using var homographyMask = new Mat();
        using var homography = Cv2.FindHomography(
            detection.ImagePoints,
            detection.ModelPoints,
            HomographyMethods.Ransac,
            options.HomographyRansacThresholdPixels,
            homographyMask,
            options.HomographyRansacMaxIterations,
            options.HomographyConfidence);
        if (homography.Empty())
        {
            return Failure("Could not compute sheet homography.", template, options);
        }

        var inlierIndices = HomographyInlierIndices(homographyMask, detection.ImagePoints.Count);
        using var refinedHomography = RefineHomography(detection, homography, inlierIndices);
        var metrics = CalculateReprojectionMetrics(
            detection.ImagePoints,
            detection.ModelPoints,
            refinedHomography,
            inlierIndices,
            options.PixelsPerMillimetre);
        var outputSize = new Size(
            (int)Math.Round(template.WidthMm * options.PixelsPerMillimetre),
            (int)Math.Round(template.HeightMm * options.PixelsPerMillimetre));
        var correction = CorrectImage(source, refinedHomography, detection, outputSize, inlierIndices, options);
        using var corrected = correction.Image;

        var outputPath = Path.Combine(
            outputDirectory,
            $"{Path.GetFileNameWithoutExtension(sourceImagePath)}.calibrated.png");
        Cv2.ImWrite(outputPath, corrected);

        return new SheetCalibrationResult(
            true,
            $"Calibrated using {detection.MatchedMarkers} markers ({detection.DictionaryName}); inliers {inlierIndices.Count}/{detection.ImagePoints.Count}, marker RMS {metrics.RmsMm:0.###} mm, max {metrics.MaxMm:0.###} mm. Select the bold 30 mm line to verify scale accuracy.",
            outputPath,
            detection.DictionaryName,
            detection.DetectedMarkers,
            detection.MatchedMarkers,
            template.WidthMm,
            template.HeightMm,
            1.0 / options.PixelsPerMillimetre,
            metrics.RmsMm,
            metrics.MaxMm,
            correction.Mode,
            null);
    }

    private static Detection? DetectBestDictionary(
        Mat source,
        ReferenceSheetTemplate template,
        SheetCalibrationOptions options)
    {
        DetectionCandidate? best = null;

        foreach (var dictionaryName in CandidateDictionaries)
        {
            var dictionary = CvAruco.GetPredefinedDictionary(dictionaryName);
            var parameters = new DetectorParameters
            {
                CornerRefinementMethod = CornerRefineMethod.Subpix,
                CornerRefinementWinSize = 7,
                CornerRefinementMaxIterations = 80,
                CornerRefinementMinAccuracy = 0.005,
                UseAruco3Detection = true
            };
            CvAruco.DetectMarkers(
                source,
                dictionary,
                out var corners,
                out var ids,
                parameters,
                out _);

            if (ids.Length == 0)
            {
                continue;
            }

            var detection = BuildDetection(dictionaryName.ToString(), ids, corners, template, options.PixelsPerMillimetre);
            var candidate = ScoreDetection(detection, options);
            if (candidate is null)
            {
                candidate = new DetectionCandidate(detection, 0, double.MaxValue);
            }

            if (IsBetterCandidate(candidate, best))
            {
                best = candidate;
            }
        }

        return best?.Detection;
    }

    private static Detection BuildDetection(
        string dictionaryName,
        int[] ids,
        Point2f[][] corners,
        ReferenceSheetTemplate template,
        double pixelsPerMillimetre)
    {
        var markerById = template.Markers.ToDictionary(marker => marker.Id);
        var imagePoints = new List<Point2d>();
        var modelPoints = new List<Point2d>();
        var matchedMarkerIds = new HashSet<int>();

        for (var i = 0; i < ids.Length; i++)
        {
            if (!markerById.TryGetValue(ids[i], out var marker))
            {
                continue;
            }

            matchedMarkerIds.Add(ids[i]);
            var imageCorners = corners[i];
            var modelCorners = marker.CornersMm;
            for (var cornerIndex = 0; cornerIndex < 4; cornerIndex++)
            {
                imagePoints.Add(ToDoublePoint(imageCorners[cornerIndex]));
                modelPoints.Add(ToPixelPoint(modelCorners[cornerIndex], pixelsPerMillimetre));
            }
        }

        return new Detection(
            dictionaryName,
            ids.Length,
            matchedMarkerIds.Count,
            imagePoints,
            modelPoints);
    }

    private static Point2d ToPixelPoint(Point2 point, double pixelsPerMillimetre) =>
        new(
            point.X * pixelsPerMillimetre,
            point.Y * pixelsPerMillimetre);

    private static Point2d ToDoublePoint(Point2f point) =>
        new(point.X, point.Y);

    private static SheetCalibrationResult Failure(
        string message,
        ReferenceSheetTemplate template,
        SheetCalibrationOptions options) =>
        new(
            false,
            message,
            null,
            null,
            0,
            0,
            template.WidthMm,
            template.HeightMm,
            1.0 / options.PixelsPerMillimetre,
            0.0,
            0.0,
            "none",
            null);

    private static CalibrationQuality ValidateCalibrationQuality(
        Mat corrected,
        ReferenceSheetTemplate template,
        SheetCalibrationOptions options)
    {
        using var gray = new Mat();
        if (corrected.Channels() == 1)
        {
            corrected.CopyTo(gray);
        }
        else
        {
            Cv2.CvtColor(corrected, gray, ColorConversionCodes.BGR2GRAY);
        }

        var controlLine = DetectControlLine(gray, template, options.PixelsPerMillimetre);
        var crosshair = DetectCrosshair(gray, template, options.PixelsPerMillimetre);
        var controlLineErrorMm = controlLine.Detected
            ? controlLine.MeasuredMm - template.ReferenceLineLengthMm
            : double.NaN;
        var crosshairOffsetMm = crosshair.Detected
            ? Distance(crosshair.Position, ToPixelPoint(template.CrosshairMm, options.PixelsPerMillimetre)) / options.PixelsPerMillimetre
            : double.NaN;
        var meetsTarget =
            controlLine.Detected &&
            Math.Abs(controlLineErrorMm) <= options.TargetAccuracyMm;

        return new CalibrationQuality(
            controlLine.Detected,
            template.ReferenceLineLengthMm,
            controlLine.MeasuredMm,
            controlLineErrorMm,
            crosshair.Detected,
            crosshairOffsetMm,
            meetsTarget);
    }

    private static ControlLineDetection DetectControlLine(
        Mat gray,
        ReferenceSheetTemplate template,
        double pixelsPerMillimetre)
    {
        var start = ToPixelPoint(template.ReferenceLineStartMm, pixelsPerMillimetre);
        var end = ToPixelPoint(template.ReferenceLineEndMm, pixelsPerMillimetre);
        var yCenter = ToPixelPoint(template.CrosshairMm, pixelsPerMillimetre).Y;
        var xPadding = 4.0 * pixelsPerMillimetre;
        var yPadding = 2.0 * pixelsPerMillimetre;
        var rect = ClampRect(
            Math.Min(start.X, end.X) - xPadding,
            yCenter - yPadding,
            Math.Abs(end.X - start.X) + (xPadding * 2.0),
            yPadding * 2.0,
            gray.Size());

        if (rect.Width <= 0 || rect.Height <= 0)
        {
            return new ControlLineDetection(false, double.NaN);
        }

        var minX = double.MaxValue;
        var maxX = double.MinValue;
        var darkPixels = 0;
        using var roi = new Mat(gray, rect);
        for (var y = 0; y < roi.Height; y++)
        {
            for (var x = 0; x < roi.Width; x++)
            {
                if (roi.Get<byte>(y, x) > 110)
                {
                    continue;
                }

                var absoluteX = rect.X + x;
                minX = Math.Min(minX, absoluteX);
                maxX = Math.Max(maxX, absoluteX);
                darkPixels++;
            }
        }

        if (darkPixels < Math.Max(8, template.ReferenceLineLengthMm * pixelsPerMillimetre * 0.25))
        {
            return new ControlLineDetection(false, double.NaN);
        }

        return new ControlLineDetection(true, (maxX - minX) / pixelsPerMillimetre);
    }

    private static CrosshairDetection DetectCrosshair(
        Mat gray,
        ReferenceSheetTemplate template,
        double pixelsPerMillimetre)
    {
        var expected = ToPixelPoint(template.CrosshairMm, pixelsPerMillimetre);
        var searchRadius = 8.0 * pixelsPerMillimetre;
        var axisBand = Math.Max(1.5, 0.35 * pixelsPerMillimetre);
        var rect = ClampRect(
            expected.X - searchRadius,
            expected.Y - searchRadius,
            searchRadius * 2.0,
            searchRadius * 2.0,
            gray.Size());

        if (rect.Width <= 0 || rect.Height <= 0)
        {
            return new CrosshairDetection(false, default);
        }

        var verticalSumX = 0.0;
        var verticalCount = 0;
        var horizontalSumY = 0.0;
        var horizontalCount = 0;
        using var roi = new Mat(gray, rect);
        for (var y = 0; y < roi.Height; y++)
        {
            for (var x = 0; x < roi.Width; x++)
            {
                if (roi.Get<byte>(y, x) > 110)
                {
                    continue;
                }

                var absoluteX = rect.X + x;
                var absoluteY = rect.Y + y;
                if (Math.Abs(absoluteX - expected.X) <= axisBand)
                {
                    verticalSumX += absoluteX;
                    verticalCount++;
                }

                if (Math.Abs(absoluteY - expected.Y) <= axisBand)
                {
                    horizontalSumY += absoluteY;
                    horizontalCount++;
                }
            }
        }

        if (verticalCount < 4 || horizontalCount < 4)
        {
            return new CrosshairDetection(false, default);
        }

        return new CrosshairDetection(
            true,
            new Point2d(verticalSumX / verticalCount, horizontalSumY / horizontalCount));
    }

    private static Rect ClampRect(double x, double y, double width, double height, Size bounds)
    {
        var left = Math.Max(0, (int)Math.Floor(x));
        var top = Math.Max(0, (int)Math.Floor(y));
        var right = Math.Min(bounds.Width, (int)Math.Ceiling(x + width));
        var bottom = Math.Min(bounds.Height, (int)Math.Ceiling(y + height));
        return new Rect(left, top, Math.Max(0, right - left), Math.Max(0, bottom - top));
    }

    private static CorrectionResult CorrectImage(
        Mat source,
        Mat homography,
        Detection detection,
        Size outputSize,
        IReadOnlyList<int> inlierIndices,
        SheetCalibrationOptions options)
    {
        if (inlierIndices.Count < PolynomialTermCount(1))
        {
            var perspectiveOnly = new Mat();
            Cv2.WarpPerspective(source, perspectiveOnly, homography, outputSize);
            return new CorrectionResult(perspectiveOnly, "perspective");
        }

        using var inverseHomography = homography.Inv();
        var inverse = ReadHomography(inverseHomography);
        var modelPoints = inlierIndices.Select(index => detection.ModelPoints[index]).ToArray();
        var residualX = new double[modelPoints.Length];
        var residualY = new double[modelPoints.Length];

        for (var i = 0; i < modelPoints.Length; i++)
        {
            var index = inlierIndices[i];
            var projected = ApplyHomography(inverse, detection.ModelPoints[index]);
            residualX[i] = detection.ImagePoints[index].X - projected.X;
            residualY[i] = detection.ImagePoints[index].Y - projected.Y;
        }

        var selection = SelectResidualCorrection(
            modelPoints,
            residualX,
            residualY,
            outputSize,
            options);
        if (selection is null)
        {
            var perspectiveOnly = new Mat();
            Cv2.WarpPerspective(source, perspectiveOnly, homography, outputSize);
            return new CorrectionResult(perspectiveOnly, "perspective");
        }

        var xSurface = PolynomialSurface.Fit(
            modelPoints,
            residualX,
            outputSize,
            selection.Degree,
            options.ResidualRegularization);
        var ySurface = PolynomialSurface.Fit(
            modelPoints,
            residualY,
            outputSize,
            selection.Degree,
            options.ResidualRegularization);
        if (xSurface is null || ySurface is null)
        {
            var perspectiveOnly = new Mat();
            Cv2.WarpPerspective(source, perspectiveOnly, homography, outputSize);
            return new CorrectionResult(perspectiveOnly, "perspective");
        }

        using var mapX = new Mat(outputSize.Height, outputSize.Width, MatType.CV_32FC1);
        using var mapY = new Mat(outputSize.Height, outputSize.Width, MatType.CV_32FC1);

        for (var y = 0; y < outputSize.Height; y++)
        {
            for (var x = 0; x < outputSize.Width; x++)
            {
                var modelPoint = new Point2d(x, y);
                var projected = ApplyHomography(inverse, modelPoint);
                mapX.Set(y, x, (float)(projected.X + xSurface.Evaluate(modelPoint)));
                mapY.Set(y, x, (float)(projected.Y + ySurface.Evaluate(modelPoint)));
            }
        }

        var corrected = new Mat();
        Cv2.Remap(
            source,
            corrected,
            mapX,
            mapY,
            InterpolationFlags.Linear,
            BorderTypes.Constant,
            Scalar.White);
        return new CorrectionResult(
            corrected,
            $"perspective + cross-validated residual warp degree {selection.Degree}");
    }

    private static DetectionCandidate? ScoreDetection(Detection detection, SheetCalibrationOptions options)
    {
        if (detection.MatchedMarkers < 4)
        {
            return null;
        }

        using var mask = new Mat();
        using var homography = Cv2.FindHomography(
            detection.ImagePoints,
            detection.ModelPoints,
            HomographyMethods.Ransac,
            options.HomographyRansacThresholdPixels,
            mask,
            options.HomographyRansacMaxIterations,
            options.HomographyConfidence);
        if (homography.Empty())
        {
            return null;
        }

        var inlierIndices = HomographyInlierIndices(mask, detection.ImagePoints.Count);
        var metrics = CalculateReprojectionMetrics(
            detection.ImagePoints,
            detection.ModelPoints,
            homography,
            inlierIndices,
            options.PixelsPerMillimetre);
        return new DetectionCandidate(detection, inlierIndices.Count, metrics.RmsMm);
    }

    private static bool IsBetterCandidate(DetectionCandidate candidate, DetectionCandidate? best)
    {
        if (best is null)
        {
            return true;
        }

        if (candidate.Detection.MatchedMarkers != best.Detection.MatchedMarkers)
        {
            return candidate.Detection.MatchedMarkers > best.Detection.MatchedMarkers;
        }

        if (candidate.InlierPointCount != best.InlierPointCount)
        {
            return candidate.InlierPointCount > best.InlierPointCount;
        }

        return candidate.RmsMm < best.RmsMm;
    }

    private static Mat RefineHomography(Detection detection, Mat roughHomography, IReadOnlyList<int> inlierIndices)
    {
        if (inlierIndices.Count < 4)
        {
            return roughHomography.Clone();
        }

        var imagePoints = inlierIndices.Select(index => detection.ImagePoints[index]).ToArray();
        var modelPoints = inlierIndices.Select(index => detection.ModelPoints[index]).ToArray();
        using var refined = Cv2.FindHomography(imagePoints, modelPoints, (HomographyMethods)0);
        return refined.Empty() ? roughHomography.Clone() : refined.Clone();
    }

    private static IReadOnlyList<int> HomographyInlierIndices(Mat mask, int pointCount)
    {
        if (mask.Empty())
        {
            return Enumerable.Range(0, pointCount).ToArray();
        }

        var indices = new List<int>();
        for (var i = 0; i < pointCount; i++)
        {
            if (mask.Get<byte>(i, 0) != 0)
            {
                indices.Add(i);
            }
        }

        return indices.Count == 0 ? Enumerable.Range(0, pointCount).ToArray() : indices;
    }

    private static ResidualCorrectionSelection? SelectResidualCorrection(
        IReadOnlyList<Point2d> modelPoints,
        IReadOnlyList<double> residualX,
        IReadOnlyList<double> residualY,
        Size outputSize,
        SheetCalibrationOptions options)
    {
        var baselineRmsMm = ResidualRmsMm(residualX, residualY, options.PixelsPerMillimetre);
        ResidualCorrectionSelection? best = null;

        for (var degree = 1; degree <= options.MaxResidualPolynomialDegree; degree++)
        {
            if (modelPoints.Count < PolynomialTermCount(degree) + 1)
            {
                continue;
            }

            var validationRmsMm = CrossValidatedResidualRmsMm(
                modelPoints,
                residualX,
                residualY,
                outputSize,
                degree,
                options);

            if (double.IsNaN(validationRmsMm) || double.IsInfinity(validationRmsMm))
            {
                continue;
            }

            if (best is null || validationRmsMm < best.CrossValidatedRmsMm)
            {
                best = new ResidualCorrectionSelection(degree, validationRmsMm);
            }
        }

        if (best is null ||
            best.CrossValidatedRmsMm > baselineRmsMm - options.MinimumResidualImprovementMm)
        {
            return null;
        }

        return best;
    }

    private static double ResidualRmsMm(
        IReadOnlyList<double> residualX,
        IReadOnlyList<double> residualY,
        double pixelsPerMillimetre)
    {
        var sumSquared = 0.0;
        for (var i = 0; i < residualX.Count; i++)
        {
            sumSquared += (residualX[i] * residualX[i]) + (residualY[i] * residualY[i]);
        }

        return residualX.Count == 0
            ? 0.0
            : Math.Sqrt(sumSquared / residualX.Count) / pixelsPerMillimetre;
    }

    private static double CrossValidatedResidualRmsMm(
        IReadOnlyList<Point2d> modelPoints,
        IReadOnlyList<double> residualX,
        IReadOnlyList<double> residualY,
        Size outputSize,
        int degree,
        SheetCalibrationOptions options)
    {
        var sumSquaredPixels = 0.0;
        var evaluated = 0;

        for (var holdout = 0; holdout < modelPoints.Count; holdout++)
        {
            var trainPoints = new List<Point2d>(modelPoints.Count - 1);
            var trainResidualX = new List<double>(modelPoints.Count - 1);
            var trainResidualY = new List<double>(modelPoints.Count - 1);

            for (var i = 0; i < modelPoints.Count; i++)
            {
                if (i == holdout)
                {
                    continue;
                }

                trainPoints.Add(modelPoints[i]);
                trainResidualX.Add(residualX[i]);
                trainResidualY.Add(residualY[i]);
            }

            var xSurface = PolynomialSurface.Fit(
                trainPoints,
                trainResidualX,
                outputSize,
                degree,
                options.ResidualRegularization);
            var ySurface = PolynomialSurface.Fit(
                trainPoints,
                trainResidualY,
                outputSize,
                degree,
                options.ResidualRegularization);
            if (xSurface is null || ySurface is null)
            {
                continue;
            }

            var dx = residualX[holdout] - xSurface.Evaluate(modelPoints[holdout]);
            var dy = residualY[holdout] - ySurface.Evaluate(modelPoints[holdout]);
            sumSquaredPixels += (dx * dx) + (dy * dy);
            evaluated++;
        }

        return evaluated == 0
            ? double.NaN
            : Math.Sqrt(sumSquaredPixels / evaluated) / options.PixelsPerMillimetre;
    }

    private static ReprojectionMetrics CalculateReprojectionMetrics(
        IReadOnlyList<Point2d> imagePoints,
        IReadOnlyList<Point2d> modelPoints,
        Mat homography,
        IReadOnlyList<int> pointIndices,
        double pixelsPerMillimetre)
    {
        var h = ReadHomography(homography);
        var sumSquaredMm = 0.0;
        var maxMm = 0.0;

        foreach (var index in pointIndices)
        {
            var projected = ApplyHomography(h, imagePoints[index]);
            var errorMm = Distance(projected, modelPoints[index]) / pixelsPerMillimetre;
            sumSquaredMm += errorMm * errorMm;
            maxMm = Math.Max(maxMm, errorMm);
        }

        var rmsMm = pointIndices.Count == 0 ? 0.0 : Math.Sqrt(sumSquaredMm / pointIndices.Count);
        return new ReprojectionMetrics(rmsMm, maxMm);
    }

    private static double[] ReadHomography(Mat homography) =>
        new[]
        {
            homography.At<double>(0, 0),
            homography.At<double>(0, 1),
            homography.At<double>(0, 2),
            homography.At<double>(1, 0),
            homography.At<double>(1, 1),
            homography.At<double>(1, 2),
            homography.At<double>(2, 0),
            homography.At<double>(2, 1),
            homography.At<double>(2, 2)
        };

    private static Point2d ApplyHomography(IReadOnlyList<double> h, Point2d point)
    {
        var denominator = (h[6] * point.X) + (h[7] * point.Y) + h[8];
        if (Math.Abs(denominator) < 1e-12)
        {
            return point;
        }

        return new Point2d(
            ((h[0] * point.X) + (h[1] * point.Y) + h[2]) / denominator,
            ((h[3] * point.X) + (h[4] * point.Y) + h[5]) / denominator);
    }

    private static double Distance(Point2d a, Point2d b)
    {
        var dx = a.X - b.X;
        var dy = a.Y - b.Y;
        return Math.Sqrt((dx * dx) + (dy * dy));
    }

    private static int PolynomialTermCount(int degree) =>
        degree switch
        {
            0 => 1,
            1 => 3,
            2 => 6,
            _ => 10
        };

    private sealed record Detection(
        string DictionaryName,
        int DetectedMarkers,
        int MatchedMarkers,
        IReadOnlyList<Point2d> ImagePoints,
        IReadOnlyList<Point2d> ModelPoints);

    private sealed record DetectionCandidate(Detection Detection, int InlierPointCount, double RmsMm);

    private sealed record CorrectionResult(Mat Image, string Mode);

    private sealed record ControlLineDetection(bool Detected, double MeasuredMm);

    private sealed record CrosshairDetection(bool Detected, Point2d Position);

    private sealed record ResidualCorrectionSelection(int Degree, double CrossValidatedRmsMm);

    private sealed record ReprojectionMetrics(double RmsMm, double MaxMm);

    private sealed class PolynomialSurface
    {
        private readonly double[] _coefficients;
        private readonly Size _size;
        private readonly int _degree;

        private PolynomialSurface(double[] coefficients, Size size, int degree)
        {
            _coefficients = coefficients;
            _size = size;
            _degree = degree;
        }

        public static PolynomialSurface? Fit(
            IReadOnlyList<Point2d> points,
            IReadOnlyList<double> values,
            Size size,
            int requestedDegree,
            double regularization)
        {
            var degree = requestedDegree;
            while (degree > 0 && points.Count < PolynomialTermCount(degree))
            {
                degree--;
            }

            var termCount = PolynomialTermCount(degree);
            var normal = new double[termCount, termCount];
            var rhs = new double[termCount];

            for (var i = 0; i < points.Count; i++)
            {
                var basis = Basis(points[i], size, degree);
                for (var row = 0; row < termCount; row++)
                {
                    rhs[row] += basis[row] * values[i];
                    for (var column = 0; column < termCount; column++)
                    {
                        normal[row, column] += basis[row] * basis[column];
                    }
                }
            }

            for (var i = 0; i < termCount; i++)
            {
                normal[i, i] += regularization;
            }

            var coefficients = Solve(normal, rhs);
            return coefficients is null ? null : new PolynomialSurface(coefficients, size, degree);
        }

        public double Evaluate(Point2d point)
        {
            var basis = Basis(point, _size, _degree);
            var value = 0.0;
            for (var i = 0; i < _coefficients.Length; i++)
            {
                value += _coefficients[i] * basis[i];
            }

            return value;
        }

        private static double[] Basis(Point2d point, Size size, int degree)
        {
            var x = ((point.X / Math.Max(1.0, size.Width - 1.0)) * 2.0) - 1.0;
            var y = ((point.Y / Math.Max(1.0, size.Height - 1.0)) * 2.0) - 1.0;

            return degree switch
            {
                0 => new[] { 1.0 },
                1 => new[] { 1.0, x, y },
                2 => new[] { 1.0, x, y, x * x, x * y, y * y },
                _ => new[]
                {
                    1.0,
                    x,
                    y,
                    x * x,
                    x * y,
                    y * y,
                    x * x * x,
                    x * x * y,
                    x * y * y,
                    y * y * y
                }
            };
        }

        private static double[]? Solve(double[,] matrix, double[] rhs)
        {
            var n = rhs.Length;
            var augmented = new double[n, n + 1];
            for (var row = 0; row < n; row++)
            {
                for (var column = 0; column < n; column++)
                {
                    augmented[row, column] = matrix[row, column];
                }

                augmented[row, n] = rhs[row];
            }

            for (var pivot = 0; pivot < n; pivot++)
            {
                var bestRow = pivot;
                var bestValue = Math.Abs(augmented[pivot, pivot]);
                for (var row = pivot + 1; row < n; row++)
                {
                    var value = Math.Abs(augmented[row, pivot]);
                    if (value > bestValue)
                    {
                        bestRow = row;
                        bestValue = value;
                    }
                }

                if (bestValue < 1e-12)
                {
                    return null;
                }

                if (bestRow != pivot)
                {
                    for (var column = pivot; column <= n; column++)
                    {
                        (augmented[pivot, column], augmented[bestRow, column]) =
                            (augmented[bestRow, column], augmented[pivot, column]);
                    }
                }

                var pivotValue = augmented[pivot, pivot];
                for (var column = pivot; column <= n; column++)
                {
                    augmented[pivot, column] /= pivotValue;
                }

                for (var row = 0; row < n; row++)
                {
                    if (row == pivot)
                    {
                        continue;
                    }

                    var factor = augmented[row, pivot];
                    for (var column = pivot; column <= n; column++)
                    {
                        augmented[row, column] -= factor * augmented[pivot, column];
                    }
                }
            }

            var solution = new double[n];
            for (var row = 0; row < n; row++)
            {
                solution[row] = augmented[row, n];
            }

            return solution;
        }
    }
}
