#ifndef DYNAMIXEL_SERVO_H
#define DYNAMIXEL_SERVO_H

#include <hardware_interface/joint_command_interface.h>
#include <control_toolbox/pid.h>
#include <boost/shared_ptr.hpp>

#include "dynamixel_slave_serial.h"
#include "compliance_control.h"

namespace dynamixel_robot_gazebo
{
  #define SERVO_MEM_SIZE   147

  class CDynServo
  { 
    private:
      typedef typename hardware_interface::EffortJointInterface::ResourceHandleType JointHandle;
      JointHandle joint;
      control_toolbox::Pid *pid;
      CComplianceControl *compliance;
      unsigned char id;
      std::string model;
      unsigned int encoder_res;
      double max_angle;
      double gear_ratio;
      double max_torque;
      double max_speed;
      unsigned char memory[SERVO_MEM_SIZE];
    protected:
      void init_pid(const std::string &joint_name,ros::NodeHandle &controller_nh);
      void init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh);
      void init_memory(void);
      void set_current_angle(double angle);
      void set_target_angle(double angle);
      void set_current_speed(double speed);
      void set_current_pwm(double pwm);
      double saturate_command(double command);
      double get_target_angle(void);
      double get_target_speed(void);
      double get_target_pwm(void);
      double get_max_pwm(void);
      bool is_position_control(void);
      void on_ping(void);
      unsigned char on_read(unsigned short int address, unsigned short int length, unsigned char *data);
      unsigned char on_write(unsigned short int address, unsigned short int length, unsigned char *data);
   public:
      CDynServo(const std::string &joint_name,CDynamixelSlaveSerial *dyn_slave,hardware_interface::EffortJointInterface* hw,ros::NodeHandle &controller_nh);
      void update(const ros::Duration& period);
      unsigned char get_id(void);
      ~CDynServo();
  };
}

#endif