OHV/Dev/OHV/VehicleControlSystem/ControlLayer/Clamp.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;
using FluentResults;
using GSG.NET.Concurrent;
using GSG.NET.Logging;
using GSG.NET.Utils;
using OHV.Common.Events;
using OHV.Common.Model;
using OHV.Common.Shareds;
using OHV.SqliteDAL;
using Prism.Events;
using VehicleControlSystem.ControlLayer.Axis;
using VehicleControlSystem.ControlLayer.Lib.EziPlusE;
using static OHV.Common.Events.AxisControlEventArgs;

namespace VehicleControlSystem.ControlLayer
{
    public class Clamp : ControlObjectBase
    {
        SqliteManager sql = null;
        List<EzAxis> axes = new List<EzAxis>();
        static Logger logger = Logger.GetLogger();

        IEventAggregator eventAggregator;
        TaskCancel taskCancel = new TaskCancel();

        eClampState _clampState;
        public eClampState ClampState
        {
            get { return this._clampState; }
            set { this.SetField( ref this._clampState, value ); }
        }

        public Clamp( SqliteManager sqliteManager, IEventAggregator ea )
        {
            this.sql = sqliteManager;
            this.eventAggregator = ea;
        }

        public void Init()
        {
            this.CreateAxis();

            this.eventAggregator.GetEvent<AxisControlPubSubEvent>().Unsubscribe( ReceivedMessageEvent );
            this.eventAggregator.GetEvent<AxisControlPubSubEvent>().Subscribe( ReceivedMessageEvent );
        }

        void ReceivedMessageEvent( AxisControlEventArgs args )
        {
            if ( args.Dir == AxisControlEventArgs.eEventDir.ToBack )
            {
                Task.Factory.StartNew( () =>
                {
                    var arg = ObjectCopyUtils.DeepClone( args );
                    switch ( arg.Kind )
                    {
                        case AxisControlEventArgs.eAxisControlKind.Stop:
                            ReqStop();
                            break;
                        case AxisControlEventArgs.eAxisControlKind.EStop:
                            break;
                        case AxisControlEventArgs.eAxisControlKind.Move:
                            ReqOrderByMove( arg );
                            break;
                        case AxisControlEventArgs.eAxisControlKind.Jog:
                            ReqJog( arg );
                            break;
                        case AxisControlEventArgs.eAxisControlKind.ServoOn:
                            ReqServoOn( arg );
                            break;
                        case AxisControlEventArgs.eAxisControlKind.ServoOff:
                            ReqServoOff( arg );
                            break;
                        case AxisControlEventArgs.eAxisControlKind.ReqCurrentPosition:
                            ReqStartAxisState();
                            break;
                        case AxisControlEventArgs.eAxisControlKind.ReqStopCurrentPosition:
                            taskCancel.Cancel();
                            taskCancel.WaitAll();
                            break;
                        case AxisControlEventArgs.eAxisControlKind.OriginReturn:
                            ReqOriginReturn( arg );
                            break;
                        case AxisControlEventArgs.eAxisControlKind.FaultReset:
                            ReqFaultReset( arg );
                            break;
                        case eAxisControlKind.NONE:
                            break;
                        case eAxisControlKind.AxisState:
                            break;
                        case eAxisControlKind.SyncMove:
                            ReqSyncMove( arg );
                            break;
                        case eAxisControlKind.SyncState:
                            ReqSyncState( arg );
                            break;
                        case eAxisControlKind.AllOriginReturn:
                            ReqAllOriginReturn();
                            break;
                        default:
                            break;
                    }
                } );
            }
        }

        private void ReqStop()
        {
            logger.D( $"JogMove - Stop Command" );
            this.axes.ForEach( x => { x.Stop(); } );
        }

        void PublishEvent( AxisControlEventArgs args )
        {
            args.Dir = AxisControlEventArgs.eEventDir.ToFront;
            this.eventAggregator.GetEvent<AxisControlPubSubEvent>().Publish( args );
        }

        #region Request Method
        private void ReqSyncState( AxisControlEventArgs args )
        {
            var leftPositionDatas = sql.AxisPositionDataDAL.GetKFromName( ConstString.AXIS_CARRIER_LOCK_LEFT ).ToList();
            var leftLockData = leftPositionDatas.Where( x => x.Name.Equals( ConstString.TEACH_POSITION_LOCK ) ).Single();
            var leftUnLockData = leftPositionDatas.Where( x => x.Name.Equals( ConstString.TEACH_POSITION_UNLOCK ) ).Single();

            var left = this.axes.Where( x => x.Config.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_LEFT ) ).Single();

