Display_D_HP/TeamAAS-VM/Services/CalibrationService.cs

using Cognex.VisionPro;
using Cognex.VisionPro.ToolBlock;
using MathNet.Numerics.LinearAlgebra;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Drawing;
using System.IO;
using System.Linq;
using System.Windows.Forms;
using TeamAAS_VP.Core;
using TeamAAS_VP.Core.Robots;
using TeamAAS_VP.Enums;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models.Calibration;
using TeamAAS_VP.Resources.Languages;

namespace TeamAAS_VP.Services
{
    /// <summary>
    /// Calibration 管理服务，负责校准文件的读取与保存以及像素到机器人位姿的转换。
    /// 线程安全：对内部集合的读/写通过 <see cref="_sync"/> 锁进行保护。
    /// 注意：构造函数不自动加载数据，调用者需要在合适时机调用 <see cref="LoadAll"/>。
    /// </summary>
    public class CalibrationService : ICalibrationService
    {
        IRobotService _robotService;
        /// <summary>
        /// 内部路径常量定义（相对路径）
        /// </summary>
        private static class Paths
        {
            public static readonly string CalibrationPath = "..//Calibration";
            public static string CalibrationListFilePath => Path.Combine(CalibrationPath ?? string.Empty, "CalibrationList.cfg");
        }

        // 同步锁对象，保护 _calibrations 的并发访问
        private readonly object _sync = new object();

        // 内存中的校准集合（用于 UI 绑定/管理）
        private ObservableCollection<CalibrationInfo> _calibrations = new ObservableCollection<CalibrationInfo>();

        /// <summary>
        /// 构造函数。注意：不在构造中自动加载校准，调用者应根据需要调用 <see cref="LoadAll"/>。
        /// </summary>
        public CalibrationService(IRobotService robotService)
        {
            _robotService = robotService ?? throw new ArgumentNullException(nameof(robotService));
        }

        /// <summary>
        /// 获取所有校准项的只读快照。
        /// 返回一个集合快照以避免外部对内部集合的直接修改。
        /// </summary>
        /// <returns>只读的 <see cref="CalibrationInfo"/> 集合</returns>
        public IReadOnlyCollection<CalibrationInfo> GetAllCalibrations()
        {
            lock (_sync)
            {
                // 返回当前集合的一个独立只读副本，避免并发问题
                return _calibrations.ToList().AsReadOnly();
            }
        }

        /// <summary>
        /// 根据 Id 获取单个校准信息。
        /// </summary>
        /// <param name="id">校准项的唯一标识</param>
        /// <returns>找到则返回 <see cref="CalibrationInfo"/>，否则返回 null</returns>
        public CalibrationInfo GetCalibration(Guid id)
        {
            lock (_sync)
            {
                return _calibrations.FirstOrDefault(c => c.Id == id);
            }
        }

