TeamAAS-2.5/TeamAAS/ViewModels/CalibrationViewModel.cs

using Prism.Commands;
using Prism.Mvvm;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.IO;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;
using System.Windows;
using TeamAAS.Camera.Interfaces;
using TeamAAS.Camera.Models;
using TeamAAS.Robot.Enums;
using TeamAAS.Robot.Interfaces;
using TeamAAS.Robot.Models;
using TeamAAS.Robot.Models.Robot;
using TeamAAS.Vision;
using TeamAAS.Vision.Calibration.Enums;
using TeamAAS.Vision.Calibration.Interfaces;
using TeamAAS.Vision.Calibration.Models;
using TeamAAS.Vision.Calibration.Services;

namespace TeamAAS.ViewModels
{
    /// <summary>
    /// 标定向导 ViewModel：驱动分步标定流程，方便调试人员单步执行。
    /// 算法本体在 <see cref="CameraCalibrationService"/>（TeamAAS.Vision），
    /// 视觉取点经当前视觉引擎的 <see cref="ICalibrationVisionProvider"/>（可整体替换）。
    /// 步骤：0 基础设置 / 1 视觉与相机 / 2 示教点位 / 3 工具坐标TCP / 4 九点标定 / 5 验证 / 6 完成。
    /// </summary>
    public class CalibrationViewModel : BindableBase
    {
        private readonly IRobotManager _robotManager;
        private readonly ICameraManager _cameraManager;
        private readonly CameraCalibrationService _calibService;
        private readonly ICalibrationService _store;
        private CancellationTokenSource _cts;

        public const int StepCount = 7;

        public CalibrationViewModel(IRobotManager robotManager, ICameraManager cameraManager)
        {
            _robotManager = robotManager;
            _cameraManager = cameraManager;
            _calibService = new CameraCalibrationService();
            _store = new CalibrationStore();
            _store.LoadAll();

            RefreshCalibrationList();
            RefreshDevices();
            WireCallbacks();
            EngineCalibrationView = (VisualEngineManager.Instance.Current ?? VisualEngineManager.Instance.GetOrLoad(App.SystemConfig?.Vision?.EngineName))?.GetCalibrationView();
        }

        #region 绑定属性

        private CalibrationInfo _Calib;
        public CalibrationInfo Calib
        {
            get { return _Calib; }
            set { SetProperty(ref _Calib, value); }
        }

        private int _StepIndex;
        public int StepIndex
        {
            get { return _StepIndex; }
            set { SetProperty(ref _StepIndex, value); RaisePropertyChanged(nameof(CanBack)); RaisePropertyChanged(nameof(CanNext)); }
        }

        private bool _IsBusy;
        public bool IsBusy
        {
            get { return _IsBusy; }
            set { SetProperty(ref _IsBusy, value); }
        }

        private string _StatusText = "就绪";
        public string StatusText
        {
            get { return _StatusText; }
            set { SetProperty(ref _StatusText, value); }
        }

        private FrameworkElement _EngineCalibrationView;
        public FrameworkElement EngineCalibrationView
        {
            get { return _EngineCalibrationView; }
            set { SetProperty(ref _EngineCalibrationView, value); }
        }

        public ObservableCollection<RobotInfo> Robots { get; } = new ObservableCollection<RobotInfo>();
        public ObservableCollection<CameraInfo> Cameras { get; } = new ObservableCollection<CameraInfo>();

        /// <summary>已保存的标定列表（来自 CalibrationStore），供界面选择加载。</summary>
        public ObservableCollection<CalibrationInfo> Calibrations { get; } = new ObservableCollection<CalibrationInfo>();

        private CalibrationInfo _SelectedCalibration;
        /// <summary>选中的已有标定：切换即把参数带回向导（Calib）。</summary>
        public CalibrationInfo SelectedCalibration
        {
            get { return _SelectedCalibration; }
            set
            {
                if (!SetProperty(ref _SelectedCalibration, value) || value == null) return;
                Calib = value;
                StepIndex = 0;
                RefreshDevices();
                // 切换标定时重新加载其视觉工具引用，避免沿用上一条的工具。
                var provider = Provider;
                if (provider != null && !string.IsNullOrEmpty(value.VisionToolRef))
                {
                    try { provider.LoadTool(value.VisionToolRef); } catch { }
                }
                SetStatus($"已加载标定：{value.Name}");
            }
        }

