OHV/Dev/OHV/VehicleControlSystem/ControlLayer/MQ/ZmqManager.cs

using GSG.NET.Concurrent;
using GSG.NET.Extensions;
using GSG.NET.Logging;
using GSG.NET.Utils;
using NetMQ;
using NetMQ.Monitoring;
using NetMQ.Sockets;
using OHV.Common.Model;
using OHV.Common.Shareds;
using Quartz.Xml;
using System;
using System.Collections;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using VehicleControlSystem.ControlLayer.IO;
using VehicleControlSystem.ControlLayer.Serial.BatteryTabos;
using VehicleControlSystem.ControlLayer.Serial.DataModel;
using VehicleControlSystem.Managers;

namespace VehicleControlSystem.ControlLayer.MQ
{
    public enum eDriveControl
    {
        None = 0,
        ParameterSave,
        ResetFault,
        CancelRoute,
        ServoOn,
        ServoOff,
        ClearWarning = 9,
    }

    public enum eDriveState
    {
        ServoOff,
        ServoOn,
        Fault,
    }

    public class ZmqMessage
    {
        public string Key { get; set; }
        public string Message { get; set; }
    }

    public class ZmqManager : ControlObjectBase, IDisposable
    {
        Logger logger = Logger.GetLogger();

        PublisherSocket pub = null;
        SubscriberSocket sub = null;
        RequestSocket req = null;
        ResponseSocket rep = null;

        NetMQPoller poller = null;
        NetMQPoller subPoller = null;
        NetMQMonitor monitor = null;
        NetMQMonitor pubMonitor = null;

        BMUManager bMUManager = null;
        Steering steering = null;

        ThreadCancel threadCancel = new ThreadCancel();

        BlockingCollection<ZmqMessage> sendQueue = new BlockingCollection<ZmqMessage>( new ConcurrentQueue<ZmqMessage>() );

        string endServer = "tcp://127.0.0.1:5567";

        private bool isReqConnected = false;
        public bool IsReqConnected
        {
            get { return isReqConnected; }
            set { isReqConnected = value; }
        }

        #region Properties

        private eDriveState driveState = eDriveState.ServoOff;

        public eDriveState DriveState
         {
            get { return driveState; }
            set 
            {
                SetField(ref this.driveState, value);
            }
        }


        private eSteeringState requestSteering = eSteeringState.None;
        public eSteeringState RequestSteering
        {
            get { return requestSteering; }
            set { SetField( ref this.requestSteering, value, false ); }
        }

        private bool isDriveMoving = false;
        public bool IsDriveMoving
        {
            get { return isDriveMoving; }
            set
            {
                if ( SetField( ref isDriveMoving, value ) )
                    Publish( "DriveStart", value.ToString() );
            }
        }

        private eOperatationMode operatationMode;
        public eOperatationMode OperationMode
        {
            get { return operatationMode; }
            set { operatationMode = value; }
        }

        private int currentPointNo;
        public int CurrentPointNo
        {
            get { return currentPointNo; }
            set
            {
                if ( SetField( ref this.currentPointNo, value ) )
                    Publish( "CurrentPoint", value.ToString() );
            }
        }

        private int segmentID;
        public int SegmentID
        {
            get { return segmentID; }
            set
            {
                if ( SetField( ref this.segmentID, value ) )
                {
                    var segment = RouteManager.Instance.Segments.Where( s => s.ID == value ).FirstOrDefault();
                    if ( segment == null )
                        return;

                    Publish( "CurrentRailFrom", segment.route_type );
                }
            }
        }

        private double frontLoadFactor;
        public double FrontLoadFactor
        {
            get { return frontLoadFactor * 0.1; }
            set { SetField( ref this.frontLoadFactor, value ); }
        }

        private double rearLoadFactor;
        public double RearLoadFactor
        {
            get { return rearLoadFactor * 0.1; }
            set { SetField( ref this.rearLoadFactor, value ); }
        }

