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 NPOI.SS.Formula.Functions;
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.Security.Cryptography.X509Certificates;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
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, //1 = save parameters to file
        ResetFault, //2 = Reset Fault & Clear Warning
        EStop, //3 = Hard Stop
        ServoOn, //4 = Servo On(ManualOp)
        ServoOff, //5 = Servo Off(ManualOp)
        SlowStop, //6 = Soft Stop
        Initialize = 8 //8 = Get Ready for AutomaticOp(ManualOp에서 On Route여야 효과있음)
    }


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

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

        BMUManager bMUManager = null;
        Steering steering = null;
        IO.EzIO iO = null;
        string endServer = "tcp://127.0.0.1:5567";

        public event Action<BitArray> OnDriveAlarmBitArrayChg;
        NetMQQueue<MqPubilshMessage> pubQueue = new NetMQQueue<MqPubilshMessage>();

        #region MQ Sockets
        PublisherSocket pub = null;
        SubscriberSocket sub = null;
        RequestSocket req = null;
        ResponseSocket rsp = null;

        NetMQPoller poller = null;
        NetMQPoller subPoller = null;
        NetMQPoller pubQueuePoller = new NetMQPoller();
        NetMQMonitor monitor = null;
        #endregion

        //ThreadCancel threadCancel = new ThreadCancel();
        //BlockingCollection<MqPubilshMessage> sendQueue = new BlockingCollection<MqPubilshMessage>(new ConcurrentQueue<MqPubilshMessage>());

        #region Properties

        private bool isReqConnected = false;
        public bool IsReqConnected
        {
            get { return isReqConnected; }
            set { SetField(ref this.isReqConnected, value); }
        }

        //TODO: Drive상태 값 표현 필요. -> 7031 ? 4012 ? 
        private eDriveState driveState = eDriveState.ServoOff;

        public eDriveState DriveState
        {
            get
            {
                return this.driveState;
            }
            set
            {
                if (value == driveState)
                    return;

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

        public bool Isinitialized
        {
            get { return this.DriveState == eDriveState.InitializeComplete; }
        }

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

        /// <summary>
        /// 현재 SegmentID 를 받아서 장애물 감지 센서의 패턴을 변경한다.
        /// NS2 Line 확장시 사용 안함 패턴 번호를 주행에서 받아서 바로 적용.
        /// </summary>
        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);
                }
            }
        }

        /// <summary>
        /// 주행 측으로 받은 장애물 감지 패턴을 외부로 전달 한다.
        /// Vehicle Class 에서 처리 하기위함.
        /// </summary>
        private int reqObstaclesPattern;
        public int ReqObstaclesPattern
        {
            get { return reqObstaclesPattern; }
            set { SetField(ref this.reqObstaclesPattern, value); }
        }

        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 = "None";
        public string CurrentMCR
        {
            get { return currentMCR; }
            set
            {
                SetField(ref this.currentMCR, 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); }
        }

        #endregion


        public ZmqManager()
        {
            //NetMQ.NetMQConfig.Cleanup();
        }

        public void Init()
        {
            pub = new PublisherSocket();
            pub.Bind("tcp://127.0.0.1:5570");

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

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

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

            req = new RequestSocket();
            this.monitor = new NetMQMonitor(req, "inproc://rep.inproc", SocketEvents.Disconnected | SocketEvents.Connected);
            this.monitor.Connected += (s, a) =>
            {
                logger.D("Request MQ Connected");
                this.IsReqConnected = true;
                this.SynchronizedParmeter();
            };
            this.monitor.Disconnected += (s, a) =>
            {
                this.IsReqConnected = false;
                logger.D("Request MQ DisConnected");
            };

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

            pubQueue.ReceiveReady += (s, e) =>
            {
                MqPubilshMessage ret = null;
                try
                {
                    while (e.Queue.TryDequeue(out ret, TimeSpan.FromMilliseconds(10)))
                    {
                        if (ret == null) continue;
                        this.pub.SendMoreFrame(ret.Key).SendFrame(ret.Message);
                        LockUtils.Wait(10);
                    }
                }
                catch (Exception ex)
                {
                    logger.E(ex);
                    this.pub.Dispose();
                    this.pub = null;
                    this.pub = new PublisherSocket();
                    this.pub.Bind("tcp://127.0.0.1:5570");
                }
            };
            pubQueuePoller.Add(pubQueue);
            pubQueuePoller.RunAsync();

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

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

        public void AssignRefObject(BMUManager bMU, Steering steering, IIO io)
        {
            this.bMUManager = bMU;
            this.bMUManager.OnChangedReceivedData += BMUManager_OnChangedReceivedData;

            this.iO = io as EzIO;
            this.steering = steering;
        }

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

        public void SynchronizedParmeter()
        {
            try
            {
                CurrentPointNo = GetCurrentPoint();
                SegmentID = GetCurrentSegment();

                this.OperationMode = GetOperationMode();
                this.DriveState = this.GetCurrentDriveState();

                var frontLeft = this.iO.IsOn("IN_F_STEERING_DETECT_LEFT");
                var frontRight = this.iO.IsOn("IN_F_STEERING_DETECT_RIGHT");
                var rearLeft = this.iO.IsOn("IN_R_STEERING_DETECT_LEFT");
                var rearRight = this.iO.IsOn("IN_R_STEERING_DETECT_RIGHT");

                this.SetSteerDetectSensorState(frontLeft, frontRight, rearLeft, rearRight);
                //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();
                this.CurrentMCR = this.GetCurrentMCR();
                this.IsCanStanbyLocation = this.GetCanStandbyReadyLocation();
            }
            catch (Exception ex)
            {
                logger.E(ex);
            }
        }

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

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

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

            this.pubQueuePoller.Stop();
            this.pubQueuePoller.Dispose();

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

            this.sub.Dispose();
            this.req.Dispose();
            this.rsp.Dispose();
            this.pub.Dispose();
            this.pubQueue.Dispose();

            NetMQConfig.Cleanup(false);
        }

        object lockPulish = new object();
        public void Publish(string key, string val)
        {
            if (this.pubQueue.IsDisposed) return;

            var msg = new MqPubilshMessage { Key = key, Message = val };
            this.pubQueue.Enqueue(msg);
        }

        #region Receive Event
        private void Rsp_ReceiveReady(object sender, NetMQSocketEventArgs e)
        {
            try
            {
                var ret = e.Socket.ReceiveMultipartStrings();
                logger.D($"[rep] - {ret[1]}");

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

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

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

                    switch (v[0])
                    {
                        case "3040":
                            {
                                int iV = 0;
                                int.TryParse(v[1], out iV);
                                this.ReqObstaclesPattern = iV;
                            }
                            break;

                        case "3050": //! Steer Control
                            {
                                logger.D($"[Steering_Rep] - 3050/{v[0]}");
                                eSteerCommand command = eSteerCommand.None;
                                switch (v[1])
                                {
                                    case "LL":
                                        command = eSteerCommand.LL;
                                        break;
                                    case "LX":
                                        command = eSteerCommand.LX;
                                        break;
                                    case "XL":
                                        command = eSteerCommand.XL;
                                        break;
                                    case "RR":
                                        command = eSteerCommand.RR;
                                        break;
                                    case "RX":
                                        command = eSteerCommand.RX;
                                        break;
                                    case "XR":
                                        command = eSteerCommand.XR;
                                        break;
                                    case "00":
                                        command = eSteerCommand.OffOff;
                                        break;
                                    case "11":
                                        command = eSteerCommand.OnOn;
                                        break;
                                    case "1X":
                                        command = eSteerCommand.FrontOn;
                                        break;
                                    case "X1":
                                        command = eSteerCommand.RearOn;
                                        break;
                                    case "0X":
                                        command = eSteerCommand.FrontOff;
                                        break;
                                    case "X0":
                                        command = eSteerCommand.RearOff;
                                        break;
                                    default:
                                        break;
                                }
                                this.steering.RequestCommand(command);
                            }
                            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 "4013":
                            switch (v[1])
                            {
                                case "0":
                                    this.DriveState = eDriveState.ServoOff;
                                    break;
                                case "1":
                                    this.DriveState = eDriveState.ServoOn;
                                    break;
                                case "2":
                                    this.DriveState = eDriveState.Fault;
                                    break;
                                case "3":
                                    this.DriveState = eDriveState.InitializeComplete;
                                    break;
                                default:
                                    this.DriveState = eDriveState.None;
                                    break;
                            }
                            break;

                        case "3009":
                            {
                                int iV = 0;

                                int.TryParse(v[1], out iV);
                                if (iV < 0) iV = 0;
                                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 "502":
                            {
                                var mb = new MemoryBuffer();
                                for (int i = 1; i < v.Length; i++)
                                {
                                    var _2b = v[i].Replace("0x", "");
                                    mb.Append(HexaUtils.StringToByte(_2b));
                                }
                                var bitArray = new BitArray(mb.ToBytes);

                                this.OnDriveAlarmBitArrayChg?.Invoke(bitArray);
                            }
                            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:
                                        rsp.SendMoreFrame("getm").SendFrame($"ok/{v[0]}/0");
                                        break;
                                    case eSteeringState.Left:
                                        rsp.SendMoreFrame("getm").SendFrame($"ok/{v[0]}/1");
                                        break;
                                    case eSteeringState.Right:
                                        rsp.SendMoreFrame("getm").SendFrame($"ok/{v[0]}/-1");
                                        break;
                                    default:
                                        rsp.SendMoreFrame("getm").SendFrame($"ok/{v[0]}/0");
                                        break;
                                }
                            }
                            break;
                        default:
                            break;
                    }
                }
                else if (ret[0].Equals("sync"))
                {
                    var v = ret[1].Split('/');
                    rsp.SendMoreFrame(ret[0]).SendFrame($"ok/{ret[1]}"); //응답처리 부터 하자.
                }
                else if (ret[0].Equals("parameter"))
                {
                    var v = ret[1].Split('/');
                    rsp.SendMoreFrame(ret[0]).SendFrame($"ok/{ret[1]}"); //응답처리 부터 하자.
                }
                else
                    rsp.SendMoreFrame(ret[0]).SendFrame($"ok/{ret[1]}");

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

        private void Sub_ReceiveReady(object sender, NetMQ.NetMQSocketEventArgs e)
        {
            try
            {
                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 "7031":
                    //    //TODO: 상태값 "4012"에 데이터 들어옴 수정 필요.
                    //    var state = Convert.ToInt32(vl[0]);
                    //    this.DriveState = (eDriveState)state;
                    //    break;

                    //case "7020":
                    //    this.CurrentMCR = vl[0];
                    //    logger.D($" <7020> Current MCR : {this.CurrentMCR}");
                    //    break;

                    case "7120":
                        this.CurrentMCR = vl[0];
                        //logger.D($" <7120> Current MCR : {this.CurrentMCR}");
                        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;
                }

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

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

        object lockRequstMq = new object();
        List<string> RequestDrive(string key, string msg)
        {
            lock (this.lockRequstMq)
            {
                var ll = new List<string>();

                if (!this.IsReqConnected)
                {
                    logger.E($"[Request] - Disconnected - {key} / {msg} ");
                    return ll;
                }

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

                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 GetCurrentPoint()
        {
            var rll = this.RequestDrive("get", "3009/0");
            if (rll.Count == 0)
            {
                logger.E($"[Get Current Point] - Current Point Time Out");
                return 0;
            }

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');

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

            return ret;
        }

        public int GetCurrentSegment()
        {
            var rll = this.RequestDrive("get", "3009/1");
            if (rll.Count == 0)
            {
                logger.E($"[Get Current Segment] - Current Segment Time Out");
                return 0;
            }

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');

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

            return ret;
        }

        public bool GetCanStandbyReadyLocation()
        {
            var rll = this.RequestDrive("get", "3009/2");
            if (rll.Count == 0)
            {
                logger.E($"[Get On Route State] - On Route State Time Out");
                return false;
            }

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');

            return !rvel[3].Equals("0");
        }

        public BitArray GetAlarmBits()
        {
            var result = this.RequestDrive("getm", "502");
            if (result.Count == 0)
            {
                logger.E($"[GetDriveState] - DriveState TimeOut");
                return null;
            }

            var vl = result[1].Split('/');
            var mb = new MemoryBuffer();
            for (int i = 2; i < vl.Length; i++)
            {
                var _2b = vl[i].Replace("0x", "");
                mb.Append(HexaUtils.StringToByte(_2b));
            }
            var bs = mb.ToBytes;

            return new BitArray(bs);
        }

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

        #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.ServoOn:
                    break;
                case eDriveControl.ServoOff:
                    break;
                case eDriveControl.EStop:
                    break;
                case eDriveControl.SlowStop:
                    break;
                case eDriveControl.Initialize:
                    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;
            }

            logger.D($"Drive Control - 4011 - {control}");

            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;
            }
            logger.D($"[Set Start Move] - {cmd}");

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

            logger.D($"Set TargetPoint - {point}");

            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;
            }
            logger.D($"Set TargetPoint - {point}");

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

        public bool SetFrontDetectedVehicle(bool isOn = false)
        {
            string val = isOn ? "1" : "0";
            var rll = this.RequestDrive("setm", $"5002/{val}");
            if (rll.Count == 0)
            {
                logger.E($"[Set Front Detected Vehicle] - Response Time Out");
                return false;
            }

            return true;
        }

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

            switch (mode)
            {
                case eOperatationMode.ManualMode:
                    val = "3";
                    break;
                case eOperatationMode.AutoMode:
                    val = "2";
                    break;
                case eOperatationMode.InitialMode:
                    break;
                default:
                    logger.E($"[Set Operation State] - Mode Default");
                    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}");
        }

        /// <summary>
        /// 전후진 조향 감지 센서의 상태를 전달한다.
        /// </summary>
        /// <param name="isFrontOn"></param>
        internal void SetSteerDetectSensorState(bool frontLeft, bool frontRight, bool rearLeft, bool rearRight)
        {
            string frontState = string.Empty;
            string rearState = string.Empty;

            if (frontLeft && !frontRight)
                frontState = "L";
            if (!frontLeft && frontRight)
                frontState = "R";
            if (!frontLeft && !frontRight)
                frontState = "X";

            if (rearLeft && !rearRight)
                rearState = "L";
            if (!rearLeft && rearRight)
                rearState = "R";
            if (!rearLeft && !rearRight)
                rearState = "X";


            var rll = this.RequestDrive("setm", $"3060/{frontState}{rearState}");
            if (rll.Count == 0)
            {
                logger.E($"[Set Steering State] - Response Time Out");
                return;
            }

            logger.D($"[Steering_State] - 3060/{frontState}{rearState}");
        }

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

        /// <summary>
        /// 현재 위치에서 급정지를 실시 한다.
        /// </summary>
        internal void SetEStop()
        {
            //var rll = this.RequestDrive("setm", $"4011/0/3");
            //if (rll.Count == 0)
            //{
            //    logger.E($"[Set Drive Command] - E Stop Time Out");
            //    return;
            //}

            //logger.D($"[Set Drive Command] - Set E Stop");

            this.SetDriveControl(eDriveControl.EStop);
        }

        /// <summary>
        /// 감속 후 Tag 위에 정지 한다.
        /// </summary>
        internal void SetDecelStop()
        {
            //var rll = this.RequestDrive("setm", $"4011/0/6");
            //if (rll.Count == 0)
            //{
            //    logger.E($"[Set Drive Command] - Decel Stop Time Out");
            //    return;
            //}

            //logger.D($"[Set Drive Command] - Set Decel Stop");

            this.SetDriveControl(eDriveControl.SlowStop);
        }

        #endregion

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

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');
            double ret = 0d;
            double.TryParse(rvel[3], 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;
        }

        /// <summary>
        /// Drive Manual State 일 때 상태 
        /// 0 = ServoOff 상태임
        /// 1 = ServoOn 상태임
        /// 2 = Fault 상태임
        /// 3 = Ready 완료(AutomaticOp 전환 가능함)
        /// </summary>
        /// <returns></returns>
        public eDriveState GetCurrentDriveState()
        {
            //TODO: GetCurrentDriveState(???) Value 입력 필요
            eDriveState state = eDriveState.None;

            var result = this.RequestDrive("getm", "4013");
            if (result.Count == 0)
            {
                logger.E($"[GetDriveState] - DriveState TimeOut");
                return state;
            }

            var vl = result[1].Split('/');
            switch (vl[2])
            {
                case "0":
                    state = eDriveState.ServoOff;
                    break;
                case "1":
                    state = eDriveState.ServoOn;
                    break;
                case "2":
                    state = eDriveState.Fault;
                    break;
                case "3":
                    state = eDriveState.InitializeComplete;
                    break;
                default:
                    state = eDriveState.None;
                    break;
            }

            return state;
        }

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

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');
            double ret = 0d;
            double.TryParse(rvel[3], out ret);

            return ret;
        }

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

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');
            double ret = 0d;
            double.TryParse(rvel[3], out ret);

            return ret;
        }

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

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');
            double ret = 0d;
            double.TryParse(rvel[3], out ret);

            return ret;
        }

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

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');
            double ret = 0d;
            double.TryParse(rvel[3], out ret);

            return ret;
        }

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

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');
            double ret = 0d;
            double.TryParse(rvel[3], out ret);

            return ret;
        }

        public double GetCreepDistance()
        {
            var rll = this.RequestDrive("get", "3013/3");
            if (rll.Count == 0)
            {
                logger.E($"[Get Creep Distance] - Creep Distance Time Out");
                return 0d;
            }

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');
            double ret = 0d;
            double.TryParse(rvel[3], out ret);

            return ret;
        }

        public string GetCurrentMCR()
        {
            var rll = this.RequestDrive("getm", "7120");
            if (rll.Count == 0)
            {
                logger.E($"[Get CurrentMCR] - CurrentMCR Time Out");
                return string.Empty;
            }

            var sArray = rll[1].Split('/');

            return sArray[2];
        }

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

        internal double GetDriveDistance()
        {
            var rll = this.RequestDrive("get", "4061/0");
            if (rll.Count == 0)
            {
                logger.E($"[Get Creep Distance] - Creep Distance Time Out");
                return 0d;
            }

            // ok/index/subindex/value
            var rvel = rll[1].Split('/');
            double ret = 0d;
            double.TryParse(rvel[3], out ret);

            return Math.Truncate(ret * 1000) / 1000; ;
        }

        #endregion

        public void RequestAlarmBits()
        {
            var alarmBits = GetAlarmBits();
            if (alarmBits != null)
                this.OnDriveAlarmBitArrayChg?.Invoke(alarmBits);
        }

        public void AlarmBitTest(bool on)
        {
            string warningList = string.Empty;

            if (on)
                warningList = "4094/0xff/0x00/0x00/0x01";
            else
                warningList = "4094/0x00/0x00/0x00/0x00";

            var v = warningList.Split('/');

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

            var bs = mb.ToBytes;
            var bitA = new BitArray(bs);

            this.OnDriveAlarmBitArrayChg?.Invoke(bitA);
        }

        private void BMUManager_OnChangedReceivedData(Serial.DataModel.ReceivedData obj)
        {
            try
            {
                var kind = CastTo<eDataKind>.From<Enum>(obj.DataKind);
                switch (kind)
                {
                    case eDataKind.Current:
                        Publish("BatteryCurrent", obj.Value.ToString());
                        break;
                    case eDataKind.Voltage:
                        Publish("BatteryVoltage", obj.Value.ToString());
                        break;
                    case eDataKind.BatteryState:
                    case eDataKind.ChargeCompleteTime:
                    case eDataKind.DisChargeCompleteTime:
                        break;
                    case eDataKind.SOC:
                        Publish("BatterySOC", 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);
            }
        }
    }
}