        private RobotInfo _SelectedRobot;
        public RobotInfo SelectedRobot
        {
            get { return _SelectedRobot; }
            set { SetProperty(ref _SelectedRobot, value); if (value != null) Calib.RobotId = value.Id; }
        }

        private CameraInfo _SelectedCamera;
        public CameraInfo SelectedCamera
        {
            get { return _SelectedCamera; }
            set
            {
                SetProperty(ref _SelectedCamera, value);
                if (value != null) { Calib.CameraID = value.Id; Calib.CameraName = value.CameraName; }
                else { Calib.CameraID = Guid.Empty; Calib.CameraName = ""; }
            }
        }

        public IEnumerable<CameraMount> CameraMounts => Enum.GetValues(typeof(CameraMount)).Cast<CameraMount>();
        public IEnumerable<PickPlaceModel> PickModels => Enum.GetValues(typeof(PickPlaceModel)).Cast<PickPlaceModel>();
        public IEnumerable<CalibTechP0Mode> P0Modes => Enum.GetValues(typeof(CalibTechP0Mode)).Cast<CalibTechP0Mode>();

        public bool CanBack => StepIndex > 0 && !IsBusy;
        public bool CanNext => StepIndex < StepCount - 1 && !IsBusy;

        private IRobot CurrentRobot => SelectedRobot != null ? _robotManager.GetRobot(SelectedRobot.Id) : null;
        private ICamera CurrentCamera => SelectedCamera != null ? _cameraManager.GetCamera(SelectedCamera.Id) : null;
        private ICalibrationVisionProvider Provider => VisualEngineManager.Instance.Current as ICalibrationVisionProvider;

        #endregion

        #region 命令

        private DelegateCommand _NextCommand;
        public DelegateCommand NextCommand => _NextCommand ?? (_NextCommand = new DelegateCommand(ExecuteNext, () => CanNext).ObservesProperty(() => IsBusy).ObservesProperty(() => StepIndex));

        private DelegateCommand _BackCommand;
        public DelegateCommand BackCommand => _BackCommand ?? (_BackCommand = new DelegateCommand(ExecuteBack, () => CanBack).ObservesProperty(() => IsBusy).ObservesProperty(() => StepIndex));

        private DelegateCommand _NewCommand;
        public DelegateCommand NewCommand => _NewCommand ?? (_NewCommand = new DelegateCommand(() => { _SelectedCalibration = null; RaisePropertyChanged(nameof(SelectedCalibration)); Calib = NewCalibration(); StepIndex = 0; RefreshDevices(); StatusText = "已新建标定"; }));

        private DelegateCommand _TestCaptureCommand;
        public DelegateCommand TestCaptureCommand => _TestCaptureCommand ?? (_TestCaptureCommand = new DelegateCommand(async () => await RunGuarded(TestCaptureAsync)));

        private DelegateCommand _TeachCenterCommand;
        public DelegateCommand TeachCenterCommand => _TeachCenterCommand ?? (_TeachCenterCommand = new DelegateCommand(async () => await TeachPointAsync(p => Calib.CenterPoint = p, "中心点")));

        private DelegateCommand _TeachHomeCommand;
        public DelegateCommand TeachHomeCommand => _TeachHomeCommand ?? (_TeachHomeCommand = new DelegateCommand(async () => await TeachPointAsync(p => Calib.HomePoint = p, "待机点")));

        private DelegateCommand _TeachMarkCommand;
        public DelegateCommand TeachMarkCommand => _TeachMarkCommand ?? (_TeachMarkCommand = new DelegateCommand(async () => await TeachPointAsync(p => Calib.MarkPoint = p, "Mark点")));

