#ifndef PMSM_CTR_H
#define PMSM_CTR_H

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

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

class PMSM_PICtrl: public Controller{
protected:
	shared_ptr<BM> Est;
	vec state;
public:
	PI_ctrl Cwq;
	PI_ctrl Cuq;
	PI_ctrl Cud;

	PMSM_PICtrl():	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)
	{
		rv = RV("{ua ub }");
		rvc = RV("{o_ia o_ib o_ua o_ub Ww o_om o_th }");
	}
	
    void adapt(const itpp::vec& data){
		vec y=data.get(0,1);
		vec u=data.get(2,3);
		Est->bayes(y,u);
		
	}
	
	
    virtual vec ctrlaction(const itpp::vec& cond) {
		vec x_est=Est->posterior().mean();
		double isa=cond(0);//x_est(0);
		double isb=cond(1);//x_est(1);
		double ome=cond(5);//x[2];//x_est(2);
		double the=cond(6);//x_est(3);
		double Ww=cond(4);
		
		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;
	};
    void from_setting(const libconfig::Setting& set){
		Controller::from_setting(set);
		Est=UI::build<BM>(set,"estim",UI::compulsory);
	}
};
UIREGISTER(PMSM_PICtrl);

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