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

using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
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.IO;
using VehicleControlSystem.ControlLayer.Motion;
using VehicleControlSystem.ControlLayer.Serial.BatteryTabos;
using VehicleControlSystem.ControlLayer.Serial.DataModel;
using VehicleControlSystem.Managers;

namespace VehicleControlSystem.ControlLayer
{
    /// <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 string currentTag = "0000";
        public string CurrentTag
        {
            get { return currentTag; }
            set
            {
                if ( SetField( ref this.currentTag , value ) )
                {
                    var info = sql.VehicleInfoDAL.GetInfo();
                    info.CurrentTag = value;
                    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 { SetField( ref this.steeringState , 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 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.GetInfo();
                    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.GetInfo();
                    info.MachineMode = value;
                    sql.VehicleInfoDAL.Update( info );
                }
            }
        }

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


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

        public bool IsMoving { get; set; }

        double batteryVoltage;
        public double BatteryVoltage 
        { 
            get { return this.batteryVoltage; } 
            set { 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 { this.SetField( ref this.batteryEnergy , value ); } 
        }

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

        bool batteryIsConnect;
        public bool BatteryIsConnect 
        { 
            get 
            {
                this.BatteryIsConnect = this.bMUManager.IsConnected;
                return this.bMUManager.IsConnected; 
            }
            set { this.SetField( ref this.batteryIsConnect , value ); }
        }
        public bool IsError { get; set; }

        public SubCmd CurrentSubCommand { get; private set; }
        #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<string> 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

        EzIO iO = null;
        GSIMotion motion = null;
        SqliteManager sql = null;
        Clamp clamp = null;
        Steering steering = null;
        AutoManager autoManager = null;
        BMUManager bMUManager = null;

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

        IEventAggregator eventAggregator;

        public Vehicle( IIO io, SqliteManager sqliteManager, IEventAggregator ea, AutoManager auto )
        {
            this.iO = io as EzIO;
            this.iO.OnChangedIO += IO_OnChangedIO;
            this.sql = sqliteManager;
            this.autoManager = auto;

            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;

            /*Drive*/
            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,
                                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 )
        {
            if ( this.autoManager.OperationModeProperty != eOperatationMode.ManualMode )
                return;

            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.steering.ControlSteering( true );
                        else
                            this.steering.ControlSteering();
                        break;
                    case DriveControlEventArgs.eControlKind.SteeringState:
                        {
                            var reply = new DriveControlEventArgs();
                            reply.ControlKind = DriveControlEventArgs.eControlKind.SteeringState;
                            if ( this.steering.IsLeft() )
                                reply.Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>( DriveControlEventArgs.eMoveDir.LEFT );
                            else
                                reply.Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>( DriveControlEventArgs.eMoveDir.RIGHT );
                            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;
                    case DriveControlEventArgs.eControlKind.NONE:
                        break;
                    case DriveControlEventArgs.eControlKind.Conveyor:
                        this.ReqConveyor( _args );
                        break;
                    default:
                        break;
                }
            }
        }

        private void ReqConveyor( DriveControlEventArgs args )
        {
            if ( args.CvDir == DriveControlEventArgs.eCvDir.CW ) //Load
                this.OnOffConveyor( true, false );
            else if ( args.CvDir == DriveControlEventArgs.eCvDir.CCW ) //Unload
                this.OnOffConveyor( true, true );
            else if ( args.CvDir == DriveControlEventArgs.eCvDir.STOP )
                this.OnOffConveyor( false, false );
        }

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

        public void Init()
        {
            this.CreateClamp();
            this.CreateSteering();
            this.CreateDrive();
            this.CreateBMUManager();

            ThreadStart();

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

            sql.VehicleInfoDAL.Update( v );
        }

        public int InitializationVehicle()
        {
            int result = 0;
            if ( this.IsDetectedCenter() ) //자제가 있으면 Lock
                result = this.clamp.Lock_Sync();
            else
                result = this.clamp.Unlock_Sync();

            if ( this.motion.IsErrorOn )
                return 22;

            this.VehicleStateProperty = eVehicleState.Idle;

            return result;
        }

        public void Dispose()
        {
            this.cancel.Cancel();
            this.cancel.StopWaitAll();
        }

        #region Request Method
        private void ReqVehicleState( DriveControlEventArgs args )
        {
            VehicleInfo state = new VehicleInfo();
            state.CurrentPosition = this.CurrentPosition;
            state.CurrentSpeed = this.CurrentSpeed;
            state.CurrentTag = this.CurrentTag;
            state.CurrentTorque = this.CurrentTorque;
            
            state.Voltage = this.BatteryVoltage;
            state.Current = this.BatteryCurrent;
            state.BatteryState = this.BatteryState;
            state.ChargeTime = this.BatteryChargeTime;
            state.DisChargeTime = this.BatteryDisChargeTime;
            state.SOC = this.BatteryStateOfCharge;
            state.SOH = this.BatteryStateOfHealth;
            state.Capacity = this.BatteryCapacity;
            state.Energy = this.BatteryEnergy;
            state.Temperature = this.BatteryTemperature;
            state.BatteryIsConnect = this.BatteryIsConnect;

            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.GetK( targetTag );
            if ( route == null )
            {
                reply.Result = Results.Fail( "Not Found Route" );
                this.DriveControlEventPublish( reply );
                return;
            }

            this.MoveTo( route.Name );
            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 )
        {
            var drive = string.Empty;

            if ( _args.JogDir == DriveControlEventArgs.eJogMoveDir.Positive )
                drive = "POSITIVE";
            else
                drive = "NEGATIVE";
        }

        void ReqCurrentPos()
        {
            var task = Task.Factory.StartNew( () =>
             {
                 while ( !this.taskCancel.Canceled )
                 {
                     LockUtils.Wait( 500 );

                     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._ThObstacleChecker ) );
        }

        //장애물 감지 Thread
        //장애물 감지 패턴 변경도 여기 하자.
        private void _ThObstacleChecker()
        {
            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" );
        }

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

            if ( this.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 );
        }

        /// <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.GetSubCmd();
                    if ( subCmd == null ) continue;

                    if ( !sql.CommandDAL.All.Any( x => x.CommandID.Equals( subCmd.CmdID ) ) )
                    {
                        if ( subCmd.CmdType == SubCmd.eCmdType.Auto ) //자동 명령중 Main Command 가 없으면 삭제.
                        {
                            sql.SubCmdDAL.Delete( subCmd );
                            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 ( ThreadInterruptedException threadInterruptedException )
                {
                }
                catch ( Exception exception )
                {
                    logger.E( exception );
                }
                finally
                {
                    LockUtils.Wait( 500 );
                }
            }
            logger.D( "Vehicle - _ThSubCmdWorker Dispose" );
        }

        #endregion

        #region Control Action Method
        public void EStop()
        {
            OnOffConveyor( false );
            //Clamp EStop
            this.clamp.ClampEStop();
            this.motion.EStop();

            this.OccurVehicleAlarm( 23 );
        }

        #region For Moving
        void Move( SubCmd sub )
        {
            if ( this.MoveTo( sub.TargetID ) )
            {
            }

            sql.SubCmdDAL.Delete( sub );
        }

        bool MoveTo( string pointID )
        {
            //this.BuzzerOnOff(true, eBuzzerKind.StartWarn);
            ////TimerUtils.Once(3000, BuzzerOnOff, false, eBuzzerKind.StartWarn );
            //Thread.Sleep(3000);
            //this.BuzzerOnOff(false);

            if ( this.VehicleStateProperty == eVehicleState.Idle )
            {
                this.OnMoveReady?.Invoke();

                var moveReadyBuzzerTime = sql.ConfigDal.GetValueToInt( ConstString.BuzzerStartReadyTime );
                Thread.Sleep( moveReadyBuzzerTime );

                this.VehicleStateProperty = eVehicleState.Move;
            }

            this.OnMoving?.Invoke();
            this.IsMoving = true;

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

            this.taskMoveCancel.Add( CheckCrossPoint() );

            //this.BuzzerOnOff(true, eBuzzerKind.Moving);
            this.motion.MoveToPoint( pointID, 100 );

            bool result = Wait4MoveDone();
            //this.BuzzerOnOff(false);

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

            if ( motion.IsStop )
            {
                this.IsMoving = false;
                this.OnMoveFinish?.Invoke();

                this.VehicleStateProperty = eVehicleState.Idle;
            }

            return result;
        }

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

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

                if ( SwUtils.Gt( st, waitTime ) )
                {
                    //Todo: 이동시간 초과 시 동작들.
                    break;
                }

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

                //20.04.04 Kang Drive 측으로 상태만 알려주면 알아서 처리함.
                //if ( this.ObstacleStateProperty != eObstacleState.Normal )
                //{
                //    if ( this.ObstacleStateProperty == eObstacleState.Blocked )
                //        this.VehicleStateProperty = eVehicleState.Blocked;

                //    if ( this.ObstacleStateProperty == eObstacleState.Abnormal )
                //    {
                //        this.VehicleStateProperty = eVehicleState.Abnormal;
                //        this.OccurVehicleAlarm( 24 );
                //        return false;
                //    }
                //}
            }

            return true;
        }

        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 = this.clamp.Unlock_Sync();
            if ( result != 0 )
            {
                this.OccurVehicleAlarm( result );
                return false;
            }

