LGES-OC5-PtoP-Welder/WeldingComponents/WeldingCommon/Views/Manual/Scanner/UCScannerMotionViewModel.cs

using Aurora;
using Aurora.Api;
using Aurora.Api.Manager;
using Aurora.View;
using AuroraLaser;
using AuroraLaserScanner;
using HarfBuzzSharp;
using Microsoft.Office.Interop.Excel;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Reactive.Linq;
using System.Windows.Input;
using System.Windows.Threading;

namespace WeldingCommon.Views
{
    public enum eAxisType
    {
        AXIS_X,
        AXIS_Y
    }

    public enum eJogDirection
    {
        MINUS,
        PLUS
    }

    public enum eMotionMode
    {
        ABS,
        REL
    };

    public enum eScannerMoveKind
    {
        CommandPos,
        Jog,
        Home
    }

    /// <summary>
    /// 레시피 화면(publish 모드)에서 매뉴얼 스캐너 VM(execute 모드)으로 보내는 모션 요청 페이로드.
    /// 매뉴얼 VM의 저장 state가 아니라 이 값으로 동작하도록 모든 입력값을 담는다.
    /// </summary>
    public class ScannerCommandMoveParam
    {
        public eScannerMoveKind Kind;
        public eAxisType Axis;
        public eJogDirection Direction;
        public double CommandPosX;
        public double CommandPosY;
        public double StepValue;
        public eMotionMode Mode;
        public double VelocityRate;
    }

    public class UCScannerMotionViewModel : NotifyPropertyChangedBase
    {
        private readonly Logger Log = new Logger("UCScannerMotionViewModel");

        public ICommand CMD_VisibleChanged { get; }
        public ICommand CMD_SpeedChanged { get; }
        public ICommand CMD_MotionModeChanged { get; }

        public ICommand CMD_JogMove { get; }
        public ICommand CMD_CommandPosMove { get; }


        private readonly IAuroraEnvironment _Aurora;

        private readonly Scanner _Scanner;
        private readonly Laser _Laser;
        private readonly ScannerConfig _ScannerConfig;

        private double _ScannerXPos;
        public double ScannerXPos { get => _ScannerXPos; set => SetProperty(ref _ScannerXPos, value); }
        private double _ScannerYPos;
        public double ScannerYPos { get => _ScannerYPos; set => SetProperty(ref _ScannerYPos, value); }
        private bool _IsScannerBusy;
        public bool IsScannerBusy { get => _IsScannerBusy; set => SetProperty(ref _IsScannerBusy, value); }

        private double _StepValue;
        public double StepValue { get => _StepValue; set => SetProperty(ref _StepValue, value); }

        private double _CommandPosX;
        public double CommandPosX { get => _CommandPosX; set => SetProperty(ref _CommandPosX, value); }
        private double _CommandPosY;
        public double CommandPosY { get => _CommandPosY; set => SetProperty(ref _CommandPosY, value); }

        private double _CommandPosLimitMinX;
        public double CommandPosLimitMinX { get => _CommandPosLimitMinX; set => SetProperty(ref _CommandPosLimitMinX, value); }
        private double _CommandPosLimitMaxX;
        public double CommandPosLimitMaxX { get => _CommandPosLimitMaxX; set => SetProperty(ref _CommandPosLimitMaxX, value); }

        private double _CommandPosLimitMinY;
        public double CommandPosLimitMinY { get => _CommandPosLimitMinY; set => SetProperty(ref _CommandPosLimitMinY, value); }
        private double _CommandPosLimitMaxY;
        public double CommandPosLimitMaxY { get => _CommandPosLimitMaxY; set => SetProperty(ref _CommandPosLimitMaxY, value); }

        private double _VelocityRate;
        private eMotionMode _MotionMode;

        // publish 모드: UI(레시피)는 직접 디바이스를 제어하지 않고, 입력값을 이벤트로 발행한다.
        //   execute 모드(기본, 매뉴얼): 이벤트를 구독해 실제 스캐너를 움직인다.
        private readonly bool _PublishMode;

