/*==========================================================================

Portions (c) Copyright 2018 IGSIO

See Doc/copyright/copyright.txt
or http://www.slicer.org/copyright/copyright.txt for details.

Program:   igtlio
Module:    $RCSfile: igtlioConnector.cxx,v $
Date:      $Date: 2018/06/28 11:34:29 $
Version:   $Revision: 1.4 $

=========================================================================auto=*/

// std includes
#include <iostream>
#include <map>
#include <sstream>
#include <string>

// OpenIGTLink includes
#include <igtl_header.h>
#include <igtlClientSocket.h>
#include <igtlCommandMessage.h>
#include <igtlMessageBase.h>
#include <igtlMessageHeader.h>
#include <igtlOSUtil.h>
#include <igtlServerSocket.h>

// VTK includes
#include <vtkCollection.h>
#include <vtkCommand.h>
#include <vtkImageData.h>
#include <vtkIntArray.h>
#include <vtkMultiThreader.h>
#include <vtkObjectFactory.h>
#include <vtkTimerLog.h>
#include <vtkVersionMacros.h>
#if VTK_MAJOR_VERSION < 9 // change to std::recursive_mutex with v9.0.0
#include <vtkMutexLock.h>
#endif

// vtksys includes
#include <vtksys/SystemTools.hxx>

// OpenIGTLinkIO includes
#include "igtlioCircularBuffer.h"
#include "igtlioCircularSectionBuffer.h"
#include "igtlioCommandConverter.h"

#include "igtlioConnector.h"

#pragma push_macro("SendMessage")
#undef SendMessage

//------------------------------------------------------------------------------
vtkStandardNewMacro(igtlioConnector);
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
const char *igtlioConnector::ConnectorTypeStr[igtlioConnector::NUM_TYPE] =
{
  "?", // TYPE_NOT_DEFINED
  "S", // TYPE_SERVER
  "C", // TYPE_CLIENT
};

//----------------------------------------------------------------------------
const char *igtlioConnector::ConnectorStateStr[igtlioConnector::NUM_STATE] =
{
  "OFF",       // OFF
  "WAIT",      // WAIT_CONNECTION
  "ON",        // CONNECTED
};

//----------------------------------------------------------------------------
igtlioConnector::igtlioConnector()
  : Type(TYPE_CLIENT)
  , State(STATE_OFF)
  , Persistent(PERSISTENT_OFF)
  , Thread(vtkMultiThreaderPointer::New())
  , ConnectionThreadID(-1)
  , ServerHostname("localhost")
  , ServerPort(18944)
  , ServerStopFlag(false)
  , RestrictDeviceName(0)
  , PushOutgoingMessageFlag(0)
  , DeviceFactory(igtlioDeviceFactoryPointer::New())
  , CheckCRC(true)
  , NextCommandID(1)
  , NextClientID(1)
{

}

//----------------------------------------------------------------------------
igtlioConnector::~igtlioConnector()
{
  this->Stop();
}

//----------------------------------------------------------------------------
void igtlioConnector::PrintSelf(ostream& os, vtkIndent indent)
{
  Superclass::PrintSelf(os,indent);

  if (this->Type == TYPE_SERVER)
    {
    os << indent << "Connector Type : SERVER\n";
    os << indent << "Listening Port #: " << this->ServerPort << "\n";
    }
  else if (this->Type == TYPE_CLIENT)
    {
    os << indent << "Connector Type: CLIENT\n";
    os << indent << "Server Hostname: " << this->ServerHostname << "\n";
    os << indent << "Server Port #: " << this->ServerPort << "\n";
    }

  switch (this->State)
    {
    case STATE_OFF:
      os << indent << "State: OFF\n";
      break;
    case STATE_WAIT_CONNECTION:
      os << indent << "State: WAIT FOR CONNECTION\n";
      break;
    case STATE_CONNECTED:
      os << indent << "State: CONNECTED\n";
      break;
    }
  os << indent << "Persistent: " << this->Persistent << "\n";
  os << indent << "Restrict Device Name: " << this->RestrictDeviceName << "\n";
  os << indent << "Push Outgoing Message Flag: " << this->PushOutgoingMessageFlag << "\n";
  os << indent << "Check CRC: " << (this->CheckCRC ? "true" : "false") << "\n";
  os << indent << "Number of devices: " << this->GetNumberOfDevices() << "\n";
  os << indent << "NextCommandID: " << this->NextCommandID << "\n";
}

//----------------------------------------------------------------------------
const char* igtlioConnector::GetServerHostname()
{
  return this->ServerHostname.c_str();
}

//----------------------------------------------------------------------------
void igtlioConnector::SetServerHostname(std::string str)
{
  if (this->ServerHostname.compare(str) == 0)
    {
    return;
    }
  this->ServerHostname = str;
  this->Modified();
}

//----------------------------------------------------------------------------
int igtlioConnector::SetTypeServer(int port)
{
  if (this->Type == TYPE_SERVER
      && this->ServerPort == port)
    {
    return 1;
    }
  this->Type = TYPE_SERVER;
  this->ServerPort = port;
  this->Modified();
  return 1;
}

//----------------------------------------------------------------------------
int igtlioConnector::SetTypeClient(std::string hostname, int port)
{
  if (this->Type == TYPE_CLIENT
      && this->ServerPort == port
      && this->ServerHostname.compare(hostname) == 0)
    {
    return 1;
    }
  this->Type = TYPE_CLIENT;
  this->ServerPort = port;
  this->ServerHostname = hostname;
  this->Modified();
  return 1;
}

//----------------------------------------------------------------------------
void igtlioConnector::SetCheckCRC(bool c)
{
  this->CheckCRC = c;
}

