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

#include "packetparser.h"
#include <unistd.h>

// constructor
// @param stream - the bytestream to read from
PacketParser::PacketParser(ByteStream *stream)
{
  this->stream = stream;
  this->state = STATE_IDLE;
  this->pkt = NULL;
  this->timeout = 100;
}

unsigned int PacketParser::getTimeout(){
  return this->timeout;
}

void PacketParser::setTimeout(unsigned int millisec){
  this->timeout = millisec;
}


void PacketParser::reset(){
  this->state = STATE_IDLE;
}

// try to parse a packet from bytestream
// @return NULL on timeout
Packet *PacketParser::parse()
{
  Packet *pkt = new Packet();
  if( this->parse(pkt) < 0 ){
    delete pkt;
    return NULL;
  }
  return pkt;
}

// try to parse a packet from bytestream
// @param pkt - memory for the packet
// @return -1 on timeout
//          0 no error
int PacketParser::parse(Packet *pkt)
{
  this->pkt = pkt;
  this->state = STATE_IDLE;
  
  for(;;)
  {
    unsigned char b;
    int bytesread = stream->readByte(&b);    
    
    // check for timeout
    for(unsigned int timer=0; bytesread < 1 && timer<timeout; usleep(1), timer+=10 ){
      bytesread = stream->readByte(&b);
    }
    
    // timeout
    if( bytesread != 1 )
      break;
    
    // next byte
    this->nextByte(b);
    
    // chech if finish
    if( state == STATE_PKT_COMPLETE ){
      return 0;
    }
    
  }
  
  // timeout
  pkt->reset();
  return -1;
}

void PacketParser::nextByte(byte b)
{
  switch(this->state)
  {
    case STATE_IDLE:
    {
      if( b == 0xFF )
        state = STATE_1ST_FF_RCVD;
      break;
    }
      
    case STATE_1ST_FF_RCVD:
    {
      if( b == 0xFF )
        state = STATE_2ND_FF_RCVD;
      else
        state = STATE_IDLE;
      break;      
    }
      
    case STATE_2ND_FF_RCVD:
    {      
      state = STATE_ID_RCVD;
      pkt->id  = b;
      break;
    }
      
    case STATE_ID_RCVD:
    {
      if( b > MAX_PACKELEN ){
        state = STATE_IDLE;
        break;
      }
      
      pkt->len = b;
      state = STATE_LENGTH_RCVD;
      break;
    }
      
    case STATE_LENGTH_RCVD:
    {
      pkt->paramPtr = 0;
      pkt->cmd = b;
      state = STATE_COMMAND_RCVD;
      break;
    }
      
    case STATE_COMMAND_RCVD:
    {
      if( pkt->paramPtr+2 >= pkt->len ){
        state = STATE_PARAM_RCVD;
      }
      else {
        pkt->param[ pkt->paramPtr++ ] = b;
        break;
      }
    }
      
    case STATE_PARAM_RCVD:
    {  
      state = STATE_PKT_COMPLETE;    
      pkt->crc = b;      
      break;      
    }
      
    default:
      state = STATE_IDLE;
  }  
  
};