        /// <summary>
        /// 添加或更新校准项：
        /// - 若 Id 存在则替换，否则追加并设置 Index
        /// - 写入单个校准 cfg 文件并更新索引文件
        /// - 保存或加载 ToolBlock（.vpp），若不存在则尝试从模板创建
        /// </summary>
        /// <param name="calib">要添加或更新的校准信息</param>
        /// <returns>保存后的 <see cref="CalibrationInfo"/>（可能为传入对象或修改后的对象）；参数为 null 则返回 null</returns>
        public CalibrationInfo AddOrUpdateCalibration(CalibrationInfo calib)
        {
            if (calib == null) return null;
            lock (_sync)
            {
                var exist = _calibrations.FirstOrDefault(c => c.Id == calib.Id);
                if (exist != null)
                {
                    // 替换已存在项（保持集合长度与 Index 不变）
                    var idx = _calibrations.IndexOf(exist);
                    _calibrations[idx] = calib;
                }
                else
                {
                    // 新增项，Index 设为当前数量 + 1
                    calib.Index = _calibrations.Count + 1;
                    _calibrations.Add(calib);
                }
            }

            // 更新时间戳
            calib.DateTime = DateTime.Now;

            // 确保校准保存目录存在
            string folder = Path.Combine(Paths.CalibrationPath, calib.Name);
            string calibFile = Path.Combine(folder, calib.Name + ".cfg");
            if (!Directory.Exists(folder)) Directory.CreateDirectory(folder);

            // 写入单个校准文件（JSON）
            FileHelper.WriteJsonFile(calib, calibFile);

            // 更新索引列表（Id -> Name）并写入文件
            var list = new Dictionary<Guid, string>();
            lock (_sync)
            {
                foreach (var item in _calibrations)
                {
                    list[item.Id] = item.Name;
                }
            }
            FileHelper.WriteJsonFile(list, Paths.CalibrationListFilePath);

            // 保存或加载 ToolBlock（.vpp）
            string prcPath = Path.Combine(folder, calib.Name + ".vpp");
            if (calib.ToolBlock != null)
            {
                // 已有 ToolBlock，直接保存
                VisionProFileHelper.SaveObjectToFileSafe(calib.ToolBlock, prcPath);
            }
            else
            {
                // 优先从当前目录加载已有 .vpp 文件，否则尝试从模板加载并保存
                if (File.Exists(prcPath))
                {
                    calib.ToolBlock = VisionProFileHelper.LoadObjectFromFileSafe<CogToolBlock>(prcPath);
                }
                else if (File.Exists("..//Vision Template//Calibration.vpp"))
                {
                    calib.ToolBlock = VisionProFileHelper.LoadObjectFromFileSafe<CogToolBlock>("..//Vision Template//Calibration.vpp");
                    VisionProFileHelper.SaveObjectToFileSafe(calib.ToolBlock, prcPath);
                }
            }

            return calib;
        }

        /// <summary>
        /// 根据 Id 移除校准项：
        /// - 从内存集合移除
        /// - 更新索引文件
        /// - 重新为剩余项排序 Index 并保存每个 cfg
        /// 注：不自动删除物理文件（除非有明确需求）
        /// </summary>
        /// <param name="id">要删除的校准项 Id</param>
        /// <returns>是否成功移除</returns>
        public bool RemoveCalibration(Guid id)
        {
            bool result = false;
            CalibrationInfo removed = null;
            lock (_sync)
            {
                removed = _calibrations.FirstOrDefault(c => c.Id == id);
                if (removed != null) result = _calibrations.Remove(removed);
            }

            // 更新索引文件（Id -> Name）
            var list = new Dictionary<Guid, string>();
            lock (_sync)
            {
                foreach (var item in _calibrations)
                {
                    list[item.Id] = item.Name;
                }
            }
            FileHelper.WriteJsonFile(list, Paths.CalibrationListFilePath);

            // 删除后重新按 Index 排序并保存每个校准文件
            var calibs = _calibrations.OrderBy(c => c.Index).ToList();
            int index = 1;
            foreach (var item in calibs)
            {
                item.Index = index;
                // 保存更新后的 cfg 文件
                string folder = Path.Combine(Paths.CalibrationPath, item.Name);
                string calibFile = Path.Combine(folder, item.Name + ".cfg");
                FileHelper.WriteJsonFile(item, calibFile);
                index++;
            }
            // 用排序后的集合替换内存集合（注意：替换时未加锁是因为已在外部加锁或逻辑保证）
            _calibrations = new ObservableCollection<CalibrationInfo>(calibs);

            // 可选：删除物理文件和 toolblock（不自动删除）

            return result;
        }

