
#include "romaa_comms_flexiport.h"
#include <stdio.h>
#include <unistd.h>

/// \brief    class constructor
///
/// \param    tty_dev: serial port device
/// \param	  baudrate: serial port baud rate
///
/// \return	  none
///
/// \author   Gonzalo F. Perez Paina
/// \date     22/03/10
romaa_comms_flexiport::romaa_comms_flexiport( std::string& tty_dev,
		unsigned baudrate )
{
	// initialize and configure serial port
  this->port_type = PORT_DEFAULT_TYPE;
  this->port_device = PORT_DEFAULT_DEVICE;
  this->port_timeout = PORT_DEFAULT_TIMEOUT;
  this->port_baudrate = PORT_DEFAULT_BAUDRATE;

  // serial_config = "type=serial,device=/dev/ttyS0",timeout=1,baud=38400"
  std::string serial_config = port_type + "," + \
                              "device=" + tty_dev + "," + \
                              port_timeout + ",";

  uart_frame_time = (10.0 / baudrate) * 1000000.0;
  
  switch ( baudrate )
  {
    case 115200: serial_config += "baud=115200";
                 break;
    case 57600:  serial_config += "baud=57600";
                 break;
    case 38400:  serial_config += "baud=38400";
                 break;
    case 19200:  serial_config += "baud=19200";
                 break;
    case 9600:   serial_config += "baud=9600";
                 break;
    default:     std::cout << "unknown baudrate " << baudrate << std::endl;
                 serial_config += port_baudrate;
                 std::cout << "set " << port_baudrate << std::endl;
  } 
  serial_port = flexiport::CreatePort( serial_config );
  serial_port->Open( );
  serial_port->Flush( );

  this->set_speed( 0.0, 0.0 );
}

/// \brief    class destructor
///
/// \param	  none
///
/// \return	  none
///
/// \author   Gonzalo F. Perez Paina
/// \date     22/03/10
romaa_comms_flexiport::~romaa_comms_flexiport()
{
	this->set_speed( 0.0, 0.0 );
	usleep( 20000 );
	serial_port->Close( );
}

/// \brief    send package 
///
/// \param    cmd: command
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     22/03/10
void
romaa_comms_flexiport::send_packet( const unsigned char& cmd )
{
  serial_buffer[0] = CMD_INIT_FRAME;
  serial_buffer[1] = PACKET_SIZE_CMD;
  serial_buffer[2] = cmd;
  if( serial_port->Write( serial_buffer, PACKET_SIZE_CMD+2 ) != 
      static_cast<int> (PACKET_SIZE_CMD+2) )
    std::cerr <<  "Did not write enough bytes." << std::endl;
  serial_port->Drain( );
  usleep((PACKET_SIZE_CMD+2)*uart_frame_time);
}

/// \brief    send package
///
/// \param    cmd: command
/// \param    uc_param: unsigned char parameter
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     22/03/10
void
romaa_comms_flexiport::send_packet( const unsigned char& cmd,
    const unsigned char& uc_param )
{
  serial_buffer[0] = CMD_INIT_FRAME;
  serial_buffer[1] = PACKET_SIZE_CMD + PACKET_SIZE_1BYTE;
  serial_buffer[2] = cmd;
  serial_buffer[3] = uc_param;
  if( serial_port->Write( serial_buffer, PACKET_SIZE_CMD+PACKET_SIZE_1BYTE+2 ) != 
      static_cast<int> (PACKET_SIZE_CMD+PACKET_SIZE_1BYTE+2) )
    std::cerr <<  "Did not write enough bytes." << std::endl;
  serial_port->Drain( );
  usleep((PACKET_SIZE_CMD+PACKET_SIZE_1BYTE+2)*uart_frame_time);
}

/// \brief    send package
///
/// \param    cmd: command
/// \param    i_param1: first integer parameter
/// \param    i_param2: second integer parameter
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     22/03/10
void
romaa_comms_flexiport::send_packet( const unsigned char& cmd,
    const int& i_param1,
    const int& i_param2 )
{
  serial_buffer[0] = CMD_INIT_FRAME;
  serial_buffer[1] = PACKET_SIZE_CMD + PACKET_SIZE_2INT;
  serial_buffer[2] = cmd;
  append_buffer( &(serial_buffer[3]), (unsigned char*)(&i_param1), 4 );
  append_buffer( &(serial_buffer[7]), (unsigned char*)(&i_param2), 4 );
  if( serial_port->Write( serial_buffer, PACKET_SIZE_CMD+PACKET_SIZE_2INT+2 ) != 
      static_cast<int> (PACKET_SIZE_CMD+PACKET_SIZE_2INT+2) )
    std::cerr <<  "Did not write enough bytes." << std::endl;
  serial_port->Drain( );
  usleep((PACKET_SIZE_CMD+PACKET_SIZE_2INT+2)*uart_frame_time);
}

