OHV/Dev/OHV/VehicleControlSystem/ControlLayer/Vehicle.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Security.Cryptography.X509Certificates;
using System.Threading;
using System.Threading.Tasks;
using FluentResults;
using GSG.NET.Concurrent;
using GSG.NET.Extensions;
using GSG.NET.LINQ;
using GSG.NET.Logging;
using GSG.NET.Quartz;
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.Actuator.Cylinder;
using VehicleControlSystem.ControlLayer.Drive;
using VehicleControlSystem.ControlLayer.IO;
using VehicleControlSystem.ControlLayer.MQ;
using VehicleControlSystem.ControlLayer.Serial.BatteryTabos;
using VehicleControlSystem.ControlLayer.Serial.DataModel;
using VehicleControlSystem.Managers;
using static VehicleControlSystem.ControlLayer.Drive.GSIDrive;

namespace VehicleControlSystem.ControlLayer
{
    public class VehicleRefObjects
    {
        public GSIDrive Drive { get; set; }
        public Clamp Clamp { get; set; }
        public Conveyor Conveyor { get; set; }
        public EzIO IO { get; set; }
        public Steering Steering { get; set; }
        public BMUManager BMUManager { get; set; }
        public ZmqManager ZmqManager { get; set; }
    }

    /// <summary>
    /// Control Layer 의 자원을 여기서 사용하자.
    /// </summary>
    public class Vehicle : ControlObjectBase, IDisposable
    {
        /// <summary>
        /// OCS Report Code
        /// 목적지에 도착해서 Load, Unload 시 발생하는 Alarm
        /// </summary>
        public enum eFailCode
        {
            Load_PortHasNotCarrier = 1,
            Load_VehicleHasCarrier,
            Unload_PortHasCarrier,
            Unload_VehicleHasNotCarrier,
            LoadPIOInterlockTimeout,
            UnlaodPIOInterlockTimeout,
        }

        static Logger logger = Logger.GetLogger();
        static Logger loggerPIO = Logger.GetLogger( "PIO" );

        #region Properties
        /// <summary>
        /// Tag 위치
        /// </summary>
        private int currentTag = 0;
        public int CurrentTag
        {
            get { return currentTag; }
            set
            {
                if ( SetField( ref this.currentTag, value ) )
                {
                    var info = sql.VehicleInfoDAL.GetAll().FirstOrDefault();
                    info.CurrentTag = value.ToString();
                    sql.VehicleInfoDAL.Update( info );

                    this.OnCurrentTagChanged?.Invoke( value );
                }
            }
        }

        /// <summary>
        /// Scale Value
        /// </summary>
        private double currentPosition;
        public double CurrentPosition
        {
            get { return currentPosition; }
            set
            {
                if ( SetField( ref this.currentPosition, value ) )
                {
                }
            }
        }

        private double currentSpeed;
        public double CurrentSpeed
        {
            get { return currentSpeed; }
            set { SetField( ref this.currentSpeed, value ); }
        }

        private double currentTorque;
        public double CurrentTorque
        {
            get { return currentTorque; }
            set { SetField( ref this.currentTorque, value ); }
        }

        private bool isContain;
        public bool IsContain
        {
            get { return isContain; }
            set { SetField( ref this.isContain, value ); }
        }

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

        private eSteeringState steeringState;
        public eSteeringState SteeringState
        {
            get { return steeringState; }
            set
            {
                if ( SetField( ref this.steeringState, value ) )
                {
                    this.refObjects.ZmqManager.SetCurrentSteeringState( value );
                }
            }
        }

        private int _obstacleDrive;
        public int ObstacleDrive { get { return this._obstacleDrive; } set { SetField( ref this._obstacleDrive, value ); } }
        private int _obstacleCurve;
        public int ObstacleCurve { get { return this._obstacleCurve; } set { SetField( ref this._obstacleCurve, value ); } }
        private int obstacleCurrent;
        public int ObstacleCurrent { get { return this.ObstacleCurrent; } set { SetField( ref this.obstacleCurrent, value ); } }

        private eObstacleState obstacleState = eObstacleState.Normal;
        public eObstacleState ObstacleStateProperty
        {
            get { return obstacleState; }
            set
            {
                if ( SetField( ref this.obstacleState, value ) )
                {
                    if ( value == eObstacleState.Blocked )
                        this.VehicleStateProperty = eVehicleState.Blocked;
                }
            }
        }

        private eVehicleState vehicleState = eVehicleState.None;
        public eVehicleState VehicleStateProperty
        {
            get { return vehicleState; }
            set
            {
                if ( SetField( ref this.vehicleState, value ) )
                {
                    var info = sql.VehicleInfoDAL.GetAll().FirstOrDefault();
                    info.VehicleState = value;
                    sql.VehicleInfoDAL.Update( info );
                }
            }
        }

        private eMachineMode machineMode = eMachineMode.LocalMode;
        public eMachineMode MachineMode
        {
            get { return machineMode; }
            set
            {
                if ( SetField( ref this.machineMode, value ) )
                {
                    var info = sql.VehicleInfoDAL.GetAll().FirstOrDefault();
                    info.MachineMode = value;
                    sql.VehicleInfoDAL.Update( info );
                }
            }
        }

        private int obstaclePattern;
        public int ObstaclePattern
        {
            get { return obstaclePattern; }
            set { SetField( ref this.obstaclePattern, value ); }
        }

        private DriveState frontDriveState;
        public DriveState FrontDriveState
        {
            get { return frontDriveState; }
            set { SetField( ref this.frontDriveState, value ); }
        }

        private DriveState rearDriveState;
        public DriveState RearDriveState
        {
            get { return rearDriveState; }
            set { SetField( ref this.rearDriveState, value ); }
        }

        //이동
        public bool Busy
        {
            get
            {
                return this.CurrentSubCommand == null ? false : true;
            }
            set { }
        }

        public bool IsMoving { get => this.refObjects.Drive.IsDriveMoving(); }

        #region Battery Property

        double batteryVoltage;
        public double BatteryVoltage
        {
            get { return this.batteryVoltage; }
            set
            {
                //var d = Math.Truncate( value * 10 ) / 10;
                //this.SetField( ref this.batteryVoltage, d );
                this.SetField( ref this.batteryVoltage, value );
            }
        }

        double batteryCurrent;
        public double BatteryCurrent
        {
            get { return this.batteryCurrent; }
            set { this.SetField( ref this.batteryCurrent, value ); }
        }

        double batteryState;
        public double BatteryState
        {
            get { return this.batteryState; }
            set { this.SetField( ref this.batteryState, value ); }
        }

        double batteryChargeTime;
        public double BatteryChargeTime
        {
            get { return this.batteryChargeTime; }
            set { this.SetField( ref this.batteryChargeTime, value ); }
        }

        double batteryDisChargeTime;
        public double BatteryDisChargeTime
        {
            get { return this.batteryDisChargeTime; }
            set { this.SetField( ref this.batteryDisChargeTime, value ); }
        }

        double batteryStateOfCharge;
        public double BatteryStateOfCharge
        {
            get { return this.batteryStateOfCharge; }
            set { this.SetField( ref this.batteryStateOfCharge, value ); }
        }

        double batteryStateOfHealth;
        public double BatteryStateOfHealth
        {
            get { return this.batteryStateOfHealth; }
            set { this.SetField( ref this.batteryStateOfHealth, value ); }
        }

        double batteryCapacity;
        public double BatteryCapacity
        {
            get { return this.batteryCapacity; }
            set { this.SetField( ref this.batteryCapacity, value ); }
        }

        double batteryEnergy;
        public double BatteryEnergy
        {
            get { return this.batteryEnergy; }
            set
            {
                var d = Math.Truncate( value * 10 ) / 10;
                this.SetField( ref this.batteryEnergy, d );
            }
        }

        double batteryTemperature;
        public double BatteryTemperature
        {
            get { return this.batteryTemperature; }
            set { this.SetField( ref this.batteryTemperature, value ); }
        }

        bool batteryIsConnect;
        public bool BatteryIsConnect
        {
            get
            {
                return batteryIsConnect;
            }
            set { this.SetField( ref this.batteryIsConnect, value ); }
        }
        #endregion

        public bool IsError { get; set; }

        public SubCmd CurrentSubCommand { get; private set; }

        double frontLoadFactor = 0;
        public double FrontLoadFactor
        {
            get { return this.frontLoadFactor; }
            set { SetField( ref this.frontLoadFactor, value ); }
        }

        double frontRpm = 0;
        public double FrontRpm
        {
            get { return this.frontRpm; }
            set { SetField( ref this.frontRpm, value ); }
        }

        double frontSpeed = 0;
        public double FrontSpeed
        {
            get { return this.frontSpeed; }
            set { SetField( ref this.frontSpeed, value ); }
        }

