#include <pr2_controller_interface/controller.h>
#include <pluginlib/class_list_macros.h>
#include <yaml-cpp/yaml.h>
#include <map>
#include <fstream>
#include <sstream>
#include <sys/time.h>
#include <time.h>
#include <err.h>


struct wave_s {
  wave_s(): t_zero(0), duration(1), torque_max(0) {}
  double t_zero;
  double duration;
  double torque_max;
};


void operator >> (YAML::Node const & node, wave_s & ww)
{
  YAML::Node const * pp;
  if (0 != (pp = node.FindValue("t_zero")))
    *pp >> ww.t_zero;
  if (0 != (pp = node.FindValue("duration")))
    *pp >> ww.duration;
  if (0 != (pp = node.FindValue("torque_max")))
    *pp >> ww.torque_max;
}


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 & nn);
  
  typedef std::map<pr2_mechanism::JointState *, wave_s> wave_map_t;
  wave_map_t wave_map_;
  timeval tstart_;
};


bool TutorialController::
starting()
{
  if (0 != gettimeofday(&tstart_, 0)) {
    warn("TutorialController::starting(): gettimeofday()");
    return false;
  }
  return true;
}


void TutorialController::
update(void)
{
  timeval tnow;
  if (0 != gettimeofday(&tnow, 0)) {
    warn("TutorialController::update(): gettimeofday()");
    return;
  }
  double const tt(tstart_.tv_sec - tnow.tv_sec + 1e-6 * tnow.tv_usec - 1e-6 * tstart_.tv_usec);
  for (wave_map_t::iterator ii(wave_map_.begin()); ii != wave_map_.end(); ++ii) {
    ii->first->commanded_effort_
      = ii->second.torque_max * sin(2 * M_PI * (tt - ii->second.t_zero) / ii->second.duration);
    fprintf(stderr, "  %5.2f", ii->first->commanded_effort_);
  }
  fprintf(stderr, "\n");
}


bool TutorialController::
stopping()
{
  return true;
}


bool TutorialController::
init(pr2_mechanism::RobotState * robot, const ros::NodeHandle & nn)
{
  std::string filename;
  if ( ! nn.getParam("controller_tutorial/filename", filename)) {
    filename = "wave.yaml";
    ROS_WARN ("falling back on default filename %s", filename.c_str());
  }
  std::ifstream is(filename.c_str());
  if ( ! is) {
    ROS_ERROR ("invalid filename %s", filename.c_str());
    return false;
  }
  YAML::Parser parser(is);
  if ( ! parser) {
    ROS_ERROR ("YAML::Parser does not seem to like file %s", filename.c_str());
    return false;
  }
  YAML::Node doc;
  parser.GetNextDocument(doc);
  
  for (YAML::Iterator it(doc.begin()); it != doc.end(); ++it) {
    
    std::string joint_name;
    wave_s wave;
    try {
      it.first() >> joint_name;
      it.second() >> wave;
    }
    catch (YAML::Exception const & ee) {
      ROS_WARN ("YAML exception in %s: %s", filename.c_str(), ee.what());
      continue;
    }
    
    pr2_mechanism::JointState * jstate(robot->getJointState(joint_name));
    if ( ! jstate) {
      ROS_ERROR ("no joint called %s", joint_name.c_str());
      return false;
    }
    wave_map_.insert(std::make_pair(jstate, wave));
    
    ROS_INFO ("added joint %s", joint_name.c_str());
  }
  
  if (wave_map_.empty()) {
    std::ostringstream msg;
    msg << "no valid joints found in " << filename << ", available joints are:\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)
      msg << "  " << (*ij)->name_ << "\n";
    ROS_ERROR ("%s", msg.str().c_str());
    return false;
  }
  
  warnx("TutorialController::init(): initialized %zu waves\n", wave_map_.size());
  
  return true;
}


PLUGINLIB_REGISTER_CLASS (TutorialController, TutorialController, controller::Controller)