        private double frontRPM;
        public double FrontRPM
        {
            get { return Math.Truncate( frontRPM * 10 ) / 10; }
            set { SetField( ref this.frontRPM, value ); }
        }

        private double rearRPM;
        public double RearRPM
        {
            get { return Math.Truncate( rearRPM * 10 ) / 10; }
            set { SetField( ref this.rearRPM, value ); }
        }

        private string currentMCR;
        public string CurrentMCR
        {
            get { return currentMCR; }
            set
            {
                SetField( ref this.currentMCR, value );
            }
        }

        //Drive State Code
        private int warningCode;
        public int WarningCode
        {
            get { return warningCode; }
            set { SetField( ref this.warningCode, value ); }
        }

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

        private string warningMessage;
        public string WarningMessage
        {
            get { return warningMessage; }
            set { SetField( ref this.warningMessage, value ); }
        }

        private string faultMessage;
        public string FaultMessage
        {
            get { return faultMessage; }
            set { SetField( ref this.faultMessage, value ); }
        }

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

        private double linearSpeed;

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

        private double curveSpeed;

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

        private double accelSpeed;

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

        private double decelSpeed;

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

        private double creepSpeed;

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

        private double creepDistance;

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

        private double jogSpeed;

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

        private bool isDriveOn;

        public bool IsDriveOn
        {
            get { return isDriveOn; }
            set 
            {
                if(SetField(ref this.isDriveOn, value))
                {

                }
            }
        }

        #endregion

        IO.EzIO iO = null;

        public ZmqManager( BMUManager bMU, Steering steering, IIO io )
        {
            this.iO = io as EzIO;

            NetMQ.NetMQConfig.Cleanup();

            this.bMUManager = bMU;
            this.steering = steering;
        }

        public void Init()
        {
            pub = new PublisherSocket();

            this.pubMonitor = new NetMQMonitor(pub, "inproc://pub.inproc", SocketEvents.Accepted);
            this.pubMonitor.Accepted += (s, e) =>
            {
                PublishSyncData();
                //logger.D("pub Connected");
            };
            this.pubMonitor.StartAsync();
            pub.Bind( "tcp://127.0.0.1:5570" );

            sub = new SubscriberSocket();
            sub.Connect( "tcp://127.0.0.1:5565" );
            sub.Subscribe( "" ); //All
            sub.ReceiveReady += Sub_ReceiveReady;

            rep = new ResponseSocket();
            rep.Bind( "tcp://127.0.0.1:5568" );
            rep.ReceiveReady += Rep_ReceiveReady;

            req = new RequestSocket();
            this.monitor = new NetMQMonitor( req, "inproc://rep.inproc", SocketEvents.Disconnected | SocketEvents.Connected );
            this.monitor.Connected += ( s, a ) =>
            {
                this.SynchronizedParmeter();
            };

            this.monitor.Disconnected += ( s, a ) => { this.IsReqConnected = false; };
            this.monitor.StartAsync();
            req.Connect( "tcp://127.0.0.1:5567" );

            this.poller = new NetMQPoller { this.rep };
            this.poller.RunAsync();

            this.subPoller = new NetMQPoller { this.sub };
            this.subPoller.RunAsync();

            this.bMUManager.OnChangedReceivedData += BMUManager_OnChangedReceivedData;
        }

        void PublishSyncData()
        {
            if ( this.iO.IsOn("IN_CV_DETECT_01"))
                Publish("Contain", true.ToString());
            else
                Publish("Contain", false.ToString());
        }

        private void SynchronizedParmeter()
        {
            this.IsReqConnected = true;

            CurrentPointNo = GetCurrentPointNo();
            SetCurrentSteeringState(this.steering.GetSteeringState());

            this.LinearSpeed = this.GetDriveLinerSpeed(); 
            this.CurveSpeed = this.GetDriveCurveSpeed();
            this.JogSPeed = this.GetJogSpeed();
            this.AccelSpeed = this.GetAccelSpeed();
            this.DecelSpeed = this.GetDecelSpeed();
            this.CreepSpeed = this.GetCreepSpeed();
            this.CreepDistance = this.GetCreepDistance();
        }

