OHV/Dev/OHV/VehicleControlSystem/ControlLayer/IO/EzIO.cs

using GSG.NET.Concurrent;
using GSG.NET.Excel;
using GSG.NET.Logging;
using GSG.NET.Quartz;
using GSG.NET.Utils;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using VehicleControlSystem.ControlLayer.Lib.EziPlusE;

namespace VehicleControlSystem.ControlLayer.IO
{
    public partial class EzIO : IIO, IDisposable
    {

        #region Bit Mask
        public readonly uint[] bitOnMask =
       {
            0x00000001, 0x00000002, 0x00000004, 0x00000008,
            0x00000010, 0x00000020, 0x00000040, 0x00000080,
            0x00000100, 0x00000200, 0x00000400, 0x00000800,
            0x00001000, 0x00002000, 0x00004000, 0x00008000,

            0x00010000, 0x00020000, 0x00040000, 0x00080000,
            0x00100000, 0x00200000, 0x00400000, 0x00800000,
            0x01000000, 0x02000000, 0x04000000, 0x08000000,
            0x10000000, 0x20000000, 0x40000000, 0x80000000
        };

        public readonly uint[] bitOffMask =
       {
            0xFFFFFFFE, 0xFFFFFFFD, 0xFFFFFFFB, 0xFFFFFFF7,
            0xFFFFFFEF, 0xFFFFFFDF, 0xFFFFFFBF, 0xFFFFFF7F,
            0xFFFFFEFF, 0xFFFFFDFF, 0xFFFFFBFF, 0xFFFFF7FF,
            0xFFFFEFFF, 0xFFFFDFFF, 0xFFFFBFFF, 0xFFFF7FFF,

            0xFFFEFFFF, 0xFFFDFFFF, 0xFFFBFFFF, 0xFFF7FFFF,
            0xFFEFFFFF, 0xFFDFFFFF, 0xFFBFFFFF, 0xFF7FFFFF,
            0xFEFFFFFF, 0xFDFFFFFF, 0xFBFFFFFF, 0xF7FFFFFF,
            0xEFFFFFFF, 0xDFFFFFFF, 0xBFFFFFFF, 0x7FFFFFFF
        };

        public readonly uint[] servoAmpInputBitOnMask =
        {
            0x04000000, 0x08000000, 0x10000000, 0x20000000,
            0x40000000, 0x80000000, 0x00400000, 0x00800000,
            0x01000000
        };

        public readonly uint[] servoAmpOutputBitMask =
        {
            0x00008000, 0x00010000, 0x00020000, 0x00040000,
            0x00080000, 0x00100000, 0x00200000, 0x00400000,
            0x00800000
        };
        #endregion

        static Logger logger = Logger.GetLogger();

        bool IsThreadAlive = true;
        public List<EzBoard> BoardList = new List<EzBoard>();

        List<BitBlock> _inPutIOList = null;
        public List<BitBlock> InPutIOList
        {
            get => this._inPutIOList;
        }

        List<BitBlock> _outPutIOList = null;
        public List<BitBlock> OutPutIOList
        {
            get => this._outPutIOList;
        }

        UInt16[] _incomingBuffer;
        UInt16[] _outcomingBuffer;

        Thread _readThread;

        bool isConnectError = false;
        public bool IsConnectError
        {
            get => this.isConnectError;
            set
            {
                if (this.isConnectError == value)
                    return;
                this.isConnectError = value;

                if (isConnectError)
                    this.qQ.Enqueue(new QoDiconnected());
                else
                    this.qQ.Enqueue(new QoConnected());

            }
        }

        TsMap<string, SyncObject> ddWaitChgBlock = new TsMap<string, SyncObject>(); 

        #region Pull Thread
        public TsQueue<QueueObject> qQ = new TsQueue<QueueObject>(512);
        public Thread pullThread;
        #endregion

        #region public Method
        public int GetBit(uint usIOAddr, bool pbval)
        {
            throw new NotImplementedException();
        }