//---------------------------------------------------------------------------
int igtlioConnector::Start()
{
  // Check if type is defined.
  if (this->Type == igtlioConnector::TYPE_NOT_DEFINED)
    {
      //vtkErrorMacro("Connector type is not defined.");
    return 0;
    }

  if (this->ConnectionThreadID >= 0)
    {
    vtkErrorMacro("Connector is already running!");
    return 0;
    }

  this->ServerStopFlag = false;
  this->ConnectionThreadID = this->Thread->SpawnThread((vtkThreadFunctionType)&igtlioConnector::ConnectionAcceptThreadFunction, this);

  // Following line is necessary in some Linux environment,
  // since it takes for a while for the thread to update
  // this->State to non STATE_OFF value. This causes error
  // after calling vtkMRMLIGTLConnectorNode::Start() in ProcessGUIEvent()
  // in vtkOpenIGTLinkIFGUI class.
  this->State = STATE_WAIT_CONNECTION;
  this->InvokeEvent(igtlioConnector::ActivatedEvent);

  return 1;
}

//---------------------------------------------------------------------------
int igtlioConnector::Stop()
{
  // Check if thread exists
  if (this->ConnectionThreadID >= 0)
  {
    if (this->Type == TYPE_SERVER && this->ServerSocket.IsNotNull())
    {
      this->ServerSocket->CloseSocket();
    }

    // NOTE: Thread should be killed by activating ServerStopFlag.
    this->ServerStopFlag = true;
    while(this->ConnectionThreadID >= 0 || this->Sockets.size() > 0)
     {
      // Sleep required to keep loop from hanging in release mode
      igtl::Sleep(1);
     }

    return 1;
    }
  else
    {
    return 0;
    }
}

//---------------------------------------------------------------------------
void* igtlioConnector::ReceiverThreadFunction(void* ptr)
{
  vtkMultiThreader::ThreadInfo* vinfo = static_cast<vtkMultiThreader::ThreadInfo*>(ptr);
  igtlioConnector* connector = static_cast<igtlioConnector*>(vinfo->UserData);
  int currentThreadID = vinfo->ThreadID;
  int clientID = -1;
  bool connected = true;
  // Communication -- common to both Server and Client
  while (!connector->ServerStopFlag && connected)
    {
    if (clientID == -1)
      {
      std::vector<int> clientIds = connector->GetClientIds();
      for (std::vector<int>::iterator clientIdIt = clientIds.begin(); clientIdIt != clientIds.end(); ++clientIdIt)
        {
        Client client = connector->GetClient(*clientIdIt);
        if (client.ThreadID == currentThreadID)
          {
            clientID = *clientIdIt;
            break;
          }
        }
      }

    connected = connector->ReceiveController(clientID);
    }

  connector->RemoveClient(clientID);

  // Signal to the threader that this thread has become free
#if VTK_MAJOR_VERSION < 9 // change to std::recursive_mutex with v9.0.0
  vinfo->ActiveFlagLock->Lock();
#else
  vinfo->ActiveFlagLock->lock();
#endif
  (*vinfo->ActiveFlag) = 0;
#if VTK_MAJOR_VERSION < 9 // change to std::recursive_mutex with v9.0.0
  vinfo->ActiveFlagLock->Unlock();
#else
  vinfo->ActiveFlagLock->unlock();
#endif
  return NULL; //why???
}

//----------------------------------------------------------------------------
igtlioCommandPointer igtlioConnector::GetOutgoingCommand(int commandId, int clientId)
{
  std::lock_guard<std::recursive_mutex> lock(this->OutgoingCommandDequeMutex);
  igtlioCommandPointer command = NULL;
  if (!this->OutgoingCommandDeque.empty())
    {
    igtlioCommandDequeType::iterator outgoingCommandIt = (std::find_if(
      this->OutgoingCommandDeque.begin(),
      this->OutgoingCommandDeque.end(),
      [this, commandId, clientId](const igtlioCommandPointer& entry)
      { return entry->GetCommandId() == commandId && (entry->GetClientId() == clientId || entry->GetClientId() == -1); }));
    if (outgoingCommandIt != this->OutgoingCommandDeque.end())
      {
      command = *outgoingCommandIt;
      }
    }
  return command;
}

//----------------------------------------------------------------------------
void igtlioConnector::RequestInvokeEvent(unsigned long eventId)
{
  std::lock_guard<std::recursive_mutex> lock(this->EventQueueMutex);
  this->EventQueue.push_back(eventId);
}

//----------------------------------------------------------------------------
void igtlioConnector::RequestPushOutgoingMessages()
{
  std::lock_guard<std::recursive_mutex> lock(this->PushOutgoingMessageMutex);
  this->PushOutgoingMessageFlag = 1;
}

