#ifndef PMSM_CTR_H
#define PMSM_CTR_H

#include <estim/particles.h>
#include <design/ctrlbase.h>
#include <design/lq_ctrl.h>
#include "pmsm.h"
#include <ctime>

#include <fstream>

/*! \defgroup PMSM 
@{
*/

using namespace bdm;

/**/
class myEKF{
private:
	double ial, ibe, ome, the, ual, ube;
	mat A, C, K, P, Q, R;
	double a,b,c,d,e,dt;
public:
	myEKF():A(4,4),C(2,4),K(4,2),P(4,4),Q(4,4),R(2,2){
		ial = ibe = ome = the = ual = ube = 0.0;
		
		a = 0.9898;
		b = 0.0072;
		c = 0.0361;
		d = 1.0;
		e = 0.0149;
		dt = 0.000125;
		
		A.zeros();
		C.zeros();
		K.zeros();
		P.zeros();
		Q.zeros();
		R.zeros();
		A(0,0) = a;
		A(1,1) = a;
		A(2,2) = d;
		A(3,3) = 1.0;
		A(3,2) = dt;
		C(0,0) = 1.0;
		C(1,1) = 1.0;
		Q(0,0) = 0.1;
		Q(1,1) = 0.1;
		Q(2,2) = 0.1;
		Q(3,3) = 0.01;
		R(0,0) = 0.05;
		R(1,1) = 0.05;
	}
	
	vec getEst(double yal, double ybe){
		//yal = yal;
		//ybe = sqrt(3.0)/3.0 * (2.0*ybe + yal);		
		//yal = 2.0*yal/3.0;
		//ybe = 2.0*ybe/3.0;
	
		A(0,2) = b*sin(the);
        	A(0,3) = b*ome*cos(the);
        	A(1,2) = -b*cos(the);
        	A(1,3) = b*ome*sin(the);
        	A(2,0) = -e*sin(the);
        	A(2,1) = e*cos(the);
        	A(2,3) = -e*(ial*cos(the)+ibe*sin(the));        	
        		
		double tial = a*ial + b*ome*sin(the) + c*ual;
        	double tibe = a*ibe - b*ome*cos(the) + c*ube;
        	double tome = d*ome + e*(ibe*cos(the) - ial*sin(the));
        	double tthe = the + dt*ome;            	
        
        	P = A*P*A.transpose() + Q;
        	K = P*C.transpose()*inv(C*P*C.transpose() + R);
        	P = P - K*C*P;        
        
        	vec yt(2);
        	yt(0) = yal - tial;
        	yt(1) = ybe - tibe; 
        	
        	mat Kyt = K*yt;        
        	tial += Kyt(0,0);
        	tibe += Kyt(1,0);
        	tome += Kyt(2,0);
        	tthe += Kyt(3,0);
        	
        	ial = tial;
		ibe = tibe;
		ome = tome;
		the = tthe;

		vec est(4);
		est(0) = tial;
		est(1) = tibe;
		est(2) = tome;
		est(3) = tthe;
		
		return est;
	}
	
	void setUab(double _ual, double _ube){
		ual = _ual;
		ube = _ube;
	}

};
/**/

/*! PI Controller*/
class PI_ctrl: public Controller{
public:
	//!integral state
	double S;
	double Pd;
	double Pi;
	double minOut;
	double maxOut;
	
	PI_ctrl(double Pd0, double Pi0, double minOut0, double maxOut0): S(0.0), Pd(Pd0), Pi(Pi0),minOut(minOut0),maxOut(maxOut0){}
	virtual vec ctrlaction ( const vec &cond ) {
			double tmp;
			tmp = Pd*cond(0)+S;
			
			S += Pi*cond(0);
			
			if (tmp>maxOut) tmp=maxOut;
			if (tmp<minOut) tmp=minOut;
			return vec_1(tmp);
	}
	void set_params(double Pd0, double Pi0){
		Pd=Pd0;
		Pi=Pi0;
	}
	void get_params(double &Pd0, double &Pi0){
		Pd0=Pd;
		Pi0=Pi;
	}
};

/*! \brief Root class for controllers of PMSM
 * 
 * It defines the interface for controllers that connect to the pmsmDSctrl. 
 * The adapt() function calls bayes() of an internal BM. Note that it is assumed that the estimator is compatible with PMSM system.
 * I.e. it must have rv={ia, ib} and rvc={ua,ub}, any other BM will not work.
 * 
 * Two cases are considered: 
 *  - 4 dimensional rv={ia, ib, om, th} and rvc={ua,ub}
 *  - 2 dimensional rv={om, th} and rvc={ua,ub,ia_{t-1}, ib_{t-1} }
 *  These are hardcoded and sitched based on dimensionality.
 */
class PMSMCtrl: public Controller{
protected:
	//! Est where rv and rcv are not used!
	shared_ptr<BM> Est;
	//! switch to use or not to use the internal estimator
	bool estim;
	
	// internal quantities filled by PMSMCtrl::ctrlaction()
	double isa;
	double isb;
	double ome;
	double the;
	double Ww;
	
public:

	PMSMCtrl():Controller()	{
		rv = RV("{ua ub }");
		rvc = RV("{o_ia o_ib o_ua o_ub o_om o_th Ww }");
	}
	
    void adapt(const itpp::vec& data){
		if (estim){
			vec y=data.get(0,1);
			vec u=data.get(2,3);
			if (Est->dimension()==2){
				static vec cond(4); //
				cond.set_subvector(0,1,u);
				Est->bayes(y,cond);
				cond.set_subvector(2,3,y); // save 1-step delayed vectors 
				
			} else {
				Est->bayes(y,u);
				
			}
			
		}
	}
	
	
    virtual vec ctrlaction(const itpp::vec& cond) {
		
		if (estim){
			if (Est->dimension()==2){
				vec x_est=Est->posterior().mean();
				isa=cond(0);
				isb=cond(1);
				ome=x_est(0);
				the=x_est(1);
			} else {
			vec x_est=Est->posterior().mean();
			isa=x_est(0);
			isb=x_est(1);
			ome=x_est(2);
			the=x_est(3);
			}
		} else {
			isa=cond(0);//x_est(0);
			isb=cond(1);//x_est(1);
			ome=cond(4);//x[2];//x_est(2);
			the=cond(5);//x_est(3);
		}
		Ww=cond(6);
		
		return empty_vec; // dummy return
	};
    void from_setting(const libconfig::Setting& set){
		Controller::from_setting(set);
		Est=UI::build<BM>(set,"estim",UI::optional);
		estim = Est; 
	}
	void log_register ( logger &L, const string &prefix ) {
		Controller::log_register(L,prefix);
		if (estim) Est->log_register(L,prefix);
	}
	void log_write() const{
		if (estim) Est->log_write();
	}
};
//UIREGISTER(PMSMCtrl); -- abstract 

class PMSM_PICtrl: public PMSMCtrl{
public:
	PI_ctrl Cwq;
	PI_ctrl Cuq;
	PI_ctrl Cud;
/**/ //myEKF mykalm;
		