        private DelegateCommand<bool?> _gripperCommand;
        public DelegateCommand<bool?> GripperCommand => _gripperCommand ?? (_gripperCommand = new DelegateCommand<bool?>(async v => await GripperAsync(v == true)));

        private DelegateCommand _RunTcpCommand;
        public DelegateCommand RunTcpCommand => _RunTcpCommand ?? (_RunTcpCommand = new DelegateCommand(async () => await RunGuarded(RunTcpAsync)));

        private DelegateCommand _RunNinePointCommand;
        public DelegateCommand RunNinePointCommand => _RunNinePointCommand ?? (_RunNinePointCommand = new DelegateCommand(async () => await RunGuarded(RunNinePointAsync)));

        private DelegateCommand _RunVerifyCommand;
        public DelegateCommand RunVerifyCommand => _RunVerifyCommand ?? (_RunVerifyCommand = new DelegateCommand(async () => await RunGuarded(RunVerifyAsync)));

        private DelegateCommand _SaveCommand;
        public DelegateCommand SaveCommand => _SaveCommand ?? (_SaveCommand = new DelegateCommand(Save));

        private DelegateCommand _LoadVisionToolCommand;
        public DelegateCommand LoadVisionToolCommand => _LoadVisionToolCommand ?? (_LoadVisionToolCommand = new DelegateCommand(LoadVisionTool));

        #endregion

        #region 初始化

        private CalibrationInfo NewCalibration()
        {
            return new CalibrationInfo
            {
                Id = Guid.NewGuid(),
                Name = "Calib_" + DateTime.Now.ToString("yyyyMMdd_HHmmss"),
                DateTime = DateTime.Now,
                CenterPoint = new RPoint(),
                HomePoint = new RPoint(),
                MarkPoint = new RPoint(),
                Pick = new PickInfo(),
                Tool = new RobotTool(),
                Arm = new RobotArm(),
                CalibPoints = new ObservableCollection<RobotPixelPoint>(),
                Speed = 20,
                Accel = 20,
                Power = true,
                Width = 100,
                Height = 80,
                Angle = 180,
                WaitBlow = 200,
                WaitPhoto = 200,
                WaitSuction = 200,
                ExposureTime = 5000,
                Gain = 0,
                IsCreate = true,
            };
        }

        /// <summary>从 Store 载入已保存标定到下拉列表；有则默认选中最近一条把参数带回界面，无则新建空白标定。</summary>
        private void RefreshCalibrationList()
        {
            Calibrations.Clear();
            foreach (var c in _store.GetAllCalibrations()) Calibrations.Add(c);

            if (Calibrations.Count > 0)
            {
                SelectedCalibration = Calibrations.OrderByDescending(c => c.DateTime).First();
            }
            else
            {
                Calib = NewCalibration();
            }
        }

        private void RefreshDevices()
        {
            Robots.Clear();
            foreach (var r in _robotManager.GetAllRobotInfos()) Robots.Add(r);
            Cameras.Clear();
            foreach (var c in _cameraManager.GetAllCameraInfos()) Cameras.Add(c);

            if (Calib.RobotId != Guid.Empty) SelectedRobot = Robots.FirstOrDefault(x => x.Id == Calib.RobotId);
            if (Calib.CameraID != Guid.Empty) SelectedCamera = Cameras.FirstOrDefault(x => x.Id == Calib.CameraID);
        }

        private void WireCallbacks()
        {
            _calibService.CaptureAndProcessCallback = CaptureAndProcessAsync;
            _calibService.PromptPlaceCalibrationBlockCallback = PromptPlaceBlockAsync;
        }