//----------------------------------------------------------------------------
void* igtlioConnector::ConnectionAcceptThreadFunction(void* ptr)
{
  vtkMultiThreader::ThreadInfo* vinfo = static_cast<vtkMultiThreader::ThreadInfo*>(ptr);
  igtlioConnector* connector = static_cast<igtlioConnector*>(vinfo->UserData);

  connector->State = STATE_WAIT_CONNECTION;
  if (connector->Type == TYPE_SERVER)
    {
    connector->ServerSocket = igtl::ServerSocket::New();
    if (connector->ServerSocket->CreateServer(connector->ServerPort) == -1)
      {
      vtkErrorWithObjectMacro(connector, "Failed to create server socket !");
      connector->ServerStopFlag = true;
      }
    }

  while (!connector->ServerStopFlag)
    {
    if (connector->Type == TYPE_SERVER)
      {
      igtl::ClientSocket::Pointer client = connector->ServerSocket->WaitForConnection(1000);
      if (client.IsNotNull()) // if client connected
        {
        std::lock_guard<std::recursive_mutex> lock(connector->ClientMutex);
        int clientThreadID = connector->Thread->SpawnThread((vtkThreadFunctionType)&igtlioConnector::ReceiverThreadFunction, connector);
        Client clientInfo = Client(connector->NextClientID++, client, clientThreadID);
        connector->Sockets.push_back(clientInfo);
        connector->RequestInvokeEvent(ClientConnectedEvent);
        }
      }
    else if (connector->Type == TYPE_CLIENT) // if this->Type == TYPE_CLIENT
      {
      if (connector->Sockets.empty())
        {
          igtl::ClientSocket::Pointer socket = igtl::ClientSocket::New();
          int r = socket->ConnectToServer(connector->ServerHostname.c_str(), connector->ServerPort, false);
          if (r == 0) // if connected to server
          {
            std::lock_guard<std::recursive_mutex> lock(connector->ClientMutex);
            int clientThreadID = connector->Thread->SpawnThread((vtkThreadFunctionType)&igtlioConnector::ReceiverThreadFunction, connector);
            Client clientInfo = Client(0, socket, clientThreadID);
            connector->Sockets.push_back(clientInfo);
            connector->RequestInvokeEvent(ClientConnectedEvent);
          }
        }
      }
    else
      {
      connector->ServerStopFlag = true;
      }

    if (connector->IsConnected())
      {
      if (connector->State == STATE_WAIT_CONNECTION)
        {
        connector->State = STATE_CONNECTED;
        // need to Request the InvokeEvent, because we are not on the main thread now
        connector->RequestInvokeEvent(igtlioConnector::ConnectedEvent);
        }
      }
    else if (connector->State == STATE_CONNECTED)
      {
      connector->State = STATE_WAIT_CONNECTION;
      connector->RequestInvokeEvent(igtlioConnector::DisconnectedEvent); // need to Request the InvokeEvent, because we are not on the main thread now
      }

    // Delay is required for situations where the ConnectToServer function exits without delay
    // Without it, the loop will be processed extremely quickly and cause the program to hang
    igtl::Sleep(100);
    }

  if (connector->Type == TYPE_SERVER && connector->ServerSocket.IsNotNull())
    {
    connector->ServerSocket->CloseSocket();
    }

  std::lock_guard<std::recursive_mutex> lock(connector->ClientMutex);
  for (std::vector<Client>::iterator clientIt = connector->Sockets.begin(); clientIt != connector->Sockets.end(); ++clientIt)
    {
    if (clientIt->Socket.IsNotNull())
      {
      clientIt->Socket->CloseSocket();
      }
    }

  connector->ConnectionThreadID = -1;
  connector->State = STATE_OFF;
  connector->RequestInvokeEvent(igtlioConnector::DeactivatedEvent); // need to Request the InvokeEvent, because we are not on the main thread now

  // Signal to the threader that this thread has become free
#if VTK_MAJOR_VERSION < 9 // change to std::recursive_mutex with v9.0.0
  vinfo->ActiveFlagLock->Lock();
#else
  vinfo->ActiveFlagLock->lock();
#endif
  (*vinfo->ActiveFlag) = 0;
#if VTK_MAJOR_VERSION < 9 // change to std::recursive_mutex with v9.0.0
  vinfo->ActiveFlagLock->Unlock();
#else
  vinfo->ActiveFlagLock->unlock();
#endif
  return 0;
}

//----------------------------------------------------------------------------
bool igtlioConnector::ReceiveController(int clientID)
{
  igtl::MessageHeader::Pointer headerMsg;
  headerMsg = igtl::MessageHeader::New();

  Client client = this->GetClient(clientID);
  if (client.ID == -1)
  {
    return false;
  }

  // check if connection is alive
  if (!client.Socket->GetConnected())
  {
    return false;
  }

  //----------------------------------------------------------------
  // Receive Header
  headerMsg->InitPack();

  vtkDebugMacro("Waiting for header of size: " << headerMsg->GetPackSize());

  // This may need to be parallelized so that other socket aren't waiting on timeouts
  bool timeout(false);
  int r = client.Socket->Receive(headerMsg->GetPackPointer(), headerMsg->GetPackSize(), timeout);

  vtkDebugMacro("Received header of size: " << headerMsg->GetPackSize());

  if (r != headerMsg->GetPackSize())
  {
    vtkDebugMacro("ignoring header, breaking. received=" << r);
    return false;
  }

  // Deserialize the header
  headerMsg->Unpack();

  //----------------------------------------------------------------
  // Check Device Name
  // Nov 16, 2010: Currently the following code only checks
  // if the device name is defined in the message.
  const char* devName = headerMsg->GetDeviceName();
  if (devName[0] == '\0')
  {
    /// Dec 7, 2010: Removing the following code, since message without
    /// device name should be handled in the MRML scene as well.
    //// If no device name is defined, skip processing the message.
    //this->Skip(headerMsg->GetBodySizeToRead());
    //continue; //  while (!this->ServerStopFlag)
  }
  //----------------------------------------------------------------
  // If device name is restricted
  else if (this->RestrictDeviceName)
  {
    // Check if the node has already been registered.
    igtlioDeviceKeyType key = igtlioDeviceKeyType::CreateDeviceKey(headerMsg);
    int registered = this->GetDevice(key).GetPointer() != NULL;
    if (registered == 0)
    {
      this->Skip(headerMsg->GetBodySizeToRead(), client);
      return true;

    }
  }

  vtkDebugMacro("completed read header : " << headerMsg->GetDeviceName() << " body size to read: " << headerMsg->GetBodySizeToRead());

  //----------------------------------------------------------------
  // Search Circular Buffer
  igtlioDeviceKeyType key = igtlioDeviceKeyType::CreateDeviceKey(headerMsg);

  // Intercept command devices before they are added to the circular buffer, and add them to the command queue
  if (std::strcmp(headerMsg->GetDeviceType(), "COMMAND") == 0 || std::strcmp(headerMsg->GetDeviceType(), "RTS_COMMAND") == 0)
  {
    if (this->ReceiveCommandMessage(headerMsg, client))
    {
      return true;
    }
  }

  igtlioCircularSectionBufferPointer circBuffer = this->GetCircularSectionBuffer(key, clientID);
  if (!circBuffer)
  {
    circBuffer = igtlioCircularSectionBufferPointer::New();
    circBuffer->SetPacketMode(igtlioCircularSectionBuffer::SinglePacketMode);
    if (strcmp(headerMsg->GetDeviceType(), "VIDEO") == 0)
    {
      circBuffer->SetPacketMode(igtlioCircularSectionBuffer::MultiplePacketsMode);
    }

    std::lock_guard<std::recursive_mutex> lock(this->CircularBufferMutex);
    this->SectionBuffer[SectionBufferKey(key, clientID)] = circBuffer;
  }

  //----------------------------------------------------------------
  // Load to the circular buffer
  if (circBuffer && circBuffer->StartPush() != -1)
  {
    //std::cerr << "Pushing into the circular buffer." << std::endl;
    circBuffer->StartPush();
    igtl::MessageBase::Pointer buffer = circBuffer->GetPushBuffer();
    buffer->SetMessageHeader(headerMsg);
    buffer->AllocatePack();

    vtkDebugMacro("Waiting to receive body:  size=" << buffer->GetPackBodySize()
      << ", GetBodySizeToRead=" << buffer->GetBodySizeToRead()
      << ", GetPackSize=" << buffer->GetPackSize());
    bool timeout(false);
    int read = client.Socket->Receive(buffer->GetPackBodyPointer(), buffer->GetPackBodySize(), timeout);
    vtkDebugMacro("Received body: " << read);
    if (read != buffer->GetPackBodySize())
    {
      if (!this->ServerStopFlag)
      {
        vtkErrorMacro("Only read " << read << " but expected to read "
          << buffer->GetPackBodySize() << "\n");
      }
      return true;
    }

    circBuffer->EndPush();
  }
  else
  {
    return false;
  }
  return true;
}

