/*!
  \file
  \brief DataSource for experiments with realistic simulator of the PMSM model
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

  Using IT++ for numerical operations
  -----------------------------------
*/

#ifndef PMSM_DS
#define PMSM_DS

#include <base/loggers.h>
#include <estim/kalman.h>
#include "simulator_zdenek/simulator.h"
#include "pmsm.h"

using namespace bdm;

//! Simulator of PMSM machine with predefined profile on omega
class pmsmDS : public DS
{
	LOG_LEVEL(pmsmDS,logvoltage);

protected:
    //! indices of logged variables
    int L_x, L_ou, L_oy, L_iu, L_optu;
    //! Setpoints of omega in timespans given by dt_prof
    vec profileWw;
    //! Setpoints of Mz in timespans given by dt_prof
    vec profileMz;
    //! time-step for profiles
    double dt_prof;
    //! Number of miliseconds per discrete time step
    int Dt;    
public:
    //! Constructor with fixed sampling period
    pmsmDS () : DS()
    {
        Dt=125;
        Drv=RV ( "{o_ua o_ub o_ia o_ib t_ua t_ub o_om o_th Mz }" );
		dtsize = Drv._dsize();
    }
    void set_parameters ( double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0 )
    {
        pmsmsim_set_parameters ( Rs0, Ls0, Fmag0, Bf0, p0, kp0, J0, Uc0, DT0, dt0 );
    }

	void getdata ( vec &dt ) const
    {
        dt.set_subvector(0,vec ( KalmanObs,6 ));
		// add measurement noise
		dt(2) += (2*randun()-1);
		dt(3) += (2*randun()-1);
		
		// true values
        dt(6)=x[2];
        dt(7)=x[3];
        dt(8)=x[8];
    }
    void write ( vec &ut ) {}

    void step()
    {
        static int ind=0;
        static double dW; // increase of W
        static double Ww; // W
        static double Mz; // W
        if ( t>=dt_prof*ind )
        {
            ind++;
            // check omega profile and set dW
			if ( ind <2 && profileWw.length() ==1 )
			{
				Ww=profileWw ( 0 );
				dW=0.0;
			}
			if ( ind<profileWw.length() )
            {
                    dW = profileWw ( ind )-profileWw ( ind-1 );
                    dW *=125e-6/dt_prof;
            }
            else
            {
                dW = 0;
            }
            // Check Mz profile and set Mz
            if ( ind<profileMz.length() )
            {
                //sudden increase
                Mz = profileMz(ind);
            }
            else
            {
                Mz = 0;
            }
        }
        Ww += dW;
        //Simulate Dt seconds!
        for ( int i=0; i<Dt; i++ )
        {
            pmsmsim_step ( Ww , Mz);
        }
// 		for ( int i=0;i<Dt;i++ ) {	pmsmsim_noreg_step ( Ww , Mz);}

        //discretization
/*        double ustep=1.2;
        KalmanObs [ 0 ] = ustep*itpp::round( KalmanObs [ 0 ]/ ustep) ;
        KalmanObs [ 1 ] = ustep*itpp::round(KalmanObs [ 1 ]/ ustep);*/
//         double istep=0.085;
//         KalmanObs [ 2 ] = istep*itpp::round( KalmanObs [ 2 ]/ istep) ;
//         KalmanObs [ 3 ] = istep*itpp::round(KalmanObs [ 3 ]/ istep);

    };

    void log_register ( logger &L , const string &prefix)
    {
		DS::log_register ( L, prefix );
		
       // log differences
        if ( log_level[logvoltage] )
        {
            L.add_vector ( log_level, logvoltage, RV("{ua, ub }"), "model" );
        }
    }

    void log_write ( logger &L )
    {
        if ( log_level[logvoltage] )
        {
            double sq3=sqrt ( 3.0 );
            double ua,ub;
            double i1=x[0];
            double i2=0.5* ( -i1+sq3*x[1] );
            double i3=0.5* ( -i1-sq3*x[1] );
            double u1=KalmanObs[0];
            double u2=0.5* ( -u1+sq3*KalmanObs[1] );
            double u3=0.5* ( -u1-sq3*KalmanObs[1] );

            double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
            double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
            double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
            ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
            ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
            log_level.store( logvoltage , vec_2 ( ua,ub ) );
        }

    }

    void set_profile ( double dt, const vec &Ww, const vec &Mz )
    {
        dt_prof=dt;
        profileWw=Ww;
        profileMz=Mz;
    }

    void from_setting( const Setting &root )
    {
		const SettingResolver& params_l(root["params"]);
		const Setting &params = params_l.result;
        set_parameters ( params["Rs"], params["Ls"], params["Fmag"], \
                         params["Bf"], params["p"], params["kp"], \
                         params["J"], params["Uc"], params["DT"], 1.0e-6 );

        // Default values of profiles for omega and Mz
        vec profW=vec("1.0");
        vec profM=vec("0.0");
        double tstep=1.0;
        UI::get( tstep, root,  "tstep");
        UI::get( profW, root, "profileW" );
        UI::get( profM, root, "profileM" );
        set_profile (tstep , profW, profM);

    }

    // TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( pmsmDS );

//! Simulator of PMSM machine with predefined profile on omega
class pmsmDSctrl : public pmsmDS
{
	double Ww;
	double ua;
	double ub;
public:
	//! Constructor with fixed sampling period
    pmsmDSctrl() : pmsmDS()
	{
		Drv.add(RV ( "{Ww }" ));
		Urv=RV ( "{ua ub }" );
		dtsize = Drv._dsize();
		utsize = Urv._dsize();
	}
		
