#include "stdafx.h"
#include <stdint.h>
#include "gimbalState.h"
#include <math.h>

//	----------------------------------------------------------------------------
//	gimbalState
//	----------------------------------------------------------------------------

gimbalState::gimbalState(char *namePar)
{
	if(namePar){
		strcpy_s(name,sizeof(name),namePar);
		nameLen = (int)(strlen(name));
	}else{
		strcpy_s(name,sizeof(name),"");
		nameLen = 0;
	}
	init();
}

//	----------------------------------------------------------------------------
//	~gimbalState
//	----------------------------------------------------------------------------

gimbalState::~gimbalState(void)
{
}

//	----------------------------------------------------------------------------
//	getFilterCoeffs
//	----------------------------------------------------------------------------

void getFilterCoeffs(filterPar_t *hsFilterPtr, hsFilterState_t *hsFilterStatePtr)
{

	double Fs = kFreq;
	double pi = 3.14159265359;

	/* notch variables */
	double w0;
	double alpha;
	double b0,b1,b2,a0,a1,a2;

	/* lag lead variable */
	double omega1, omega2;
	double k0, k1, k2;

	memset(hsFilterStatePtr, 0, sizeof(hsFilterState_t));

	if( hsFilterPtr->ftype == FILTER_NOTCH){

		w0 = 2.0*pi*hsFilterPtr->f1/Fs;
		alpha = sin(w0)/(2.0*hsFilterPtr->q);

		/*notch:      H(s) = (s^2 + 1) / (s^2 + s/Q + 1)*/

		b0 =  1;
		b1 = -2*cos(w0);
		b2 =  1;
		a0 =  1 + alpha;
		a1 = -2*cos(w0);
		a2 =  1 - alpha;

		/*    y[n] = (b0/a0)*x[n] + (b1/a0)*x[n-1] + (b2/a0)*x[n-2] - (a1/a0)*y[n-1] - (a2/a0)*y[n-2] */
		if(a0!=0){
			hsFilterStatePtr->x0Gain = (b0/a0);
			hsFilterStatePtr->xMinus1Gain = (b1/a0);
			hsFilterStatePtr->xMinus2Gain = (b2/a0);
			hsFilterStatePtr->yMinus1Gain = (a1/a0);
			hsFilterStatePtr->yMinus2Gain = (a2/a0);
		}

	} else if( hsFilterPtr->ftype == FILTER_LOWPASS){

		w0 = 2.0*pi*hsFilterPtr->f1/Fs;
		alpha = sin(w0)/(2.0*hsFilterPtr->q);

		/*LPF:        H(s) = 1 / (s^2 + s/Q + 1) */

		b0 =  (1 - cos(w0))/2;
		b1 =   1 - cos(w0);
		b2 =  (1 - cos(w0))/2;
		a0 =   1 + alpha;
		a1 =  -2*cos(w0);
		a2 =   1 - alpha;

		if(a0!=0){
			hsFilterStatePtr->x0Gain = (b0/a0);
			hsFilterStatePtr->xMinus1Gain = (b1/a0);
			hsFilterStatePtr->xMinus2Gain = (b2/a0);
			hsFilterStatePtr->yMinus1Gain = (a1/a0);
			hsFilterStatePtr->yMinus2Gain = (a2/a0);
		}

	}
	else if( hsFilterPtr->ftype == FILTER_LAGLEAD){
		/* lag lead filter - 1st order ( no [n-2] terms)*/
		if(hsFilterPtr->f1>0 && hsFilterPtr->q>0 ){
			omega1 = hsFilterPtr->f1 * (2 * 3.141592654);
			omega2 = hsFilterPtr->q * omega1;
			k1 = exp( -omega1 / Fs );
			k2 = exp( -omega2 / Fs );
			k0 = ( 1.0 - k1 )/( 1.0 - k2 );
			hsFilterStatePtr->x0Gain = k0;
			hsFilterStatePtr->xMinus1Gain = -k0 * k2;
			hsFilterStatePtr->xMinus2Gain = 0;
			hsFilterStatePtr->yMinus1Gain = -k1;
			hsFilterStatePtr->yMinus2Gain = 0;
		}
	}
}