        private readonly DispatcherTimer _MonitorTimer;

        // PAM, RUW 처럼 2개 이상의 scanner 를 사용하는 경우가 있어, scannerId 를 받는 형태로 생성
        public UCScannerMotionViewModel(IAuroraEnvironment aurora, string scannerId, bool publishMode = false)
        {
            _Aurora = aurora;
            _PublishMode = publishMode;

            _Scanner = _Aurora.DeviceManager[scannerId] as Scanner;
            _Laser = _Aurora.DeviceManager[CommonDeviceIds.Laser] as Laser;
            _ScannerConfig = _Aurora.DeviceManager.GetDeviceConfig(scannerId) as ScannerConfig;

            InitVariable();

            CMD_VisibleChanged = new DelegateCommand(VisibleChanged);
            CMD_SpeedChanged = new DelegateCommand(PerformCmdSpeedChanged);
            CMD_MotionModeChanged = new DelegateCommand(PerformCmdMotionModeChanged);
            CMD_JogMove = new DelegateCommand(PerformCmdJogMove);
            CMD_CommandPosMove = new DelegateCommand(PerformCmdCommandPosMove);

            // execute 모드만 원격 모션 요청을 구독한다(중복 실행 방지). UI 스레드에서 처리.
            if (!_PublishMode) {
                _Aurora.EventObservable.AsObservable(CommonEventIds.EVENT_SCANNER_COMMAND_MOVE)
                    .ObserveOnDispatcher()
                    .Subscribe(OnRemoteMotionRequested);
            }

            _MonitorTimer = new DispatcherTimer();
            _MonitorTimer.Tick += OnMonitoring;
            _MonitorTimer.Interval = TimeSpan.FromMilliseconds(100);
        }

        private void PublishMotion(ScannerCommandMoveParam param)
        {
            _Aurora.EventPublisher.Publish(
                new EventMessage("UCScannerMotionViewModel.Publish", CommonEventIds.EVENT_SCANNER_COMMAND_MOVE, param));
        }

        private void OnRemoteMotionRequested(EventMessage msg)
        {
            if (msg == null || !(msg.Param is ScannerCommandMoveParam p)) {
                return;
            }

            switch (p.Kind) {
                case eScannerMoveKind.CommandPos:
                    MoveCommandXYPos(p.Axis, p.CommandPosX, p.CommandPosY, p.Mode, p.VelocityRate);
                    break;

                case eScannerMoveKind.Jog:
                    if (IsMainSequenceRunning()) return;
                    MoveJogXYPos(p.Axis, p.Direction, p.StepValue, p.VelocityRate);
                    break;

                case eScannerMoveKind.Home:
                    if (IsMainSequenceRunning()) return;
                    MoveJogHome(p.VelocityRate);
                    break;
            }
        }

        private bool IsMainSequenceRunning()
        {
            return _Aurora.SequenceEntry[CommonSequenceIds.SeqAutoRun].RunState == SequenceRunState.Running;
        }

        private void InitVariable()
        {
            _VelocityRate = 1;
            _MotionMode = eMotionMode.ABS;
        }

        private void VisibleChanged(object obj)
        {
            bool visible = (bool)obj;
            if (visible) {
                Init_XYLimitPos();
                _MonitorTimer.Start();
            } else {
                _MonitorTimer.Stop();
            }
        }

        private void Init_XYLimitPos()
        {
            // TODO: Limit 값을 Min, Max 로 따로 Setting 입력값으로 가져와야 할 수도 있음. 우선 현상태 유지
            CommandPosLimitMinX = -1 * _ScannerConfig.ScanXSize / 2;
            CommandPosLimitMaxX = _ScannerConfig.ScanXSize / 2;

            CommandPosLimitMinY = -1 * _ScannerConfig.ScanYSize / 2;
            CommandPosLimitMaxY = _ScannerConfig.ScanYSize / 2;

            //CommandPosXLimit = string.Format($"{-1 * _ScannerConfig.ScanXSize / 2} ~ {_ScannerConfig.ScanXSize / 2}");
            //CommandPosYLimit = string.Format($"{-1 * _ScannerConfig.ScanYSize / 2} ~ {_ScannerConfig.ScanYSize / 2}");
        }

