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.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
        private double currentPosition;

        /// <summary>
        /// Tag 위치
        /// </summary>
        public double CurrentPosition
        {
            get { return currentPosition; }
            set
            {
                if ( this.currentPosition == value ) return;

                this.currentPosition = value;
                this.OnCurrentPotisionChanged?.Invoke( (int)value );
            }
        }

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

            private set { }
        }
        public bool IsMoving { get; set; }
        public double BatteryVolt { get; set; }
        public bool IsError { get; set; }
        public bool IsContain { get { return this.IsDetectedCenter(); } }

        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<int> OnCurrentPotisionChanged;

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

        public event Action<eFailCode> OnFailReport;
        #endregion

        IIO iO = null;
        GSIMotion motion = null;
        SqliteManager sql = null;
        Clamp clamp = null;
        Steering steering = null;
        AutoManager autoManager = null;

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

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

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

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

        private eVehicleState vehicleState;
        public eVehicleState VehicleStateProperty
        {
            get { return vehicleState; }
            set { SetField( ref this.vehicleState, value ); }
        }

        IEventAggregator eventAggregator;

        public Vehicle( IIO io, SqliteManager sqliteManager, IEventAggregator ea, AutoManager auto )
        {
            this.iO = io;
            this.motion = new GSIMotion();
            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;

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

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

            ThreadStart();
        }

        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
        void ReqFaultReset(DriveControlEventArgs _args)
        {
            //TODO:[20/03/18 ys-hwang] Drive Assign
            var drive = 0;

            //var result = drive.ResetAmpFault();

            var msg = new DriveControlEventArgs
            {

            };

            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 = drive.CurrentPosition,
                     };
                     
                     //this.DriveControlEventPublish( msg );
                 }
             } );

            this.taskCancel.Add( task );
        }

        void ReqDriveOn( DriveControlEventArgs _args)
        {
            //TODO:[20/03/18 ys-hwang] Drive On
            var drive = "Drive Name";
            //drive.On();

            var msg = new DriveControlEventArgs
            {

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

            this.DriveControlEventPublish( msg );
        }

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

            var msg = new DriveControlEventArgs
            {
                
            };
            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.autoManager.ProcessAlarm(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.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();

            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 )
        {
            var route = sql.RouteDal.GetRoute( sub.TargetID );

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

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

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

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

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

            return true;
        }

        public bool UnloadCarrier( SubCmd sub )
        {
            var route = sql.RouteDal.GetRoute( sub.TargetID );

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

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

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

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

            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.autoManager.ProcessAlarm(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 Machanical 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;
        }

        void CreateClamp()
        {
            this.clamp = new Clamp( this.sql, this.eventAggregator );
            this.clamp.Init();
        }

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

        #endregion

        #region Event Subscribe 
        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

    }
}