// ============================================================================
// 文件名: ChessboardCalibrator.cs
// 描述: 棋盘格标定器，实现基于棋盘格的相机内参标定
// 功能:
//   - 从多张棋盘格图像中检测角点
//   - 计算相机内参矩阵和畸变系数
//   - 图像去畸变处理
//   - 计算重投影误差评估标定质量
//   - 标定结果的保存和加载（JSON格式）
// 算法: 使用 Zhang's 标定方法进行相机标定
// ============================================================================

using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;
using Emgu.CV.Util;
using System.IO;
using System.Runtime.InteropServices;
using System.Text.Json;

namespace XP.Camera.Calibration;

public class ChessboardCalibrator
{
    public class CalibrationResult
    {
        public double[][] CameraMatrix { get; set; } = new double[3][];
        public double[] DistortionCoeffs { get; set; } = Array.Empty<double>();
        public double ReprojectionError { get; set; }
        public DateTime CalibrationTime { get; set; }
    }

    private Mat? _cameraMatrix;
    private Mat? _distCoeffs;
    private double _reprojectionError;
    private List<double> _perImageErrors = new List<double>();

    public double ReprojectionError => _reprojectionError;
    public List<double> PerImageErrors => _perImageErrors;

    // 进度报告委托
    public delegate void ProgressReportHandler(int current, int total, string message);

    public event ProgressReportHandler? ProgressChanged;

    public bool CalibrateFromImages(List<string> imagePaths, int boardWidth, int boardHeight, float squareSize, out string errorMsg)
    {
        errorMsg = "";
        var objectPoints = new VectorOfVectorOfPoint3D32F();
        var imagePoints = new VectorOfVectorOfPointF();
        var imageSize = new System.Drawing.Size();

        var objp = new MCvPoint3D32f[boardWidth * boardHeight];
        for (int i = 0; i < boardHeight; i++)
            for (int j = 0; j < boardWidth; j++)
                objp[i * boardWidth + j] = new MCvPoint3D32f(j * squareSize, i * squareSize, 0);

        int validImages = 0;
        int totalImages = imagePaths.Count;

        // 第一阶段：检测角点
        for (int idx = 0; idx < totalImages; idx++)
        {
            var path = imagePaths[idx];
            ProgressChanged?.Invoke(idx + 1, totalImages * 2, $"检测角点 ({idx + 1}/{totalImages})");

            var img = CvInvoke.Imread(path, ImreadModes.Grayscale);
            if (img.IsEmpty) continue;

            imageSize = img.Size;
            var corners = new VectorOfPointF();

            if (CvInvoke.FindChessboardCorners(img, new System.Drawing.Size(boardWidth, boardHeight), corners))
            {
                // 亚像素级角点精化
                CvInvoke.CornerSubPix(img, corners, new System.Drawing.Size(5, 5), new System.Drawing.Size(-1, -1),
                    new MCvTermCriteria(30, 0.001));

                objectPoints.Push(new VectorOfPoint3D32F(objp));
                imagePoints.Push(corners);
                validImages++;
            }
            img.Dispose();
        }

        if (validImages < 3)
        {
            errorMsg = $"有效图像不足，需要至少3张，当前{validImages}张";
            ProgressChanged?.Invoke(0, 100, "标定失败");
            return false;
        }

        // 第二阶段：执行标定
        ProgressChanged?.Invoke(totalImages, totalImages * 2, "执行相机标定...");

        _cameraMatrix = new Mat();
        _distCoeffs = new Mat();
        var rvecs = new VectorOfMat();
        var tvecs = new VectorOfMat();

        _reprojectionError = CvInvoke.CalibrateCamera(objectPoints, imagePoints, imageSize, _cameraMatrix, _distCoeffs,
            rvecs, tvecs, CalibType.Default, new MCvTermCriteria(30, 1e-6));

        // 第三阶段：计算每张图像的重投影误差
        _perImageErrors.Clear();
        for (int i = 0; i < objectPoints.Size; i++)
        {
            ProgressChanged?.Invoke(totalImages + i + 1, totalImages * 2, $"计算重投影误差 ({i + 1}/{objectPoints.Size})");

            var projectedPoints = new VectorOfPointF();
            CvInvoke.ProjectPoints(objectPoints[i], rvecs[i], tvecs[i], _cameraMatrix, _distCoeffs, projectedPoints);

            double error = 0;
            for (int j = 0; j < projectedPoints.Size; j++)
            {
                var dx = imagePoints[i][j].X - projectedPoints[j].X;
                var dy = imagePoints[i][j].Y - projectedPoints[j].Y;
                error += Math.Sqrt(dx * dx + dy * dy);
            }
            _perImageErrors.Add(error / projectedPoints.Size);
        }

        ProgressChanged?.Invoke(totalImages * 2, totalImages * 2, "标定完成");
        return true;
    }

