//  created:    2019/03/12
//  filename:   VrpnLite.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    vrpnlite, to forward it to bth for exemple
//              usefull to reduce vrpn frame size
//
/*********************************************************************/

#include "VrpnLite.h"
#include <VrpnClient.h>
#include <VrpnObject.h>
#include <Thread.h>
#include <UdpSocket.h>
#include <FrameworkManager.h>
#include <Quaternion.h>
#include <string.h>

using namespace std;
using namespace flair::core;
using namespace flair::sensor;

VrpnLite::VrpnLite(string clientAddress,string serveurAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
  vrpnclient=new VrpnClient("vrpn", serveurAddress,80); 
  VrpnObject* vrpnobject = new VrpnObject("x4_0",vrpnclient->GetTabWidget(),vrpnclient);
  //VrpnObject* vrpnobject2 = new VrpnObject("target",vrpnclient->GetTabWidget(),vrpnclient);
  
  vrpnobjects.push_back(vrpnobject);
  //vrpnobjects.push_back(vrpnobject2);
  
  dataSocket = new UdpSocket(this,"client socket",clientAddress);
  
  vrpnclient->Start();
}

VrpnLite::~VrpnLite() {
}

void VrpnLite::Run(void) {
  while (!ToBeStopped()) {
    
    WaitUpdate(vrpnobjects.at(0));//tood improve this wait, to be sure all vrpnobject are up to date; or one thread by object?
    //could be also lighter to send only one frame with all objects
    int i=0;
    for (vector<VrpnObject*>::iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
      SendObject(*it,i);
      i++;
    }
    
  }
}

void VrpnLite::SendObject(const VrpnObject* vrpnobject,uint8_t id) const{
    Vector3Df objectPosition;
    Quaternion objectQuaternion;
    
    vrpnobject->GetPosition(objectPosition);
    vrpnobject->GetQuaternion(objectQuaternion);
    Time time=vrpnobject->GetLastPacketTime();
    
    float position[3];
    position[0]=objectPosition.x;
    position[1]=objectPosition.y;
    position[2]=objectPosition.z;
    float quaternion[4];
    quaternion[0]=objectQuaternion.q0;
    quaternion[1]=objectQuaternion.q1;
    quaternion[2]=objectQuaternion.q2;
    quaternion[3]=objectQuaternion.q3;
    
    for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));
    for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));
    dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
    
    char datas[sizeof(id) + sizeof(position)+sizeof(quaternion)+ sizeof(time)];
    
    datas[0]=id;
    memcpy(datas+sizeof(id),position, sizeof(position));
    memcpy(datas +sizeof(id)+ sizeof(position),quaternion, sizeof(quaternion));
    memcpy(datas+sizeof(id) + sizeof(position)+sizeof(quaternion),&time, sizeof(time));
    
    dataSocket->SendMessage(datas,sizeof(datas));
}