#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)
{
  vel_ = msg->data;
  pos_.clear();
}

void ESCTest::positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
{
  pos_ = msg->data;
  vel_.clear();
}

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);
}

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

  ros::Rate loop_rate(100);
  while (nh_.ok())
  {
    if (!vel_.empty())
      system_->step(vel_);
    else if (!pos_.empty())
      system_->set(pos_);
    publish();

    loop_rate.sleep();
    ros::spinOnce();
  }  
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "esc_test");
  
  ESCFunction *function;
  
  if (argc > 1 && strcmp(argv[1], "-2") == 0)
  {
    std::vector<float> mean, sigma;
    mean.push_back(0.5);
    mean.push_back(0.2);
    sigma.push_back(0.15);
    sigma.push_back(0.15);
    function = new Gauss2D(2, -1, mean, sigma);
  }
  else
    function = new Gauss1D(2.5, -1.5, 0.4, 0.27/sqrt(2.0));
  
  ESCTest test;
  
  test.init(new ESCSystem(function));
  test.spin();
  
  return 0;
}