#include "mpf_double.h"
#include "math.h"
#include <stdio.h>

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

static double qth;
static double qom;
static double r;

#define MAXrand 100
#define MAXrandn 100
static const double RArandu[MAXrand]={0.8147, 0.9058, 0.1270, 0.9134, 0.6324, 0.0975, 0.2785, 0.5469, 0.9575, 0.9649,
                                      0.1576, 0.9706, 0.9572, 0.4854, 0.8003, 0.1419, 0.4218, 0.9157, 0.7922, 0.9595, 0.6557, 0.0357,
                                      0.8491, 0.9340, 0.6787, 0.7577, 0.7431, 0.3922, 0.6555, 0.1712, 0.7060, 0.0318, 0.2769, 0.0462,
                                      0.0971, 0.8235, 0.6948, 0.3171, 0.9502, 0.0344, 0.4387, 0.3816, 0.7655, 0.7952, 0.1869, 0.4898,
                                      0.4456, 0.6463, 0.7094, 0.7547, 0.2760, 0.6797, 0.6551, 0.1626, 0.1190, 0.4984, 0.9597, 0.3404,
                                      0.5853, 0.2238, 0.7513, 0.2551, 0.5060, 0.6991, 0.8909, 0.9593, 0.5472, 0.1386, 0.1493, 0.2575,
                                      0.8407, 0.2543, 0.8143, 0.2435, 0.9293, 0.3500, 0.1966, 0.2511, 0.6160, 0.4733, 0.3517, 0.8308,
                                      0.5853, 0.5497, 0.9172, 0.2858, 0.7572, 0.7537, 0.3804, 0.5678, 0.0759, 0.0540, 0.5308, 0.7792,
                                      0.9340, 0.1299, 0.5688, 0.4694, 0.0119, 0.3371
                                     };
static const double *randu_i=&RArandu[0];
const static double *randu_last=&RArandu[MAXrand-1];

static const double RArandn[MAXrandn]={
	0.6353, -0.6313, -1.0181,  0.4801,  0.4855,  0.9901, -0.5412, -1.0290,  1.0641, -0.0132,
	-0.6014, -2.3252, -0.1821,  0.6682, -0.0050,  0.2189, -1.3335,  0.2431, -0.2454, -0.5803,
	0.5512, -1.2316,  1.5210, -0.0783, -0.2762,  0.2617,  1.0727, -1.2566, -1.5175,  2.1363,
	-1.0998,  1.0556, -0.0384,  0.8892,  1.2765,  1.2134, -0.7121, -0.3472,  0.0097, -0.2576,
	0.0860, -0.1132,  1.2274,  2.3093,  1.8634, -0.2747, -0.0113, -0.9414,  0.0714, -1.4095,
	-2.0046,  0.3792, -0.6962,  0.5246, -0.5226, -0.1331, -0.0008, -1.1746,  0.3165,  1.7701,
	-0.4931,  0.9442,  0.0075, -0.0118,  0.1034, -1.2705, -0.2494, -1.0211,  0.4998,  0.3255,
	0.4620, -2.1204, -0.7829,  0.9131, -0.8076, -1.6636,  0.3966, -0.4017,  1.2781, -1.1190,
	-0.3210, -0.6447,  0.5869,  0.0559,  0.6804, -0.7036, -0.2640,  0.1737, -0.5478,  0.6204,
	1.2366, -0.7043, -0.2512, -1.1071, -2.3646,  0.2809, -1.6640, -0.1161,  0.2608,  1.2698
};
static const double *randn_i=&RArandn[0];
const static double *randn_last=&RArandn[MAXrandn-1];

double randu() {
	if ( randu_i==randu_last )
		randu_i =&RArandu[0];
	else
		randu_i++;

	return *randu_i;
}

double randn() {
	if ( randn_i==randn_last )
		randn_i =&RArandn[0];
	else
		randn_i++;

	return *randn_i;
}

void resample() {
	int N_babies[N];
	double cumdist;
	int i;
	double ui;

	double u0 = randu();

	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
	int i_from=0;
	int i_to=0;
	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 double &isa, const double &isb , const double &usa, const double &usb ) {

	double isd;
	double isq;
	double usd;
	double usq;

	double Cd;
	double Cq;
	double CC;
	double oCC;

	double difid;
	double difiq;

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

	double maxlwi, sumlwi;
	int i;
// implementation starts here
	for ( i=0; i<N; i++ ) {
		Pt[i] = 1.0*1.0*Pt[i]+qom; // Pt is now predictive variance
		th[i] =th[i] + om[i]*_dt+sqrt ( qth+_dt*_dt*Pt[i] ) *  randn() ;
		while ( th[i]>M_PI ) th[i]=th[i]-2*M_PI;
		while ( th[i]<-M_PI ) th[i]=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;
		usd = cth[i]*usa+sth[i]*usb;
		usq = -sth[i]*usa+cth[i]*usb;

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

		difid=isd- ( 1.0- ( _ad ) ) *isdm[i] - _cd *usd;
		difiq=isq- ( 1.0- ( _aq ) ) *isqm[i] - _cq *usq;

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

		Kd = Pt[i]*Cd*ro;
		Kq = Pt[i]*Cq*ro;

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

		ypd = Cd*om[i];
		ypq = Cq*om[i];

		detRy = ro/r;
		om[i] = 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 ) ;
		
		isdm[i]=isd;
		isqm[i]=isq;
	}
	maxlwi=-1e10;
	for ( i=0;i<N;i++ ) {
		if ( lwi[i]>maxlwi ) maxlwi=lwi[i];
	}
	for ( i=0;i<N;i++ ) lwi[i]-=maxlwi;
	for ( i=0;i<N;i++ ) w[i]=exp ( lwi[i] ); // always resample -> wmin=1/n;

	sumlwi=0.0;
	for ( i=0;i<N;i++ ) sumlwi+=w[i];
	for ( i=0;i<N;i++ ) w[i]/=sumlwi;
	resample();
}


void mpf_init(double qom0, double qth0, double r0) {
	r=r0;
	qth=qth0;
	qom=qom0;
	int i;
	for ( i=0; i<N; i++ ) {
		th[i]=randu() *2*M_PI - M_PI;
		om[i]=0;
		Pt[i]=1;
		isdm[i]=0;
		isqm[i]=0;
		w[i]=1.0/N;
	}

}
void mpf_mean ( double Ecosth, double Esinth, double Eome ) {
	int i;
	Ecosth=0.0;
	Esinth=0.0;
	Eome=0.0;
	for ( i=0;i<N;i++ ) {
		Ecosth+=w[i]*cth[i];
		Esinth+=w[i]*sth[i];
		Eome+=w[i]*om[i];
	}

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