#include "libPF.h"

using namespace itpp;

using std::endl;

PF::PF(vec w0){w=w0/sum(w0); n=w.length();}

ivec PF::resample( RESAMPLING_METHOD method ) {
	ivec ind=zeros_i( n );
	ivec N_babies = zeros_i( n );
	vec cumDist = cumsum( w );
	vec u( n );
	int i,j,parent;
	double u0;

	switch ( method ) {
	case MULTINOMIAL:
		u( n - 1 ) = pow( URNG.sample(), 1.0 / n );
		for ( i = n - 2;i >= 0;i-- ) {
			u( i ) = u( i + 1 ) * pow( URNG.sample(), 1.0 / ( i + 1 ) );
		}
		break;
	case STRATIFIED:
		for ( i = 0;i < n;i++ ) {
			u( i ) = ( i + URNG.sample() ) / n;
		}
		break;
	case SYSTEMATIC:
		u0 = URNG.sample();
		for ( i = 0;i < n;i++ ) {
			u( i ) = ( i + u0 ) / n;
		}
		break;
	default:
		it_error( "PF::resample(): Unknown resampling method" );
	}
	
// 	// <<<<<<<<<<<
// 	using std::cout;
// 	cout << "resampling:" << endl;
// 	cout << w <<endl;
// 	cout << cumDist <<endl;
// 	cout << u <<endl;

	// U is now full
	j = 0;
	for ( i = 0;i < n;i++ ) {
		while ( u( i ) > cumDist( j ) ) j++;
		N_babies( j )++;
	}

// 	// <<<<<<<<<<<
// 	cout << N_babies <<endl;

	// We have assigned new babies for each Particle
	// Now, we fill the resulting index such that:
	// * particles with at least one baby should not move *
	// This assures that reassignment can be done inplace;

	// find the first parent;
	parent=0; while(N_babies(parent)==0) parent++;
	// Build index
	for ( i = 0;i < n;i++ ) {
		if ( N_babies( i ) > 0 ){
			ind( i ) = i;
			N_babies( i ) --; //this index was now replicated;
			}
		else {
			// test if the parent has been fully replicated 
			// if yes, find the next one
			while((N_babies(parent)==0) || (N_babies(parent)==1 && parent>i) ) parent++;
			// Replicate parent
			ind (i) = parent;
			N_babies( parent ) --; //this index was now replicated;
		}
/*			// <<<<<<<<<<<
			cout <<ind << endl;*/
	}
	return ind;
}

TrivialPF::TrivialPF( mpdf &par0, mpdf &obs0, mpdf &prop0, int n0 ) : PF(1/n*ones(n)) {
	is_proposal = true;
	prop = &prop0;
	par = &par0;
	obs = &obs0;
}
TrivialPF::TrivialPF( mpdf &par0, mpdf &obs0,  int n0 ) : PF(1/n*ones(n)) {
	is_proposal = false;
	par = &par0;
	obs = &obs0;
}

void TrivialPF::bayes( const vec &dt , bool evalll ) {
	int i;
	vec oldp;
	double ll, gl, sum = 0.0;
	Sort<double> S;
	ivec ind, iw;
	/*
	//generate new samples
	for ( i=0;i<n;i++ ) {
		prop->evalcond( ptcls( i ), &prop_cond );
		ptcls( i ) = prop_cond.sample();
		gl = prop_cond.eval( ptcls( i ) );

		obs.evalcond( ptcls( i ), &obs_cond );
		ll = obs_cond.eval( dt );
		w( i ) *= ll/gl;
	}
	//renormalize
	for ( i=0;i<n;i++ ){sum+=w( i );};
	w( i ) /=sum; //?
	//
	ind = resample();
	iw = S.sort_index( 0,n-1,w ); // the first one in iw is the strongest

	for ( i=0;i<n;i++ ) {
		ptcls( i ) = ptcls( i ); //potentionally dangerous!
	}
	*/
}