#include <boost/bind.hpp>
#include "gazebo/gazebo.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include <stdio.h>

namespace gazebo
{   
  class SetJoints : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 
    {
      this->model = _parent;
      
      this->j2_controller = new physics::JointController(model);

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&SetJoints::OnUpdate, this, _1));
    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {
      static double angle(0.01);
      static int timer=0;
      static float dir = 0.01;

      if(timer > 1000)
      {
          angle += dir;

          if(angle > 0.5 || angle < -0.5)
          {
            // reverse direction
            dir = dir*-1;
          }
          
          timer=0;
      }
      
      //j2_controller->SetJointPosition(this->model->GetJoint("knee"), angle);
      j2_controller->SetJointPosition(this->model->GetJoint("hipX"), angle*2);
      j2_controller->SetJointPosition(this->model->GetJoint("hipY"), angle*2);     
      j2_controller->SetJointPosition(this->model->GetJoint("ancleX"), angle*2);    
      j2_controller->SetJointPosition(this->model->GetJoint("ancleY"), angle*2);  
      
      timer++;
    }

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    private: physics::JointController *j2_controller;

  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(SetJoints)
}