#include "math.h"
#include <stdio.h>
#include "fast_exp.h"
#include "fastexp.h"
#include "mpf_double.h"
#include "RArandn.h"

static floatx om[N];
static floatx Pt[N];
static floatx sth[N];
static floatx cth[N];
static floatx isdm[N];
static floatx isqm[N];
static floatx th[N];
static floatx w[N];
static floatx lwi[N];

static floatx qth;
static floatx qom;
static floatx r;
static floatx rinv;


floatx qrandu() {
	if ( randu_i==randu_last )
		randu_i =&RArandu[0];
	else
		randu_i++;

	return *randu_i;
}

floatx qrandn() {
	if ( randn_i==randn_last )
		randn_i =&RArandn[0];
	else
		randn_i++;

	return *randn_i;
}

void resample() {
	int N_babies[N];
	floatx cumdist;
	int i;
	floatx ui;
	int i_from=0;
	int i_to=0;

	floatx u0 = qrandu();

	int j=0;
	N_babies[0]=0;
	cumdist = w[0];
	for ( i = 0; i < N; i++ ) {
		ui  = ( i + u0 ) / N;
		while ( ui > cumdist ) {
			j++;
			N_babies[j]=0;
			cumdist+=w[j];
		}
		N_babies [j] ++;
	}
	while (j++<=(N-1)) { // delete all N_babies after j
		N_babies[j]=0; 
	}

	// COPY

	while ( i_from<N ) {
		while ( N_babies[i_from]>1 ) { // 1 baby stays where it is
			
			while ( N_babies[i_to]>=1 ) i_to++; // find first empty slot

			//copy it
			Pt[i_to]=Pt[i_from];
			om[i_to]=om[i_from];
			th[i_to]=th[i_from];
			isdm[i_to]=isdm[i_from];
			isqm[i_to]=isqm[i_from];

			N_babies[i_to]++;
			N_babies[i_from]--;
		}
		i_from++;
	}
}

void mpf_bayes ( const floatx isa, const floatx isb , const floatx usa, const floatx usb ) {

	floatx isd;
	floatx isq;
	floatx usd;
	floatx usq;

	floatx Cd;
	floatx Cq;
	floatx CC;
	floatx oCC;

	floatx difid;
	floatx difiq;

	floatx zeta;
	floatx Kd;
	floatx Kq;
	floatx ro;
	floatx ypd;
	floatx ypq;
	floatx detRy;
	floatx ydiffd;
	floatx ydiffq;
	floatx ydC;

	floatx maxlwi, sumlwi;
	int i;
	
	floatx *th_i, *Pt_i, *om_i, *isdm_i, *isqm_i;
	floatx *w_i, *lw_i;
	floatx *sth_i, *cth_i;
// implementation starts here
	th_i = &th[0];
	Pt_i = &Pt[0];
	om_i = &om[0];
	isdm_i = &isdm[0];
	isqm_i = &isqm[0];
	sth_i = &sth[0];
	cth_i = &cth[0];
	for ( i=0; i<N; i++ ) {

		// time update
		*Pt_i = 1.0*1.0*(*Pt_i)+qom; // Pt is now predictive variance
		*th_i += *om_i*_dt+qth *  qrandn() ;
		while ( *th_i>M_PI ) *th_i -=2*M_PI;
		while ( *th_i<-M_PI ) *th_i+=2*M_PI;

		*sth_i=sin ( *th_i );
		*cth_i=cos ( *th_i );
		
		isd = *cth_i*isa+*sth_i*isb;
		isq = -*sth_i*isa+*cth_i*isb;
		// process old voltage
		usd = (*cth_i)*usa+(*sth_i)*usb;
		usq = -(*sth_i)*usa+(*cth_i)*usb;

		Cd = isq*_dt;
		Cq = -_b - isd* _dt;

		difid=isd- _ad  * (*isdm_i) - _cd *(usd);
		difiq=isq- _aq  * (*isqm_i) - _cq *(usq);

		CC=Cd*Cd+Cq*Cq;
		zeta = *Pt_i/ ( r+*Pt_i*CC );
		oCC = ( 1-zeta*CC );
		ro = oCC*rinv;

		Kd = *Pt_i*Cd*ro;
		Kq = *Pt_i*Cq*ro;

		(*Pt_i)*=( 1.0- ( Kd*Cd+Kq*Cq ) );

		ypd = Cd**om_i;
		ypq = Cq**om_i;

		//detRy = ro/r;
		*om_i +=  Kd* ( difid - ypd ) +Kq* ( difiq-ypq );
		ydiffd = ( ypd-difid );
		ydiffq = ( ypq-difiq );
		ydC = ydiffd*Cd + ydiffq*Cq;

		//lwi[i] = 0.5* ( log ( detRy ) + ( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) /r ) ;
		lwi[i] = 0.5*( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) *rinv ;
		
		*isdm_i=isd;
		*isqm_i=isq;
		
		// shift all counters
		th_i++;
		Pt_i++;
		om_i++;
		isdm_i++;
		isqm_i++;
		sth_i++;
		cth_i++;

	}
/*	maxlwi=-1e10;
	lw_i = &lwi[0];
	for ( i=0;i<N;i++,lw_i++ ) {
		if ( *lw_i>maxlwi ) maxlwi=*lw_i;
	}*/
	lw_i = &lwi[0];
	w_i = &w[0];
	for ( i=0;i<N;i++,lw_i++,w_i++ ) {
//		*lw_i-=maxlwi;
		(*w_i)=fasterexp ( *lw_i ); // always resample -> wmin=1/n;
	}

	sumlwi=0.0;
	w_i = &w[0];
	for ( i=0;i<N;i++,w_i++ ) sumlwi+=*w_i;
	w_i = &w[0];
	for ( i=0;i<N;i++,w_i++ ) *w_i/=sumlwi;
	
	// NEFF
	floatx sw2=0.0;
	for ( i=0;i<N;i++ ) sw2+=w[i]*w[i];
	printf("%f\n",1./sw2*N);
	
	resample();
}