	PMSM_PICtrl():PMSMCtrl(),
	Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200), 
	Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200),
	Cud(20.0, 20.0*0.000125/0.005, -1200, 1200) {}
		
	
	virtual vec ctrlaction(const itpp::vec& cond) {
		PMSMCtrl::ctrlaction(cond); // fills isa,isb,om,the
		//cout << isa << " " << isb << " ";
/**/		//vec est = mykalm.getEst(isa, isb);
/**/		//isa = est(0);
/**/		//isb = est(1);
/**/		//ome = est(2);
/**/		//the = est(3);
		//cout << isa << " " << isb << endl;	
		
		double Isd = isa*cos(the)+isb*sin(the);
		double Isq = isb*cos(the)-isa*sin(the);
		
		double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec
		
		double ud = (Cud.ctrlaction(vec_1(-Isd)))(0);
		double uq = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0);
		
		const double Ls0=0.003465; // inductance
		const double Fmag0= 0.1989; // Magnetic??
		
		ud-=Ls0*ome*Iqw; // har
		uq+=Fmag0*ome;
		
		double Um = sqrt(ud*ud+uq*uq);
		double beta = atan(uq/ud);
		if (ud<0) beta += M_PI;
		beta +=the;
		
		vec uab(2);
		uab(0)=Um*cos(beta);		   // usx = usa
		uab(1)=Um*sin(beta);    // usy
/**/		//mykalm.setUab(uab(0),uab(1));		
		return uab;
	};
};
UIREGISTER(PMSM_PICtrl);

/*************************************/
/*			LQ alpa-beta		  */
/*************************************/

class PMSM_LQCtrl: public PMSMCtrl{
public:
/*
PMSMCtrl:
	double isa;
	double isb;
	double ome;
	double the;
	double Ww;
*/	

//PMSM parameters
	const double a;
	const double b;
	const double c;
	const double d;
	const double e;
	
//input penalty
	double r;
	
//time step
	const double Dt;
	
//receding horizon
	int rec_hor;
	
//system matrices
	mat A; //5x5
	mat B; //5x2
	//mat C; //2x5
	mat Q; //5x5
	mat R; //2x2	
	
//control matrix
	mat L;
	vec uab;
	vec icond;
	
//control maximum
	double MAXu;
	int MAXuflag;
	
//lqg controler class
	LQG_universal lq; 	
	
//prediction
	vec p_isa, p_isb, p_ome, p_the;
		
	PMSM_LQCtrl():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
				r(0.005), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
				A(5, 5), B(5, 2), Q(5, 5), R(2, 2),
				uab(2), icond(6), MAXu(100.0), MAXuflag(0)	{			
		//set fix matrices elements & dimensions
		A.zeros();
			A(0, 0) = A(1, 1) = a;
			A(2, 2) = d;
			A(2, 4) = d - 1;
			A(3, 2) = A(3, 4) = Dt;
			A(3, 3) = A(4, 4) = 1.0;
		B.zeros();
			B(0, 0) = B(1, 1) = c;
		//C.zeros();
		//	C(0, 0) = C(1, 1) = 1.0;
		Q.zeros();
			Q(2, 2) = 1.0;
		R.zeros();
								
	}
		
	
	virtual vec ctrlaction(const itpp::vec& cond) {
		PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
		
		int i;
		lq.resetTime();
		
		//create prediction
		/*p_isa.zeros();
		p_isb.zeros();
		p_ome.zeros();
		p_the.zeros();
		p_isa(0) = isa;
		p_isb(0) = isb;
		p_ome(0) = ome;
		p_the(0) = the;
		
		for(i = 0; i < rec_hor-1; i++){
			p_isa(i+1) = a*p_isa(i) + b*p_ome(i)*sin(p_the(i)) + c*0;//uab(0); //use last used control
			p_isb(i+1) = a*p_isb(i) - b*p_ome(i)*cos(p_the(i)) + c*0;//uab(1);
			p_ome(i+1) = d*p_ome(i) + e*(p_isb(i)*cos(p_the(i)) - p_isa(i)*sin(p_the(i)));
			p_the(i+1) = p_the(i) + Dt*p_ome(i);
		}*/
		
			A(0, 2) = b*sin(the);
			//A(0, 3) = b*ome*cos(the);			
			A(0, 4) = b*sin(the);
			A(1, 2) = -b*cos(the);
			//A(1, 3) = b*ome*sin(the);			
			A(1, 4) = -b*cos(the);
			A(2, 0) = -e*sin(the);
			A(2, 1) = e*cos(the);	
			//A(2, 3) = -e*(isa*cos(the) + isb*sin(the));		
			
			lq.Models(0).A = A;		
		
		//create control matrix
		for(i = rec_hor; i > 0; i--){		
			//set variable matrices elements (A matrix only)
			/*A(0, 2) = b*sin(p_the(i));
			A(0, 3) = b*(p_ome(i))*cos(p_the(i));
			A(0, 4) = b*sin(p_the(i));
			A(1, 2) = -b*cos(p_the(i));
			A(1, 3) = b*(p_ome(i))*sin(p_the(i));
			A(1, 4) = -b*cos(p_the(i));
			A(2, 0) = -e*sin(p_the(i));
			A(2, 1) = e*cos(p_the(i));
			A(2, 3) = -e*(p_isa(i)*cos(p_the(i)) + p_isb(i)*sin(p_the(i)));
			
			lq.Models(0).A = A;	*/	
			lq.redesign();
		}
		lq.redesign();
		
		L = lq.getL();
		icond(0) = isa;
		icond(1) = isb;
		icond(2) = ome - Ww;
		icond(3) = the;
		icond(4) = Ww;
		icond(5) = 0;
		vec tmp = L*icond;		    
		
		uab = tmp(0,1);		
		
		if(MAXuflag == 1){ //square cut off
			if(uab(0) > MAXu) uab(0) = MAXu;
			else if(uab(0) < -MAXu) uab(0) = -MAXu;
			if(uab(1) > MAXu) uab(1) = MAXu;
			else if(uab(1) < -MAXu) uab(1) = -MAXu;
		}
		else if(MAXuflag == 2){ //circular cut off
			double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
			double uangl = atan2(uab(1), uab(0));
			if (uampl > MAXu) uampl = MAXu;
			uab(0) = uampl*cos(uangl);
			uab(1) = uampl*sin(uangl);
		}
		//else{ //(MAXuflag == 0) //no cut off }
		
		return uab;
	}
	
	void from_setting(const Setting &set){
		PMSMCtrl::from_setting(set);
		UI::get(r,set, "r", UI::optional);
		UI::get(rec_hor,set, "h", UI::optional);
		UI::get(MAXu,set, "MAXu", UI::optional);
		UI::get(MAXuflag,set, "MAXuflag", UI::optional);
	}

	void validate(){
		R(0,0)=R(1,1)=r;

		p_isa.set_length(rec_hor+1);
		p_isb.set_length(rec_hor+1);
		p_ome.set_length(rec_hor+1);
		p_the.set_length(rec_hor+1);

		Array<quadraticfn> qloss(2);
		qloss(0).Q.setCh(Q);
		qloss(0).rv = RV("x", 5, 1);
		qloss(1).Q.setCh(R);
		qloss(1).rv = RV("u", 2, 0);		
		lq.Losses = qloss;		

				//set lq
		lq.rv = RV("u", 2, 0);			
		lq.set_rvc(RV("x", 5, 0));
		lq.horizon = rec_hor;	
		
		Array<linfnEx> model(2);
		model(0).A = A;
		model(0).B = vec("0 0 0 0 0");
		model(0).rv = RV("x", 5, 0);
		model(0).rv_ret = RV("x", 5, 1);
		model(1).A = B;
		model(1).B = vec("0 0");
		model(1).rv = RV("u", 2, 0);
		model(1).rv_ret = RV("x", 5, 1);
		lq.Models = model;
		
		lq.finalLoss.Q.setCh(Q);
		lq.finalLoss.rv = RV("x", 5, 1);
		
		lq.validate();
						
		uab.zeros();

	}
};
UIREGISTER(PMSM_LQCtrl);