//----------------------------------------------------------------------------
bool igtlioConnector::ReceiveCommandMessage(igtl::MessageHeader::Pointer headerMsg, Client& client)
{
  igtlioDeviceKeyType key = igtlioDeviceKeyType::CreateDeviceKey(headerMsg);

  igtl::MessageBase::Pointer buffer = igtl::MessageBase::New();
  buffer->InitBuffer();
  buffer->SetMessageHeader(headerMsg);
  buffer->AllocateBuffer();

  vtkDebugMacro("Waiting to receive body:  size=" << buffer->GetBufferBodySize()
    << ", GetBodySizeToRead=" << buffer->GetBodySizeToRead()
    << ", GetPackSize=" << buffer->GetPackSize());
  bool timeout(false);
  int read = client.Socket->Receive(buffer->GetBufferBodyPointer(), buffer->GetBufferBodySize(), timeout);
  vtkDebugMacro("Received body: " << read);
  if (read != buffer->GetBufferBodySize())
  {
    if (!this->ServerStopFlag)
    {
      vtkErrorMacro("Only read " << read << " but expected to read " << buffer->GetBufferBodySize() << "\n");
    }
    return false;
  }

  std::lock_guard<std::recursive_mutex> lock(this->IncomingCommandQueueMutex);
  this->IncomingCommandQueue.push(IncomingCommandType(client.ID, buffer));

  return true;
}

//----------------------------------------------------------------------------
int igtlioConnector::SendData(igtlUint64 size, unsigned char* data, Client& client)
{
  if (client.Socket.IsNull())
    {
    return 0;
    }

  // check if connection is alive
  if (!client.Socket->GetConnected())
    {
    return 0;
    }

  return client.Socket->Send(data, size);  // return 1 on success, otherwise 0.
}

//----------------------------------------------------------------------------
int igtlioConnector::Skip(igtlUint64 length, Client& client, int skipFully /* = 1 */)
{
  unsigned char dummy[256];
  igtlUint64 block  = 256;
  igtlUint64 n      = 0;
  igtlUint64 remain = length;

  do
    {
    if (remain < block)
      {
      block = remain;
      }

    bool timeout(false);
    n = client.Socket->Receive(dummy, block, timeout, skipFully);
    remain -= n;
    }
  while (remain > 0 || (skipFully && n < block));

  return (length - remain);
}

//----------------------------------------------------------------------------
std::vector<int> igtlioConnector::GetClientIds()
{
  std::vector<int> clientIds;
  std::lock_guard<std::recursive_mutex> lock(this->ClientMutex);
  for (std::vector<Client>::iterator clientIt = this->Sockets.begin(); clientIt != this->Sockets.end(); ++clientIt)
  {
    clientIds.push_back(clientIt->ID);
  }
  return clientIds;
}

//----------------------------------------------------------------------------
igtlioConnector::Client igtlioConnector::GetClient(int clientId)
{
  std::lock_guard<std::recursive_mutex> lock(this->ClientMutex);
  for (std::vector<Client>::iterator clientIt = this->Sockets.begin(); clientIt != this->Sockets.end(); ++clientIt)
  {
    if (clientIt->ID == clientId)
    {
      return *clientIt;
    }
  }

  return Client(-1, nullptr, -1);
}

//----------------------------------------------------------------------------
bool igtlioConnector::RemoveClient(int clientId)
{
  std::lock_guard<std::recursive_mutex> lock(this->ClientMutex);
  std::vector<Client>::iterator clientIt = (std::find_if(
    this->Sockets.begin(),
    this->Sockets.end(),
    [this, clientId](const Client& entry)
  { return entry.ID == clientId; }));

  if (clientIt != this->Sockets.end())
  {
    clientIt->Socket->CloseSocket();
    this->Sockets.erase(clientIt);
    this->RequestInvokeEvent(ClientDisconnectedEvent);
    return true;
  }

  return false;
}

//----------------------------------------------------------------------------
unsigned int igtlioConnector::GetUpdatedSectionBuffersList(NameListType& nameList)
{
  nameList.clear();

  igtlioCircularSectionBufferMap::iterator iter;
  for (iter = this->SectionBuffer.begin(); iter != this->SectionBuffer.end(); iter ++)
    {
    if (iter->second != NULL && iter->second->IsUpdated())
      {
      nameList.push_back(iter->first);
      }
    }
  return nameList.size();
}

//----------------------------------------------------------------------------
igtlioCircularSectionBufferPointer igtlioConnector::GetCircularSectionBuffer(const igtlioDeviceKeyType &key, const int clientID)
{
  igtlioCircularSectionBufferMap::iterator iter = this->SectionBuffer.find(SectionBufferKey(key, clientID));
  if (iter != this->SectionBuffer.end())
    {
    return this->SectionBuffer[SectionBufferKey(key, clientID)]; // the key has been found in the list
    }
  else
    {
    return NULL;  // nothing found
    }
}