void mpf_init(floatx qom0, floatx qth0, floatx r0) {
	int i;
	r=r0;
	rinv = 1.0/r;
	qth=qth0;
	qom=qom0;

	for ( i=0; i<N; i++ ) {
		th[i]=qrandu() *2*M_PI - M_PI;
		sth[i]=sin ( th[i] );
		cth[i]=cos ( th[i] );
		
		om[i]=0;
		Pt[i]=10.;
		isdm[i]=0;
		isqm[i]=0;
		w[i]=1.0/N;
	}

}
void mpf_mean ( floatx *Ecosth, floatx *Esinth, floatx *Eome ) {
	int i;
	floatx *w_i=&w[0];
	floatx *cth_i=&cth[0];
	floatx *sth_i=&sth[0];
	floatx *om_i=&om[0];
	*Ecosth=0.0;
	*Esinth=0.0;
	*Eome=0.0;
	for ( i=0;i<N;i++,w_i++,cth_i++,sth_i++,om_i++ ) {
		*Ecosth+=(*w_i)*(*cth_i);
		*Esinth+=(*w_i)*(*sth_i);
		*Eome+=(*w_i)*(*om_i);
	}

}
void mpf_th(floatx th1[N]){
	int i;
	for (i=0;i<N;i++) th1[i]=th[i];
}

floatx kalman_om( const floatx isa, const floatx isb , const floatx usa, const floatx usb, const floatx th ) {

	floatx isd;
	floatx isq;
	floatx usd;
	floatx usq;

	floatx Cd;
	floatx Cq;
	floatx CC;
	floatx oCC;

	floatx difid;
	floatx difiq;

	floatx zeta;
	floatx Kd;
	floatx Kq;
	floatx ro;
	floatx ypd;
	floatx ypq;
	floatx detRy;
	floatx ydiffd;
	floatx ydiffq;
	floatx ydC;

	static floatx Pt;
	static floatx om;
	floatx cth, sth;
	static floatx isdm, isqm;

	
		Pt = 1.0*1.0*Pt+qom; // Pt is now predictive variance
		//while ( th>M_PI ) th=th-2*M_PI;
		//while ( th<-M_PI ) th=th+2*M_PI;

		sth=sin ( th );
		cth=cos ( th );

		isd = cth*isa+sth*isb;
		isq = -sth*isa+cth*isb;
		usd = cth*usa+sth*usb;
		usq = -sth*usa+cth*usb;

		Cd = isq*_dt;
		Cq = -_b - isd* _dt;

		difid=isd- _ad  *isdm - _cd *usd;
		difiq=isq- _aq  *isqm - _cq *usq;

		CC=Cd*Cd+Cq*Cq;
		zeta = Pt/ ( r+Pt*CC );
		oCC = ( 1-zeta*CC );
		ro = oCC*rinv;

		Kd = Pt*Cd*ro;
		Kq = Pt*Cq*ro;

		Pt=Pt* ( 1- ( Kd*Cd+Kq*Cq ) );

		ypd = Cd*om;
		ypq = Cq*om;

		detRy = ro*rinv;
		om = om + Kd* ( difid - ypd ) +Kq* ( difiq-ypq );
		ydiffd = ( ypd-difid );
		ydiffq = ( ypq-difiq );
		ydC = ydiffd*Cd + ydiffq*Cq;

		isdm=isd;
		isqm=isq;
		
		return om;
}
