Display_D_HP/TeamAAS-VM/ViewModels/Calibration/CalibToolViewModel.cs

using Cognex.VisionPro;
using Cognex.VisionPro.ToolBlock;
using MaterialDesignThemes.Wpf;
using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using NPOI.SS.Formula.Functions;
using OpenCvSharp;
using Prism.Commands;
using Prism.Events;
using Prism.Ioc;
using Prism.Mvvm;
using Prism.Regions;
using Prism.Services.Dialogs;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Reflection.Metadata;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Media.Media3D;
using Team.FFFeederService.Interfaces;
using TeamAAS_VP.Controls;
using TeamAAS_VP.Core;
using TeamAAS_VP.Events;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models;
using TeamAAS_VP;
using static OpenCvSharp.LineIterator;
using TeamAAS_VP.Resources.Languages;
using TeamAAS_VP.Models.Calibration;
using TeamAAS_VP.Services;
using System.Threading;

namespace TeamAAS_VP.ViewModels.Calibration
{
    public class CalibToolViewModel : BindableBase, IDialogAware
    {
        IRegionManager _regionManager;
        IRegionNavigationService _regionNavigationService;
        IContainerProvider _container;
        IDialogService _dialogService;
        IEventAggregator _eventAggregator;
        IConfigService _configService;
        IRobotService _robotService;
        ICameraService _cameraService;
        IFeederService _feederService;
        ICameraCalibrationService _cameraCalibrationService;

        private CancellationTokenSource _cts;
        #region 属性

        private ICogImage _Image;

        public ICogImage Image
        {
            get { return _Image; }
            set { SetProperty(ref _Image, value); }
        }

        private Cognex.VisionPro.CogGraphicCollection _Graphic;

        public Cognex.VisionPro.CogGraphicCollection Graphic
        {
            get { return _Graphic; }
            set { SetProperty(ref _Graphic, value); }
        }

        private CalibrationInfo _SelectCalibration;
        
        public CalibrationInfo SelectCalibration
        {
            get { return _SelectCalibration; }
            set { SetProperty(ref _SelectCalibration, value); }
        }

        public string Title=> "校准工具坐标";

        private double _Angle = 180;
        /// <summary>
        /// 机器人移动的范围U
        /// </summary>
        public double Angle
        {
            get { return _Angle; }
            set { SetProperty(ref _Angle, value); }
        }

        public RobotTool Tool { get; set; }
        /// <summary>
        /// 机器人与相机之间的旋转矩阵
        /// </summary>
        public Matrix<double> RobotCameraRotationMatrix {  get; set; }  
        public Management management { get; set; }
        public IRobot Robot { get; set; }
        public IVoiceCoilMotorFeeder Feeder { get; set; }

        private bool _IsRunning=true;

        public bool IsRunning
        {
            get { return _IsRunning; }
            set { SetProperty(ref _IsRunning,value); }
        }

        private ICamera _Camera;

        public ICamera Camera
        {
            get { return _Camera; }
            set { SetProperty(ref _Camera, value); }
        }
        #endregion

        #region 命令
        private DelegateCommand _ConfirmCommand;
        public DelegateCommand ConfirmCommand =>
            _ConfirmCommand ?? (_ConfirmCommand = new DelegateCommand(ExecuteConfirmCommand).ObservesCanExecute(() => IsRunning));

        private DelegateCommand _CancelCommand;
        public DelegateCommand CancelCommand =>
            _CancelCommand ?? (_CancelCommand = new DelegateCommand(ExecuteCancelCommand).ObservesCanExecute(() => IsRunning));

        private DelegateCommand _StartCalibCommand;
        public DelegateCommand StartCalibCommand =>
            _StartCalibCommand ?? (_StartCalibCommand = new DelegateCommand(ExecuteStartCalibCommand).ObservesCanExecute(()=> IsRunning));

        
        #endregion

        #region 事件
        public event Action<IDialogResult> RequestClose;
        #endregion

        public CalibToolViewModel(IRegionManager regionManager, IEventAggregator ea, IRegionNavigationService regionNavigationService, IContainerProvider container, IDialogService dialogService,
            IConfigService configService, IRobotService robotService, ICameraService cameraService, IFeederService feederService, ICameraCalibrationService cameraCalibrationService)
        {
            _regionManager = regionManager;
            _regionNavigationService = regionNavigationService;
            _container = container;
            _dialogService = dialogService;
            _eventAggregator = ea;
            _configService = configService;
            _robotService = robotService;
            _cameraService = cameraService;
            _feederService = feederService;
            _cameraCalibrationService = cameraCalibrationService;
        }

        #region 方法

        /// <summary>
        /// 确定
        /// </summary>
        void ExecuteConfirmCommand()
        {
            if (Tool==null)
            {
                return;
            }

            IDialogParameters parameters = new DialogParameters();
            parameters.Add("Tool", Tool);
            parameters.Add("RobotCameraRotationMatrix", RobotCameraRotationMatrix);
            parameters.Add("Angle", Angle);
            RequestClose?.Invoke(new DialogResult(ButtonResult.OK, parameters));
        }
        
        /// <summary>
        /// 取消
        /// </summary>
        void ExecuteCancelCommand()
        {
            IDialogParameters parameters = new DialogParameters();
            parameters.Add("Tool", Tool);
            RequestClose?.Invoke(new DialogResult(ButtonResult.Cancel, parameters));
        }