            this.iO.OutputOn( "OUT_PIO_SENSOR_ONOFF" );
            result = this.PIOAndLoad( sub.TargetID );
            if ( result != 0 )
            {
                this.iO.OutputOff( "OUT_PIO_SENSOR_ONOFF" );
                this.OccurVehicleAlarm( result );
                return false;
            }
            this.iO.OutputOff( "OUT_PIO_SENSOR_ONOFF" );

            result = this.clamp.Lock_Sync();
            if ( result != 0 )
            {
                this.OccurVehicleAlarm( result );
                return false;
            }

            //Load, Unload 가 끝나면 메인 Command 를 완료 했다고 판단.
            sql.CommandDAL.UpdateState( sub.CmdID, eCommandState.Complete );
            sql.SubCmdDAL.Delete( sub );

            this.VehicleStateProperty = eVehicleState.Idle;

            return true;
        }

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

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

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

            int result = this.clamp.Unlock_Sync();
            if ( result != 0 )
            {
                this.OccurVehicleAlarm( result );
                return false;
            }

            this.iO.OutputOn( "OUT_PIO_SENSOR_ONOFF" );

            result = this.PIOAndUnload( sub.TargetID );
            if ( result != 0 )
            {
                this.iO.OutputOff( "OUT_PIO_SENSOR_ONOFF" );
                this.OccurVehicleAlarm( result );
                return false;
            }
            this.iO.OutputOff( "OUT_PIO_SENSOR_ONOFF" );

            sql.CommandDAL.UpdateState( sub.CmdID, eCommandState.Complete );
            sql.SubCmdDAL.Delete( sub );

            this.VehicleStateProperty = eVehicleState.Idle;

            return true;
        }

        private void BatteryCharge( SubCmd subCmd )
        {
            this.VehicleStateProperty = eVehicleState.Charge;

            this.iO.OutputOn( "OUT_PIO_SENSOR_ONOFF" );
            
            this.PIOBatteryCharge( subCmd );
            
            this.iO.OutputOff( "OUT_PIO_SENSOR_ONOFF" );

            sql.CommandDAL.UpdateState( subCmd.CmdID, eCommandState.Complete );
            sql.SubCmdDAL.Delete( subCmd );

            this.VehicleStateProperty = eVehicleState.Idle;
        }

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

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

            var pioTimeout = sql.ConfigDal.GetValueToInt( ConstString.PIOTimeOut );

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

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

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

            this.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.iO.IsOn( "IN_PIO_READY" ) || this.iO.IsOn( "IN_PIO_RECEIVE_RUN" ) )
                    break;

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

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

            PIOClear();

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

            return 0;
        }

        #endregion

        #region Check Method
        bool CheckObstacle()
        {
            //if ( this.iO.IsOn( "IN_OBSTRUCTION_DETECT_SAFETY" ) || this.iO.IsOn( "IN_OBSTRUCTION_DETECT_ERROR" ) )
            //{
            //    this.ObstacleStateProperty = eObstacleState.Abnormal;
            //    this.motion.SetObstacleState( this.ObstacleStateProperty );
            //    return true;
            //}

            //if ( this.iO.IsOn( "IN_OBSTRUCTION_DETECT_STOP" ) )
            //{
            //    this.ObstacleStateProperty = eObstacleState.Blocked;
            //    this.motion.SetObstacleState( this.ObstacleStateProperty );
            //    return true;
            //}

            //if ( this.iO.IsOn( "IN_OBSTRUCTION_DETECT_SLOW" ) )
            //{
            //    this.ObstacleStateProperty = eObstacleState.Decelerate;
            //    this.motion.SetObstacleState( this.ObstacleStateProperty );
            //    return true;
            //}

            this.ObstacleStateProperty = eObstacleState.Normal;
            this.motion.SetObstacleState( this.ObstacleStateProperty );

            return false;
        }

        #endregion

        #region Mechanical Method

        #region Conveyor
        /// <summary>
        /// (Run = true, CW = true CCW = false)
        /// </summary>
        /// <param name="isOn"></param>
        /// <param name="isLoad">bit On 시 Unload 방향 진행.</param>
        /// <returns></returns>
        int OnOffConveyor( bool isOn, bool isLoad = false )
        {
            if ( IsInverterError() )
                return 16;

            if ( isLoad )
                this.iO.OutputOn( "OUT_CV_CWCCW" );
            else
                this.iO.OutputOff( "OUT_CV_CWCCW" );

            if ( isOn )
                this.iO.OutputOn( "OUT_CV_RUN" );
            else
                this.iO.OutputOff( "OUT_CV_RUN" );

            return 0;
        }

        void SetConveyorSpeed( bool IsHight )
        {
            if ( IsHight )
                this.iO.WriteOutputIO( "OUT_CV_DA", true );
            else
                this.iO.WriteOutputIO( "OUT_CV_DA", false );
        }

        bool IsCvRun() => this.iO.IsOn( "OUT_CV_RUN" );
        bool IsCvCWCCW() => this.iO.IsOn( "OUT_CV_CWCCW" );

        /// <summary>
        /// 입구 감지 로딩시 감속 사용
        /// </summary>
        /// <returns></returns>
        bool IsDetectedLoadStart() => this.iO.IsOn( "IN_CV_DETECT_00" );

        /// <summary>
        /// 실물 감지
        /// </summary>
        /// <returns></returns>
        public bool IsDetectedCenter() => this.iO.IsOn( "IN_CV_DETECT_01" );

        bool IsDetectedLoadStop() => this.iO.IsOn( "IN_CV_DETECT_02" );
        bool IsInverterError() => !this.iO.IsOn( "IN_CV_ERROR" ); //Normal Close로 생각 됨.
        bool IsLifterPositinCheck() => this.iO.IsOn( "IN_LIFTER_POSITION_DETECT" );
        bool IsLifterDuplication() => this.iO.IsOn( "IN_LIFTER_DUPLICATION_DETECT" );
        bool IsPIOInterLockOn() => this.iO.IsOn( "OUT_PIO_INTERLOCK" );

        public int ConveyorLoad()
        {
            if ( IsDetectedCenter() )
                return 9;

            logger.D( "[Manual Load] - Conveyor On" );
            OnOffConveyor( true, true );

            bool isStartDetected = false;
            long sTime = SwUtils.CurrentTimeMillis;
            while ( true )
            {
                if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) ) //Wait 20Sec
                {
                    OnOffConveyor( false, true );
                    return 10;
                }

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

                if ( !this.IsDetectedLoadStart() && isStartDetected )
                {
                    this.SetConveyorSpeed( false );
                    logger.D( "[Manual Load] - Conveyor Slow State" );
                }

                if ( IsDetectedLoadStop() )
                    break;
            }
            OnOffConveyor( false );
            logger.D( "[Manual Load] - Conveyor Off" );

            return 0;
        }

        public int ConveyorUnload()
        {
            if ( !IsDetectedCenter() )
                return 11;

            logger.D( "[Manual Unload] - Conveyor On" );
            OnOffConveyor( true, false );

            long sTime = SwUtils.CurrentTimeMillis;
            while ( true )
            {
                if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) ) //Wait 20Sec
                {
                    OnOffConveyor( false, false );
                    return 12;
                }

                if ( !IsDetectedLoadStart() && !IsDetectedCenter() )
                    break;
            }
            Thread.Sleep( 2000 );
            OnOffConveyor( false );
            logger.D( "[Manual Unload] - Conveyor Off" );

            return 0;
        }

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

            this.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
            var pioTimeout = sql.ConfigDal.GetValueToInt( ConstString.PIOTimeOut );

            if ( this.IsInverterError() )
                return 16;

            if ( this.IsLifterPositinCheck() )
                return 14;

            if ( !this.IsLifterDuplication() )
            {
                this.OnFailReport?.Invoke( eFailCode.Load_PortHasNotCarrier );
                return 0;
            }

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

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

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

            if ( !this.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.SetConveyorSpeed( true );
            this.OnOffConveyor( true , true );
            this.iO.WriteOutputIO( "OUT_PIO_RECEIVE_RUN" , true );
            loggerPIO.I( "[Vehicle] - Conveyor Run" );
            this.OnConveyorStart?.Invoke( true );

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

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

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

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

                if ( !this.IsDetectedLoadStart() && isStartDetected )
                    this.SetConveyorSpeed( false );

                if ( this.IsDetectedLoadStop() ) break;

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

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

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

            this.iO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE" , true );
            this.iO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE" , false , 1000 );

            this.OnLoadComplete?.Invoke();
#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.iO.WriteOutputIO( "OUT_PIO_READY", true );
            loggerPIO.I( "[Vehicle] - 1 Ready On" );

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

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

            Thread.Sleep( 1000 );
            this.OnUnloadComplete?.Invoke();
#else
            var pioTimeout = sql.ConfigDal.GetValueToInt( ConstString.PIOTimeOut );

            if ( this.IsInverterError() )
                return 16;

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

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

            if ( this.IsLifterPositinCheck() )
                return 13;

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

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

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

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

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

            this.SetConveyorSpeed( true );
            this.OnOffConveyor( true );
            this.OnConveyorStart?.Invoke( false );

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

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

                if ( this.iO.IsOn( "IN_PIO_RECEIVE_COMPLITE" ) )
                    break;
            }

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

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

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

            this.OnUnloadComplete?.Invoke();
