#ifndef		__ROMAA_COMMS_FLEXIPORT
#define 	__ROMAA_COMMS_FLEXIPORT

#define NDEBUG

#include	<iostream>
#include	"commands.h"
#include	<string.h>

#include <gearbox/flexiport/flexiport.h>
#include <gearbox/flexiport/port.h>

#define   PORT_DEFAULT_TYPE       "type=serial"
#define   PORT_DEFAULT_DEVICE     "device=/dev/ttyUSB0"
#define   PORT_DEFAULT_TIMEOUT    "timeout=1"
#define   PORT_DEFAULT_BAUDRATE   "baud=38400"

#define   BUFFER_SIZE             1024

#define   LOG_SPEED               CMD_LOG_VW
#define   LOG_ODOMETRY            CMD_LOG_ODOM
#define   LOG_PWM                 CMD_LOG_PWM
#define   LOG_WHEEL_LINEAR_SPEED  CMD_LOG_WHEEL_V
#define   LOG_WHEEL_ANGULAR_SPEED CMD_LOG_WHEEL_W
#define   LOG_WHEEL_DISTANCE      CMD_LOG_WHEEL_D
#define   LOG_ENCODER_COUNTER     CMD_LOG_ENC_COUNTER
#define   LOG_STOP                CMD_LOG_STOP

#define   CLOSE_LOOP_MODE         CMD_SET_CLOSE_LOOP_MODE
#define   OPEN_LOOP_MODE          CMD_SET_OPEN_LOOP_MODE
#define   MOTOR_LOOP_MODE         CMD_SET_MOTOR_LOOP_MODE

#define   PACKET_SIZE_CMD         1
#define   PACKET_SIZE_1BYTE       1
#define   PACKET_SIZE_2INT        8
#define   PACKET_SIZE_2FLOAT      8
#define   PACKET_SIZE_3FLOAT      12

// RoMAA microcontroller 4byte-integer
typedef union {
  int i_data;
  unsigned char uc_data[4];
} romaa_int_data_t;

// RoMAA microcontroller 4byte-float
typedef union {
  float f_data;
  unsigned char uc_data[4];
} romaa_float_data_t;

/// \class    romaa_comms_flexiport "romaa_comms_flexiport.h"
///
/// \brief    RoMAA communication class
///
/// \version  2.0
/// \author   Gonzalo F. Perez Paina, gfperezpaina@gmail.com, UTN-FRC, CIII, Córdoba
/// \date     22/03/10
class romaa_comms_flexiport
{
  public:

    romaa_comms_flexiport( std::string& tty_dev,
        unsigned baudrate );                               // constructor
    ~romaa_comms_flexiport();                              // destructor

    void reset();

    // Motor methos
    void enable_motors( );
    void disable_motors( );

    // PID controller methods
    void set_motor_pid( const float& motor_kp,
        const float& motor_ki,
        const float& motor_kd );                           // set motor PID parameters to RoMAA robot

    void set_vw_pid( const float& vw_kp,
        const float& vw_ki,
        const float& vw_kd );                              // set vw PID parameters to RoMAA robot

    int get_motor_pid( float& motor_kp,
        float& motor_ki,
        float& motor_kd );                                 // get motor PID parameter

    int get_vw_pid( float& vw_kp,
        float& vw_ki,
        float& vw_kd );                                    // get vw control PID parameter

    // loop mode methods
    void set_loop_mode( const unsigned char& loop_mode);   // set loop mode (CLOSE_LOOP, OPEN_LOOP, MOTOR_LOOP)
    int get_loop_mode( unsigned char& loop_mode );        // get loop mode

    // speed methods
    void set_speed( const float& v,
        const float& w );                                  // set lineal and angular speeds to RoMAA robot
    int get_speed( float& v,
        float& w );                                        // get lineal speed in [m/s] and
                                                           // angular speed in [rad/s] from RoMAA robot
    // wheel speed methods
    void set_wheel_speed( const float& v1,
        const float& v2 );                                 // set wheel linear speed

