#include <ITABlockMath.h>
#include <memory.h>
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif

void bm_CMul(float* pfDestRe, float* pfDestIm, const float* pfRe1, const float* pfIm1, const float* pfRe2, const float* pfIm2, unsigned int uiCount) {
	for (unsigned int i=0; i<uiCount; i++) {
		*pfDestRe++ = *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
		*pfDestIm++ = *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
	}
}

void bm_CMul(float* pfDest, const float* pfSrc1, const float* pfSrc2, unsigned int uiCount) {
	for (unsigned int i=0; i<uiCount; i++)
		*pfDest++ = *pfSrc1++ * *pfSrc2++;
}

void bm_CMul_css(float* pfDest, const float* pfSource1, const float* pfSource2, unsigned int uiCssArraySize) {
	float *pfDestRe = pfDest;
	float *pfDestIm = pfDest + uiCssArraySize/2;
	const float *pfRe1 = pfSource1;
	const float *pfIm1 = pfSource1 + uiCssArraySize/2;
	const float *pfRe2 = pfSource2;
	const float *pfIm2 = pfSource2 + uiCssArraySize/2;

	for (unsigned int i=0; i<uiCssArraySize/2; i++) {
		*pfDestRe++ = *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
		*pfDestIm++ = *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
	}
}

void bm_CMulMag_css(float* pfDest, const float* pfSrcCmx, const float* pfSrcMag, unsigned int uiCssArraySize, float fFactor) {
	float *pfDestRe = pfDest;
	float *pfDestIm = pfDest + uiCssArraySize/2;
	const float *pfRe1 = pfSrcCmx;
	const float *pfIm1 = pfSrcCmx + uiCssArraySize/2;

	for (unsigned int i=0; i<uiCssArraySize/2; i++) {
		*pfDestRe++ = *pfRe1++ * *pfSrcMag * fFactor;
		*pfDestIm++ = *pfIm1++ * *pfSrcMag++ * fFactor;
	}
}

void bm_CMulAdd(float* pfDestRe, float* pfDestIm, const float* pfRe1, const float* pfIm1, const float* pfRe2, const float* pfIm2, unsigned int uiCount) {
	for (unsigned int i=0; i<uiCount; i++) {
		*pfDestRe++ += *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
		*pfDestIm++ += *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
	}
}

void bm_CMulAdd_css(float* pfDest, const float* pfSource1, const float* pfSource2, unsigned int uiCssArraySize) {
	float *pfDestRe = pfDest;
	float *pfDestIm = pfDest + uiCssArraySize/2;
	const float *pfRe1 = pfSource1;
	const float *pfIm1 = pfSource1 + uiCssArraySize/2;
	const float *pfRe2 = pfSource2;
	const float *pfIm2 = pfSource2 + uiCssArraySize/2;

	for (unsigned int i=0; i<uiCssArraySize/2; i++) {
		*pfDestRe++ += *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
		*pfDestIm++ += *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
	}
}

void bm_CMulSub(float* pfDestRe, float* pfDestIm, const float* pfRe1, const float* pfIm1, const float* pfRe2, const float* pfIm2, unsigned int uiCount) {
	for (unsigned int i = 0; i < uiCount; i++ ) {
		*pfDestRe++ -= *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
		*pfDestIm++ -= *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
	}
}

void bm_CMulSub_css(float* pfDest, const float* pfSource1, const float* pfSource2, unsigned int uiCssArraySize) {
	float *pfDestRe = pfDest;
	float *pfDestIm = pfDest + uiCssArraySize/2;
	const float *pfRe1 = pfSource1;
	const float *pfIm1 = pfSource1 + uiCssArraySize/2;
	const float *pfRe2 = pfSource2;
	const float *pfIm2 = pfSource2 + uiCssArraySize/2;

	for (unsigned int i=0; i<uiCssArraySize/2; i++ ) {
		*pfDestRe++ -= *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
		*pfDestIm++ -= *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
	}
}