/*************************************/
/*			LQ d-q 1			  */
/*************************************/

class PMSM_LQCtrl_dq: public PMSMCtrl{
public:
/*
PMSMCtrl:
	double isa;
	double isb;
	double ome;
	double the;
	double Ww;
*/	

//PMSM parameters
	const double a;
	const double b;
	const double c;
	const double d;
	const double e;
	
//input penalty
	double r;
	double rpd; //penalize differences u - u_old
	
//time step
	const double Dt;
	
//receding horizon
	int rec_hor;
	
//system matrices
	mat A; //5x5
	mat B; //5x2
	//mat C; //2x5
	mat Q; //5x5
	mat R; //2x2
	mat Rpd; //2x4
	
//control matrix
	mat L;
	vec uab;	
	vec icondpd;
	vec u_old;
	
//control maximum
	double MAXu;
	int MAXuflag;
	
//lqg controler class
	LQG_universal lq; 	
	
//prediction
	vec p_isa, p_isb, p_ome, p_the;
		
	PMSM_LQCtrl_dq():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
				uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0)	{			
		//set fix matrices elements & dimensions
		A.zeros();
			A(0, 0) = A(1, 1) = a;
			A(1, 2) = A(1, 4) = -b;
			A(2, 1) = e;
			A(2, 2) = d;
			A(2, 4) = d - 1;
			A(3, 2) = A(3, 4) = Dt;
			A(3, 3) = A(4, 4) = 1.0;
		B.zeros();
			B(0, 0) = B(1, 1) = c;		
		Q.zeros();
			Q(2, 2) = 1.0;
		R.zeros();
		Rpd.zeros();
		u_old.zeros();						
	}
		
	
	virtual vec ctrlaction(const itpp::vec& cond) {
		PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
				
		vec udq;
		vec tmp;
		
		lq.resetTime();
				
		L = lq.getL();
		
		icondpd(0) = isa*cos(the) + isb*sin(the);
		icondpd(1) = isb*cos(the) - isa*sin(the);
		icondpd(2) = ome - Ww;
		icondpd(3) = the;
		icondpd(4) = Ww;
		icondpd(5) = u_old(0);
		icondpd(6) = u_old(1);
		icondpd(7) = 0;	
			
		tmp = L*icondpd;		
						
		udq = tmp(0,1);
		
		//uab = udq; //set size
		uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
		uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
		
		if(MAXuflag == 1){ //square cut off
			if(uab(0) > MAXu) uab(0) = MAXu;
			else if(uab(0) < -MAXu) uab(0) = -MAXu;
			if(uab(1) > MAXu) uab(1) = MAXu;
			else if(uab(1) < -MAXu) uab(1) = -MAXu;
		}
		else if(MAXuflag == 2){ //circular cut off
			double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
			double uangl = atan2(uab(1), uab(0));
			if (uampl > MAXu) uampl = MAXu;
			uab(0) = uampl*cos(uangl);
			uab(1) = uampl*sin(uangl);
		}
		//else{ //(MAXuflag == 0) //no cut off }
		
		u_old = udq;
		
		return uab;
	}
	
	void from_setting(const Setting &set){
		PMSMCtrl::from_setting(set);
		UI::get(r,set, "r", UI::optional);
		UI::get(rec_hor,set, "h", UI::optional);
		UI::get(MAXu,set, "MAXu", UI::optional);
		UI::get(MAXuflag,set, "MAXuflag", UI::optional);
		UI::get(rpd,set, "rpd", UI::optional);		
	}

	void validate(){
		R(0, 0) = R(1, 1) = r;
		Rpd(0, 0) = Rpd(1, 1) = rpd;
		Rpd(0, 2) = Rpd(1, 3) = -rpd;
				
		p_isa.set_length(rec_hor+1);
		p_isb.set_length(rec_hor+1);
		p_ome.set_length(rec_hor+1);
		p_the.set_length(rec_hor+1);

		Array<quadraticfn> qloss(3);
		qloss(0).Q.setCh(Q);
		qloss(0).rv = RV("x", 5, 1);		
		qloss(1).Q.setCh(R);
		qloss(1).rv = RV("u", 2, 0);
		qloss(2).Q.setCh(Rpd);		
		qloss(2).rv = RV("u", 2, 0);
		qloss(2).rv.add(RV("u", 2, -1));		
		lq.Losses = qloss;		

		//set lq
		lq.rv = RV("u", 2, 0);	
		RV tmp = RV("x", 5, 0);
		tmp.add(RV("u", 2, -1));
		lq.set_rvc(tmp);		
		lq.horizon = rec_hor;	
		
		Array<linfnEx> model(2);
		model(0).A = A;
		model(0).B = vec("0 0 0 0 0");
		model(0).rv = RV("x", 5, 0);
		model(0).rv_ret = RV("x", 5, 1);
		model(1).A = B;
		model(1).B = vec("0 0");
		model(1).rv = RV("u", 2, 0);
		model(1).rv_ret = RV("x", 5, 1);
		lq.Models = model;
		
		lq.finalLoss.Q.setCh(Q);
		lq.finalLoss.rv = RV("x", 5, 1);
				
		lq.validate();
						
		uab.zeros();
				
		//create control matrix
		for(int i = rec_hor; i > 0; i--){								
			lq.redesign();
		}
		lq.redesign();
	}
};
UIREGISTER(PMSM_LQCtrl_dq);

/*************************************/
/*			LQ d-q 2			  */
/*************************************/

class PMSM_LQCtrl_dq2: public PMSMCtrl{
public:
/*
PMSMCtrl:
	double isa;
	double isb;
	double ome;
	double the;
	double Ww;
*/	

//PMSM parameters
	const double a;
	const double b;
	const double c;
	const double d;
	const double e;
	
//input penalty
	double r;
	double rpd; //penalize differences u - u_old
	
//time step
	const double Dt;
	
//receding horizon
	int rec_hor;
	
//system matrices
	mat A; //5x5
	mat B; //5x2
	//mat C; //2x5
	mat Q; //5x5
	mat R; //2x2
	mat Rpd; //2x4	
	
//control matrix
	mat L;
	vec uab,udq;
	vec icondpd;
	vec u_old;
	
//control maximum
	double MAXu;
	int MAXuflag;
	
//lqg controler class
	LQG_universal lq; 	
	
//
//	vec p_isd, p_isq, p_ome, p_the;
	double p_isd, p_isq;
		