        double frontTorque = 0;
        public double FrontTorque
        {
            get { return this.frontTorque; }
            set { SetField( ref this.frontTorque, value ); }
        }

        double rearLoadFactor = 0;
        public double RearLoadFactor
        {
            get { return this.rearLoadFactor; }
            set { SetField( ref this.rearLoadFactor, value ); }
        }

        double rearRpm = 0;
        public double RearRpm
        {
            get { return this.rearRpm; }
            set { SetField( ref this.rearRpm, value ); }
        }

        double rearSpeed = 0;
        public double RearSpeed
        {
            get { return this.rearSpeed; }
            set { SetField( ref this.rearSpeed, value ); }
        }

        double rearTorque = 0;
        public double RearTorque
        {
            get { return this.rearTorque; }
            set { SetField( ref this.rearTorque, value ); }
        }
        #endregion

        #region Event 
        public event Action OnMoveReady;
        public event Action OnMoving;
        public event Action OnMoveFinish;

        public event Action OnChargingStart;
        public event Action OnCharging;
        public event Action OnChargingFull;
        public event Action<double> OnBatteryVelueChanged;

        public event Action<bool> OnPIOStart;
        public event Action<bool> OnConveyorStart;
        public event Action<bool> OnConveyorStop;
        public event Action<bool> OnCarrierDetected;
        public event Action OnLoadComplete;
        public event Action OnUnloadComplete;

        public event Action<int> OnCurrentTagChanged;

        public event Action OnManualMove;
        public event Action OnManualLoad;
        public event Action OnManualUnload;
        public event Action OnManualCharging;

        public event Action<eFailCode> OnFailReport;
        #endregion

        #region List.
        List<ICylinder> cylinders = new List<ICylinder>();

        List<string> obstacleBitList = new List<string>();
        #endregion

        VehicleRefObjects refObjects = null;

        SqliteManager sql = null;
        AutoManager autoManager = null;

        ThreadCancel cancel = new ThreadCancel();
        TaskCancel taskCancel = new TaskCancel();
        TaskCancel taskMoveCancel = new TaskCancel();

        IEventAggregator eventAggregator;

        public Vehicle( VehicleRefObjects vehicleRef, IIO io, SqliteManager sqliteManager, IEventAggregator ea, AutoManager auto )
        {
            this.refObjects = vehicleRef;

            this.refObjects.IO.OnChangedIO += IO_OnChangedIO;

            this.refObjects.BMUManager.OnConnect += BMUManager_OnConnect;
            this.refObjects.BMUManager.OnDisconnect += BMUManager_OnDisconnect;
            this.refObjects.BMUManager.OnChangedReceivedData += BMUManager_OnChangedReceivedData;
            this.refObjects.BMUManager.OnFirstColtd += BMUManager_OnFirstColtd;

            this.refObjects.Drive.PropertyChanged += Motion_PropertyChanged;

            this.refObjects.Clamp.PropertyChanged += Clamp_PropertyChanged;

            this.refObjects.Steering.OnSteeringError += Steering_OnSteeringError;
            this.refObjects.Steering.PropertyChanged += Steering_PropertyChanged;

            this.refObjects.ZmqManager.PropertyChanged += ZmqManager_PropertyChanged;


            this.sql = sqliteManager;
            this.autoManager = auto;
            this.autoManager.OnOperationModeChanged += AutoManager_OnOperationModeChanged;

            this.obstacleBitList.AddRange( new string[]
            {
                "OUT_OBSTRUCTION_PATTERN_00",
                "OUT_OBSTRUCTION_PATTERN_01",
                "OUT_OBSTRUCTION_PATTERN_02",
                "OUT_OBSTRUCTION_PATTERN_03",
                "OUT_OBSTRUCTION_PATTERN_04",
            } );

            this.eventAggregator = ea;

            this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Unsubscribe( ReceiveDriveControlEvent );
            this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Subscribe( ReceiveDriveControlEvent );

            this.eventAggregator.GetEvent<ObstacleControlPubSubEvent>().Unsubscribe( ObstacleReceiveEvent );
            this.eventAggregator.GetEvent<ObstacleControlPubSubEvent>().Subscribe( ObstacleReceiveEvent );
        }


        private void ObstacleReceiveEvent( ObstacleControlEventArgs obj )
        {
            if ( this.autoManager.OperationModeProperty != eOperatationMode.ManualMode )
                return;

            if ( obj.EventDir == ObstacleControlEventArgs.eEventDir.ToBack )
            {
                switch ( obj.ControlKind )
                {
                    case ObstacleControlEventArgs.eControlKind.NONE:
                        break;
                    case ObstacleControlEventArgs.eControlKind.DRIVE:
                        //this.ChgObstacleDetectPattern( new Random().Next( 0 , 40 ) );
                        break;
                    case ObstacleControlEventArgs.eControlKind.CURVE:
                        //this.ChgObstacleDetectPattern( new Random().Next( 0 , 40 ) );
                        break;
                    case ObstacleControlEventArgs.eControlKind.STATE:
                        //var value = this.GetObstacleDetectPattern();
                        break;
                    case ObstacleControlEventArgs.eControlKind.INFO:
                        {
                            var msg = new ObstacleControlEventArgs
                            {
                                ControlKind = ObstacleControlEventArgs.eControlKind.INFO,
                                Drive = this.ObstacleDrive,
                                Curve = this.ObstacleCurve,
                                Current = this.GetObstacleDetectPattern(),
                                ObstacleState = this.ObstacleStateProperty.ToString()
                            };
                            this.ObstacleControlEventPublish( msg );
                        }
                        break;
                    case ObstacleControlEventArgs.eControlKind.SAVE:
                        {
                            this.ObstacleCurve = obj.Curve;
                            this.ObstacleDrive = obj.Drive;

                            var reply = new ObstacleControlEventArgs
                            {
                                ControlKind = ObstacleControlEventArgs.eControlKind.SAVE
                            };
                            reply.Result = Results.Ok( ObstacleControlEventArgs.eControlKind.SAVE );
                            this.ObstacleControlEventPublish( reply );
                        }
                        break;
                }
            }
        }

        private void ObstacleControlEventPublish( ObstacleControlEventArgs args )
        {
            args.EventDir = ObstacleControlEventArgs.eEventDir.ToFront;
            this.eventAggregator.GetEvent<ObstacleControlPubSubEvent>().Publish( args );
        }

        private void ReceiveDriveControlEvent( DriveControlEventArgs _args )
        {
            var msg = _args;
            if ( msg.EventDir == DriveControlEventArgs.eEventDir.ToBack )
            {
                switch ( msg.ControlKind )
                {
                    case DriveControlEventArgs.eControlKind.MOVE:
                        this.ReqMoveToPos( _args );
                        break;

                    case DriveControlEventArgs.eControlKind.STOP:
                        break;

                    case DriveControlEventArgs.eControlKind.Steering:
                        if ( msg.MoveDir == DriveControlEventArgs.eMoveDir.LEFT )
                            this.refObjects.Steering.ControlSteering( true );
                        else
                            this.refObjects.Steering.ControlSteering();
                        break;

                    case DriveControlEventArgs.eControlKind.SteeringState:
                        {
                            var reply = new DriveControlEventArgs();
                            reply.ControlKind = DriveControlEventArgs.eControlKind.SteeringState;
                            if ( this.refObjects.Steering.IsLeft() )
                            {
                                reply.Args = eSteeringState.Left;
                                reply.Result = FluentResults.Results.Ok();
                            }
                            else if ( this.refObjects.Steering.IsRight() )
                            {
                                reply.Args = eSteeringState.Right;
                                reply.Result = FluentResults.Results.Ok();
                            }
                            else
                                reply.Result = FluentResults.Results.Fail( new Error() );

                            this.DriveControlEventPublish( reply );
                        }
                        break;

                    case DriveControlEventArgs.eControlKind.ReqCurrentPos:
                        //this.ReqCurrentPos();
                        break;

                    case DriveControlEventArgs.eControlKind.ReqStopCurrentPos:
                        //this.taskCancel.Cancel();
                        //this.taskCancel.WaitAll();
                        break;

                    case DriveControlEventArgs.eControlKind.FaultReset:
                        this.ReqFaultReset( _args );
                        break;

                    case DriveControlEventArgs.eControlKind.DriveON:
                        this.ReqDriveOn( _args );
                        break;

                    case DriveControlEventArgs.eControlKind.DriveOFF:
                        this.ReqDriveOff( _args );
                        break;

                    case DriveControlEventArgs.eControlKind.JOG:
                        this.ReqJog( _args );
                        break;

                    case DriveControlEventArgs.eControlKind.VehicleState:
                        ReqVehicleState( _args );
                        break;

                    default:
                        break;
                }
            }
        }

