#include <pr2_controller_interface/controller.h>
#include <pluginlib/class_list_macros.h>
#include <stdio.h>


class TutorialController
  : public controller::Controller
{
public:
  virtual bool starting();
  virtual void update(void);
  virtual bool stopping();
  virtual bool init(pr2_mechanism::RobotState *robot, const ros::NodeHandle &n);
};


bool TutorialController::
starting()
{
  fprintf(stderr, "TutorialController::starting()\n");
  return true;
}


void TutorialController::
update(void)
{
}


bool TutorialController::
stopping()
{
  fprintf(stderr, "TutorialController::stopping()\n");
  return true;
}


bool TutorialController::
init(pr2_mechanism::RobotState *robot, const ros::NodeHandle &n)
{
  fprintf(stderr, "TutorialController::init()\n");
  
  typedef std::vector<pr2_mechanism::Joint*> jointvec_t;
  jointvec_t const & jvec(robot->model_->joints_);
  for (jointvec_t::const_iterator ij(jvec.begin()); ij != jvec.end(); ++ij) {
    fprintf(stderr, "  n: %s\tt: %d\tmin: %g\tmax: %g\n",
	       (*ij)->name_.c_str(), (*ij)->type_, (*ij)->joint_limit_min_, (*ij)->joint_limit_max_);
  }
  
  return true;
}


PLUGINLIB_REGISTER_CLASS (TutorialController, TutorialController, controller::Controller)
