/*
 * nn_esc_1d.cpp
 *
 *  Created on: Jul 26, 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 neural network extremum seeking control
 *
 * * References:
 * - M. Teixeira and S. Zak, “Analog neural nonderivative optimizers,” IEEE Transactions on Neural Networks, vol. 9, pp. 629–638, 1998.
 * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
 */
#include "esc_nn/nn_esc_1d.h"

NNESC1D::NNESC1D(){
	M_ = 0;
	A_ = 0;
	ddelta_ = 0;
	delta_ = 0;
	B_ = 0;
	w_switch_old_ = 0;
	a_switch_old_ = 0;
	yr_ = 0;
	period_ = 0;
	min_peak_ = 0;
	vel_ref_ = 0;
	w_switch_ = 0;
	min_peak_detect_init_ = false;
	initialized_ = false;
}

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

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

std::vector<double> NNESC1D::monitor(){
	std::vector<double> monitor_vals;
	monitor_vals.push_back(yr_);
	monitor_vals.push_back(min_peak_);
	monitor_vals.push_back(w_switch_);

	return monitor_vals;

}

std::vector<std::string> NNESC1D::monitorNames(){
	std::vector<std::string> monitor_names;
	monitor_names.push_back("driving input value");
	monitor_names.push_back("minimum peak detector output");
	monitor_names.push_back("w switch value");

	return monitor_names;
}

NNESC1D::NNESC1D(double A,double M, double B, double ddelta, double delta, double period){
	init(A, M, B, ddelta, delta, period);
}

void NNESC1D::init(double A, double M, double B, double ddelta, double delta, double period){
	A_ = A;
	M_ = M;
	B_ = B;
	ddelta_ = ddelta;
	delta_ = delta;
	period_ = period;
	w_switch_old_ = 0;
	a_switch_old_ = A;
	yr_ = 0;
	min_peak_ = 0;
	vel_ref_ = 0;
	w_switch_ = 0;
	min_peak_detect_init_ = false;
	initialized_ = true;
}

std::vector<double> NNESC1D::step(double obj_val){
	if (!initialized_){
		fprintf(stderr,"The neural network ESC (1D) is not initialized... It will not be executed. \n");
		return std::vector<double>();
	}

	if(!min_peak_detect_init_){
		yr_ = obj_val;
		min_peak_detect_init_ = true;
	}

	double e = yr_ - obj_val;
	vel_ref_ = aSwitch(e);
	min_peak_ = minPeakDetect(-e);
	w_switch_ = wSwitch(-e);
	yr_ = yr_ + (w_switch_+min_peak_)*period_;
	std::vector<double> output;
	output.push_back(vel_ref_);
	return output;
}
double NNESC1D::wSwitch(double e_minus){
	if(e_minus<-delta_){
		w_switch_old_ = 0;
		return 0;
	}
	else if(e_minus>delta_){
		w_switch_old_ = B_;
		return B_;
	}
	else
		return w_switch_old_;

}

double NNESC1D::minPeakDetect(double e_minus){
	if(e_minus>0)
		return 0;
	else
		return -M_;
}

double NNESC1D::aSwitch(double e){
	if( e < -ddelta_ ){
		a_switch_old_ = -A_;
		return -A_;
	}
	else if(e>=ddelta_){
		a_switch_old_ = A_;
		return A_;
	}
	else
		return a_switch_old_;

}