//	----------------------------------------------------------------------------
//	default
//	----------------------------------------------------------------------------

void gimbalState::defaultSettings(void)
{
	static filterPar_t hsFilterP = {FILTER_LAGLEAD,31.867f,5.0318f};
	static hsFilterState_t hsFilterS;
	getFilterCoeffs(&hsFilterP,&hsFilterS);
	memset(par.filtP,0,sizeof(par.filtP));//par.filtP
	memset(hsFilterState,0,sizeof(hsFilterState));
	for(int i=0;i<3;i++){
		par.filtP[i][0] = hsFilterP;
		hsFilterState[i][0] = hsFilterS;
		numfilt[i] = 1;
		par.pidP[i].kP[0] = 1.0f;
		par.pidP[i].kI[0] = 0.015f;
		par.pidP[i].kILim[0] = 1.0f;
		par.pidP[i].kT = 0.025f;
		par.pidP[i].kC = 0;
	}
}

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

void gimbalState::init(void)
{
	memset(&par,0,sizeof(par));

	memset(numfilt,0,sizeof(numfilt));
	memset(numfiltpar,0,sizeof(numfiltpar));
	memset(par.filtP,0,sizeof(par.filtP));
	memset(hsFilterState,0,sizeof(hsFilterState));

	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 gimbalState::copy(gimbalState in)
{
	memcpy(&par,			&in.par,			sizeof(par)				);

	memcpy(numfilt,			&in.numfilt,		sizeof(numfilt)			);
	memcpy(numfiltpar,		&in.numfiltpar,		sizeof(numfiltpar)		);
	memcpy(hsFilterState,	&in.hsFilterState,	sizeof(hsFilterState)	);
	memcpy(par.filtP,		&in.par.filtP,		sizeof(par.filtP)		);

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

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

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

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

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

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

void gimbalState::saveState(FILE *sfile)
{
	char *n = name;

	for(int axis=0;axis<NiAXES;axis++){
		char *e = "pidParam";
		char *ff = "%s.%s[%i].%s %f\n";
		fprintf(sfile,ff,n,e,axis,"kP"					,par.pidP[axis].kP			);
		fprintf(sfile,ff,n,e,axis,"kI"					,par.pidP[axis].kI			);
		fprintf(sfile,ff,n,e,axis,"kD"					,par.pidP[axis].kD			);
		fprintf(sfile,ff,n,e,axis,"kILim"				,par.pidP[axis].kILim		);
		fprintf(sfile,ff,n,e,axis,"kT"					,par.pidP[axis].kT			);
		fprintf(sfile,ff,n,e,axis,"kC"					,par.pidP[axis].kC			);
	}

	for(int axis=0;axis<NoAXES;axis++){
		char *e = "outerP";
		char *ff = "%s.%s[%i].%s %f\n";
		fprintf(sfile,ff,n,e,axis,"kP"					,par.outerP[axis].kP			);
		fprintf(sfile,ff,n,e,axis,"kFF"					,par.outerP[axis].kFF			);
		fprintf(sfile,ff,n,e,axis,"kTach"				,par.outerP[axis].kTach			);
	}

	for(int axis=0;axis<NiAXES;axis++){
		fprintf(sfile,"%s.%s[%i] %i\n",n,"numfiltPar",axis	,numfiltpar[axis]			);
		char *e = "par.filtP";
		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,"f1"		,par.filtP[axis][filter].f1		);
			fprintf(sfile,ff,n,e,axis,filter,"q"		,par.filtP[axis][filter].q		);
			fprintf(sfile,fi,n,e,axis,filter,"ftype"	,par.filtP[axis][filter].ftype	);
		}
	}

	for(int axis=0;axis<NiAXES3;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"		,hsFilterState[axis][filter].x0Gain			);
			fprintf(sfile,ff,n,e,axis,filter,"xMinus1Gain"	,hsFilterState[axis][filter].xMinus1Gain	);
			fprintf(sfile,ff,n,e,axis,filter,"xMinus2Gain"	,hsFilterState[axis][filter].xMinus2Gain	);
			fprintf(sfile,ff,n,e,axis,filter,"yMinus1Gain"	,hsFilterState[axis][filter].yMinus1Gain	);
			fprintf(sfile,ff,n,e,axis,filter,"yMinus2Gain"	,hsFilterState[axis][filter].yMinus2Gain	);
		}
	}

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

	for(int axis=0;axis<NiAXES3;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<NiAXES3;axis++){
		char *e = "numffts";
		char *fi = "%s.%s[%i] %i\n";
		fprintf(sfile,fi,n,e,axis							,numffts[axis]			);
	}

	for(int axis=0;axis<NiAXES3;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<NiAXES3;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<NiAXES3;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<NiAXES3;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 gimbalState::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 gimbalState::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 gimbalState::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 gimbalState::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;
}

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

