<commit_before>/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

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

#include "mitkIGTLDevice.h"
//#include "mitkIGTException.h"
//#include "mitkIGTTimeStamp.h"
#include <itkMutexLockHolder.h>
#include <itksys/SystemTools.hxx>
#include <cstring>
#include <chrono>

#include <igtlTransformMessage.h>
#include <mitkIGTLMessageCommon.h>

#include <igtl_status.h>

//remove later
#include <igtlTrackingDataMessage.h>

#include <igtlTrackingDataMessage.h>

static const int SOCKET_SEND_RECEIVE_TIMEOUT_MSEC = 1;

typedef itk::MutexLockHolder<itk::FastMutexLock> MutexLockHolder;

mitk::IGTLDevice::IGTLDevice() :
//  m_Data(mitk::DeviceDataUnspecified),
  m_State(mitk::IGTLDevice::Setup),
  m_Name("Unspecified Device"),
  m_StopCommunication(false),
  m_Hostname("127.0.0.1"),
  m_PortNumber(-1),
  m_MultiThreader(nullptr), m_SendThreadID(0), m_ReceiveThreadID(0), m_ConnectThreadID(0)
{
  m_StopCommunicationMutex = itk::FastMutexLock::New();
  m_StateMutex = itk::FastMutexLock::New();
//  m_LatestMessageMutex = itk::FastMutexLock::New();
  m_SendingFinishedMutex = itk::FastMutexLock::New();
  m_ReceivingFinishedMutex = itk::FastMutexLock::New();
  m_ConnectingFinishedMutex = itk::FastMutexLock::New();
  // execution rights are owned by the application thread at the beginning
  m_SendingFinishedMutex->Lock();
  m_ReceivingFinishedMutex->Lock();
  m_ConnectingFinishedMutex->Lock();
  m_MultiThreader = itk::MultiThreader::New();
//  m_Data = mitk::DeviceDataUnspecified;
//  m_LatestMessage = igtl::MessageBase::New();

  m_MessageFactory = mitk::IGTLMessageFactory::New();
  m_SendQueue      = mitk::IGTLMessageQueue::New();
  m_ReceiveQueue   = mitk::IGTLMessageQueue::New();
  m_CommandQueue   = mitk::IGTLMessageQueue::New();

  //setup measurements
  this->m_Measurement = mitk::IGTLMeasurements::GetInstance();
}


mitk::IGTLDevice::~IGTLDevice()
{
  /* stop communication and disconnect from igtl device */
  if (GetState() == Running)
  {
    this->StopCommunication();
  }
  if (GetState() == Ready)
  {
    this->CloseConnection();
  }
  /* cleanup tracking thread */
  if (m_MultiThreader.IsNotNull())
  {
     if ((m_SendThreadID != 0))
     {
        m_MultiThreader->TerminateThread(m_SendThreadID);
     }
     if ((m_ReceiveThreadID != 0))
     {
        m_MultiThreader->TerminateThread(m_ReceiveThreadID);
     }
     if ((m_ConnectThreadID != 0))
     {
        m_MultiThreader->TerminateThread(m_ConnectThreadID);
     }
  }
  m_MultiThreader = nullptr;
}

mitk::IGTLDevice::IGTLDeviceState mitk::IGTLDevice::GetState() const
{
  MutexLockHolder lock(*m_StateMutex);
  return m_State;
}


void mitk::IGTLDevice::SetState( IGTLDeviceState state )
{
  itkDebugMacro("setting  m_State to " << state);

  m_StateMutex->Lock();
//  MutexLockHolder lock(*m_StateMutex); // lock and unlock the mutex

  if (m_State == state)
  {
    m_StateMutex->Unlock();
    return;
  }
  m_State = state;
  m_StateMutex->Unlock();
  this->Modified();
}


bool mitk::IGTLDevice::TestConnection()
{

  return true;
}