        /// <summary>拍照 → 视觉引擎取特征点（引擎可换）。</summary>
        private async Task<(bool IsSuccess, double X, double Y, double U, int ImageWidth, int ImageHeight)> CaptureAndProcessAsync()
        {
            var cam = CurrentCamera;
            var provider = Provider;
            if (cam == null || provider == null)
            {
                SetStatus("未选择相机或当前视觉引擎不支持自动取点");
                return (false, 0, 0, 0, 0, 0);
            }

            // 惰性加载视觉标定工具（若已配置 .vpp 引用但尚未加载）。
            if (provider.CurrentTool == null && !string.IsNullOrEmpty(Calib.VisionToolRef))
            {
                provider.LoadTool(Calib.VisionToolRef);
            }

            try
            {
                cam.SetExposureTime(Calib.ExposureTime);
                cam.SetGain(Calib.Gain);
            }
            catch { }

            var img = cam.Grab();
            if (img == null) return (false, 0, 0, 0, 0, 0);

            if (Calib.IsDistortionCorrection) img = await provider.Undistort(img);

            var r = await provider.FindCalibPoint(img);
            int w = img.Width, h = img.Height;
            return (r.found, r.px, r.py, r.angle, w, h);
        }

        private Task<bool> PromptPlaceBlockAsync()
        {
            bool ok = false;
            var app = Application.Current;
            if (app != null && app.Dispatcher != null && !app.Dispatcher.CheckAccess())
            {
                app.Dispatcher.Invoke(new Action(() =>
                    ok = MessageBox.Show("请将标定块摆放到中心/吸嘴下，摆好后点击“确定”继续。", "标定提示",
                        MessageBoxButton.OKCancel, MessageBoxImage.Question) == MessageBoxResult.OK));
            }
            else
            {
                ok = MessageBox.Show("请将标定块摆放到中心/吸嘴下，摆好后点击“确定”继续。", "标定提示",
                    MessageBoxButton.OKCancel, MessageBoxImage.Question) == MessageBoxResult.OK;
            }
            return Task.FromResult(ok);
        }

        #endregion

        #region 步骤动作

        private async Task TestCaptureAsync()
        {
            var r = await CaptureAndProcessAsync();
            SetStatus(r.IsSuccess
                ? $"取点成功：像素=({r.X:F2}, {r.Y:F2}) 角度={r.U:F2} 图像={r.ImageWidth}x{r.ImageHeight}"
                : "取点失败：未找到特征点或相机/引擎不可用");
        }

        private async Task TeachPointAsync(Action<RPoint> assign, string label)
        {
            var robot = CurrentRobot;
            if (robot == null || !robot.IsConnected) { SetStatus("机器人未连接，无法示教" + label); return; }
            var pos = await robot.GetRobotPosAsync();
            assign(pos);
            SetStatus($"{label}已示教：({pos.X:F3}, {pos.Y:F3}, {pos.Z:F3}, U={pos.U:F3})");
        }

        private async Task GripperAsync(bool on)
        {
            var robot = CurrentRobot;
            if (robot == null || !robot.IsConnected) { SetStatus("机器人未连接"); return; }
            await robot.CalibOutIOAsync(on);
            SetStatus(on ? "已吸气/夹紧" : "已破真空/张开");
        }

        private async Task RunTcpAsync()
        {
            var robot = CurrentRobot;
            if (!EnsureReady(robot)) return;
            _cts = new CancellationTokenSource();
            var r = await _calibService.CalibrateUncalibratedCameraTool(Calib, robot, _cts.Token);
            SetStatus(r.IsSuccess
                ? $"工具坐标标定完成：Tool=({r.ToolX:F3}, {r.ToolY:F3}) 像素比例=({r.PixelScaleX:F5}, {r.PixelScaleY:F5}) mm/px"
                : "工具坐标标定失败");
        }

        private async Task RunNinePointAsync()
        {
            var robot = CurrentRobot;
            if (!EnsureReady(robot)) return;
            _cts = new CancellationTokenSource();
            var r = await _calibService.AutoNinePointCalibration(Calib, robot, _cts.Token);
            if (!r.IsSuccess) { SetStatus("九点标定失败"); return; }
            Calib.AffineTransformationMaterial = r.AffineTransformationMaterial.ToArray();
            Calib.CalibPoints = new ObservableCollection<RobotPixelPoint>(r.NinePoint);
            Calib.CalibrationResult = r.Result;
            SetStatus($"九点标定完成：RMS={r.Result.RMS:F4} mm 最大误差={r.Result.MaxErrorValue:F4} mm 旋转={r.Result.Rotate:F3}°");
        }