        private void BMUManager_OnChangedReceivedData( Serial.DataModel.ReceivedData obj )
        {
            try
            {
                var kind = CastTo<eDataKind>.From<Enum>( obj.DataKind );
                switch ( kind )
                {
                    case eDataKind.Current:
                        this.pub.SendMoreFrame( "BatteryCurrent" ).SendFrame( obj.Value.ToString() );
                        break;
                    case eDataKind.Voltage:
                        this.pub.SendMoreFrame( "BatteryVoltage" ).SendFrame( obj.Value.ToString() );
                        break;
                    case eDataKind.BatteryState:
                    case eDataKind.ChargeCompleteTime:
                    case eDataKind.DisChargeCompleteTime:
                        break;
                    case eDataKind.SOC:
                        this.pub.SendMoreFrame( "BatterySOC" ).SendFrame( obj.Value.ToString() );
                        break;
                    case eDataKind.SOH:
                    case eDataKind.ResidualCapacity:
                    case eDataKind.ResidualEnergy:
                    case eDataKind.Temperature:
                        break;
                    default:
                        break;
                }

            }
            catch ( Exception ex )
            {
                logger.E( ex );
            }
        }

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

            this.monitor.Stop();
            this.monitor.Dispose();

            this.pubMonitor.Stop();
            this.pubMonitor.Dispose();

            this.poller.Stop();
            this.poller.Dispose();

            this.subPoller.Stop();
            this.subPoller.Dispose();

            this.sub.Dispose();
            this.req.Dispose();
            this.rep.Dispose();
            this.pub.Dispose();
        }

        object lockPulish = new object();
        public void Publish( string key, string val )
        {
            lock ( lockPulish )
                this.pub.SendMoreFrame( key ).SendFrame( val );
        }

        private void Rep_ReceiveReady( object sender, NetMQSocketEventArgs e )
        {
            var ret = e.Socket.ReceiveMultipartStrings();
            logger.D( $"[rep] - {ret[1]}" );

            //rep.SendMoreFrame( ret[0] ).SendFrame( $"ok/{ret[1]}" ); //응답처리 부터 하자.

            try
            {
                if ( ret[0].Equals( "setm" ) )
                {
                    var v = ret[1].Split( '/' );

                    rep.SendMoreFrame( "setm" ).SendFrame( $"ok/{v[0]}" ); //응답처리 부터 하자.

                    switch ( v[0] )
                    {
                        case "3050":
                            {
                                logger.D( $"[Steering_Rep] - 3050/{v[0]}" );
                                switch ( v[1] )
                                {
                                    case "1":
                                        this.RequestSteering = eSteeringState.Left;
                                        break;
                                    case "-1":
                                        this.RequestSteering = eSteeringState.Right;
                                        break;
                                    default:
                                        this.RequestSteering = eSteeringState.None;
                                        break;
                                }
                            }
                            break;

                        case "111":
                            {
                                if ( v[1].Equals( "1" ) )
                                    OperationMode = eOperatationMode.AutoMode;
                                else
                                    OperationMode = eOperatationMode.ManualMode;
                            }
                            break;

                        case "113":
                            {
                            }
                            break;

                        case "4012":

                            if ( v[1].Equals( "3" ) )
                                IsDriveMoving = true;
                            else
                                IsDriveMoving = false;
                            break;

                        case "3009":
                            {
                                int iV = 0;
                                int.TryParse( v[1], out iV );
                                this.CurrentPointNo = iV;
                                int.TryParse( v[2], out iV );
                                this.SegmentID = iV;

                                //TODO: 6.29 DriveAutoReadyState
                                if ( v[3].Equals( "0" ) )
                                    IsCanStanbyLocation = false;
                                else
                                    IsCanStanbyLocation = true;
                            }
                            break;

                        case "4094":
                            {
                                var mb = new MemoryBuffer();
                                for ( int i = 1; i < v.Length; i++ )
                                {
                                    var _2b = v[i].Replace( "0x", "" );
                                    mb.Append( HexaUtils.StringToByte( _2b ) );
                                }
                                var ba = new BitArray( mb.ToBytes );
                            }
                            break;

                        case "4098":
                            break;

                        default:
                            break;
                    }
                }
                else if ( ret[0].Equals( "getm" ) )
                {
                    var v = ret[1].Split( '/' );
                    switch ( v[0] )
                    {
                        case "3060":
                            {
                                switch ( this.steering.GetSteeringState() )
                                {
                                    case eSteeringState.None:
                                        rep.SendMoreFrame( "getm" ).SendFrame( $"ok/{v[0]}/0" );
                                        break;
                                    case eSteeringState.Left:
                                        rep.SendMoreFrame( "getm" ).SendFrame( $"ok/{v[0]}/1" );
                                        break;
                                    case eSteeringState.Right:
                                        rep.SendMoreFrame( "getm" ).SendFrame( $"ok/{v[0]}/-1" );
                                        break;
                                    default:
                                        rep.SendMoreFrame( "getm" ).SendFrame( $"ok/{v[0]}/0" );
                                        break;
                                }
                            }
                            break;
                        default:
                            break;
                    }
                }
                else if ( ret[0].Equals( "sync" ) )
                {
                    var v = ret[1].Split( '/' );
                    rep.SendMoreFrame( ret[0] ).SendFrame( $"ok/{ret[1]}" ); //응답처리 부터 하자.

                }
                else if ( ret[0].Equals( "parameter" ) )
                {
                    var v = ret[1].Split( '/' );
                    rep.SendMoreFrame( ret[0] ).SendFrame( $"ok/{ret[1]}" ); //응답처리 부터 하자.

                }
                else
                    rep.SendMoreFrame( ret[0] ).SendFrame( $"ok/{ret[1]}" );

            }
            catch ( Exception exception )
            {
                logger.E( exception );
            }
        }

