/*
 *  packet.cpp
 *
 * $Id: packet.cpp 2 2008-07-30 09:28:03Z mschneider $
 * $URL: svn://localhost/Bioloid/trunk/packet.cpp $
 
 * \version   $Rev: 2 $
 * \author    Markus Schneider
 * \date      17.07.08
 *
 */

#include "packet.h"
#include <string.h>

Packet::Packet()
{  
}

Packet::Packet(byte id, byte cmd, byte param[], byte paramcnt)
{
  this->id = id;
  this->len = paramcnt + 2;
  this->cmd = cmd;
  this->paramPtr = 0;
  for(int i=0; i<paramcnt; i++)
    this->param[paramPtr++] = param[i];
  this->crc = calcChecksum();
}

Packet::~Packet(){}

byte Packet::calcChecksum() const
{
  int crc = 0x0;
  crc += this->id;
  crc += this->len;
  crc += this->cmd;
  for(int i=0; i<paramPtr; i++)
    crc += this->param[i];
                
  return  ~(byte)crc;
}

Packet::Packet(const Packet &other)
{     
  // copy memory
  this->id = other.id;
  this->len = other.len;
  this->cmd = other.cmd;
  this->paramPtr = other.paramPtr;
  this->crc = other.crc;
  memcpy(this->param,other.param,(other.paramPtr * sizeof(byte)));
}

Packet &Packet::operator=(const Packet &other)
{
  // Check for self-assignment!
  if (this == &other) return *this; 
  
  // copy memory
  this->id = other.id;
  this->len = other.len;
  this->cmd = other.cmd;
  this->paramPtr = other.paramPtr;
  this->crc = other.crc;
  memcpy(this->param,other.param,(other.paramPtr * sizeof(byte)));
  
  return *this;
}

bool Packet::operator==(const Packet &other) const
{ 
  if( this->id != other.id ) return false;
  if( this->len != other.len ) return false;
  if( this->cmd != other.cmd ) return false;
  if( this->paramPtr != other.paramPtr ) return false;
  if( this->crc != other.crc ) return false;
  if( memcmp(this->param,other.param,(other.paramPtr * sizeof(byte))) != 0 ) return false;
  
  return true;
}

bool Packet::operator!=(const Packet &other) const
{ 
  return !operator==(other);
}

bool Packet::crcError() const
{
  return (calcChecksum() != crc);
}

void Packet::fprint(FILE *stream) const
{
  fprintf(stream, "0xff 0xff 0x%x 0x%x 0x%x ", id, len, cmd);
  for(int i=0; i<paramPtr; i++){
    fprintf(stream, "0x%x ", param[i]);
  }
  fprintf(stream, "0x%x\n", crc);
}

void Packet::reset()
{
  this->id  = 0;
  this->len = 0;
  this->cmd = 0;
  this->paramPtr = 0;
  this->crc = 0;
}
