#include "tuneState.h"
#include <string.h>

#define PI 3.14159265358979323846
#define OSCILLATOR_BUFFER_SIZE 20000

double minFreq = 1;
double maxFreq = (kFreq/2)-1;
double amp =  1.;
double dT  =  1./kFreq;
double in[OSCILLATOR_BUFFER_SIZE];
double out[OSCILLATOR_BUFFER_SIZE];

//	----------------------------------------------------------------------------
//	tuneState
//	----------------------------------------------------------------------------

tuneState::tuneState(char *namePar)
{
	if(namePar){
		strncpy(name,namePar,sizeof(name));
		nameLen = strlen(name);
	} else {
		strncpy(name,"",sizeof(name));
		nameLen = 0;
	}
//	init();
}

//	----------------------------------------------------------------------------
//	~tuneState
//	----------------------------------------------------------------------------

tuneState::~tuneState(void)
{
}

//	----------------------------------------------------------------------------
//	init
//	----------------------------------------------------------------------------

void tuneState::init(void)
{
	memset(pidP,0,sizeof(pidP));
	memset(geoP,0,sizeof(geoP));
	memset(posP,0,sizeof(posP));

	memset(nbode,0,sizeof(nbode));
	memset(gain,0,sizeof(gain));
	memset(phase,0,sizeof(phase));
	memset(freqs,0,sizeof(freqs));

	memset(numOpen,0,sizeof(numOpen));
	memset(gainOpen,0,sizeof(gainOpen));
	memset(phaseOpen,0,sizeof(phaseOpen));
	memset(freqsOpen,0,sizeof(freqsOpen));

	memset(numstep,0,sizeof(numstep));
	memset(stepsamples,0,sizeof(stepsamples));
	memset(numffts,0,sizeof(numffts));
	memset(ffts,0,sizeof(ffts));

	memset(nShake,0,sizeof(nShake));
	memset(gainShake,0,sizeof(gainShake));
	memset(phaseShake,0,sizeof(phaseShake));
	memset(freqsShake,0,sizeof(freqsShake));
	memset(jitterShake,0,sizeof(jitterShake));
}

//	----------------------------------------------------------------------------
//	copy
//	----------------------------------------------------------------------------

void tuneState::copy(tuneState in)
{
	memcpy(pidP,			&in.pidP,			sizeof(pidP)			);
	memcpy(geoP,			&in.geoP,			sizeof(geoP)			);
	memcpy(posP,			&in.posP,			sizeof(posP)			);
	memcpy(outerP,			&in.outerP,			sizeof(outerP)			);
	memcpy(lensP,			&in.lensP,			sizeof(lensP)			);

	filt.copy(in.filt);

	memcpy(gain,			&in.gain,			sizeof(gain)			);
	memcpy(phase,			&in.phase,			sizeof(phase)			);
	memcpy(freqs,			&in.freqs,			sizeof(freqs)			);
	memcpy(nbode,			&in.nbode,			sizeof(nbode)			);

	memcpy(filtergains,		&in.filtergains,	sizeof(filtergains)		);
	memcpy(filterphase,		&in.filterphase,	sizeof(filterphase)		);
	memcpy(filterfreqs,		&in.filterfreqs,	sizeof(filterfreqs)		);

	memcpy(gainOpen,		&in.gainOpen,		sizeof(gainOpen)		);
	memcpy(phaseOpen,		&in.phaseOpen,		sizeof(phaseOpen)		);
	memcpy(freqsOpen,		&in.freqsOpen,		sizeof(freqsOpen)		);
	memcpy(numOpen,			&in.numOpen,		sizeof(numOpen)			);

	memcpy(gainShake,		&in.gainShake,		sizeof(gainShake)		);
	memcpy(phaseShake,		&in.phaseShake,		sizeof(phaseShake)		);
	memcpy(freqsShake,		&in.freqsShake,		sizeof(freqsShake)		);
	memcpy(jitterShake,		&in.jitterShake,	sizeof(jitterShake)		);
	memcpy(nShake,			&in.nShake,			sizeof(nShake)			);

	memcpy(stepsamples,		&in.stepsamples,	sizeof(stepsamples)		);
	memcpy(numstep,			&in.numstep,		sizeof(numstep)			);

	memcpy(numffts,			&in.numffts,		sizeof(numffts)			);
	memcpy(ffts,			&in.ffts,			sizeof(ffts)			);
}

