// LMS algorithm adaptive filtering for the Caltech 40m -- Sept. 2011 by M. Prijatelj

#define nDOF 8
#define nWIT 21
#define nTAPS 1000

void ADAPT_XFCODE(double *in, int inSize, double *out, int outSize) {

//First the DOFs and their switches
//int nDOF=8;
//int nTAPS=2000;
//int nWIT=21;
static double DOFctrl[nDOF]; 			//EPICS controls to switch the channels on/off
static double DOF[nDOF][nTAPS]; //Actual DOF error signals

//Then the seismometers
static double WIT[nWIT][nTAPS];

//Then the parameters for the adaptive filtering
//double nTAPS=in[nDOF+nWIT+1];
double mu;
double decay;
double reset;

static double FILTER[nDOF][nTAPS]; 	//Filter coefficients
static double AUXFILT[nDOF][nWIT][nTAPS];	//Aux. variable to hold 3D filter coeff matrix before it gets flattened into 2D [i][j] array
static double OLDAUXFILT[nDOF][nWIT][nTAPS];	//Aux. variable to hold 3D filter coeff matrix before it gets flattened into 2D [i][j] array
static double OUTPUT[nDOF][nTAPS];
double OUTPUTAUX[nDOF][nWIT][nTAPS];
static double timer;

/*
mu=in[nDOF+nWIT-1+1];
decay=in[nDOF+nWIT-1+2];
reset=in[nDOF+nWIT-1+3];

int i=0;
int j=0;
int k=0;
int m=0;
int n=0;
if(reset != 0) timer=0;

if(timer < 10)						//Initialize the filter to 1, output to 0
{
	for(i=0;i<nDOF;i++)
	{
		for(k=0;k<nWIT;k++)
		{
		for(j=0;j<nTAPS;j++)
			{
			DOF[i][j]=0;
			WIT[k][j]=0;
			OUTPUTAUX[i][k][j]=1;
			AUXFILT[i][k][j]=1;
			OLDAUXFILT[i][k][j]=1;
			OUTPUT[i][j]=0;
			}
		}
	}
}
/*
for(i=nDOF;i<(nDOF+nWIT);i++)		//Prepare (all) witness data into one array
{
	for(j=0;j<nTAPS;j++)
	{
		WIT[i-nDOF][j+1]=WIT[i-nDOF][j];	//Make room for newest element
	}
	WIT[i-nDOF][0]=in[i];			//Copy the newest seis. data into the witness array
}

for(i=0;i<nDOF;i++)					//Prepare a DOF array
{
	if(DOFctrl[i] == 0)
	{
		out[i]=0;
		continue;
	}
	for(k=0;k<nTAPS;k++)
	{
		DOF[i][k+1]=DOF[i][k];			//Make room for newest element
	}
	DOF[i][0]=in[nDOF+i];				//Copy the error signal data into the error signal array (Ignore the DOFctrl channels in the beginning)
}

for(i=0;i<nDOF;i++)					//Calculate the full output w, using the old filter coeff. [DOF][WIT][TAPS]
{
	if(DOFctrl[i] == 0)	continue; //out[i]=0 because of higher up anyway.
	for(k=0;k<nWIT;k++)
	{
		for(j=0;j<nTAPS;j++)
		{
		OUTPUTAUX[i][k][j] = AUXFILT[i][k][j] * WIT[k][j];  //Calculate all kind of cross couplings
		}
	}
}

for(i=0;i<nDOF;i++)					//Flatten the output to 2D array along the DOF axis [DOF][TAPS]
{
	if(DOFctrl[i] == 0) continue;
	for(k=0;k<nWIT;k++)
	{
		for(j=0;j<nTAPS;j++)
		{
		//FILTER[i][j]=Filter[i][j] + AUXFILT[i][k][j];
		OUTPUT[i][j]= 0 + OUTPUTAUX[i][k][j];							//Sum over the witness channels, flattens to 2D Array
		}
	}
}


for(i=0;i<nDOF;i++)					//There should be a simpler way
{
	if(DOFctrl[i] == 0)	continue; 
	for(k=0;k<nWIT;k++)
	{
		for(j=0;j<nTAPS;j++)
		{
		OLDAUXFILT[i][k][j] = AUXFILT[i][k][j];
		}
	}
}


for(i=0;i<nDOF;i++)					//Calculate the new filter coefficients
{
	if(DOFctrl[i] == 0) continue; //out[i]=0 because of higher up anyway.
	for(k=0;k<nWIT;k++)
	{
		for(j=0;j<nTAPS;j++)
		{
		AUXFILT[i][k][j]=OLDAUXFILT[i][j][k] + mu * WIT[k][j] * DOF[i][j];  //The new full filter coefficients
		}
	}
}

for(i=1;i<nDOF;i++)					//Put out the filter output calculated with the old filter coeff.
{
	if(DOFctrl[i] == 0) continue;
	out[i]=OUTPUT[i][0];
}
*/
if( timer < 2000) timer++;

}