        private void Sub_ReceiveReady( object sender, NetMQ.NetMQSocketEventArgs e )
        {
            var ret = e.Socket.ReceiveMultipartStrings();
            //logger.I( ret );

            var vl = ret[1].Split( '/' );

            switch ( ret[0] )
            {
                case "7028":
                    {
                        var ll = Convert.ToDouble( vl[0] );
                        this.FrontRPM = Math.Truncate( ll * 100 ) / 100;
                        var li = Convert.ToDouble( vl[1] );
                        this.RearRPM = Math.Truncate( li * 100 ) / 100;
                    }
                    break;

                case "7030":
                    {
                        var ll = Convert.ToDouble( vl[0] );
                        this.FrontLoadFactor = Math.Truncate( ll * 100 ) / 100;
                        var rL = Convert.ToDouble( vl[1] );
                        this.RearLoadFactor = Math.Truncate( rL * 100 ) / 100;
                    }
                    break;

                case "7121":
                    this.CurrentMCR = vl[0];
                    break;

                case "111": //0=StartUp, 1=DeviceOpened, 2=DiviceOpenFailed, 3=ManualOP, 4=AutomaticOp, 5=DeviceCloseed, 6=Finished
                    break;

                case "3010": //진행중의 길의 모양, 0이면 정지 상태
                    {
                        var v = ret[1].Split( ';' );
                        switch ( v[0] )
                        {
                            case "1":
                                //this.RequestSteering = eSteeringState.Left;
                                break;
                            case "-1":
                                //this.RequestSteering = eSteeringState.Right;
                                break;
                            default:
                                //this.RequestSteering = eSteeringState.None;
                                break;
                        }
                    }
                    break;
                //case "Tag":
                //    {
                //        var v = ret[1].Split( ';' );
                //        this.CurrentPointNo = Convert.ToInt16(v[0]);
                //    }
                //break;

                default:
                    break;
            }
        }

