/*
 * 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();

	reset();
	enabled_ = true;
	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::Monitors>("monitor_out",1);
	}

	sub_enable_ = n_->subscribe("enable", 1, &ESCROS::enableCallback, this);
	pub_stopped_ = n_->advertise<std_msgs::Empty>("stopping_conditions_met",1);
}

void ESCROS::objValWithStateCallback(esc_ros::StateValue msg)
{
	obj_val_ = msg.value;
	state_vec_.resize(msg.state.size());
	for (size_t ii=0; ii < state_vec_.size(); ++ii)
  	  state_vec_[ii] = msg.state[ii];
	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::reset(){

	initialized_ = true;
	first_obj_val_received_ = false;
	reference_zeroed_ = false;
	opt_dim_ = 0;
}

void ESCROS::enableCallback(std_msgs::Bool msg)
{
	if (msg.data){
		reset();
		esc_->reset();
		enabled_ = true;
		ROS_INFO("[esc_ros]: ESC enabled.");
	}
	else
		enabled_ = false;
}

void ESCROS::step()
{

	if(initialized_){
		if(first_obj_val_received_){
			if(enabled_){
				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;
				}
				if(opt_dim_ == 0){
					opt_dim_ = out.size();
				}

				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::Monitors msg_monitor;
					std::vector<double> values = esc_->monitor();

					msg_monitor.values.resize(values.size());
					for (size_t ii=0; ii < values.size(); ++ii)
					  msg_monitor.values[ii] = values[ii];
					msg_monitor.names = esc_->monitorNames();
					pub_monitor_.publish(msg_monitor);
				}
			}
			else if(!reference_zeroed_){

				std_msgs::Float32MultiArray msg;

				if (opt_dim_ != 0){
					msg.data.resize(opt_dim_);

					for(size_t i = 0; i<opt_dim_; i++)
						msg.data[i] = 0;

					pub_ref_.publish(msg);
					opt_dim_ = 0;
				}
				reference_zeroed_ = true;

			}

		}

	}
	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()) {
		if(esc_->isStoppingConditionsMet()){
			std_msgs::Empty msg_stopped;
			pub_stopped_.publish(msg_stopped);
			enabled_ = false;
		}
		step();
		ros::spinOnce();
		loop_rate.sleep();
	}
}