//	----------------------------------------------------------------------------
//	filterBode
//	----------------------------------------------------------------------------

void tuneState::filterBode(int axis, int filterOut){

	int i;
	double outputReal;
	double outputImag;
	double inputReal;
	double inputImag;
	double input, output, sine, cosine;

	double lfmin = log(minFreq);
	double lfmax = log(maxFreq);
	double kfreq = (lfmax-lfmin)/(FILTBODEMAX-1);

	for (int ifreq=0; ifreq < FILTBODEMAX; ifreq++) {
		double logfreq = lfmin + ifreq*kfreq;
		double freq = exp(logfreq);
		int buffersize;

		buffersize = (int)(OSCILLATOR_BUFFER_SIZE/freq);
		if(buffersize<500)buffersize=500;
		if(buffersize>OSCILLATOR_BUFFER_SIZE)buffersize=OSCILLATOR_BUFFER_SIZE;

		for (i = 0; i < buffersize; i++) {
			in[i] = amp * sin(freq * i * 2.0 * PI * dT);
			if(filterOut==NiFILT){
				out[i] = filt.runfilters(in[i],axis);
			}else{
				out[i] = filt.runfilter(in[i],axis,filterOut);
			}
		 }

		inputReal = 0;
		inputImag = 0;
		outputReal = 0;
		outputImag = 0;

		for (i = 0; i < buffersize; i++) {

			sine = sin(freq * i * 2.0 * PI * dT);
			cosine = cos(freq * i * 2.0 * PI * dT);

			input = in[i];
			output = out[i];

			inputReal += sine * input;
			inputImag -= cosine * input;
			outputReal += sine * output;
			outputImag -= cosine * output;
		}

		filtergains[axis][filterOut][ifreq] = (float)sqrt((outputReal * outputReal + outputImag * outputImag) / (inputReal * inputReal + inputImag * inputImag));
		filterphase[axis][filterOut][ifreq] = (float)((180./PI)*(atan2(inputImag, inputReal) - atan2(outputImag, outputReal)));
		filterfreqs[axis][filterOut][ifreq] = (float)freq;
	}
}

#if 0

//	----------------------------------------------------------------------------
//	copyAxis
//	----------------------------------------------------------------------------

void tuneState::copyAxis(tuneState in, int axis)
{
	pidParam[axis]					=in.pidParam[axis];

	numfilt[axis]					=in.numfilt[axis];
	numfiltpar[axis]				=in.numfiltpar[axis];
	memcpy(&hsFilterState[axis],	&in.hsFilterState[axis],	sizeof(hsFilterState[axis])	);
	memcpy(&hsFilter[axis],			&in.hsFilter[axis],			sizeof(hsFilter[axis])		);

	nbode[axis]						=in.nbode[axis];
	memcpy(&gain[axis],				&in.gain[axis],				sizeof(gain[axis])			);
	memcpy(&phase[axis],			&in.phase[axis],			sizeof(phase[axis])			);
	memcpy(&freqs[axis],			&in.freqs[axis],			sizeof(freqs[axis])			);

	numOpen[axis]					=in.numOpen[axis];
	memcpy(&gainOpen[axis],			&in.gainOpen[axis],			sizeof(gainOpen[axis])		);
	memcpy(&phaseOpen[axis],		&in.phaseOpen[axis],		sizeof(phaseOpen[axis])		);
	memcpy(&freqsOpen[axis],		&in.freqsOpen[axis],		sizeof(freqsOpen[axis])		);

	numstep[axis]					=in.numstep[axis];
	memcpy(&stepsamples[axis],		&in.stepsamples[axis],		sizeof(stepsamples[axis])	);

	numffts[axis]					=in.numffts[axis];
	memcpy(&ffts[axis],				&in.ffts[axis],				sizeof(ffts[axis])	);

	nShake[axis]					=in.nShake[axis];
	memcpy(gainShake[axis],			&in.gainShake[axis],		sizeof(gainShake[axis])		);
	memcpy(phaseShake[axis],		&in.phaseShake[axis],		sizeof(phaseShake[axis])	);
	memcpy(freqsShake[axis],		&in.freqsShake[axis],		sizeof(freqsShake[axis])	);
	memcpy(jitterShake[axis],		&in.jitterShake[axis],		sizeof(jitterShake[axis])	);
}