        public void ReqConveyorMove( string dir )
        {
            if ( dir.Equals( "CW" ) )
                this.refObjects.Conveyor.OnOffConveyor( true, false );
            else if ( dir.Equals( "CCW" ) )
                this.refObjects.Conveyor.OnOffConveyor( true, true );
            else if ( dir.Equals( "STOP" ) )
                this.refObjects.Conveyor.OnOffConveyor( false, false );
        }

        private void DriveControlEventPublish( DriveControlEventArgs args )
        {
            args.EventDir = DriveControlEventArgs.eEventDir.ToFront;
            this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Publish( args );
        }

        public void Init()
        {
            ThreadStart();

            //TimerUtils.Once(5000, () => { this.CurrentPosition = 1000; });
            var v = sql.VehicleInfoDAL.GetAll().FirstOrDefault();
            v.CurrentTag = "0000";
            v.VehicleState = eVehicleState.None;
            v.MachineMode = eMachineMode.LocalMode;

            sql.VehicleInfoDAL.Update( v );
        }

        public int InitializationVehicle()
        {
#if SIMULATION
            this.VehicleStateProperty = eVehicleState.Idle;
            return 0;
#else
            int result = 0;

            if ( this.refObjects.Conveyor.IsDetectedCenter() ) //자제가 있으면 Lock
                result = this.refObjects.Clamp.Lock_Sync();
            else
                result = this.refObjects.Clamp.Unlock_Sync();

            if ( result != 0 )
            {
                this.VehicleStateProperty = eVehicleState.Abnormal;
                return result;
            }

            if ( this.refObjects.Drive.IsErrorOn )
                return 22;

            this.VehicleStateProperty = eVehicleState.Idle;

            return result;
#endif
        }

        public void Dispose()
        {
            this.taskCancel.Cancel();

            this.cancel.Cancel();
            this.cancel.StopWaitAll();

            //this.bMUManager.Disconnect();

            //this.drive.Dispose();
        }

        #region Request Method
        private void ReqVehicleState( DriveControlEventArgs args )
        {
            VehicleInfo state = new VehicleInfo();
            state.CurrentPosition = this.CurrentPosition;
            state.CurrentSpeed = this.CurrentSpeed;
            state.CurrentTag = this.CurrentTag.ToString();
            state.CurrentTorque = this.CurrentTorque;

            var msg = new DriveControlEventArgs();
            msg.ControlKind = args.ControlKind;
            msg.Args = state;

            DriveControlEventPublish( msg );
        }

        private void ReqMoveToPos( DriveControlEventArgs args )
        {
            //var result = drive.Move( args.PositionTag );
            //this.MoveTo( "1111" );
            var reply = new DriveControlEventArgs();

            int targetTag = args.TargetRouteID;
            var route = sql.RouteDal.Get( x => x.Name.Equals( targetTag ) ).FirstOrDefault();
            if ( route == null )
            {
                reply.Result = Results.Fail( "Not Found Route" );
                this.DriveControlEventPublish( reply );
                return;
            }

            var subCommand = new SubCmd();
            subCommand.TargetID = route.Name;

            this.MoveTo( subCommand );
            reply.Result = Results.Ok( "Position Move" );
            this.DriveControlEventPublish( reply );
        }

        void ReqFaultReset( DriveControlEventArgs _args )
        {
            var drive = 0;

            //var result = drive.ResetAmpFault();

            var msg = new DriveControlEventArgs
            {
                ControlKind = DriveControlEventArgs.eControlKind.FaultReset
            };

            msg.Result = Results.Ok( "Drive On" );
            this.DriveControlEventPublish( msg );
        }

        void ReqJog( DriveControlEventArgs _args )
        {
            if ( _args.JogDir == DriveControlEventArgs.eJogMoveDir.Positive )
            {
                //this.drive.JogForWard();
            }
            else
            {
                //this.drive.JogBackward();
            }
        }

        Logger batteryLogger = Logger.GetLogger( "BatteryLogger" );
        void ReqCurrentPos()
        {
            var task = Task.Factory.StartNew( () =>
             {
                 while ( !this.taskCancel.Canceled )
                 {
                     LockUtils.Wait( 1000 );

                     batteryLogger.I( $"SOC - {this.BatteryStateOfCharge} / Current - {this.BatteryCurrent} / Voltage - {this.BatteryVoltage} / Capacity - {this.BatteryCapacity} / Energy - {this.BatteryEnergy}" );

                     //var msg = new DriveControlEventArgs
                     //{
                     //    EventDir = DriveControlEventArgs.eEventDir.ToFront,
                     //    ControlKind = DriveControlEventArgs.eControlKind.ReqCurrentPos,
                     //    CurrentPosition = new Random().Next( 0, 1000 ),
                     //};

                     //this.DriveControlEventPublish( msg );
                 }
             } );

            this.taskCancel.Add( task );
        }

        void ReqDriveOn( DriveControlEventArgs _args )
        {
            var drive = "Drive Name";
            //drive.On();

            var msg = new DriveControlEventArgs
            {
                ControlKind = DriveControlEventArgs.eControlKind.DriveON
            };
            msg.Result = Results.Ok( "Drive On" );

            this.DriveControlEventPublish( msg );
        }

        void ReqDriveOff( DriveControlEventArgs _args )
        {
            var drive = "Drive Name";
            //drive.Off();

            var msg = new DriveControlEventArgs
            {
                ControlKind = DriveControlEventArgs.eControlKind.DriveOFF
            };
            msg.Result = Results.Ok( "Drive On" );

            this.DriveControlEventPublish( msg );
        }
        #endregion

        #region Thread
        void ThreadStart()
        {
            this.cancel.AddGo( new Action( this._ThSubCmdWorker ) );
            this.cancel.AddGo( new Action( this._ThVehicleStateCheck ) );
        }

        //장애물 감지 Thread
        //장애물 감지 패턴 변경도 여기 하자.
        private void _ThVehicleStateCheck()
        {
            while ( !this.cancel.Canceled )
            {
                try
                {
                    //if ( this.autoManager.OperationModeProperty == eOperatationMode.AutoMode )
                    //this.CheckObstacle();

                    //this.CheckIOState();
                }
                catch ( ThreadInterruptedException threadInterruptedException )
                {
                }
                catch ( Exception exception )
                {
                    logger.E( exception );
                }
                finally
                {
                    LockUtils.Wait( 5 );
                }
            }
            logger.D( "Vehicle - _ThObstacleChecker Dispose" );
        }


        /// <summary>
        /// Scheduler 가 주는 Sub Command 를 이용하여 동작하자.
        /// </summary>
        public void _ThSubCmdWorker()
        {
            while ( !this.cancel.Canceled )
            {
                try
                {
                    if ( this.ObstacleStateProperty != eObstacleState.Normal ) //장애물 감지 상태 시 조그 동작만 가능하게.
                        continue;

                    if ( this.autoManager.AutoModeStateProperty != eAutoModeState.Run ) // 
                        continue;

                    var subCmd = sql.SubCmdDAL.GetAll().FirstOrDefault();
                    if ( subCmd == null ) continue;

                    if ( !sql.CommandDAL.GetAll().Any( x => x.CommandID.Equals( subCmd.CmdID ) ) )
                    {
                        if ( subCmd.CmdType == SubCmd.eCmdType.Auto ) //자동 명령중 Main Command 가 없으면 삭제.
                        {
                            sql.SubCmdDAL.Delete( subCmd.ID );
                            logger.I( $"SubCmd Deleted - ID={subCmd.ID}, CommandID={subCmd.CmdID}" );
                        }
                    }

                    switch ( subCmd.Type )
                    {
                        case eSubCommandType.Move:
                            this.CurrentSubCommand = subCmd;
                            this.Move( subCmd );
                            break;

                        case eSubCommandType.Load:
                            this.CurrentSubCommand = subCmd;
                            this.LoadCarrier( subCmd );
                            break;

                        case eSubCommandType.Unload:
                            this.CurrentSubCommand = subCmd;
                            this.UnloadCarrier( subCmd );
                            break;

                        case eSubCommandType.Charge:
                            this.CurrentSubCommand = subCmd;
                            this.BatteryCharge( subCmd );
                            break;
                        default:
                            break;
                    }
                }
                catch ( Exception exception )
                {
                    logger.E( exception );
                }
                finally
                {
                    LockUtils.Wait( 500 );
                }
            }
            logger.D( "Vehicle - _ThSubCmdWorker Dispose" );
        }

        #endregion

        #region Control Action Method
        public void EStop()
        {
            this.refObjects.Conveyor.OnOffConveyor( false );
            //Clamp EStop
            this.refObjects.Clamp.ClampEStop();
            //this.drive.EStop();

            this.OccurVehicleAlarm( 23 );
        }