//---------------------------------------------------------------------------
void igtlioConnector::ImportDataFromCircularBuffer()
{
  igtlioConnector::NameListType nameList;
  this->GetUpdatedSectionBuffersList(nameList);

  igtlioConnector::NameListType::iterator nameIter;
  for (nameIter = nameList.begin(); nameIter != nameList.end(); nameIter ++)
    {
    igtlioDeviceKeyType key = nameIter->Key;
    int clientID = nameIter->ClientID;

    igtlioCircularSectionBuffer* circBuffer = this->GetCircularSectionBuffer(key, clientID);
    circBuffer->StartPull();
    igtlioDevicePointer device = NULL;
    while(circBuffer->IsSectionBufferInProcess())
    {
    igtl::MessageBase::Pointer messageFromBuffer = circBuffer->GetPullBuffer();

    igtlioDeviceCreatorPointer deviceCreator = DeviceFactory->GetCreator(key.GetBaseTypeName());

    if (!deviceCreator)
      {
      vtkErrorMacro(<< "Received unknown device type " << messageFromBuffer->GetDeviceType() << ", device=" << messageFromBuffer->GetDeviceName());
      continue;
      }

    device = this->GetDevice(key);

    if ((device.GetPointer()!=NULL) && !(igtlioDeviceKeyType::CreateDeviceKey(device)==igtlioDeviceKeyType::CreateDeviceKey(messageFromBuffer)))
        {
          vtkErrorMacro(
              << "Received an IGTL message of the wrong type, device=" << key.name
              << " has type " << device->GetDeviceType()
        << " got type " << messageFromBuffer->GetDeviceType()
                );
          continue;
        }

    if (!device && !this->RestrictDeviceName)
      {
        device = deviceCreator->Create(key.name);
        this->AddDevice(device);
      }

      device->SetClientID(clientID);
      device->SetMessageDirection(igtlioDevice::MESSAGE_DIRECTION_IN);
      device->InvokeEvent(igtlioDevice::AboutToReceiveEvent);
      device->ReceiveIGTLMessage(messageFromBuffer, this->CheckCRC);
      device->InvokeEvent(igtlioDevice::ReceiveEvent);
    }
    if (device)
      {
      device->Modified();
      }
    circBuffer->EndPull();
    }
}

//---------------------------------------------------------------------------
void igtlioConnector::ImportEventsFromEventBuffer()
{
  // Invoke all events in the EventQueue

  bool emptyQueue=true;
  unsigned long eventId=0;
  do
  {
    emptyQueue=true;
    std::lock_guard<std::recursive_mutex> lock(this->EventQueueMutex);
    if (this->EventQueue.size()>0)
    {
      eventId=this->EventQueue.front();
      this->EventQueue.pop_front();
      emptyQueue=false;

      // Invoke the event
      this->InvokeEvent(eventId);
    }

  } while (!emptyQueue);
}

//---------------------------------------------------------------------------
void igtlioConnector::PushOutgoingMessages()
{
  int push = 0;

  // Read PushOutgoingMessageFlag and reset it.
  {
    std::lock_guard<std::recursive_mutex> lock(this->PushOutgoingMessageMutex);
    push = this->PushOutgoingMessageFlag;
    this->PushOutgoingMessageFlag = 0;
  }

  if (push)
    {
      std::lock_guard<std::recursive_mutex> lock(this->DeviceMutex);
      for (unsigned i=0; i<this->Devices.size(); ++i)
        {
          if (this->Devices[i]->MessageDirectionIsOut() && this->Devices[i]->GetPushOnConnect())
            this->PushNode(this->Devices[i]);
        }
    }
}

//----------------------------------------------------------------------------
void igtlioConnector::PeriodicProcess()
{
  this->ParseCommands();
  this->PruneCompletedCommands();

  this->ImportDataFromCircularBuffer();
  this->ImportEventsFromEventBuffer();
  this->PushOutgoingMessages();
}

//----------------------------------------------------------------------------
igtlioCommandPointer igtlioConnector::SendCommand(std::string name, std::string content, IGTLIO_SYNCHRONIZATION_TYPE synchronized, double timeout_s/*=5*/, igtl::MessageBase::MetaDataMap* metaData/*=NULL*/, int clientID/*=-1*/)
{
  igtlioCommandPointer command = igtlioCommandPointer::New();
  command->SetClientId(clientID);
  command->SetName(name);
  command->SetCommandContent(content);
  command->SetTimeoutSec(timeout_s);
  command->SetBlocking(synchronized == IGTLIO_BLOCKING);
  if (metaData)
    {
    command->SetCommandMetaData(*metaData);
    }

  int success = this->SendCommand(command);
  if (!success)
    {
    return NULL;
    }
  return command;
}