//	----------------------------------------------------------------------------
//	saveState
//	----------------------------------------------------------------------------

void tuneState::saveState(FILE *sfile){

	char *n = name;

	for(int axis=0;axis<3;axis++){
		char *e = "pidParam";
		char *ff = "%s.%s[%i].%s %f\n";
		fprintf(sfile,ff,n,e,axis,"pGain"					,S2F*pidParam[axis].pGain			);
		fprintf(sfile,ff,n,e,axis,"iGain"					,S2F*pidParam[axis].iGain			);
		fprintf(sfile,ff,n,e,axis,"dGain"					,S2F*pidParam[axis].dGain			);
		fprintf(sfile,ff,n,e,axis,"errorLimit"				,S2F*pidParam[axis].errorLimit		);
	}

	for(int axis=0;axis<3;axis++){
		fprintf(sfile,"%s.%s[%i] %i\n",n,"numfiltPar",axis	,numfiltpar[axis]						);
		char *e = "hsFilter";
		char *ff = "%s.%s[%i][%i].%s %f\n";
		char *fi = "%s.%s[%i][%i].%s %i\n";
		for(int filter=0;filter<FILTMAX;filter++){
			fprintf(sfile,ff,n,e,axis,filter,"hsFrequency"	,S2F*hsFilter[axis][filter].hsFrequency	);
			fprintf(sfile,ff,n,e,axis,filter,"hsQValue"		,S2F*hsFilter[axis][filter].hsQValue	);
			fprintf(sfile,ff,n,e,axis,filter,"hsAValue"		,S2F*hsFilter[axis][filter].hsAValue	);
			fprintf(sfile,fi,n,e,axis,filter,"hsType"		,	 hsFilter[axis][filter].hsType		);
			fprintf(sfile,ff,n,e,axis,filter,"limit"		,S2F*hsFilter[axis][filter].limit		);
#if defined(BAE)
			fprintf(sfile,ff,n,e,axis,filter,"x0Gain"		,S2F*hsFilter[axis][filter].x0Gain		);
			fprintf(sfile,ff,n,e,axis,filter,"xMinus1Gain"	,S2F*hsFilter[axis][filter].xMinus1Gain	);
			fprintf(sfile,ff,n,e,axis,filter,"xMinus2Gain"	,S2F*hsFilter[axis][filter].xMinus2Gain	);
			fprintf(sfile,ff,n,e,axis,filter,"yMinus1Gain"	,S2F*hsFilter[axis][filter].yMinus1Gain	);
			fprintf(sfile,ff,n,e,axis,filter,"yMinus2Gain"	,S2F*hsFilter[axis][filter].yMinus2Gain	);
#endif
		}
	}

	for(int axis=0;axis<3;axis++){
		fprintf(sfile,"%s.%s[%i] %i\n",n,"numfilt",axis		,numfilt[axis]									);
		char *e = "hsFilterState";
		char *ff = "%s.%s[%i][%i].%s %f\n";
		for(int filter=0;filter<FILTMAX;filter++){
			fprintf(sfile,ff,n,e,axis,filter,"x0Gain"		,S2F*hsFilterState[axis][filter].x0Gain			);
			fprintf(sfile,ff,n,e,axis,filter,"xMinus1Gain"	,S2F*hsFilterState[axis][filter].xMinus1Gain	);
			fprintf(sfile,ff,n,e,axis,filter,"xMinus2Gain"	,S2F*hsFilterState[axis][filter].xMinus2Gain	);
			fprintf(sfile,ff,n,e,axis,filter,"yMinus1Gain"	,S2F*hsFilterState[axis][filter].yMinus1Gain	);
			fprintf(sfile,ff,n,e,axis,filter,"yMinus2Gain"	,S2F*hsFilterState[axis][filter].yMinus2Gain	);
		}
	}

	for(int axis=0;axis<3;axis++){
		char *e = "numstep";
		char *fi = "%s.%s[%i] %i\n";
		fprintf(sfile,fi,n,e,axis							,numstep[axis]			);
	}

	for(int axis=0;axis<3;axis++){
		for(int ii=0;ii<numstep[axis];ii++){
			char *e = "stepsamples";
			char *ff = "%s.%s[%i][%i] %f\n";
			fprintf(sfile,ff,n,e,axis,ii					,stepsamples[axis][ii]	);
		}
	}

	for(int axis=0;axis<3;axis++){
		char *e = "numffts";
		char *fi = "%s.%s[%i] %i\n";
		fprintf(sfile,fi,n,e,axis							,numffts[axis]			);
	}

	for(int axis=0;axis<3;axis++){
		for(int ii=0;ii<numffts[axis];ii++){
			char *e = "ffts";
			char *ff = "%s.%s[%i][%i] %f\n";
			fprintf(sfile,ff,n,e,axis,ii					,ffts[axis][ii]	);
		}
	}

	for(int axis=0;axis<3;axis++){
		fprintf(sfile,"%s.%s[%i] %i\n",n,"nbode",axis		,nbode[axis]			);
		char *ff = "%s.%s[%i][%i] %f\n";
		for(int ii=0;ii<nbode[axis];ii++){
			fprintf(sfile,ff,n,"gain",axis,ii				,gain[axis][ii]			);
			fprintf(sfile,ff,n,"phase",axis,ii				,phase[axis][ii]		);
			fprintf(sfile,ff,n,"freqs",axis,ii				,freqs[axis][ii]		);
		}
	}

	for(int axis=0;axis<3;axis++){
		fprintf(sfile,"%s.%s[%i] %i\n",n,"numOpen",axis		,numOpen[axis]			);
		char *ff = "%s.%s[%i][%i] %f\n";
		for(int ii=0;ii<numOpen[axis];ii++){
			fprintf(sfile,ff,n,"gainOpen",axis,ii			,gainOpen[axis][ii]		);
			fprintf(sfile,ff,n,"phaseOpen",axis,ii			,phaseOpen[axis][ii]	);
			fprintf(sfile,ff,n,"freqsOpen",axis,ii			,freqsOpen[axis][ii]	);
		}
	}

	for(int axis=0;axis<3;axis++){
		fprintf(sfile,"%s.%s[%i] %i\n",n,"nShake",axis		,nShake[axis]			);
		char *ff = "%s.%s[%i][%i] %f\n";
		for(int ii=0;ii<nShake[axis];ii++){
			fprintf(sfile,ff,n,"gainShake",axis,ii			,gainShake[axis][ii]	);
			fprintf(sfile,ff,n,"phaseShake",axis,ii			,phaseShake[axis][ii]	);
			fprintf(sfile,ff,n,"freqsShake",axis,ii			,freqsShake[axis][ii]	);
			fprintf(sfile,ff,n,"jitterShake",axis,ii		,jitterShake[axis][ii]	);
		}
	}
}