        public int GetBit(string strIOAddr, bool pbVal)
        {
            throw new NotImplementedException();
        }

        public int GetByte(uint usIOAddr, byte pcValue)
        {
            throw new NotImplementedException();
        }

        public int GetByte(string strIOAddr, byte pcValue)
        {
            throw new NotImplementedException();
        }

        public int GetWord(uint usIOAddr, short pwValue)
        {
            throw new NotImplementedException();
        }

        public int GetWord(string strIOAddr, short pwValue)
        {
            throw new NotImplementedException();
        }

        public int Initialize(List<EzBoard> ezBoards)
        {
            this.BoardList = ezBoards;

            this._incomingBuffer = new ushort[ezBoards.Count];
            this._outcomingBuffer = new ushort[ezBoards.Count];

            this.IsThreadAlive = true;
            _readThread = new Thread(this.IOThread);
            this._readThread.IsBackground = true;
            this._readThread.Start();

            return 0;
        }

        #endregion

        public bool IsDeviceOpened()
        {
            throw new NotImplementedException();
        }

        public bool IsOff(uint usIOAddr)
        {
            throw new NotImplementedException();
        }

        public bool IsOff(string ioTag, bool isInput = true)
        {
            BitBlock bit = null;
            if (isInput)
                bit = this._inPutIOList.Where(x => ioTag.Equals(x.Tag)).FirstOrDefault();
            else
                bit = this._outPutIOList.Where(x => ioTag.Equals(x.Tag)).FirstOrDefault();

            return !bit.IsBitOn;
        }

        public bool IsOn(uint usIOAddr)
        {
            throw new NotImplementedException();
        }

        //public int IsOn( string strIOAddr, bool pbVal )
        //{
        //    var io = this._inPutIOList.Where( x => strIOAddr.Equals( x.Tag ) ).FirstOrDefault();
        //    if( io == null )
        //        return -1;
        //    else
        //    {
        //    }
        //    return 0;
        //}

        public bool IsOn(string ioTag, bool isInput = true)
        {
            BitBlock bit = null;
            if (isInput)
                bit = this._inPutIOList.Where(x => ioTag.Equals(x.Tag)).FirstOrDefault();
            else
                bit = this._outPutIOList.Where(x => ioTag.Equals(x.Tag)).FirstOrDefault();

            return bit.IsBitOn;
        }

        public bool WaitChangeInputIO(bool exp, int timeout, string tag)
        {
            if (exp == IsOn(tag))
                return true;

            Assert.IsFalse(ddWaitChgBlock.ContainsKey(tag), "WaitChg InputIO already waiting:{0}", tag);
            try
            {
                var so = new SyncObject { };
                ddWaitChgBlock.Add(tag, so);
                so.Lock();
                return so.Await(timeout);
            }
            finally
            {
                this.ddWaitChgBlock.Remove(tag);
            }
        }

        public int LoadIOMap(string strFileName)
        {
            var bl = new ExcelMapper(strFileName).Fetch<EzBoard>("BOARD").ToList();
            this.BoardList = bl;

            var il = new ExcelMapper(strFileName).Fetch<BitBlock>("IN_IO").ToList();
            var inputIO = il.Where(x => !string.IsNullOrEmpty(x.Tag)).ToList();
            this._inPutIOList = inputIO;

            var ol = new ExcelMapper(strFileName).Fetch<BitBlock>("OUT_IO").ToList();
            var outputIO = ol.Where(x => !string.IsNullOrEmpty(x.Tag)).ToList();
            this._outPutIOList = outputIO;

            return 0;
        }

        public int OutputOff(uint usIOAddr)
        {
            throw new NotImplementedException();
        }

