#include "GenFading.h"
#include <stdlib.h>

void CFadingEnergyGenerator::Initialize(int n, double fd)
{
	m_branchNumber = n;
	m_maxOmega = 2*PI*fd;

	m_nos = pow(2, ceil(log10(m_branchNumber)/log10(2)));

	m_cosBeta.resize(m_nos);
	m_sinBeta.resize(m_nos);
	m_omega.resize(m_nos);
	m_theta.resize(m_nos);

	m_coeffReal.resize(m_nos);
	m_coeffImag.resize(m_nos);

	short ii;

	for(ii=1; ii<=m_nos; ii++)
	{
		m_cosBeta[ii-1] = cos(PI/m_nos*ii)*sqrt(2/((double)m_nos));
		m_sinBeta[ii-1] = sin(PI/m_nos*ii)*sqrt(2/((double)m_nos));

		m_omega[ii-1] = m_maxOmega*cos(2*PI/(4*m_nos)*(ii-0.5));
		m_theta[ii-1] = 2*PI*((float)rand())/RAND_MAX;
		// m_theta[ii-1] = 0;
	}

	this->Update(1);
}

void CFadingEnergyGenerator::Update(int numOfSteps)
{
	valarray<double> aux(numOfSteps, m_nos);
	valarray<double> oscValue;

	m_theta += m_omega*aux;  // update the phase of oscillators
	oscValue = cos(m_theta); // oscValue stores cos(m_theta) for fast Walsh transform

	aux = m_cosBeta*oscValue;
	fwt(m_coeffReal, aux, m_nos);

	aux = m_sinBeta*oscValue;
	fwt(m_coeffImag, aux, m_nos);
}

void CFadingEnergyGenerator::GetPwrDB(double* pDbArray)
{
	for(int ii=0; ii<m_branchNumber; ii++)
		*(pDbArray+ii) = 10*log10(m_coeffReal[ii]*m_coeffReal[ii]+m_coeffImag[ii]*m_coeffImag[ii]);

}

void CFadingEnergyGenerator::GetFd(double* pFd)
{
	*pFd = m_maxOmega/2/PI;
}

void CFadingEnergyGenerator::SetFd(double fd)
{
	m_maxOmega = 2*PI*fd;

	for(short ii=1; ii<=m_nos; ii++)
		m_omega[ii] = m_maxOmega*cos(2*PI/(4*m_nos)*(ii-0.5));	
}

void CFadingEnergyGenerator::GetMag(double* pMagArray)
{
	for(int ii=0; ii<m_branchNumber; ii++)
		*(pMagArray+ii) = sqrt(m_coeffReal[ii]*m_coeffReal[ii]+m_coeffImag[ii]*m_coeffImag[ii]);
}

void CFadingEnergyGenerator::GetComplexEnvelope(double *pRealArray,double *pImagArray)
{
	for(int ii=0; ii<m_branchNumber; ii++)
	{
		*(pRealArray+ii) = m_coeffReal[ii];
		*(pImagArray+ii) = m_coeffImag[ii];
	}
}

void CFadingEnergyGenerator::SetN(int n)
{
	this->Initialize(n, m_maxOmega/2/PI);
}

void CFadingEnergyGenerator::GetN(int* pN)
{
	*pN = m_branchNumber;
}


void CFadingEnergyGenerator::fwt(valarray<double> & z, valarray<double> & x, unsigned int n)
{
	// fast Walsh transform without multiplication
	
	int i,b,k;
	int m=1;
	double p,q;

	for(i=0;i<(n-1);i+=2)
	{
		p=x[i]+x[i+1];
		q=x[i]-x[i+1];
		x[i]=p;
		x[i+1]=q;
	}


	while((m<<=1)<n)
	{
		b=k=0;

		do {
				for(i=0;i<m;i+=2)
				{
					z[k]=x[b+i]+x[b+i+m];
					z[k+1]=x[b+i]-x[b+i+m];
					z[k+2]=x[b+i+1]-x[b+i+m+1];
					z[k+3]=x[b+i+1]+x[b+i+m+1];
					k+=4;
				}
				b+=2*m;
		} while(k<n);

		for(i=0;i<n;i++) x[i]=z[i];
	}
}