	PMSM_LQCtrl_dq2():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
				uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0)	{			
		//set fix matrices elements & dimensions
		A.zeros();
			A(0, 0) = A(1, 1) = a;
			A(2, 1) = e;
			A(2, 2) = d;
			A(2, 4) = d - 1;
			A(3, 2) = A(3, 4) = Dt;
			A(3, 3) = A(4, 4) = 1.0;
		B.zeros();
			B(0, 0) = B(1, 1) = c;		
		Q.zeros();
			Q(2, 2) = 1.0;
		R.zeros();
		Rpd.zeros();
		u_old.zeros();						
	}
		
	
	virtual vec ctrlaction(const itpp::vec& cond) {
		PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
		
		int i;
		vec tmp;
		
		lq.resetTime();
	
		//create prediction
		/*p_isd.zeros();
		p_isq.zeros();
		p_ome.zeros();
		p_the.zeros();*/
		p_isd = isa*cos(the) + isb*sin(the);//isa;
		p_isq = isb*cos(the) - isa*sin(the);//isb;
		/*p_ome(0) = ome;
		p_the(0) = the;
		
		for(i = 0; i < rec_hor-1; i++){			
			p_isd(i+1) = a*p_isd(i) + p_isq(i)*p_ome(i)*Dt; + c*0.5*(udq(0)+u_old(0));			
			p_isq(i+1) = -p_isd(i)*p_ome(i)*Dt + a*p_isq(i) - b*p_ome(i); + c*0.5*(udq(1)+u_old(1));
			p_ome(i+1) = d*p_ome(i) + e*p_isq(i);
			p_the(i+1) = p_the(i) + Dt*p_ome(i);			
		}*/
		
		A(0, 1) = Dt*ome;
		//A(0, 2) = Dt*p_isq;
		//A(0, 4) = Dt*p_isq;
		A(1, 0) = -Dt*ome;
		A(1, 2) = /*-Dt*p_isd*/-b;
		A(1, 4) = /*-Dt*p_isd*/-b;
			
		lq.Models(0).A = A;	
		
		//create control matrix
		for(i = rec_hor; i > 0; i--){		
			//set variable matrices elements (A matrix only)			
			/*A(0, 1) = Dt*p_ome(i);
			A(0, 2) = Dt*p_isq(i);
			A(0, 4) = Dt*p_isq(i);
			A(1, 0) = -Dt*p_ome(i);
			A(1, 2) = -Dt*p_isd(i);
			A(1, 4) = -Dt*p_isd(i);
			
			lq.Models(0).A = A;	*/	
			lq.redesign();
		}
		lq.redesign();
		
		L = lq.getL();
	
		icondpd(0) = isa*cos(the) + isb*sin(the);
		icondpd(1) = isb*cos(the) - isa*sin(the);
		icondpd(2) = ome - Ww;
		icondpd(3) = the;
		icondpd(4) = Ww;
		icondpd(5) = u_old(0);
		icondpd(6) = u_old(1);
		icondpd(7) = 0;
			
		tmp = L*icondpd;			
				
		udq = tmp(0,1);
		
		//uab = udq; //set size
		uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
		uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
		
		if(MAXuflag == 1){ //square cut off
			if(uab(0) > MAXu) uab(0) = MAXu;
			else if(uab(0) < -MAXu) uab(0) = -MAXu;
			if(uab(1) > MAXu) uab(1) = MAXu;
			else if(uab(1) < -MAXu) uab(1) = -MAXu;
		}
		else if(MAXuflag == 2){ //circular cut off
			double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
			double uangl = atan2(uab(1), uab(0));
			if (uampl > MAXu) uampl = MAXu;
			uab(0) = uampl*cos(uangl);
			uab(1) = uampl*sin(uangl);
		}
		//else{ //(MAXuflag == 0) //no cut off }
		
		u_old = udq;
	
		return uab;
	}
	
	void from_setting(const Setting &set){
		PMSMCtrl::from_setting(set);
		UI::get(r,set, "r", UI::optional);
		UI::get(rec_hor,set, "h", UI::optional);
		UI::get(MAXu,set, "MAXu", UI::optional);
		UI::get(MAXuflag,set, "MAXuflag", UI::optional);
		UI::get(rpd,set, "rpd", UI::optional);
	}

	void validate(){
		R(0, 0) = R(1, 1) = r;
		Rpd(0, 0) = Rpd(1, 1) = rpd;		
		Rpd(0, 2) = Rpd(1, 3) = -rpd;		
		
		/*p_isd.set_length(rec_hor+1);
		p_isq.set_length(rec_hor+1);
		p_ome.set_length(rec_hor+1);
		p_the.set_length(rec_hor+1);*/

		Array<quadraticfn> qloss(3);
		qloss(0).Q.setCh(Q);
		qloss(0).rv = RV("x", 5, 1);		
		qloss(1).Q.setCh(R);
		qloss(1).rv = RV("u", 2, 0);
		qloss(2).Q.setCh(Rpd);
		qloss(2).rv = RV("u", 2, 0);
		qloss(2).rv.add(RV("u", 2, -1));		
		lq.Losses = qloss;		

		//set lq
		lq.rv = RV("u", 2, 0);		
		RV tmp = RV("x", 5, 0);
		tmp.add(RV("u", 2, -1));	
		lq.set_rvc(tmp);
		
		lq.horizon = rec_hor;	
		
		Array<linfnEx> model(2);
		model(0).A = A;
		model(0).B = vec("0 0 0 0 0");
		model(0).rv = RV("x", 5, 0);
		model(0).rv_ret = RV("x", 5, 1);
		model(1).A = B;
		model(1).B = vec("0 0");
		model(1).rv = RV("u", 2, 0);
		model(1).rv_ret = RV("x", 5, 1);
		lq.Models = model;
		
		lq.finalLoss.Q.setCh(Q);
		lq.finalLoss.rv = RV("x", 5, 1);
				
		lq.validate();
						
		uab.zeros();
		udq.zeros();		
	}
};
UIREGISTER(PMSM_LQCtrl_dq2);

/*************************************/
/*		LQ d-q Bicriterial         */
/*************************************/

class PMSM_LQCtrl_bic: public PMSMCtrl{
public:
/*
PMSMCtrl:
	double isa;
	double isb;
	double ome;
	double the;
	double Ww;
*/	

//PMSM parameters
	const double a;
	const double b;
	const double c;
	const double d;
	const double e;
	
//input penalty
	double r;
	double rpd; //penalize differences u - u_old
	
//time step
	const double Dt;
	
//receding horizon
	int rec_hor;
	
//system matrices
	mat A; //5x5
	mat B; //5x2
	//mat C; //2x5
	mat Q; //5x5
	mat R; //2x2
	mat Rpd; //2x4	
	
//control matrix
	mat L;
	vec uab,udq;
	vec icondpd;
	vec u_old;
	
//control maximum
	double MAXu;
	int MAXuflag;
	
//lqg controler class
	LQG_universal lq; 	
	
//pomega
	//double Pome;
	double qqq;
	vec bcb;
	double bcbv;
	
//
//	vec p_isd, p_isq, p_ome, p_the;
	double p_isd, p_isq;
		
