#include <cstdlib>
#include "NewGon.h"
//#include "std_msgs/Float64.h"


// Here the NewGon can be configured.
int configureNewGon(NewGon * pGon)
{
    pGon->setName("Wayne's NewGon");
    pGon->axis[1].setName("s1");
    return 0;
}

//int publishROS(ros::Publisher * pOutput_pub, NewGon * pGon)
//{
//    std_msgs::Float64 msg;
//    msg.data = pGon->axis[1].x;
//     pOutput_pub->publish(msg);
//}


int main(int argc, char **argv)
{
  //Initialize NewGon
  NewGon myGon;
  configureNewGon(&myGon);

  //Configure ROS
  ros::init(argc, argv, "command_node");
  ros::NodeHandle n;
  ros::ServiceServer srv_set = n.advertiseService("set", &NewGon::setValueROS, &myGon); //advertise callback 
  ros::ServiceServer srv_get = n.advertiseService("get", &NewGon::getValueROS, &myGon); //advertise callback 
  ros::Publisher  output_pub = n.advertise<command::newgon_state>("newgon_state", 1000);

  ROS_INFO("Ready to process.");

  //Configure Main Loop
  ros::Rate r(100); //100 Hz main loop
  myGon.setRate(100); 
  
  //MAIN LOOP (keep running while ROS is ok.)
  while (ros::ok())
  {
      ros::spinOnce(); //will call all the callbacks waiting to be called at that point in time.
      
      switch (myGon.MODE)
      {
          case 0:  //UNINITIALIZED
              //myGon.acceptSetPosRequest = false;
              //myGon.acceptInitRequest   = true;
              break;

          case 1: //INITIALIZING

              break;

          case 2:  //READY FOR TARGET COMMANDS

              myGon.step();
              break;

          case 3:  //READY FOR MOTION TRAJECTORIES
          
              break;

          case 4:  //IDLE
          
              break;

          default:  //ERROR
              
              break;
      }

      myGon.publishROS(&output_pub);//publish values 
      r.sleep();
  }

  return 0;
}     
