Display_D_HP/TeamAAS-VM/ViewModels/Product/CalculateOffsetUViewModel.cs

using MathNet.Numerics.LinearAlgebra;
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.Collections.ObjectModel;
using System.Linq;
using System.Security.Cryptography;
using System.Threading.Tasks;
using System.Windows;
using Team.FFFeederService.Interfaces;
using TeamAAS_VP.Events;
using TeamAAS_VP.Core;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models;
using TeamAAS_VP;
using MaterialDesignThemes.Wpf;
using TeamAAS_VP.Controls;
using System.Web.UI.WebControls;
using TouchSocket.Core;
using Cognex.VisionPro;
using Cognex.VisionPro.ToolBlock;
using System.Drawing;
using TeamAAS_VP.Resources.Languages;

namespace TeamAAS_VP.ViewModels.Product
{
    public class CalculateOffsetUViewModel : BindableBase, IDialogAware
    {
        IRegionManager _regionManager;
        IRegionNavigationService _regionNavigationService;
        IContainerProvider _container;
        IDialogService _dialogService;
        IEventAggregator _eventAggregator;
        IConfigService _configService;
        IRobotService _robotService;
        ICameraService _cameraService;
        IFeederService _feederService;
        ICalibrationService _calibrationService;

        #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 ProductModel _SelectProduct;

        /// <summary>
        /// 选中的产品
        /// </summary>
        public ProductModel SelectProduct
        {
            get { return _SelectProduct; }
            set { SetProperty(ref _SelectProduct, value); }
        }

        private ProcedureModel _SelectProcedure;

        /// <summary>
        /// 选中的流程
        /// </summary>
        public ProcedureModel SelectProcedure
        {
            get { return _SelectProcedure; }
            set { SetProperty(ref _SelectProcedure, value); }
        }

        private Management _management;

        public Management management
        {
            get { return _management; }
            set { SetProperty(ref _management, value); }
        }

        private IRobot _Robot;

        public IRobot Robot
        {
            get { return _Robot; }
            set { SetProperty(ref _Robot, value); }
        }

        public string Title => Lang.自动计算取料角度和工具坐标;

        private ToolInfo _ToolInfo;

        public ToolInfo ToolInfo
        {
            get { return _ToolInfo; }
            set { SetProperty(ref _ToolInfo,value); }
        }

        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 _TechPlacePosCommand;
        public DelegateCommand TechPlacePosCommand =>
            _TechPlacePosCommand ?? (_TechPlacePosCommand = new DelegateCommand(ExecuteTechPlacePosCommand).ObservesCanExecute(() => IsRunning));

        private DelegateCommand _TechPickPosCommand;
        public DelegateCommand TechPickPosCommand =>
            _TechPickPosCommand ?? (_TechPickPosCommand = new DelegateCommand(ExecuteTechPickPosCommand).ObservesCanExecute(() => IsRunning));

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

        private DelegateCommand<object> _BlowOrInhalControlCommand;
        public DelegateCommand<object> BlowOrInhalControlCommand =>
            _BlowOrInhalControlCommand ?? (_BlowOrInhalControlCommand = new DelegateCommand<object>(ExecuteBlowOrInhalControlCommand).ObservesCanExecute(() => IsRunning));

        
        #endregion

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

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

            //订阅权限登录事件
            _eventAggregator.GetEvent<UserLoginNotification>().Subscribe(LoginChange);
            _calibrationService = calibrationService;
        }

        #region 方法

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