	PMSM_LQCtrl_bic():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), //qqq(0.0),
				uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), bcb(2), bcbv(0.0)	{			
		//set fix matrices elements & dimensions
		A.zeros();
			A(0, 0) = A(1, 1) = a;
			A(2, 1) = e;
			A(2, 2) = d;
			A(2, 4) = d - 1;
			A(3, 2) = A(3, 4) = Dt;
			A(3, 3) = A(4, 4) = 1.0;
			//A(5, 5) = 1.0;
		B.zeros();
			B(0, 0) = B(1, 1) = c;		
		Q.zeros();
			Q(2, 2) = 1.0;			
		R.zeros();
		Rpd.zeros();
		u_old.zeros();		
		
		//Pome = 10.0;	
					
	}
		
	
	virtual vec ctrlaction(const itpp::vec& cond) {
		PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
		
		int i;
		vec tmp;
		
		lq.resetTime();
	
		//create prediction		
		p_isd = isa*cos(the) + isb*sin(the);//isa;
		p_isq = isb*cos(the) - isa*sin(the);//isb;
				
		A(0, 1) = Dt*ome;
		//A(0, 2) = Dt*p_isq;
		//A(0, 4) = Dt*p_isq;
		A(1, 0) = -Dt*ome;
		A(1, 2) = /*-Dt*p_isd*/-b;
		A(1, 4) = /*-Dt*p_isd*/-b;
			
		lq.Models(0).A = A;	
		
		//create control matrix
		for(i = rec_hor; i > 0; i--){					
			lq.redesign();
		}
		lq.redesign();
		
		L = lq.getL();
	
		icondpd(0) = isa*cos(the) + isb*sin(the);
		icondpd(1) = isb*cos(the) - isa*sin(the);
		icondpd(2) = ome - Ww;
		icondpd(3) = the;
		icondpd(4) = Ww;
		icondpd(5) = u_old(0);
		icondpd(6) = u_old(1);
		//icondpd(7) = sqrt(Pome);
		icondpd(7) = 0;
			
		tmp = L*icondpd;			
				
		udq = tmp(0,1);//(1.0 + abs(Pome/ome));
		
		//bicriterial
		udq += sign(ome)*bcb;
		
		//uab = udq; //set size
		uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
		uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
		
		if(MAXuflag == 1){ //square cut off
			if(uab(0) > MAXu) uab(0) = MAXu;
			else if(uab(0) < -MAXu) uab(0) = -MAXu;
			if(uab(1) > MAXu) uab(1) = MAXu;
			else if(uab(1) < -MAXu) uab(1) = -MAXu;
		}
		else if(MAXuflag == 2){ //circular cut off
			double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
			double uangl = atan2(uab(1), uab(0));
			if (uampl > MAXu) uampl = MAXu;
			uab(0) = uampl*cos(uangl);
			uab(1) = uampl*sin(uangl);
		}
		//else{ //(MAXuflag == 0) //no cut off }
		
		//variance Pome evolution
		//Pome *= 1.0/(1.0+abs(Pome/ome));
		
		u_old = udq;
	
		return uab;
	}
	
	void from_setting(const Setting &set){
		PMSMCtrl::from_setting(set);
		UI::get(r,set, "r", UI::optional);
		UI::get(rec_hor,set, "h", UI::optional);
		UI::get(MAXu,set, "MAXu", UI::optional);
		UI::get(MAXuflag,set, "MAXuflag", UI::optional);
		UI::get(rpd,set, "rpd", UI::optional);
		//UI::get(qqq,set, "qqq", UI::optional);
		UI::get(bcbv,set, "bcbv", UI::optional);
	}

	void validate(){
		R(0, 0) = R(1, 1) = r;
		Rpd(0, 0) = Rpd(1, 1) = rpd;		
		Rpd(0, 2) = Rpd(1, 3) = -rpd;	
		//Q(5, 5) = qqq;	
		
		//bicriterial
		bcb(0) = -bcbv;
		bcb(1) = bcbv;
		
		/*p_isd.set_length(rec_hor+1);
		p_isq.set_length(rec_hor+1);
		p_ome.set_length(rec_hor+1);
		p_the.set_length(rec_hor+1);*/

		Array<quadraticfn> qloss(3);
		qloss(0).Q.setCh(Q);
		qloss(0).rv = RV("x", 5, 1);		
		qloss(1).Q.setCh(R);
		qloss(1).rv = RV("u", 2, 0);
		qloss(2).Q.setCh(Rpd);
		qloss(2).rv = RV("u", 2, 0);
		qloss(2).rv.add(RV("u", 2, -1));		
		lq.Losses = qloss;		

		//set lq
		lq.rv = RV("u", 2, 0);		
		RV tmp = RV("x", 5, 0);
		tmp.add(RV("u", 2, -1));	
		lq.set_rvc(tmp);
		
		lq.horizon = rec_hor;	
		
		Array<linfnEx> model(2);
		model(0).A = A;
		model(0).B = vec("0 0 0 0 0");
		model(0).rv = RV("x", 5, 0);
		model(0).rv_ret = RV("x", 5, 1);
		model(1).A = B;
		model(1).B = vec("0 0");
		model(1).rv = RV("u", 2, 0);
		model(1).rv_ret = RV("x", 5, 1);
		lq.Models = model;
		
		lq.finalLoss.Q.setCh(Q);
		lq.finalLoss.rv = RV("x", 5, 1);
				
		lq.validate();
						
		uab.zeros();
		udq.zeros();		
	}
};
UIREGISTER(PMSM_LQCtrl_bic);

/*************************************/
/*			LQ d-q 1 bicriterial 2*/
/*************************************/

class PMSM_LQCtrl_bic2: public PMSMCtrl{
public:
/*
PMSMCtrl:
	double isa;
	double isb;
	double ome;
	double the;
	double Ww;
*/	

//PMSM parameters
	const double a;
	const double b;
	const double c;
	const double d;
	const double e;
	
//input penalty
	double r;
	double rpd; //penalize differences u - u_old
	
//time step
	const double Dt;
	
//receding horizon
	int rec_hor;
	
//system matrices
	mat A; //5x5
	mat B; //5x2
	//mat C; //2x5
	mat Q; //5x5
	mat R; //2x2
	mat Rpd; //2x4
	
//control matrix
	mat L;
	vec uab;	
	vec icondpd;
	vec u_old;
	
//control maximum
	double MAXu;
	int MAXuflag;
	
//lqg controler class
	LQG_universal lq; 	
	
//prediction
	vec p_isa, p_isb, p_ome, p_the;
	
//bicrit param
	double bcbv;
	
	
//pi version	
	PI_ctrl Cwq;
	PI_ctrl Cuq;
	PI_ctrl Cud;
	
	
		