            var rightPositionDatas = sql.AxisPositionDataDAL.GetKFromName( ConstString.AXIS_CARRIER_LOCK_RIGHT ).ToList();
            var rightLockData = leftPositionDatas.Where( x => x.Name.Equals( ConstString.TEACH_POSITION_LOCK ) ).Single();
            var rightUnLockData = leftPositionDatas.Where( x => x.Name.Equals( ConstString.TEACH_POSITION_UNLOCK ) ).Single();

            var right = this.axes.Where( x => x.Config.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_RIGHT ) ).Single();

            if ( left.CurrentPosition == leftLockData.Value && right.CurrentPosition == rightLockData.Value )
                this.ClampState = eClampState.LOCK;
            else if ( left.CurrentPosition == leftUnLockData.Value && right.CurrentPosition == rightUnLockData.Value )
                this.ClampState = eClampState.UNLOCK;
            else
                this.ClampState = eClampState.NONE;

            //this.ClampState = eClampState.NONE;

            var msg = new AxisControlEventArgs
            {
                Kind = eAxisControlKind.SyncState
            };

            this.PublishEvent( msg );
        }

        private void ReqSyncMove( AxisControlEventArgs args )
        {
            var result = 0;
            var reply = new AxisControlEventArgs
            {
                Kind = eAxisControlKind.SyncMove
            };

            if ( args.PosDir == ePosDir.Lock )
            {
                result = this.Lock_Sync();
                if ( result == 0 )
                {
                    reply.Result = Results.Ok();
                    this.ClampState = eClampState.LOCK;
                }
                else
                {
                    var alarm = this.sql.AlarmDAL.GetK( result );
                    if ( alarm == null )
                        alarm = new Alarm() { Name = "UnKnown Alarm", Text = "Not Define" };

                    var error = new Error().WithMetadata( "Alarm", alarm );
                    reply.Result = Results.Fail( error );

                    this.ClampState = eClampState.NONE;
                }
            }
            else
            {
                result = this.Unlock_Sync();
                if ( result == 0 )
                {
                    reply.Result = Results.Ok();
                    this.ClampState = eClampState.UNLOCK;
                }
                else
                {
                    var alarm = this.sql.AlarmDAL.GetK( result );
                    if ( alarm == null )
                        alarm = new Alarm() { Name = "UnKnown Alarm", Text = "Not Define" };

                    var error = new Error().WithMetadata( "Alarm", alarm );
                    reply.Result = Results.Fail( error );

                    this.ClampState = eClampState.NONE;
                }
            }

            this.PublishEvent( reply );
        }

        private void ReqFaultReset( AxisControlEventArgs args )
        {
            var axis = this.axes.Where( x => x.Config.AxisName.Equals( args.AxisName ) ).FirstOrDefault();
            int resultNo = axis.ResetAmpFault();

            var msg = new AxisControlEventArgs
            {
                AxisName = args.AxisName,
                Dir = AxisControlEventArgs.eEventDir.ToFront,
                Kind = AxisControlEventArgs.eAxisControlKind.FaultReset,
            };
            msg.Result = Results.Ok( "Servo Fault Reset" );
            this.PublishEvent( msg );
        }

        private void ReqServoOn( AxisControlEventArgs args )
        {
            var axis = this.axes.Where( x => x.Config.AxisName.Equals( args.AxisName ) ).FirstOrDefault();
            axis.ServoOn();

            var msg = new AxisControlEventArgs
            {
                AxisName = args.AxisName,
                Dir = AxisControlEventArgs.eEventDir.ToFront,
                Kind = AxisControlEventArgs.eAxisControlKind.ServoOn,
            };
            msg.Result = Results.Ok( "Servo On" );
            this.PublishEvent( msg );
        }

        private void ReqServoOff( AxisControlEventArgs args )
        {
            var axis = this.axes.Where( x => x.Config.AxisName.Equals( args.AxisName ) ).FirstOrDefault();
            axis.ServoOff();

            var msg = new AxisControlEventArgs
            {
                AxisName = args.AxisName,
                Dir = AxisControlEventArgs.eEventDir.ToFront,
                Kind = AxisControlEventArgs.eAxisControlKind.ServoOff,
            };
            msg.Result = Results.Ok( "Servo Off" );
            this.PublishEvent( msg );
        }

        void ReqStartAxisState()
        {
            var task = Task.Factory.StartNew( () =>
             {
                 double[] beforePosition = new double[this.axes.Count];
                 beforePosition.Initialize();

                 while ( !this.taskCancel.Canceled )
                 {
                     LockUtils.Wait( 500 );

                     int index = 0;
                     //this.axes.ForEach(a =>
                     //{
                     //    if (beforePosition[index] != a.CurrentPosition)
                     //    {
                     //        beforePosition[index] = a.CurrentPosition;

                     //        var msg = new AxisControlEventArgs
                     //        {
                     //            AxisName = a.Config.AxisName,
                     //            Dir = AxisControlEventArgs.eEventDir.ToFront,
                     //            Kind = AxisControlEventArgs.eAxisControlKind.CurrentPosition,
                     //            CurrentPosition = a.CurrentPosition,
                     //        };
                     //        this.PublishEvent(msg);
                     //    }
                     //    index++;
                     //});
                     var msg = new AxisControlEventArgs();
                     msg.Kind = AxisControlEventArgs.eAxisControlKind.AxisState;
                     msg.Args = new Dictionary<string, object>();
                     this.axes.ForEach( a =>
                     {
                         var state = new AxisState();
                         state.IsServoOn = a.IsServoOn();
                         state.IsOriginReturn = a.IsOriginReturn();
                         state.IsAmpFault = a.IsAmpFault();
                         state.CurrentPosition = a.CurrentPosition;

                         if ( a.Config.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_LEFT ) )
                             msg.Args.Add( "Left", state );
                         else
                             msg.Args.Add( "Right", state );
                     } );
                     this.PublishEvent( msg );
                 }
             } );

            this.taskCancel.Add( task );
        }

        void ReqJog( AxisControlEventArgs arg )
        {
            var velocity = arg.JogVelocity;
            if ( arg.JogDir == AxisControlEventArgs.eJogMoveDir.Positive )
                this.JogMove( arg.AxisName, E_JogMoveDir.Positive, velocity );
            else
                this.JogMove( arg.AxisName, E_JogMoveDir.Negative, velocity );
        }

        void ReqOriginReturn( AxisControlEventArgs arg )
        {
            int result = 0;
            if ( arg.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_LEFT ) )
                result = this.LeftOriginReturn();
            else
                result = this.RightOriginReturn();

            var msg = new AxisControlEventArgs
            {
                AxisName = arg.AxisName,
                Kind = AxisControlEventArgs.eAxisControlKind.OriginReturn,
            };
            if ( result != EziMOTIONPlusELib.FMM_OK )
                msg.Result = Results.Fail( "Origin Fail" );
            else
                msg.Result = Results.Ok();

            this.PublishEvent( msg );
        }

        void ReqAllOriginReturn()
        {
            var msg = new AxisControlEventArgs
            {
                Kind = eAxisControlKind.AllOriginReturn
            };

            var ll = this.AllOriginReturn();

            LockUtils.Wait( 3000 );

            var result = this.IsAllAxisOriginReturn();
            if ( result == 5 )
                msg.Result = Results.Fail( "Left Fail" );
            else if ( result == 6 )
                msg.Result = Results.Fail( "Right Fail" );
            else if ( result <= 0 )
                msg.Result = Results.Ok();

            this.PublishEvent( msg );
        }

        void ReqOrderByMove( AxisControlEventArgs arg )
        {
            var msg = new AxisControlEventArgs();
            msg.Kind = AxisControlEventArgs.eAxisControlKind.Move;
            msg.PosDir = arg.PosDir;
            int result = 0;

            if ( arg.PosDir == AxisControlEventArgs.ePosDir.Lock )
            {
                result = this.MoveToLockPosition( arg.AxisName );
                if ( result != 0 )
                {
                    var alarm = this.sql.AlarmDAL.GetK( result );
                    if ( alarm == null )
                        alarm = new Alarm() { Name = "Unknown Alarm", Text = "Not define" };

                    var error = new Error().WithMetadata( "Alarm", alarm );
                    msg.Result = Results.Fail( error );
                }
                else
                {
                    msg.Result = Results.Ok();
                }
            }
            else
            {
                result = this.MoveToUnlockPosition( arg.AxisName );
                if ( result != 0 )
                {
                    var alarm = this.sql.AlarmDAL.GetK( result );
                    if ( alarm == null )
                        alarm = new Alarm() { Name = "Unknown Alarm", Text = "Not define" };

                    var error = new Error().WithMetadata( "Alarm", alarm );
                    msg.Result = Results.Fail( error );
                }
                else
                {
                    msg.Result = Results.Ok();
                }
            }

            this.PublishEvent( msg );
        }

        #endregion

        public void ClampEStop()
        {
            this.axes.ForEach( x => { x.EmergencyStop(); } );
        }

        void CreateAxis()
        {
            sql.AxisConfigDAL.All.ForEach( config =>
             {
                 Axis.AxisConfig c = new Axis.AxisConfig
                 {
                     AxisName = config.Name,
                     Scale = config.Scale,
                     Address = config.Address,
                     AxisNo = config.Id,
                 };

                 //if ( config.Name.Equals( ConstString.AXIS_CARRIER_LOCK_LEFT ) )
                 //    c.AxisNo = 40;
                 //else
                 //    c.AxisNo = 41;

                 axes.Add( new EzAxis( c ) );
             } );

            this.axes.ForEach( axis =>
             {
                 var data = sql.AxisVelocityDataDAL.GetK( axis.Config.AxisName );
                 Axis.AxisData axisData = new AxisData
                 {
                     AxisNo = data.id,
                     AxisName = data.AxisName,
                     AutoRunVelocity = data.AutoRunVelocity,
                     AutoRunAccelerate = data.AutoRunAccelerate,
                     AutoRunDecelerate = data.AutoRunDecelerate,
                     ManualRunVelocity = data.ManualRunVelocity,
                     JogFast = data.JogFast,
                     JogSlow = data.JogSlow,
                     Tolerance = data.Tolerance,
                 };

                 axis.Initialize( axisData );
             } );
        }

        public int LeftOriginReturn()
        {
            var servo = this.axes.Where( s => s.Config.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_LEFT ) ).Single();
            return servo.OriginReturn( true );
        }

        public int RightOriginReturn()
        {
            var servo = this.axes.Where( s => s.Config.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_RIGHT ) ).Single();
            return servo.OriginReturn( true );
        }

        public int AllOriginReturn()
        {
            int result = 0;

            this.axes.ForEach( axis =>
            {
                if ( axis.IsAmpFault() )
                    axis.ResetAmpFault();

                Thread.Sleep( 500 );

                if ( axis.IsServoOff() )
                    axis.ServoOn();
            } );
            Thread.Sleep( 500 );

            result = LeftOriginReturn();
            if ( result != EziMOTIONPlusELib.FMM_OK )
                return 7;

            Thread.Sleep( 1000 );

            result = RightOriginReturn();
            if ( result != EziMOTIONPlusELib.FMM_OK )
                return 8;

            return result;
        }

        public int IsAllAxisOriginReturn()
        {
            int result = 0;
            this.axes.ForEach( x =>
             {
                 if ( !x.IsOriginReturn() )
                 {
                     if ( x.Config.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_LEFT ) )
                         result = 5;
                     else
                         result = 6;
                     return;
                 }
             } );

            return result;
        }

        #region Data
        public void SavePositionData( string pos, double left, double right )
        {
            var dataList = sql.AxisPositionDataDAL.GetKFromPostion( pos );
            var sqlLeft = dataList.Where( x => x.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_LEFT ) ).Single();
            var sqlRight = dataList.Where( x => x.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_RIGHT ) ).Single();

            sqlLeft.Value = left;
            sql.AxisPositionDataDAL.Update( sqlLeft );

            sqlRight.Value = right;
            sql.AxisPositionDataDAL.Update( sqlRight );
        }
        #endregion

        public int JogMove( string axisName, E_JogMoveDir dir, int velocity )
        {
            logger.D( $"JogMove - Axis[{axisName}] / Dir[{dir}] / vel[{velocity}]" );

            var axis = this.axes.Where( x => x.Config.AxisName.Equals( axisName ) ).Single();
            return axis.JogMove( dir, velocity );
        }

        public int MoveToLockPosition( string axisName )
        {
            var servo = this.axes.Where( x => x.Config.AxisName.Equals( axisName ) ).Single();
            var axisPositionDatas = sql.AxisPositionDataDAL.GetKFromName( axisName ).ToList();
            var data = axisPositionDatas.Where( x => x.Name.Equals( ConstString.TEACH_POSITION_LOCK ) ).Single();

            int result = servo.Move( data.Value );

            return result;
        }

        public int MoveToUnlockPosition( string axisName )
        {
            var servo = this.axes.Where( x => x.Config.AxisName.Equals( axisName ) ).Single();
            var axisPositionDatas = sql.AxisPositionDataDAL.GetKFromName( axisName ).ToList();
            var data = axisPositionDatas.Where( x => x.Name.Equals( ConstString.TEACH_POSITION_UNLOCK ) ).Single();

            int result = servo.Move( data.Value );

            return result;
        }

        /// <summary>
        /// 두개를 동시에 Lock
        /// </summary>
        /// <returns></returns>
        public int Lock_Sync()
        {
#if SIMULATION
            Thread.Sleep(2000);
#else
            int result = 0;

            result = this.IsAllAxisOriginReturn();
            if ( result != 0 )
                return result;

            var leftPositionDatas = sql.AxisPositionDataDAL.GetKFromName( ConstString.AXIS_CARRIER_LOCK_LEFT ).ToList();
            var leftData = leftPositionDatas.Where( x => x.Name.Equals( ConstString.TEACH_POSITION_LOCK ) ).Single();
            var left = this.axes.Where( x => x.Config.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_LEFT ) ).Single();

            var rightPositionDatas = sql.AxisPositionDataDAL.GetKFromName( ConstString.AXIS_CARRIER_LOCK_RIGHT ).ToList();
            var rightData = leftPositionDatas.Where( x => x.Name.Equals( ConstString.TEACH_POSITION_LOCK ) ).Single();
            var right = this.axes.Where( x => x.Config.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_RIGHT ) ).Single();

            result = left.StartMove( leftData.Value, 0 );
            if ( result != EziMOTIONPlusELib.FMM_OK )
                return 1;

            result = right.StartMove( rightData.Value, 0 );
            if ( result != EziMOTIONPlusELib.FMM_OK )
                return 3;

            result = left.Wait4Done();
            if ( result != EziMOTIONPlusELib.FMM_OK )
                return 1;

            result = right.Wait4Done();
            if ( result != EziMOTIONPlusELib.FMM_OK )
                return 1;