//	-------------------------------------------------------------------
//	match1  name.arrayName[index1][index0].elementName
//	-------------------------------------------------------------------

int tuneState::match1(char *s, char *arrayName, int *index1,int max1, int *index0, int max0, char *elementName){

	int res = 0;
	int i0;
	int i1;
	char fs[240];
	char ss[240];

	sprintf_s(fs,sizeof(fs),"%s.%s[%%i][%%i].%s",name,arrayName,elementName);

	if(2==sscanf_s(s,fs,&i1,&i0)){
		sprintf_s(ss,sizeof(ss),fs,i1,i0);
		if(0==strcmp(s,ss)){
			*index0=i0<max0?i0:max0-1;
			*index1=i1<max1?i1:max1-1;
			res = 1;
		}
	}
	return res;
}
//	-------------------------------------------------------------------
//	match2  name.arrayName[index0]
//	-------------------------------------------------------------------

int tuneState::match2(char *s, char *arrayName, int *index0, int max0){

	int res = 0;
	int i0;
	char fs[240];
	char ss[240];

	sprintf_s(fs,sizeof(fs),"%s.%s[%%i]",name,arrayName);

	if(1==sscanf_s(s,fs,&i0)){
		sprintf_s(ss,sizeof(ss),fs,i0);
		if(0==strcmp(s,ss)){
			*index0=i0<max0?i0:max0-1;
			res = 1;
		}
	}
	return res;
}

