/*
 * Video On Demand Samples
 *
 * Copyright (C) 2015 Microchip Technology Germany II GmbH & Co. KG
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * You may also obtain this software under a propriety license from Microchip.
 * Please contact Microchip for further information.
 *
 */

#define NM_VERSION_MAJOR       ((uint8_t)3)
#define NM_VERSION_MINOR       ((uint8_t)0)
#define NM_VERSION_BUGFIX      ((uint8_t)4)
#define NM_VERSION_BUILD       ((uint8_t)0)

#define IPC_PORT_NUMBER_NM           (5533)
#define FBLOCK_NETWORK_MANAGER       (10)
#define FUNC_NM_SERVERVERSION        (1)
#define FUNC_NM_SUBSCRIBE_MCM        (2)
#define FUNC_NM_UNSUBSCRIBE_MCM      (3)
#define FUNC_NM_RECEIVED_MCM         (4)
#define FUNC_NM_SEND_MCM_ADR         (5)
#define FUNC_NM_SEND_MCM_DEV         (6)
#define FUNC_NM_COUNTER_ROUTE        (10)
#define FUNC_NM_EXECUTE_CONFIG       (15)
#define FUNC_NM_EXECUTE_SCRIPT_FILE  (20)
#define FUNC_NM_EXECUTE_SCRIPT_MEM   (21)
#define FUNC_NM_NET_STATE            (30)
#define FUNC_NM_CONNECTION_LIST      (100)
#define FUNC_NM_RING_BREAK_DIAGNOSIS (200)

#define IPC_TEMP_BUFFER_SIZE    (200000)

#define AMOUNT_OF_NETWORK_STATES 10 

#include <DoxyGenStartPage.h>
#include <stdio.h>
#include <stdint.h>
#include <stddef.h>
#include <unistd.h>
#include <stdlib.h>
#include <signal.h>
#include <pthread.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <sys/stat.h>
#include <unistd.h>
#include <pthread.h>
#include <fcntl.h>
#include <dirent.h>
#include <exception>
#include "Console.h"
#include "Network.h"
#include "Board.h"
#include "MacAddr.h"
#include "MostIpc.h"
#include "ConnectionInfo.h"

using namespace std;

class CNetworkManagerHandler;
static CNetworkManagerHandler *s_mainClass = NULL;

/*----------------------------------------------------------*/
/*! \brief Structure which holds information to address an MOST device via it's source and target address.
 */
/*----------------------------------------------------------*/
typedef struct
{
    ///The MOST ring instance, starting with 0 for the first MOST ring.
    uint32_t mostInst;
    ///The MOST source address (may be not accurate).
    uint32_t sourceAddr;
    ///The MOST target address.
    uint32_t targetAddr;
    ///The MOST Function Block Identifier
    uint32_t nFBlock;
    ///The MOST Function Block Instance Identifier
    uint32_t nInst;
    ///The MOST Function Block Function Identifier
    uint32_t nFunc;
    ///The MOST Function Block Function Operation Type
    uint32_t nOpType;
    ///The amount of bytes stored in Payload
    uint32_t nPayloadLen;
} MostMcmAdr_t;

/*----------------------------------------------------------*/
/*! \brief Structure which holds information to address an MOST device via it's device id and device instance (like described in the XML file).
 */
/*----------------------------------------------------------*/
typedef struct
{
    ///The MOST ring instance, starting with 0 for the first MOST ring.
    uint32_t mostInst;
    ///The device identifier (group address) as specified in the XML file.
    uint32_t deviceId;
    ///The instance number of the device. Starting with 0 for the first device with deviceId.
    uint32_t deviceInst;
    ///The MOST Function Block Identifier
    uint32_t nFBlock;
    ///The MOST Function Block Instance Identifier
    uint32_t nInst;
    ///The MOST Function Block Function Identifier
    uint32_t nFunc;
    ///The MOST Function Block Function Operation Type
    uint32_t nOpType;
    ///The amount of bytes stored in Payload
    uint32_t nPayloadLen;
} MostMcmDev_t;

/*----------------------------------------------------------*/
/*! \brief Structure which holds information to address an MOST device via it's MOST instance and the MOST target address.
 */
/*----------------------------------------------------------*/
typedef struct
{
    ///The MOST ring instance, starting with 0 for the first MOST ring.
    uint32_t mostInst;
    ///The MOST target address.
    uint32_t targetAddr;
} MostPacketAddr_t;

/*----------------------------------------------------------*/
/*! \brief Structure which holds information of all MOST specific states.
 */
/*----------------------------------------------------------*/
typedef struct
{
	//If this value is false, all other values must be ignored.
    bool isValid;
	///The MOST ring instance, starting with 0 for the first MOST ring.
    uint8_t mostInstance; 
	///If set to true, the MOST ring is fully functional. Otherwise, no data can be transported.
    bool available; 
	///Shows the amount of nodes for the specific MOST ring.
    uint8_t maxPos; 
	///Shows how many bytes in a MOST frame is reserved for Packet (IP) data.
    uint16_t packetBW;
} NetworkInformation_t;

static void PrintMenu();

/*!
* \brief  Applications main entry point and event handler, as it implements callbacks from CNetworkListener.
*         It controls the Network via the CNetwork class.
*         Isochronous TX connections will be automatically connected to the CMultiplexer class.
*/
class CNetworkManagerHandler : public CNetworkListner, public CThread
{
private:
    class MostListener
    {
    private:
        CSafeVector<CMsgAddr *> allListeners;

    public:
        MostListener()
        {
        }

        ~MostListener()
        {
            for (uint32_t i = 0; i < allListeners.Size(); i++)
                allListeners[i]->RemoveReference();
            allListeners.RemoveAll(false);
        }

        void RemoveListener( CMsgAddr *pAddr )
        {
            if( NULL == pAddr )
                return;
            allListeners.Remove(pAddr);
            pAddr->RemoveReference();
        }