        public int OutputOff(string strIOAddr)
        {
            int result = 0;

            var outIO = this._outPutIOList.Where(x => strIOAddr.Equals(x.Tag)).FirstOrDefault();
            var boardType = this.BoardList.Where(x => x.BoardID == outIO.BoardNo).FirstOrDefault().Type;

            uint bitMask = 0;
            if (boardType == E_EzboardType.Servo)
            {
                bitMask = this.servoAmpOutputBitMask[outIO.Index];
                result = EziMOTIONPlusELib.FAS_SetIOOutput(outIO.BoardNo, 0, bitMask);
            }
            else
            {
                bitMask = bitOnMask[outIO.Index];
                result = EziMOTIONPlusELib.FAS_SetOutput(outIO.BoardNo, 0, bitMask);
            }

            return result;
        }

        public int OutputOn(uint usIOAddr)
        {
            //var outIO = this._inPutIOList.Where( x => strIOAddr.Equals( x.Tag ) ).FirstOrDefault();

            //EziMOTIONPlusELib.FAS_SetIOOutput( boardNo, writeMask, 0 );

            return 0;
        }

        public int OutputOn(string outputTag)
        {
            int result = 0;

            var outIO = this._outPutIOList.Where(x => outputTag.Equals(x.Tag)).FirstOrDefault();
            var boardType = this.BoardList.Where(x => x.BoardID == outIO.BoardNo).FirstOrDefault().Type;

            uint bitMask = 0;
            if (boardType == E_EzboardType.Servo)
            {
                bitMask = this.servoAmpOutputBitMask[outIO.Index];
                result = EziMOTIONPlusELib.FAS_SetIOOutput(outIO.BoardNo, bitMask, 0);
            }
            else
            {
                bitMask = bitOnMask[outIO.Index];
                result = EziMOTIONPlusELib.FAS_SetOutput(outIO.BoardNo, bitMask, 0);
            }

            return result;
        }

        #region 일정시간 이후 동작 구현
        void WriteOutputOn(string outputTag)
        {
            int result = 0;

            var outIO = this._outPutIOList.Where(x => outputTag.Equals(x.Tag)).FirstOrDefault();
            var boardType = this.BoardList.Where(x => x.BoardID == outIO.BoardNo).FirstOrDefault().Type;

            uint bitMask = 0;
            if (boardType == E_EzboardType.Servo)
            {
                bitMask = this.servoAmpOutputBitMask[outIO.Index];
                result = EziMOTIONPlusELib.FAS_SetIOOutput(outIO.BoardNo, bitMask, 0);
            }
            else
            {
                bitMask = bitOnMask[outIO.Index];
                result = EziMOTIONPlusELib.FAS_SetOutput(outIO.BoardNo, bitMask, 0);
            }
        }
        void WriteOutputOff(string strIOAddr)
        {
            int result = 0;

            var outIO = this._outPutIOList.Where(x => strIOAddr.Equals(x.Tag)).FirstOrDefault();
            var boardType = this.BoardList.Where(x => x.BoardID == outIO.BoardNo).FirstOrDefault().Type;

            uint bitMask = 0;
            if (boardType == E_EzboardType.Servo)
            {
                bitMask = this.servoAmpOutputBitMask[outIO.Index];
                result = EziMOTIONPlusELib.FAS_SetIOOutput(outIO.BoardNo, 0, bitMask);
            }
            else
            {
                bitMask = bitOnMask[outIO.Index];
                result = EziMOTIONPlusELib.FAS_SetOutput(outIO.BoardNo, 0, bitMask);
            }
        }

        public void WriteOutputIO(string tag, bool on, int after = 0)
        {
            if ( on)
            {
                if (after > 0)
                    TimerUtils.Once(after, WriteOutputOn, tag);
                else
                    WriteOutputOn(tag);
            }
            else
            {
                if (after > 0)
                    TimerUtils.Once(after, WriteOutputOff, tag);
                else
                    WriteOutputOff(tag);
            }
        } 
        #endregion

        public int OutputToggle(uint usIOAddr)
        {
            throw new NotImplementedException();
        }

        public int OutputToggle(string strIOAddr)
        {
            throw new NotImplementedException();
        }