//	-------------------------------------------------------------------
//	match3  name.arrayName[index0].elementName
//	-------------------------------------------------------------------

int tuneState::match3(char *s, char *arrayName, int *index0, int max0, char *elementName){

	int res = 0;
	int i0;
	char fs[240];
	char ss[240];

	sprintf_s(fs,sizeof(fs),"%s.%s[%%i].%s",name,arrayName,elementName);

	if(1==sscanf_s(s,fs,&i0)){
		sprintf_s(ss,sizeof(ss),fs,i0);
		if(0==strcmp(s,ss)){
			*index0=i0<max0?i0:max0-1;
			res = 1;
		}
	}
	return res;
}

//	-------------------------------------------------------------------
//	match5  name.arrayName[index1][index0]
//	-------------------------------------------------------------------

int tuneState::match5(char *s, char *arrayName, int *index1,int max1, int *index0, int max0){

	int res = 0;
	int i0;
	int i1;
	char fs[240];
	char ss[240];

	sprintf_s(fs,sizeof(fs),"%s.%s[%%i][%%i]",name,arrayName);

	if(2==sscanf_s(s,fs,&i1,&i0)){
		sprintf_s(ss,sizeof(ss),fs,i1,i0);
		if(0==strcmp(s,ss)){
			*index0=i0<max0?i0:max0-1;
			*index1=i1<max1?i1:max1-1;
			res = 1;
		}
	}
	return res;
}
//	----------------------------------------------------------------------------
//	getReportScaled
//	----------------------------------------------------------------------------

scaled_t tuneState::getReportScaled(FILE *sfile){

	float value = 0;
	char c = 0;

	while(1==fread(&c,1,1,sfile)){
		if(c==':'){
			if(1==fscanf_s(sfile,"%f", &value)){
				return(F2S(value));
			} else {
				break;
			}
		}
	}
	return(0);
}

//	----------------------------------------------------------------------------
//	getReportFilter
//	----------------------------------------------------------------------------

int tuneState::getReportFilter(FILE *sfile){

	char s[240];
	char c = 0;

	while(1==fread(&c,1,1,sfile)){
		if(c==':'){
			if(1==fscanf_s(sfile,"%s", s, sizeof(s))){
					 if(s[0]=='N')return 1;
				else if(s[0]=='L')return 0;
				else return 0;
			}
		}
	}
	return 0;
}

//	----------------------------------------------------------------------------
//	readState
//	----------------------------------------------------------------------------

