/*
 * node.cpp
 *
 *  Created on: Jul 31, 2012
 *      Author: Berk Calli
 *      Organization: Delft Biorobotics Lab., Delft University of Technology
 *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
 *
 *	Node for two dimensional neural network extremum seeking control
 */

#include <esc_nn/nn_esc_2d.h>
#include "esc_ros/esc_ros.h"

int main(int argc, char **argv) {

	ros::init(argc, argv, "nn_esc_2d");
	ros::NodeHandle n("~");

	double A, B, M, ddelta1, ddelta2, ddelta3, delta, period,stoping_min_val;
	int stopping_cycle_number;
	if (!n.getParam("A", A)){
		ROS_WARN("[nn_esc_2d]: Failed to get the parameter A from the parameter server. Using the default value.");
		A = 0;
	}
	if (!n.getParam("B", B)){
		ROS_WARN("[nn_esc_2d]: Failed to get the parameter B from the parameter server. Using the default value.");
		B = 0;
	}
	if (!n.getParam("M", M)){
		ROS_WARN("[nn_esc_2d]: Failed to get the parameter M from the parameter server. Using the default value.");
		M = 0;
	}
	if (!n.getParam("ddelta1", ddelta1)){
		ROS_WARN("[nn_esc_2d]: Failed to get the parameter ddelta1 from the parameter server. Using the default value.");
		ddelta1 = 0;
	}
	if (!n.getParam("ddelta2", ddelta2)){
		ROS_WARN("[nn_esc_2d]: Failed to get the parameter ddelta2 from the parameter server. Using the default value.");
		ddelta2 = 0;
	}
	if (!n.getParam("ddelta3", ddelta3)){
		ROS_WARN("[nn_esc_2d]: Failed to get the parameter ddelta3 from the parameter server. Using the default value.");
		ddelta3 = 0;
	}
	if (!n.getParam("delta", delta)){
		ROS_WARN("[nn_esc_2d]: Failed to get the parameter delta from the parameter server. Using the default value.");
		delta = 0;
	}
	if (!n.getParam("period", period)){
		ROS_WARN("[nn_esc_2d]: Failed to get the parameter period from the parameter server. Using the default value.");
		period = 0;
	}

	if (!n.getParam("stopping_condition/cycle_number", stopping_cycle_number)){
		ROS_WARN("[nn_esc_1D]: Failed to get the parameter stopping_condition/cycle_number from the parameter server. Using the default value.");
		stopping_cycle_number = 0;
	}

	if (!n.getParam("stopping_condition/min_val_change_per_cycle", stoping_min_val)){
		ROS_WARN("[nn_esc_1D]: Failed to get the parameter stopping_condition/min_val_change_per_cycle from the parameter server. Using the default value.");
		stoping_min_val = 0;
	}

	ESCROS esc_ros(&n);
	NNESC2D* nn_esc_2d = new NNESC2D(A,M,B,ddelta1,ddelta2,ddelta3,delta,period,stopping_cycle_number,stoping_min_val);
	esc_ros.init(nn_esc_2d);
	esc_ros.spin();

	return 0;
}