        public int PutByte(uint usIOAddr, byte pcValue)
        {
            throw new NotImplementedException();
        }

        public int PutByte(string strIOAddr, byte pcValue)
        {
            throw new NotImplementedException();
        }

        public int PutWord(uint usIOAddr, short pwValue)
        {
            throw new NotImplementedException();
        }

        public int PutWord(string strIOAddr, short pwValue)
        {
            throw new NotImplementedException();
        }

        #region Thread
        public void RunIOThread()
        {
            try
            {
                ThreadStart();
            }
            catch (Exception)
            {
            }
            finally
            {
            }
        }
        public void ThreadStart()
        {
            this.IsThreadAlive = true;
            this._readThread = ThreadUtils.Invoke(IOThread);

            this.pullThread = ThreadUtils.Invoke(PullQueueThread);
        }
        void IOThread()
        {
            var sTime = SwUtils.CurrentTimeMillis;

            while (IsThreadAlive)
            {
                Thread.Sleep(5);
                try
                {
                    if (!IsConnetedAllBoard())
                    {
                        if (!ConnectAllBoard())
                        {
                            IsConnectError = true;
                        }
                        Thread.Sleep(1000);
                        continue;
                    }
                    IsConnectError = false;

                    ReadBoardIO();

                    //Console.WriteLine( $"Read Time - { SwUtils.Elapsed( sTime ) } mm" );
                    //sTime = SwUtils.CurrentTimeMillis;
                }
                catch (Exception ex)
                {
                    logger.E($"Exception [EzIO IORead Thread] - {ex.StackTrace}");
                }
            }
            logger.D("[EzIO] - Dispose");
        }

        void ReadBoardIO()
        {
            foreach (var board in this.BoardList)
            {
                if (!IsConnetedBoard(board.BoardID))
                    continue;

                switch (board.Type)
                {
                    case E_EzboardType.Servo:
                        {
                            uint axisDIn = 0, axisDOut = 0, axisStatus = 0;
                            int cmdPos = 0, actPos = 0, posErr = 0, actVel = 0;
                            ushort posItemNo = 0;

                            if (EziMOTIONPlusELib.FAS_GetAllStatus(board.BoardID, ref axisDIn, ref axisDOut, ref axisStatus,
                                ref cmdPos, ref actPos, ref posErr, ref actVel, ref posItemNo) == EziMOTIONPlusELib.FMM_OK)
                            {
                                var input = this._inPutIOList.Where(x => x.BoardNo == board.BoardID).DefaultIfEmpty().ToList();
                                input.ForEach(i =>
                               {
                                   var isOn = Convert.ToBoolean(axisDIn & this.servoAmpInputBitOnMask[i.Index]) ? true : false;
                                   if (i.IsBitOn != isOn) i.IsChanged = true;
                                   i.IsBitOn = isOn;
                               });

                                var output = this._outPutIOList.Where(x => x.BoardNo == board.BoardID).DefaultIfEmpty().ToList();
                                output.ForEach(o =>
                               {
                                   var isOn = Convert.ToBoolean(axisDOut & this.servoAmpOutputBitMask[o.Index]) ? true : false;
                                   if (o.IsBitOn != isOn) o.IsChanged = true;
                                   o.IsBitOn = isOn;
                               });
                            }
                        }
                        break;

                    case E_EzboardType.IOInput:
                        {
                            uint inValue = 0, latchValue = 0;
                            int result = EziMOTIONPlusELib.FMM_OK;
                            result = EziMOTIONPlusELib.FAS_GetInput(board.BoardID, ref inValue, ref latchValue);
                            if (result == EziMOTIONPlusELib.FMM_OK)
                            {
                                var input = this._inPutIOList.Where(x => x.BoardNo == board.BoardID).DefaultIfEmpty().ToList();
                                input.ForEach(i =>
                               {
                                   var isOn = Convert.ToBoolean(inValue & this.bitOnMask[i.Index]) ? true : false;
                                   if (i.IsBitOn != isOn) i.IsChanged = true;
                                   i.IsBitOn = isOn;
                               });
                            }
                            else
                                logger.E($"EzIO - [{board.BoardID}] Read Fail Error Code {result}");
                        }
                        break;

                    case E_EzboardType.IOOutput:
                        {
                            uint outValue = 0, outStatus = 0;
                            int result = EziMOTIONPlusELib.FMM_OK;
                            result = EziMOTIONPlusELib.FAS_GetOutput(board.BoardID, ref outValue, ref outStatus);
                            if (result == EziMOTIONPlusELib.FMM_OK)
                            {
                                var output = this._outPutIOList.Where(x => x.BoardNo == board.BoardID).DefaultIfEmpty().ToList();
                                output.ForEach(o =>
                               {
                                   var isOn = Convert.ToBoolean(outValue & this.bitOnMask[o.Index]) ? true : false;
                                   if (o.IsBitOn != isOn) o.IsChanged = true;
                                   o.IsBitOn = isOn;
                               });
                            }
                            else
                                logger.E($"EzIO - [{board.BoardID}] Read Fail Error Code {result}");
                        }
                        break;

                    default:
                        break;
                }
            }

            FireChangedIO();
        }