void gimbalState::readState(FILE *sfile)
{
	char s[240];
	float value;

	int i0,ia;
	int BM = BODEMAX;
	int FM = FILTMAX;
	int SM = IMU_BUF_POINTS;
	int Ni = NiAXES;
	int No = NoAXES;
	int N3 = NiAXES3;

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

		     if(match3(s, "pidParam"	  ,&ia,Ni,		 "kP"		))par.pidP[ia].kP[0]			= value;
		else if(match3(s, "pidParam"	  ,&ia,Ni,		 "kI"		))par.pidP[ia].kI[0]			= value;
		else if(match3(s, "pidParam"	  ,&ia,Ni,		 "kD"		))par.pidP[ia].kD[0]			= value;
		else if(match3(s, "pidParam"	  ,&ia,Ni,		 "kILim"	))par.pidP[ia].kILim[0]			= value;
		else if(match3(s, "pidParam"	  ,&ia,Ni,		 "kT"		))par.pidP[ia].kT				= value;
		else if(match3(s, "pidParam"	  ,&ia,Ni,		 "kC"		))par.pidP[ia].kC				= value;

		else if(match3(s, "outerP"		  ,&ia,No,		 "kP"		))par.outerP[ia].kP				= value;
		else if(match3(s, "outerP"		  ,&ia,No,		 "kFF"		))par.outerP[ia].kFF			= value;
		else if(match3(s, "outerP"		  ,&ia,No,		 "kTach"	))par.outerP[ia].kTach			= value;

		else if(match2(s, "numfiltpar"	  ,&ia,Ni					))numfiltpar[ia]				= (int)value;
		else if(match1(s, "par.filtP"	  ,&ia,Ni,&i0,FM, "f1"		))par.filtP[ia][i0].f1			= value;
		else if(match1(s, "par.filtP"	  ,&ia,Ni,&i0,FM, "q"		))par.filtP[ia][i0].q			= value;
		else if(match1(s, "par.filtP"	  ,&ia,Ni,&i0,FM, "ftype"	))par.filtP[ia][i0].ftype		= (int)value;

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

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

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

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

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

}

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

int gimbalState::getnumfilt(int axis)
{
	int count = 0;

	for(int filter=0;filter<FILTMAX;filter++){
		if(par.filtP[axis][filter].f1 || par.filtP[axis][filter].ftype){
			count++;
		}else{
			break;
		}
	}
	return count;
}

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

void gimbalState::hsFilterNotify()
{
	int axis;
	int filter;
	filterPar_t *hsFilterPtr;
	hsFilterState_t *hsFilterStatePtr;

	memset(hsFilterState, 0, sizeof(hsFilterState));
	static filterPar_t *ff = &par.filtP[0][0];

	for (axis = 0; axis < 3; axis++){
		for (filter = 0; filter < FILTMAX; filter++){
			hsFilterPtr = par.filtP[axis] + filter;	
			hsFilterStatePtr = hsFilterState[axis] + filter;
			getFilterCoeffs(hsFilterPtr,hsFilterStatePtr);
		}
	}
}