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() {Arg0 = "EzIO" } );
                else
                    this.qQ.Enqueue( new QoConnected() {Arg0 = "EzIO" } );
            }
        }

        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 )
                            {
                                //if ( this._inPutIOList.Where( x => x.BoardNo == board.BoardID ).Count() <= 0 ) break;

                                var input = this._inPutIOList.Where( x => x.BoardNo == board.BoardID ).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 ).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 ).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 ).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, o.Arg0 );
                    else if ( o is QoDiconnected )
                        DelegateUtils.Invoke( OnDiscontd, o.Arg0 );
                    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
    }
}