/*
 * approx_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 one dimensional approximation based extremum seeking control
 *
 * * References:
 * - C. Zhang and R. Ordonez, “Robust and adaptive design of numerical optimization-based extremum seeking control,” Automatica, vol. 45, pp. 634–646, 2009.
 * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications," IROS 2012.
 */

#include "esc_approx/approx_esc_2d.h"

ApproxESC2D::ApproxESC2D(){
	data_size_ = 0;
	k_grad_ = 0;
	init_vel_ = 0;
	sampling_ = 0;
	ptr_ = 0;
	initialized_ = false;
}

ApproxESC2D::ApproxESC2D(int data_size, double k_grad, double init_vel, int sampling){
	init(data_size,k_grad,init_vel,sampling);
}

void ApproxESC2D::init(int data_size, double k_grad, double init_vel, int sampling){
	data_size_ = data_size;
	k_grad_ = k_grad;
	init_vel_ = init_vel;
	sample_ = 0;
	sampling_ = sampling;
	ptr_ = 0;
	states_ = Eigen::MatrixXf::Zero(2,data_size);
	obj_vals_ = Eigen::VectorXf::Zero(data_size_);
	state_curr_.resize(2);
	vel_ref_.resize(2);
	initialized_ = true;
}

std::vector<double> ApproxESC2D::step(std::vector<double> state, double obj_val){
	if(initialized_){
		if(sample_ % sampling_ == 0){

			states_(0,ptr_) = state[0];
			states_(1,ptr_) = state[1];
			obj_vals_(ptr_) = obj_val;



			if (sample_<sampling_*data_size_ ){
				vel_ref_(0) = init_vel_*2.0/3.0;
				vel_ref_(1) = init_vel_;
			}/*
			else if(grad_val[0]>2 || grad_val[0]<-2){
				vel_ref_(0) = init_vel_;
				vel_ref_(1) = init_vel_;
			}*/
			else{
				Eigen::MatrixXf V(data_size_,4);
				V = Eigen::MatrixXf::Zero(data_size_, 4);

				for (int i = 0; i<data_size_; i++){
					V(i,0) = states_(0,i)*states_(1,i);
					V(i,1) = states_(0,i);
					V(i,2) = states_(1,i);
					V(i,3) = 1;
				}

				Eigen::VectorXf coef = Eigen::VectorXf::Zero(data_size_);

				printf("V = \n");
				for (int i = 0; i<data_size_; i++)
					printf("    %f %f %f %f      %f \n",V(i,0),V(i,1),V(i,2),V(i,3),obj_vals_(i));

				//coef = V.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(obj_vals_);
				coef = V.colPivHouseholderQr().solve(obj_vals_);
				state_curr_(0) = states_(0,ptr_);
				state_curr_(1) = states_(1,ptr_);
				Eigen::VectorXf grad_val =  Eigen::VectorXf::Zero(2);

				grad_val(0) = coef(0)*state_curr_(1)+coef(1);
				grad_val(1) = coef(0)*state_curr_(0)+coef(2);

				printf("grad_val[0] = %f \n",grad_val(0));
				printf("coef[0] = %f coef[1] = %f coef[2] = %f\n",coef(0),coef(1),coef(2));
				printf("in \n");
				vel_ref_ = -k_grad_*grad_val;
			}

			printf("\n");

			sample_ = sample_+1;
			ptr_ = ptr_+1;
			if(ptr_>=data_size_)
				ptr_ = 0;


			std::vector<double> out;
			out.push_back(vel_ref_(0));
			out.push_back(vel_ref_(1));
			return out;
		}
		else{
			sample_ = sample_+1;
			std::vector<double> out;
			out.push_back(vel_ref_(0));
			out.push_back(vel_ref_(1));
			return out;
		}
	}
	else{
		fprintf(stderr,"The approximation based ESC (1D) is not initialized... It will not be executed. \n");
		return std::vector<double>();
	}
}

ESC::inputType ApproxESC2D::getInputType(){
	return ESC::inputStateValue;
}
ESC::outputType ApproxESC2D::getOutputType(){
	return ESC::outputVelocity;
}

std::vector<double> ApproxESC2D::monitor(){
	return std::vector<double> ();
}
std::vector<std::string> ApproxESC2D::monitorNames(){
	return std::vector<std::string>();
}