unsigned int mitk::IGTLDevice::ReceivePrivate(igtl::Socket* socket)
{
  // Create a message buffer to receive header
  igtl::MessageHeader::Pointer headerMsg;
  headerMsg = igtl::MessageHeader::New();

  // Initialize receive buffer
  headerMsg->InitPack();

  // Receive generic header from the socket
  int r =
     socket->Receive(headerMsg->GetPackPointer(), headerMsg->GetPackSize(), 0);

  MITK_INFO << "Server received r = " << r;

  if (r == 0) //connection error
  {
    // an error was received, therefor the communication with this socket
    // must be stoppedy
    return IGTL_STATUS_NOT_PRESENT;
  }
  else if (r == -1 ) //timeout
  {
    // a timeout was received, this is no error state, thus, do nothing
    return IGTL_STATUS_TIME_OUT;
  }
  else if (r == headerMsg->GetPackSize())
  {
    MITK_INFO << "Is Pack Size";
    // Deserialize the header and check the CRC
    // ERROR HERE: This probably means the header data is corrupted...
    int crcCheck = headerMsg->Unpack(1);

    MITK_INFO << "CRC Check: " << crcCheck << " Bool: " << ((crcCheck & igtl::MessageHeader::UNPACK_HEADER) == true);
    MITK_INFO << "headerMsg: " << headerMsg;

    if (crcCheck & igtl::MessageHeader::UNPACK_HEADER)
    {
      // Allocate a time stamp
      igtl::TimeStamp::Pointer ts;
      ts = igtl::TimeStamp::New();

      // Get time stamp
      igtlUint32 sec;
      igtlUint32 nanosec;

      headerMsg->GetTimeStamp(ts);
      ts->GetTimeStamp(&sec, &nanosec);

//      std::cerr << "Time stamp: "
//                << sec << "."
//                << nanosec << std::endl;

//      std::cerr << "Dev type and name: " << headerMsg->GetDeviceType() << " "
//                << headerMsg->GetDeviceName() << std::endl;

//      headerMsg->Print(std::cout);

      //check the type of the received message
      //if it is a GET_, STP_ or RTS_ command push it into the command queue
      //otherwise continue reading the whole message from the socket
      const char* curDevType = headerMsg->GetDeviceType();
      if ( std::strstr( curDevType, "GET_" ) != nullptr ||
           std::strstr( curDevType, "STP_" ) != nullptr ||
           std::strstr( curDevType, "RTS_" ) != nullptr)
      {
        this->m_CommandQueue->PushMessage(headerMsg);
        this->InvokeEvent(CommandReceivedEvent());
        return IGTL_STATUS_OK;
      }

      //Create a message according to the header message
      igtl::MessageBase::Pointer curMessage;
      curMessage = m_MessageFactory->CreateInstance(headerMsg);

      //check if the curMessage is created properly, if not the message type is
      //not supported and the message has to be skipped
      if ( curMessage.IsNull() )
      {
        socket->Skip(headerMsg->GetBodySizeToRead(), 0);
        MITK_ERROR("IGTLDevice") << "The received type is not supported. Please "
                                    "add it to the message factory.";
        return IGTL_STATUS_NOT_FOUND;
      }

      //insert the header to the message and allocate the pack
      curMessage->SetMessageHeader(headerMsg);
      curMessage->AllocatePack();

      // Receive transform data from the socket
      int receiveCheck = 0;
      receiveCheck = socket->Receive(curMessage->GetPackBodyPointer(),
         curMessage->GetPackBodySize(), 0);


      // measure the time
      long long timeStamp6 = std::chrono::high_resolution_clock::now().time_since_epoch().count();

      if ( receiveCheck > 0 )
      {
        int c = curMessage->Unpack(1);
        if ( !(c & igtl::MessageHeader::UNPACK_BODY) )
        {
          return IGTL_STATUS_CHECKSUM_ERROR;
        }

        //save timestamp 6 now because we know the index
        // Here again, is a explicit conversion to TrackingDataMessage ONLY... Why?
        igtl::TrackingDataMessage* tdMsg =<commit_msg>we want to read everything<commit_after>/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

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

#include "mitkIGTLDevice.h"
//#include "mitkIGTException.h"
//#include "mitkIGTTimeStamp.h"
#include <itkMutexLockHolder.h>
#include <itksys/SystemTools.hxx>
#include <cstring>
#include <chrono>

#include <igtlTransformMessage.h>
#include <mitkIGTLMessageCommon.h>

#include <igtl_status.h>

//remove later
#include <igtlTrackingDataMessage.h>

#include <igtlTrackingDataMessage.h>

static const int SOCKET_SEND_RECEIVE_TIMEOUT_MSEC = 1;

typedef itk::MutexLockHolder<itk::FastMutexLock> MutexLockHolder;

mitk::IGTLDevice::IGTLDevice() :
//  m_Data(mitk::DeviceDataUnspecified),
  m_State(mitk::IGTLDevice::Setup),
  m_Name("Unspecified Device"),
  m_StopCommunication(false),
  m_Hostname("127.0.0.1"),
  m_PortNumber(-1),
  m_MultiThreader(nullptr), m_SendThreadID(0), m_ReceiveThreadID(0), m_ConnectThreadID(0)
{
  m_StopCommunicationMutex = itk::FastMutexLock::New();
  m_StateMutex = itk::FastMutexLock::New();
//  m_LatestMessageMutex = itk::FastMutexLock::New();
  m_SendingFinishedMutex = itk::FastMutexLock::New();
  m_ReceivingFinishedMutex = itk::FastMutexLock::New();
  m_ConnectingFinishedMutex = itk::FastMutexLock::New();
  // execution rights are owned by the application thread at the beginning
  m_SendingFinishedMutex->Lock();
  m_ReceivingFinishedMutex->Lock();
  m_ConnectingFinishedMutex->Lock();
  m_MultiThreader = itk::MultiThreader::New();
//  m_Data = mitk::DeviceDataUnspecified;
//  m_LatestMessage = igtl::MessageBase::New();

  m_MessageFactory = mitk::IGTLMessageFactory::New();
  m_SendQueue      = mitk::IGTLMessageQueue::New();
  m_ReceiveQueue   = mitk::IGTLMessageQueue::New();
  m_CommandQueue   = mitk::IGTLMessageQueue::New();

  //setup measurements
  this->m_Measurement = mitk::IGTLMeasurements::GetInstance();
}


mitk::IGTLDevice::~IGTLDevice()
{
  /* stop communication and disconnect from igtl device */
  if (GetState() == Running)
  {
    this->StopCommunication();
  }
  if (GetState() == Ready)
  {
    this->CloseConnection();
  }
  /* cleanup tracking thread */
  if (m_MultiThreader.IsNotNull())
  {
     if ((m_SendThreadID != 0))
     {
        m_MultiThreader->TerminateThread(m_SendThreadID);
     }
     if ((m_ReceiveThreadID != 0))
     {
        m_MultiThreader->TerminateThread(m_ReceiveThreadID);
     }
     if ((m_ConnectThreadID != 0))
     {
        m_MultiThreader->TerminateThread(m_ConnectThreadID);
     }
  }
  m_MultiThreader = nullptr;
}

mitk::IGTLDevice::IGTLDeviceState mitk::IGTLDevice::GetState() const
{
  MutexLockHolder lock(*m_StateMutex);
  return m_State;
}


void mitk::IGTLDevice::SetState( IGTLDeviceState state )
{
  itkDebugMacro("setting  m_State to " << state);

  m_StateMutex->Lock();
//  MutexLockHolder lock(*m_StateMutex); // lock and unlock the mutex

  if (m_State == state)
  {
    m_StateMutex->Unlock();
    return;
  }
  m_State = state;
  m_StateMutex->Unlock();
  this->Modified();
}


bool mitk::IGTLDevice::TestConnection()
{

  return true;
}

unsigned int mitk::IGTLDevice::ReceivePrivate(igtl::Socket* socket)
{
  // Create a message buffer to receive header
  igtl::MessageHeader::Pointer headerMsg;
  headerMsg = igtl::MessageHeader::New();

  // Initialize receive buffer
  headerMsg->InitPack();

  // Receive generic header from the socket
  int r =
     socket->Receive(headerMsg->GetPackPointer(), headerMsg->GetPackSize(), 0);

  MITK_INFO << "Server received r = " << r;

  if (r == 0) //connection error
  {
    // an error was received, therefor the communication with this socket
    // must be stoppedy
    return IGTL_STATUS_NOT_PRESENT;
  }
  else if (r == -1 ) //timeout
  {
    // a timeout was received, this is no error state, thus, do nothing
    return IGTL_STATUS_TIME_OUT;
  }
  else if (r == headerMsg->GetPackSize())
  {
    MITK_INFO << "Is Pack Size";
    // Deserialize the header and check the CRC
    // ERROR HERE: This probably means the header data is corrupted...
    int crcCheck = headerMsg->Unpack(1);

    MITK_INFO << "CRC Check: " << crcCheck << " Bool: " << ((crcCheck & igtl::MessageHeader::UNPACK_HEADER) == true);
    MITK_INFO << "headerMsg: " << headerMsg;

    if (crcCheck & igtl::MessageHeader::UNPACK_HEADER)
    {
      // Allocate a time stamp
      igtl::TimeStamp::Pointer ts;
      ts = igtl::TimeStamp::New();

      // Get time stamp
      igtlUint32 sec;
      igtlUint32 nanosec;

      headerMsg->GetTimeStamp(ts);
      ts->GetTimeStamp(&sec, &nanosec);

//      std::cerr << "Time stamp: "
//                << sec << "."
//                << nanosec << std::endl;

//      std::cerr << "Dev type and name: " << headerMsg->GetDeviceType() << " "
//                << headerMsg->GetDeviceName() << std::endl;

//      headerMsg->Print(std::cout);

      //check the type of the received message
      //if it is a GET_, STP_ or RTS_ command push it into the command queue
      //otherwise continue reading the whole message from the socket
      const char* curDevType = headerMsg->GetDeviceType();
      if ( std::strstr( curDevType, "GET_" ) != nullptr ||
           std::strstr( curDevType, "STP_" ) != nullptr ||
           std::strstr( curDevType, "RTS_" ) != nullptr)
      {
        this->m_CommandQueue->PushMessage(headerMsg);
        this->InvokeEvent(CommandReceivedEvent());
        return IGTL_STATUS_OK;
      }

      //Create a message according to the header message
      igtl::MessageBase::Pointer curMessage;
      curMessage = m_MessageFactory->CreateInstance(headerMsg);

      //check if the curMessage is created properly, if not the message type is
      //not supported and the message has to be skipped
      if ( curMessage.IsNull() )
      {
        socket->Skip(headerMsg->GetBodySizeToRead(), 0);
        MITK_ERROR("IGTLDevice") << "The received type is not supported. Please "
                                    "add it to the message factory.";
        return IGTL_STATUS_NOT_FOUND;
      }

      //insert the header to the message and allocate the pack
      curMessage->SetMessageHeader(headerMsg);
      curMessage->AllocatePack();

      // Receive transform data from the socket
      int receiveCheck = 0;
      receiveCheck = socket->Receive(curMessage->GetPackBodyPointer(),
         curMessage->GetPackBodySize(), 1);


      // measure the time
      long long timeStamp6 = std::chrono::high_resolution_clock::now().time_since_epoch().count();

      if ( receiveCheck > 0 )
      {
        int c = curMessage->Unpack(1);
        if ( !(c & igtl::MessageHeader::UNPACK_BODY) )
        {
          return IGTL_STATUS_CHECKSUM_ERROR;
        }

        //save timestamp 6 now because we know the index
        // Here again, is a explicit conversion to TrackingDataMessage ONLY... Why?
        igtl::TrackingDataMessage* tdMsg =
{'fraction_non_alphanumeric': 0.14190535064221443, 'fraction_numerical': 0.00398379009547359, 'mean_word_length': 3.500772797527048, 'pattern_counts': {'":': 0, '<': 107, '<?xml version=': 0, '>': 99, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 2, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3731847', 'n_tokens_mistral': 4643, 'n_tokens_neox': 4586, 'n_words': 1060}