        private void OnMonitoring(object sender, EventArgs e)
        {
            double xPos, yPos;
            if (_Scanner != null && _Scanner.IsActivated) {
                _Scanner.GetXYPos(out xPos, out yPos);
                ScannerXPos = xPos;
                ScannerYPos = yPos;

                IsScannerBusy = _Scanner.GetBusyStatus();
            } else {
                ScannerXPos = 0;
                ScannerYPos = 0;
                IsScannerBusy = false;
            }

        }

        private void PerformCmdSpeedChanged(object commandParameter)
        {
            switch ((string)commandParameter) {
                case "Low": {
                        _VelocityRate = 0.5;
                    }
                    break;

                case "Mid": {
                        _VelocityRate = 1;
                    }
                    break;

                case "High": {
                        _VelocityRate = 1.5;
                    }
                    break;
            }
        }

        private void PerformCmdMotionModeChanged(object commandParameter)
        {
            switch ((string)commandParameter) {
                case "ABS": {
                        _MotionMode = eMotionMode.ABS;
                    }
                    break;

                case "REL": {
                        _MotionMode = eMotionMode.REL;
                    }
                    break;
            }
        }

        private void PerformCmdJogMove(object commandParameter)
        {
            // TODO: Left, Right, Up, Down 은 장비 Scanner 의 방향에 따라 달라질 수 있다
            //  양산중에는 동작 하면 안됨
            if (IsMainSequenceRunning())
                return;

            eAxisType axis;
            eJogDirection direction;
            switch ((string)commandParameter) {
                case "Left": axis = eAxisType.AXIS_X; direction = eJogDirection.MINUS; break;
                case "Right": axis = eAxisType.AXIS_X; direction = eJogDirection.PLUS; break;
                case "Up": axis = eAxisType.AXIS_Y; direction = eJogDirection.PLUS; break;
                case "Down": axis = eAxisType.AXIS_Y; direction = eJogDirection.MINUS; break;
                case "Home":
                    if (_PublishMode) {
                        PublishMotion(new ScannerCommandMoveParam {
                            Kind = eScannerMoveKind.Home,
                            VelocityRate = _VelocityRate,
                        });
                    } else {
                        MoveJogHome(_VelocityRate);
                    }
                    return;

                default:
                    return;
            }

            if (_PublishMode) {
                PublishMotion(new ScannerCommandMoveParam {
                    Kind = eScannerMoveKind.Jog,
                    Axis = axis,
                    Direction = direction,
                    StepValue = StepValue,
                    VelocityRate = _VelocityRate,
                });
                return;
            }

            MoveJogXYPos(axis, direction, StepValue, _VelocityRate);
        }

        private void MoveJogXYPos(eAxisType axisType, eJogDirection direction, double stepValue, double velocityRate)
        {
            double xPos = 0.0;
            double yPos = 0.0;
            switch (axisType) {
                case eAxisType.AXIS_X: {
                        if (direction == eJogDirection.MINUS) {
                            xPos = stepValue * -1;
                            yPos = 0;
                        } else if (direction == eJogDirection.PLUS) {
                            xPos = stepValue;
                            yPos = 0;
                        }
                    }
                    break;

                case eAxisType.AXIS_Y: {
                        if (direction == eJogDirection.MINUS) {
                            xPos = 0;
                            yPos = stepValue * -1;
                        } else if (direction == eJogDirection.PLUS) {
                            xPos = 0;
                            yPos = stepValue;
                        }
                    }
                    break;

                default:
                    break;
            }

            // 현재 Curruent Pos 기준으로, 입력방향으로 입력값이 Limit 를 넘어 서는지 확인 (Rel 임)
            if (CheckOverLimitPos(ScannerXPos + xPos, ScannerYPos + yPos)) {
                Log.E(string.Format("움직이려는 범위가 스캐너 범위를 초과하였습니다."));
                return;
            }

            double finalVelocity = _ScannerConfig.JumpSpeed * velocityRate;
            _Scanner.SetJumpSpeedCtrl(finalVelocity);
            _Scanner.JumpRel(xPos, yPos);

            //Jump 후 Speed 원복
            _Scanner.SetJumpSpeedCtrl(_ScannerConfig.JumpSpeed);
        }