//----------------------------------------------------------------------------
int igtlioConnector::SendCommand(igtlioCommandPointer command)
{
  if (command->IsInProgress())
    {
    vtkWarningMacro("SendCommand: Command " << command->GetCommandId() << "-" << command->GetName() << " is already in progress! Attempting to cancel and resend.");
    this->CancelCommand(command);
    }

  command->SetCommandId(this->NextCommandID++);
  command->SetDirectionOut();

  igtlioBaseConverter::HeaderData headerData = igtlioBaseConverter::HeaderData();
  headerData.deviceName = "command";
  headerData.timestamp = vtkTimerLog::GetUniversalTime();

  igtlioCommandConverter::ContentData content = igtlioCommandConverter::ContentData();
  content.id = command->GetCommandId();
  content.name = command->GetName();
  content.content = command->GetCommandContent();

  igtl::CommandMessage::Pointer commandMessage = igtl::CommandMessage::New();
  igtl::MessageBase::MetaDataMap metaData = command->GetCommandMetaData();
  igtlioCommandConverter::toIGTL(headerData, content, &commandMessage, metaData);

  Client client = this->GetClient(command->GetClientId());
  if (client.ID == -1)
  {
    bool result = true;
    std::vector<int> clientIds = this->GetClientIds();
    for (std::vector<int>::iterator clientIdIt = clientIds.begin(); clientIdIt != clientIds.end(); ++clientIdIt)
    {
      Client currentClient = this->GetClient(*clientIdIt);
      if (currentClient.ID == -1)
      {
        continue;
      }

      int success = this->SendData(commandMessage->GetBufferSize(), (unsigned char*)commandMessage->GetBufferPointer(), currentClient);
      if (success)
      {
        {
          std::lock_guard<std::recursive_mutex> lock(this->OutgoingCommandDequeMutex);
          this->OutgoingCommandDeque.push_back(command);
        }
        command->SetStatus(igtlioCommandStatus::CommandWaiting);
        command->SetSentTimestamp(vtkTimerLog::GetUniversalTime());

        while (command->GetBlocking() && !command->IsCompleted())
        {
          this->PeriodicProcess();
          vtksys::SystemTools::Delay(5);
        }
      }
      else
      {
        vtkErrorMacro("Could not send command " << command->GetCommandId() << ": \"" << command->GetName() << "\"to client");
        command->SetStatus(igtlioCommandStatus::CommandFailed);
      }
      result &= success == 1;
    }
    return result ? 1 : 0;
  }
  else
  {
    int success = this->SendData(commandMessage->GetBufferSize(), (unsigned char*)commandMessage->GetBufferPointer(), client);
    if (success)
    {
      {
        std::lock_guard<std::recursive_mutex> lock(this->OutgoingCommandDequeMutex);
        this->OutgoingCommandDeque.push_back(command);
      }
      command->SetStatus(igtlioCommandStatus::CommandWaiting);
      command->SetSentTimestamp(vtkTimerLog::GetUniversalTime());

      while (command->GetBlocking() && !command->IsCompleted())
      {
        this->PeriodicProcess();
        vtksys::SystemTools::Delay(5);
      }
    }
    else
    {
      vtkErrorMacro("Could not send command " << command->GetCommandId() << ": \"" << command->GetName() << "\"to client");
      command->SetStatus(igtlioCommandStatus::CommandFailed);
    }
    return success;
  }
}

//----------------------------------------------------------------------------
int igtlioConnector::ConnectedClientsCount() const
{
  int total(0);
  std::lock_guard<std::recursive_mutex> lock(this->ClientMutex);
  for (std::vector<Client>::const_iterator clientIt = this->Sockets.begin(); clientIt != this->Sockets.end(); ++clientIt)
  {
    if (clientIt->Socket->GetConnected())
    {
      total++;
    }
  }

  return total;
}

//----------------------------------------------------------------------------
int igtlioConnector::SendCommandResponse(int commandId, int clientId/*=-1*/)
{
  igtlioCommandPointer response = igtlioCommandPointer::New();
  response->SetCommandId(commandId);

  return this->SendCommandResponse(response);
}

//----------------------------------------------------------------------------
int igtlioConnector::SendCommandResponse(igtlioCommandPointer command)
{

  if (command->GetStatus() != igtlioCommandStatus::CommandWaiting)
    {
    return 0;
    }

  igtl::RTSCommandMessage::Pointer responseMessage = igtl::RTSCommandMessage::New();
  igtlioBaseConverter::HeaderData headerData = igtlioBaseConverter::HeaderData();
  igtlioCommandConverter::ContentData content = igtlioCommandConverter::ContentData();
  content.id = command->GetCommandId();
  content.name = command->GetName();
  content.content = command->GetResponseContent();

  igtl::MessageBase::MetaDataMap metaData = command->GetResponseMetaData();
  igtlioCommandConverter::toIGTLResponse(headerData, content, &responseMessage, metaData);

  Client client = GetClient(command->GetClientId());
  if (client.ID == -1)
  {
    return 0;
  }

  int success = this->SendData(responseMessage->GetBufferSize(), (unsigned char*)responseMessage->GetBufferPointer(), client);
  command->SetStatus(igtlioCommandStatus::CommandResponseSent);
  return success;
}

//----------------------------------------------------------------------------
void igtlioConnector::CancelCommand(int commandId, int clientId/*=-1*/)
{
  igtlioCommandPointer command = this->GetOutgoingCommand(commandId, clientId);
  this->CancelCommand(command);
}

//----------------------------------------------------------------------------
void igtlioConnector::CancelCommand(igtlioCommandPointer command)
{
  if (!command)
    {
    return;
    }

  if (command->IsInProgress())
    {
    command->SetStatus(igtlioCommandStatus::CommandCancelled);
    this->PruneCompletedCommands();
    this->InvokeEvent(igtlioCommand::CommandCancelledEvent, command.GetPointer());
    command->InvokeEvent(igtlioCommand::CommandCancelledEvent);
    }
}