        object lockObj = new object();
        bool Request( string topic, string value, out string response )
        {
            logger.D( $"req - {value}" );

            List<string> repll = new List<string>();
            response = string.Empty;


            lock ( lockObj )
            {
                if ( !this.IsReqConnected )
                    return false;
                try
                {
                    //if ( this.req.HasOut )
                    this.req.SendMoreFrame( topic ).SendFrame( value );

                    if ( this.req.TryReceiveMultipartStrings( TimeSpan.FromSeconds( 5 ), ref repll ) )
                        return true;
                    else
                        return false;

                }
                catch ( Exception e )
                {
                    logger.E( e );
                    this.req.Disconnect( "tcp://127.0.0.1:5567" );
                    //this.monitor.Stop();
                    //this.monitor.Dispose();

                    //this.monitor = new NetMQMonitor( req, "inproc://rep.inproc", SocketEvents.Disconnected | SocketEvents.Connected );
                    //this.monitor.Connected += ( s, a ) => { this.IsReqConnected = true; CurrentPointNo = GetCurrentPointNo(); };
                    //this.monitor.Disconnected += ( s, a ) => { this.IsReqConnected = false; };
                    //this.monitor.StartAsync();

                    req.Connect( "tcp://127.0.0.1:5567" );

                    return false;
                }
            }
        }

        bool SetRequest( string index, string value )
        {
            logger.D( $"req - {index} / {value}" );

            List<string> repll = new List<string>();

            if ( !this.IsReqConnected )
                return false;

            lock ( lockObj )
            {

                try
                {
                    //if ( this.req.HasOut )
                    this.req.SendMoreFrame( "setm" ).SendFrame( index + "/" + value );

                    if ( !this.req.TryReceiveMultipartStrings( TimeSpan.FromSeconds( 5 ), ref repll ) )
                        return false;
                }
                catch ( Exception e )
                {
                    logger.E( e );
                    this.req.Disconnect( "tcp://127.0.0.1:5567" );
                    //this.monitor.Stop();
                    //this.monitor.Dispose();

                    //this.monitor = new NetMQMonitor( req, "inproc://rep.inproc", SocketEvents.Disconnected | SocketEvents.Connected );
                    //this.monitor.Connected += ( s, a ) => { this.IsReqConnected = true; CurrentPointNo = GetCurrentPointNo(); };
                    //this.monitor.Disconnected += ( s, a ) => { this.IsReqConnected = false; };
                    //this.monitor.StartAsync();

                    req.Connect( "tcp://127.0.0.1:5567" );

                    return false;
                }
            }

            return true;
        }

        bool GetRequst( string index, out List<string> vll )
        {
            logger.D( $"req - {index}" );

            vll = new List<string>();

            if ( !this.IsReqConnected )
                return false;

            lock ( lockObj )
            {

                try
                {
                    //if ( this.req.HasOut )
                    this.req.SendMoreFrame( "getm" ).SendFrame( index );

                    List<string> repll = new List<string>();

                    if ( !this.req.TryReceiveMultipartStrings( TimeSpan.FromSeconds( 5 ), ref repll ) )
                        return false;

                    repll.RemoveAt( 0 );

                    var ret = repll[0].Split( '/' );
                    repll.Clear();

                    foreach ( var item in ret )
                    {
                        if ( item.Equals( "ok" ) || item.Equals( "nok" ) || item.Equals( index ) )
                            continue;
                        repll.Add( item );
                    }
                    vll = repll;
                }
                catch ( Exception e )
                {
                    logger.E( e );
                    this.req.Dispose();
                    req = new RequestSocket();
                    req.Connect( "tcp://127.0.0.1:5567" );
                    return false;
                }
            }

            return true;
        }

        #region RequestSocket
        List<string> GetRequest( string index )
        {
            var rll = RequestDrive( "getm", index );
            if ( rll.Count == 0 )
                return rll;

            rll.RemoveAt( 0 );

            var ret = rll[0].Split( '/' );

            List<string> retll = new List<string>();
            foreach ( var item in ret )
            {
                if ( item.Equals( "ok" ) || item.Equals( "nok" ) || item.Equals( index ) )
                    continue;

                retll.Add( item );
            }

            return retll;
        }