        #region For Moving
        void Move( SubCmd sub )
        {
            //Move 시작 시 충전 중이면 충전 중지 실행.
            int result = 0;
            if ( this.IsCharging() || this.VehicleStateProperty == eVehicleState.Charge )
            {
                result = this.StopBatteryCharge();
                if ( result != 0 )
                {
                    OccurVehicleAlarm( result );
                    return;
                }
            }

            var iTarget = Convert.ToInt32( sub.TargetID );
            if ( iTarget != CurrentTag ) //하나의 목표 위치로 왔을때 현재 위치와 동일 하면 이동안함.
            {
                result = this.MoveTo( sub );
                if ( result != ConstInt.EXECUTE_SUCCESS )
                {
                    OccurVehicleAlarm( result );
                }
            }

            this.taskMoveCancel.Cancel();
            this.taskMoveCancel.WaitAll();

            sql.SubCmdDAL.Clean();
        }

        int MoveTo( SubCmd sub )
        {
            if ( this.VehicleStateProperty == eVehicleState.Idle )
            {
                this.OnMoveReady?.Invoke();

                var moveReadyBuzzerTime = Convert.ToInt32( sql.ConfigDal.GetById( ConstString.BuzzerStartReadyTime ).Value );
                Thread.Sleep( moveReadyBuzzerTime );

                this.VehicleStateProperty = eVehicleState.Move;
            }

            this.OnMoving?.Invoke();

            //이전에 있던 작업들 종료 및 삭제
            this.taskMoveCancel.Cancel();
            this.taskMoveCancel.WaitAll();
            this.taskMoveCancel.Add( CheckCrossPoint() );

            this.VehicleStateProperty = eVehicleState.Move;

            int result = this.refObjects.Drive.MoveToPoint( sub, 100 );
            if ( result != ConstInt.EXECUTE_SUCCESS )
                return result;

            result = this.Wait4MoveDone();
            if ( result != ConstInt.EXECUTE_SUCCESS )
                return result;

            //Drive 에서 정지 확인 후 상태 변경
            if ( this.refObjects.Drive.IsDriveStop() )
            {
                this.OnMoveFinish?.Invoke();

                this.VehicleStateProperty = eVehicleState.Idle;
                logger.D( "Move Finish" );
            }

            return result;
        }

        int Wait4MoveDone()
        {
            int waitTime = ConstUtils.ONE_HOUR; //설정 할 수있게.
            long st = SwUtils.CurrentTimeMillis;

            //Todo: 이동시 확인 사항들.
            while ( true )
            {
                Thread.Sleep( 5 );

                if ( SwUtils.Gt( st, waitTime ) )
                {
                    //Todo: 이동시간 초과 시 동작들.
                    logger.D( "Wait4MoveDone Time Over" );
                    return 39;
                }

                //Todo: 이동중 명령이 삭제 되면 처리 할일들.
                //이동중 메인 명력이 없어진다면 정지 후 
                if ( null == sql.CommandDAL.GetById( this.CurrentSubCommand.CmdID ) )
                {
                    logger.D( "[Wait Move Done] - 메인 명령 사라짐" );
                    var cmd = sql.CommandDAL.GetAll();
                    if ( cmd == null )
                    {
                        logger.D( "[Wait Move Done] - Main Command not Exist Motion Stop" );
                        this.refObjects.Drive.Stop();
                        return 40;
                    }
                    else
                    {
                        logger.D( "[Wait Move Done] - Main Command not Exist Motion command, New Command Exist" );
                        break;
                    }
                }

                if ( this.refObjects.Drive.IsDriveStop() ) //Move Stop
                    break;
            }

            return ConstInt.EXECUTE_SUCCESS;
        }

        Task CheckCrossPoint()
        {
            var task = Task.Run( () =>
            {
                long sTime = SwUtils.CurrentTimeMillis;

                while ( !this.taskMoveCancel.Canceled )
                {
                    Thread.Sleep( 10 );

                    //ToDo: approach Cross Point Check

                    //ToDo: Obstacle Laser Sensor Pattern Change Method 구현 필요.
                }
            } );

            return task;
        }
        #endregion

        public bool LoadCarrier( SubCmd sub )
        {
            this.VehicleStateProperty = eVehicleState.Load;

            //var route = sql.RouteDal.GetRoute( sub.TargetID );

            //if ( !CorrectPosition( route, this.CurrentPosition ) )
            //{
            //    this.OccurVehicleAlarm( 20 );
            //    return false; //Alarm
            //}

            int result = 0;

            result = this.PIOAndLoad( sub.TargetID );
            if ( result != 0 )
            {
                this.PIOSensorOff();
                this.OccurVehicleAlarm( result );
                return false;
            }
            this.PIOSensorOff();

            //Load, Unload 가 끝나면 메인 Command 를 완료 했다고 판단.
            var cmd = sql.CommandDAL.GetById( sub.CmdID );
            cmd.State = eCommandState.Complete;
            sql.CommandDAL.Update( cmd );
            sql.SubCmdDAL.Clean();

            LockUtils.Wait( 1000 );
            this.OnLoadComplete?.Invoke(); //일찍 주면 다음 명령을 500ms 안에 주는 현상 있음. 그러니까 천천히 주자 
            this.VehicleStateProperty = eVehicleState.Idle;

            return true;
        }

        public bool UnloadCarrier( SubCmd sub )
        {
            this.VehicleStateProperty = eVehicleState.Unload;

            var targetNo = Convert.ToInt32( sub.TargetID );
            if ( this.CurrentTag != targetNo )
            {
                this.OccurVehicleAlarm( 21 );
                return false; //Alarm
            }

            //var route = sql.RouteDal.GetRoute( sub.TargetID );

            //if ( !CorrectPosition( route, this.CurrentPosition ) )
            //{
            //    this.OccurVehicleAlarm( 21 );
            //    return false; //Alarm
            //}

            //PIO 내부로 이동.
            //int result = this.clamp.Unlock_Sync();
            //if ( result != 0 )
            //{
            //    this.OccurVehicleAlarm( result );
            //    return false;
            //}

            int result = 0;
            result = this.PIOAndUnload( sub.TargetID );
            if ( result != 0 )
            {
                this.refObjects.IO.OutputOn( "OUT_PIO_SENSOR_ONOFF" );
                this.OccurVehicleAlarm( result );
                return false;
            }

            this.PIOSensorOff();

            var cmd = sql.CommandDAL.GetById( sub.CmdID );
            cmd.State = eCommandState.Complete;
            sql.CommandDAL.Update( cmd );
            sql.SubCmdDAL.Clean();

            LockUtils.Wait( 1000 );
            this.OnUnloadComplete?.Invoke(); //일찍 주면 다음 명령을 500ms 안에 주는 현상 있음. 그러니까 천천히 주자 
            this.VehicleStateProperty = eVehicleState.Idle;

            logger.D( $"[Unloading End]--------------------------------------" );

            return true;
        }

        public void BatteryCharge( SubCmd subCmd )
        {
            int result = 0;
            result = this.StartBatteryCharge();
            if ( result != ConstInt.EXECUTE_SUCCESS )
                this.OccurVehicleAlarm( result );

            var cmd = sql.CommandDAL.GetById( subCmd.CmdID );
            cmd.State = eCommandState.Complete;
            sql.CommandDAL.Update( cmd );
            sql.SubCmdDAL.Clean();
        }