        void FireChangedIO()
        {
            this.InPutIOList.ForEach(i =>
            {
                if (i.IsChanged)
                {
                    i.IsChanged = false;
                    var clone = ObjectCopyUtils.DeepClone<BitBlock>(i);
                    this.qQ.Enqueue(new QoChangedIO { Arg0 = clone });
                    NotifySyncBit(clone);
                }
            });

            this.OutPutIOList.ForEach(o =>
            {
                if (o.IsChanged)
                {
                    o.IsChanged = false;
                    var clone = ObjectCopyUtils.DeepClone<BitBlock>(o);
                    this.qQ.Enqueue(new QoChangedIO { Arg0 = clone });
                }
            });
        }

        void PullQueueThread()
        {
            while(IsThreadAlive)
            {
                try
                {
                    var o = qQ.Dequeue();

                    if (o is QoConnected)
                        DelegateUtils.Invoke(OnContd);
                    else if (o is QoDiconnected)
                        DelegateUtils.Invoke(OnDiscontd);
                    else if (o is QoChangedIO)
                        DelegateUtils.Invoke(OnChangedIO, o.Arg0);
                    else if (o is QoWriteIO)
                        DelegateUtils.Invoke(OnWriteIO, o.Arg0);
                    else if (o is QoLog)
                        DelegateUtils.Invoke(OnLog, o.Arg0, o.Arg1);
                    else
                        Assert.Fail("Unk Object {0}", o);
                }
                catch (ThreadAbortException) 
                {
                }
                catch (Exception e)
                {
                    logger.E(e);
                }
            }
            logger.D("[EzIO] - PullQueue Thread Dispose");
        }
        #endregion

        /// <summary>
        /// InputIO 변경 알림
        /// </summary>
        /// <param name="block"></param>
        void NotifySyncBit(BitBlock block)
        {
            if (ddWaitChgBlock.ContainsKey(block.Tag))
            {
                var so = ddWaitChgBlock[block.Tag];
                so.Expect = block;
                so.Notify();
            }
        }

        bool IsConnetedBoard(int boardNo)
        {
            return EziMOTIONPlusELib.FAS_IsSlaveExist(boardNo) == 1;
        }

        bool IsConnetedAllBoard()
        {
            foreach (var b in this.BoardList)
            {
                if (!IsConnetedBoard(b.BoardID))
                {
                    this.qQ.Clear();//연결이 끊어지면 내용을 삭제.
                    return false;
                }
            }

            return true;
        }