        List<string> RequestDrive( string key, string msg )
        {
            logger.D( $"[Request] - {key} / {msg}" );

            var client = this.CreateRequestSocket( endServer );
            client.SendMoreFrame( key ).SendFrame( msg );

            var ll = new List<string>();

            //if ( !client.Poll( TimeSpan.FromMilliseconds( 2000 ) ) )
            //    isSuccess = false;

            if ( !client.TryReceiveMultipartStrings( TimeSpan.FromSeconds( 1 ), ref ll ) )
                logger.D( $"[Reply Timeout] - {key} / {msg}" );

            this.TerminateRequestSocket( client );

            return ll;
        }

        RequestSocket CreateRequestSocket( string endServer )
        {
            var guid = Guid.NewGuid();
            var client = new RequestSocket
            {
                Options =
                {
                    Linger = TimeSpan.Zero,
                    Identity = Encoding.Unicode.GetBytes(guid.ToString())
                }
            };

            client.Connect( endServer );
            //client.ReceiveReady += Client_ReceiveReady;

            return client;
        }

        void TerminateRequestSocket( RequestSocket client )
        {
            client.Disconnect( this.endServer );
            client.Close();
        }

        private void Client_ReceiveReady( object sender, NetMQSocketEventArgs e )
        {
            var rsp = e.Socket.ReceiveMultipartStrings();

            logger.D( $"[Reply] - {rsp[0]} / {rsp[1]}" );

            switch ( rsp[0] )
            {
                case "3009":
                    break;
                default:
                    break;
            }
        }
        #endregion

        #region Get Request Method
        public int GetWarningCode()
        {
            int ret = 0;

            var rll = this.GetRequest( "4094" );
            if ( rll.Count == 0 )
                return 0;

            if ( !int.TryParse( rll[0], out ret ) )
                logger.E( $"[Get Warning Code] - Try Parse Error" );

            return ret;
        }

        public int GetFaultCode()
        {
            int ret = 0;

            var rll = this.GetRequest( "4098" );
            if ( rll.Count == 0 )
                return 0;

            if ( !int.TryParse( rll[0], out ret ) )
                logger.E( $"[Get Fault Code] - Try Parse Error" );

            return ret;
        }

        public eOperatationMode GetOperationMode()
        {
            var rll = GetRequest( "111" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Get Operation Mode] - Response Time Out" );
                return eOperatationMode.InitialMode;
            }

