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.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;
        public string CurrentTag
        {
            get { return currentTag; }
            set 
            { 
                if ( SetField(ref this.currentTag, value))
                    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); }
        }

        private eSteeringState steeringState;
        public eSteeringState SteeringState
        {
            get { return steeringState; }
            set { SetField(ref this.steeringState, 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;
        public eVehicleState VehicleStateProperty
        {
            get { return vehicleState; }
            set { SetField(ref this.vehicleState, value); }
        }

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

        public bool IsMoving { get; set; }
        public double BatteryVolt { get; set; }
        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;

        ThreadCancel cancel = new ThreadCancel();
        TaskCancel taskCancel = 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 );

            /*IO*/
            this.eventAggregator.GetEvent<IOControlPubSubEvent>().Unsubscribe( ReceiveIOControlEvent );
            this.eventAggregator.GetEvent<IOControlPubSubEvent>().Subscribe( ReceiveIOControlEvent );
        }

        private void ReceiveIOControlEvent( IOControlEventArgs _args )
        {
            if ( this.autoManager.OperationModeProperty != eOperatationMode.ManualMode )
                return;

            var msg = _args;
            if(msg.EventDir == IOControlEventArgs.eEventDir.ToBack)
            {
                switch ( msg.ControlKind )
                {
                    case IOControlEventArgs.eControlKind.USE:
                        break;
                    case IOControlEventArgs.eControlKind.ON:
                        break;
                    case IOControlEventArgs.eControlKind.OFF:
                        break;
                    case IOControlEventArgs.eControlKind.STATE:
                        break;
                }
            }
        }

        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:
                        {
                            DriveControlEventArgs 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;
                    default:
                        break;
                }
            }
        }

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

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

            ThreadStart();

            //TimerUtils.Once(5000, () => { this.CurrentPosition = 1000; });
        }

        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;

            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;

            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)
        {
            //TODO:[20/03/18 ys-hwang] Drive Assign
            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)
        {
            //TODO:[20/03/18 ys-hwang] Drive Jog Request
            var drive = string.Empty;

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

        void ReqCurrentPos()
        {
            //TODO:[20/03/18 ys-hwang] Drive Current Position Publish

            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();
                }
                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.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 SubCmd.eType.Move:
                            this.CurrentSubCommand = subCmd;
                            this.Move(subCmd);
                            break;

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

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

                        case SubCmd.eType.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()
        {
            //Clamp EStop
            this.clamp.ClampEStop();
            this.motion.EStop();

            this.OccurVehicleAlarm(23);
        }

        void Move(SubCmd sub)
        {
            if (this.MoveTo(sub.TargetID))
            {
                sql.SubCmdDAL.Delete(sub);
            }
            else
            {
                if (this.ObstacleStateProperty == eObstacleState.Blocked)
                {
                }
            }
        }

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

            this.OnMoveReady?.Invoke();

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

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

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

            bool result = Wait4MoveDone();
            this.IsMoving = false;
            //this.BuzzerOnOff(false);
            this.OnMoveFinish?.Invoke();

            this.VehicleStateProperty = eVehicleState.Idle;

            return result;
        }

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

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

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

                if (this.ObstacleStateProperty == eObstacleState.Blocked)
                    return false;

                //Todo: 이동중 명령이 삭제 되면 처리 할일들.
                //if (!sql.SubCmdDAL.HasK(this.CurrentSubCommand.ID))
                //{
                //}
            }

            return true;
        }

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

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

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

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

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

            this.VehicleStateProperty = eVehicleState.Idle;

            return true;
        }

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

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

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

            PIOClear();
            loggerPIO.I($"Start Unload PIO - [{sub.TargetID}]");
            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);
            this.iO.WriteOutputIO("OUT_PIO_SEND_COMPLITE", false, 1000);

            this.OnUnloadComplete?.Invoke();


            return true;
        }

        #endregion

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

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

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

            this.ObstacleStateProperty = eObstacleState.Normal;

            return false;
        }

        #endregion

        #region Mechanical Method

        #region Conveyor
        int OnOffConveyor(bool isOn, bool isCW = false)
        {
            if (IsInverterError())
                return 16;

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

        /// <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");
        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");

        int Load_Carrier()
        {
            if (IsDetectedCenter())
                return 9;

            OnOffConveyor(true, true);

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

                if (IsDetectedLoadStart())
                    break;
            }

            return 0;
        }

        int UnloadCarrier()
        {
            if (!IsDetectedLoadStart())
                return 11;

            OnOffConveyor(true, true);

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

                if (!IsDetectedLoadStart())
                    break;
            }

            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);
#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();

            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 );
            this.iO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE", false, 1000 );

            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();
        }
        void CreateDrive()
        {
            this.motion = new GSIMotion(this.sql);
            this.motion.PropertyChanged += Motion_PropertyChanged;
        }

        #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>
        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++;
            });

            return true;
        }

        int GetObstacleDetectPattern()
        {
            int bitIndex = 0;

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

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

            return BitUtils.ChgInt32(bitArray);
        }

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

    }
}