        /// <summary>
        /// 加载所有校准数据：
        /// - 确保 CalibrationPath 存在
        /// - 读取索引文件，遍历项并加载各自的 cfg 与 .vpp（如果存在）
        /// - 忽略单个文件加载异常，继续加载其他文件
        /// </summary>
        public void LoadAll()
        {
            lock (_sync)
            {
                if (!Directory.Exists(Paths.CalibrationPath)) Directory.CreateDirectory(Paths.CalibrationPath);

                _calibrations.Clear();
                if (!File.Exists(Paths.CalibrationListFilePath))
                {
                    // 若索引文件不存在，写入空索引并返回
                    FileHelper.WriteJsonFile(new Dictionary<Guid, string>(), Paths.CalibrationListFilePath);
                    return;
                }

                var list = FileHelper.ReadJsonFile<Dictionary<Guid, string>>(Paths.CalibrationListFilePath);
                if (list == null) return;

                foreach (var item in list)
                {
                    try
                    {
                        // 使用 FilePath.CalibrationPath（项目全局路径）构建单个 cfg 路径
                        string filePath = Path.Combine(FilePath.CalibrationPath, item.Value, item.Value + ".cfg");
                        if (File.Exists(filePath))
                        {
                            var calib = FileHelper.ReadJsonFile<CalibrationInfo>(filePath);
                            // 尝试加载对应的 .vpp ToolBlock
                            string prcPath = Path.Combine(FilePath.CalibrationPath, calib.Name, calib.Name + ".vpp");
                            if (File.Exists(prcPath))
                            {
                                calib.ToolBlock = VisionProFileHelper.LoadObjectFromFileSafe<CogToolBlock>(prcPath);
                            }
                            _calibrations.Add(calib);
                        }
                    }
                    catch
                    {
                        // 忽略单个文件错误，继续加载其他文件
                    }
                }
            }
        }

        /// <summary>
        /// 将内存中的所有校准保存到文件：
        /// - 为每个校准创建目录并写入 cfg
        /// - 若有 ToolBlock，则保存 .vpp
        /// - 写入索引文件
        /// </summary>
        public void SaveAll()
        {
            lock (_sync)
            {
                var list = new Dictionary<Guid, string>();
                foreach (var item in _calibrations)
                {
                    list[item.Id] = item.Name;
                    string folder = Path.Combine(FilePath.CalibrationPath, item.Name);
                    if (!Directory.Exists(folder)) Directory.CreateDirectory(folder);
                    string filePath = Path.Combine(folder, item.Name + ".cfg");
                    FileHelper.WriteJsonFile(item, filePath);
                    if (item.ToolBlock != null)
                    {
                        string prcPath = Path.Combine(folder, item.Name + ".vpp");
                        VisionProFileHelper.SaveObjectToFileSafe(item.ToolBlock, prcPath);
                    }
                }
                FileHelper.WriteJsonFile(list, Paths.CalibrationListFilePath);
            }
        }