        private async Task RunVerifyAsync()
        {
            var robot = CurrentRobot;
            if (!EnsureReady(robot)) return;
            if (Calib.AffineTransformationMaterial == null) { SetStatus("请先完成九点标定"); return; }
            _cts = new CancellationTokenSource();
            var r = await _calibService.ExecuteCalibrationValidation(Calib, robot, _cts.Token);
            if (!r.IsSuccess) { SetStatus("验证失败"); return; }
            Calib.CalibrationTestResult = r.Result;
            SetStatus($"验证完成：RMSE=({r.Result.RMSEX:F4}, {r.Result.RMSEY:F4}) mm");
        }

        private void Save()
        {
            try
            {
                if (string.IsNullOrWhiteSpace(Calib.Name)) { SetStatus("名称为空，无法保存"); return; }
                // 保存视觉工具引用（引擎自定义，如 .vpp 路径）。
                var provider = Provider;
                if (provider != null)
                {
                    string dir = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "Calibration", Calib.Name);
                    Calib.VisionToolRef = provider.SaveTool(dir);
                }
                _store.AddOrUpdateCalibration(Calib);
                Calib.IsCreate = false;
                // 让已保存标定进入下拉列表并保持选中（不触发重新加载）。
                if (!Calibrations.Any(x => ReferenceEquals(x, Calib))) Calibrations.Add(Calib);
                _SelectedCalibration = Calib;
                RaisePropertyChanged(nameof(SelectedCalibration));
                SetStatus($"已保存标定：{Calib.Name}");
            }
            catch (Exception ex)
            {
                SetStatus("保存失败：" + ex.Message);
            }
        }

        private void LoadVisionTool()
        {
            var provider = Provider;
            if (provider == null) { SetStatus("当前视觉引擎不支持标定工具"); return; }
            var dlg = new Microsoft.Win32.OpenFileDialog { Filter = "VisionPro 工具块|*.vpp|所有文件|*.*" };
            if (dlg.ShowDialog() != true) return;
            Calib.VisionToolRef = dlg.FileName;
            provider.LoadTool(dlg.FileName);
            SetStatus("已加载视觉标定工具：" + Path.GetFileName(dlg.FileName));
        }

        private bool EnsureReady(IRobot robot)
        {
            if (robot == null || !robot.IsConnected) { SetStatus("机器人未连接"); return false; }
            if (Provider == null) { SetStatus("当前视觉引擎不支持自动标定取点"); return false; }
            if (Calib.Pick == null) Calib.Pick = new PickInfo();
            return true;
        }

        #endregion

        #region 导航

        private void ExecuteNext()
        {
            int next = StepIndex + 1;
            // XY 模组无需单独的工具坐标(TCP)步骤，跳过。
            if (next == 3 && Calib.CameraMount == CameraMount.MobileDown_XYPlatform) next = 4;
            if (next < StepCount) StepIndex = next;
        }

        private void ExecuteBack()
        {
            int prev = StepIndex - 1;
            if (prev == 3 && Calib.CameraMount == CameraMount.MobileDown_XYPlatform) prev = 2;
            if (prev >= 0) StepIndex = prev;
        }

        #endregion

        private void SetStatus(string msg)
        {
            var app = Application.Current;
            if (app != null && app.Dispatcher != null && !app.Dispatcher.CheckAccess())
                app.Dispatcher.Invoke(new Action(() => StatusText = msg));
            else
                StatusText = msg;
        }

        /// <summary>统一守护：防重入 + 异常兜底 + 状态提示。</summary>
        private async Task RunGuarded(Func<Task> action)
        {
            if (IsBusy) return;
            IsBusy = true;
            try
            {
                await action();
            }
            catch (Exception ex)
            {
                SetStatus("执行异常：" + ex.Message);
            }
            finally
            {
                try { if (_cts != null && !_cts.IsCancellationRequested) _cts.Cancel(); _cts?.Dispose(); }
                catch { }
                _cts = null;
                IsBusy = false;
            }
        }
    }
}