#endif

            this.ClampState = eClampState.LOCK;
            return EziMOTIONPlusELib.FMM_OK;
        }

        /// <summary>
        /// 두개의 Axis 를 동시에 Unlock
        /// </summary>
        /// <returns></returns>
        public int Unlock_Sync()
        {
#if SIMULATION
            Thread.Sleep(2000);
#else
            int result = 0;

            result = this.IsAllAxisOriginReturn();
            if ( result != 0 )
                return result;

            var leftPositionDatas = sql.AxisPositionDataDAL.GetKFromName( ConstString.AXIS_CARRIER_LOCK_LEFT ).ToList();
            var leftData = leftPositionDatas.Where( x => x.Name.Equals( ConstString.TEACH_POSITION_UNLOCK ) ).Single();
            var left = this.axes.Where( x => x.Config.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_LEFT ) ).Single();

            var rightPositionDatas = sql.AxisPositionDataDAL.GetKFromName( ConstString.AXIS_CARRIER_LOCK_RIGHT ).ToList();
            var rightData = leftPositionDatas.Where( x => x.Name.Equals( ConstString.TEACH_POSITION_UNLOCK ) ).Single();
            var right = this.axes.Where( x => x.Config.AxisName.Equals( ConstString.AXIS_CARRIER_LOCK_RIGHT ) ).Single();

            result = left.StartMove( leftData.Value, 0 );
            if ( result != EziMOTIONPlusELib.FMM_OK )
                return 2;

            result = right.StartMove( rightData.Value, 0 );
            if ( result != EziMOTIONPlusELib.FMM_OK )
                return 4;

            result = left.Wait4Done();
            if ( result != EziMOTIONPlusELib.FMM_OK )
                return 2;

            result = right.Wait4Done();
            if ( result != EziMOTIONPlusELib.FMM_OK )
                return 4;
#endif

            this.ClampState = eClampState.UNLOCK;
            return EziMOTIONPlusELib.FMM_OK;
        }
    }
}