        private void MoveJogHome(double velocityRate)
        {
            double finalVelocity = _ScannerConfig.JumpSpeed * velocityRate;
            _Scanner.SetJumpSpeedCtrl(finalVelocity);
            _Scanner.JumpAbs(0, 0);

            //Jump 후 Speed 원복
            _Scanner.SetJumpSpeedCtrl(_ScannerConfig.JumpSpeed);
        }

        private void PerformCmdCommandPosMove(object commandParameter)
        {
            eAxisType axis;
            switch ((string)commandParameter) {
                case "X": axis = eAxisType.AXIS_X; break;
                case "Y": axis = eAxisType.AXIS_Y; break;
                default: return;
            }

            if (_PublishMode) {
                PublishMotion(new ScannerCommandMoveParam {
                    Kind = eScannerMoveKind.CommandPos,
                    Axis = axis,
                    CommandPosX = CommandPosX,
                    CommandPosY = CommandPosY,
                    Mode = _MotionMode,
                    VelocityRate = _VelocityRate,
                });
                return;
            }

            MoveCommandXYPos(axis, CommandPosX, CommandPosY, _MotionMode, _VelocityRate);
        }

        private void MoveCommandXYPos(eAxisType axisType, double commandPosX, double commandPosY, eMotionMode motionMode, double velocityRate)
        {
            double xPos = 0.0;
            double yPos = 0.0;
            switch (axisType) {
                case eAxisType.AXIS_X: {
                        xPos = commandPosX;

                        if (motionMode == eMotionMode.ABS) {
                            yPos = _Scanner.GetCommandPosY();
                        } else {
                            yPos = 0.0;
                        }
                    }
                    break;

                case eAxisType.AXIS_Y: {
                        if (motionMode == eMotionMode.ABS) {
                            xPos = _Scanner.GetCommandPosX();
                        } else {
                            xPos = 0.0;
                        }

                        yPos = commandPosY;
                    }
                    break;

                default:
                    break;
            }

            // 현재 Curruent Pos 기준으로, 입력방향으로 입력값이 Limit 를 넘어 서는지 확인
            if (motionMode == eMotionMode.ABS) {
                if (CheckOverLimitPos(xPos, yPos)) {
                    Log.E(string.Format("움직이려는 범위가 스캐너 범위를 초과하였습니다."));
                    return;
                }
            } else {
                if (CheckOverLimitPos(ScannerXPos + xPos, ScannerYPos + yPos)) {
                    Log.E(string.Format("움직이려는 범위가 스캐너 범위를 초과하였습니다."));
                    return;
                }
            }

            double finalVelocity = _ScannerConfig.JumpSpeed * velocityRate;
            _Scanner.SetJumpSpeedCtrl(finalVelocity);

            if (motionMode == eMotionMode.ABS) {
                _Scanner.JumpAbs(xPos, yPos);
            } else {
                _Scanner.JumpRel(xPos, yPos);
            }

            //Jump 후 Speed 원복
            _Scanner.SetJumpSpeedCtrl(_ScannerConfig.JumpSpeed);
        }

        private bool CheckOverLimitPos(double calcPosX, double calcPosY)
        {
            if (calcPosX < CommandPosLimitMinX || calcPosX > CommandPosLimitMaxX) {
                return true;
            } else if (calcPosY < CommandPosLimitMinY || calcPosY > CommandPosLimitMaxY) {
                return true;
            }

            return false;
        }
    }
}