        void AddListener( CMsgAddr *pAddr )
        {
            if( NULL == pAddr )
                return;

            for (uint32_t i = 0; i < allListeners.Size(); i++)
            {
                CMsgAddr *l = allListeners[i];
                if (NULL != l 
                    && l->GetProtocol() == pAddr->GetProtocol()
                    && 0 == strcmp(l->GetInetAddress(), pAddr->GetInetAddress()))
                {
                    if (l->GetPort() == pAddr->GetPort())
                    {
                        //Already registered with the same port, do not continue
                        return;
                    }
                    else
                    {
                        //The client reconnected with different (random) port.
                        //Remove old connection
                        allListeners.Remove(l);
                        l->RemoveReference();
                        break;
                    }
                }
            }
            pAddr->AddReference();
            allListeners.PushBack( pAddr );
        }

        uint32_t GetCount()
        {
            return allListeners.Size();
        }

        CMsgAddr *GetListener( uint32_t index )
        {
            return allListeners[index];
        }
    };
    
    NetworkInformation_t allStates[AMOUNT_OF_NETWORK_STATES];
    CNetwork *network;
    bool allowThreadRun;
    uint32_t updateCount;
    static const uint32_t updateCountMaxVal = 3;

    CMostIpc *mostIpc;
    CMsgFilter *serverVersion_Get;
    CMsgFilter *subscribeMcm_Set;
    CMsgFilter *unsubscribeMcm_Set;
    CMsgFilter *sendMcmAdr_Set;
    CMsgFilter *sendMcmDev_Set;
    CMsgFilter *counterRoute_Get;
    CMsgFilter *executeConfigFile_Set;
    CMsgFilter *executeScriptFile_Set;
    CMsgFilter *executeScriptBuffer_Set;
    CMsgFilter *connectionList_Get;
    CMsgFilter *ringBreakDiagnosis_Get;
    CMsgFilter *netState_Get;

    MostListener netStateListener;
    MostListener mcmListener;
    MostListener connectionListener;
    MostListener rbdListener;

public:
    CConnectionInfoContainer infoContainer;

    CNetworkManagerHandler() : CThread( "CNetworkManagerHandler",
                               false ), allowThreadRun( true ), updateCount( updateCountMaxVal )
    {
        memset(allStates, 0, sizeof(allStates));
        
        network = CNetwork::GetInstance();
        network->AddListener( this );

        mostIpc = new CMostIpc( IPC_PORT_NUMBER_NM, true );

        serverVersion_Get = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_SERVERVERSION,
            CMostMsg::OP_GET, OnServerVersion_Get );
        mostIpc->RegisterMessageHandler( serverVersion_Get );