            try
            {
                IDialogParameters parameters = new DialogParameters();
                parameters.Add("ToolInfo", ToolInfo);
                RequestClose?.Invoke(new DialogResult(ButtonResult.OK, parameters));
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(ex.Message, ex);
            }
        }

        /// <summary>
        /// 取消
        /// </summary>
        void ExecuteCancelCommand()
        {
            try
            {
                IDialogParameters parameters = new DialogParameters();
                parameters.Add("ToolInfo", ToolInfo);
                RequestClose?.Invoke(new DialogResult(ButtonResult.Cancel, parameters));
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(ex.Message, ex);
            }
        }

        public bool CanCloseDialog()
        {
            return true;
        }

        public void OnDialogClosed()
        {
            if (Robot!=null)
            {
                Robot.EnterDebugMode = false;
            }
        }

        public void OnDialogOpened(IDialogParameters parameters)
        {
            try
            {
                SelectProcedure = _container.Resolve<ProcessBasicsViewModel>().SelectProcedure;
                Camera = _cameraService.GetCamera(SelectProcedure.CameraId);
                management = _container.Resolve<Management>();
                management = _container.Resolve<Management>();
                if (SelectProcedure.RobotId!=Guid.Empty)
                {
                    Robot = _robotService.GetRobot(SelectProcedure.RobotId);
                    Robot.EnterDebugMode = true;
                }
                
                if (SelectProcedure.FeederId!=Guid.Empty)
                {
                    Feeder = _feederService.GetFeeder(SelectProcedure.FeederId);
                }
                
                var param = parameters.GetValue<ToolInfo>("ToolInfo");
                if (param != null)
                {
                    ToolInfo = param;
                }
                if (Robot == null || !Robot.IsConnected)
                {
                    MessageBox.Show(Lang.机器人未连接);
                }
                else
                {
                    Robot.Timeout = 5000;
                    Robot.SFree();
                }
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(ex.Message, ex);
            }
        }

        /// <summary>
        /// 自动校准计算
        /// </summary>
        async void ExecuteStartCalibCommand()
        {
            try
            {
                if (Robot == null || !Robot.IsConnected)
                {
                    return;
                }
                IsRunning = false;
                Robot.Timeout = 1000000;
                await Robot.SLockAsync();
                //发送校准的参数
                await Robot.CalibParameAsync(ToolInfo.PickInfo, 20, 50, false, 300, 300);

                var pos = ToolInfo.Place.Clone();
                //pos.Z = 0;
                //移动机器人至待机位置
                await Robot.CalibMotionAsync(pos, 0);

                //打开吸气
                await Robot.CalibOutIOAsync(true);

                var view = new ShowMessage(Lang.提示, $"{Lang.请将产品放置在放料位置}！");
                //show the dialog
                var result = await DialogHost.Show(view, "CalculateOffsetUDialog", null, null, null);
                if (!((bool)result)) { IsRunning = true; return; }


                //移动机器人至放料位置
                await Robot.CalibMotionAsync(ToolInfo.Place, 0);

                await Task.Delay(300);

                var calib = _calibrationService.GetAllCalibrations().FirstOrDefault(cal => cal.Id == SelectProcedure.CalibrationId);
                if (calib == null)
                {
                    throw new Exception(Lang.没有可用的校准);
                }
                ///feeder中计算工具坐标
                if (calib.CameraMount== Enums.CameraMount.FixedDown && SelectProcedure.FeederId!= Guid.Empty)
                {
                    //移动机器人至取料位置
                    await Robot.CalibMotionAsync(ToolInfo.Pick, 0);
                    //关闭吸气
                    await Robot.CalibOutIOAsync(false);
                    await Task.Delay(300);

                    //移动机器人至待机位置
                    await Robot.CalibMotionAsync(pos, 0);

                    var campos = await Photo();

                    //-------------------------------------------------------------------------------------------
                    //U轴旋转确认机器人与相机旋转方向
                    var posPick = ToolInfo.Pick.Clone();
                    posPick.U += 10;
                    //移动机器人至取料位置
                    await Robot.CalibMotionAsync(ToolInfo.Pick, 0);
                    //打开吸气
                    await Robot.CalibOutIOAsync(true);
                    await Task.Delay(300);
                    //机器人旋转角度
                    await Robot.CalibMotionAsync(posPick, 0);
                    //关闭吸气
                    await Robot.CalibOutIOAsync(false);
                    await Task.Delay(300);

                    //移动机器人至待机位置
                    await Robot.CalibMotionAsync(pos, 0);

                    var campos1 = await Photo();

                    if (campos1.Item3 - campos.Item3 > 0)
                    {
                        ToolInfo.IsForward = true;
                    }
                    else
                    {
                        ToolInfo.IsForward = false;
                    }

                    //计算工具坐标
                    ToolCoord toolCoord = new ToolCoord();
                    toolCoord.ComputeTool(ToolInfo.Pick.X, ToolInfo.Pick.Y, ToolInfo.Pick.U, campos.Item1, campos.Item2);
                    if (ToolInfo.Tool == null) ToolInfo.Tool = new RobotTool();
                    ToolInfo.Tool.X = toolCoord.X;
                    ToolInfo.Tool.Y = toolCoord.Y;


                    //计算取料角度补偿值
                    if (ToolInfo.IsForward)
                    {
                        ToolInfo.OffsetU = ToolInfo.Pick.U - campos.Item3;
                    }
                    else
                    {
                        ToolInfo.OffsetU = ToolInfo.Pick.U + campos.Item3;
                    }
                    ToolInfo.OffsetZ = ToolInfo.Pick.Z;

                    //机器人旋转角度
                    await Robot.CalibMotionAsync(posPick, 0);
                    //打开吸气
                    await Robot.CalibOutIOAsync(true);
                    await Task.Delay(300);
                    //移动机器人至待机位置
                    await Robot.CalibMotionAsync(pos, 0);
                }
                //目前只考虑到固定向上相机计算工具坐标
                else
                {
                    //移动机器人至取料位置
                    await Robot.CalibMotionAsync(ToolInfo.Pick, 0);
                    var campos = await Photo();

                    //-------------------------------------------------------------------------------------------
                    //U轴旋转确认机器人与相机旋转方向
                    var posPick = ToolInfo.Pick.Clone();
                    posPick.U += 10;
                    //机器人旋转角度
                    await Robot.CalibMotionAsync(posPick, 0);
                    var campos1 = await Photo();

                    if (campos1.Item3 - campos.Item3 > 0)
                    {
                        ToolInfo.IsForward = true;
                    }
                    else
                    {
                        ToolInfo.IsForward = false;
                    }

                    //计算工具坐标
                    ToolCoord toolCoord = new ToolCoord();
                    toolCoord.ComputeTool(ToolInfo.Pick.X, ToolInfo.Pick.Y, ToolInfo.Pick.U, campos.Item1, campos.Item2);
                    if (ToolInfo.Tool == null) ToolInfo.Tool = new RobotTool();
                    ToolInfo.Tool.X = toolCoord.X;
                    ToolInfo.Tool.Y = toolCoord.Y;


                    //计算取料角度补偿值
                    if (ToolInfo.IsForward)
                    {
                        ToolInfo.OffsetU = ToolInfo.Pick.U - campos.Item3;
                    }
                    else
                    {
                        ToolInfo.OffsetU = ToolInfo.Pick.U + campos.Item3;
                    }
                    ToolInfo.OffsetZ = ToolInfo.Pick.Z;
                }

                if (ToolInfo.Tool.Number > 0)
                {
                    await Robot.SetToolAsync(ToolInfo.Tool.Number, ToolInfo.Tool.X, ToolInfo.Tool.Y);
                }
                view = new ShowMessage(Lang.提示, $"{Lang.校准完成}！",false);
                //show the dialog
                result = await DialogHost.Show(view, "CalculateOffsetUDialog", null, null, null);
                //MessageBox.Show("完成");
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(Lang.自动校准计算工具坐标和U补偿时出错, ex);
                var view = new ShowMessage("Error", ex.Message, false);
                //show the dialog
                var result = await DialogHost.Show(view, "CalculateOffsetUDialog", null, null, null);
            }
            finally
            {
                IsRunning = true;
            }
        }

        /// <summary>
        /// 示教取料点
        /// </summary>
        async void ExecuteTechPickPosCommand()
        {
            try
            {
                IsRunning = false;
                if (Robot == null || !Robot.IsConnected)
                {
                    return;
                }
                ToolInfo.Pick = await Robot.GetRobotPosAsync();
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(Lang.示教取料点时出错, ex);
                MessageBox.Show(ex.Message);
            }
            finally
            {
                IsRunning=true;
            }
        }

        /// <summary>
        /// 示教放料点
        /// </summary>
        async void ExecuteTechPlacePosCommand()
        {
            try
            {
                IsRunning = false;
                if (Robot==null || !Robot.IsConnected)
                {
                    return;
                }
                ToolInfo.Place = await Robot.GetRobotPosAsync();
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(Lang.示教放料点时出错, ex);
                MessageBox.Show(ex.Message);
            }
            finally
            {
                IsRunning = true;
            }
        }

        /// <summary>
        /// 相机拍照
        /// </summary>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        private async Task<Tuple<double, double, double>> Photo()
        {
            if (Feeder != null)
            {
                await Feeder.OpenLightAsync();
                await Feeder.SetLightValueAsync((ushort)SelectProcedure.LightValue);
                await Task.Delay(200);
            }

            //采集图像
            if (Camera != null)
            {
                SelectProcedure.ToolBlock.Inputs["InputImage"].Value = Camera.Grab();
                Image = (ICogImage)SelectProcedure.ToolBlock.Inputs["InputImage"].Value;
            }
            Graphic = null;
            SelectProcedure.ToolBlock.Run();

            if (Feeder != null)
            {
                await Feeder.CloseLightAsync();
            }
            CogToolBlockTerminalCollection outputCollection = SelectProcedure.ToolBlock.Outputs;
            Graphic = outputCollection["Graphic"].Value as CogGraphicCollection;
            bool Found = (bool)(outputCollection["Found"].Value);
            if (Found)
            {
                var strpoints = outputCollection["Point"].Value.ToString();
                string[] items = strpoints.Split(',');
                var calib = _calibrationService.GetAllCalibrations().FirstOrDefault(cal => cal.Id == SelectProcedure.CalibrationId);
                if (calib == null)
                {
                    throw new Exception(Lang.没有可用的校准);
                }
                Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(calib.AffineTransformationMaterial);
                double x = double.Parse(items[0]);
                double y = double.Parse(items[1]);
                double angle = double.Parse(items[2]);
                var rpos = matrix * Vector<double>.Build.Dense(new double[] { x, y, 1 });
                return new Tuple<double, double, double>(rpos[0], rpos[1],angle);
            }
            else
            {
                throw new Exception(Lang.拍照失败);
            }
            
        }

        async void ExecuteBlowOrInhalControlCommand(object isInhal)
        {
            try
            {
                if (Robot == null || !Robot.IsConnected)
                {
                    return;
                }
                IsRunning = false;
                Robot.Timeout = 10000;
                await Robot.CalibOutIOAsync(!(bool)isInhal);
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError(Lang.示教取料点时出错, ex);
                MessageBox.Show(ex.Message);
            }
            finally
            {
                IsRunning = true;
            }
            
        }
        #endregion

        private bool _IsAllowEdit = false;

        public bool IsAllowEdit
        {
            get { return _IsAllowEdit; }
            set { SetProperty(ref _IsAllowEdit, value); }
        }

        private void LoginChange(Models.User user)
        {
            if (user.userPart == Enums.UserPart.Operator)
            {
                IsAllowEdit = false;
            }
            else
            {
                IsAllowEdit = true;
            }
        }
    }
}