// SPDX-FileCopyrightText: Copyright (c) Kitware Inc.
// SPDX-FileCopyrightText: Copyright (c) Sandia Corporation
// SPDX-License-Identifier: BSD-3-Clause
#include "pqVRUIConnection.h"

#include "pqActiveObjects.h"
#include "pqDataRepresentation.h"
#include "pqView.h"

#include "vtkCamera.h"
#include "vtkMath.h"
#include "vtkMatrix4x4.h"
#include "vtkObjectFactory.h"
#include "vtkPVXMLElement.h"
#include "vtkRenderWindow.h"
#include "vtkRenderer.h"
#include "vtkSMDoubleVectorProperty.h"
#include "vtkSMPropertyHelper.h"
#include "vtkSMRenderViewProxy.h"
#include "vtkSMRepresentationProxy.h"
#include "vtkStringFormatter.h"
#include "vtkStringScanner.h"
#include "vtkTransform.h"
#include "vtkVRQueue.h"
#include "vtkVRUIPipe.h"
#include "vtkVRUIServerState.h"
#include "vtkVRUITrackerState.h"

#include <QDateTime>
#include <QDebug>
#include <QMutex>
#ifdef QTSOCK
#include <QTcpSocket>
#else
#include <fcntl.h>
#include <netdb.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
#endif
#include <algorithm>
#include <iostream>
#include <sstream>
#include <vector>

// ----------------------------------------------------------------------------
class pqVRUIConnection::pqInternals
{
public:
  // --------------------------------------------------------------------------
  pqInternals()
  {
#ifdef QTSOCK
    this->Socket = false;
#else
    this->Socket = -1;
#endif
    this->Active = false;
    this->Pipe = 0;
    this->State = 0;
    this->StateMutex = 0;
    this->Streaming = false; // streaming
    //  this->PacketSignalCond=0;        // for streaming
    //   this->PacketSignalCondMutex=0; // for streaming
  }

  // --------------------------------------------------------------------------
  ~pqInternals()
  {
    if (this->Pipe != 0)
    {
      delete this->Pipe;
    }
    if (this->State != 0)
    {
      delete this->State;
    }
  }

#ifdef QTSOCK
  QTcpSocket* Socket;
#else
  int Socket;
#endif
  bool Active;
  vtkVRUIPipe* Pipe;
  vtkVRUIServerState* State;
  QMutex* StateMutex;
  bool Streaming; // streaming
  // QWaitCondition *PacketSignalCond; // for streaming
  // QMutex *PacketSignalCondMutex;    // for streaming

  // --------------------------------------------------------------------------
  void initSocket(std::string address, std::string port)
  {
#ifdef QTSOCK
    this->Socket = new QTcpSocket;
#ifdef VRUI_ENABLE_DEBUG
    qDebug() << QString(address.c_str()) << "::" << QString(port.c_str()).toInt();
#endif

    this->Socket->connectToHost(QString(address.c_str()),
      QString(port.c_str()).toInt()); // ReadWrite?
#else
    struct sockaddr_in client_addr;
    struct hostent* hp;

    this->Socket = socket(AF_INET, SOCK_STREAM, 0);
    if (this->Socket < 0)
    {
#ifdef VRUI_ENABLE_DEBUG
      qDebug() << "Error opening stream socket";
      abort();
#endif
    }

    /* Name socket using file system name */
    hp = gethostbyname(address.c_str());
    if (hp == 0)
    {
      vtk::print(stderr, "{:s}: unknown host\n", address);
      return;
    }
    bcopy(hp->h_addr, &client_addr.sin_addr, hp->h_length);

    client_addr.sin_family = AF_INET;
    uint16_t sin_port;
    VTK_FROM_CHARS_IF_ERROR_RETURN(port, sin_port, );
    client_addr.sin_port = htons(sin_port);

    if (::connect(this->Socket, (struct sockaddr*)&client_addr, sizeof(struct sockaddr_in)) < 0)
    { /* TODO: why is this "sockaddr", when the type is "sockaddr_in" ?? */
      close(this->Socket);
      perror("connecting stream socket");
      return;
    }
#endif
  }

  // --------------------------------------------------------------------------
  void initPipe() { this->Pipe = new vtkVRUIPipe(this->Socket); }