#endif

            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.iO.OutputOff( x ); } );
        }

        #endregion

        #endregion

        #region Hardware Create Method
        void CreateSteering()
        {
            this.steering = new Steering( this.iO, this.sql, this.eventAggregator );
            this.steering.OnSteeringError += Steering_OnSteeringError;
            this.steering.PropertyChanged += Steering_PropertyChanged;
        }
        void CreateClamp()
        {
            this.clamp = new Clamp( this.sql, this.eventAggregator );
            this.clamp.Init();
            this.clamp.PropertyChanged += Clamp_PropertyChanged;
        }
        void CreateDrive()
        {
            this.motion = new GSIMotion( this.sql );
            this.motion.PropertyChanged += Motion_PropertyChanged;
        }
        void CreateBMUManager()
        {
            this.bMUManager = new BMUManager();
            this.bMUManager.BMUConfig = new Serial.BatteryTabos.Config() { ID = "0" };
            this.bMUManager.OnConnect += BMUManager_OnConnect;
            this.bMUManager.OnDisconnect += BMUManager_OnDisconnect;
            this.bMUManager.OnChangedReceivedData += BMUManager_OnChangedReceivedData;
            this.bMUManager.OnFirstColtd += BMUManager_OnFirstColtd;

            this.bMUManager.Connect(BMUManager.eCANSelect.Advantech);
        }

        private void BMUManager_OnDisconnect( string obj, bool state )
        {
            this.BatteryIsConnect = state;
            this.OccurVehicleAlarm( 32 );
        }

        private void BMUManager_OnConnect( string obj, bool state )
        {
            this.BatteryIsConnect = state;
        }

        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 * obj.Scale;
                    break;
                case eDataKind.Current:
                    this.BatteryCurrent = (double)obj.Value * obj.Scale;
                    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 / obj.Scale;
                    break;
                case eDataKind.DisChargeCompleteTime:
                    if ( obj.Value == null || obj.Value <= 0 )
                        return;
                    this.BatteryDisChargeTime = ( double )obj.Value / obj.Scale;
                    break;
                case eDataKind.SOC:
                    this.BatteryStateOfCharge = ( double )obj.Value * obj.Scale;
                    break;
                case eDataKind.SOH:
                    this.BatteryStateOfHealth = ( double )obj.Value * obj.Scale;
                    break;
                case eDataKind.ResidualCapacity:
                    this.BatteryCapacity = ( double )obj.Value * obj.Scale;
                    break;
                case eDataKind.ResidualEnergy:
                    this.BatteryEnergy = ( double )obj.Value * obj.Scale;
                    break;
                case eDataKind.Temperature:
                    this.BatteryTemperature = ( double )obj.Value * obj.Scale;
                    break;
                default:
                    break;
            }
        }

        #endregion

        #region Help Method
        /// <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.iO.OutputOff( b );
                 else
                     this.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.iO.IsOn( b, false ) )
                     bitArray.Set( bitIndex, false );
                 else
                     bitArray.Set( bitIndex, true );
                 bitIndex++;
             } );

            return BitUtils.ChgInt32( bitArray );
        }

        void OccurVehicleAlarm( int alarmID )
        {
            this.MachineMode = eMachineMode.LocalMode;
            this.VehicleStateProperty = eVehicleState.Abnormal;
            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;
        }
        #endregion

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

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

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

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

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

        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;
            }
        }

        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 )
        {
            if ( bit.Tag.Equals( "IN_CV_DETECT_01" ) )
            {
                this.IsContain = bit.IsBitOn;
            }
        }

        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
    }
}