/*
 * esc_ros.cpp
 *
 *  Created on: Jul 26, 2012
 *      Author: Berk Calli, Wouter Caarls
 *      Organization: Delft Biorobotics Lab., Delft University of Technology
 *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
 *
 *	ROS wrapper for extremum seeking control nodes.
 */
#include <esc_ros/esc_ros.h>


ESCROS::ESCROS(ros::NodeHandle* n){
	n_ = n;
	initialized_ = false;

}

void ESCROS::init(ESC* esc)
{
	if(!n_)
		n_ = new ros::NodeHandle();

	esc_ = esc;

	if (!n_->getParam("period", period_)){
		ROS_WARN("[nn_esc_1D]: Failed to get the parameter period from the parameter server. Using the default value.");
		period_ = 0;
	}

	if (!n_->getParam("monitor", monitor_)){
		ROS_WARN("[nn_esc_1D]: Failed to get the parameter monitor from the parameter server. Using the default value.");
		monitor_ = false;
	}
	if(esc_->getInputType() == esc_->inputValue)
		sub_obj_val_ = n_->subscribe("obj_val", 1, &ESCROS::objValCallback, this);
	else if (esc_->getInputType() == esc_->inputStateValue)
		sub_obj_val_ = n_->subscribe("esc_in", 1, &ESCROS::objValWithStateCallback, this);
	else
		ROS_WARN("The input value is not specified. The subscriber will not be created.");

	if(esc_->getOutputType() == esc_->outputVelocity)
		pub_ref_ = n_->advertise<std_msgs::Float32MultiArray>("vel_ref",1);
	else if(esc_->getOutputType() == esc_->outputPosition)
		pub_ref_ = n_->advertise<std_msgs::Float32MultiArray>("pos_ref",1);
	else
		ROS_WARN("The output value is not specified. The output of the ESC can not be published.");

	if(monitor_){
		pub_monitor_ = n_->advertise<esc_ros::monitor>("monitor_out",1);
	}

	initialized_ = true;
	first_obj_val_received_ = false;

}

void ESCROS::objValWithStateCallback(esc_ros::esc_w_state msg)
{
	obj_val_ = msg.obj_val;
	state_vec_ = msg.states;
	if(!first_obj_val_received_)
		first_obj_val_received_ = true;

}

void ESCROS::objValCallback(std_msgs::Float32 msg)
{
	obj_val_ = msg.data;
	if(!first_obj_val_received_)
		first_obj_val_received_ = true;

}

void ESCROS::step()
{

	if(initialized_){
		if(first_obj_val_received_){
			std::vector<double> out;
			if(esc_->getInputType() == esc_->inputValue)
				out = esc_->step(obj_val_);
			else if (esc_->getInputType() == esc_->inputStateValue)
				out = esc_->step(state_vec_,obj_val_);
			else
				ROS_WARN("Invalid input type for the extremum seeking controller. The step function will not be executed.");

			if (out.empty()){
				return;
			}

			std_msgs::Float32MultiArray msg;
			for(size_t i=0;i<out.size();i++)
				msg.data.push_back(out[i]);
			pub_ref_.publish(msg);

			if(monitor_){
				esc_ros::monitor msg_monitor;
				msg_monitor.monitor_values = esc_->monitor();
				msg_monitor.monitor_names = esc_->monitorNames();
				pub_monitor_.publish(msg_monitor);
			}
		}
	}
	else
		ROS_WARN("ESC class is not initialized. Its functions will not be executed.");
}

void ESCROS::spin()
{
	ros::Rate loop_rate(1.0/period_);

	while (n_->ok()) {
		ros::spinOnce();
		step();
		loop_rate.sleep();
	}
}