        subscribeMcm_Set = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_SUBSCRIBE_MCM,
            CMostMsg::OP_SET, OnSubscribeMcm_Set );
        mostIpc->RegisterMessageHandler( subscribeMcm_Set );

        unsubscribeMcm_Set = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_UNSUBSCRIBE_MCM,
            CMostMsg::OP_SET, OnUnsubscribeMcm_Set );
        mostIpc->RegisterMessageHandler( unsubscribeMcm_Set );

        sendMcmAdr_Set = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_SEND_MCM_ADR,
            CMostMsg::OP_SET, OnMcmSendAddress_Set );
        mostIpc->RegisterMessageHandler( sendMcmAdr_Set );

        sendMcmDev_Set = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_SEND_MCM_DEV,
            CMostMsg::OP_SET, OnMcmSendDevice_Set );
        mostIpc->RegisterMessageHandler( sendMcmDev_Set );

        counterRoute_Get = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_COUNTER_ROUTE,
            CMostMsg::OP_GET, OnCounterRoute_Get );
        mostIpc->RegisterMessageHandler( counterRoute_Get );

        executeConfigFile_Set = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_EXECUTE_CONFIG,
            CMostMsg::OP_SET, OnExecuteConfigFile_Set );
        mostIpc->RegisterMessageHandler( executeConfigFile_Set );
        
        executeScriptFile_Set = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_EXECUTE_SCRIPT_FILE,
            CMostMsg::OP_SET, OnExecuteScriptFile_Set );
        mostIpc->RegisterMessageHandler( executeScriptFile_Set );

        executeScriptBuffer_Set = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_EXECUTE_SCRIPT_MEM,
            CMostMsg::OP_SET, OnExecuteScriptBuffer_Set );
        mostIpc->RegisterMessageHandler( executeScriptBuffer_Set );

        connectionList_Get = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_CONNECTION_LIST,
            CMostMsg::OP_GET, OnConnectionList_Get );
        mostIpc->RegisterMessageHandler( connectionList_Get );

        ringBreakDiagnosis_Get = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_RING_BREAK_DIAGNOSIS,
            CMostMsg::OP_GET, OnRingBreakDiagnosis_Get );
        mostIpc->RegisterMessageHandler( ringBreakDiagnosis_Get );

        netState_Get = new CMsgFilter( FBLOCK_NETWORK_MANAGER, FUNC_NM_NET_STATE,
            CMostMsg::OP_GET, OnNetState_Get );
        mostIpc->RegisterMessageHandler( netState_Get );

        Start();
    }

    virtual ~CNetworkManagerHandler()
    {
        allowThreadRun = false;
        Stop();
        infoContainer.DestroyAllInfos();
        if( NULL != network )
        {
            network->RemoveListener( this );
            delete network;
            network = NULL;
        }
        if( NULL != mostIpc )
        {
            delete mostIpc;
            mostIpc = NULL;
        }
        if( NULL != subscribeMcm_Set )
        {
            delete subscribeMcm_Set;
            subscribeMcm_Set = NULL;
        }
        if( NULL != unsubscribeMcm_Set )
        {
            delete unsubscribeMcm_Set;
            unsubscribeMcm_Set = NULL;
        }
        if( NULL != sendMcmAdr_Set )
        {
            delete sendMcmAdr_Set;
            sendMcmAdr_Set = NULL;
        }
        if( NULL != sendMcmDev_Set )
        {
            delete sendMcmDev_Set;
            sendMcmDev_Set = NULL;
        }
        if( NULL != counterRoute_Get )
        {
            delete counterRoute_Get;
            counterRoute_Get = NULL;
        }
        if( NULL != executeConfigFile_Set )
        {
            delete executeConfigFile_Set;
            executeConfigFile_Set = NULL;
        }
        if( NULL != executeScriptFile_Set )
        {
            delete executeScriptFile_Set;
            executeScriptFile_Set = NULL;
        }
        if( NULL != executeScriptBuffer_Set )
        {
            delete executeScriptBuffer_Set;
            executeScriptBuffer_Set = NULL;
        }
        if( NULL != connectionList_Get )
        {
            delete connectionList_Get;
            connectionList_Get = NULL;
        }
        if( NULL != ringBreakDiagnosis_Get )
        {
            delete ringBreakDiagnosis_Get;
            ringBreakDiagnosis_Get = NULL;
        }
        if( NULL != netState_Get )
        {
            delete netState_Get;
            netState_Get = NULL;
        }
    }

    static void OnServerVersion_Get( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnServerVersion_Get parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        ConsolePrintf( PRIO_HIGH,
            GREEN"MOST-IPC Client connected. ip=%s"RESETCOLOR"\n", pAddr->GetInetAddress() );

        const uint8_t version[] =
        {
            NM_VERSION_MAJOR, NM_VERSION_MINOR,
            NM_VERSION_BUGFIX, NM_VERSION_BUILD
        };
        CMostMsgTx *mostMsg = new CMostMsgTx( pAddr,FBLOCK_NETWORK_MANAGER,
            0, FUNC_NM_SERVERVERSION, CMostMsg::OP_STATUS, sizeof ( version ),
            version, 500, NULL, NULL );
        s_mainClass->mostIpc->SendMsg( mostMsg );
        mostMsg->RemoveReference();
    }

    static void OnSubscribeMcm_Set( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnSubscribeMcm_Set parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        ConsolePrintf( PRIO_MEDIUM,
            GREEN"MOST-IPC Client subscribed for MCM. ip=%s"RESETCOLOR"\n", pAddr->GetInetAddress() );
        s_mainClass->mcmListener.AddListener( pAddr );
    }

    static void OnUnsubscribeMcm_Set( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnUnsubscribeMcm_Set parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        ConsolePrintf( PRIO_MEDIUM,
            GREEN"MOST-IPC Client unsubscribed for MCM. ip=%s"RESETCOLOR"\n", pAddr->GetInetAddress() );
        s_mainClass->mcmListener.RemoveListener( pAddr );
    }

    static void OnMcmSendAddress_Set( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnMcmSend_Set parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        MostMcmAdr_t *mcm = ( MostMcmAdr_t * )pMsg->GetPayload();
        if( NULL == mcm || pMsg->GetPayloadLen() < sizeof( MostMcmAdr_t ) )
        {
            ConsolePrintf( PRIO_ERROR,
                RED"OnMcmSend_Set payload length is too small to carry a MCM message."RESETCOLOR"\n" );
            return;
        }
        ConsolePrintfStart( PRIO_MEDIUM,
            GREEN"MOST-IPC Client send MCM to MOST address. ip=%s, ring-inst:%d"\
            "target:0x%X, fblock:0x%X, inst:%d, func:0x%x, op:0x%X, p:0x[", pAddr->GetInetAddress(), mcm->mostInst,
            mcm->targetAddr, mcm->nFBlock, mcm->nInst, mcm->nFunc, mcm->nOpType );
        uint8_t *payload = &( ( uint8_t * )mcm )[sizeof( MostMcmAdr_t )];
        for( uint32_t i = 0; i < mcm->nPayloadLen; i++ )
        {
            ConsolePrintfContinue( " %02X", payload[i] );
        }
        ConsolePrintfExit( "]"RESETCOLOR"\n" );

        s_mainClass->SendMostControlMessage( mcm->mostInst, mcm->targetAddr, mcm->nFBlock, mcm->nInst, mcm->nFunc,
            mcm->nOpType, mcm->nPayloadLen, payload );
    }

    static void OnMcmSendDevice_Set( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnMcmSend_Set parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        MostMcmDev_t *mcm = ( MostMcmDev_t * )pMsg->GetPayload();
        if( NULL == mcm || pMsg->GetPayloadLen() < sizeof( MostMcmDev_t ) )
        {
            ConsolePrintf( PRIO_ERROR,
                RED"OnMcmSend_Set payload length is too small to carry a MCM message."RESETCOLOR"\n" );
            return;
        }
        ConsolePrintfStart( PRIO_MEDIUM,
            GREEN"MOST-IPC Client send MCM to specific device. ip=%s, ring-inst:%d"\
            "device-id:0x%X, device-instance:%d, fblock:0x%X, inst:%d, func:0x%x, op:0x%X, p:0x[",
            pAddr->GetInetAddress(), mcm->mostInst, mcm->deviceId, mcm->deviceInst, mcm->nFBlock, mcm->nInst,
            mcm->nFunc, mcm->nOpType );
        uint8_t *payload = &( ( uint8_t * )mcm )[sizeof( MostMcmAdr_t )];
        for( uint32_t i = 0; i < mcm->nPayloadLen; i++ )
        {
            ConsolePrintfContinue( " %02X", payload[i] );
        }
        ConsolePrintfExit( "]"RESETCOLOR"\n" );

        s_mainClass->SendMostControlMessage( mcm->mostInst, mcm->deviceId, mcm->deviceInst, mcm->nFBlock, mcm->nInst,
            mcm->nFunc, mcm->nOpType, mcm->nPayloadLen, payload );
    }

    static void OnCounterRoute_Get( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnCounterRoute_Get parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        CNetwork::Route_t *inRoute = ( CNetwork::Route_t * )pMsg->GetPayload();
        if( NULL == inRoute || pMsg->GetPayloadLen() < sizeof( CNetwork::Route_t ) )
        {
            ConsolePrintf( PRIO_ERROR,
                RED"OnCounterRoute_Get payload length is too small to carry a Route_t message."RESETCOLOR
                "\n" );
            return;
        }
        CNetwork::Route_t *outRoutes = NULL;
        uint32_t routeElements = s_mainClass->network->GetCounterPartOfRoute( inRoute, &outRoutes );
        ConsolePrintfStart( PRIO_MEDIUM,
            GREEN"MOST-IPC Client asked for counter route for: (deviceType:0x%X, inst:%d, channelId:%d), Result:",
            inRoute->deviceType, inRoute->instance, inRoute->channelId );
        for( uint32_t i = 0; NULL != outRoutes && i < routeElements; i++ )
        {
            ConsolePrintfContinue( " (deviceType:0x%X, inst:%d, channelId:%d)", outRoutes[i].deviceType,
                outRoutes[i].instance, outRoutes[i].channelId );
        }
        ConsolePrintfExit( RESETCOLOR"\n" );
        uint32_t bufferLen = ( routeElements + 1 ) * sizeof( CNetwork::Route_t );
        uint8_t *buffer = ( uint8_t * )malloc( bufferLen );
        if( NULL == buffer )
        {
            free( outRoutes );
            ConsolePrintf( PRIO_ERROR,
                RED"OnCounterRoute_Get payload failed to allocate memory for response."RESETCOLOR
                "\n" );
            return;
        }
        //Copy request to response element 0
        memcpy( buffer, inRoute, sizeof( CNetwork::Route_t ) );
        //Copy the results beginning at element 1
        if( 0 != routeElements )
            memcpy( &buffer[sizeof( CNetwork::Route_t )], outRoutes, routeElements * sizeof( CNetwork::Route_t ) );
        free( outRoutes );

        CMostMsgTx *mostMsg = new CMostMsgTx( pAddr, //Addr
            FBLOCK_NETWORK_MANAGER, // nFBlock
            0, // nInst
            FUNC_NM_COUNTER_ROUTE, // nFunc
            CMostMsg::OP_STATUS, // nOpType
            bufferLen, //nPayloadLen
            buffer, //Payload
            500, //nTimeoutMs
            NULL, //MessageSentCB
            NULL ); //UserContext
        s_mainClass->mostIpc->SendMsg( mostMsg );
        mostMsg->RemoveReference();
        free( buffer );
    }

    static void OnExecuteConfigFile_Set( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg || 0 == pMsg->GetPayloadLen() )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnExecuteScriptFile_Set parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        char *fileName = ( char * )pMsg->GetPayload();
        ConsolePrintf( PRIO_HIGH, "Executing new config file:%s\n", fileName);
        s_mainClass->network->LoadConfig(fileName);
    }
    
    static void OnExecuteScriptFile_Set( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnExecuteScriptFile_Set parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        MostPacketAddr_t *addr = ( MostPacketAddr_t * )pMsg->GetPayload();
        if( NULL == addr || pMsg->GetPayloadLen() <= sizeof( MostPacketAddr_t ) )
        {
            ConsolePrintf( PRIO_ERROR,
                RED"OnExecuteScriptFile_Set payload length is too small to carry a MCM message."RESETCOLOR"\n" );
            return;
        }
        char *payload = ( char * )( pMsg->GetPayload() + sizeof( MostPacketAddr_t ) );
        s_mainClass->network->ExecuteXmlScriptFromFile( addr->mostInst, addr->targetAddr, payload );
    }

    static void OnExecuteScriptBuffer_Set( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnExecuteScriptFile_Set parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        MostPacketAddr_t *addr = ( MostPacketAddr_t * )pMsg->GetPayload();
        if( NULL == addr || pMsg->GetPayloadLen() <= sizeof( MostPacketAddr_t ) )
        {
            ConsolePrintf( PRIO_ERROR,
                RED"OnExecuteScriptFile_Set payload length is too small to carry a MCM message."RESETCOLOR"\n" );
            return;
        }
        char *payload = ( char * )( pMsg->GetPayload() + sizeof( MostPacketAddr_t ) );
        uint32_t payloadLen = pMsg->GetPayloadLen() - sizeof( MostPacketAddr_t );
        s_mainClass->network->ExecuteXmlScriptFromMemory( addr->mostInst, addr->targetAddr, payload, payloadLen );
    }

    static void OnConnectionList_Get( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnConnectionList_Get parameters are invalid."RESETCOLOR"\n" );
            return;
        }

        s_mainClass->connectionListener.AddListener( pAddr );

        char *pBuffer = ( char * )malloc( IPC_TEMP_BUFFER_SIZE );
        uint32_t used = s_mainClass->infoContainer.SerializeToString( pBuffer, IPC_TEMP_BUFFER_SIZE );
        CMostMsgTx *mostMsg = new CMostMsgTx( pAddr, //Addr
            FBLOCK_NETWORK_MANAGER, // nFBlock
            0, // nInst
            FUNC_NM_CONNECTION_LIST, // nFunc
            CMostMsg::OP_STATUS, // nOpType
            used, //nPayloadLen
            ( uint8_t * )pBuffer, //Payload
            500, //nTimeoutMs
            NULL, //MessageSentCB
            NULL ); //UserContext
        s_mainClass->mostIpc->SendMsg( mostMsg );
        mostMsg->RemoveReference();
        free( pBuffer );
    }

    static void OnRingBreakDiagnosis_Get( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnRingBreakDiagnosis_Get parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        s_mainClass->rbdListener.AddListener( pAddr );
        s_mainClass->network->GetRingBreakDiagnosisResult( 0, 1 );
    }

    static void OnNetState_Get( CMsgAddr *pAddr, CMostMsg *pMsg )
    {
        if( NULL == s_mainClass || NULL == pAddr || NULL == pMsg )
        {
            ConsolePrintf( PRIO_ERROR, RED"OnNetState_Get parameters are invalid."RESETCOLOR"\n" );
            return;
        }
        s_mainClass->netStateListener.AddListener( pAddr );
        
        for (uint32_t i = 0; i < AMOUNT_OF_NETWORK_STATES; i++)
        {
            if (!s_mainClass->allStates[i].isValid)
                continue;
            s_mainClass->SendNetworkState(pAddr, s_mainClass->allStates[i].mostInstance,
                s_mainClass->allStates[i].available, s_mainClass->allStates[i].maxPos,
                s_mainClass->allStates[i].packetBW);
        }
    }
    
    void SendNetworkState( CMsgAddr *addr, uint8_t mostInstance, bool available, uint8_t maxPos, uint16_t packetBW )
    {
        uint8_t p[5];
        p[0] = mostInstance;
        p[1] = available;
        p[2] = maxPos;
        p[3] = packetBW /256;
        p[4] = packetBW %256;
        CMostMsgTx *mostMsg = new CMostMsgTx( addr, //Addr
            FBLOCK_NETWORK_MANAGER, // nFBlock
            0, // nInst
            FUNC_NM_NET_STATE, // nFunc
            CMostMsg::OP_STATUS, // nOpType
            sizeof(p), //nPayloadLen
            p, //Payload
            500, //nTimeoutMs
            NULL, //MessageSentCB
            NULL ); //UserContext
        s_mainClass->mostIpc->SendMsg( mostMsg );
        mostMsg->RemoveReference();
    }