/// \brief    send package
///
/// \param    cmd: command
/// \param    f_param1: first float parameter
/// \param    f_param2: second float parameter
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     22/03/10
void
romaa_comms_flexiport::send_packet( const unsigned char& cmd,
    const float& f_param1,
    const float& f_param2 )
{
  serial_buffer[0] = CMD_INIT_FRAME;
  serial_buffer[1] = PACKET_SIZE_CMD + PACKET_SIZE_2FLOAT;
  serial_buffer[2] = cmd;
  append_buffer( &(serial_buffer[3]), (unsigned char*)(&f_param1), 4 );
  append_buffer( &(serial_buffer[7]), (unsigned char*)(&f_param2), 4 );
  if( serial_port->Write( serial_buffer, PACKET_SIZE_CMD+PACKET_SIZE_2FLOAT+2 ) != 
      static_cast<int> (PACKET_SIZE_CMD+PACKET_SIZE_2FLOAT+2) )
    std::cerr <<  "Did not write enough bytes." << std::endl;
  serial_port->Drain( );
  usleep((PACKET_SIZE_CMD+PACKET_SIZE_2FLOAT+2)*uart_frame_time);
}

/// \brief    send package
///
/// \param    cmd: command
/// \param    f_param1: first float parameter
/// \param    f_param2: second float parameter
/// \param    f_param3: third float parameter
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     22/03/10
void
romaa_comms_flexiport::send_packet( const unsigned char& cmd,
    const float& f_param1,
    const float& f_param2,
    const float& f_param3 )
{
  serial_buffer[0] = CMD_INIT_FRAME;
  serial_buffer[1] = PACKET_SIZE_CMD + PACKET_SIZE_3FLOAT;
  serial_buffer[2] = cmd;
  append_buffer( &(serial_buffer[3]), (unsigned char*)(&f_param1), 4 );
  append_buffer( &(serial_buffer[7]), (unsigned char*)(&f_param2), 4 );
  append_buffer( &(serial_buffer[11]), (unsigned char*)(&f_param3), 4 );
  if( serial_port->Write( serial_buffer, PACKET_SIZE_CMD+PACKET_SIZE_3FLOAT+2 ) != 
      static_cast<int> (PACKET_SIZE_CMD+PACKET_SIZE_3FLOAT+2) )
    std::cerr <<  "Did not write enough bytes." << std::endl;
  serial_port->Drain( );
  usleep((PACKET_SIZE_CMD+PACKET_SIZE_3FLOAT+2)*uart_frame_time);
}

/// \brief    enable motors
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     09/04/10
void
romaa_comms_flexiport::enable_motors( )
{
  send_packet( CMD_ENABLE_MOTOR );
}

/// \brief    disable motors
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     09/04/10
void
romaa_comms_flexiport::disable_motors( )
{
  send_packet( CMD_DISABLE_MOTOR );
}

/// \brief    set motor PID controller constants
///
/// \param    motor_kp: KP constant
/// \param    motor_ki: KI constant
/// \param    motor_kd: KD constant
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
void
romaa_comms_flexiport::set_motor_pid( const float& motor_pid_kp,
    const float& motor_pid_ki,
    const float& motor_pid_kd )
{
  send_packet( CMD_SET_MOTOR_PID,
      motor_pid_kp, motor_pid_ki, motor_pid_kd );
}