	PMSM_LQCtrl_bic2():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4),
				uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0),
				/*Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200), 
				Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200),
				Cud(20.0, 20.0*0.000125/0.005, -1200, 1200)*/
				Cwq(10.0, 0.0005, -1200, 1200), 
				Cuq(0.4, 0.0005, -1200, 1200),
				Cud(0.4, 0.0005, -1200, 1200)	{			
		//set fix matrices elements & dimensions
		A.zeros();
			A(0, 0) = A(1, 1) = a;
			A(1, 2) = A(1, 4) = -b;
			A(2, 1) = e;
			A(2, 2) = d;
			A(2, 4) = d - 1;
			A(3, 2) = A(3, 4) = Dt;
			A(3, 3) = A(4, 4) = 1.0;
		B.zeros();
			B(0, 0) = B(1, 1) = c;		
		Q.zeros();
			Q(2, 2) = 1.0;
		R.zeros();
		Rpd.zeros();
		u_old.zeros();						
	}
		
	
	virtual vec ctrlaction(const itpp::vec& cond) {
		PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
				
		vec udq;
		vec tmp;
		
		lq.resetTime();
				
		L = lq.getL();
		
		icondpd(0) = isa*cos(the) + isb*sin(the);
		icondpd(1) = isb*cos(the) - isa*sin(the);
		icondpd(2) = ome - Ww;
		icondpd(3) = the;
		icondpd(4) = Ww;
		icondpd(5) = u_old(0);
		icondpd(6) = u_old(1);
		icondpd(7) = 0;	
			
		tmp = L*icondpd;		
						
		udq = tmp(0,1);
		
		
		
		double Isd = isa*cos(the)+isb*sin(the);
		double Isq = isb*cos(the)-isa*sin(the);
		
		double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec
		
		udq(0) = (Cud.ctrlaction(vec_1(-Isd)))(0);
		udq(1) = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0);
		
		const double Ls0=0.003465; // inductance
		const double Fmag0= 0.1989; // Magnetic??
		
		udq(0)-=Ls0*ome*Iqw; // har
		udq(1)+=Fmag0*ome;	
		
		
		
		//bicriterial
		double omerand = ome;// + sqrt(5.0e-6)*randn();
		udq(0) -= sign(omerand)*bcbv;
		udq(1) += sign(omerand)*bcbv;	
		
				
		//uab = udq; //set size
		uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
		uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
		
		if(MAXuflag == 1){ //square cut off
			if(uab(0) > MAXu) uab(0) = MAXu;
			else if(uab(0) < -MAXu) uab(0) = -MAXu;
			if(uab(1) > MAXu) uab(1) = MAXu;
			else if(uab(1) < -MAXu) uab(1) = -MAXu;
		}
		else if(MAXuflag == 2){ //circular cut off
			double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
			double uangl = atan2(uab(1), uab(0));
			if (uampl > MAXu) uampl = MAXu;
			uab(0) = uampl*cos(uangl);
			uab(1) = uampl*sin(uangl);
		}
		//else{ //(MAXuflag == 0) //no cut off }
		
		u_old = udq;
		
		return uab;			
	}
	
	void from_setting(const Setting &set){
		PMSMCtrl::from_setting(set);
		UI::get(r,set, "r", UI::optional);
		UI::get(rec_hor,set, "h", UI::optional);
		UI::get(MAXu,set, "MAXu", UI::optional);
		UI::get(MAXuflag,set, "MAXuflag", UI::optional);
		UI::get(rpd,set, "rpd", UI::optional);	
		UI::get(bcbv,set, "bcbv", UI::optional);	
	}

	void validate(){
		R(0, 0) = R(1, 1) = r;
		Rpd(0, 0) = Rpd(1, 1) = rpd;
		Rpd(0, 2) = Rpd(1, 3) = -rpd;
				
		p_isa.set_length(rec_hor+1);
		p_isb.set_length(rec_hor+1);
		p_ome.set_length(rec_hor+1);
		p_the.set_length(rec_hor+1);

		Array<quadraticfn> qloss(3);
		qloss(0).Q.setCh(Q);
		qloss(0).rv = RV("x", 5, 1);		
		qloss(1).Q.setCh(R);
		qloss(1).rv = RV("u", 2, 0);
		qloss(2).Q.setCh(Rpd);		
		qloss(2).rv = RV("u", 2, 0);
		qloss(2).rv.add(RV("u", 2, -1));		
		lq.Losses = qloss;		

		//set lq
		lq.rv = RV("u", 2, 0);	
		RV tmp = RV("x", 5, 0);
		tmp.add(RV("u", 2, -1));
		lq.set_rvc(tmp);		
		lq.horizon = rec_hor;	
		
		Array<linfnEx> model(2);
		model(0).A = A;
		model(0).B = vec("0 0 0 0 0");
		model(0).rv = RV("x", 5, 0);
		model(0).rv_ret = RV("x", 5, 1);
		model(1).A = B;
		model(1).B = vec("0 0");
		model(1).rv = RV("u", 2, 0);
		model(1).rv_ret = RV("x", 5, 1);
		lq.Models = model;
		
		lq.finalLoss.Q.setCh(Q);
		lq.finalLoss.rv = RV("x", 5, 1);
				
		lq.validate();
						
		uab.zeros();
				
		//create control matrix
		for(int i = rec_hor; i > 0; i--){								
			lq.redesign();
		}
		lq.redesign();
	}
};
UIREGISTER(PMSM_LQCtrl_bic2);

/*************************************/
/*			PI bicriterial 3 s vice EKF pro urceni nejlepsi bikrit*/
/*************************************/

class PMSM_LQCtrl_bic3: public PMSMCtrl{
	LOG_LEVEL(PMSM_LQCtrl_bic3, logModel);
public:
	
/*
PMSMCtrl:
	double isa;
	double isb;
	double ome;
	double the;
	double Ww;
*/	

//PMSM parameters
	const double a;
	const double b;
	const double c;
	const double d;
	const double e;
	
//input penalty
	double r;
	double rpd; //penalize differences u - u_old
	
//time step
	const double Dt;
	
//receding horizon
	int rec_hor;
	
//system matrices
	mat A; //5x5
	mat B; //5x2
	//mat C; //2x5
	mat Q; //5x5
	mat R; //2x2
	mat Rpd; //2x4
	
//control matrix
	mat L;
	vec uab;	
	vec icondpd;
	vec u_old;
	
//control maximum
	double MAXu;
	int MAXuflag;
	
//lqg controler class
	LQG_universal lq; 	
	
//prediction
	vec p_isa, p_isb, p_ome, p_the;
	
//bicrit param
	double bcbv;
	
	
//pi version	
	PI_ctrl Cwq;
	PI_ctrl Cuq;
	PI_ctrl Cud;
	
// 5KF
	mat Ptp, Kt, Ared, Cred, Qred, Rred;
	mat Pt1,Pt2,Pt3,Pt4,Pt5;
	vec varPth;
	int timeid;
	ofstream f;

	int biver;
	
// inj
	double injkon, injome, injphi;
	int minindex;
		