        /// <summary>
        /// 校准转换：将像素坐标转换为机器人位置坐标（X,Y）及角度 U。
        /// 逻辑摘要：
        /// 1. 使用 calib.AffineTransformationMaterial 构建 3x3 仿射矩阵，将像素 [X,Y,1] 映射到机器人坐标系（单位：mm/像素转换后）。
        /// 2. 根据摄像机安装方式（CameraMount）进行不同的后处理：
        ///    - MobileJ4: 采用基于 MarkPoint 的旋转矩阵，将像素直接转换的点旋转并平移到机器人当前 TOOL0 下的位置
        ///    - MobileDown_XYPlatform: 类似 MobileJ4，但当前实现中角度置为 0（不旋转）
        ///    - 其他情况：如果提供 robotCoord，则通过 ToolCoord 计算工具坐标（考虑机器人品牌对角度符号的影响）
        /// 3. 对于某些 CameraMount，输入角度需要取反（例如 FixedDown、MobileJ2、MobileJ4），以匹配机器人坐标系习惯
        /// </summary>
        /// <param name="pixelCoord">像素坐标及方向：(X, Y, angle)。angle 表示图像中测量的角度（度）</param>
        /// <param name="robotCoord">机器人当前位姿数组，通常为 [X, Y, U]，可为 null（视场景而定）</param>
        /// <param name="calib">要使用的校准数据</param>
        /// <param name="robotBrand">机器人品牌，用于处理角度符号等差异</param>
        /// <returns>
        /// 返回元组 (IsSucceed, X, Y, U)：
        /// - IsSucceed: 总是 true（当前实现未对数学运算做失败判定），调用者可根据需要扩展错误处理
        /// - X, Y: 转换后的机器人坐标（单位同 calib 所用单位）
        /// - U: 角度（经过可能的符号调整）
        /// </returns>
        public (bool IsSucceed, double X, double Y, double U) ConvertPixelToPosition((double X, double Y, double angle) pixelCoord, double[] robotCoord, CalibrationInfo calib, RobotBrand robotBrand = RobotBrand.Default)
        {
            double angle = pixelCoord.angle;

            // 从校准数据获取仿射变换矩阵（3x3）
            Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(calib.AffineTransformationMaterial);

            // 某些安装方式需要取反角度以匹配机器人坐标系定义
            if (calib.CameraMount == CameraMount.FixedDown || calib.CameraMount == CameraMount.MobileJ2 || calib.CameraMount == CameraMount.MobileJ4 || calib.CameraMount == CameraMount.MobileDown_XYPlatform || calib.CameraMount == CameraMount.MobileDown_IndependentXYPlatform)
            {
                angle *= -1;
            }

            // 将像素坐标通过仿射矩阵转换到机器人坐标（rpos 是长度 3 的向量：[X, Y, w]，通常 w 为 1）
            var rpos = matrix * Vector<double>.Build.Dense(new double[] { pixelCoord.X, pixelCoord.Y, 1 });

            // 针对不同的 CameraMount 做进一步坐标变换
            if (calib.CameraMount == Enums.CameraMount.MobileJ4)
            {
                // 当 CameraMount 为 MobileJ4 时：
                // - rpos 的前两维表示像素->mm 转换后的点 (Robot_X, Robot_Y)
                // - 需要基于机器人当前 TOOL0 坐标以及 MarkPoint 的角度进行旋转和平移
                double Robot_X = rpos[0];
                double Robot_Y = rpos[1];

                // 机器人当前 TOOL0 下的位姿
                double curpos_x = robotCoord[0];
                double curpos_y = robotCoord[1];
                double curpos_u = robotCoord[2];

                // 旋转角（弧度） = (curpos_u - calib.MarkPoint.U) * PI / 180
                double angle1 = Math.PI * (curpos_u - calib.MarkPoint.U) / 180;

                // 2x2 旋转矩阵
                Matrix<double> rotationMatrix = Matrix<double>.Build.DenseOfArray(new double[,]
                {
                    { Math.Cos(angle1), -Math.Sin(angle1) },
                    { Math.Sin(angle1),  Math.Cos(angle1) }
                });

                // 将相对向量 p3 旋转并平移到当前工具坐标系下
                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { Robot_X - calib.CalibPoints[0].Robot.X, Robot_Y - calib.CalibPoints[0].Robot.Y });
                Vector<double> phere = Vector<double>.Build.Dense(new double[] { curpos_x, curpos_y });

                var cc = rotationMatrix * p3 + phere;
                rpos = Vector<double>.Build.Dense(new double[] { cc[0], cc[1] });
            }
            else if (calib.CameraMount == Enums.CameraMount.MobileDown_XYPlatform)
            {
                // MobileDown_XYPlatform 模式下，当前实现与 MobileJ4 类似，但角度暂时设为 0（不旋转）
                double Robot_X = rpos[0];
                double Robot_Y = rpos[1];

                double curpos_x = robotCoord[0];
                double curpos_y = robotCoord[1];
                double curpos_u = robotCoord[2];

                // 角度暂用 0（如有需要可基于 curpos_u - calib.MarkPoint.U 做旋转）
                double angle1 = 0;
                Matrix<double> rotationMatrix = Matrix<double>.Build.DenseOfArray(new double[,]
                {
                    { Math.Cos(angle1), -Math.Sin(angle1) },
                    { Math.Sin(angle1),  Math.Cos(angle1) }
                });

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

                var cc = rotationMatrix * p3 + phere;
                rpos = Vector<double>.Build.Dense(new double[] { cc[0], cc[1] }); // Mark 点在 tool0 下的坐标
            }
            else if (calib.CameraMount == Enums.CameraMount.MobileDown_IndependentXYPlatform)
            {
                //获取相机对应的移动模组的编号
                int moduleIndex = calib.CameraMotionModuleIndex;
                //获取机器人对象
                XYZU_Robot robot = _robotService.GetRobot(calib.RobotId) as XYZU_Robot;
                //获取当前移动模组的位置
                var movePos = robot?.GetCameraPos(moduleIndex);

                //将此时的坐标转换成独立模组坐标系下的坐标
                //旋转矩阵
                Matrix<double> R = Matrix<double>.Build.DenseOfArray(calib.IndependentDownCameraMotionModuleRotationMatrix);
                //平移矩阵
                Matrix<double> T = Matrix<double>.Build.DenseOfArray(calib.IndependentDownCameraMotionModuleTranslationMatrix);
                //平移向量
                Vector<double> PT = Vector<double>.Build.Dense(new double[] { T[0, 2], T[1, 2] });

                Vector<double> p100 = Vector<double>.Build.Dense(new double[] { calib.IndependentDownCameraMotionModulePoint.X, calib.IndependentDownCameraMotionModulePoint.Y });
                Vector<double> p101 = Vector<double>.Build.Dense(new double[] { movePos.X, movePos.Y });
                //计算独立模组坐标系下的坐标
                Vector<double> pl1 = R.Inverse() * (rpos - PT);
                Vector<double> pl2 = pl1 + (p101 - p100);
                //转换回机器人坐标系下的坐标
                var finalrpos = R * pl2 + PT;
                return (true, finalrpos[0], finalrpos[1], angle);
            }
            else
            {
                // 其他 CameraMount：如果提供了 robotCoord，则将仿射变换得到的点转换为工具坐标
                if (robotCoord != null)
                {
                    double curposX = robotCoord[0];
                    double curposY = robotCoord[1];
                    double curposU = robotCoord[2];

                    // 使用 ToolCoord 计算工具坐标（ToolCoord 内封装了从机器人基坐标到工具坐标的数学）
                    ToolCoord tool = new ToolCoord();

                    // Schneider 机器人角度符号可能与其他品牌相反，做兼容处理
                    if (robotBrand == RobotBrand.Schneider)
                    {
                        curposU *= -1;
                    }

                    // 计算工具坐标并将 rpos 替换为工具坐标的 X,Y
                    tool.ComputeTool(curposX, curposY, curposU, rpos[0], rpos[1]);
                    rpos = Vector<double>.Build.Dense(new double[] { tool.X, tool.Y });
                }
            }