            if ( rll[0].Equals( "4" ) )
                return eOperatationMode.AutoMode;
            else
                return eOperatationMode.ManualMode;
        }

        public int GetCurrentPointNo()
        {
            int ret = 0;

            var rll = this.GetRequest( "3009" );
            if ( rll.Count == 0 )
                return 0;

            if ( !int.TryParse( rll[0], out ret ) )
                logger.E( $"[Get Current Point] - Try Parse Error" );

            return ret;
        }
        #endregion

        #region Set Request Method
        internal int SetDriveControl( eDriveControl control )
        {
            int result = ConstInt.EXECUTE_SUCCESS;

            switch ( control )
            {
                case eDriveControl.None:
                    break;
                case eDriveControl.ParameterSave:
                    break;
                case eDriveControl.ResetFault:
                    break;
                case eDriveControl.CancelRoute:
                    break;
                case eDriveControl.ServoOn:
                    break;
                case eDriveControl.ServoOff:
                    break;
                case eDriveControl.ClearWarning:
                    break;
                default:
                    break;
            }

            var rll = this.RequestDrive( "setm", $"4011/{( (int)control ).ToString()}" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Drive Control] - Response Time Out" );
                return 38;
            }

            return result;
        }

        internal int SetStartMove( bool isArrayCommand = false )
        {
            string cmd = string.Empty;
            if ( isArrayCommand )
                cmd = "3";
            else
                cmd = "2";

            var rll = this.RequestDrive( "setm", $"3031/{cmd}" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Start Move] - Response Time Out" );
                return 38;
            }

            return ConstInt.EXECUTE_SUCCESS;
        }

        internal int SetTargetPoint( string point )
        {
            var rll = this.RequestDrive( "setm", $"3033/{point}" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set TargetPoint] - Response Time Out" );
                return 38;
            }

            return ConstInt.EXECUTE_SUCCESS;
        }

        internal int SetTargetPointArray( string point )
        {
            var rll = this.RequestDrive( "setm", $"3034/{point}" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set TargetPoint] - Response Time Out" );
                return 38;
            }
            return ConstInt.EXECUTE_SUCCESS;
        }

        internal void SetObstruction( eObstacleState state )
        {
            string outStr = string.Empty;
            string val = string.Empty;

            switch ( state )
            {
                case eObstacleState.Normal:
                    val = "0";
                    break;
                   
                case eObstacleState.Abnormal:
                    val = "3";
                    break;

                case eObstacleState.Blocked:
                    val = "2";
                    break;

                case eObstacleState.Decelerate:
                    val = "1";
                    break;

                default:
                    break;
            }

            var rll = this.RequestDrive( "setm", $"5001/{val}" );
            if ( rll.Count == 0 )
                logger.E( $"[Set Obstruction] - Response Time Out" );
        }

        internal bool SetOperationState( eOperatationMode mode )
        {
            string outStr = string.Empty;
            string val = string.Empty;

            if ( mode == eOperatationMode.AutoMode )
                val = "2";
            else if ( mode == eOperatationMode.ManualMode )
                val = "3";
            else
                return false;

            var rll = this.RequestDrive( "setm", $"112/{val}" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Operation State] - Response Time Out" );
                return false;
            }

            return true;
        }

        internal void SetMCState( bool isOn )
        {
            string outStr = string.Empty;
            string val = string.Empty;

            if ( isOn )
                val = "1";
            else
                val = "0";

            var rll = this.RequestDrive( "setm", $"5000/{val}" );
            if ( rll.Count == 0 )
                logger.E( $"[Set MCState] - Response Time Out" );
        }

        internal void SetCurrentSteeringState( eSteeringState state )
        {
            string outStr = string.Empty;
            string val = string.Empty;

            switch ( state )
            {
                case eSteeringState.None:
                    val = "0";
                    break;
                case eSteeringState.Left:
                    val = "1";
                    break;
                case eSteeringState.Right:
                    val = "-1";
                    break;
                default:
                    break;
            }
            var rll = this.RequestDrive( "setm", $"3060/{val}" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Current Steering] - Response Time Out" );
                return;
            }

            logger.D( $"[Steering_Req] - 3060/{val}" );
        }

        internal void SetSafetyBumper()
        {
            var rll = this.RequestDrive( "setm", $"6105/1" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Safety Bumper] - Bumper is Pushed Time Out" );
                return;
            }

            logger.D( $"[Set Safety Bumper] - Bumper is Pushed" );
        }

        /// <summary>
        /// forward 1, backward -1
        /// </summary>
        /// <param name="isForward"></param>
        internal void SetJogMove( bool isForward = true )
        {
            string v = string.Empty;
            if ( isForward )
                v = "1";
            else
                v = "-1";

            logger.E( $"Jog Dir : <True +, False -> {isForward}" );

            var rll = this.RequestDrive( "setm", $"4091/{v}" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Jog Move] - Jog Move Time Out" );
                return;
            }
        }

        internal void SetQuickStop()
        {
            var rll = this.RequestDrive( "setm", $"4092/3" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Drive Command] - Quick Stop Time Out" );
                return;
            }

            logger.D( $"[Set Drive Command] - Set Quick Stop" );
        }

        #endregion

        #region Get/Set Parameter
        public double GetJogSpeed()
        {
            var rll = this.RequestDrive( "getm", "4053" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Jog Speed] - Jog Speed Time Out" );
                return 0d;
            }

            double ret = 0d;
            double.TryParse( rll[0], out ret );

            return ret;
        }

        /// <summary>
        /// 전진 후진 동시에 같은 값으로 설정.
        /// </summary>
        /// <param name="speed"></param>
        internal bool SetJogSpeed( double speed )
        {
            var rll = this.RequestDrive( "setm", $"4053/{speed}/{speed}" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Drive Speed] - Drive Speed Time Out" );
                return false;
            }

            //if ( !this.SetRequest( "6105", "1" ) )
            //    logger.E( $"[Set Safety Bumper] - Bumper is Pushed Time Out" );

            logger.D( $"[Set Jog Speed] - Set Jog Speed {speed}" );

            return true;
        }

        public double GetDriveLinerSpeed()
        {
            var rll = this.RequestDrive( "getm", "3016" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Drive Speed] - Drive Speed Time Out" );
                return 0d;
            }

            double ret = 0d;
            double.TryParse( rll[0], out ret );

            return ret;
        }
        public double GetDriveCurveSpeed()
        {
            var rll = this.RequestDrive( "getm", "3016" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Drive Speed] - Drive Speed Time Out" );
                return 0d;
            }

            double ret = 0d;
            double.TryParse( rll[1], out ret );

            return ret;
        }

        public double GetAccelSpeed()
        {
            //TODO: GetAccelSpeed(???) Value 입력 필요
            var rll = this.RequestDrive("getm", "9999");
            if(rll.Count == 0)
            {
                logger.E($"[Get Accel Speed] - Accel Speed Time Out");
                return 0d;
            }

            var ret = 0d;
            double.TryParse(rll[1], out ret);

            return ret;
        }

        public double GetDecelSpeed()
        {
            //TODO: GetDecelSpeed(??) Value 입력 필요
            var rll = this.RequestDrive("getm", "9999");
            if (rll.Count == 0)
            {
                logger.E($"[Get Decel Speed] - Decel Speed Time Out");
                return 0d;
            }

            var ret = 0d;
            double.TryParse(rll[1], out ret);

            return ret;
        }

        public double GetCreepSpeed()
        {
            //TODO: GetCreepSpeed(???) Vlaue 입력 필요
            var rll = this.RequestDrive("getm", "9999");
            if (rll.Count == 0)
            {
                logger.E($"[Get Creep Speed] - Creep Speed Time Out");
                return 0d;
            }

            var ret = 0d;
            double.TryParse(rll[1], out ret);

            return ret;
        }

        public double GetCreepDistance()
        {
            //TODO: GetCreepDistance(???) Vlaue 입력 필요
            var rll = this.RequestDrive("getm", "9999");
            if (rll.Count == 0)
            {
                logger.E($"[Get Creep Distance] - Creep Distance Time Out");
                return 0d;
            }

            var ret = 0d;
            double.TryParse(rll[1], out ret);

            return ret;
        }

        internal bool SetDriveSpeed( string linear, string curve )
        {
            var rll = this.RequestDrive("setm", $"3016/{linear}/{curve}");
            
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Drive Speed] - Drive Speed Time Out" );
                return false;
            }

            //if ( !this.SetRequest( "6105", "1" ) )
            //    logger.E( $"[Set Safety Bumper] - Bumper is Pushed Time Out" );

            logger.D( $"[Set Drive Speed] - Set Drive Speed Liner{linear}, Curve{curve}" );

            return true;
        }

        internal bool SetDriveSpeedProfile( string accel, string decel, string creep, string creepDistance )
        {
            var rll = this.RequestDrive( "setm", $"3013/{accel}/{decel}/{creep}/{creepDistance}" );
            if ( rll.Count == 0 )
            {
                logger.E( $"[Set Drive Speed] - Drive Speed Time Out" );
                return false;
            }

            //if ( !this.SetRequest( "6105", "1" ) )
            //    logger.E( $"[Set Safety Bumper] - Bumper is Pushed Time Out" );

            logger.D( $"[Set Drive Speed] - Set Drive Speed Accel {accel}, Decel {decel}, Creep{creep}/{creepDistance}" );

            return true;
        }

        #endregion
    }
}