public:

    static CNetworkManagerHandler *GetInstance()
    {
        if( NULL == s_mainClass )
            s_mainClass = new CNetworkManagerHandler();
        return s_mainClass;
    }

    static void DestroyInstance()
    {
        if( NULL != s_mainClass )
        {
            delete s_mainClass;
            s_mainClass = NULL;
        }
    }

    void SetPromiscuousMode( bool promiscuousMode )
    {
        network->SetPromiscuousMode( promiscuousMode );
    }

    virtual void Run()
    {
        if( allowThreadRun )
        {
            if( !network->ActionsPending()
                && ( updateCount > 0 )
                && ( 0 == --updateCount ) )
            {
                if( 0 != network->GetAmountOfLocalNodes() )
                {
                    infoContainer.PrintTable( false, false );
                    SendConnectionList();
                }
                else
                {
                    ConsolePrintf( PRIO_ERROR, RED \
                        "No local attached INIC was found. Are the drivers loaded? Was the device found, try 'lsusb'?" \
                        RESETCOLOR"\n" );
                }
            }
        }
        usleep( 100000 );
    }

    void SendConnectionList()
    {
        char *pBuffer = ( char * )malloc( IPC_TEMP_BUFFER_SIZE );
        uint32_t used = s_mainClass->infoContainer.SerializeToString( pBuffer, IPC_TEMP_BUFFER_SIZE );
        for( uint32_t i = 0; i < connectionListener.GetCount(); i++ )
        {
            CMostMsgTx *mostMsg = new CMostMsgTx( connectionListener.GetListener( i ), //Addr
                FBLOCK_NETWORK_MANAGER, // nFBlock
                0, // nInst
                FUNC_NM_CONNECTION_LIST, // nFunc
                CMostMsg::OP_STATUS, // nOpType
                used, //nPayloadLen
                ( uint8_t * )pBuffer, //Payload
                500, //nTimeoutMs
                NULL, //MessageSentCB
                NULL ); //UserContext
            s_mainClass->mostIpc->SendMsg( mostMsg );
            mostMsg->RemoveReference();
        }
        free( pBuffer );
    }

    /*----------------------------------------------------------*/
    /*! \brief Determines, if there are ongoing MOST transactions enqueued.
    *  \return true, if there are still transactions ongoing. false, the system is idle.
    */
    /*----------------------------------------------------------*/
    bool ActionsPending()
    {
        return network->ActionsPending();
    }

    /*----------------------------------------------------------*/
    /*! \brief Loads the given configuration XML file. This will start up the MOST ring, and the
    *         connections will be created.
    *  \param szConfigXml - string holding only the filename to the XML file.
    *  \param szSearchPath - string holding the path to all the XML files.
    *  \return true, if the file could be parsed, and the initial MOST setup was successful.
    */
    /*----------------------------------------------------------*/
    bool LoadConfig( const char *szConfigXml, const char *szSearchPath )
    {
        return network->LoadConfig( szConfigXml, szSearchPath );
    }

    /*----------------------------------------------------------*/
    /*! \brief Connects the given source with the given sink. The index is relative to the list,
    reported by the PrintTable method of the CConnectionInfo class.
    *  \return true, if the connection may be acceptable. The transactions were enqueued to be performed asynchronously.
    *          false, the connection can not be established.
    */
    /*----------------------------------------------------------*/
    bool ConnectDevices( uint32_t sourceIndex, uint32_t sinkIndex )
    {
        bool success = false;
        if( sourceIndex < infoContainer.GetAllInfoAmount()
            && sinkIndex < infoContainer.GetAllInfoAmount() )
        {
            CConnectionInfo *source = infoContainer.GetInfo( sourceIndex );
            CConnectionInfo *sink = infoContainer.GetInfo( sinkIndex );
            success = network->ConnectSourceToSink( source->macAddr, source->channelId, sink->macAddr,
                sink->channelId );
            if( success )
            {
                sink->mostConnectionLabel = source->mostConnectionLabel;
            }
        }
        return success;
    }

    /*----------------------------------------------------------*/
    /*! \brief Retrieves the corresponding sink index for the given MAC address
    *  \param pMacAddress - Pointer to the byte array with length 6, holding the MAC address
    *  \return The sink index if found, otherwise 0xFFFFFFFF
    */
    /*----------------------------------------------------------*/
    uint32_t GetSinkIndex( uint8_t *pMacAddress )
    {
        uint32_t returnVal = 0xFFFFFFFF;
        if( NULL == pMacAddress )
            return returnVal;
        CMacAddr *mac1 = new CMacAddr( pMacAddress[0], pMacAddress[1],
            pMacAddress[2], pMacAddress[3], pMacAddress[4], pMacAddress[5] );
        for( uint32_t i = 0; i < infoContainer.GetAllInfoAmount(); i++ )
        {
            CConnectionInfo *info = infoContainer.GetInfo( i );
            if( NULL == info )
                continue;
            CMacAddr *mac2 = info->macAddr;
            if( *mac1 == *mac2 )
            {
                if( !info->isTX )
                {
                    returnVal = i;
                    break;
                }
            }
        }
        delete mac1;
        return returnVal;
    }

    /*----------------------------------------------------------*/
    /*! \brief Sends the given Control Message out to the given MOST ring.
    *  \param mostInstance - The MOST ring instance, starting with 0 for the first MOST ring.
    *  \param targetAddr - The MOST target address (0x100, 0x401, etc..)
    *  \param nFBlock - The MOST Function Block Identifier
    *  \param nInst - The MOST Function Block Instance Identifier
    *  \param nFunc - The MOST Function Block Function Identifier
    *  \param nPayloadLen - The amount of bytes stored in Payload
    *  \param Payload - The pointer to the payload byte array.
    */
    /*----------------------------------------------------------*/
    bool SendMostControlMessage( TMostInstace mostInstance, uint32_t targetAddr, uint32_t nFBlock, uint32_t nInst,
        uint32_t nFunc, uint8_t nOpType, uint32_t nPayloadLen, const uint8_t *Payload )
    {
        return network->SendMostControlMessage( mostInstance, targetAddr, nFBlock, nInst, nFunc, nOpType, nPayloadLen,
            Payload );
    }

    bool GetRingBreakDiagnosisResult()
    {
        return network->GetRingBreakDiagnosisResult( 0, 1 );
    }

    void DebugEnableMost(bool enabled)
    {
        network->EnableMost(enabled);
    }

    /*----------------------------------------------------------*/
    /*! \brief Sends the given Control Message out to the given MOST ring.
    *  \param mostInstance - The MOST ring instance, starting with 0 for the first MOST ring.
    *  \param deviceId - The device identifier (group address) as specified in the XML file.
    *  \param devInst - The instance number of the device. Starting with 0 for the first device with deviceId.
    *  \param nFBlock - The MOST Function Block Identifier
    *  \param nInst - The MOST Function Block Instance Identifier
    *  \param nFunc - The MOST Function Block Function Identifier
    *  \param nPayloadLen - The amount of bytes stored in Payload
    *  \param Payload - The pointer to the payload byte array.
    */
    /*----------------------------------------------------------*/
    bool SendMostControlMessage( TMostInstace mostInstance, TDeviceId deviceId, uint8_t devInst, uint32_t nFBlock,
        uint32_t nInst, uint32_t nFunc, uint8_t nOpType, uint32_t nPayloadLen, const uint8_t *Payload )
    {
        return network->SendMostControlMessage( mostInstance, deviceId, devInst, nFBlock, nInst, nFunc, nOpType,
            nPayloadLen, Payload );
    }
    
    virtual void OnNetworkState( uint8_t mostInstance, bool available, uint8_t maxPos, uint16_t packetBW )
    {
        ConsolePrintf( PRIO_LOW, ">>>>>>>>>>>>>> OnNetworkState available=%s, mpr=%d, sbc=%d\n",
                      available ? "yes" : "no", maxPos, packetBW );
        
        if (mostInstance >= AMOUNT_OF_NETWORK_STATES)
        {
            ConsolePrintf(PRIO_ERROR, RED"OnNetworkState MOST instance is out "\
                "of range, please increase AMOUNT_OF_NETWORK_STATES define"RESETCOLOR"\n");
        }
        else
        {
            allStates[mostInstance].isValid = true;
            allStates[mostInstance].available = available;
            allStates[mostInstance].maxPos = maxPos;
            allStates[mostInstance].packetBW = packetBW;
        }
        
        for( uint32_t i = 0; i < netStateListener.GetCount(); i++ )
        {
            SendNetworkState( netStateListener.GetListener( i ),mostInstance, available, maxPos, packetBW );
        }
    }

    virtual void OnChannelAvailable( CMacAddr *macAddr, TDeviceId deviceId, uint8_t devInst, TChannelId channelId,
        TMostInstace mostInstance, EPDataType_t dataType, TBlockwidth reservedBandwidth, bool isSourceDevice,
        bool inSocketCreated, bool outSocketCreated, bool socketsConnected, int32_t bufferSize,
        uint32_t mostConnectionLabel, int16_t splittedOffset, int16_t splittedMaxBandwidth, uint16_t packetsPerXact,
        const char *deviceName )
    {
        if( NULL != macAddr )
        {
            CConnectionInfo *info = infoContainer.GetInfo( macAddr, channelId, mostInstance, isSourceDevice );
            if( NULL == info )
                return;
            updateCount = updateCountMaxVal;
            info->deviceType = deviceId;
            info->deviceInstance = devInst;
            info->dataType = dataType;
            info->reservedBandwidth = reservedBandwidth;
            info->bufferSize = bufferSize;
            info->inSocketCreated = inSocketCreated;
            info->outSocketCreated = outSocketCreated;
            info->socketsConnected = socketsConnected;
            info->mostConnectionLabel = mostConnectionLabel;
            info->splittedOffset = splittedOffset;
            info->splittedMaxBandwidth = splittedMaxBandwidth;
            info->packetsPerXact = packetsPerXact;
            if( NULL != deviceName && strlen( deviceName ) != 0 )
            {
                strncpy( info->deviceName, deviceName, sizeof( info->deviceName ) );
            }
        }
        else
        {
            ConsolePrintf( PRIO_LOW, ">>>>>>>>>>>>>> OnChannelAvailable, MAC:unknown, channel-Id:%d, " \
                "MOST-Inst:%d, TX:%d, in:%d, out:%d, con:%d, name:%s\n", channelId, mostInstance, isSourceDevice,
                inSocketCreated, outSocketCreated, socketsConnected, deviceName );
        }
    }

    virtual void OnChannelUnavailable( CMacAddr *macAddr, TChannelId channelId, uint8_t mostInstance )
    {
        updateCount = updateCountMaxVal;
        ConsolePrintf( PRIO_LOW, ">>>>>>>>>>>>>> OnChannelUnavailable, MAC:%s, channel-Id:%d" \
            ", MOST-Inst:%d\n", macAddr->ToString(), ( uint32_t )channelId, mostInstance );

        infoContainer.DestroyInfo( macAddr, channelId, mostInstance );
    }

    virtual void OnMostControlMessage( uint8_t devInst, uint32_t sourceAddr, uint32_t targetAddr, uint32_t nFBlock,
        uint32_t nInst, uint32_t nFunc, uint8_t nOpType, uint32_t nPayloadLen, const uint8_t *Payload )
    {
        ConsolePrintfStart( PRIO_MEDIUM, ">>>>>>>>>>>>>> OnMostControlMessage, source:0x%X, " \
            "target:0x%X, FBlock:0x%X, Inst:%d, Func:0x%X, OP:0x%X, Len:%d, P:0x[", sourceAddr, targetAddr, nFBlock,
            nInst, nFunc, nOpType, nPayloadLen );
        for( uint32_t i = 0; NULL != Payload && i < nPayloadLen; i++ )
            ConsolePrintfContinue( "%02X ", Payload[i] );
        ConsolePrintfExit( " ]\n" );

        uint32_t bufLen = sizeof( MostMcmAdr_t ) + nPayloadLen;
        MostMcmAdr_t *mcm = ( MostMcmAdr_t * )calloc( 1, bufLen );
        if( NULL == mcm )
        {
            ConsolePrintf( PRIO_ERROR, RED"Failed to allocate memory: OnMostControlMessage"RESETCOLOR"\n" );
            return;
        }
        mcm->sourceAddr = sourceAddr;
        mcm->targetAddr = targetAddr;
        mcm->nFBlock = nFBlock;
        mcm->nInst = nInst;
        mcm->nFunc = nFunc;
        mcm->nOpType = nOpType;
        mcm->nPayloadLen = nPayloadLen;
        uint8_t *pBuffer = ( uint8_t * )mcm;
        memcpy( &pBuffer[sizeof( MostMcmAdr_t )], Payload, nPayloadLen );

        for( uint32_t i = 0; i < mcmListener.GetCount(); i++ )
        {
            CMsgAddr *c = mcmListener.GetListener( i );
            if( NULL == c )
                break;
            CMostMsgTx *mostMsg = new CMostMsgTx( c, FBLOCK_NETWORK_MANAGER,
                0, FUNC_NM_RECEIVED_MCM, CMostMsg::OP_STATUS, bufLen,
                pBuffer, 500, NULL, NULL );
            mostIpc->SendMsg( mostMsg );
            mostMsg->RemoveReference();
        }
    }

    virtual void OnRingBreakDiagnosisResultV3( uint8_t devInst, uint16_t nodeAddress, uint8_t result,
        uint8_t position, uint8_t status, uint16_t id )
    {
        uint8_t pBuffer[8];
        pBuffer[0] = devInst;
        pBuffer[1] = nodeAddress / 256;
        pBuffer[2] = nodeAddress % 256;
        pBuffer[3] = result;
        pBuffer[4] = position;
        pBuffer[5] = status;
        pBuffer[6] = id / 256;
        pBuffer[7] = id % 256;

        for( uint32_t i = 0; i < rbdListener.GetCount(); i++ )
        {
            CMsgAddr *c = rbdListener.GetListener( i );
            if( NULL == c )
                break;
            CMostMsgTx *mostMsg = new CMostMsgTx( c, FBLOCK_NETWORK_MANAGER,
                0, FUNC_NM_RING_BREAK_DIAGNOSIS, CMostMsg::OP_STATUS, sizeof( pBuffer ),
                pBuffer, 500, NULL, NULL );
            mostIpc->SendMsg( mostMsg );
            mostMsg->RemoveReference();
        }
    }
};