  // --------------------------------------------------------------------------
  bool connect()
  {
#ifdef VRUI_ENABLE_DEBUG
    std::cout << "Trying to connect" << std::endl;
#endif
    this->Pipe->Send(vtkVRUIPipe::CONNECT_REQUEST);
    if (!this->Pipe->WaitForServerReply(30000)) // 30s
    {
      std::cerr << "Timeout while waiting for CONNECT_REPLY" << endl;
      delete this->Pipe;
      this->Pipe = 0;
      return false;
    }
    if (this->Pipe->Receive() != vtkVRUIPipe::CONNECT_REPLY)
    {
      std::cerr << "Mismatching message while waiting for CONNECT_REPLY" << endl;
      delete this->Pipe;
      this->Pipe = 0;
      return false;
    }
    this->State = new vtkVRUIServerState;
    this->StateMutex = new QMutex;
    this->Pipe->ReadLayout(this->State);
    return true;
  }

  // --------------------------------------------------------------------------
  void activate()
  {
    if (!this->Active)
    {
      this->Pipe->Send(vtkVRUIPipe::ACTIVATE_REQUEST);
      this->Active = true;
    }
  }

  // --------------------------------------------------------------------------
  void deactivate()
  {
    if (this->Active)
    {
      this->Active = false;
      this->Pipe->Send(vtkVRUIPipe::DEACTIVATE_REQUEST);
    }
  }

  // --------------------------------------------------------------------------
  void startStream()
  {
    if (this->Active)
    {
#ifdef VRUI_ENABLE_DEBUG
      std::cout << "start streaming" << std::endl;
#endif
      this->Streaming = true;
      this->Pipe->Send(vtkVRUIPipe::STARTSTREAM_REQUEST);
    }
  }

  // --------------------------------------------------------------------------
  void stopStream()
  {
    if (this->Streaming)
    {
      this->Streaming = false;
      this->Pipe->Send(vtkVRUIPipe::STOPSTREAM_REQUEST);
    }
  }

  // --------------------------------------------------------------------------
  // Streaming routine called when streaming is used
  void readStream()
  {
    this->StateMutex->lock();
    this->Pipe->ReadState(this->State);
    this->StateMutex->unlock();
    bool done = false;
    vtkVRUIPipe::MessageTag m = this->Pipe->Receive();
    switch (m)
    {
      case vtkVRUIPipe::PACKET_REPLY:
#ifdef VRUI_ENABLE_DEBUG
        std::cout << "thread:PACKET_REPLY ok : tag=" << m << endl;
#endif
        this->StateMutex->lock();
        this->Pipe->ReadState(this->State);
        this->StateMutex->unlock();

        break;
      case vtkVRUIPipe::STOPSTREAM_REPLY:
        std::cout << "thread:STOPSTREAM_REPLY ok : tag=" << m << endl;
        done = true;
        break;
      default:
        std::cerr << "thread: Mismatching message while waiting for PACKET_REPLY: tag="
                  << this->Pipe->GetString(m) << "::" << m << endl;
        done = true;
        break;
    }
  }
};

// ----------------------------------------------------------------------------
pqVRUIConnection::pqVRUIConnection(QObject* parentObject)
  : Superclass(parentObject)
{
  this->Internals = new pqInternals();
  this->Initialized = false;
  this->_Stop = false;
  this->Address = "";
  this->Port = "8555";
  this->Name = "";
  this->Type = "VRUI";
  this->TrackerPresent = false;
  this->ValuatorPresent = false;
  this->ButtonPresent = false;
  this->TrackerTransformPresent = false;
  this->Transformation = vtkMatrix4x4::New();
  this->Matrix = vtkMatrix4x4::New();
  this->ZUpToYUpMatrix = vtkMatrix4x4::New();
}

