#ifndef __ROMAA_H
#define __ROMAA_H

#include <unistd.h>
#include <iostream>
#include <libplayercore/playercore.h>
#include "romaa_comms_flexiport.h"

#define DEFAULT_SERIALPORT			"/dev/ttyUSB0"
#define DEFAULT_BAUDRAE					115200L

#define	DEFAULT_MOTOR_PID_KP		10.0
#define	DEFAULT_MOTOR_PID_KI		0.0
#define	DEFAULT_MOTOR_PID_KD		0.0
#define	DEFAULT_VW_PID_KP				0.0
#define	DEFAULT_VW_PID_KI				0.0
#define	DEFAULT_VW_PID_KD				0.0

#define DEFAULT_T_ODOMETRY			50
#define DERAULT_T_LOOP					20


class Romaa : public ThreadedDriver
{
  public:
    
    // Constructor; need that
    Romaa(ConfigFile* cf, int section);

    // This method will be invoked on each incoming message
    virtual int ProcessMessage(QueuePointer &resp_queue, 
				player_msghdr * hdr,
				void * data);

  private:

    // Main function for device thread.
    virtual void Main();
    virtual int MainSetup();
    virtual void MainQuit();

		std::string port;
		unsigned int baudrate;
		romaa_comms_flexiport* romaa_comms;

		float motor_pid_kp, motor_pid_ki, motor_pid_kd;
		float vw_pid_kp, vw_pid_ki, vw_pid_kd;

    float x, y, theta, v, w;
    float size_w, size_l, size_h;
    float odom_offset_x, odom_offset_y;

    int left_enccounter, right_enccounter;

		unsigned char t_odometry;
		unsigned char t_loop;

    bool wheel_control;
//    bool speed_enccount;

		// position2d interface
    player_devaddr_t 							    position2d_addr;
    player_position2d_geom_t			    position2d_geom;
    player_position2d_data_t 			    position2d_data;
    player_position2d_cmd_vel_t 	    position2d_cmd_vel;
};

#endif
