/*
Copyright (C) GLI Interactive LLC 2007. Public domain.

Permission is hereby granted, free of charge, to any person or organization
obtaining a copy of the software and accompanying documentation covered by
this license (the "Software") to use, reproduce, display, distribute,
execute, and transmit the Software, and to prepare derivative works of the
Software, and to permit third-parties to whom the Software is furnished to
do so, all subject to the following:

The copyright notices in the Software and this entire statement, including
the above license grant, this restriction and the following disclaimer,
must be included in all copies of the Software, in whole or in part, and
all derivative works of the Software, unless such copies or derivative
works are solely in the form of machine-executable object code generated by
a source language processor.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
*/
#include "vrpn_Tracker_MotionNode.h"
#ifdef  VRPN_USE_MOTIONNODE

#ifdef sleep
#  undef sleep
#endif
#include <plugin/Device.hpp>


// Define the type of our Sampler handle.
typedef ::MotionNode::SDK::Device::Sampler<> sampler_type;

// Static Manager instance, on demand.
::MotionNode::SDK::Device::Manager<sampler_type> & StaticManager()
{
  static ::MotionNode::SDK::Device::Manager<sampler_type> manager;
  return manager;
}


vrpn_Tracker_MotionNode::vrpn_Tracker_MotionNode(const char * name, vrpn_Connection * c,
                                                 unsigned num_sensor,
                                                 const char * address_in,
                                                 unsigned port_in)
  : vrpn_Tracker(name, c), m_num_sensor(num_sensor), m_handle(NULL)
{
  //printf("vrpn_Tracker_MotionNode::vrpn_Tracker_MotionNode()\n");

  register_server_handlers();

  {
    std::string address("127.0.0.1");
    std::size_t port = 32079;
    if (NULL != address_in) {
      address.assign(address);
    }
    if (port_in > 0) {
      port = port_in;
    }

    sampler_type * sampler = new sampler_type(address, port);

    try {
      if (StaticManager().attach(*sampler)) {
        // Attempt to read a single sample from
        // the stream. Use a 1 second blocking call.
        sampler_type::data_type data;
        if (sampler->get_data_block(data, 1)) {
          m_handle = sampler;
        } else {
          fprintf(stderr, "MotionNode driver failed to start sampling, device not currently reading\n");
        }
      }
    } catch (std::exception & e) {
      fprintf(stderr, "%\n", e.what());
    }
  }

  if (NULL != m_handle) {
    vrpn_Tracker::status = vrpn_TRACKER_SYNCING;
  } else {
    vrpn_Tracker::status = vrpn_TRACKER_FAIL;
  }
}
 
vrpn_Tracker_MotionNode::~vrpn_Tracker_MotionNode()
{
  //printf("vrpn_Tracker_MotionNode::~vrpn_Tracker_MotionNode()\n");

  if (NULL != m_handle) {
    sampler_type * sampler = reinterpret_cast<sampler_type *>(m_handle);

    // Stop sampling.
    try {
      StaticManager().detach(*sampler);
    } catch (std::exception &) {
    }

    delete sampler;
    m_handle = NULL;
  }
}

void vrpn_Tracker_MotionNode::mainloop()
{
  //printf("vrpn_Tracker_MotionNode::mainloop()\n");

  // Call the generic server mainloop, since we are a server
  server_mainloop();

  get_report();
}

void vrpn_Tracker_MotionNode::get_report()
{
  if (NULL != m_handle) {
    sampler_type * sampler = reinterpret_cast<sampler_type *>(m_handle);

    sampler_type::Data data;
    if (sampler->get_data_block(data, 1) && !data.empty()) {

      // Copy the MotionNode SDK preview data map into the
      // local quaternion storage.
      std::size_t index = 0;
      for (sampler_type::Data::iterator itr=data.begin(); itr!=data.end(); ++itr) {
        int i = std::distance(data.begin(), itr);

        if ((i >= 0) && (i < static_cast<int>(m_num_sensor))) {

          std::vector<float> q = itr->second.getQuaternion(false);

          vrpn_Tracker::d_sensor = i;
          vrpn_Tracker::d_quat[0] = q[1];
          vrpn_Tracker::d_quat[1] = q[2];
          vrpn_Tracker::d_quat[2] = q[3];
          vrpn_Tracker::d_quat[3] = q[0];

          send_report();
        }
      }

    }

  }

}

void vrpn_Tracker_MotionNode::send_report()
{
  // Send the message on the connection
  if (NULL != vrpn_Tracker::d_connection) {
    char buffer[1024];
    int	len = encode_to(buffer);
    if (vrpn_Tracker::d_connection->pack_message(len, timestamp, position_m_id, d_sender_id, buffer, vrpn_CONNECTION_LOW_LATENCY)) {
      fprintf(stderr, "MotionNode: cannot write message: tossing\n");
    }
  }
}

#endif // VRPN_USE_MOTIONNODE
