#include <math.h>
#include <nsp.h>
#include <nspfft.h>
#include <nspwin.h>
#include <nspcvrt.h>
#include <string>
#include "patrecog.h"

const int FFTSize=4096*2;
const int FFTSize2=FFTSize>>1;

int nPeaks;
SPeak Peak[MAXPEAKS];

CPatRecog::CPatRecog()
{
	FFTData=(float*)calloc(FFTSize2, sizeof(float));
	pFFTInput=(float*)calloc(FFTSize+2, sizeof(float));
}

CPatRecog::~CPatRecog()
{
	free(FFTData);
	free(pFFTInput);
}

int Length2Order(int length)
{
  int order = 0;

  while(0 != length) {
    length >>= 1;
    order ++;
  }
  order--;
  return order;
}

int CompareFFTIdx(const void *arg1, const void *arg2)
{
	SPeak f1=*((SPeak*)arg1);
	SPeak f2=*((SPeak*)arg2);
	if (f1.fAmp==f2.fAmp)
		return 0;
	if (f1.fAmp<f2.fAmp)
		return -1;
	return 1;
}

void CPatRecog::CalcPeaks(float *AvgFFTData)
{
	nPeaks=0;
	float fHiPeak=0.0f;
	int b=0;
	int nWndSize;
	for (int i=0;i<FFTSize2;i++)
	{
		if (AvgFFTData[i]>fHiPeak)
			fHiPeak=AvgFFTData[i];
	}
	if (fHiPeak<=0.0f)
		return;
	while (b<FFTSize2)
	{
		nWndSize=PEAKWINDOWSIZE;
		if ((nWndSize+b)>=FFTSize2)
			nWndSize=FFTSize2-b;
		for (int j=0;j<nWndSize;j++)
		{
			PeakWnd[j].nIdx=b+j;
			PeakWnd[j].fAmp=AvgFFTData[b+j];
		}
		qsort((void*)PeakWnd, nWndSize, sizeof(SPeak), CompareFFTIdx);
		if ((PeakWnd[nWndSize-1].fAmp-PeakWnd[nWndSize>>1].fAmp)>PEAKDIFF)
		{
			if (((b+nWndSize)<FFTSize2) && (b>0))
			{
				if ((AvgFFTData[b+nWndSize]<PeakWnd[nWndSize-1].fAmp) && (AvgFFTData[b-1]<PeakWnd[nWndSize-1].fAmp))
				{
					PeakWnd[nWndSize-1].fAmp=PeakWnd[nWndSize-1].fAmp/fHiPeak;
					Peak[nPeaks++]=PeakWnd[nWndSize-1];
				}
			}
		}
		b+=nWndSize;
	}
//	if (!nPeaks)
//		printf("no pattern found\n");
}

int CPatRecog::LoadPatternSet(const char *pszFilename)
{
	if (!pszFilename)
		return 0;
	FILE *pFile=fopen(pszFilename, "rb");
	if (!pFile)
		return 0;
	nPatterns=0;
	unsigned char c;
	bool bComplete=false;
	while ((!feof(pFile)) && (!bComplete))
	{
		fread(&c, 1, 1, pFile);
		if (c)
		{
			for (int i=0;i<c;i++)
			{
				fread(&(Pattern[nPatterns].sName[i]), 1, 1, pFile);
			}
			Pattern[nPatterns].sName[i]='\0';
			fread(&(Pattern[nPatterns].nPeaks), 4, 1, pFile);
			for (i=0;i<Pattern[nPatterns].nPeaks;i++)
			{
				fread(&(Pattern[nPatterns].Peak[i].nIdx), 4, 1, pFile);
				fread(&(Pattern[nPatterns].Peak[i].fAmp), 4, 1, pFile);
			}
			printf("%03d: read pattern %s, peaks: %i\n", nPatterns, Pattern[nPatterns].sName, Pattern[nPatterns].nPeaks);
			nPatterns++;
		}else
			bComplete=true;
	}
	fclose(pFile);
	return 0;
}

int CPatRecog::Analyze(float *pInputData, int nInputDataLen, float fThreshold, float *pfError, float *pfAmp)
{
	int p=0;
	float fMaxAmp=0.0f;
	for (int i=0;i<FFTSize;i++)
	{
		pFFTInput[i]=pInputData[p++];
		if (p>=nInputDataLen)
			p=0;
		if (pFFTInput[i]>fMaxAmp)
			fMaxAmp=pFFTInput[i];
	}
	if (pfAmp)
		(*pfAmp)=fMaxAmp;
	if (fMaxAmp<fThreshold)
	{
//		printf("signal too low\n");
		return -2;
	}
	nspsWinHamming(pFFTInput, FFTSize);
	nspsRealFft(pFFTInput, Length2Order(FFTSize), NSP_Forw);
	nspcbMag((SCplx*)pFFTInput, FFTData, FFTSize2);
	CalcPeaks(FFTData);
	int nRetVal=ApproximatePattern(pfError);
//	printf("approximated pattern: %03d, error: %2.5f", nRetVal, fError);
//	if (nRetVal>=0)
//		printf(" (%s)\n", Pattern[nRetVal].sName);
//	else
//		printf("\n");
	return nRetVal;
}

#define SHIFT_ERR_COEFF		0.2f
#define SCALE_ERR_COEFF		2.5f

int CPatRecog::ApproximatePattern(float *pfError)
{
	if (!nPeaks)
		return -1;
	int nLowPat=-1;
	float fLowPatVal=1E8;
	int nPeakUsed[PEAKWINDOWSIZE];
	for (int i=0;i<nPatterns;i++)
	{
		float fErrVal=0.0f;
		memset(nPeakUsed, 0, PEAKWINDOWSIZE*sizeof(int));
		for (int j=0;j<nPeaks;j++)
		{
			float fLowPeakVal=1E8;
			for (int k=0;k<Pattern[i].nPeaks;k++)
			{
				float fPeakErrVal=(float)(fabs(Peak[j].fAmp-Pattern[i].Peak[k].fAmp)*SCALE_ERR_COEFF+
																	fabs(Peak[j].nIdx-Pattern[i].Peak[k].nIdx)*SHIFT_ERR_COEFF);
				if (fPeakErrVal<fLowPeakVal)
					fLowPeakVal=fPeakErrVal;
			}
			fErrVal+=fLowPeakVal;
		}
		if (fErrVal<fLowPatVal)
		{
			nLowPat=i;
			fLowPatVal=fErrVal;
		}
	}
	if (pfError)
		(*pfError)=fLowPatVal;
	return nLowPat;
}