		void getdata ( vec &dt ) const
		{
			pmsmDS::getdata(dt);
			dt(9)=Ww;
		}
		void write (const vec &ut ) {
			ua = ut(0);
			ub = ut(1);
		}
		
		void step()
		{
			static int ind=0;
			static double dW; // increase of W
			static double Mz; // W
			t+=125e-6;
			if ( t>=dt_prof*ind )
			{
				ind++;
				// check omega profile and set dW
				if ( ind <2 && profileWw.length() ==1 )
				{
					Ww=profileWw ( 0 );
					dW=0.0;
				}
				if ( ind<profileWw.length() )
				{
					dW = profileWw ( ind )-profileWw ( ind-1 );
					dW *=125e-6/dt_prof;
				}
				else
				{
					dW = 0;
				}
				// Check Mz profile and set Mz
				if ( ind<profileMz.length() )
				{
					//sudden increase
					Mz = profileMz(ind);
				}
				else
				{
					Mz = 0;
				}
			}
			Ww += dW;
			//Simulate Dt seconds!
			for ( int i=0; i<Dt; i++ )
			{
				pmsmsim_noreg_step (ua , ub);
			}
	};
	
	};
	
	UIREGISTER ( pmsmDSctrl );
	

//! This class behaves like BM but it is evaluating EKF
class pmsmCRB : public EKFfull
{
protected:
    vec interr;
    vec old_true;
    vec secder;
    int L_CRB;
    int L_err;
    int L_sec;
public:
    //! constructor
    pmsmCRB():EKFfull()
    {
        old_true=zeros(6);
    }

    void bayes(const vec &dt)
    {
        static vec umin(2);
		vec u(2);
		vec &mu = est._mu();
		//assume we know state exactly:
        vec true_state=vec(x,4); // read from pmsm
        mu=true_state;

        //integration error
        old_true(4)=KalmanObs[4];
        old_true(5)=KalmanObs[5];// add U
        u(0) = KalmanObs[0]; // use the required value for derivatives
        u(1) = KalmanObs[1];
        interr = (true_state - pfxu->eval(old_true));

        //second derivative
        IMpmsm2o* pf = dynamic_cast<IMpmsm2o*>(pfxu.get());
        if (pf)
        {
            secder=pf->eval2o(u-umin);
        }

        umin =u;
        EKFfull::bayes(dt);
        old_true.set_subvector(0,true_state);
    }

    void from_setting( const Setting &root )
    {
        shared_ptr<diffbifn> IM = UI::build<diffbifn>(root, "IM");
        shared_ptr<diffbifn> OM = UI::build<diffbifn>(root, "OM");

        //parameters

        //statistics
        int dim=IM->dimension();

        vec mu0;
        if(root.exists("mu0"))
			UI::get( mu0, root, "mu0");
		else
            mu0=zeros(dim);

        mat P0;
        if(root.exists( "dP0" ))
		{
			vec dP0;	
			UI::get(dP0,root, "dP0");
            P0=diag(dP0);
		}
		else if (root.exists("P0"))
			UI::get(P0,root, "P0");
		else
            P0=eye(dim);

        set_statistics(mu0,P0);

        vec dQ;
        UI::get( dQ, root, "dQ");
        vec dR;
        UI::get( dR, root, "dR");
        set_parameters(IM, OM, diag(dQ) , diag(dR));

        //connect
        shared_ptr<RV> drv = UI::build<RV>(root, "drv");
        set_yrv(*drv);
        shared_ptr<RV> rv = UI::build<RV>(root, "rv");
        set_rv(*rv);
    }

    // TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( pmsmCRB );


//! This class behaves like BM but it is evaluating EKF
class pmsmCRBMz : public EKFfull
{
protected:
    int L_CRB;
public:
    //! constructor
    pmsmCRBMz():EKFfull() {}

    void bayes(const vec &dt)
    {
//assume we know state exactly:
        vec true_state(5);
        true_state.set_subvector(0,vec(x,4)); // read from pmsm
        true_state(4)=x[8];

        vec &mu = est._mu();
        mu = true_state;
        //hack for ut
        EKFfull::bayes(dt);
    }


    void from_setting( const Setting &root )
    {
        shared_ptr<diffbifn> IM = UI::build<diffbifn>(root,"IM");
		shared_ptr<diffbifn> OM = UI::build<diffbifn>(root,"OM");

        //statistics
        int dim=IM->dimension();
        vec mu0;
        if( root.exists( "mu0"))
			UI::get(mu0, root, "mu0");
		else
            mu0=zeros(dim);

        mat P0;

		if(root.exists("dP0"))
		{
			vec dP0;				
			UI::get(dP0, root, "dP0");
			P0=diag(dP0);
		}
		else if(root.exists("P0"))
			UI::get( P0, root, "P0" );
		else
			P0=eye(dim);

        set_statistics(mu0,P0);

        vec dQ;
        UI::get(dQ, root, "dQ");
        vec dR;
        UI::get(dR, root, "dR");
        set_parameters(IM, OM, diag(dQ), diag(dR));

        //connect
		shared_ptr<RV> drv = UI::build<RV>(root, "drv");
        set_yrv(*drv);
		shared_ptr<RV> rv = UI::build<RV>(root, "rv");
        set_rv(*rv);
    }

    // TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( pmsmCRBMz );

#endif