        /// <summary>
        /// 开始校准
        /// </summary>
        async void ExecuteStartCalibCommand()
        {
            if (SelectCalibration == null) return;
            if (Robot == null || !Robot.IsConnected) return;
            Robot.Timeout = 600000;
            try
            {
                if (Angle == 0)
                    Angle = 10;
                
                var view = new ShowMessage(Lang.Tool校准, Lang.是否开始自动校准工具坐标);
                //show the dialog
                var result = await DialogHost.Show(view, "CalibToolDialog", null, null, (sender,e) =>{
                    try
                    {
                        if (_cts != null && !_cts.IsCancellationRequested)
                        {
                            _cts.Cancel();
                            _cts.Dispose();
                            _cts = null;
                        }
                        
                    }
                    catch (Exception)
                    {

                    }
                });
                if (!((bool)result)) return;
                IsRunning = false;
                SelectCalibration.Angle = Angle;
                _cts = new CancellationTokenSource();
                var toolResult = await _cameraCalibrationService.CalibrateUncalibratedCameraTool(SelectCalibration, Robot, _cts.Token);
                if (toolResult.IsSuccess)
                {
                    if (Tool == null)
                    {
                        Tool = new RobotTool();
                    }
                    Tool.X = toolResult.ToolX;
                    Tool.Y = toolResult.ToolY;
                    RobotCameraRotationMatrix = toolResult.RotationMatrix;

                    //MessageBox.Show("校准完成");
                    view = new ShowMessage(Lang.Tool校准, Lang.校准完成, false);
                    await DialogHost.Show(view, "CalibToolDialog", null, null, null);

                    IDialogParameters parameters = new DialogParameters();
                    parameters.Add("Tool", Tool);
                    parameters.Add("RobotCameraRotationMatrix", RobotCameraRotationMatrix);
                    parameters.Add("Angle", Angle);
                    RequestClose?.Invoke(new DialogResult(ButtonResult.OK, parameters));
                }
                else
                {
                    view = new ShowMessage(Lang.Tool校准, Lang.校准失败, false);
                    //show the dialog
                    await DialogHost.Show(view, "CalibToolDialog", null, null, null);
                }
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("相机校准-校准验证界面示加载时出错!", ex);
                var view = new ShowMessage(Lang.Tool校准, $"{ex.Message}", false);
                //show the dialog
                var result = await DialogHost.Show(view, "CalibToolDialog", null, null, null);
            }
            finally
            {
                try
                {
                    if (_cts != null && !_cts.IsCancellationRequested)
                    {
                        _cts.Cancel();
                        _cts.Dispose();
                        
                    }
                    _cts = null;
                }
                catch (Exception)
                {

                }
                IsRunning = true;
            }
        }

        public bool CanCloseDialog()
        {
            return true;
        }

        public void OnDialogClosed()
        {
            _cameraCalibrationService.CaptureAndProcessCallback = null;
        }

        public void OnDialogOpened(IDialogParameters parameters)
        {
            SelectCalibration = parameters.GetValue<CalibrationInfo>("SelectCalibration");
            management = _container.Resolve<Management>();
            if (SelectCalibration != null)
            {
                Camera = _cameraService.GetCamera(SelectCalibration.CameraID);
                Robot = _robotService.GetRobot(SelectCalibration.RobotId);
                if (SelectCalibration.FeederId!=Guid.Empty)
                {
                    Feeder = _feederService.GetFeeder(SelectCalibration.FeederId);
                }
                Angle = SelectCalibration.Angle;
            }
            _cameraCalibrationService.CaptureAndProcessCallback = null;
            _cameraCalibrationService.CaptureAndProcessCallback = CaptureAndProcess;
        }

        /// <summary>
        /// 相机拍照
        /// </summary>
        /// <returns></returns>
        private async Task<(bool IsSuccess, double X, double Y, double U, int ImageWidth, int ImageHeight)> CaptureAndProcess()
        {
            if (Feeder != null)
            {
                Feeder.OpenLight();
                Feeder.SetLightTimeOut((ushort)SelectCalibration.Brightness);
            }
            await Task.Delay(200);
            //采集图像
            if (Camera != null)
            {
                ICogImage image = Camera.Grab();
                if (SelectCalibration.IsDistortionCorrection && SelectCalibration.CalibCheckerboardTool != null)
                {
                    SelectCalibration.CalibCheckerboardTool.InputImage = image;
                    SelectCalibration.CalibCheckerboardTool.Run();
                    image = SelectCalibration.CalibCheckerboardTool.OutputImage;
                }
                SelectCalibration.ToolBlock.Inputs["InputImage"].Value = image;
                
            }
            App.Current.Dispatcher.Invoke(() =>
            {
                Image = (ICogImage)SelectCalibration.ToolBlock.Inputs["InputImage"].Value;
                Graphic = null;
            });
            
            SelectCalibration.ToolBlock.Run();
            CogToolBlockTerminalCollection outputCollection = SelectCalibration.ToolBlock.Outputs;
            App.Current.Dispatcher.Invoke(() =>
            {
                Graphic = outputCollection["Graphic"].Value as CogGraphicCollection;
            });
            bool Found = (bool)(outputCollection["Found"].Value);
            if (Found)
            {
                var strpoints = outputCollection["Point"].Value.ToString();
                string[] items = strpoints.Split(',');
                double x = double.Parse(items[0]);
                double y = double.Parse(items[1]);
                return (true, x, y, 0, Image.Width, Image.Height);
            }
            else
            {
                return (false, 0, 0, 0, Image.Width, Image.Height);
            }
        }
        #endregion
    }
}