/*
 * sm_esc_1d.cpp
 *
 *  Created on: Jul 30, 2012
 *      Author: Berk Calli
 *      Organization: Delft Biorobotics Lab., Delft University of Technology
 *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
 *
 *  Class for one dimensional sliding mode extremum seeking control with periodic driving signal
 *
 * * References:
 * - H. Yu and U. Ozguner, “Extremum-seeking Control Strategy for ABS System with Time Delay,” ACC 2002.
 * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
 */

#include "esc_sm/sm_esc_1d.h"

SMESC1D::SMESC1D(){
	rho_ = 0;
	k_ = 0;
	alpha_ = 0;
	driving_input_ = 0;
	driving_input_init_ = false;
	initialized_ = false;
}

SMESC1D::SMESC1D(double rho, double k, double alpha){
	init(rho,k,alpha);
}

void SMESC1D::init(double rho, double k, double alpha){
	rho_ = rho;
	k_ = k;
	alpha_ = alpha;
	driving_input_ = 0;
	driving_input_init_ = false;
	initialized_ = true;
}

ESC::inputType SMESC1D::getInputType(){
	return ESC::inputValue;
}

ESC::outputType SMESC1D::getOutputType(){
	return ESC::outputVelocity;
}

std::vector<double> SMESC1D::step(double obj_val){
	if (!initialized_){
		fprintf(stderr,"The sliding mode ESC (1D) is not initialized... step function will not be executed. \n");
		return std::vector<double>();
	}
	if(!driving_input_init_){
		driving_input_ = obj_val;
		driving_input_init_ = true;
	}

	driving_input_ = driving_input_- rho_;
	double s = obj_val - driving_input_;
	vel_ref_ = k_*sign(sin(PI/alpha_*s));
	std::vector<double> output;
	output.push_back(vel_ref_);
	return output;
}

int SMESC1D::sign(double value){
	if (value>0){
		return 1;
	}
	else if (value<0){
		return -1;
	}
	else{
		return 0;
	}
}

std::vector<double> SMESC1D::monitor(){
	std::vector<double> monitor_values;
	monitor_values.push_back(driving_input_);
	return monitor_values;
}
std::vector<std::string> SMESC1D::monitorNames(){
	std::vector<std::string> monitor_names;
	monitor_names.push_back("driving input");
	return monitor_names;
}

void SMESC1D::reset(){
	driving_input_ = 0;
	driving_input_init_ = false;
}