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

#include "dynamixel.h"
#include "packet.h"

#include <string.h>
#include <stdio.h>

Dynamixel::Dynamixel(const byte _id, Connection *_conn)
  : id(_id), conn(_conn)
{
  if( _id > 253 ){
    fprintf(stderr, "[ERROR] Dynamixel::Dynamixel() invalid ID: %d\n", id);
    return;
  }
}

byte Dynamixel::getId() const
{
  return id;
}

bool Dynamixel::ping() const
{
  Packet pkt(id, INST_PING, NULL, 0);
  return ( send(&pkt, &pkt) == NO_ERROR );
}

int Dynamixel::send(const Packet *txpkt, Packet *rxpkt) const
{
  int status;
  
  status = conn->write(txpkt);
  if( status != 0 )
    return E_TX;
    
  status = conn->read(rxpkt);
  if( status != 0 )
    return E_RX;    
    
  if( rxpkt->crcError() )
    return E_CRC;
    
  if( rxpkt->error != 0 )
    return rxpkt->error;
    
  return NO_ERROR;
}

int Dynamixel::readByte(const byte address, byte buff[], const byte nbytes) const
{
  int status;
  byte param[2];
  param[0] = address;
  param[1] = nbytes;
  
  Packet pkt(id, INST_READ, param, 2);
  status = send(&pkt, &pkt);
    
  for(int i=0; i<nbytes; i++)
      buff[i] = pkt.param[i];
      
  return status;
}


int Dynamixel::readByte(const byte address, byte *buff) const{
  return readByte(address, buff, 1);
}

int Dynamixel::readWord(const byte address, word buff[], const byte nwords) const
{
  int status;
  byte param[2];
  param[0] = address;
  param[1] = nwords*2;
  
  Packet pkt(id, INST_READ, param, 2);
  status = send(&pkt, &pkt);
  
  for(int i=0; i<nwords; i++){
    buff[i] = byte2word(pkt.param[i*2+1], pkt.param[i*2]);
  }
    
  return status;  
}

int Dynamixel::readWord(const byte address, word *buff) const{
  return readWord(address, buff, 1);
}


int Dynamixel::writeByte(const byte address, const byte param[], const byte nbytes) const
{
  byte buffer[nbytes+1];
  buffer[0] = address;
  memcpy(&buffer[1], param, nbytes*sizeof(byte));
  
  Packet pkt(id, INST_WRITE, buffer, nbytes+1);
  return send(&pkt, &pkt);        
}

int Dynamixel::writeByte(const byte address, const byte value) const{
  return writeByte(address, &value, 1);
}

int Dynamixel::writeWord(const byte address, const word param[], const byte nwords) const
{
  byte buffer[nwords*2+1];
  buffer[0] = address;
  
  byte ptr = 1;
  for(int i=0; i<nwords; i++){
    buffer[ptr++] = lowbyte(param[i]);
    buffer[ptr++] = highbyte(param[i]);
  }
  
  Packet pkt(id, INST_WRITE, buffer, nwords*2+1);
  return send(&pkt, &pkt);
}

int Dynamixel::writeWord(const byte address, const word value) const{
  return writeWord(address, &value, 1);
}