	PMSM_LQCtrl_bic3():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149),
				r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), 					//for r is a default value rewrited by pcp.txt file value
				A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), biver(0),
				uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0),
				Ptp(2,2),Kt(2,2),Ared(2,2),Cred(2,2),Qred(2,2),Rred(2,2),
				Pt1(2,2),Pt2(2,2),Pt3(2,2),Pt4(2,2),Pt5(2,2), varPth(5),				
				/*Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200), 
				Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200),
				Cud(20.0, 20.0*0.000125/0.005, -1200, 1200)*/
				Cwq(10.0, 0.0005, -1200, 1200), 
				Cuq(0.4, 0.0005, -1200, 1200),
				Cud(0.4, 0.0005, -1200, 1200)	{			
		//set fix matrices elements & dimensions
		A.zeros();
			A(0, 0) = A(1, 1) = a;
			A(1, 2) = A(1, 4) = -b;
			A(2, 1) = e;
			A(2, 2) = d;
			A(2, 4) = d - 1;
			A(3, 2) = A(3, 4) = Dt;
			A(3, 3) = A(4, 4) = 1.0;
		B.zeros();
			B(0, 0) = B(1, 1) = c;		
		Q.zeros();
			Q(2, 2) = 1.0;
		R.zeros();
		Rpd.zeros();
		u_old.zeros();	
		
		Ared(0,0) = d;
		Ared(1,0) = Dt;
		Ared(1,1) = 1.0;
		Qred(0,0) = 0.1;
		Qred(1,1) = 0.01;		
		Rred(0,0) = Rred(1,1) = 0.5; 
		Pt1(0,0) = Pt2(0,0) = Pt3(0,0) = Pt4(0,0) = Pt5(0,0) = 1.0;	
		Pt1(1,1) = Pt2(1,1) = Pt3(1,1) = Pt4(1,1) = Pt5(1,1) = 1.0;	
		
		timeid = 0;			
		f.open("skf.dat", ios::out);
		
		injkon = injome = injphi = 0.0;
		
		srand(time(NULL));
	}
	
	~PMSM_LQCtrl_bic3(){
		f.close();
	}	
	
	virtual vec ctrlaction(const itpp::vec& cond) {
		PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww
				
		vec udq(2);
				
		double Isd = isa*cos(the)+isb*sin(the);
		double Isq = isb*cos(the)-isa*sin(the);
		
		double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec
		
		udq(0) = (Cud.ctrlaction(vec_1(-Isd)))(0);
		udq(1) = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0);
		
		const double Ls0=0.003465; // inductance
		const double Fmag0= 0.1989; // Magnetic??
		
		udq(0)-=Ls0*ome*Iqw; // har
		udq(1)+=Fmag0*ome;	
		
		
		
		//bicriterial/////////////////////////////////////////////////////////////
		//verze 1: signum
		if(biver == 1){
			udq(0) -= sign(ome)*bcbv;
			udq(1) += sign(ome)*bcbv;
		}
		//*/	
		
		//verze 2: casovy posun
		if(biver == 2){
		     double psi = (d*d - b*e)*ome + (a + d)*e*Isq - e*Dt*Isd*ome;
		     udq(1) += sign(psi)*bcbv;
		    
		     double ksi = d*psi + a*c*e*udq(1) - e*(a*b + b*d + a*Dt*(1+d)*Isd)*ome + e*(a*a - b*e - a*e*Dt*Isd)*Isq - e*Dt*Dt*(d*ome + e*Isq)*Isq*ome;
		     udq(0) -= sign(ksi)*bcbv;
          }
          //*/
          
          //verze 3: 3KFdq 
          if((biver == 3)||(biver == 30)){   
		     double du_al = bcbv*cos(the);
			double du_be = bcbv*sin(the);      
			double ia = a*isa + b*ome*sin(the) + c*(uab(0) + du_al);
			double ib = a*isb - b*ome*cos(the) + c*(uab(1) + du_be);    
			double om = d*ome + e*(isb*cos(the) - isa*sin(the));
			double th = the + Dt*ome;		
			Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));
			Cred(0,0) = b*sin(th);
			Cred(0,1) = b*om*cos(th);
			Cred(1,0) = -b*cos(th);
			Cred(1,1) = b*om*sin(th);		
			Ptp = Ared*Pt1*Ared.T() + Qred;
			Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
			Pt1 = (eye(2) - Kt*Cred)*Ptp;
			varPth(0) = Pt1(1,1);
			if(biver == 30) varPth(0) = Pt1(0,0)*Pt1(1,1) - Pt1(0,1)*Pt1(1,0);		
		
			ia = a*isa + b*ome*sin(the) + c*(uab(0) - du_al);
			ib = a*isb - b*ome*cos(the) + c*(uab(1) - du_be);				
			Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));				
			Ptp = Ared*Pt2*Ared.T() + Qred;
			Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
			Pt2 = (eye(2) - Kt*Cred)*Ptp;
			varPth(1) = Pt2(1,1);
			if(biver == 30) varPth(1) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
				
			ia = a*isa + b*ome*sin(the) + c*(uab(0));
			ib = a*isb - b*ome*cos(the) + c*(uab(1));				
			Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));				
			Ptp = Ared*Pt5*Ared.T() + Qred;
			Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
			Pt5 = (eye(2) - Kt*Cred)*Ptp;
			varPth(2) = Pt5(1,1);
			if(biver == 30) varPth(2) = Pt5(0,0)*Pt5(1,1) - Pt5(0,1)*Pt5(1,0);
				
			minindex = 0;
			for(int i = 1; i < 3; i++){
				if(varPth(i) < varPth(minindex)){				
					minindex = i;
				}
			}
		
			f << timeid << "\t" << minindex << endl;
			timeid++;
		
			switch(minindex){
				case 0:
					udq(0) += bcbv;				
					break;
				case 1:
					udq(0) -= bcbv;				
					break;			
				case 2:				
					break;
			}
		}
          //*/
          
          //verze 5: konst. v d
          if(biver == 5){
          	udq(0) += 2*bcbv;
          }
          //*/
          
          //verze 6: injektaz v dq
          if(biver == 6){
          	udq(0) += injkon*cos(injome*Dt*timeid + injphi);
          	//udq(1) += injkon*cos(injome*Dt*timeid + injphi);
          	
          	timeid++;
          }
          //*/
          
          //verze 8: injektaz v dq
          if(biver == 8){
          	udq(0) += injkon*cos(injome*Dt*timeid + injphi);
          	udq(1) += injkon*sin(injome*Dt*timeid + injphi);
          	
          	timeid++;
          }
          //*/
          
          //uab = udq; //set size
		uab(0) = udq(0)*cos(the) - udq(1)*sin(the);
		uab(1) = udq(1)*cos(the) + udq(0)*sin(the);
          
          //verze 4: 5KF          
          if((biver == 4)||(biver == 40)){
			double ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv);
			double ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);    
			double om = d*ome + e*(isb*cos(the) - isa*sin(the));
			double th = the + Dt*ome;		
			Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));
			Cred(0,0) = b*sin(th);
			Cred(0,1) = b*om*cos(th);
			Cred(1,0) = -b*cos(th);
			Cred(1,1) = b*om*sin(th);		
			Ptp = Ared*Pt1*Ared.T() + Qred;
			Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
			Pt1 = (eye(2) - Kt*Cred)*Ptp;
			varPth(0) = Pt1(1,1);
			if(biver == 40) varPth(0) = Pt1(0,0)*Pt1(1,1) - Pt1(0,1)*Pt1(1,0);
		
			ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv);
			ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);				
			Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));				
			Ptp = Ared*Pt2*Ared.T() + Qred;
			Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
			Pt2 = (eye(2) - Kt*Cred)*Ptp;
			varPth(1) = Pt2(1,1);
			if(biver == 40) varPth(1) = Pt2(0,0)*Pt2(1,1) - Pt2(0,1)*Pt2(1,0);
		
			ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv);
			ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);				
			Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));				
			Ptp = Ared*Pt3*Ared.T() + Qred;
			Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
			Pt3 = (eye(2) - Kt*Cred)*Ptp;
			varPth(2) = Pt3(1,1);
			if(biver == 40) varPth(2) = Pt3(0,0)*Pt3(1,1) - Pt3(0,1)*Pt3(1,0);
		
			ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv);
			ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);				
			Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));				
			Ptp = Ared*Pt4*Ared.T() + Qred;
			Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
			Pt4 = (eye(2) - Kt*Cred)*Ptp;
			varPth(3) = Pt4(1,1);
			if(biver == 40) varPth(3) = Pt4(0,0)*Pt4(1,1) - Pt4(0,1)*Pt4(1,0);
		
			ia = a*isa + b*ome*sin(the) + c*(uab(0));
			ib = a*isb - b*ome*cos(the) + c*(uab(1));				
			Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));				
			Ptp = Ared*Pt5*Ared.T() + Qred;
			Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred);
			Pt5 = (eye(2) - Kt*Cred)*Ptp;
			varPth(4) = Pt5(1,1);
			if(biver == 40) varPth(4) = Pt5(0,0)*Pt5(1,1) - Pt5(0,1)*Pt5(1,0);
				
			minindex = 0;
			for(int i = 1; i < 5; i++){
				if(varPth(i) < varPth(minindex)){				
					minindex = i;
				}
			}
		
			f << timeid << "\t" << minindex << endl;
			timeid++;
		
			switch(minindex){
				case 0:
					uab(0) += bcbv;
					uab(1) += bcbv;
					break;
				case 1:
					uab(0) += bcbv;
					uab(1) -= bcbv;
					break;
				case 2:
					uab(0) -= bcbv;
					uab(1) += bcbv;
					break;
				case 3:
					uab(0) -= bcbv;
					uab(1) -= bcbv;
					break;
				case 4:				
					break;
			}
		}
          //*/
          
          //verze 7: injektaz v alfa beta
          if(biver == 7){
          	uab(0) += injkon*cos(injome*Dt*timeid + injphi);
          	uab(1) += injkon*sin(injome*Dt*timeid + injphi);
          	
          	timeid++;
          }
          
          //verze 9: nahodne
          if(biver == 9){
          	minindex = rand() % 5;
          	switch(minindex){
				case 0:
					uab(0) += bcbv;
					uab(1) += bcbv;
					break;
				case 1:
					uab(0) += bcbv;
					uab(1) -= bcbv;
					break;
				case 2:
					uab(0) -= bcbv;
					uab(1) += bcbv;
					break;
				case 3:
					uab(0) -= bcbv;
					uab(1) -= bcbv;
					break;
				case 4:				
					break;
			}
          }
          
          //verze 10: postupne
          if(biver == 10){
          	minindex = timeid % 5;
          	switch(minindex){
				case 0:
					uab(0) += bcbv;
					uab(1) += bcbv;
					break;
				case 1:
					uab(0) += bcbv;
					uab(1) -= bcbv;
					break;
				case 2:
					uab(0) -= bcbv;
					uab(1) += bcbv;
					break;
				case 3:
					uab(0) -= bcbv;
					uab(1) -= bcbv;
					break;
				case 4:				
					break;
			}
			timeid++;
          }
          
          //verze 11: inj. round
          if(biver == 11){
          	double deltaua = itpp::round(sin(injome*Dt*timeid + injphi));
          	double deltaub = itpp::round(cos(injome*Dt*timeid + injphi));
          	uab(0) += injkon*deltaua;
          	uab(1) += injkon*deltaub;		     
		   
		   	if(deltaua*deltaub == 0) minindex = 4;
		   	else if(deltaua > 0){
		   		if(deltaub > 0) minindex = 0;
		   		else minindex = 1;
		   	}
		   	else{
		   		if(deltaub > 0) minindex = 2;
		   		else minindex = 3;
		   	}
		   
			timeid++;   
		}
          //*/
		
		//////////////////////////////////////////////////////////////////////////
				
		
		
		if(MAXuflag == 1){ //square cut off
			if(uab(0) > MAXu) uab(0) = MAXu;
			else if(uab(0) < -MAXu) uab(0) = -MAXu;
			if(uab(1) > MAXu) uab(1) = MAXu;
			else if(uab(1) < -MAXu) uab(1) = -MAXu;
		}
		else if(MAXuflag == 2){ //circular cut off
			double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1));
			double uangl = atan2(uab(1), uab(0));
			if (uampl > MAXu) uampl = MAXu;
			uab(0) = uampl*cos(uangl);
			uab(1) = uampl*sin(uangl);
		}
		//else{ //(MAXuflag == 0) //no cut off }
		
		//u_old = udq;
		
		return uab;			
	}
	
	void from_setting(const Setting &set){
		PMSMCtrl::from_setting(set);
		UI::get(r,set, "r", UI::optional);
		UI::get(rec_hor,set, "h", UI::optional);
		UI::get(MAXu,set, "MAXu", UI::optional);
		UI::get(MAXuflag,set, "MAXuflag", UI::optional);
		UI::get(rpd,set, "rpd", UI::optional);	
		UI::get(bcbv,set, "bcbv", UI::optional);	
		UI::get(biver,set, "biver", UI::optional);
		UI::get(injkon,set, "injkon", UI::optional);
		UI::get(injome,set, "injome", UI::optional);
		UI::get(injphi,set, "injphi", UI::optional);
		
		UI::get(Qred(0,0),set, "Qred00", UI::optional);
		UI::get(Qred(0,1),set, "Qred01", UI::optional);
		UI::get(Qred(1,0),set, "Qred10", UI::optional);
		UI::get(Qred(1,1),set, "Qred11", UI::optional);
		UI::get(Rred(0,0),set, "Rred00", UI::optional);
		UI::get(Rred(0,1),set, "Rred01", UI::optional);
		UI::get(Rred(1,0),set, "Rred10", UI::optional);
		UI::get(Rred(1,1),set, "Rred11", UI::optional);
		
		double Pi,Pd;
		Cwq.get_params(Pd,Pi);
		UI::get(Pi,set,"PIw_Pi",UI::optional);
		UI::get(Pd,set,"PIw_Pd",UI::optional);
		Cwq.set_params(Pd,Pi);
		
		Cud.get_params(Pd,Pi);
		UI::get(Pi,set,"PIu_Pi",UI::optional);
		UI::get(Pd,set,"PIu_Pd",UI::optional);
		Cud.set_params(Pd,Pi);
		Cuq.set_params(Pd,Pi);

	}

	void validate(){
		R(0, 0) = R(1, 1) = r;
		Rpd(0, 0) = Rpd(1, 1) = rpd;
		Rpd(0, 2) = Rpd(1, 3) = -rpd;
				
		p_isa.set_length(rec_hor+1);
		p_isb.set_length(rec_hor+1);
		p_ome.set_length(rec_hor+1);
		p_the.set_length(rec_hor+1);

		Array<quadraticfn> qloss(3);
		qloss(0).Q.setCh(Q);
		qloss(0).rv = RV("x", 5, 1);		
		qloss(1).Q.setCh(R);
		qloss(1).rv = RV("u", 2, 0);
		qloss(2).Q.setCh(Rpd);		
		qloss(2).rv = RV("u", 2, 0);
		qloss(2).rv.add(RV("u", 2, -1));		
		lq.Losses = qloss;		

		//set lq
		lq.rv = RV("u", 2, 0);	
		RV tmp = RV("x", 5, 0);
		tmp.add(RV("u", 2, -1));
		lq.set_rvc(tmp);		
		lq.horizon = rec_hor;	
		
		Array<linfnEx> model(2);
		model(0).A = A;
		model(0).B = vec("0 0 0 0 0");
		model(0).rv = RV("x", 5, 0);
		model(0).rv_ret = RV("x", 5, 1);
		model(1).A = B;
		model(1).B = vec("0 0");
		model(1).rv = RV("u", 2, 0);
		model(1).rv_ret = RV("x", 5, 1);
		lq.Models = model;
		
		lq.finalLoss.Q.setCh(Q);
		lq.finalLoss.rv = RV("x", 5, 1);
				
		lq.validate();
						
		uab.zeros();
				
		//create control matrix
		for(int i = rec_hor; i > 0; i--){								
			lq.redesign();
		}
		lq.redesign();
	}
	
	void log_register ( logger &L, const string &prefix ) {
		PMSMCtrl::log_register(L,prefix);
		L.add_vector ( log_level, logModel, RV ( 1), prefix );
	}
	void log_write() const{
		PMSMCtrl::log_write();
		log_level.store( logModel , double(minindex)); 
		
	}

};
UIREGISTER(PMSM_LQCtrl_bic3);


/*!@}*/
#endif //PMSM_CTR_H