//----------------------------------------------------------------------------
void igtlioConnector::ParseCommands()
{
  std::lock_guard<std::recursive_mutex> lock(this->IncomingCommandQueueMutex);

  while (!this->IncomingCommandQueue.empty())
    {
    int clientID = this->IncomingCommandQueue.front().ClientID;
    igtl::MessageBase::Pointer message = this->IncomingCommandQueue.front().CommandMessage;
    this->IncomingCommandQueue.pop();

    if (message->GetMessageType() == "COMMAND")
      {
      igtlioCommandConverter::ContentData content = igtlioCommandConverter::ContentData();
      igtlioBaseConverter::HeaderData headerData = igtlioBaseConverter::HeaderData();
      igtl::MessageBase::MetaDataMap metaData = igtl::MessageBase::MetaDataMap();
      igtlioCommandConverter::fromIGTL(message, &headerData, &content, false, metaData);

      igtlioCommandPointer command = igtlioCommandPointer::New();
      command->SetClientId(clientID);
      command->SetCommandId(content.id);
      command->SetName(content.name);
      command->SetCommandContent(content.content);
      command->SetCommandMetaData(metaData);
      command->SetStatus(igtlioCommandStatus::CommandWaiting);
      command->SetDirectionIn();

      this->InvokeEvent(igtlioCommand::CommandReceivedEvent, command.GetPointer());
      }
    else if (message->GetMessageType() == "RTS_COMMAND")
      {
      igtl::RTSCommandMessage::Pointer responseMessage = static_cast<igtl::RTSCommandMessage*>(message.GetPointer());
      igtlioCommandConverter::ContentData content = igtlioCommandConverter::ContentData();
      igtlioBaseConverter::HeaderData headerData = igtlioBaseConverter::HeaderData();
      igtl::MessageBase::MetaDataMap metaData = igtl::MessageBase::MetaDataMap();
      igtlioCommandConverter::fromIGTLResponse(message, &headerData, &content, false, metaData);

      igtlioCommandPointer command = this->GetOutgoingCommand(content.id, clientID);
      if (!command)
        {
        vtkWarningMacro("Response for command ID: " << content.id << " from client " << clientID << "  matches no outgoing commands!");
        continue;
        }
      command->SetResponseContent(content.content);
      command->SetResponseMetaData(metaData);
      command->SetStatus(igtlioCommandStatus::CommandResponseReceived);

      this->InvokeEvent(igtlioCommand::CommandResponseEvent, command.GetPointer());
      command->InvokeEvent(igtlioCommand::CommandResponseEvent);
      }
    }
}

//----------------------------------------------------------------------------
void igtlioConnector::PruneCompletedCommands()
{
  igtlioCommandDequeType completedCommands = igtlioCommandDequeType();
    {
    std::lock_guard<std::recursive_mutex> lock(this->OutgoingCommandDequeMutex);

    for (igtlioCommandDequeType::iterator outgoingCommandIt = this->OutgoingCommandDeque.begin();
         outgoingCommandIt != this->OutgoingCommandDeque.end(); ++outgoingCommandIt)
      {
      igtlioCommandPointer command = (*outgoingCommandIt);
      double currentTimestamp = vtkTimerLog::GetUniversalTime();
      double elapsedTimeSec = currentTimestamp - command->GetSentTimestamp();
      if (command->GetStatus() == CommandWaiting && elapsedTimeSec > command->GetTimeoutSec())
        {
        completedCommands.push_back(command);
        command->SetStatus(CommandExpired);
        this->InvokeEvent(igtlioCommand::CommandExpiredEvent, command.GetPointer());
        command->InvokeEvent(igtlioCommand::CommandExpiredEvent);
        }
      else if (command->GetStatus() == CommandResponseReceived || command->GetStatus() == CommandCancelled)
        {
        completedCommands.push_back(command);
        }
      }

    // Remove completed commands from the deque
    for (igtlioCommandDequeType::iterator completedCommandIt = completedCommands.begin();
       completedCommandIt != completedCommands.end(); ++completedCommandIt)
      {
      igtlioCommandPointer command = (*completedCommandIt);
      this->OutgoingCommandDeque.erase(
        std::remove(this->OutgoingCommandDeque.begin(),
                    this->OutgoingCommandDeque.end(),
                    command),
        this->OutgoingCommandDeque.end());
      }
    }

  // Invoke igtlioCommand::CommandCompletedEvent on all pruned commands
  for (igtlioCommandDequeType::iterator completedCommandIt = completedCommands.begin();
       completedCommandIt != completedCommands.end(); ++completedCommandIt)
    {
    igtlioCommandPointer command = (*completedCommandIt);
    this->InvokeEvent(igtlioCommand::CommandCompletedEvent, command.GetPointer());
    command->InvokeEvent(igtlioCommand::CommandCompletedEvent);
    }
}

//----------------------------------------------------------------------------
igtlioDevicePointer igtlioConnector::AddDeviceIfNotPresent(igtlioDeviceKeyType key)
{
  igtlioDevicePointer device = GetDevice(key);

  if (!device)
  {
    device = GetDeviceFactory()->create(key.type, key.name);
    this->AddDevice(device);
  }

  return device;
}

//----------------------------------------------------------------------------
int igtlioConnector::AddDevice(igtlioDevicePointer device)
{
  if (this->GetDevice(igtlioDeviceKeyType::CreateDeviceKey(device))!=NULL)
    {
    vtkErrorMacro("Failed to add igtl device: " << device->GetDeviceName() << " already present");
    return 0;
    }

  device->SetTimestamp(vtkTimerLog::GetUniversalTime());
  {
  std::lock_guard<std::recursive_mutex> lock(this->DeviceMutex);
  this->Devices.push_back(device);
  }

  //TODO: listen to device events?
  unsigned int deviceEvent = device->GetDeviceContentModifiedEvent();
  device->AddObserver((unsigned long)deviceEvent, this, &igtlioConnector::DeviceContentModified);
  this->InvokeEvent(igtlioConnector::NewDeviceEvent, device.GetPointer());
  return 1;
}

//----------------------------------------------------------------------------
void igtlioConnector::DeviceContentModified(vtkObject *caller, unsigned long event, void *callData )
{
  igtlioDevice* modifiedDevice = reinterpret_cast<igtlioDevice*>(callData);
  if (modifiedDevice)
    {
      this->InvokeEvent(igtlioConnector::DeviceContentModifiedEvent, modifiedDevice);
    }
}

//----------------------------------------------------------------------------
int igtlioConnector::RemoveDevice(igtlioDevicePointer device)
{
  std::lock_guard<std::recursive_mutex> lock(this->DeviceMutex);
  igtlioDeviceKeyType key = igtlioDeviceKeyType::CreateDeviceKey(device);
  for (unsigned i=0; i< this->Devices.size(); ++i)
  {
    if (igtlioDeviceKeyType::CreateDeviceKey(this->Devices[i])==key)
    {
      this->Devices.erase(this->Devices.begin()+i);
      this->InvokeEvent(igtlioConnector::RemovedDeviceEvent, device.GetPointer());
      return 1;
    }
  }
  vtkErrorMacro("Failed to remove igtl device: " << device->GetDeviceName());
  return 0;
}

