/*!
\file
\brief Multi-Estimator (developped for PMSM)

 */

#include "base/user_info.h"
#include "base/loggers.h"
#include "estim/kalman.h"
#include "estim/mixtures.h"
#include "pmsmDS.h"
#include "filters.h"
#include "base/datasources.h"
#include "simulator_zdenek/ekf_example/ekf_obj.h"

using namespace bdm;
#ifdef MEX
#include <itpp/itmex.h>
#include <mex/mex_BM.h>
#include <mex/mex_logger.h>
#include <mex/mex_datasource.h>
#include <mex/mex_function.h>

void mexFunction ( int n_output, mxArray *output[], int n_input, const mxArray *input[] ) {
    // Check the number of inputs and output arguments
    if ( n_input<2 ) mexErrMsgTxt ( "Usage:\n"
                                        "[Res,estimators,Res2]=estimator(system, estimators, experiment, logger)\n"
                                        "  system     = struct('class','datasource',...);  % Estimated system\n"
                                        "  estimators = {struct('class','estimator',...),  % Estimators\n"
                                        "                struct('class','estimator',...),...} \n"
                                        "  === optional ==="
                                        "  experiment = struct('ndat',10);                 % number of data in experiment, full length of finite datasources, 10 otherwise \n"
                                        "  logger     = struct('class','mexlogger');       % How to store results, default=mexlog, i.e. matlab structure\n\n"
                                        "Output:\n"
                                        "  Res          Matlab structure with logged results, \n"
                                        "  estimators   Array of estimators updated with data \n"
                                        "  Res2         When logfull log_level is on, this structure is filled with structures of posterior densities\n\n"
                                        "see documentation of classes datasource, BM, and mexlogger and their offsprings in BDM." );

    RV::clear_all();
    //CONFIG
    UImxArray F;
    try {
        F.addGroup ( input[0],"system" );
        F.addList ( input[1],"estimators" );
        if ( n_input>2 ) {
            F.addGroup ( input[2],"experiment" );
        }
        if ( n_input>3 ) {
            F.addGroup ( input[3],"logger" );
        }
    } catch ( SettingException e ) {
        it_error ( "error: "+string ( e.getPath() ) );
    }

    //DBG
    F.writeFile ( "pmsm_estim.cfg" );

#else
int main ( int argc, char* argv[] ) {
    const char *fname;
    if ( argc>1 ) {
        fname = argv[1];
    }
    else {
        cout << "Missing configuration file.\n Usage: \n $> estimator config_file.cfg"<<endl;
        abort();
    }
    UIFile F ( fname );
#endif

    shared_ptr<logger> L = UI::build <logger>( F, "logger" );
    shared_ptr<DS>  pDS = UI::build<DS> ( F, "system" );
    Array<shared_ptr<BM> > Es;			// array of estimators
    UI::get( Es, F, "estimators" );
    int nE = Es.length();	//number of estimators
    int Ndat;				//number of data records
    F.lookupValue ( "experiment.ndat",Ndat );

    if ( !L ) {
#ifdef MEX
        //mex logger has only from_setting constructor - we  have to fill it...
        L=new mexlog ( Ndat );
#else
        L=new stdlog();
#endif
    }
    pDS->log_register ( *L, "true" );
    string Ename;
    for (int i=0; i<nE; i++) {
        try {
            UI::get ( Ename, F.getRoot()["estimators"][i], "name",UI::optional );
        } catch ( ...) {
            Ename="Est"+num2str ( i );
        }
        Es(i)->log_register(*L,Ename); // estimate
    }
    L->init();

    vec dt=zeros ( pDS->_drv()._dsize() );   //data variable
    Array<datalink*> Dls(nE);
    Array<datalink*> Dlsc(nE);
    Array<datalink_buffered*> Dls_buf (0);
    for ( int i=0; i<Es.length(); i++ ) {
        //check if they are properly named
        bdm_assert(Es(i)->_yrv()._dsize() == Es(i)->dimensiony(), "Estimator["+num2str(i)+"] does not name yrv properly."
                   "size(yrv)="+num2str(Es(i)->_yrv()._dsize() ) + ", declared dimension of y="+num2str(Es(i)->dimensiony()));
        bdm_assert(Es(i)->_rvc()._dsize() == Es(i)->dimensionc(), "Estimator["+num2str(i)+"] does not name rvc properly."
                   "size(rvc)="+num2str(Es(i)->_rvc()._dsize() ) + ", declared dimension of rvc="+num2str(Es(i)->dimensionc()));
        //connect actual data
        Dls ( i ) = new datalink ( Es ( i )->_yrv(),pDS->_drv() ); //datalink between a datasource and estimator
        //connect data in condition
        if (Es ( i )->_rvc().mint()<0) {
            //delayed values are required

            //create delayed dl
            int ith_buf=Dls_buf.size();
            Dls_buf.set_size( ith_buf + 1, true);
            Dls_buf(ith_buf) = new datalink_buffered();
            //add dl to list of buffered DS
            Dlsc(i) = Dls_buf(ith_buf);
            Dlsc(i)->set_connection ( Es ( i )->_rvc(),pDS->_drv() ); //datalink between a datasource and estimator

            bdm_assert_debug(Dlsc(i)->_downsize() == Es ( i )->_rvc()._dsize(), "Data required by Est[" + num2str(i) + "], " +
                             Es(i)->_rvc().to_string() + ", are not available in DS drv:" + pDS->_drv().to_string(););
        } else {
            Dlsc ( i ) = new datalink ( Es ( i )->_rvc(),pDS->_drv() ); //datalink between a datasource and estimator
        }
    }

    // Main cycle
    for ( int tK=1;tK<Ndat;tK++ ) {
        // Data Source
        pDS->getdata ( dt );					// read data
        pDS->log_write ();

        // Estimators
        for (int i=0; i<nE; i++) {
            Es(i)->bayes ( Dls(i)->pushdown ( dt ), Dlsc(i)->pushdown(dt) );		// update estimates

            Es(i)->log_write ();
        }
        // Regulators
        L->step();
		pDS->step();							// simulator step
		for (int i=0; i<Dls_buf.length(); i++){
			Dls_buf(i)->store_data(dt);
		}
	}

    L->finalize();
#ifdef MEX
    mexlog* mL=dynamic_cast<mexlog*> ( L.get() );

    if ( mL ) { // user wants output!!
        if ( n_output<1 ) mexErrMsgTxt ( "Wrong number of output variables!" );
        output[0] = mL->toCell();
        if (n_output>1) { // write estimators
            UImxArray Ests;
            UI::save(Es, Ests,"estimators");
            output[1]=UImxArray::create_mxArray(Ests);
        }
        if (n_output>2) {
            mL->_setting_conf().setAutoConvert(true);
            output[2]= UImxArray::create_mxArray(mL->_setting_conf().getRoot());
        }
    }
#endif

    for (int i=0; i<nE; i++) {
        delete Dls(i);
        delete Dlsc(i);
    }
}