
#include "ros/ros.h"

#include "std_msgs/String.h"
#include "rss_msgs/BumpMsg.h"
#include "rss_msgs/RobotLocation.h"
#include "rss_msgs/OdometryMsg.h"

#include "simulation.h"

using namespace rss_msgs;

double curTime() {
  return ros::Time::now().toSec();
}

int main(int argc, char **argv) {
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "simulate_orc");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */
  ROS_INFO("Adding subscribers to simulation module.");
  simulation::Simulator simulation;
  ros::Subscriber motor_sub =
      n.subscribe("command/Motors", 1, &simulation::Simulator::simulateMotion, &simulation);
  ros::Publisher odo_pub =
      n.advertise<OdometryMsg>("/rss/odometry", 1000);

  double last_odo_publish = curTime();
  double prevTime = curTime();
  while (true) {
    if (curTime() - last_odo_publish > 0.1) {
      OdometryMsg upd;
      upd.x = simulation.x;
      upd.y = simulation.y;
      upd.theta = simulation.theta;
      odo_pub.publish(upd);
      last_odo_publish = curTime();
    }

    if (curTime()-prevTime > 0.001) {
      simulation.step(curTime()-prevTime);
      prevTime = curTime();
    }

    ros::spinOnce();
  }

  return 0;
}