int tuneState::readReport(FILE *sfile){

	char s[240];

// 30.1.1  pGain: 1.0000
// 30.1.2  iGain: 0.0150
// 30.1.3  dGain: 0.0000
// 30.1.4  Error Limit: 66.0000
// 30.2.1  pGain: 0.9000
// 30.2.2  iGain: 0.0150
// 30.2.3  dGain: 0.0000
// 30.2.4  Error Limit: 66.6667
// 30.3.1  pGain: 1.1000
// 30.3.2  iGain: 0.0150
// 30.3.3  dGain: 0.0000
// 30.3.4  Error Limit: 66.6667

//[1.4]-> da ge 24 
//Yaw High Speed Filters				   Type: FLASH
//24.1.1  Frequency: 32.0000
//24.1.2  Q (notch): 0.0000
//24.1.3  f2/f1 (laglead): 5.0000
//24.1.4  Type: LAGLEAD
//24.1.5  limit: 0.0000
//24.2.1  Frequency: 210.0000
//24.2.2  Q (notch): 10.0000
//24.2.3  f2/f1 (laglead): 0.0000
//24.2.4  Type: NOTCH
//24.2.5  limit: 0.0000
//
//[1.4]-> da ge 26 
//Pitch High Speed Filters				 Type: FLASH
//26.1.1  Frequency: 32.0000
//26.1.2  Q (notch): 0.0000
//26.1.3  f2/f1 (laglead): 5.0000
//26.1.4  Type: LAGLEAD
//26.1.5  limit: 0.0000
//
//[1.4]-> da ge 28 
//Roll High Speed Filters				  Type: FLASH
//28.1.1  Frequency: 32.0000
//28.1.2  Q (notch): 0.0000
//28.1.3  f2/f1 (laglead): 5.0000
//28.1.4  Type: LAGLEAD
//28.1.5  limit: 0.0000


	rewind(sfile);
	fscanf_s(sfile,"%s", &s, sizeof(s));
	if(strcmp(s,"Reading")){
		rewind(sfile);
		return 0;
	}else{
		while(fscanf_s(sfile,"%s", &s, sizeof(s))==1){
			unsigned int it;
			unsigned int i1;
			unsigned int i2;
			unsigned int axis;
			unsigned int filter;
			if( (6==strlen(s)) && (s[2]=='.') && (s[4]=='.') ){
				s[2]=' ';
				s[4]=' ';
				if(3==sscanf_s(s,"%i %i %i",&it,&i1,&i2)){
					axis = max(0,min(2,(i1 - 1)));
					switch(it){
						case 30:
							switch(i2){
								case 1:	pidParam[axis].pGain		 = getReportScaled(sfile); break;
								case 2:	pidParam[axis].iGain		 = getReportScaled(sfile); break;
								case 3:	pidParam[axis].dGain		 = getReportScaled(sfile); break;
								case 4:	pidParam[axis].errorLimit	 = getReportScaled(sfile); break;
							}
							break;
						case 24:
						case 26:
						case 28:
							axis = (it-24)/2;
							filter = max(0,min(FILTMAX-1,i1-1));
							switch(i2){
								case 1:	hsFilter[axis][filter].hsFrequency	 = getReportScaled(sfile); break;
								case 2:	hsFilter[axis][filter].hsQValue		 = getReportScaled(sfile); break;
								case 3:	hsFilter[axis][filter].hsAValue		 = getReportScaled(sfile); break;
								case 4:	hsFilter[axis][filter].hsType		 = getReportFilter(sfile); break;
								case 5:	hsFilter[axis][filter].limit		 = getReportScaled(sfile); break;
							}
							break;
					}
				}
			}
		}
		hsFilterNotify();
		return 1;
	}
}
//	----------------------------------------------------------------------------
//	readState
//	----------------------------------------------------------------------------