// ----------------------------------------------------------------------------
pqVRUIConnection::~pqVRUIConnection()
{
  this->ZUpToYUpMatrix->Delete();
  this->Matrix->Delete();
  this->Transformation->Delete();
  this->Internals->stopStream();
  this->Internals->deactivate();
  delete this->Internals;
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::addButton(std::string id, std::string name)
{
  std::stringstream returnStr;
  returnStr << "button." << id;
  this->ButtonMapping[returnStr.str()] = name;
  this->ButtonPresent = true;
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::addValuator(std::string id, std::string name)
{
  std::stringstream returnStr;
  returnStr << "valuator." << id;
  this->ValuatorMapping[returnStr.str()] = name;
  this->ValuatorPresent = true;
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::addTracking(std::string id, std::string name)
{
  std::stringstream returnStr;
  returnStr << "tracker." << id;
  this->TrackerMapping[returnStr.str()] = name;
  this->TrackerPresent = true;
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::setName(std::string name)
{
  this->Name = name;
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::setAddress(std::string address)
{
  this->Address = address;
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::setQueue(vtkVRQueue* queue)
{
  this->EventQueue = queue;
}

// ----------------------------------------------------------------------------
bool pqVRUIConnection::init()
{
  // Initialize the socket connection;
  this->Internals->initSocket(this->Address, this->Port);
  this->Internals->initPipe();
  if (!this->Internals->connect())
    return false;
  this->Internals->activate();
  // this->Internals->startStream();
  this->Initialized = true;
  return true;
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::run()
{
  while (!this->_Stop)
  {
    if (this->Initialized)
    {
      if (this->Internals->Streaming)
      {
        this->Internals->readStream();
      }
      else
      {
        this->callback();
      }
    }
  }
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::stop()
{
  this->_Stop = true;
  this->Internals->stopStream();
  this->Internals->deactivate();
  this->Initialized = false;
  QThread::wait();
}

// ----------------------------------------------------------------------------
std::string pqVRUIConnection::name(int eventType, int id)
{
  std::stringstream returnStr, connection, e;
  if (this->Name.size())
    returnStr << this->Name << ".";
  else
    returnStr << this->Address << ".";
  switch (eventType)
  {
    case VALUATOR_EVENT:
      e << "valuator." << id;
      if (this->ValuatorMapping.find(e.str()) != this->ValuatorMapping.end())
        returnStr << this->ValuatorMapping[e.str()];
      else
        returnStr << e.str();
      break;
    case BUTTON_EVENT:
      e << "button." << id;
      if (this->ButtonMapping.find(e.str()) != this->ButtonMapping.end())
        returnStr << this->ButtonMapping[e.str()];
      else
        returnStr << e.str();
      break;
    case TRACKER_EVENT:
      e << "tracker." << id;
      if (this->TrackerMapping.find(e.str()) != this->TrackerMapping.end())
        returnStr << this->TrackerMapping[e.str()];
      else
        returnStr << e.str();
      break;
  }
  return returnStr.str();
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::verifyConfig(const char* id, const char* name)
{
  if (!id)
  {
    qWarning() << "\"id\" should be specified";
  }
  if (!name)
  {
    qWarning() << "\"name\" should be specified";
  }
}

// ----------------------------------------------------------------------------
bool pqVRUIConnection::configure(vtkPVXMLElement* child, vtkSMProxyLocator*)
{
  bool returnVal = false;
  if (child->GetName() && strcmp(child->GetName(), "VRUIConnection") == 0)
  {
    for (unsigned cc = 0; cc < child->GetNumberOfNestedElements(); ++cc)
    {
      vtkPVXMLElement* e = child->GetNestedElement(cc);
      if (e && e->GetName())
      {
        const char* id = e->GetAttributeOrEmpty("id");
        const char* name = e->GetAttributeOrEmpty("name");
        this->verifyConfig(id, name);

        if (strcmp(e->GetName(), "Button") == 0)
        {
          this->addButton(id, name);
        }
        else if (strcmp(e->GetName(), "Valuator") == 0)
        {
          this->addValuator(id, name);
        }
        else if (strcmp(e->GetName(), "Tracker") == 0)
        {
          this->addTracking(id, name);
        }
        else if (strcmp(e->GetName(), "TrackerTransform") == 0)
        {
          this->configureTransform(e);
        }

        else
        {
          qWarning() << "Unknown Device type: \"" << e->GetName() << "\"";
        }
        returnVal = true;
      }
    }
  }
  return returnVal;
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::configureTransform(vtkPVXMLElement* child)
{
  if (child->GetName() && strcmp(child->GetName(), "TrackerTransform") == 0)
  {
    child->GetVectorAttribute("value", 16, (double*)this->Transformation->Element);
    this->TrackerTransformPresent = true;
  }
}

// ----------------------------------------------------------------------------
vtkPVXMLElement* pqVRUIConnection::saveConfiguration() const
{
  vtkPVXMLElement* child = vtkPVXMLElement::New();
  child->SetName("VRUIConnection");
  child->AddAttribute("name", this->Name.c_str());
  child->AddAttribute("address", this->Address.c_str());
  child->AddAttribute("port", this->Port.c_str());
  saveButtonEventConfig(child);
  saveValuatorEventConfig(child);
  saveTrackerEventConfig(child);
  saveTrackerTransformationConfig(child);
  return child;
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::saveButtonEventConfig(vtkPVXMLElement* child) const
{
  if (!this->ButtonPresent)
    return;
  for (std::map<std::string, std::string>::const_iterator it = this->ButtonMapping.begin();
       it != this->ButtonMapping.end(); ++it)
  {
    std::string key = it->first;
    std::string value = it->second;
    std::replace(key.begin(), key.end(), '.', ' ');
    std::istringstream stm(key);
    std::vector<std::string> token;
    for (;;)
    {
      std::string word;
      if (!(stm >> word))
        break;
      token.push_back(word);
    }
    vtkPVXMLElement* e = vtkPVXMLElement::New();
    if (strcmp(token[0].c_str(), "button") == 0)
    {
      e->SetName("Button");
      e->AddAttribute("id", token[1].c_str());
      e->AddAttribute("name", value.c_str());
    }
    child->AddNestedElement(e);
    e->FastDelete();
  }
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::saveValuatorEventConfig(vtkPVXMLElement* child) const
{
  if (!this->ValuatorPresent)
    return;
  for (std::map<std::string, std::string>::const_iterator it = this->ValuatorMapping.begin();
       it != this->ValuatorMapping.end(); ++it)
  {
    std::string key = it->first;
    std::string value = it->second;
    std::replace(key.begin(), key.end(), '.', ' ');
    std::istringstream stm(key);
    std::vector<std::string> token;
    for (;;)
    {
      std::string word;
      if (!(stm >> word))
        break;
      token.push_back(word);
    }
    vtkPVXMLElement* e = vtkPVXMLElement::New();
    if (strcmp(token[0].c_str(), "valuator") == 0)
    {
      e->SetName("Valuator");
      e->AddAttribute("id", token[1].c_str());
      e->AddAttribute("name", value.c_str());
    }
    child->AddNestedElement(e);
    e->FastDelete();
  }
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::saveTrackerEventConfig(vtkPVXMLElement* child) const
{
  if (!this->TrackerPresent)
    return;
  for (std::map<std::string, std::string>::const_iterator it = this->TrackerMapping.begin();
       it != this->TrackerMapping.end(); ++it)
  {
    std::string key = it->first;
    std::string value = it->second;
    std::replace(key.begin(), key.end(), '.', ' ');
    std::istringstream stm(key);
    std::vector<std::string> token;
    for (;;)
    {
      std::string word;
      if (!(stm >> word))
        break;
      token.push_back(word);
    }
    vtkPVXMLElement* e = vtkPVXMLElement::New();
    if (strcmp(token[0].c_str(), "tracker") == 0)
    {
      e->SetName("Tracker");
      e->AddAttribute("id", token[1].c_str());
      e->AddAttribute("name", value.c_str());
    }
    child->AddNestedElement(e);
    e->FastDelete();
  }
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::saveTrackerTransformationConfig(vtkPVXMLElement* child) const
{
  if (!this->TrackerTransformPresent)
    return;
  vtkPVXMLElement* transformationMatrix = vtkPVXMLElement::New();
  transformationMatrix->SetName("TrackerTransform");
  std::stringstream matrix;
  for (int i = 0; i < 16; ++i)
  {
    matrix << double(*((double*)this->Transformation->Element + i)) << " ";
  }
  transformationMatrix->AddAttribute("value", matrix.str().c_str());
  child->AddNestedElement(transformationMatrix);
  transformationMatrix->FastDelete();
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::setTransformation(vtkMatrix4x4* matrix)
{
  for (int i = 0; i < 4; ++i)
  {
    for (int j = 0; j < 4; ++j)
    {
      this->Transformation->SetElement(i, j, matrix->GetElement(i, j));
    }
  }
  this->TrackerTransformPresent = true;
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::callback()
{
  if (this->Initialized)
  {

#ifdef VRUI_ENABLE_DEBUG
    std::cout << "callback()" << std::endl;
#endif
    this->Internals->StateMutex->lock();
    this->getAndEnqueueButtonData();
    this->getAndEnqueueValuatorData();
    this->getAndEnqueueTrackerData();
    this->Internals->StateMutex->unlock();

    this->getNextPacket(); // for the next step
  }
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::getNextPacket()
{
  if (this->Internals->Active)
  {
    if (this->Internals->Streaming)
    {
      // With a thread
      // this->Internals->PacketSignalCondMutex->lock();
      // this->Internals->PacketSignalCond->wait(this->Internals->PacketSignalCondMutex);
      // this->Internals->PacketSignalCondMutex->unlock();
    }
    else
    {
      // With a loop
      this->Internals->Pipe->Send(vtkVRUIPipe::PACKET_REQUEST);
      if (this->Internals->Pipe->WaitForServerReply(30000))
      {
        if (this->Internals->Pipe->Receive() != vtkVRUIPipe::PACKET_REPLY)
        {
          std::cout << "VRUI Mismatching message while waiting for PACKET_REPLY" << std::endl;
          abort();
        }
        else
        {
          this->Internals->StateMutex->lock();
          this->Internals->Pipe->ReadState(this->Internals->State);
          this->Internals->StateMutex->unlock();

          //          this->PacketNotificationMutex->lock();
          //          this->PacketNotificationMutex->unlock();
        }
      }
      else
      {
        std::cout << "timeout for PACKET_REPLY" << endl;
      }
    }
  }
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::newValuatorValue(std::vector<float>* data)
{
  vtkVREvent temp;
  temp.connId = this->Address;
  temp.name = name(VALUATOR_EVENT);
  temp.eventType = VALUATOR_EVENT;
#if QT_VERSION < QT_VERSION_CHECK(6, 0, 0)
  temp.timeStamp = QDateTime::currentDateTime().toTime_t();
#else
  temp.timeStamp = QDateTime::currentDateTime().currentSecsSinceEpoch();
#endif
  temp.data.valuator.num_channels = (int)(*data).size();
  for (unsigned int i = 0; i < (*data).size(); ++i)
  {
    temp.data.valuator.channel[i] = (*data)[i];
  }
  this->EventQueue->Enqueue(temp);
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::newButtonValue(int state, int button)
{
  vtkVREvent temp;
  temp.connId = this->Address;
  temp.name = this->name(BUTTON_EVENT, button);
  temp.eventType = BUTTON_EVENT;
#if QT_VERSION < QT_VERSION_CHECK(6, 0, 0)
  temp.timeStamp = QDateTime::currentDateTime().toTime_t();
#else
  temp.timeStamp = QDateTime::currentDateTime().currentSecsSinceEpoch();
#endif
  temp.data.button.button = button;
  temp.data.button.state = state;
  this->EventQueue->Enqueue(temp);
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::newTrackerValue(vtkSmartPointer<vtkVRUITrackerState> data, int sensor)
{
  vtkVREvent temp;
  temp.connId = this->Address;
  temp.name = name(TRACKER_EVENT, sensor);
  temp.eventType = TRACKER_EVENT;
#if QT_VERSION < QT_VERSION_CHECK(6, 0, 0)
  temp.timeStamp = QDateTime::currentDateTime().toTime_t();
#else
  temp.timeStamp = QDateTime::currentDateTime().currentSecsSinceEpoch();
#endif
  temp.data.tracker.sensor = sensor;
  float rotMatrix[3][3];
  float pos[3];
  float q[4];
  data->GetPosition(pos);
  data->GetUnitQuaternion(q);

#if defined(VRUI_ENABLE_DEBUG) || 0
  std::cout << "pos=(" << pos[0] << "," << pos[1] << "," << pos[2] << ")" << endl;
  std::cout << "q=(" << q[0] << "," << q[1] << "," << q[2] << "," << q[3] << ")" << endl;
#endif

  // VTK expects quaternion in the format of (real, i, j, k), where as
  // FreeVR VRUI Daemon is giving us (i,j,k, real)
  float vtkQuat[4] = { q[3], q[0], q[1], q[2] };

  vtkMath::QuaternionToMatrix3x3(&vtkQuat[0], rotMatrix);

  this->Matrix->Element[0][0] = rotMatrix[0][0];
  this->Matrix->Element[1][0] = rotMatrix[1][0];
  this->Matrix->Element[2][0] = rotMatrix[2][0];
  this->Matrix->Element[3][0] = 0.0;

  this->Matrix->Element[0][1] = rotMatrix[0][1];
  this->Matrix->Element[1][1] = rotMatrix[1][1];
  this->Matrix->Element[2][1] = rotMatrix[2][1];
  // this->Matrix->Element[1][3] = pos[2]*1/12;
  this->Matrix->Element[3][1] = 0.0;

  this->Matrix->Element[0][2] = rotMatrix[0][2];
  this->Matrix->Element[1][2] = rotMatrix[1][2];
  this->Matrix->Element[2][2] = rotMatrix[2][2];
  // this->Matrix->Element[2][3] = pos[1]*-1/12;
  this->Matrix->Element[3][2] = 0.0;

  this->Matrix->Element[0][3] = pos[0] / 12.0;
  this->Matrix->Element[1][3] = pos[1] / 12.0;
  this->Matrix->Element[2][3] = pos[2] / 12.0;
  this->Matrix->Element[3][3] = 1.0f;

#if defined(VRUI_ENABLE_DEBUG) || 0
  if (sensor)
  {
    std::cout << "Pre multiplication matrix: " << std::endl;
    this->Matrix->PrintSelf(std::cout, vtkIndent(0));
  }
#endif

  this->ZUpToYUpMatrix->Element[0][0] = 1.0;
  this->ZUpToYUpMatrix->Element[1][0] = 0.0;
  this->ZUpToYUpMatrix->Element[2][0] = 0.0;
  this->ZUpToYUpMatrix->Element[3][0] = 0.0;

  this->ZUpToYUpMatrix->Element[0][1] = 0.0;
  this->ZUpToYUpMatrix->Element[1][1] = 0.0;
  this->ZUpToYUpMatrix->Element[2][1] = -1.0;
  this->ZUpToYUpMatrix->Element[3][1] = 0.0;

  this->ZUpToYUpMatrix->Element[0][2] = 0.0;
  this->ZUpToYUpMatrix->Element[1][2] = 1.0;
  this->ZUpToYUpMatrix->Element[2][2] = 0.0;
  this->ZUpToYUpMatrix->Element[3][2] = 0.0;

  this->ZUpToYUpMatrix->Element[0][3] = 0.0;
  this->ZUpToYUpMatrix->Element[1][3] = 0.0;
  this->ZUpToYUpMatrix->Element[2][3] = 0.0;
  this->ZUpToYUpMatrix->Element[3][3] = 1.0;

  vtkMatrix4x4::Multiply4x4(this->ZUpToYUpMatrix, this->Matrix, this->Matrix);

#if defined(VRUI_ENABLE_DEBUG) || 0
  if (sensor)
  {
    std::cout << "Post multiplication matrix: " << std::endl;
    this->Matrix->PrintSelf(std::cout, vtkIndent(0));
  }
#endif

#if defined(VRUI_ENABLE_DEBUG) || 0
  if (sensor)
    std::cout << "post pos=(" << this->Matrix->Element[0][3] << "," << this->Matrix->Element[1][3]
              << "," << this->Matrix->Element[2][3] << ")" << endl;
#endif

  temp.data.tracker.matrix[0] = this->Matrix->Element[0][0];
  temp.data.tracker.matrix[1] = this->Matrix->Element[0][1];
  temp.data.tracker.matrix[2] = this->Matrix->Element[0][2];
  temp.data.tracker.matrix[3] = this->Matrix->Element[0][3];

  temp.data.tracker.matrix[4] = this->Matrix->Element[1][0];
  temp.data.tracker.matrix[5] = this->Matrix->Element[1][1];
  temp.data.tracker.matrix[6] = this->Matrix->Element[1][2];
  temp.data.tracker.matrix[7] = this->Matrix->Element[1][3];

  temp.data.tracker.matrix[8] = this->Matrix->Element[2][0];
  temp.data.tracker.matrix[9] = this->Matrix->Element[2][1];
  temp.data.tracker.matrix[10] = this->Matrix->Element[2][2];
  temp.data.tracker.matrix[11] = this->Matrix->Element[2][3];

  temp.data.tracker.matrix[12] = this->Matrix->Element[3][0];
  temp.data.tracker.matrix[13] = this->Matrix->Element[3][1];
  temp.data.tracker.matrix[14] = this->Matrix->Element[3][2];
  temp.data.tracker.matrix[15] = this->Matrix->Element[3][3];

  this->EventQueue->Enqueue(temp);
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::getAndEnqueueButtonData()
{
  std::vector<bool>* buttons = this->Internals->State->GetButtonStates();
  for (unsigned int i = 0; i < (*buttons).size(); ++i)
  {
    newButtonValue((*buttons)[i], i);
  }
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::getAndEnqueueValuatorData()
{
  std::vector<float>* valuator = this->Internals->State->GetValuatorStates();
  newValuatorValue(valuator);
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::getAndEnqueueTrackerData()
{
  std::vector<vtkSmartPointer<vtkVRUITrackerState>>* trackers =
    this->Internals->State->GetTrackerStates();

  for (unsigned int i = 0; i < (*trackers).size(); ++i)
  {
    newTrackerValue((*trackers)[i], i);
  }
}

// ----------------------------------------------------------------------------
void pqVRUIConnection::setPort(std::string port)
{
  this->Port = port;
}
