using MathNet.Numerics.LinearAlgebra;
using OpenCvSharp;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;
using TeamAAS.Robot.Core;
using TeamAAS.Robot.Enums;
using TeamAAS.Robot.Interfaces;
using TeamAAS.Robot.Models;
using TeamAAS.Robot.Models.Robot;
using TeamAAS.Vision.Calibration.Enums;
using TeamAAS.Vision.Calibration.Models;

namespace TeamAAS.Vision.Calibration.Services
{
    /// <summary>
    /// 相机标定算法服务（自 TeamAAS-2.0 移植，保持数值行为一致）。
    ///
    /// 与视觉库/相机/UI 解耦：
    /// - 机器人动作走注入的 <see cref="IRobot"/>；
    /// - 拍照取点走 <see cref="CaptureAndProcessCallback"/>（由宿主用 ICamera + 视觉引擎实现）；
    /// - 需要人工摆放标定块的提示走 <see cref="PromptPlaceCalibrationBlockCallback"/>；
    /// - 线性代数用 MathNet（工具坐标换算复用 Robot 层 ToolCoord），九点仿射用 OpenCvSharp。
    /// </summary>
    public class CameraCalibrationService
    {
        /// <summary>
        /// 触发相机拍照并进行图像处理的回调。
        /// 字段含义：IsSuccess 是否成功；X/Y 特征点像素坐标；U 特征朝向；ImageWidth/ImageHeight 图像尺寸。
        /// </summary>
        public Func<Task<(bool IsSuccess, double X, double Y, double U, int ImageWidth, int ImageHeight)>> CaptureAndProcessCallback { get; set; }

        /// <summary>
        /// 提示人工"将标定块放到中心/吸嘴下"并等待确认的回调；返回 true 表示已摆好可继续。
        /// 未设置时视为自动跳过（无需人工确认）。
        /// </summary>
        public Func<Task<bool>> PromptPlaceCalibrationBlockCallback { get; set; }

        /// <summary>由 <see cref="RPoint"/> 模板 + 平面坐标构造一个标定点（Z/U/V/W/Hand/Local/Tool 沿用模板）。</summary>
        private static RPoint MakePoint(int number, Vector<double> p, RPoint template)
        {
            return new RPoint
            {
                Number = number,
                X = (float)p[0],
                Y = (float)p[1],
                Z = template.Z,
                U = template.U,
                V = template.V,
                W = template.W,
                Hand = template.Hand,
                Local = template.Local,
                Tool = template.Tool
            };
        }

        private async Task<bool> PromptPlaceBlockAsync()
        {
            if (PromptPlaceCalibrationBlockCallback == null) return true;
            return await PromptPlaceCalibrationBlockCallback();
        }

        /// <summary>
        /// 固定向下相机校准工具坐标（相机固定俯视、工具不随 J4 移动）。
        /// </summary>
        public async Task<(bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY)> CalibrateFixedDownCameraTool(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            double pixelScaleX = 0;
            double pixelScaleY = 0;
            Matrix<double> RobotCameraRotationMatrix = null;

            // 移动机器人至待机位置。
            bool isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
            if (!isSuccess) return (false, null, 0, 0, 0, 0);
            if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);

            if (calibrationInfo.Pick.PickPlaceModel == PickPlaceModel.Suction)
            {
                // 打开吸气。
                await robot.CalibOutIOAsync(true);
            }
            else
            {
                // 张开夹爪（张开为 false，闭合为 true）。
                await robot.CalibOutIOAsync(false);
            }

            if (!await PromptPlaceBlockAsync()) return (false, null, 0, 0, 0, 0);

            if (calibrationInfo.Pick.PickPlaceModel == PickPlaceModel.Clamp)
            {
                // 夹紧夹爪。
                await robot.CalibOutIOAsync(true);
            }

            // 计算相机与机器人坐标系的旋转矩阵。
            var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
            if (!robotCameraRotationMatrix.IsSuccess) return (false, null, 0, 0, 0, 0);
            pixelScaleX = robotCameraRotationMatrix.PixelScaleX;
            pixelScaleY = robotCameraRotationMatrix.PixelScaleY;
            RobotCameraRotationMatrix = robotCameraRotationMatrix.RotationMatrix;

            // 相机拍照并处理图像。
            var photoResult = await CaptureAndProcessCallback();
            if (!photoResult.IsSuccess) return (false, null, 0, 0, 0, 0);
            PointF P0 = new PointF((float)(photoResult.X), (float)(photoResult.Y));

            RPoint RobotU = calibrationInfo.CenterPoint.Clone();
            RobotU.U += (float)calibrationInfo.Angle;
            // 移动机器人至中心点。
            isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
            if (!isSuccess) return (false, null, 0, 0, 0, 0);
            if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);
            // 打开吸气。
            await robot.CalibOutIOAsync(true);