    public Mat UndistortImage(Mat inputImage)
    {
        if (_cameraMatrix == null || _distCoeffs == null)
            return inputImage;

        var output = new Mat();
        CvInvoke.Undistort(inputImage, output, _cameraMatrix, _distCoeffs);
        return output;
    }

    public Mat? DrawChessboardCorners(string imagePath, int boardWidth, int boardHeight)
    {
        var img = CvInvoke.Imread(imagePath);
        if (img.IsEmpty) return null;

        var gray = new Mat();
        CvInvoke.CvtColor(img, gray, ColorConversion.Bgr2Gray);
        var corners = new VectorOfPointF();

        if (CvInvoke.FindChessboardCorners(gray, new System.Drawing.Size(boardWidth, boardHeight), corners))
        {
            CvInvoke.CornerSubPix(gray, corners, new System.Drawing.Size(11, 11), new System.Drawing.Size(-1, -1),
                new MCvTermCriteria(30, 0.001));
            CvInvoke.DrawChessboardCorners(img, new System.Drawing.Size(boardWidth, boardHeight), corners, true);
        }

        gray.Dispose();
        return img;
    }

    public void SaveCalibration(string filePath)
    {
        if (_cameraMatrix == null || _distCoeffs == null)
            throw new InvalidOperationException("请先执行标定");

        var result = new CalibrationResult
        {
            CameraMatrix = new double[3][],
            DistortionCoeffs = new double[_distCoeffs.Rows],
            ReprojectionError = _reprojectionError,
            CalibrationTime = DateTime.Now
        };

        for (int i = 0; i < 3; i++)
        {
            result.CameraMatrix[i] = new double[3];
            for (int j = 0; j < 3; j++)
            {
                double[] data = new double[1];
                Marshal.Copy(_cameraMatrix.DataPointer + (i * 3 + j) * 8, data, 0, 1);
                result.CameraMatrix[i][j] = data[0];
            }
        }

        for (int i = 0; i < _distCoeffs.Rows; i++)
        {
            double[] data = new double[1];
            Marshal.Copy(_distCoeffs.DataPointer + i * 8, data, 0, 1);
            result.DistortionCoeffs[i] = data[0];
        }

        var json = JsonSerializer.Serialize(result, new JsonSerializerOptions { WriteIndented = true });
        File.WriteAllText(filePath, json);
    }

    public bool LoadCalibration(string filePath)
    {
        if (!File.Exists(filePath)) return false;

        try
        {
            var json = File.ReadAllText(filePath);
            var result = JsonSerializer.Deserialize<CalibrationResult>(json);

            if (result == null) return false;

            _reprojectionError = result.ReprojectionError;

            _cameraMatrix = new Mat(3, 3, DepthType.Cv64F, 1);
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    Marshal.Copy(new[] { result.CameraMatrix[i][j] }, 0, _cameraMatrix.DataPointer + (i * 3 + j) * 8, 1);

            _distCoeffs = new Mat(result.DistortionCoeffs.Length, 1, DepthType.Cv64F, 1);
            for (int i = 0; i < result.DistortionCoeffs.Length; i++)
                Marshal.Copy(new[] { result.DistortionCoeffs[i] }, 0, _distCoeffs.DataPointer + i * 8, 1);

            return true;
        }
        catch
        {
            return false;
        }
    }
}