        /// <summary>
        /// Battery Charge
        /// 충전 시 PIO 를 해야 함.
        /// </summary>
        /// <param name="sub"></param>
        /// <returns></returns>
        int PIOBatteryCharge( SubCmd sub )
        {
            var route = sql.RouteDal.GetById( sub.TargetID );

            if ( !CorrectPosition( route, this.CurrentPosition ) )
            {
                this.OccurVehicleAlarm( 21 );
                return 0; //Alarm
            }

            var pioTimeout = CastTo<int>.From<string>( sql.ConfigDal.GetById( ConstString.PIOTimeOut ).Value );

            PIOClear();
            loggerPIO.I( $"Start Charge PIO - [{sub.TargetID}]" );
            if ( !this.refObjects.IO.IsOn( "IN_PIO_READY" ) )
            {
                loggerPIO.E( "[Port] - 1 Ready not On" );
                this.OccurVehicleAlarm( 25 );
                return 0;
            }

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_READY", true );
            loggerPIO.I( "[Vehicle] - 1 Ready On" );

            if ( !this.refObjects.IO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_RECEIVE_RUN" ) )
            {
                PIOClear();
                loggerPIO.E( "[Port] - 2 Receive CV Run Timeout" );
                this.OccurVehicleAlarm( 26 );
                return 0;
            }

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_SENDING_RUN", true );
            loggerPIO.I( "[Vehicle] - 2 Send Run On" );

            var sTime = SwUtils.CurrentTimeMillis;
            while ( true )
            {
                Thread.Sleep( 5 );

                if ( !this.refObjects.IO.IsOn( "IN_PIO_READY" ) || this.refObjects.IO.IsOn( "IN_PIO_RECEIVE_RUN" ) )
                    break;

                if ( null == sql.CommandDAL.GetById( this.CurrentSubCommand.CmdID ) )
                {
                    PIOClear();
                    logger.D( "[Wait Charging] - 메인 명령 사라짐" );
                    break;
                }
            }

            if ( !this.refObjects.IO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_RECEIVE_COMPLITE" ) )
            {
                PIOClear();
                loggerPIO.E( "[Port] - 3 Receive Complete Timeout" );
                this.OccurVehicleAlarm( 26 );
                return 0;
            }

            PIOClear();

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE", true );
            Thread.Sleep( 1000 );
            this.refObjects.IO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE", false );

            return 0;
        }

        public int StartBatteryCharge()
        {
#if SIMULATION
            this.PIOSensorOn();

            var pioTimeout = CastTo<int>.From<string>( sql.ConfigDal.GetById( ConstString.PIOTimeOut ).Value );

            PIOClear();
            loggerPIO.I( $"Start Battery Charge PIO" );

            //if ( !this.iO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_SENDABLE" ) )
            //{
            //    PIOClear();
            //    loggerPIO.E( "[Port] - 4 Ready Time Out" );
            //    return 34;
            //}
            Thread.Sleep( 1000 );
            loggerPIO.E( "[Port] - 4 Ready On" );

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECEIVABLE", true );
            loggerPIO.I( "[Vehicle] - 4 Receivable" );

            //if ( !this.iO.WaitChangeInputIO( true, 20000, "IN_PIO_SEND_RUN" ) )
            //{
            //    PIOClear();
            //    loggerPIO.E( "[Port] - 5 Sending Run Time Out" );
            //    return 35;
            //}
            Thread.Sleep( 1000 );
            loggerPIO.I( "[Port] - 5 Sending Run On" );

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECEIVE_RUN", true );
            loggerPIO.I( "[Vehicle] - 5 Receive Run On" );

            this.VehicleStateProperty = eVehicleState.Charge;
            this.OnChargingStart?.Invoke();
            Thread.Sleep( 1000 );
            this.OnCharging?.Invoke();

            return 0;
#else
            this.PIOSensorOn();

            var pioTimeout = Convert.ToInt32( sql.ConfigDal.GetById( ConstString.PIOTimeOut ).Value );

            PIOClear();
            loggerPIO.I( $"Start Battery Charge PIO" );

            if ( !this.refObjects.IO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_SENDABLE" ) )
            {
                PIOClear();
                loggerPIO.E( "[Port] - 4 Ready Time Out" );
                return 34;
            }
            loggerPIO.E( "[Port] - 4 Ready On" );

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECEIVABLE", true );
            loggerPIO.I( "[Vehicle] - 4 Receivable" );

            if ( !this.refObjects.IO.WaitChangeInputIO( true, 20000, "IN_PIO_SEND_RUN" ) )
            {
                PIOClear();
                loggerPIO.E( "[Port] - 5 Sending Run Time Out" );
                return 35;
            }
            loggerPIO.I( "[Port] - 5 Sending Run On" );

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECEIVE_RUN", true );
            loggerPIO.I( "[Vehicle] - 5 Receive Run On" );

            this.VehicleStateProperty = eVehicleState.Charge;

            return 0;
#endif

        }

        public int StopBatteryCharge()
        {
#if SIMULATION
            loggerPIO.I( $"Stop Battery Charge PIO" );

            var pioTimeout = CastTo<int>.From<string>( sql.ConfigDal.GetById( ConstString.PIOTimeOut ).Value );

            this.PIOClear();

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", true );
            loggerPIO.I( "[Vehicle] Receive Complete On" );

            //if ( !this.iO.WaitChangeInputIO( true, 20000, "IN_PIO_SEND_COMPLITE" ) )
            //{
            //    this.iO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", false );
            //    loggerPIO.E( "[Port]  IN_PIO_SEND_COMPLITE On Time Out" );
            //    return 36;
            //}
            Thread.Sleep( 1000 );
            loggerPIO.I( "[Port] Send Complete On" );

            Thread.Sleep( 1000 );
            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", false );

            this.OnChargingFull?.Invoke();

            this.PIOSensorOff();

            this.VehicleStateProperty = eVehicleState.Idle;

            return 0;
#else
            loggerPIO.I( $"Stop Battery Charge PIO" );

            var pioTimeout = Convert.ToInt32( sql.ConfigDal.GetById( ConstString.PIOTimeOut ).Value );

            this.PIOClear();

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", true );
            loggerPIO.I( "[Vehicle] Receive Complete On" );

            if ( !this.refObjects.IO.WaitChangeInputIO( true, 10000, "IN_PIO_SEND_COMPLITE" ) )
            {
                this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", false );
                loggerPIO.E( "[Port]  IN_PIO_SEND_COMPLITE On Time Out" );
                return 36;
            }
            loggerPIO.I( "[Port] Send Complete On" );

            Thread.Sleep( 1000 );
            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", false );

            this.PIOSensorOff();

            this.VehicleStateProperty = eVehicleState.Idle;

            return 0;
#endif
        }

        #endregion

        #region Check Method
        bool CheckObstacle()
        {
            if ( this.refObjects.IO.IsOff( "IN_OBSTRUCTION_DETECT_SAFETY" ) || this.refObjects.IO.IsOff( "IN_OBSTRUCTION_DETECT_ERROR" ) )
            {
                this.ObstacleStateProperty = eObstacleState.Abnormal;
                this.OccurVehicleAlarm( 9999 );
            }
            else if ( this.refObjects.IO.IsOff( "IN_OBSTRUCTION_DETECT_STOP" ) )
            {
                this.ObstacleStateProperty = eObstacleState.Blocked;
            }
            else if ( this.refObjects.IO.IsOff( "IN_OBSTRUCTION_DETECT_SLOW" ) )
            {
                this.ObstacleStateProperty = eObstacleState.Decelerate;
            }
            else
            {
                this.ObstacleStateProperty = eObstacleState.Normal;
            }

            this.refObjects.Drive.SetObstacleState( this.ObstacleStateProperty );

            return false;
        }

        void CheckIOState()
        {
            //이미 알람이면 체크 안함.
            if ( this.VehicleStateProperty == eVehicleState.Abnormal ) return;

            if ( this.refObjects.IO.IsConnectError ) return;

            //if ( this.iO.IsOn( "IN_EMS_SW" ) ) this.OccurVehicleAlarm( 28 );
            //if ( !this.iO.IsOn( "IN_CP_ON_SAFETY" ) ) this.OccurVehicleAlarm( 31 );
            //if ( !this.iO.IsOn( "IN_CP_ON_24V" ) ) this.OccurVehicleAlarm( 30 );
            //if ( !this.iO.IsOn( "IN_MC_ON" ) ) this.OccurVehicleAlarm( 29 );
        }

        #endregion

        #region Mechanical Method

        #region IO
        /// <summary>
        /// Out Put 을 On 이면 Sensor Off
        /// </summary>
        public void PIOSensorOn() => this.refObjects.IO.OutputOff( "OUT_PIO_SENSOR_ONOFF" );
        /// <summary>
        /// Out Put 을 Off 이면 Sensor On
        /// </summary>
        public void PIOSensorOff() => this.refObjects.IO.OutputOn( "OUT_PIO_SENSOR_ONOFF" );

        /// <summary>
        /// 충전 중일때 MTL PIO Bit 가 살아 있다 
        /// 이걸 보고 판단 하자. 충전 단자 전진 상태임.
        /// </summary>
        /// <returns></returns>
        public bool IsCharging() => this.refObjects.IO.IsOn( "IN_PIO_SEND_RUN" ) && this.refObjects.IO.IsOn( "IN_PIO_SENDABLE" );
        #endregion

        #region Drive
        //public void DriveServoOff() => this.drive.ServoOff();
        //public void DriveServoOn() => this.drive.ServoOn();
        #endregion

        #region Conveyor
        public void ConveyorOff() => this.refObjects.Conveyor.OnOffConveyor( false );
        public int ConveyorLoad() => this.refObjects.Conveyor.ConveyorLoad();
        public int ConveyorUnload() => this.refObjects.Conveyor.ConveyorUnload();

        public int PIOAndLoad( string targetName )
        {
#if SIMULATION
            PIOClear();
            loggerPIO.I( $"Start Load PIO - [{targetName}]" );
            this.OnPIOStart?.Invoke( true );

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECEIVE_RUN", true );
            loggerPIO.I( "[Vehicle] - 4 Receive Run On" );

            Thread.Sleep( 1000 );//상대 IO 기다린다 생각.
            loggerPIO.E( "[Port] - 4 Ready On" );

            //Conveyor Start
            loggerPIO.I( "[Vehicle] - Conveyor Run" );
            this.OnConveyorStart?.Invoke( true );

            Thread.Sleep( 10000 );//Conveyor 구동
            this.OnCarrierDetected?.Invoke( true );

            PIOClear();
            Thread.Sleep( 1000 );
            this.OnConveyorStop?.Invoke( true );

            Thread.Sleep( 1000 );
            this.OnLoadComplete?.Invoke();
#else
            this.PIOSensorOn();
            LockUtils.Wait( 500 );

            int result = 0;
            var pioTimeout = Convert.ToInt32( sql.ConfigDal.GetById( ConstString.PIOTimeOut ).Value );

            result = this.refObjects.Clamp.Unlock_Sync();
            if ( result != 0 )
            {
                this.OccurVehicleAlarm( result );
                return result;
            }

            if ( this.refObjects.Conveyor.IsInverterError() )
                return 16;

            if ( !this.refObjects.Conveyor.IsLifterPositinCheck() )
                return 14;

            //Todo: Sensor Setting 이 후 주석 풀기.
            //if ( !this.IsLifterDuplication() )
            //{
            //    this.OnFailReport?.Invoke( eFailCode.Load_PortHasNotCarrier );
            //    return 0;
            //}

            if ( this.refObjects.Conveyor.IsDetectedCenter() )
            {
                this.OnFailReport?.Invoke( eFailCode.Load_VehicleHasCarrier );
                return 0;
            }

            PIOClear();
            loggerPIO.I( $"Start Load PIO - [{targetName}]" );
            this.OnPIOStart?.Invoke( true );

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECEIVABLE", true );
            loggerPIO.I( "[Vehicle] - 4 Receivable" );

            if ( !this.refObjects.IO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_SENDABLE" ) )
            {
                PIOClear();
                loggerPIO.E( "[Port] - 4 Ready Time Out" );
                this.OnFailReport?.Invoke( eFailCode.LoadPIOInterlockTimeout );
                return 0;
            }
            loggerPIO.E( "[Port] - 4 Ready On" );


            //this.conveyor.SetConveyorSpeed( true );
            this.refObjects.Conveyor.OnOffConveyor( true, true );
            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECEIVE_RUN", true, 1000 ); //1Sec 이후 On
            loggerPIO.I( "[Vehicle] - Conveyor Run" );
            this.OnConveyorStart?.Invoke( true );

            if ( !this.refObjects.IO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_SEND_RUN" ) )
            {
                this.refObjects.Conveyor.OnOffConveyor( false, true );
                PIOClear();
                loggerPIO.E( "[Port] - 5 Sending Run Time Out" );
                this.OnFailReport?.Invoke( eFailCode.LoadPIOInterlockTimeout );
                return 0;
            }
            loggerPIO.I( "[Port] - 5 Sending Run On" );

            bool isStartDetected = false;
            var sTime = SwUtils.CurrentTimeMillis;
            while ( true )
            {
                LockUtils.Wait( 10 );

                if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) )
                {
                    PIOClear();
                    this.refObjects.Conveyor.OnOffConveyor( false, true );
                    loggerPIO.E( "[Vehicle] Conveyor Wait Time Out" );
                    this.OnFailReport?.Invoke( eFailCode.LoadPIOInterlockTimeout );

                    if ( this.refObjects.Conveyor.IsDetectedLoadStart() ) // 감지가 시작 되었으면 이동중 Error 로 판단 설비를 정지 상태로
                        return 10; //Conveyor Moving Timeout
                    else
                        return 0;
                }

                if ( this.refObjects.Conveyor.IsDetectedLoadStart() && !isStartDetected )
                    isStartDetected = true;

                if ( !this.refObjects.Conveyor.IsDetectedLoadStart() && isStartDetected ) { }
                //this.conveyor.SetConveyorSpeed( false );

                if ( this.refObjects.Conveyor.IsDetectedLoadStop() ) break;

                if ( this.refObjects.Conveyor.IsPIOInterLockOn() )
                {
                    PIOClear();
                    this.refObjects.Conveyor.OnOffConveyor( false ); //Stop
                    loggerPIO.E( "[Port] PIO InterLock On " );
                    return 19; //
                }
            }

            if ( this.refObjects.Conveyor.IsDetectedCenter() )
                this.OnCarrierDetected?.Invoke( true );

            this.refObjects.Conveyor.OnOffConveyor( false ); //Stop
            PIOClear();
            this.OnConveyorStop?.Invoke( true );
            loggerPIO.I( "[Vehicle] Conveyor Stop" );

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", true );
            loggerPIO.I( "[Vehicle] Receive Complete On" );
            if ( !this.refObjects.IO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_SEND_COMPLITE" ) )
            {
                this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", false );
                loggerPIO.E( "[Port]  IN_PIO_SEND_COMPLITE On Time Out" );
            }
            loggerPIO.I( "[Port] Send Complete On" );

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", false, 1000 );

            loggerPIO.I( $"End Load PIO - [{targetName}]" );

            result = this.refObjects.Clamp.Lock_Sync();
            if ( result != 0 )
            {
                this.OccurVehicleAlarm( result );
                return result;
            }
#endif

            return 0;
        }

        public int PIOAndUnload( string targetName )
        {
#if SIMULATION
            PIOClear();
            loggerPIO.I( $"Start Unload PIO - [{targetName}]" );
            this.OnPIOStart?.Invoke( false );

            Thread.Sleep( 1000 );

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_READY", true );
            loggerPIO.I( "[Vehicle] - 1 Ready On" );

            Thread.Sleep( 1000 );
            this.OnConveyorStart?.Invoke( false );

            Thread.Sleep( 10000 );
            this.refObjects.Conveyor.OnOffConveyor( false ); //Stop
            this.OnConveyorStop?.Invoke( false );
            PIOClear();

            Thread.Sleep( 1000 );
            this.OnUnloadComplete?.Invoke();
#else
            this.PIOSensorOn();
            LockUtils.Wait( 500 );

            int result = 0;

            var pioTimeout = Convert.ToInt32( sql.ConfigDal.GetById( ConstString.PIOTimeOut ).Value );

            if ( this.refObjects.Conveyor.IsInverterError() )
                return 16;

            //if ( this.refObjects.Conveyor.IsLifterDuplication() )
            //{
            //    this.OnFailReport?.Invoke( eFailCode.Unload_PortHasCarrier );
            //    return 0;
            //}

            if ( !this.refObjects.Conveyor.IsDetectedCenter() )
            {
                this.OnFailReport?.Invoke( eFailCode.Unload_VehicleHasNotCarrier );
                return 0;
            }

            if ( !this.refObjects.Conveyor.IsLifterPositinCheck() )
                return 13;

            PIOClear();
            loggerPIO.I( $"Start Unload PIO - [{targetName}]" );
            this.OnPIOStart?.Invoke( false );

            if ( !this.refObjects.IO.IsOn( "IN_PIO_READY" ) )
            {
                loggerPIO.E( "[Port] - 1 Ready not On" );
                this.OnFailReport?.Invoke( eFailCode.UnlaodPIOInterlockTimeout );
                return 0;
            }

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_READY", true );
            loggerPIO.I( "[Vehicle] - 1 Ready On" );

            if ( !this.refObjects.IO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_RECEIVE_RUN" ) )
            {
                PIOClear();
                loggerPIO.E( "[Port] - 2 Receive CV Run Timeout" );
                this.OnFailReport?.Invoke( eFailCode.UnlaodPIOInterlockTimeout );
                return 0;
            }
            loggerPIO.E( "[Port] - 2 Receive CV Run On" );

            result = this.refObjects.Clamp.Unlock_Sync();
            if ( result != 0 )
            {
                this.OccurVehicleAlarm( result );
                return result;
            }

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_SENDING_RUN", true );
            loggerPIO.I( "[Vehicle] - 2 Send Run On" );

            //this.conveyor.SetConveyorSpeed( true );
            this.refObjects.Conveyor.OnOffConveyor( true );
            this.OnConveyorStart?.Invoke( false );

            var sTime = SwUtils.CurrentTimeMillis;
            while ( true )
            {
                if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) )
                {
                    PIOClear();
                    this.refObjects.Conveyor.OnOffConveyor( false, true );
                    loggerPIO.E( "[Port] Conveyor Wait Time Out" );
                    this.OnFailReport?.Invoke( eFailCode.UnlaodPIOInterlockTimeout );

                    if ( this.refObjects.Conveyor.IsDetectedLoadStart() || this.refObjects.Conveyor.IsDetectedCenter() ) //중간에 걸려 있다고 생각해서 알람 처리.
                        return 12; //Conveyor Moving Timeout
                    else
                        return 0;
                }


                if ( !this.refObjects.Conveyor.IsDetectedLoadStart() && !this.refObjects.Conveyor.IsDetectedCenter() )
                {
                    if ( this.refObjects.IO.IsOn( "IN_PIO_RECEIVE_COMPLITE" ) )
                    {
                        loggerPIO.I( "[Port] - 3 Receive Complete On" );
                        break;
                    }
                }
            }

            if ( !this.refObjects.Conveyor.IsDetectedCenter() )
                this.OnCarrierDetected?.Invoke( false );

            this.refObjects.Conveyor.OnOffConveyor( false ); //Stop
            this.OnConveyorStop?.Invoke( false );
            PIOClear();

            this.refObjects.IO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE", true );
            Thread.Sleep( 1000 );
            this.refObjects.IO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE", false );
            loggerPIO.I( "[Vehicle] - 3 Send Complete OnOff" );
#endif
            loggerPIO.I( $"End Unload PIO - [{targetName}]" );

            return 0;
        }

        void PIOClear()
        {
            string[] pio = { "OUT_PIO_READY", "OUT_PIO_SENDING_RUN", "OUT_PIO_SEND_COMPLITE", "OUT_PIO_RECEIVABLE", "OUT_PIO_RECEIVE_RUN", "OUT_PIO_RECIVE_COMPLITE", "OUT_PIO_INTERLOCK" };
            pio.FwEach( x => { this.refObjects.IO.OutputOff( x ); } );
        }

        #endregion

        #endregion

        #region Help Method

        /// <summary>
        /// 현재 Point 가 변경될때 장애물 감지 센서의 패턴을 변경한다.
        /// </summary>
        /// <param name="v"></param>
        private void ObstaclePatternChange( int v )
        {
            if ( !RouteManager.Instance.Segments.Any( s => s.ID == v ) )
            {
                logger.E( "Current Tag Not Exist RouteManager Point List" );
                return;
            }

            //Todo: 현재 Segment ID 를 가져 와서 변경하는 방식으로 변경 필요.
            var obstacle = RouteManager.Instance.Obstacles.Where( o => o.segmentID == v ).Single();

            this.ChgObstacleDetectPattern( obstacle.fieldset );
        }

        /// <summary>
        /// 현재 좌표 값이 등록된 Route 에 맞는 위치인지 확인한다.
        /// 판단 기준은 Route 에 Tolerance 범위를 사용.
        /// </summary>
        /// <param name="route"></param>
        /// <param name="currentPosition"></param>
        /// <returns></returns>
        bool CorrectPosition( Route route, double currentPosition )
        {
            var rScale = route.ScaleValue;
            var rTolerance = route.ScaleTolerance;
            var result = currentPosition - rScale;
            if ( rTolerance < Math.Abs( result ) )
                return false;

            return true;
        }

        /// <summary>
        /// if no is zero, Laser Off
        /// bit Off, On, On, On,On Area1
        /// </summary>
        /// <param name="no"> 0 == Off Laser</param>
        /// <returns></returns>
        public bool ChgObstacleDetectPattern( int no )
        {
            var bitArray = BitUtils.ChgBitArray( no );

            int bitIndex = 0;
            this.obstacleBitList.ForEach( b =>
             {
                 if ( bitArray[bitIndex] )
                     this.refObjects.IO.OutputOff( b );
                 else
                     this.refObjects.IO.OutputOn( b );
                 bitIndex++;
             } );

            ObstaclePattern = no;

            return true;
        }

        public int GetObstacleDetectPattern()
        {
            int bitIndex = 0;

            BitArray bitArray = new BitArray( this.obstacleBitList.Count );

            this.obstacleBitList.ForEach( b =>
             {
                 if ( this.refObjects.IO.IsOn( b, false ) )
                     bitArray.Set( bitIndex, false );
                 else
                     bitArray.Set( bitIndex, true );
                 bitIndex++;
             } );

            return BitUtils.ChgInt32( bitArray );
        }

        /// <summary>
        /// Vehicle 이동 및 동작 중 Alarm 발생 시 처리
        /// </summary>
        /// <param name="alarmID"></param>
        public void OccurVehicleAlarm( int alarmID )
        {
            this.MachineMode = eMachineMode.LocalMode;
            this.VehicleStateProperty = eVehicleState.Abnormal;
            this.ConveyorOff();

            this.autoManager.ProcessAlarm( alarmID );
        }

        public void SetObstaclePattern( ObstacleControlEventArgs.eControlKind state, int value )
        {
            if ( state == ObstacleControlEventArgs.eControlKind.DRIVE )
            {
                this.ObstacleDrive = value;
                ChgObstacleDetectPattern( this.ObstacleDrive );
            }
            else if ( state == ObstacleControlEventArgs.eControlKind.CURVE )
            {
                this.ObstacleCurve = value;
                ChgObstacleDetectPattern( this.ObstacleCurve );
            }
            else
                return;
        }

        public eSteeringState GetESteeringState() => this.refObjects.Steering.GetSteeringState();
        #endregion

        #region Event Subscribe 
        private void BMUManager_OnDisconnect( string obj )
        {
            this.BatteryIsConnect = false;
            this.OccurVehicleAlarm( 32 );
        }

        private void BMUManager_OnConnect( string obj )
        {
            this.BatteryIsConnect = true;
            //ReqCurrentPos();
        }

        private void BMUManager_OnFirstColtd( List<ReceivedData> obj )
        {
        }

        private void BMUManager_OnChangedReceivedData( Serial.DataModel.ReceivedData obj )
        {
            var kind = CastTo<eDataKind>.From<Enum>( obj.DataKind );
            switch ( kind )
            {
                case eDataKind.Voltage:
                    this.BatteryVoltage = (double)obj.Value;
                    break;
                case eDataKind.Current:
                    this.BatteryCurrent = (double)obj.Value;
                    break;
                case eDataKind.BatteryState:
                    if ( obj.Value == null )
                        return;
                    this.BatteryState = (double)obj.Value;
                    break;
                case eDataKind.ChargeCompleteTime:
                    if ( obj.Value == null /*|| obj.Value <= 0 */)
                        return;
                    this.BatteryChargeTime = (double)obj.Value;
                    if ( this.BatteryChargeTime > 0 )
                        this.VehicleStateProperty = eVehicleState.Charge;
                    break;
                case eDataKind.DisChargeCompleteTime:
                    if ( obj.Value == null || obj.Value <= 0 )
                        return;
                    this.BatteryDisChargeTime = (double)obj.Value;
                    break;
                case eDataKind.SOC:
                    this.BatteryStateOfCharge = (double)obj.Value;
                    if ( this.BatteryStateOfCharge >= 80 )
                    {
                        if ( this.autoManager.AutoModeStateProperty != eAutoModeState.Run )
                            return;

                        if ( this.VehicleStateProperty == eVehicleState.Charge || this.IsCharging() )
                        {
                            int result = ConstInt.EXECUTE_SUCCESS;
                            result = this.StopBatteryCharge();
                            if ( result != ConstInt.EXECUTE_SUCCESS )
                                this.OccurVehicleAlarm( result );
                        }
                    }
                    break;
                case eDataKind.SOH:
                    this.BatteryStateOfHealth = (double)obj.Value;
                    break;
                case eDataKind.ResidualCapacity:
                    this.BatteryCapacity = (double)obj.Value;
                    break;
                case eDataKind.ResidualEnergy:
                    this.BatteryEnergy = (double)obj.Value;
                    break;
                case eDataKind.Temperature:
                    this.BatteryTemperature = (double)obj.Value;
                    break;
                default:
                    break;
            }
        }

        private void AutoManager_OnOperationModeChanged( eOperatationMode obj )
        {
            this.refObjects.Drive.SetDriveOperationMode( obj );
        }

        private void ZmqManager_PropertyChanged( object sender, System.ComponentModel.PropertyChangedEventArgs e )
        {
            var property = sender.GetType().GetProperty( e.PropertyName );
            var newValue = property.GetValue( sender, null );

            switch ( e.PropertyName )
            {
                case "SegmentID":
                    {
                        var v = CastTo<int>.From<object>( newValue );
                        this.ObstaclePatternChange( v );
                    }
                    break;

                case "RequestSteering":
                    {
                        var v = CastTo<eSteeringState>.From<object>( newValue );
                        switch ( v )
                        {
                            case eSteeringState.None:
                                break;
                            case eSteeringState.Left:
                                this.refObjects.Steering.ControlSteering( true );
                                break;
                            case eSteeringState.Right:
                                this.refObjects.Steering.ControlSteering();
                                break;
                            default:
                                break;
                        }
                    }
                    break;

                case "CurrentPointNo":
                    {
                        var v = CastTo<int>.From<object>( newValue );
                        logger.D( $"{this.CurrentTag} -> {v}" );
                        this.CurrentTag = v;
                    }
                    break;

                case "RearDriveState":
                    {
                        var v = CastTo<DriveState>.From<object>( newValue );
                        this.RearDriveState = v;
                    }
                    break;

                case "FrontLoadFactor":
                    {
                        var v = CastTo<double>.From<object>( newValue );
                        this.FrontLoadFactor = v;
                        this.FrontTorque = Math.Truncate( ( ( v * 1.9 ) / 1000 ) * 100 ) / 100;
                    }
                    break;

                case "FrontRPM":
                    {
                        var v = CastTo<double>.From<object>( newValue );
                        this.FrontRpm = v;

                        var ll = ( ( ( ( v / 60 ) * ( 2 * Math.PI ) ) * 0.06 ) / 10 );
                        this.FrontSpeed = Math.Truncate( ll * 100 ) / 100;
                    }
                    break;

                case "RearLoadFactor":
                    {
                        var v = CastTo<double>.From<object>( newValue );
                        this.RearLoadFactor = v;
                        this.RearTorque = Math.Truncate( ( ( v * 1.9 ) / 1000 ) * 100 ) / 100;
                    }
                    break;

                case "RearRPM":
                    {
                        var v = CastTo<double>.From<object>( newValue );
                        this.RearRpm = v;

                        var ll = ( ( ( ( v / 60 ) * ( 2 * Math.PI ) ) * 0.06 ) / 10 );
                        this.RearSpeed = Math.Truncate( ll * 100 ) / 100;
                    }
                    break;

                default:
                    break;
            }
        }

        private void Motion_PropertyChanged( object sender, System.ComponentModel.PropertyChangedEventArgs e )
        {
            var property = sender.GetType().GetProperty( e.PropertyName );
            var newValue = property.GetValue( sender, null );

            switch ( e.PropertyName )
            {
                case "CurrentPos":
                    {
                        var v = CastTo<double>.From<object>( newValue );
                        this.CurrentPosition = v;
                    }
                    break;
                //case "CurrentTag":
                //    {
                //        var v = CastTo<int>.From<object>( newValue );
                //        logger.D( $"{this.CurrentTag} -> {v}" );
                //        this.CurrentTag = v;
                //    }
                //    break;
                case "CurrentSpeed":
                    {
                        var v = CastTo<double>.From<object>( newValue );
                        this.CurrentSpeed = v;
                    }
                    break;
                case "CurrentTorque":
                    {
                        var v = CastTo<double>.From<object>( newValue );
                        this.CurrentTorque = v;
                    }
                    break;
                case "ReqSteeringState":
                    //{
                    //    var v = CastTo<eSteeringState>.From<object>( newValue );
                    //    switch ( v )
                    //    {
                    //        case eSteeringState.None:
                    //            break;
                    //        case eSteeringState.Left:
                    //            this.refObjects.Steering.ControlSteering( true );
                    //            break;
                    //        case eSteeringState.Right:
                    //            this.refObjects.Steering.ControlSteering();
                    //            break;
                    //        default:
                    //            break;
                    //    }
                    //}
                    break;
                case "DriveServoState":
                    {
                        var v = CastTo<eDriveServoState>.From<object>( newValue );
                        if ( v == eDriveServoState.ServoOn )
                            this.refObjects.IO.OutputOn( "OUT_DRIVE_BRAKE_OFF" );
                        else
                            this.refObjects.IO.OutputOff( "OUT_DRIVE_BRAKE_OFF" );
                    }
                    break;
                case "FrontDriveState":
                    {
                        var v = CastTo<DriveState>.From<object>( newValue );
                        this.FrontDriveState = v;
                    }
                    break;
                case "RearDriveState":
                    {
                        var v = CastTo<DriveState>.From<object>( newValue );
                        this.RearDriveState = v;
                    }
                    break;

                    //case "FrontLoadFactor":
                    //    {
                    //        var v = CastTo<double>.From<object>( newValue );
                    //        this.FrontLoadFactor = v;
                    //        this.FrontTorque = Math.Truncate( ( ( v * 1.9 ) / 1000 ) * 100 ) / 100;
                    //    }
                    //    break;
                    //case "FrontRpm":
                    //    {
                    //        var v = CastTo<double>.From<object>( newValue );
                    //        this.FrontRpm = v;

                    //        var ll = ( ( ( ( v / 60 ) * ( 2 * Math.PI ) ) * 0.06 ) / 10 );
                    //        this.FrontSpeed = Math.Truncate( ll * 100 ) / 100;
                    //    }
                    //    break;
                    //case "RearLoadFactor":
                    //    {
                    //        var v = CastTo<double>.From<object>( newValue );
                    //        this.RearLoadFactor = v;
                    //        this.RearTorque = Math.Truncate( ( ( v * 1.9 ) / 1000 ) * 100 ) / 100;
                    //    }
                    //    break;
                    //case "RearRpm":
                    //    {
                    //        var v = CastTo<double>.From<object>( newValue );
                    //        this.RearRpm = v;

                    //        var ll = ( ( ( ( v / 60 ) * ( 2 * Math.PI ) ) * 0.06 ) / 10 );
                    //        this.RearSpeed = Math.Truncate( ll * 100 ) / 100;
                    //    }
                    break;
                default:
                    break;
            }
        }

        private void Steering_PropertyChanged( object sender, System.ComponentModel.PropertyChangedEventArgs e )
        {
            var property = sender.GetType().GetProperty( e.PropertyName );
            var newValue = property.GetValue( sender, null );

            //Todo: 나중에 Test 하자
            //var ownPropperty = this.GetType().GetProperty(e.PropertyName);
            if ( e.PropertyName.Equals( "SteeringState" ) )
            {
                var v = CastTo<eSteeringState>.From<object>( newValue );
                this.SteeringState = v;
                logger.D( $"steering - {v}" );
            }
        }

        private void Clamp_PropertyChanged( object sender, System.ComponentModel.PropertyChangedEventArgs e )
        {
            var property = sender.GetType().GetProperty( e.PropertyName );
            var newValue = property.GetValue( sender, null );

            if ( e.PropertyName.Equals( "ClampState" ) )
            {
                var v = CastTo<eClampState>.From<object>( newValue );
                this.ClampState = v;
            }
        }

        private void IO_OnChangedIO( BitBlock bit )
        {
            switch ( bit.Tag )
            {
                case "IN_CV_DETECT_01":
                    this.IsContain = bit.IsBitOn;
                    this.refObjects.ZmqManager.Publish( "Contain", this.IsContain.ToString() );
                    break;

                case "IN_MC_ON":
                    if ( bit.IsBitOn )
                    {
                        if ( !this.refObjects.IO.IsOn( "IN_EMS_SW" ) && this.refObjects.IO.IsOn( "IN_CP_ON_SAFETY" ) && this.refObjects.IO.IsOn( "IN_CP_ON_24V" ) )
                            this.VehicleStateProperty = eVehicleState.Idle;
                    }
                    else
                        this.OccurVehicleAlarm( 29 );
                    break;

                case "IN_EMS_SW":
                    if ( bit.IsBitOn )
                        this.OccurVehicleAlarm( 28 );
                    break;

                case "IN_CP_ON_SAFETY":
                    if ( !bit.IsBitOn )
                        this.OccurVehicleAlarm( 31 );
                    break;

                case "IN_CP_ON_24V":
                    if ( !bit.IsBitOn )
                        this.OccurVehicleAlarm( 30 );
                    break;

                case "IN_OBSTRUCTION_DETECT_ERROR":
                case "IN_OBSTRUCTION_DETECT_SAFETY":
                case "IN_OBSTRUCTION_DETECT_STOP":
                case "IN_OBSTRUCTION_DETECT_SLOW":
                    CheckObstacle();
                    break;

                default:
                    break;
            }
        }

        private void Steering_OnSteeringError( object sender, int e )
        {
            if ( e != 0 )
            {
                logger.E( $"[Steering] - Control Error {e}" );
                this.OccurVehicleAlarm( e );
            }
            else
            {
                var msg = new DriveControlEventArgs()
                {
                    EventDir = DriveControlEventArgs.eEventDir.ToFront,
                    ControlKind = DriveControlEventArgs.eControlKind.Steering,
                    Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>( DriveControlEventArgs.eMoveDir.LEFT ),
                };

                this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Publish( msg );
            }

        }

        #endregion
    }
}