/****************************************************************************/
/* Copyright (c) 2007 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : StatePacket.cc                                                */
/* Author   :                                                               */
/* Project  : Onboard Deliberative Autonomy                                 */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2007                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/

#include <string.h>
#include <time.h>
#include "StatePacket.h"


StatePacket::StatePacket()
  : _packet(NULL)
{
  // Initialize header
  //
  _header.version = 1.0;
    
  // Initialize data
  //
  _data.test  = 0.;
  
  // Initialize packet
  //
  _packetSize = sizeof(_data) + sizeof(_header);
  _packet = new char(_packetSize);
}


StatePacket::~StatePacket()
{
  delete _packet;
}

unsigned StatePacket::pack()
{
  memcpy(_packet, &_header, sizeof(_header));
  memcpy(_packet+sizeof(_header), &_data, sizeof(_data));
  return packetSize();
}


unsigned StatePacket::unpack(const char* packet, unsigned packetSize)
{
  if (packetSize != _packetSize)
    return 1;

  memcpy(&_header, packet, sizeof(_header));
  memcpy(&_data, packet+sizeof(_header), sizeof(_data));
  return 0;
}

unsigned StatePacket::packetSize()
{
  return _packetSize;
}