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

#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
{

protected:
    //! indeces 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;
    //! options for logging, - log predictions of 'true' voltage
    bool opt_modu;
    //! options for logging, -
public:
    //! Constructor with fixed sampling period
    pmsmDS () : DS()
    {
        Dt=125;
        Yrv=RV ( "{o_ua o_ub o_ia o_ib t_ua t_ub o_om o_th Mz }" );
		ytsize = Yrv._dsize();
		Drv = Yrv;
    }
    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 );
    }
    //! parse options: "modelu" => opt_modu=true;
    void set_options ( string &opt )
    {
        opt_modu = ( opt.find ( "modelu" ) !=string::npos );
    }
    void getdata ( vec &dt ) const
    {
        dt.set_subvector(0,vec ( KalmanObs,6 ));
        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 )
    {
        L_x = L.add_vector ( rx, "x" );
        L_oy = L.add_vector ( ry, "o" );
        L_ou = L.add_vector ( ru, "o" );
        L_iu = L.add_vector ( ru, "t" );
        // log differences
        if ( opt_modu )
        {
            L_optu = L.add_vector ( ru, "model" );
        }
    }

    void log_write ( logger &L )
    {
        L.log_vector ( L_x, vec ( x,4 )	);
        L.log_vector ( L_oy, vec_2 ( KalmanObs[2],KalmanObs[3] ) );
        L.log_vector ( L_ou, vec_2 ( KalmanObs[0],KalmanObs[1] ) );
        L.log_vector ( L_iu, vec_2 ( KalmanObs[4],KalmanObs[5] ) );
        if ( opt_modu )
        {
            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;
            L.log_vector ( L_optu , 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;
        root.lookupValue( "tstep", tstep );
        UI::get( profW, root, "profileW" );
        UI::get( profM, root, "profileM" );
        set_profile (tstep , profW, profM);

        string opts;
        if ( root.lookupValue( "options", opts ) )
            set_options(opts);
    }

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

UIREGISTER ( pmsmDS );


//! 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 log_add(logger &L, const string &name="" )
    {
        L_CRB=L.add_vector(rx,"crb");
        L_err=L.add_vector(rx,"err");
        L_sec=L.add_vector(rx,"d2");
    }
    void logit(logger &L)
    {
        L.log_vector(L_err, interr);
        L.log_vector(L_CRB,diag(_R()));
        L.log_vector(L_sec,secder);
    }

    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 log_add(logger &L, const string &name="" )
    {
        L_CRB=L.add_vector(concat(rx,RV("Mz",1,0)),"crbz");
    }
    void logit(logger &L)
    {
        L.log_vector(L_CRB,diag(_R()));
    }

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