/// \brief    get motor PID controller constants
///
/// \param    motor_kp: KP constant
/// \param    motor_ki: KI constant
/// \param    motor_kd: KD constant
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
int
romaa_comms_flexiport::get_motor_pid( float& motor_pid_kp,
    float& motor_pid_ki,
    float& motor_pid_kd )
{
  serial_port->Flush( );
  send_packet( CMD_GET_MOTOR_PID );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_3FLOAT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_3FLOAT ) )
  {
    motor_pid_kp = bytes_to_float( &(serial_buffer[2]) );
    motor_pid_ki = bytes_to_float( &(serial_buffer[6]) );
    motor_pid_kd = bytes_to_float( &(serial_buffer[10]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    set vw PID controller constants
///
/// \param    vw_kp: KP constant
/// \param    vw_ki: KI constant
/// \param    vw_kd: KD constant
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
void
romaa_comms_flexiport::set_vw_pid( const float& vw_pid_kp,
    const float& vw_pid_ki,
    const float& vw_pid_kd )
{
  send_packet( CMD_SET_VW_PID,
      vw_pid_kp, vw_pid_ki, vw_pid_kd );
}

/// \brief    get vw PID controller constants
///
/// \param    vw_kp: KP constant
/// \param    vw_ki: KI constant
/// \param    vw_kd: KD constant
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
int
romaa_comms_flexiport::get_vw_pid( float& vw_pid_kp,
    float& vw_pid_ki,
    float& vw_pid_kd )
{
  serial_port->Flush( );
  send_packet( CMD_GET_VW_PID );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_3FLOAT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_3FLOAT ) )
  {
    vw_pid_kp = bytes_to_float( &(serial_buffer[2]) );
    vw_pid_ki = bytes_to_float( &(serial_buffer[6]) );
    vw_pid_kd = bytes_to_float( &(serial_buffer[10]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    set linear and angular speed
///
/// \param    v: linear speed
/// \param    w: angular speed
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
void
romaa_comms_flexiport::set_speed( const float& v,
    const float& w )
{
  send_packet( CMD_SET_SPEED, v, w );
}

/// \brief    get linear and angular speed
///
/// \param    v: linear speed
/// \param    w: angular speed
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
int
romaa_comms_flexiport::get_speed( float& v,
    float& w )
{
  serial_port->Flush( );
  send_packet( CMD_GET_SPEED );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_2FLOAT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_2FLOAT ) )
  {
    v = bytes_to_float( &(serial_buffer[2]) );
    w = bytes_to_float( &(serial_buffer[6]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    set wheel linear speed
///
/// \param    v1: linear speed of wheel1
/// \param    v2: linear speed of wheel2
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     28/04/10
void
romaa_comms_flexiport::set_wheel_speed( const float& v1,
    const float& v2 )
{
  send_packet( CMD_SET_WHEEL_SPEED, v1, v2 );
}

/// \brief    set loop mode
///
/// \param    loop_mode: loop mode (CLOSE_LOOP_MODE, OPEN_LOOP_MODE, MOTOR_LOOP_MODE)
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     09/04/10
void
romaa_comms_flexiport::set_loop_mode( const unsigned char& loop_mode )
{
  send_packet( loop_mode );
}

/// \brief    get loop mode
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
int
romaa_comms_flexiport::get_loop_mode( unsigned char& loop_mode )
{
  serial_port->Flush( );
  send_packet( CMD_GET_LOOP_MODE );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_CMD+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_CMD ) )
  {
    loop_mode = serial_buffer[2];
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    set motor PWM
///
/// \param    pwm1: PWM of motor1
/// \param    pwm2: PWM of motor2
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
void
romaa_comms_flexiport::set_pwm( const int& pwm1,
    const int& pwm2 )
{
  send_packet( CMD_SET_PWM,
      pwm1, pwm2 );
}

/// \brief    get motor PWM
///
/// \param    pwm1: PWM of motor1
/// \param    pwm2: PWM of motor2
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
int
romaa_comms_flexiport::get_pwm( int& pwm1,
    int& pwm2 )
{
  serial_port->Flush( );
  send_packet( CMD_GET_PWM );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_2INT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_2INT ) )
  {
    pwm1 = bytes_to_int( &(serial_buffer[2]) );
    pwm2 = bytes_to_int( &(serial_buffer[6]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    reset odometry
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
void
romaa_comms_flexiport::reset_odometry( )
{
  send_packet( CMD_RESET_ODOMETRY );
}
/// \brief    reset
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
void
romaa_comms_flexiport::reset( )
{
  send_packet( CMD_RESET );
}
/// \brief    set odometry
///
/// \param    x: odometry x value
/// \param    y: odometry y value
/// \param    theta: odometry theta value
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
void
romaa_comms_flexiport::set_odometry( const float& x,
    const float& y,
    const float& theta )
{
  send_packet( CMD_SET_ODOMETRY,
      x, y, theta );
}

/// \brief    get odometry
///
/// \param    x: odometry x value
/// \param    y: odometry y value
/// \param    theta: odometry theta value
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
int
romaa_comms_flexiport::get_odometry( float& x,
    float& y,
    float& theta )
{
  serial_port->Flush( );
  send_packet( CMD_GET_ODOMETRY );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_3FLOAT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_3FLOAT ) )
  {
    x = bytes_to_float( &(serial_buffer[2]) );
    y = bytes_to_float( &(serial_buffer[6]) );
    theta = bytes_to_float( &(serial_buffer[10]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    set kinematic parameters
///
/// \param    wheel_radious: wheel radious
/// \param    wheelbase: wheelbase
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     09/04/10
void
romaa_comms_flexiport::set_kinematic_params( const float& wheel_radious,
    const float& wheelbase )
{
  send_packet( CMD_SET_KINEMATIC, 
      wheel_radious, wheelbase );
}

/// \brief    get kinematic parameters
///
/// \param    wheel_radious: wheel radious
/// \param    wheelbase: wheelbase
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     09/04/10
int
romaa_comms_flexiport::get_kinematic_params( float& wheel_radious,
    float& wheelbase )
{
  serial_port->Flush( );
  send_packet( CMD_GET_KINEMATIC );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_2FLOAT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_2FLOAT ) )
  {
    wheel_radious = bytes_to_float( &(serial_buffer[2]) );
    wheelbase = bytes_to_float( &(serial_buffer[6]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    get geometry body size
///
/// \param    body_width: body width
/// \param    body_lenght: body lenght
/// \param    body_high: body high
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     09/04/10
int
romaa_comms_flexiport::get_body_geometry( float& body_width,
    float& body_lenght,
    float& body_high )
{
  serial_port->Flush( );
  send_packet( CMD_GET_BODY_GEOM );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_3FLOAT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_3FLOAT ) )
  {
    body_width = bytes_to_float( &(serial_buffer[2]) );
    body_lenght = bytes_to_float( &(serial_buffer[6]) );
    body_high = bytes_to_float( &(serial_buffer[10]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    set loop time
///
/// \param    t_loop: loop time
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
void
romaa_comms_flexiport::set_t_loop( const unsigned char& t_loop )
{
  send_packet( CMD_SET_T_LOOP, t_loop );
}

/// \brief    get loop time
///
/// \param    t_loop: loot time
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
int
romaa_comms_flexiport::get_t_loop( unsigned char& t_loop )
{
  serial_port->Flush( );
  send_packet( CMD_GET_T_LOOP );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_1BYTE+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_1BYTE ) )
  {
    t_loop = serial_buffer[2];
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    set sampling time
///
/// \param    t_sampling: sampling time
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
void
romaa_comms_flexiport::set_t_sampling( const unsigned char& t_sampling )
{
  send_packet( CMD_SET_T_SAMPLING, t_sampling );
}

/// \brief    get sampling time
///
/// \param    t_sampling: sampling time
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
int
romaa_comms_flexiport::get_t_sampling( unsigned char& t_sampling )
{
  serial_port->Flush( );
  send_packet( CMD_GET_T_SAMPLING );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_1BYTE+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_1BYTE ) )
  {
    t_sampling = serial_buffer[2];
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    set odometry time
///
/// \param    t_odometry: odometry time
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
void
romaa_comms_flexiport::set_t_odometry( const unsigned char& t_odometry )
{
  send_packet( CMD_SET_T_ODOMETRY, t_odometry);
}

/// \brief    get odometry time
///
/// \param    t_odometry: odometry time
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     23/03/10
int
romaa_comms_flexiport::get_t_odometry( unsigned char& t_odometry )
{
  serial_port->Flush( );
  send_packet( CMD_GET_T_ODOMETRY );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_1BYTE+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_1BYTE ) )
  {
    t_odometry = serial_buffer[2];
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    get encoder counters
///
/// \param    encoder_counter1: encoder counter 1
/// \param    encoder_counter2: encoder counter 2
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
int
romaa_comms_flexiport::get_encoder_counter( int& encoder_counter1,
    int& encoder_counter2 )
{
  serial_port->Flush( );
  send_packet( CMD_GET_ENC_COUNTER );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_2INT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_2INT ) )
  {
    encoder_counter1 = bytes_to_int( &(serial_buffer[2]) );
    encoder_counter2 = bytes_to_int( &(serial_buffer[6]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    get wheel linear speed
///
/// \param    wheel_linear_speed1: linear speed of wheel 1
/// \param    wheel_linear_speed2: linear speed of wheel 2
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
int
romaa_comms_flexiport::get_wheel_linear_speed( float& wheel_linear_speed1,
    float& wheel_linear_speed2 )
{
  serial_port->Flush( );
  send_packet( CMD_GET_WHEEL_V );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_2FLOAT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_2FLOAT ) )
  {
    wheel_linear_speed1 = bytes_to_float( &(serial_buffer[2]) );
    wheel_linear_speed2 = bytes_to_float( &(serial_buffer[6]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    get wheel angular speed
///
/// \param    wheel_angular_speed1: angular speed of wheel 1
/// \param    wheel_angular_speed2: angular speed of wheel 2
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
int
romaa_comms_flexiport::get_wheel_angular_speed( float& wheel_angular_speed1,
    float& wheel_angular_speed2 )
{
  serial_port->Flush( );
  send_packet( CMD_GET_WHEEL_W );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_2FLOAT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_2FLOAT ) )
  {
    wheel_angular_speed1 = bytes_to_float( &(serial_buffer[2]) );
    wheel_angular_speed2 = bytes_to_float( &(serial_buffer[6]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    get wheel distance
///
/// \param    wheel_distance1: wheel distance of wheel 1
/// \param    wheel_distance2: wheel distance of wheel 2
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
int
romaa_comms_flexiport::get_wheel_distance( float& wheel_distance1,
    float& wheel_distance2 )
{
  serial_port->Flush( );
  send_packet( CMD_GET_WHEEL_D );

  memset( serial_buffer, 0, BUFFER_SIZE );
  if( serial_port->ReadFull( serial_buffer, PACKET_SIZE_2FLOAT+2 ) < 0 )
  {
    std::cerr << "Read failed." << std::endl;
    return -1;
  }
  if( ( serial_buffer[0] == CMD_INIT_FRAME ) &&
      ( serial_buffer[1] == PACKET_SIZE_2FLOAT ) )
  {
    wheel_distance1 = bytes_to_float( &(serial_buffer[2]) );
    wheel_distance2 = bytes_to_float( &(serial_buffer[6]) );
    return 0;
  }
  else
  {
    return -1;
  }
}

/// \brief    start data logging
///
/// \param    log: define of log to start
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
void
romaa_comms_flexiport::init_data_logging( const int& log )
{
  send_packet( (unsigned char)log );
}

/// \brief    stop data logging
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
void
romaa_comms_flexiport::stop_data_logging( )
{
  send_packet( (unsigned char)LOG_STOP );
}


/// \brief    print linear and angular speed
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
void
romaa_comms_flexiport::print_speed( )
{
  float v, w;
  if( this->get_speed( v, w ) != -1 )
    std::cout << v << " " << w << std::endl;
  else
    std::cout << "print_speed() error" << std::endl;
}

/// \brief    print pwm
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
void
romaa_comms_flexiport::print_pwm( )
{
  int pwm1, pwm2;
  if( this->get_pwm( pwm1, pwm2 ) != -1 )
    std::cout << pwm1 << " " << pwm2 << std::endl;
  else
    std::cout << "print_pwm() error" << std::endl;
}

/// \brief    print encoder counters
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
void
romaa_comms_flexiport::print_encoder_counter( )
{
  int encoder_counter1, encoder_counter2;
  if( this->get_encoder_counter( encoder_counter1, encoder_counter2 ) != -1 )
    std::cout << encoder_counter1 << " " << encoder_counter2 << std::endl;
  else
    std::cout << "print_encoder_counter() error" << std::endl;
}

/// \brief    print wheel linear speed
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
void
romaa_comms_flexiport::print_wheel_linear_speed( )
{
  float wheel_linear_speed1, wheel_linear_speed2;
  if( this->get_wheel_linear_speed( wheel_linear_speed1, wheel_linear_speed2 ) != -1 )
    std::cout << wheel_linear_speed1 << " " << wheel_linear_speed2 << std::endl;
  else
    std::cout << "print_wheel_linear_speed() error" << std::endl;
}

/// \brief    print wheel angular speed
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
void
romaa_comms_flexiport::print_wheel_angular_speed( )
{
  float wheel_angular_speed1, wheel_angular_speed2;
  if( this->get_wheel_angular_speed( wheel_angular_speed1, wheel_angular_speed2 ) != -1 )
    std::cout << wheel_angular_speed1 << " " << wheel_angular_speed2 << std::endl;
  else
    std::cout << "print_wheel_angular_speed() error" << std::endl;
}

/// \brief    print wheel distances
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     25/03/10
void
romaa_comms_flexiport::print_wheel_distance( )
{
  float wheel_distance1, wheel_distance2;
  if( this->get_wheel_distance( wheel_distance1, wheel_distance2 ) != -1 )
    std::cout << wheel_distance1 << " " << wheel_distance2 << std::endl;
  else
    std::cout << "print_wheel_distance() error" << std::endl;
}

/// \brief    print odometry
///
/// \param    none
///
/// \return   none
///
/// \author   Gonzalo F. Perez Paina
/// \date     09/04/10
void
romaa_comms_flexiport::print_odometry( )
{
  float x, y, theta;
  if( this->get_odometry( x, y, theta ) != -1 )
    std::cout << x << " " << y << " " << theta << std::endl;
  else
    std::cout << "print_odometry() error" << std::endl;
}

