/*!
  \file
  \brief DataSource for experiments with Aimsun
  \author Jan prikryl.

*/

#include <base/loggers.h>

using namespace bdm;

//! Simulator of traffic - AIMSUN
class AimsunDS : 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
    AimsunDS () : 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);

    };

 

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