            // 返回最终结果，IsSucceed 在当前实现中总为 true（可根据需要扩展）
            return (true, rpos[0], rpos[1], angle);
        }

        /// <summary>
        /// 通过 calibId 获取校准并执行像素->位置转换。
        /// </summary>
        /// <param name="pixelCoord">像素坐标及角度 (X, Y, angle)</param>
        /// <param name="robotCoord">机器人当前位姿数组 [X, Y, U]</param>
        /// <param name="calibId">校准项 Id</param>
        /// <param name="robotBrand">机器人品牌（可选）</param>
        /// <returns>返回 (IsSucceed, X, Y, U)</returns>
        public (bool IsSucceed, double X, double Y, double U) ConvertPixelToPosition((double X, double Y, double angle) pixelCoord, double[] robotCoord, Guid calibId, RobotBrand robotBrand = RobotBrand.Default)
        {

            var calib = GetCalibration(calibId);
            if (calib == null)
            {
                // 若未找到校准数据，返回失败标志
                return (false, 0, 0, 0);
            }
            var result = ConvertPixelToPosition(pixelCoord, robotCoord, calib, robotBrand);
            return result;

        }

        /// <summary>
        /// Dispose 时保存所有校准数据并抑制终结化。
        /// </summary>
        public void Dispose()
        {
            SaveAll();
            GC.SuppressFinalize(this);
        }
    }
}