void tuneState::readState(FILE *sfile){

	char s[240];
	float value;

	int i0,ia;
	int BM = BODEMAX;
	int FM = FILTMAX;
	int SM = IMU_BUF_COUNT;

	if(readReport(sfile))return;


	rewind(sfile);

	while(fscanf_s(sfile,"%s %f", &s, sizeof(s), &value)==2){

			 if(match3(s, "pidParam"	  ,&ia,3,		 "pGain"		))pidParam[ia].pGain				= F2S(value);
		else if(match3(s, "pidParam"	  ,&ia,3,		 "iGain"		))pidParam[ia].iGain				= F2S(value);
		else if(match3(s, "pidParam"	  ,&ia,3,		 "dGain"		))pidParam[ia].dGain				= F2S(value);
		else if(match3(s, "pidParam"	  ,&ia,3,		 "errorLimit"	))pidParam[ia].errorLimit			= F2S(value);

		else if(match2(s, "numfiltpar"	  ,&ia,3						))numfiltpar[ia]					= (int)value;
		else if(match1(s, "hsFilter"	  ,&ia,3,&i0,FM, "hsFrequency"	))hsFilter[ia][i0].hsFrequency		= F2S(value);
		else if(match1(s, "hsFilter"	  ,&ia,3,&i0,FM, "hsQValue"		))hsFilter[ia][i0].hsQValue			= F2S(value);
		else if(match1(s, "hsFilter"	  ,&ia,3,&i0,FM, "hsAValue"		))hsFilter[ia][i0].hsAValue			= F2S(value);
		else if(match1(s, "hsFilter"	  ,&ia,3,&i0,FM, "hsType"		))hsFilter[ia][i0].hsType			= (int)value;
		else if(match1(s, "hsFilter"	  ,&ia,3,&i0,FM, "limit"		))hsFilter[ia][i0].limit			= F2S(value);

		else if(match2(s, "numfiltpar"	  ,&ia,3						))numfiltpar[ia]					= (int)value;
		else if(match1(s, "hsFilterState" ,&ia,3,&i0,FM, "x0Gain"		))hsFilterState[ia][i0].x0Gain		= F2S(value);
		else if(match1(s, "hsFilterState" ,&ia,3,&i0,FM, "xMinus1Gain"	))hsFilterState[ia][i0].xMinus1Gain = F2S(value);
		else if(match1(s, "hsFilterState" ,&ia,3,&i0,FM, "xMinus2Gain"	))hsFilterState[ia][i0].xMinus2Gain = F2S(value);
		else if(match1(s, "hsFilterState" ,&ia,3,&i0,FM, "yMinus1Gain"	))hsFilterState[ia][i0].yMinus1Gain = F2S(value);
		else if(match1(s, "hsFilterState" ,&ia,3,&i0,FM, "yMinus2Gain"	))hsFilterState[ia][i0].yMinus2Gain = F2S(value);
		else if(match1(s, "hsFilterState" ,&ia,3,&i0,FM, "yMinus2Gain"	))hsFilterState[ia][i0].yMinus2Gain	= F2S(value);

		else if(match2(s, "numstep"		  ,&ia,3						))numstep[ia]						= (int)value;
		else if(match5(s, "stepsamples"	  ,&ia,3,&i0,SM					))stepsamples[ia][i0]				= value;

		else if(match2(s, "numffts"		  ,&ia,3						))numffts[ia]						= (int)value;
		else if(match5(s, "ffts"		  ,&ia,3,&i0,SM					))ffts[ia][i0]						= value;
		
		else if(match2(s, "nbode"		  ,&ia,3						))nbode[ia]							= (int)value;
		else if(match5(s, "gain"		  ,&ia,3,&i0,BM					))gain[ia][i0]						= value;
		else if(match5(s, "phase"		  ,&ia,3,&i0,BM					))phase[ia][i0]						= value;
		else if(match5(s, "freqs"		  ,&ia,3,&i0,BM					))freqs[ia][i0]						= value;

		else if(match2(s, "numOpen"		  ,&ia,3						))numOpen[ia]						= (int)value;
		else if(match5(s, "gainOpen"	  ,&ia,3,&i0,BM					))gainOpen[ia][i0]					= value;
		else if(match5(s, "phaseOpen"	  ,&ia,3,&i0,BM					))phaseOpen[ia][i0]					= value;
		else if(match5(s, "freqsOpen"	  ,&ia,3,&i0,BM					))freqsOpen[ia][i0]					= value;

		else if(match2(s, "nShake"		  ,&ia,3						))nShake[ia]						= (int)value;
		else if(match5(s, "gainShake"	  ,&ia,3,&i0,BM					))gainShake[ia][i0]					= value;
		else if(match5(s, "phaseShake"	  ,&ia,3,&i0,BM					))phaseShake[ia][i0]				= value;
		else if(match5(s, "freqsShake"	  ,&ia,3,&i0,BM					))freqsShake[ia][i0]				= value;
		else if(match5(s, "jitterShake"	  ,&ia,3,&i0,BM					))jitterShake[ia][i0]				= value;
	}

}

//	-------------------------------------------------------------------
//	getnumfilt
//	-------------------------------------------------------------------

int tuneState::getnumfilt(int axis){

	int count = 0;

	for(int filter=0;filter<FILTMAX;filter++){
		if(hsFilter[axis][filter].hsFrequency || hsFilter[axis][filter].hsType){
			count++;
		}else{
			break;
		}
	}
	return count;
}

//	-------------------------------------------------------------------
//	hsFilterNotify
//	-------------------------------------------------------------------

void tuneState::hsFilterNotify()
{
	int axis;
	int filter;
	filtP *hsFilterPtr;
	hsFilterState_t *hsFilterStatePtr;

	memset(hsFilterState, 0, sizeof(hsFilterState));

	for (axis = 0; axis < 3; axis++) {
		for (filter = 0; filter < FILTMAX; filter++) {
			hsFilterPtr = hsFilter[axis] + filter;	
			hsFilterStatePtr = hsFilterState[axis] + filter;
			getFilterCoeffs(hsFilterPtr,hsFilterStatePtr);
		}
	}
}
#endif
