#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <esc_ros/StateValue.h>
#include <esc_test/esc_test.h>

void ESCTest::velocityCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
{
  system_->step(msg->data);
  publish();
}

void ESCTest::positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
{
  system_->set(msg->data);
  publish();
}

void ESCTest::publish()
{
  std_msgs::Float32 val;
  val.data = system_->value();
  val_pub_.publish(val);

  esc_ros::StateValue state_val;
  state_val.state = system_->state();
  state_val.value = system_->value();
  state_val_pub_.publish(state_val);
}

void ESCTest::init(ESCSystem *system)
{
  ROS_INFO("Initializing ESC test node");

  system_ = system;
  
  val_pub_ = nh_.advertise<std_msgs::Float32>("obj_val", 1, true);
  state_val_pub_ = nh_.advertise<esc_ros::StateValue>("esc_in", 1, true);
  vel_sub_ = nh_.subscribe("vel_ref", 1, &ESCTest::velocityCallback, this);
  pos_sub_ = nh_.subscribe("pos_ref", 1, &ESCTest::positionCallback, this);

  publish();  
}

void ESCTest::spin()
{
  ROS_INFO("Spinning");

  ros::spin();
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "esc_test");
  
  ESCTest test;
  
  test.init(new ESCSystem(new Gauss(-1, 2, 1)));
  test.spin();
  
  return 0;
}