        bool ConnectAllBoard()
        {
            foreach (var b in this.BoardList)
            {
                if (IsConnetedBoard(b.BoardID))
                    continue;

                var addr = b.IPAddress.Split( '.' );
                byte boardNo = Convert.ToByte( addr[3] );
                if (EziMOTIONPlusELib.FAS_Connect((byte)192, (byte)168, (byte)0, boardNo, b.BoardID ) != EziMOTIONPlusELib.FMM_OK)
                {
                    logger.E($"EzIO - Connect Fail {b.BoardID}");
                    return false;
                }
            }

            return true;
        }

        public int Terminate()
        {
            this.IsThreadAlive = false;
            if (this._readThread != null && this._readThread.IsAlive)
            {
                if (!this._readThread.Join(3000))
                    this._readThread.Abort();
            }

            this.BoardList.ForEach(b =>
           {
               EziMOTIONPlusELib.FAS_Close(b.BoardID);
           });

            return 0;
        }

        public int Initialize()
        {
            throw new NotImplementedException();
        }

        int GetModuleAndBitNoByAddress(uint usIOAddr, out int boardNo, out int bitNo)
        {
            long nOffset = 0;
            long nCount = 0;
            long nAddress = usIOAddr;
            int INPUT_ORIGIN = 1000;
            int OUTPUT_ORIGIN = 2000;
            boardNo = 0;
            bitNo = 0;

            if (usIOAddr < OUTPUT_ORIGIN)
            {
                nOffset = usIOAddr - INPUT_ORIGIN;
                for (int nModNo = 0; nModNo < this.BoardList.Count; nModNo++)
                {
                    // Get Input CH Count Using Module ID
                    //AxdInfoGetInputCount( nModNo, nCount );
                    if (nOffset - nCount < 0)
                    {
                        boardNo = nModNo;
                        bitNo = (int)nOffset;
                        return 0;
                    }
                    nOffset -= nCount;
                }
            }
            else
            {
                nOffset = usIOAddr - OUTPUT_ORIGIN;
                for (int nModNo = 0; nModNo < this.BoardList.Count; nModNo++)
                {
                    // Get Output CH Count Using Module ID
                    //AxdInfoGetOutputCount( nModNo, nCount );
                    if (nOffset - nCount < 0)
                    {
                        boardNo = nModNo;
                        bitNo = (int)nOffset;
                        return 0;
                    }
                    nOffset -= nCount;
                }
            }

            return 0;
        }

        #region IDisposable Support
        private bool disposedValue = false; // 중복 호출을 검색하려면

        protected virtual void Dispose(bool disposing)
        {
            if (!disposedValue)
            {
                if (disposing)
                {
                    // TODO: 관리되는 상태(관리되는 개체)를 삭제합니다.
                    if (this.pullThread != null)
                    {
                        this.IsThreadAlive = false;
                        if (!this.pullThread.Join(2000) && this.pullThread.IsAlive)
                        {
                            this.pullThread.Abort();
                        }
                        this.pullThread = null;
                    }

                    ThreadUtils.Kill(this.pullThread);

                    Terminate(); //Board Close 
                }

                // TODO: 관리되지 않는 리소스(관리되지 않는 개체)를 해제하고 아래의 종료자를 재정의합니다.
                // TODO: 큰 필드를 null로 설정합니다.

                disposedValue = true;
            }
        }

        // TODO: 위의 Dispose(bool disposing)에 관리되지 않는 리소스를 해제하는 코드가 포함되어 있는 경우에만 종료자를 재정의합니다.
        // ~EzIO()
        // {
        //   // 이 코드를 변경하지 마세요. 위의 Dispose(bool disposing)에 정리 코드를 입력하세요.
        //   Dispose(false);
        // }

        // 삭제 가능한 패턴을 올바르게 구현하기 위해 추가된 코드입니다.
        public void Dispose()
        {
            // 이 코드를 변경하지 마세요. 위의 Dispose(bool disposing)에 정리 코드를 입력하세요.
            Dispose(true);
            // TODO: 위의 종료자가 재정의된 경우 다음 코드 줄의 주석 처리를 제거합니다.
            // GC.SuppressFinalize(this);
        }
        #endregion
    }
}