void bm_CDiv(float* pfDestRe, float* pfDestIm, const float* pfZRe, const float* pfZIm, const float* pfNRe, const float* pfNIm, unsigned int uiCount) {
	float fNenner = 0.0;

	for (unsigned int i=0; i<uiCount; i++) {
		fNenner = *pfNRe * *pfNRe + *pfNIm * *pfNIm;
		
		*pfDestRe = *pfZRe * *pfNRe + *pfZIm * *pfNIm;
		*pfDestRe++ /= fNenner;
		
		*pfDestIm = *pfNRe++ * *pfZIm++ - *pfZRe++ * *pfNIm++;
		*pfDestIm++ /= fNenner; 
	}
}

/*
void bm_CMulMag_css(float *pfDest, const float *pfSrcCmx, const std::vector<float> &pvSrcMag, unsigned int uiCssArraySize, float fFactor) {
      float *pfDestRe = pfDest;
      float *pfDestIm = pfDest + uiCssArraySize/2;
      const float *pfRe1 = pfSrcCmx;
      const float *pfIm1 = pfSrcCmx + uiCssArraySize/2;

      for (unsigned int i=0; i<uiCssArraySize/2; i++) {
              *pfDestRe++ = *pfRe1++ * pvSrcMag[i] * fFactor;
              *pfDestIm++ = *pfIm1++ * pvSrcMag[i] * fFactor;
      }
}
*/

void bm_CDiv_css(float* pfDest, const float* pfZ, const float* pfN, unsigned int uiCssArraySize) {
	float *pfDestRe = pfDest;
	float *pfDestIm = pfDest + uiCssArraySize/2;
	const float *pfZRe = pfZ;
	const float *pfZIm = pfZ + uiCssArraySize/2;
	const float *pfNRe = pfN;
	const float *pfNIm = pfN + uiCssArraySize/2;

	float fNenner = 0.0;

	for (unsigned int i=0; i<uiCssArraySize/2; i++) {
		fNenner = *pfNRe * *pfNRe + *pfNIm * *pfNIm;
		
		*pfDestRe = *pfZRe * *pfNRe + *pfZIm * *pfNIm;
		*pfDestRe++ /= fNenner;
		
		*pfDestIm = *pfNRe++ * *pfZIm++ - *pfZRe++ * *pfNIm++;
		*pfDestIm++ /= fNenner; 
	}
}

void bm_Add(float* pfDest, const float* pfSummand, unsigned int uiCount) {
	for (unsigned int i=0; i<uiCount; i++)
		*pfDest++ += *pfSummand++;
}

void bm_ContinueMagnitudePreservePhase(float *pfRe, float *pfIm, unsigned int uiCount) {
	float fFactor = 0.0;
	float fConstMag = sqrtf((powf(*pfRe,2)) + (powf(*pfIm,2)));

	for (unsigned int i=0 ; i<uiCount; i++) {
		fFactor = fConstMag / sqrtf((powf(*pfRe,2)) + (powf(*pfIm,2))) ;
		*pfRe =  fFactor * *pfRe++;
		*pfIm =  fFactor * *pfIm++;
	}
}

void bm_ContinueMagnitudePreservePhase_css(float *pfData, float fStartFrequency, float fSamplerate, unsigned int uiCssArraySize) {
	unsigned int uiStartBin = (unsigned int)floor(fStartFrequency * uiCssArraySize / fSamplerate );
	float *pfRe = pfData;
	float *pfIm = pfData + uiCssArraySize/2;
	float fFactor = 0.0;
	float fConstMag = sqrtf( powf(pfRe[uiStartBin],2) + powf(pfIm[uiStartBin],2) );
	for (unsigned int i=uiStartBin ; i<((uiCssArraySize/2) - 1); i++) {
		fFactor = fConstMag / sqrtf( powf(pfRe[i],2) + powf(pfIm[i],2) ) ;
		pfRe[i] =  fFactor * pfRe[i];
		pfIm[i] =  fFactor * pfIm[i];
	}

	pfRe[(uiCssArraySize/2) - 1] =  0;
	pfIm[(uiCssArraySize/2) - 1] =  0;
}

