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

#include "connection.h"

Connection::Connection(SerialPort *_sp) 
  : sp(_sp), pp(new PacketParser(_sp))
{  
}

Connection::~Connection()
{
  delete pp;
}

int Connection::write(const Packet *pkt) const 
{
  byte TxBuff[6+pkt->paramPtr];
  TxBuff[0] = 0xFF;
  TxBuff[1] = 0xFF;
  TxBuff[2] = pkt->id;
  TxBuff[3] = pkt->len;
  TxBuff[4] = pkt->cmd;
  // TODO rplace trhough memcpy
  for(int i=0; i<pkt->paramPtr; ++i)
    TxBuff[5+i] = pkt->param[i];
  TxBuff[5+pkt->paramPtr] = pkt->crc;

  int written = sp->writeData(TxBuff,6+pkt->paramPtr);
  
  if( written == (6+pkt->paramPtr) )
    return  0;
  else
    return -1;
}

int Connection::read(Packet *pkt) const 
{
  return pp->parse(pkt);
}