static void PrintMenu()
{
    ConsolePrintfStart( PRIO_HIGH, "Main Menu, press key and enter:\n" );
    ConsolePrintfContinue( "-------------------------------\n" );
    ConsolePrintfContinue( " m - Print this menu\n" );
    ConsolePrintfContinue( " p - Print Connection List\n" );
    ConsolePrintfContinue( " c - Connect Source and Sink\n" );
    ConsolePrintfContinue( " s - Send sample control message to MOST ring\n" );
    ConsolePrintfContinue( " r - Get Ring Break Diagnosis Result\n" );
    ConsolePrintfContinue( " x - Exit this application\n" );
    ConsolePrintfContinue( "==Debugging==\n" );
    ConsolePrintfContinue( " 0 - Turn off MOST\n" );
    ConsolePrintfContinue( " 1 - Turn on MOST\n" );
    ConsolePrintfExit( "-------------------------------\n" );
}

int main( int argc, char *argv[] )
{
    int sourceIndex, sinkIndex;

    ConsoleInit( false );

    char *configName = NULL;
    char *searchPath = NULL;
    bool disableMenu = true;
    ConsolePrio_t prio = PRIO_HIGH;
    bool promiscuousMode = false;

    if( 2 == argc && strstr( argv[1], ".xml" ) )
    {
        configName = argv[1];
    }
    else
    {
        for( int32_t i = 1; i < argc; i++ )
        {
            if( strstr( argv[i], "-i" ) )
            {
                if( argc <= ( i + 1 ) )
                {
                    ConsolePrintf( PRIO_ERROR,
                        RED"-i parameter requires configuration filename as next parameter"RESETCOLOR"\n" );
                    return -1;
                }
                configName = argv[i + 1];
                i++;
            }
            else if( strstr( argv[i], "-p" ) )
            {
                if( argc <= ( i + 1 ) )
                {
                    ConsolePrintf( PRIO_ERROR,
                        RED"-p parameter requires search path as next parameter"RESETCOLOR"\n" );
                    return -1;
                }
                searchPath = argv[i + 1];
                i++;
            }
            else if( strstr( argv[i], "-vv" ) )
            {
                prio = PRIO_LOW;
            }
            else if( strstr( argv[i], "-v" ) )
            {
                prio = PRIO_MEDIUM;
            }
            else if( strstr( argv[i], "--help" ) )
            {
                ConsolePrintfStart( PRIO_HIGH, "Usage: %s [OPTION] ... [FILE] ... \n", argv[0] );
                ConsolePrintfContinue(
                    "Example MOST4.0 NetworkManager, setup and configures all devices in an entire MOST network.\n" );

                ConsolePrintfContinue(
                    "\t-i\t\t input configuration file, if there is no other options -i may be left away\n" );
                ConsolePrintfContinue( "\t-p\t\t path to search the configurations file, if not set,"\
                    "the path will be extracted from the input configuration file\n" );
                ConsolePrintfContinue( "\t-m\t\t enable user menu\n" );
                ConsolePrintfContinue( "\t-s\t\t enable MEP promiscuous mode (Ethernet SPY)\n" );
                ConsolePrintfContinue( "\t-v\t\t verbose mode - print debug info\n" );
                ConsolePrintfContinue( "\t-vv\t\t super verbose mode - print even more debug info\n" );
                ConsolePrintfContinue( "Example: %s config.xml\n", argv[0] );
                ConsolePrintfContinue( "Example: %s -i /root/config.xml\n", argv[0] );
                ConsolePrintfExit( "Example: %s -i config.xml -p /root\n", argv[0] );
                return 0;
            }
            else if( strstr( argv[i], "-m" ) )
            {
                disableMenu = false;
            }
            else if( strstr( argv[i], "-s" ) )
            {
                ConsolePrintf( PRIO_HIGH, YELLOW"Warning promiscuous mode is activated (-s),"\
                    " this may have negative impact on system load"RESETCOLOR"\n" );
                promiscuousMode = true;
            }
            else
            {
                ConsolePrintf( PRIO_ERROR, RED"Ignoring command line parameter"\
                    " at pos %d: %s, type '%s --help' to see the correct parameter syntax"\
                    RESETCOLOR"\n", i, argv[i], argv[0] );
            }
        }
    }
    if( NULL == searchPath && NULL != configName )
    {
        uint32_t len = strlen( configName );
        if( 0 != len )
        {
            uint32_t stLen = ( len + 1 );
            searchPath = ( char * )calloc( stLen, sizeof( char ) );
            strncpy( searchPath, configName, stLen );
            int32_t pos = len;
            for(; pos >= 0; pos-- )
            {
                if( '/' == searchPath[pos] )
                {
                    searchPath[pos] = '\0';
                    configName = &searchPath[pos + 1];
                    break;
                }
            }
            if( pos <= 0 )
                strncpy( searchPath, ".", stLen );
        }
    }

    ConsoleSetPrio( prio );
    ConsolePrintf( PRIO_HIGH, BLUE"========== Network Manager Start =========="RESETCOLOR"\n" );
    CNetworkManagerHandler *mainClass = CNetworkManagerHandler::GetInstance();
    mainClass->SetPromiscuousMode( promiscuousMode );

    if( NULL != configName && NULL != searchPath )
    {
        ConsolePrintf( PRIO_HIGH, "Starting with configuration:'%s'. Search path:'%s'\n", configName, searchPath );
        if( !s_mainClass->LoadConfig( configName, searchPath ) )
        {
            ConsolePrintf( PRIO_ERROR, RED"Failed to load configuration"RESETCOLOR"\n" );
        }
    }
    else
    {
        ConsolePrintf( PRIO_HIGH, "Starting without configuration. It must then be loaded via IPC command\n" );
    }

    int32_t timeout = 5;
    do
    {
        usleep( 1000000 );
    }
    while( mainClass->ActionsPending() && --timeout > 0 );
    if( timeout <= 0 )
        ConsolePrintf( PRIO_ERROR,
            RED"Warning: main routine waited unusually long for finishing network setup"RESETCOLOR"\n" );

    if( disableMenu || !isatty( fileno( stdin ) ) )
    {
        while( true )
            sleep( 1 );
    }
    else
    {
        PrintMenu();

        while( true )
        {
            int c = getchar();
            fflush( stdin );
            switch( c )
            {
            case 'M':
            case 'm':
                PrintMenu();
                break;

            case 'R':
            case 'r':
                mainClass->GetRingBreakDiagnosisResult();
                break;

            case 'P':
            case 'p':
                mainClass->infoContainer.PrintTable( false, false );
                break;

            case 'C':
            case 'c':
                mainClass->infoContainer.PrintTable( false, false );
                ConsolePrintf( PRIO_HIGH, "Enter Source and Target Index:" );
                if( 0 != scanf( "%d %d", &sourceIndex, &sinkIndex ) )
                {
                    ConsolePrintf( PRIO_HIGH, "Connecting Source Index %d with Sink Index %d\n", sourceIndex,
                        sinkIndex );
                    if( mainClass->ConnectDevices( sourceIndex, sinkIndex ) )
                    {
                        do
                        {
                            usleep( 100000 );
                            ConsolePrintf( PRIO_HIGH, "Wait for connection to be finished.\n" );
                        }
                        while( mainClass->ActionsPending() );
                        mainClass->infoContainer.PrintTable( false, false );
                    }
                    else
                    {
                        ConsolePrintf( PRIO_ERROR, RED"Failed to connect devices"RESETCOLOR"\n" );
                    }
                }
                break;

            case 'S':
            case 's':
                {
                    char test[] = "Hello World!\0";
                    mainClass->SendMostControlMessage( 0, 0x3C8, 0xF0, 0, 0x100, 1,
                        strlen( test ), ( uint8_t * )test );
                    break;
                }

            case 'X':
            case 'x':
                CNetworkManagerHandler::DestroyInstance();
                usleep( 1000000 );
                ConsolePrintf( PRIO_HIGH, BLUE"========== Network Manager End =========="RESETCOLOR"\n" );
                ConsoleDeinit();
                return 0;
            case '0':
                mainClass->DebugEnableMost(false);
                break;
            case '1':
                mainClass->DebugEnableMost(true);
                break;
            default:
                break;
            }
            usleep( 10000 ); //Avoid high CPU load, if terminal is disconnected (running as daemon)
        }
    }
}