            // 机器人 U 轴旋转。
            isSuccess = await robot.CalibMotionAsync(RobotU, null);
            if (!isSuccess) return (false, null, 0, 0, 0, 0);
            if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);

            // 关闭吸气。
            await robot.CalibOutIOAsync(false);
            // 移动机器人至待机位置。
            isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
            if (!isSuccess) return (false, null, 0, 0, 0, 0);
            if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);

            // 相机拍照并处理图像。
            photoResult = await CaptureAndProcessCallback();
            if (!photoResult.IsSuccess) return (false, null, 0, 0, 0, 0);
            PointF curPixel = new PointF((float)(photoResult.X), (float)(photoResult.Y));

            bool IsFinish = false;
            for (int i = 0; i < 10; i++)
            {
                // 计算需要移动的偏移量，逼近中心点。
                double offsetx = (P0.X - curPixel.X) * pixelScaleX;
                double offsety = (P0.Y - curPixel.Y) * pixelScaleY;
                var pos = RobotCameraRotationMatrix.Inverse() * Vector<double>.Build.Dense(new double[] { offsetx, offsety });
                // 移动机器人取标定块。
                isSuccess = await robot.CalibMotionAsync(RobotU, null);
                if (!isSuccess) return (false, null, 0, 0, 0, 0);
                if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);
                // 打开吸气。
                await robot.CalibOutIOAsync(true);

                RobotU.X += (float)pos[0];
                RobotU.Y += (float)pos[1];
                // 机器人移动。
                isSuccess = await robot.CalibMotionAsync(RobotU, null);
                if (!isSuccess) return (false, null, 0, 0, 0, 0);
                if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);

                // 关闭吸气。
                await robot.CalibOutIOAsync(false);
                // 移动机器人至待机位置。
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
                if (!isSuccess) return (false, null, 0, 0, 0, 0);
                if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);

                photoResult = await CaptureAndProcessCallback();
                if (!photoResult.IsSuccess) return (false, null, 0, 0, 0, 0);
                curPixel = new PointF((float)(photoResult.X), (float)(photoResult.Y));
                offsetx = P0.X - curPixel.X;
                offsety = P0.Y - curPixel.Y;
                if (Math.Abs(offsetx) < 1 && Math.Abs(offsety) < 1)
                {
                    // 移动机器人取标定块。
                    isSuccess = await robot.CalibMotionAsync(RobotU, null);
                    if (!isSuccess) return (false, null, 0, 0, 0, 0);
                    if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);
                    // 打开吸气。
                    await robot.CalibOutIOAsync(true);
                    // 移动机器人至待机位置。
                    isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
                    if (!isSuccess) return (false, null, 0, 0, 0, 0);
                    IsFinish = true;
                    break;
                }
                if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);
            }

            if (IsFinish)
            {
                Vector<double> p0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { RobotU.X, RobotU.Y });
                ToolCoord toolCoord = new ToolCoord();
                double u1 = calibrationInfo.CenterPoint.U;
                double u2 = RobotU.U;
                if (robot.Brand == RobotBrand.Schneider)
                {
                    u1 *= -1;
                    u2 *= -1;
                }
                toolCoord.ComputeTool(p0, p1, u1, u2);
                if (calibrationInfo.Tool == null) calibrationInfo.Tool = new RobotTool();
                calibrationInfo.Tool.X = toolCoord.X;
                calibrationInfo.Tool.Y = toolCoord.Y;
                return (true, RobotCameraRotationMatrix, calibrationInfo.Tool.X, calibrationInfo.Tool.Y, pixelScaleX, pixelScaleY);
            }
            return (false, null, 0, 0, 0, 0);
        }

        /// <summary>
        /// J4 移动向下相机校准工具坐标（相机/工具随 J4 运动，需考虑 J4 旋转偏转）。
        /// </summary>
        public async Task<(bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY)> CalibrateJ4MovingDownCameraTool(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            double pixelScaleX = 0;
            double pixelScaleY = 0;
            Matrix<double> RobotCameraRotationMatrix = null;

            // 移动机器人至中心点。
            bool isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
            if (!isSuccess) return (false, null, 0, 0, 0, 0);

            // 计算相机与机器人坐标系的旋转矩阵。
            var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
            if (!robotCameraRotationMatrix.IsSuccess) return (false, null, 0, 0, 0, 0);
            pixelScaleX = robotCameraRotationMatrix.PixelScaleX;
            pixelScaleY = robotCameraRotationMatrix.PixelScaleY;
            RobotCameraRotationMatrix = robotCameraRotationMatrix.RotationMatrix;

            // 自动移动机器人至图像中心位置。
            var autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
            if (!autoMoveResult.IsSuccess) return (false, null, 0, 0, 0, 0);
            calibrationInfo.CenterPoint.X = (float)autoMoveResult.X;
            calibrationInfo.CenterPoint.Y = (float)autoMoveResult.Y;

            RPoint RobotU = calibrationInfo.CenterPoint.Clone();
            RobotU.U += (float)calibrationInfo.Angle;

            // 机器人 U 轴旋转。
            isSuccess = await robot.CalibMotionAsync(RobotU, RobotU.Z);
            if (!isSuccess) return (false, null, 0, 0, 0, 0);
            if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);

            // 相机拍照并处理图像。
            var photoResult = await CaptureAndProcessCallback();
            if (!photoResult.IsSuccess) return (false, null, 0, 0, 0, 0);

            autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
            if (!autoMoveResult.IsSuccess) return (false, null, 0, 0, 0, 0);
            RobotU.X = (float)autoMoveResult.X;
            RobotU.Y = (float)autoMoveResult.Y;

            Vector<double> p0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
            Vector<double> p1 = Vector<double>.Build.Dense(new double[] { RobotU.X, RobotU.Y });
            ToolCoord toolCoord = new ToolCoord();
            double u1 = calibrationInfo.CenterPoint.U;
            double u2 = RobotU.U;
            if (robot.Brand == RobotBrand.Schneider)
            {
                u1 *= -1;
                u2 *= -1;
            }
            toolCoord.ComputeTool(p0, p1, u1, u2);
            if (calibrationInfo.Tool == null) calibrationInfo.Tool = new RobotTool();
            calibrationInfo.Tool.X = toolCoord.X;
            calibrationInfo.Tool.Y = toolCoord.Y;

            return (true, RobotCameraRotationMatrix, calibrationInfo.Tool.X, calibrationInfo.Tool.Y, pixelScaleX, pixelScaleY);
        }

        /// <summary>
        /// 固定向上相机校准工具坐标。
        /// </summary>
        public async Task<(bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY)> CalibrateFixedUpCameraTool(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            double pixelScaleX = 0;
            double pixelScaleY = 0;
            Matrix<double> RobotCameraRotationMatrix = null;

            // 移动机器人至中心点。
            bool isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
            if (!isSuccess) return (false, null, 0, 0, 0, 0);

            if (calibrationInfo.Pick.PickPlaceModel == PickPlaceModel.Suction)
            {
                // 打开吸气。
                await robot.CalibOutIOAsync(true);
            }
            else
            {
                // 张开夹爪。
                await robot.CalibOutIOAsync(false);
            }

            if (!await PromptPlaceBlockAsync()) return (false, null, 0, 0, 0, 0);

            if (calibrationInfo.Pick.PickPlaceModel == PickPlaceModel.Clamp)
            {
                // 夹紧夹爪。
                await robot.CalibOutIOAsync(true);
            }

            // 计算相机与机器人坐标系的旋转矩阵。
            var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
            if (!robotCameraRotationMatrix.IsSuccess) return (false, null, 0, 0, 0, 0);
            pixelScaleX = robotCameraRotationMatrix.PixelScaleX;
            pixelScaleY = robotCameraRotationMatrix.PixelScaleY;
            RobotCameraRotationMatrix = robotCameraRotationMatrix.RotationMatrix;

            // 自动移动机器人至图像中心位置。
            var autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
            if (!autoMoveResult.IsSuccess) return (false, null, 0, 0, 0, 0);
            calibrationInfo.CenterPoint.X = (float)autoMoveResult.X;
            calibrationInfo.CenterPoint.Y = (float)autoMoveResult.Y;

            RPoint RobotU = calibrationInfo.CenterPoint.Clone();
            RobotU.U += (float)calibrationInfo.Angle;

            // 机器人 U 轴旋转。
            isSuccess = await robot.CalibMotionAsync(RobotU, RobotU.Z);
            if (!isSuccess) return (false, null, 0, 0, 0, 0);
            if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0, 0, 0);

            // 相机拍照并处理图像。
            var photoResult = await CaptureAndProcessCallback();
            if (!photoResult.IsSuccess) return (false, null, 0, 0, 0, 0);

            autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
            if (!autoMoveResult.IsSuccess) return (false, null, 0, 0, 0, 0);
            RobotU.X = (float)autoMoveResult.X;
            RobotU.Y = (float)autoMoveResult.Y;

            Vector<double> p0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
            Vector<double> p1 = Vector<double>.Build.Dense(new double[] { RobotU.X, RobotU.Y });
            ToolCoord toolCoord = new ToolCoord();
            double u1 = calibrationInfo.CenterPoint.U;
            double u2 = RobotU.U;
            if (robot.Brand == RobotBrand.Schneider)
            {
                u1 *= -1;
                u2 *= -1;
            }
            toolCoord.ComputeTool(p0, p1, u1, u2);
            if (calibrationInfo.Tool == null) calibrationInfo.Tool = new RobotTool();
            calibrationInfo.Tool.X = toolCoord.X;
            calibrationInfo.Tool.Y = toolCoord.Y;

            return (true, RobotCameraRotationMatrix, calibrationInfo.Tool.X, calibrationInfo.Tool.Y, pixelScaleX, pixelScaleY);
        }

        /// <summary>
        /// 通过未标定相机校准工具坐标：按 <see cref="CameraMount"/> 分发到对应标定方法。
        /// </summary>
        public async Task<(bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY)> CalibrateUncalibratedCameraTool(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            if (calibrationInfo == null) return (false, null, 0, 0, 0, 0);
            if (robot == null || !robot.IsConnected) return (false, null, 0, 0, 0, 0);

            // 设置校准参数。
            await robot.CalibParameAsync(calibrationInfo.Pick, calibrationInfo.Speed, calibrationInfo.Accel, calibrationInfo.Power, calibrationInfo.WaitSuction, calibrationInfo.WaitBlow);
            (bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY) calibresult;
            if (calibrationInfo.CameraMount == CameraMount.FixedDown)
            {
                calibresult = await CalibrateFixedDownCameraTool(calibrationInfo, robot, cancellationToken);
            }
            else if (calibrationInfo.CameraMount == CameraMount.FixedUp)
            {
                calibresult = await CalibrateFixedUpCameraTool(calibrationInfo, robot, cancellationToken);
            }
            else if (calibrationInfo.CameraMount == CameraMount.MobileJ4)
            {
                calibresult = await CalibrateJ4MovingDownCameraTool(calibrationInfo, robot, cancellationToken);
            }
            else
            {
                return (false, null, 0, 0, 0, 0);
            }
            if (!calibresult.IsSuccess) return (false, null, 0, 0, 0, 0);

            return (true, calibresult.RotationMatrix, calibresult.ToolX, calibresult.ToolY, calibresult.PixelScaleX, calibresult.PixelScaleY);
        }

        /// <summary>
        /// 自动识别机器人坐标系与图像坐标系的旋转矩阵（三点法）与像素尺度。
        /// </summary>
        public async Task<(bool IsSuccess, Matrix<double> RotationMatrix, double PixelScaleX, double PixelScaleY)> AutoIdentifyRobotCameraRotationMatrix(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            // 移动距离，单位 mm。
            float distance = ((float)calibrationInfo.Height) / 3;

            //-------------------------------------------P0-----------------------------------
            bool isSuccess;
            if (calibrationInfo.CameraMount == CameraMount.FixedDown)
            {
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
                if (!isSuccess) return (false, null, 0, 0);
                // 关闭吸气。
                await robot.CalibOutIOAsync(false);
            }
            else
            {
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, calibrationInfo.CenterPoint.Z);
                if (!isSuccess) return (false, null, 0, 0);
            }

            RPoint RobotP0 = await robot.GetRobotPosAsync();
            RPoint RobotP1 = RobotP0.Clone();
            RPoint RobotP2 = RobotP0.Clone();
            RobotP1.X += distance;
            RobotP2.Y += distance;

            if (calibrationInfo.CameraMount == CameraMount.FixedDown)
            {
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
                if (!isSuccess) return (false, null, 0, 0);
            }

            var result = await CaptureAndProcessCallback();
            if (!result.IsSuccess) return (false, null, 0, 0);
            PointF P0 = new PointF((float)result.X, (float)result.Y);

            //-------------------------------------------P1-----------------------------------
            if (calibrationInfo.CameraMount == CameraMount.FixedDown)
            {
                isSuccess = await robot.CalibMotionAsync(RobotP0, null);
                if (!isSuccess) return (false, null, 0, 0);
                if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0);
                await robot.CalibOutIOAsync(true);
                isSuccess = await robot.CalibMotionAsync(RobotP1, null);
                if (!isSuccess) return (false, null, 0, 0);
                if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0);
                await robot.CalibOutIOAsync(false);
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
                if (!isSuccess) return (false, null, 0, 0);
            }
            else
            {
                isSuccess = await robot.CalibMotionAsync(RobotP1, RobotP1.Z);
                if (!isSuccess) return (false, null, 0, 0);
            }
            if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0);

            result = await CaptureAndProcessCallback();
            if (!result.IsSuccess) return (false, null, 0, 0);
            PointF P1 = new PointF((float)result.X, (float)result.Y);

            //-------------------------------------------P2-----------------------------------
            if (calibrationInfo.CameraMount == CameraMount.FixedDown)
            {
                isSuccess = await robot.CalibMotionAsync(RobotP1, null);
                if (!isSuccess) return (false, null, 0, 0);
                if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0);
                await robot.CalibOutIOAsync(true);
                isSuccess = await robot.CalibMotionAsync(RobotP2, null);
                if (!isSuccess) return (false, null, 0, 0);
                if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0);
                await robot.CalibOutIOAsync(false);
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
                if (!isSuccess) return (false, null, 0, 0);
            }
            else
            {
                isSuccess = await robot.CalibMotionAsync(RobotP2, RobotP2.Z);
                if (!isSuccess) return (false, null, 0, 0);
            }
            if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0);

            result = await CaptureAndProcessCallback();
            if (!result.IsSuccess) return (false, null, 0, 0);
            PointF P2 = new PointF((float)result.X, (float)result.Y);

            //-------------------------------------------P0--------------------------------------
            if (calibrationInfo.CameraMount == CameraMount.FixedDown)
            {
                isSuccess = await robot.CalibMotionAsync(RobotP2, null);
                if (!isSuccess) return (false, null, 0, 0);
                if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0);
                await robot.CalibOutIOAsync(true);
                isSuccess = await robot.CalibMotionAsync(RobotP0, null);
                if (!isSuccess) return (false, null, 0, 0);
                if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0);
                await robot.CalibOutIOAsync(false);
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
                if (!isSuccess) return (false, null, 0, 0);
            }
            else
            {
                isSuccess = await robot.CalibMotionAsync(RobotP0, RobotP0.Z);
                if (!isSuccess) return (false, null, 0, 0);
            }
            if (cancellationToken.IsCancellationRequested) return (false, null, 0, 0);

            //-----------------------------------------计算旋转矩阵------------------------------
            Matrix<double> M = CalculateImageRotationMatrix(P0, P1, P2);
            //-----------------------------------------计算像素和毫米比例-----------------------
            Vector<double> P11 = Vector<double>.Build.Dense(new double[] { distance, 0 });
            Vector<double> P21 = Vector<double>.Build.Dense(new double[] { P1.X, P1.Y }) - Vector<double>.Build.Dense(new double[] { P0.X, P0.Y });
            double PixelScaleX = Math.Abs((M * P11)[0] / P21[0]);   // mm/pixel
            double PixelScaleY = Math.Abs((M * P11)[1] / P21[1]);   // mm/pixel
            return (true, M, PixelScaleX, PixelScaleY);
        }

        /// <summary>
        /// 计算图像坐标系到机器人坐标系的旋转矩阵（三点基向量法）。
        /// </summary>
        public Matrix<double> CalculateImageRotationMatrix(PointF P0, PointF P1, PointF P2)
        {
            Vector<double> Point0 = Vector<double>.Build.Dense(new double[] { (double)P0.X, (double)P0.Y });
            Vector<double> Point1 = Vector<double>.Build.Dense(new double[] { (double)P1.X, (double)P1.Y });
            Vector<double> Point2 = Vector<double>.Build.Dense(new double[] { (double)P2.X, (double)P2.Y });

            Vector<double> E1 = (Point1 - Point0) / (Point1 - Point0).L2Norm();
            Vector<double> E2 = (Point2 - Point0 - E1.DotProduct(Point2 - Point0) * E1) / (Point2 - Point0 - E1.DotProduct(Point2 - Point0) * E1).L2Norm();

            Matrix<double> R = Matrix<double>.Build.DenseOfColumnVectors(E1, E2);
            return R;
        }

        /// <summary>
        /// 自动移动机器人到图像中心位置（视觉伺服逼近）。
        /// </summary>
        public async Task<(bool IsSuccess, double X, double Y, double U)> AutoMoveRobotToImageCenter(IRobot robot, Matrix<double> matrix, double PixelScaleX, double PixelScaleY, CancellationToken cancellationToken)
        {
            bool IsFinish = false;

            var result = await CaptureAndProcessCallback();
            if (!result.IsSuccess) return (false, 0, 0, 0);
            PointF curPixel = new PointF((float)result.X, (float)result.Y);
            PointF P0 = new PointF((float)(result.ImageWidth / 2), (float)(result.ImageHeight / 2));

            var curpos = robot.GetRobotPos();
            for (int i = 0; i < 10; i++)
            {
                double offsetx = (P0.X - curPixel.X) * PixelScaleX;
                double offsety = (P0.Y - curPixel.Y) * PixelScaleY;
                var pos = matrix.Inverse() * Vector<double>.Build.Dense(new double[] { offsetx, offsety });
                curpos.X += (float)pos[0];
                curpos.Y += (float)pos[1];
                bool isSuccess = await robot.CalibMotionAsync(curpos, curpos.Z);
                if (!isSuccess) return (false, 0, 0, 0);

                result = await CaptureAndProcessCallback();
                if (!result.IsSuccess) return (false, 0, 0, 0);
                curPixel = new PointF((float)result.X, (float)result.Y);

                offsetx = P0.X - curPixel.X;
                offsety = P0.Y - curPixel.Y;
                if (Math.Abs(offsetx) < 1 && Math.Abs(offsety) < 1)
                {
                    IsFinish = true;
                    break;
                }
                if (cancellationToken.IsCancellationRequested) return (false, 0, 0, 0);
            }
            if (IsFinish)
            {
                var finalPos = robot.GetRobotPos();
                return (true, finalPos.X, finalPos.Y, finalPos.U);
            }
            return (false, 0, 0, 0);
        }

        /// <summary>
        /// 自动九点标定：以中心点为原点生成 3x3 九点，逐点运动+拍照，OpenCV 求仿射，计算精度指标。
        /// </summary>
        public async Task<(bool IsSuccess, Matrix<double> AffineTransformationMaterial, RobotPixelPoint[] NinePoint, CalibrationResult Result)> AutoNinePointCalibration(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            if (calibrationInfo == null) return (false, null, null, null);
            if (robot == null || !robot.IsConnected) return (false, null, null, null);

            double IntervalX = calibrationInfo.Width / 2;
            double IntervalY = calibrationInfo.Height / 2;
            Vector<double>[] grid =
            {
                Vector<double>.Build.Dense(new double[] { -IntervalX, -IntervalY }),
                Vector<double>.Build.Dense(new double[] { 0, -IntervalY }),
                Vector<double>.Build.Dense(new double[] { IntervalX, -IntervalY }),
                Vector<double>.Build.Dense(new double[] { IntervalX, 0 }),
                Vector<double>.Build.Dense(new double[] { 0, 0 }),
                Vector<double>.Build.Dense(new double[] { -IntervalX, 0 }),
                Vector<double>.Build.Dense(new double[] { -IntervalX, IntervalY }),
                Vector<double>.Build.Dense(new double[] { 0, IntervalY }),
                Vector<double>.Build.Dense(new double[] { IntervalX, IntervalY }),
            };
            // 设置校准参数。
            await robot.CalibParameAsync(calibrationInfo.Pick, calibrationInfo.Speed, calibrationInfo.Accel, calibrationInfo.Power, calibrationInfo.WaitSuction, calibrationInfo.WaitBlow);

            bool isSuccess;
            var P0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
            if (robot.Brand == RobotBrand.XYZ_Platform)
            {
                // 移动机器人至中心点。
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
                if (!isSuccess) return (false, null, null, null);
                if (cancellationToken.IsCancellationRequested) return (false, null, null, null);
                // 计算相机与机器人坐标系的旋转矩阵。
                var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
                if (!robotCameraRotationMatrix.IsSuccess) return (false, null, null, null);
                // 自动移动机器人至图像中心位置。
                var autoMoveResult = await AutoMoveRobotToImageCenter(robot, robotCameraRotationMatrix.RotationMatrix, robotCameraRotationMatrix.PixelScaleX, robotCameraRotationMatrix.PixelScaleY, cancellationToken);
                if (!autoMoveResult.IsSuccess) return (false, null, null, null);
                calibrationInfo.CenterPoint.X = (float)autoMoveResult.X;
                calibrationInfo.CenterPoint.Y = (float)autoMoveResult.Y;
                if (calibrationInfo.CameraMount != CameraMount.MobileDown_XYPlatform)
                {
                    calibrationInfo.MarkPoint = calibrationInfo.CenterPoint.Clone();
                }
                calibrationInfo.RobotCameraRotationMatrix = robotCameraRotationMatrix.RotationMatrix.ToArray();
            }

            // 机器人坐标系与图像坐标系之间的旋转矩阵。
            Matrix<double> RobotCameraRotationMatrix = Matrix<double>.Build.DenseOfArray(calibrationInfo.RobotCameraRotationMatrix);

            // 将九点相对偏移旋转到机器人坐标系并叠加中心点。
            List<RPoint> rPoints = new List<RPoint>();
            for (int i = 0; i < grid.Length; i++)
            {
                var p = RobotCameraRotationMatrix.Inverse() * grid[i] + P0;
                rPoints.Add(MakePoint(i + 1, p, calibrationInfo.CenterPoint));
            }

            List<PointF> PixelPoslist = new List<PointF>();
            if (calibrationInfo.CalibPoints == null)
            {
                calibrationInfo.CalibPoints = new System.Collections.ObjectModel.ObservableCollection<RobotPixelPoint>();
            }
            else
            {
                calibrationInfo.CalibPoints.Clear();
            }

            // 移动相机标定时，需要先记住标定块的绝对坐标（相对机器人基坐标系）P0。
            if (calibrationInfo.CameraMount == CameraMount.MobileJ4)
            {
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
                if (!isSuccess) return (false, null, null, null);
                if (cancellationToken.IsCancellationRequested) return (false, null, null, null);

                // 将当前吸嘴在 Tool 0 下的坐标转换为工具坐标系下的坐标，并记录。
                ToolCoord toolCoord = new ToolCoord();
                toolCoord.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
                Vector<double> p = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
                var pn = toolCoord.GetToolnCoord(p, calibrationInfo.CenterPoint.U);
                if (calibrationInfo.MarkPoint == null) calibrationInfo.MarkPoint = new RPoint();
                calibrationInfo.MarkPoint = calibrationInfo.CenterPoint.Clone();
                calibrationInfo.MarkPoint.X = (float)pn[0];
                calibrationInfo.MarkPoint.Y = (float)pn[1];
            }

            for (int i = 0; i < rPoints.Count; i++)
            {
                // 移动机器人至第 i 个标定点。
                if (calibrationInfo.CameraMount == CameraMount.FixedDown)
                {
                    isSuccess = await robot.CalibMotionAsync(rPoints[i], null);
                    if (!isSuccess) return (false, null, null, null);
                    if (cancellationToken.IsCancellationRequested) return (false, null, null, null);
                    // 关闭吸气。
                    await robot.CalibOutIOAsync(false);
                    // 移动机器人至待机位置。
                    isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
                    if (!isSuccess) return (false, null, null, null);
                }
                else
                {
                    isSuccess = await robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
                    if (!isSuccess) return (false, null, null, null);
                }
                if (cancellationToken.IsCancellationRequested) return (false, null, null, null);

                // 相机拍照并处理图像。
                var photoResult = await CaptureAndProcessCallback();
                if (!photoResult.IsSuccess) return (false, null, null, null);
                PointF curPixel = new PointF((float)(photoResult.X), (float)(photoResult.Y));
                PixelPoslist.Add(curPixel);

                if (calibrationInfo.CameraMount == CameraMount.FixedDown)
                {
                    // 移动机器人至第 i 个标定点。
                    isSuccess = await robot.CalibMotionAsync(rPoints[i], null);
                    if (!isSuccess) return (false, null, null, null);
                    if (cancellationToken.IsCancellationRequested) return (false, null, null, null);
                    // 打开吸气。
                    await robot.CalibOutIOAsync(true);
                }

                calibrationInfo.CalibPoints.Add(new RobotPixelPoint()
                {
                    Number = i + 1,
                    Robot = new PointF(rPoints[i].X, rPoints[i].Y),
                    Pixel = new PointF(PixelPoslist[i].X, PixelPoslist[i].Y)
                });
                if (cancellationToken.IsCancellationRequested) return (false, null, null, null);
            }
            if (calibrationInfo.CameraMount == CameraMount.FixedDown)
            {
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
                if (!isSuccess) return (false, null, null, null);
            }
            if (cancellationToken.IsCancellationRequested) return (false, null, null, null);

            Mat matPixel = new Mat(9, 2, MatType.CV_64F);
            Mat matRobot = new Mat(9, 2, MatType.CV_64F);

            for (int i = 0; i < 9; i++)
            {
                matPixel.Set<double>(i, 0, PixelPoslist[i].X);
                matPixel.Set<double>(i, 1, PixelPoslist[i].Y);
                // 移动相机模式需要根据机器人九点运动的反向变换，使 Mark 点移动到相机中心。
                if (calibrationInfo.CameraMount == CameraMount.MobileJ4 || calibrationInfo.CameraMount == CameraMount.MobileDown_XYPlatform)
                {
                    double ox = rPoints[i].X - rPoints[0].X;
                    double oy = rPoints[i].Y - rPoints[0].Y;
                    matRobot.Set<double>(i, 0, calibrationInfo.MarkPoint.X - ox);
                    matRobot.Set<double>(i, 1, calibrationInfo.MarkPoint.Y - oy);
                }
                else
                {
                    // 将 Tool 0 下的坐标转换为 Tool n 下的坐标。
                    if (calibrationInfo.Tool != null)
                    {
                        ToolCoord toolCoord = new ToolCoord();
                        toolCoord.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
                        double u1 = rPoints[i].U;
                        if (robot.Brand == RobotBrand.Schneider) u1 *= -1;
                        var trans = toolCoord.GetToolnCoord(Vector<double>.Build.Dense(new double[] { rPoints[i].X, rPoints[i].Y }), u1);
                        matRobot.Set<double>(i, 0, trans[0]);
                        matRobot.Set<double>(i, 1, trans[1]);
                    }
                    else
                    {
                        matRobot.Set<double>(i, 0, rPoints[i].X);
                        matRobot.Set<double>(i, 1, rPoints[i].Y);
                    }
                }
            }
            Mat mat2d = Cv2.EstimateAffine2D(matPixel, matRobot);
            if (mat2d == null || mat2d.Empty()) return (false, null, null, null);

            double a = mat2d.Get<double>(0, 0);
            double b = mat2d.Get<double>(0, 1);
            double tx = mat2d.Get<double>(0, 2);
            double c = mat2d.Get<double>(1, 0);
            double d = mat2d.Get<double>(1, 1);
            double ty = mat2d.Get<double>(1, 2);
            double[,] vv = { { a, b, tx }, { c, d, ty } };
            Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(vv);
            calibrationInfo.AffineTransformationMaterial = matrix.ToArray();

            // 2x2 仿射部分的列向量分别表示图像 X/Y 轴在机器人坐标系中的方向和比例。
            double pixelScaleX = Math.Sqrt(a * a + c * c);
            double pixelScaleY = Math.Sqrt(b * b + d * d);
            double rotateFromXAxis = ToDegree(Math.Atan2(c, a));
            double rotateFromYAxis = ToDegree(Math.Atan2(-b, d));
            double rotate = AverageAngle(rotateFromXAxis, rotateFromYAxis);
            double dot = a * b + c * d;
            double axisLengthProduct = pixelScaleX * pixelScaleY;
            double axisAngle = axisLengthProduct > 0 ? ToDegree(Math.Acos(Clamp(dot / axisLengthProduct, -1, 1))) : 90;
            double shearAngle = axisAngle - 90;
            double determinant = a * d - b * c;

            // RMS(均方根值) 标定偏差。
            double sumX = 0, sumY = 0;
            List<double> differenceX = new List<double>();
            List<double> differenceY = new List<double>();
            for (int i = 0; i < 9; i++)
            {
                var pixel = Vector<double>.Build.Dense(new double[] { PixelPoslist[i].X, PixelPoslist[i].Y, 1 });
                if (calibrationInfo.CameraMount == CameraMount.MobileJ4 || calibrationInfo.CameraMount == CameraMount.MobileDown_XYPlatform)
                {
                    double ox = rPoints[i].X - rPoints[0].X;
                    double oy = rPoints[i].Y - rPoints[0].Y;
                    differenceX.Add((calibrationInfo.MarkPoint.X - ox) - (matrix * pixel)[0]);
                    differenceY.Add((calibrationInfo.MarkPoint.Y - oy) - (matrix * pixel)[1]);
                }
                else
                {
                    if (calibrationInfo.Tool != null)
                    {
                        ToolCoord toolCoord = new ToolCoord();
                        toolCoord.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
                        double u1 = rPoints[i].U;
                        if (robot.Brand == RobotBrand.Schneider) u1 *= -1;
                        var trans = toolCoord.GetToolnCoord(Vector<double>.Build.Dense(new double[] { rPoints[i].X, rPoints[i].Y }), u1);
                        differenceX.Add(trans[0] - (matrix * pixel)[0]);
                        differenceY.Add(trans[1] - (matrix * pixel)[1]);
                    }
                    else
                    {
                        differenceX.Add(rPoints[i].X - (matrix * pixel)[0]);
                        differenceY.Add(rPoints[i].Y - (matrix * pixel)[1]);
                    }
                }
                sumX += Math.Pow(differenceX[differenceX.Count - 1], 2);
                sumY += Math.Pow(differenceY[differenceY.Count - 1], 2);
            }
            double rmsX = Math.Sqrt(sumX / 9);
            double rmsY = Math.Sqrt(sumY / 9);
            var errorDistances = differenceX.Zip(differenceY, (dx, dy) => Math.Sqrt(dx * dx + dy * dy)).ToList();

            CalibrationResult calibrationResult = new CalibrationResult();
            calibrationResult.ScaleX = pixelScaleX;
            calibrationResult.ScaleY = pixelScaleY;
            calibrationResult.TranslationX = tx;
            calibrationResult.TranslationY = ty;
            calibrationResult.Rotate = rotate;
            calibrationResult.ImageXAxisAngle = rotateFromXAxis;
            calibrationResult.ImageYAxisAngle = rotateFromYAxis;
            calibrationResult.ShearAngle = shearAngle;
            calibrationResult.ScaleRatio = pixelScaleY == 0 ? 0 : pixelScaleX / pixelScaleY;
            calibrationResult.RMSX = rmsX;
            calibrationResult.RMSY = rmsY;
            calibrationResult.RMS = Math.Sqrt(errorDistances.Select(value => value * value).Average());
            calibrationResult.MeanError = errorDistances.Average();
            calibrationResult.MaxErrorValue = errorDistances.Max();
            calibrationResult.MeanErrorX = differenceX.Average();
            calibrationResult.MeanErrorY = differenceY.Average();
            calibrationResult.MaxErrorValueX = differenceX.Select(Math.Abs).Max();
            calibrationResult.MaxErrorValueY = differenceY.Select(Math.Abs).Max();
            calibrationResult.Determinant = determinant;
            calibrationInfo.CalibrationResult = calibrationResult;

            return (true, matrix, calibrationInfo.CalibPoints.ToArray(), calibrationResult);
        }

        /// <summary>
        /// 执行校准验证：在 5 个验证点上复算误差，评估标定质量。
        /// </summary>
        public async Task<(bool IsSuccess, CalibrationTestResult Result)> ExecuteCalibrationValidation(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            if (calibrationInfo == null) return (false, null);
            if (robot == null || !robot.IsConnected) return (false, null);

            double IntervalX = calibrationInfo.Width * 0.90 / 2;
            double IntervalY = calibrationInfo.Height * 0.90 / 2;
            Vector<double>[] grid =
            {
                Vector<double>.Build.Dense(new double[] { -IntervalX, -IntervalY }),
                Vector<double>.Build.Dense(new double[] { IntervalX, -IntervalY }),
                Vector<double>.Build.Dense(new double[] { 0, 0 }),
                Vector<double>.Build.Dense(new double[] { -IntervalX, IntervalY }),
                Vector<double>.Build.Dense(new double[] { IntervalX, IntervalY }),
            };

            var P0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
            Matrix<double> RobotCameraRotationMatrix = Matrix<double>.Build.DenseOfArray(calibrationInfo.RobotCameraRotationMatrix);

            List<RPoint> rPoints = new List<RPoint>();
            for (int i = 0; i < grid.Length; i++)
            {
                var p = RobotCameraRotationMatrix.Inverse() * grid[i] + P0;
                rPoints.Add(MakePoint(i + 1, p, calibrationInfo.CenterPoint));
            }

            bool isSuccess;
            List<Vector<double>> TestPos = new List<Vector<double>>();
            for (int i = 0; i < rPoints.Count; i++)
            {
                if (calibrationInfo.CameraMount == CameraMount.FixedDown)
                {
                    isSuccess = await robot.CalibMotionAsync(rPoints[i], null);
                    if (!isSuccess) return (false, null);
                    if (cancellationToken.IsCancellationRequested) return (false, null);
                    await robot.CalibOutIOAsync(false);
                    isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
                    if (!isSuccess) return (false, null);
                }
                else
                {
                    isSuccess = await robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
                    if (!isSuccess) return (false, null);
                }
                if (cancellationToken.IsCancellationRequested) return (false, null);

                var photoResult = await CaptureAndProcessCallback();
                if (!photoResult.IsSuccess) return (false, null);
                PointF pixelpos = new PointF((float)(photoResult.X), (float)(photoResult.Y));

                Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(calibrationInfo.AffineTransformationMaterial);
                var rpos = matrix * Vector<double>.Build.Dense(new double[] { pixelpos.X, pixelpos.Y, 1 });
                TestPos.Add(rpos);
                if (calibrationInfo.CameraMount == CameraMount.FixedDown)
                {
                    isSuccess = await robot.CalibMotionAsync(rPoints[i], null);
                    if (!isSuccess) return (false, null);
                    await robot.CalibOutIOAsync(true);
                }
                if (cancellationToken.IsCancellationRequested) return (false, null);
            }

            if (calibrationInfo.CameraMount == CameraMount.FixedDown)
            {
                isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
                if (!isSuccess) return (false, null);
            }

            // RMS（均方根值）标定偏差。
            double sumX = 0, sumY = 0;
            List<double> differenceX = new List<double>();
            List<double> differenceY = new List<double>();
            for (int i = 0; i < 5; i++)
            {
                if (calibrationInfo.CameraMount == CameraMount.MobileJ4 || calibrationInfo.CameraMount == CameraMount.MobileDown_XYPlatform)
                {
                    double Robot_X = TestPos[i][0];
                    double Robot_Y = TestPos[i][1];
                    double curpos_x = rPoints[i].X;
                    double curpos_y = rPoints[i].Y;
                    double curpos_u = rPoints[i].U;

                    double angle = Math.PI * (curpos_u - calibrationInfo.MarkPoint.U) / 180;
                    Matrix<double> rotationMatrix = Matrix<double>.Build.DenseOfArray(new double[,]
                    {
                        { Math.Cos(angle), -Math.Sin(angle) },
                        { Math.Sin(angle), Math.Cos(angle) }
                    });

                    Vector<double> p3 = Vector<double>.Build.Dense(new double[] { Robot_X - calibrationInfo.CalibPoints[0].Robot.X, Robot_Y - calibrationInfo.CalibPoints[0].Robot.Y });
                    Vector<double> phere = Vector<double>.Build.Dense(new double[] { curpos_x, curpos_y });
                    var cc = rotationMatrix * p3 + phere;

                    differenceX.Add(cc[0] - calibrationInfo.MarkPoint.X);
                    differenceY.Add(cc[1] - calibrationInfo.MarkPoint.Y);
                }
                else
                {
                    if (calibrationInfo.Tool != null)
                    {
                        ToolCoord toolCoord = new ToolCoord();
                        toolCoord.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
                        double u1 = rPoints[i].U;
                        if (robot.Brand == RobotBrand.Schneider) u1 *= -1;
                        var trans = toolCoord.GetToolnCoord(Vector<double>.Build.Dense(new double[] { rPoints[i].X, rPoints[i].Y }), u1);
                        differenceX.Add(trans[0] - TestPos[i][0]);
                        differenceY.Add(trans[1] - TestPos[i][1]);
                    }
                    else
                    {
                        differenceX.Add(rPoints[i].X - TestPos[i][0]);
                        differenceY.Add(rPoints[i].Y - TestPos[i][1]);
                    }
                }
                sumX += Math.Pow(differenceX[differenceX.Count - 1], 2);
                sumY += Math.Pow(differenceY[differenceY.Count - 1], 2);
            }
            double rmsX = Math.Sqrt(sumX / 5);
            double rmsY = Math.Sqrt(sumY / 5);

            CalibrationTestResult calibrationTestResult = new CalibrationTestResult();
            calibrationTestResult.MaxErrorValueX = differenceX.Max();
            calibrationTestResult.MinErrorValueX = differenceX.Min();
            calibrationTestResult.MaxErrorValueY = differenceY.Max();
            calibrationTestResult.MinErrorValueY = differenceY.Min();
            calibrationTestResult.RMSEX = rmsX;
            calibrationTestResult.RMSEY = rmsY;
            return (true, calibrationTestResult);
        }

        private static double ToDegree(double radians) => radians * 180 / Math.PI;

        private static double AverageAngle(double angle1, double angle2)
            => NormalizeAngle(angle1 + NormalizeAngle(angle2 - angle1) / 2);

        private static double NormalizeAngle(double angle)
        {
            while (angle > 180) angle -= 360;
            while (angle <= -180) angle += 360;
            return angle;
        }

        private static double Clamp(double value, double min, double max)
        {
            if (value < min) return min;
            return value > max ? max : value;
        }
    }
}