    // open loop PWM methos
    void set_pwm( const int& pwm1,
        const int& pwm2 );                                 // set motor PWM
    int get_pwm( int& pwm1,
        int& pwm2 );                                       // get motor PWM

    // odometry methods
    void reset_odometry();                                 // reset odometry of RoMAA robot
    void set_odometry( const float& x,
        const float& y,
        const float& theta );                              // set odometry to RoMAA robot
    int get_odometry( float& x,                           // get odometry from RoMAA robot
        float& y,
        float& theta );

    // kinematic parameters methods
    void set_kinematic_params( const float& wheel_radious,
        const float& wheelbase );                          // set kinematic params
    int get_kinematic_params( float& wheel_radious,
        float& wheelbase );                                // get kinematic params

    int get_body_geometry( float& body_width,
        float& body_lenght,
        float& body_high );

    // logging methods
    void init_data_logging( const int& log );
    void stop_data_logging( );

    // timer control methods
    void set_t_loop( const unsigned char& t_loop );
    int get_t_loop( unsigned char& t_loop );
    void set_t_sampling( const unsigned char& t_sampling );
    int get_t_sampling( unsigned char& t_sampling );
    void set_t_odometry( const unsigned char& t_odometry );
    int get_t_odometry( unsigned char& t_odometry );

    // encoder and wheel methods
    int get_encoder_counter( int& encoder_counter1,
        int& encoder_counter2 );
    int get_wheel_linear_speed( float& wheel_linear_speed1,
        float& wheel_linear_speed2 );
    int get_wheel_angular_speed( float& wheel_angular_speed1,
        float& wheel_angular_speed2 );
    int get_wheel_distance( float& wheel_distance1,
        float& wheel_distance2 );

    // printing methods
    void print_speed( );
    void print_pwm( );
    void print_encoder_counter( );
    void print_wheel_linear_speed( );
    void print_wheel_angular_speed( );
    void print_wheel_distance( );
    void print_odometry( );

  protected:

  private:

    std::string port_type;
    std::string port_device;
    std::string port_timeout;
    std::string port_baudrate;
    float uart_frame_time;

    flexiport::Port* serial_port;
    unsigned char serial_buffer[BUFFER_SIZE];

    void append_buffer( unsigned char* buff,
        const unsigned char* data,
        int n )
    {
      for( int i = 0; i < n; i++ )
        buff[i] = data[i];
    }

    void send_packet( const unsigned char& cmd );          // send command package
    void send_packet( const unsigned char& cmd,
        const unsigned char& uc_param );                   // send 1_uchar package
    void send_packet( const unsigned char& cmd,
        const int& i_param1,
        const int& i_param2 );                             // send 2_int package
    void send_packet( const unsigned char& cmd,
        const float& f_param1,
        const float& f_param2 );                           // send 2_float package
    void send_packet( const unsigned char& cmd,
        const float& f_param1,
        const float& f_param2,
        const float& f_param3 );                           // send 3_float package

    float bytes_to_float( unsigned char* b_data )
    {
      romaa_float_data.uc_data[0] = b_data[0];
      romaa_float_data.uc_data[1] = b_data[1];
      romaa_float_data.uc_data[2] = b_data[2];
      romaa_float_data.uc_data[3] = b_data[3];
      return (romaa_float_data.f_data);
    }

    int bytes_to_int( unsigned char* b_data )
    {
      romaa_int_data.uc_data[0] = b_data[0];
      romaa_int_data.uc_data[1] = b_data[1];
      romaa_int_data.uc_data[2] = b_data[2];
      romaa_int_data.uc_data[3] = b_data[3];
      return (romaa_int_data.i_data);
    }

    romaa_int_data_t    romaa_int_data;
    romaa_float_data_t  romaa_float_data;
};

#endif