//---------------------------------------------------------------------------
unsigned int igtlioConnector::GetNumberOfDevices() const
{
  return this->Devices.size();
}

//---------------------------------------------------------------------------
void igtlioConnector::RemoveDevice(int index)
{
  std::lock_guard<std::recursive_mutex> lock(this->DeviceMutex);
  //TODO: disconnect listen to device events?
  igtlioDevicePointer device = this->Devices[index]; // ensure object lives until event has completed
  this->Devices.erase(this->Devices.begin()+index);
  this->InvokeEvent(igtlioConnector::RemovedDeviceEvent, device.GetPointer());
}

//---------------------------------------------------------------------------
igtlioDevicePointer igtlioConnector::GetDevice(int index)
{
  std::lock_guard<std::recursive_mutex> lock(this->DeviceMutex);
  return this->Devices[index];
}

//---------------------------------------------------------------------------
igtlioDevicePointer igtlioConnector::GetDevice(igtlioDeviceKeyType key)
{
  std::lock_guard<std::recursive_mutex> lock(this->DeviceMutex);
  for (unsigned i=0; i< this->Devices.size(); ++i)
    if (igtlioDeviceKeyType::CreateDeviceKey(this->Devices[i])==key)
      return this->Devices[i];
  return igtlioDevicePointer();
}

//---------------------------------------------------------------------------
bool igtlioConnector::HasDevice(igtlioDevicePointer d )
{
  std::lock_guard<std::recursive_mutex> lock(this->DeviceMutex);
    for(unsigned i=0; i< this->Devices.size(); ++i)
        if( this->Devices[i] == d )
            return true;
    return false;
}

//---------------------------------------------------------------------------
int igtlioConnector::SendMessage(igtlioDeviceKeyType device_id, igtlioDevice::MESSAGE_PREFIX prefix/*=Device::MESSAGE_PREFIX_NOT_DEFINED*/, int clientId)
{
  igtlioDevicePointer device = this->GetDevice(device_id);
  if (!device)
    {
      vtkErrorMacro("Sending OpenIGTLinkMessage: " << device_id.type << "/" << device_id.name<< ", device not found");
      return 0;
    }

  std::set<igtlioDevice::MESSAGE_PREFIX> supportedPrefixes = device->GetSupportedMessagePrefixes();
  if (supportedPrefixes.find(prefix) == supportedPrefixes.end())
    {
    vtkErrorMacro("Sending OpenIGTLinkMessage: " << device_id.type << "/" << device_id.name << ", invalid prefix");
    return 0;
    }

  //TODO replace prefix with message-type or similar - giving the basic message same status as the queries
  igtl::MessageBase::Pointer msg = device->GetIGTLMessage(prefix);

  if (!msg)
    {
      vtkErrorMacro("Sending OpenIGTLinkMessage: " << device_id.type << "/" << device_id.name << ", message not available from device");
      return 0;
    }

  Client client = this->GetClient(clientId);
  if (client.ID == -1)
  {
    bool result = true;
    std::vector<int> clientIds = this->GetClientIds();
    for (std::vector<int>::iterator clientIdIt = clientIds.begin(); clientIdIt != clientIds.end(); ++clientIdIt)
    {
      Client currentClient = this->GetClient(*clientIdIt);
      if (currentClient.ID == -1)
      {
        continue;
      }

      int r = this->SendData(msg->GetPackSize(), (unsigned char*)msg->GetPackPointer(), currentClient);
      if (r == 0)
      {
        vtkDebugMacro("Sending OpenIGTLinkMessage: " << device_id.type << "/" << device_id.name << " failed.");
        result &= false;
      }
      else
      {
        result &= true;
      }
    }
    return result ? 1 : 0;
  }
  else
  {
    int r = this->SendData(msg->GetPackSize(), (unsigned char*)msg->GetPackPointer(), client);
    if (r == 0)
    {
      vtkDebugMacro("Sending OpenIGTLinkMessage: " << device_id.type << "/" << device_id.name << " failed.");
      return 0;
    }
    return r;
  }
}

//----------------------------------------------------------------------------
igtlioDeviceFactoryPointer igtlioConnector::GetDeviceFactory()
{
  return DeviceFactory;
}

//----------------------------------------------------------------------------
void igtlioConnector::SetDeviceFactory(igtlioDeviceFactoryPointer val)
{
  if (val==DeviceFactory)
    return;
  DeviceFactory = val;
  this->Modified();
}

//----------------------------------------------------------------------------
bool igtlioConnector::IsConnected()
{
  std::lock_guard<std::recursive_mutex> lock(this->ClientMutex);
  for (std::vector<Client>::iterator clientIt = this->Sockets.begin(); clientIt != this->Sockets.end(); ++clientIt)
    {
    if (clientIt->Socket.IsNotNull())
      {
      if (clientIt->Socket->GetConnected())
        {
        return true;
        }
      }
    }

  return false;
}

//---------------------------------------------------------------------------
int igtlioConnector::PushNode(igtlioDevicePointer node, int event, int clientId /*=-1*/)
{
  // TODO: verify that removed event argument is OK
  // TODO: clientId doesn't make sense (whole function doesn't make sense), remove as parameter?
  bool result = true;
  std::vector<int> clientIds = this->GetClientIds();
  for (std::vector<int>::iterator clientIdIt = clientIds.begin(); clientIdIt != clientIds.end(); ++clientIdIt)
  {
    result &= (this->SendMessage(igtlioDeviceKeyType::CreateDeviceKey(node), igtlioDevice::MESSAGE_PREFIX_NOT_DEFINED, *clientIdIt) == 1);
  }
  return result ? 1 : 0;
}

//---------------------------------------------------------------------------
bool operator<(const igtlioConnector::SectionBufferKey& lhs, const igtlioConnector::SectionBufferKey& rhs)
{
  if (lhs.Key < rhs.Key)
  {
    return true;
  }
  if (lhs.Key == rhs.Key)
  {
    return lhs.ClientID < rhs.ClientID;
  }
  return false;
};

#pragma pop_macro("SendMessage")