void bm_MaxMag(float *pfData, unsigned int uiCount, float &fValue, unsigned int &uiPosition) {
	float fTmp = 0.0;
	uiPosition = 0;
	
	for (unsigned int i=0 ; i<uiCount; i++) {
		if (fabs(pfData[i]) > fTmp) {
			uiPosition = i;
			fTmp= pfData[i]; 
		}
	}

	fValue = fTmp;
}

void bm_Max(float *pfData, unsigned int uiCount, float &fValue, unsigned int &uiPosition) {
	float fTmp = 0.0;
	uiPosition = 0;
	
	for (unsigned int i=0 ; i<uiCount; i++) {
		if (pfData[i] > fTmp) {
			uiPosition = i;
			fTmp= pfData[i]; 
		}
	}
	fValue = fTmp ;
}

void bm_CyclicMove(float *pfData, unsigned int uiCount) {
	float fTmp = 0.0;
	unsigned int uiOffset = uiCount/2;

	for (unsigned int i=0 ; i<uiOffset; i++) {
		fTmp = pfData[i];
		pfData[i] = pfData[i + uiOffset];
		pfData[i + uiOffset] = fTmp;
	}
}

void bm_FreqIdent_css(float *pfData, unsigned int uiCssArraySize) {
	for (unsigned int i=0 ; i<uiCssArraySize/2; i++)
		pfData[i] = 1.0;

	memset(pfData + uiCssArraySize/2, 0, (uiCssArraySize/2) * sizeof(float));													
}

void bm_SubSampleShift_css(float *pfData, float fSubSampleShift, unsigned int uiCssArraySize) {
	float fPhi = 0.0;
	float fCosinusPhi = 0.0;
	float fSinusPhi	= 0.0;
	float fTmpRe = 0.0;
	float fTmpIm = 0.0;
	float *pfRe = pfData;
	float *pfIm = pfData + uiCssArraySize / 2;

	float fDeltaPhi = (float)-2.0 * (float)M_PI * fSubSampleShift / (float)uiCssArraySize;

	for (unsigned int i=0; i<uiCssArraySize/2; i++) {
		fPhi = fDeltaPhi * i;
		fCosinusPhi	= cosf(fPhi);
		fSinusPhi = sinf(fPhi);

		fTmpRe = pfRe[i];
		fTmpIm = pfIm[i];
				
		pfRe[i] = fTmpRe * fCosinusPhi - fTmpIm * fSinusPhi ;
		pfIm[i] = fTmpRe * fSinusPhi   + fTmpIm * fCosinusPhi ;
	}
}

void bm_SubSampleShift_SHDI(float *pfData, float fSubSampleShift, unsigned int uiCoeffs) {
	unsigned int uiDFTSize = (uiCoeffs-1)*2;
	float fPhi, fCosinusPhi, fSinusPhi;
	float fTmpRe, fTmpIm;

	//float *pfRe = pfData;
	//float *pfIm = pfData + uiCssArraySize / 2;

	float fDeltaPhi = (float) -1.0 * (float) M_PI * fSubSampleShift / (float) uiDFTSize;

	for (unsigned int i=0; i<uiCoeffs; i++) {
		fPhi = fDeltaPhi * i;
		fCosinusPhi	= cosf(fPhi);
		fSinusPhi = sinf(fPhi);

		fTmpRe = pfData[2*i];
		fTmpIm = pfData[2*i+1];
				
		pfData[2*i]   = fTmpRe * fCosinusPhi - fTmpIm * fSinusPhi;
		pfData[2*i+1] = fTmpRe * fSinusPhi   + fTmpIm * fCosinusPhi;
	}
}

unsigned int bm_GetNextFFTLength(unsigned int uiLength) {
	unsigned int uiFFTDegree = (unsigned int)floor(0.5 + log((float)uiLength)/log((float)2));
	return ((unsigned int) pow(2.0f, (float) uiFFTDegree));
}