/*
 * nn_esc_2d.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
 *
 * Class for two 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_2d.h"

NNESC2D::NNESC2D(){
	M_ = 0;
	A_ = 0;
	ddelta1_ = 0;
	ddelta2_ = 0;
	ddelta3_ = 0;
	delta_ = 0;
	B_ = 0;
	w_switch_old_ = 0;
	a_switch1_old_ = 0;
	a_switch2_old_ = 0;
	a_switch3_old_ = 0;
	yr_ = 0;
	period_ = 0;
	min_peak_ = 0;
	vel_ref_.resize(2);
	vel_ref_old_.resize(2);
	vel_ref_[0] = 0;
	vel_ref_[1] = 0;
	w_switch_ = 0;
	min_peak_detect_init_ = false;
	initialized_ = false;
}

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

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

std::vector<double> NNESC2D::monitor(){
	std::vector<double> monitor_vals;
	monitor_vals.push_back(yr_);
	monitor_vals.push_back(min_peak_);
	monitor_vals.push_back(w_switch_);
	monitor_vals.push_back(yr_+ddelta1_);
	monitor_vals.push_back(yr_+ddelta2_);
	monitor_vals.push_back(yr_+ddelta3_);
	monitor_vals.push_back(yr_+delta_);

	return monitor_vals;

}

std::vector<std::string> NNESC2D::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");
	monitor_names.push_back("threshold value 1");
	monitor_names.push_back("threshold value 2");
	monitor_names.push_back("threshold value 3");
	monitor_names.push_back("threshold value 4");

	return monitor_names;
}

NNESC2D::NNESC2D(double A,double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period, int stopping_cycle_number, double stoping_min_val){
	init(A, M, B, ddelta1, ddelta2, ddelta3, delta, period, stopping_cycle_number, stoping_min_val);
}

void NNESC2D::init(double A, double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period, int stopping_cycle_number, double stoping_min_val){
	A_ = A;
	M_ = M;
	B_ = B;
	ddelta1_ = ddelta1;
	ddelta2_ = ddelta2;
	ddelta3_ = ddelta3;
	delta_ = delta;
	period_ = period;
	reset();
	initialized_ = true;
	stopping_cycle_number_ = stopping_cycle_number;
	stoping_min_val_ = stoping_min_val;
}

std::vector<double> NNESC2D::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_){
		min_peak_ = obj_val;
		yr_ = min_peak_;
		min_peak_detect_init_ = true;
		obj_val_cycle_init_ = obj_val;
		vel_ref_old_[0] = vel_ref_[0];
		vel_ref_old_[1] = vel_ref_[1];
	}

	double e = yr_ - obj_val;
	double s[3];
	s[0] = aSwitch1(e);
	s[1] = aSwitch2(e);
	s[2] = aSwitch3(e);

	vel_ref_[1] = s[0]+s[1];
	vel_ref_[0] = s[1]+s[2];

	min_peak_ = minPeakDetect(e);
	w_switch_ = wSwitch(e);
	yr_ = yr_ + (w_switch_+min_peak_)*period_;

	if((vel_ref_old_[0] != vel_ref_[0] || vel_ref_old_[1] != vel_ref_[1]) && vel_ref_[1] == A_ && vel_ref_[0] == 0){
		if(obj_val_cycle_init_ - obj_val < stoping_min_val_){
			nn_cycle_count_++;
		}
		else
			nn_cycle_count_ = 0;
		obj_val_cycle_init_ = obj_val;
	}
	//printf("[nn_esc_2d]: nn_cycle_count = %d \n",nn_cycle_count_);
	vel_ref_old_[0] = vel_ref_[0];
	vel_ref_old_[1] = vel_ref_[1];

	return vel_ref_;
}
double NNESC2D::wSwitch(double e){
	if(e>delta_){
		w_switch_old_ = 0;
		return 0;
	}
	else if(e<-delta_){
		w_switch_old_ = B_;
		return B_;
	}
	else
		return w_switch_old_;

}

double NNESC2D::minPeakDetect(double e){
	if(e<=0)
		return 0;
	else
		return -M_;
}

double NNESC2D::aSwitch1(double e){
	if( e < -ddelta1_ ){
		a_switch1_old_ = -A_;
		return -A_;
	}
	else if(e>ddelta1_){
		a_switch1_old_ = A_;
		return A_;
	}
	else
		return a_switch1_old_;
}

double NNESC2D::aSwitch2(double e){

	if( e < -ddelta2_ ){
		a_switch2_old_ = A_;
		return A_;

	}
	else if(e>ddelta2_){
		a_switch2_old_ = 0;
		return 0;
	}
	else
		return a_switch2_old_;
}

double NNESC2D::aSwitch3(double e){
	if( e < -ddelta3_ ){
		a_switch3_old_ = -2*A_;
		return -2*A_;
	}
	else if(e>ddelta3_){
		a_switch3_old_ = 0;
		return 0;
	}
	else
		return a_switch3_old_;
}

void NNESC2D::reset(){
	w_switch_old_ = 0;
	a_switch1_old_ = A_;
	a_switch2_old_ = 0;
	a_switch3_old_ = 0;
	vel_ref_.resize(2);
	vel_ref_[0] = 0;
	vel_ref_[1] = 0;
	vel_ref_old_.resize(2);
	vel_ref_old_[0] = 0;
	vel_ref_old_[1] = 0;
	yr_ = 0;
	min_peak_ = 0;
	w_switch_ = 0;
	nn_cycle_count_ = 0;
	min_peak_detect_init_ = false;
}

bool NNESC2D::isStoppingConditionsMet(){
	if(stopping_cycle_number_ <= 0)
		return false;
	else if(nn_cycle_count_ > stopping_cycle_number_){
		return true;
	}
	else
		return false;
}