TeamAAS-2.5/TeamAAS.Core/TeamAAS.Vision/Calibration/Services/CalibrationStore.cs

using MathNet.Numerics.LinearAlgebra;
using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.IO;
using System.Linq;
using System.Text;
using TeamAAS.Robot.Core;
using TeamAAS.Robot.Enums;
using TeamAAS.Vision.Calibration.Enums;
using TeamAAS.Vision.Calibration.Interfaces;
using TeamAAS.Vision.Calibration.Models;

namespace TeamAAS.Vision.Calibration.Services
{
    /// <summary>
    /// 标定管理服务：负责校准配置的读取/保存（Newtonsoft.Json）与运行时像素→机器人坐标换算。
    /// 视觉工具的持久化由各引擎的 <see cref="ICalibrationVisionProvider"/> 负责，本服务只保存其引用 <see cref="CalibrationInfo.VisionToolRef"/>。
    /// </summary>
    public class CalibrationStore : ICalibrationService
    {
        private readonly string _calibrationPath;
        private readonly string _listFilePath;
        private readonly object _sync = new object();
        private ObservableCollection<CalibrationInfo> _calibrations = new ObservableCollection<CalibrationInfo>();

        public CalibrationStore() : this(Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "Calibration"))
        {
        }

        public CalibrationStore(string calibrationPath)
        {
            _calibrationPath = calibrationPath;
            _listFilePath = Path.Combine(_calibrationPath, "CalibrationList.json");
        }

        private static readonly JsonSerializerSettings JsonSettings = new JsonSerializerSettings
        {
            Formatting = Formatting.Indented,
            TypeNameHandling = TypeNameHandling.None,
            ReferenceLoopHandling = ReferenceLoopHandling.Ignore,
        };

        private static void WriteJson<T>(T value, string path)
        {
            string dir = Path.GetDirectoryName(path);
            if (!string.IsNullOrEmpty(dir) && !Directory.Exists(dir)) Directory.CreateDirectory(dir);
            File.WriteAllText(path, JsonConvert.SerializeObject(value, JsonSettings), Encoding.UTF8);
        }

        private static T ReadJson<T>(string path)
        {
            if (!File.Exists(path)) return default(T);
            return JsonConvert.DeserializeObject<T>(File.ReadAllText(path, Encoding.UTF8), JsonSettings);
        }

        public IReadOnlyCollection<CalibrationInfo> GetAllCalibrations()
        {
            lock (_sync)
            {
                return _calibrations.ToList().AsReadOnly();
            }
        }

        public CalibrationInfo GetCalibration(Guid id)
        {
            lock (_sync)
            {
                return _calibrations.FirstOrDefault(c => c.Id == id);
            }
        }

        public CalibrationInfo AddOrUpdateCalibration(CalibrationInfo calib)
        {
            if (calib == null) return null;
            lock (_sync)
            {
                var exist = _calibrations.FirstOrDefault(c => c.Id == calib.Id);
                if (exist != null)
                {
                    var idx = _calibrations.IndexOf(exist);
                    _calibrations[idx] = calib;
                }
                else
                {
                    calib.Index = _calibrations.Count + 1;
                    _calibrations.Add(calib);
                }
            }

            calib.DateTime = DateTime.Now;
            calib.IsCreate = false;
            string folder = Path.Combine(_calibrationPath, calib.Name);
            string calibFile = Path.Combine(folder, calib.Name + ".json");
            if (!Directory.Exists(folder)) Directory.CreateDirectory(folder);

            WriteJson(calib, calibFile);
            WriteList();
            return calib;
        }

        public bool RemoveCalibration(Guid id)
        {
            bool result;
            lock (_sync)
            {
                var removed = _calibrations.FirstOrDefault(c => c.Id == id);
                result = removed != null && _calibrations.Remove(removed);
            }

            WriteList();

            // 删除后重新编号并保存。
            List<CalibrationInfo> calibs;
            lock (_sync) { calibs = _calibrations.OrderBy(c => c.Index).ToList(); }
            int index = 1;
            foreach (var item in calibs)
            {
                item.Index = index++;
                string folder = Path.Combine(_calibrationPath, item.Name);
                WriteJson(item, Path.Combine(folder, item.Name + ".json"));
            }
            lock (_sync) { _calibrations = new ObservableCollection<CalibrationInfo>(calibs); }
            return result;
        }

        private void WriteList()
        {
            var list = new Dictionary<Guid, string>();
            lock (_sync)
            {
                foreach (var item in _calibrations) list[item.Id] = item.Name;
            }
            WriteJson(list, _listFilePath);
        }

        public void LoadAll()
        {
            lock (_sync)
            {
                if (!Directory.Exists(_calibrationPath)) Directory.CreateDirectory(_calibrationPath);
                _calibrations.Clear();
                if (!File.Exists(_listFilePath))
                {
                    WriteJson(new Dictionary<Guid, string>(), _listFilePath);
                    return;
                }

                var list = ReadJson<Dictionary<Guid, string>>(_listFilePath);
                if (list == null) return;

                foreach (var item in list)
                {
                    try
                    {
                        string filePath = Path.Combine(_calibrationPath, item.Value, item.Value + ".json");
                        if (File.Exists(filePath))
                        {
                            var calib = ReadJson<CalibrationInfo>(filePath);
                            if (calib != null) _calibrations.Add(calib);
                        }
                    }
                    catch
                    {
                        // 忽略单个文件错误，继续加载其他配置。
                    }
                }
            }
        }

        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(_calibrationPath, item.Name);
                    if (!Directory.Exists(folder)) Directory.CreateDirectory(folder);
                    WriteJson(item, Path.Combine(folder, item.Name + ".json"));
                }
                WriteJson(list, _listFilePath);
            }
        }

        /// <summary>
        /// 校准转换：根据像素坐标与机器人当前坐标换算目标位姿（按相机安装方式分支）。
        /// </summary>
        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;
            Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(calib.AffineTransformationMaterial);
            if (calib.CameraMount == CameraMount.FixedDown || calib.CameraMount == CameraMount.MobileJ2 || calib.CameraMount == CameraMount.MobileJ4)
            {
                angle *= -1;
            }
            var rpos = matrix * Vector<double>.Build.Dense(new double[] { pixelCoord.X, pixelCoord.Y, 1 });

            if (calib.CameraMount == CameraMount.MobileJ4)
            {
                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];

                double angle1 = Math.PI * (curpos_u - calib.MarkPoint.U) / 180;
                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] });
            }
            else if (calib.CameraMount == CameraMount.MobileDown_XYPlatform)
            {
                double Robot_X = rpos[0];
                double Robot_Y = rpos[1];
                double curpos_x = robotCoord[0];
                double curpos_y = robotCoord[1];

                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] });
            }
            else
            {
                if (robotCoord != null)
                {
                    double curposX = robotCoord[0];
                    double curposY = robotCoord[1];
                    double curposU = robotCoord[2];

                    ToolCoord tool = new ToolCoord();
                    if (robotBrand == RobotBrand.Schneider) curposU *= -1;
                    tool.ComputeTool(curposX, curposY, curposU, rpos[0], rpos[1]);
                    rpos = Vector<double>.Build.Dense(new double[] { tool.X, tool.Y });
                }
            }
            return (true, rpos[0], rpos[1], angle);
        }

        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);
            return ConvertPixelToPosition(pixelCoord, robotCoord, calib, robotBrand);
        }

        public void Dispose()
        {
            try { SaveAll(); } catch { }
            GC.SuppressFinalize(this);
        }
    }
}