#ifndef PMSM_CTR_H
#define PMSM_CTR_H

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

#include <fstream>

/*! \defgroup PMSM 
@{
*/

using namespace bdm;


/*! 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);
	}
};

/*! \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.
 * 
 */
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 t_ua t_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);
			Est->bayes(y,u);
		}
	}
	
	
    virtual vec ctrlaction(const itpp::vec& cond) {
		
		if (estim){
			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;
		
	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
		
		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
		
		return uab;
	};
};
UIREGISTER(PMSM_PICtrl);

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
	const double MAXu;
	
//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)	{			
		//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);
		}
		
		//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) = 1;
		vec tmp = L*icond;		    
		
		uab = tmp(0,1);
		
		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;
		
		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);
	}

	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);

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