// --------------------------------------------------------------
#include <string.h>
#include <math.h>
#include "nco.h"

// --------------------------------------------------------------
// NCO
// Scalar version
// --------------------------------------------------------------
void NCO_Init(nco_t *pObj, radio_float_t omega, radio_float_t phi)
{
	pObj->omega = omega;
	pObj->phi = phi;
	memset(&pObj->state,0,sizeof(cpx_t));
}

void NCO_Free(nco_t *pObj)
{
	(void)pObj;
}

void NCO_Reinit(nco_t *pObj, radio_float_t omega, radio_float_t phi)
{
	NCO_Free(pObj);
	NCO_Init(pObj, omega, phi);
}

void NCO_SetOmega(nco_t *pObj, radio_float_t omega)
{
	pObj->omega = omega;
}

void NCO_SetPhi(nco_t *pObj, radio_float_t phi)
{
	pObj->phi = phi;
}

radio_float_t NCO_GetOmega(nco_t *pObj)
{
	return pObj->omega;
}

radio_float_t NCO_GetPhi(nco_t *pObj)
{
	return pObj->phi;
}

void NCO_Process(nco_t *pObj, radio_float_t dOmega, radio_float_t dPhi)
{
	pObj->phi += (pObj->omega + dOmega);

	if (pObj->phi < -(radio_float_t)0.5)
		pObj->phi += (radio_float_t)1.0;

	if (pObj->phi >= +(radio_float_t)0.5)
		pObj->phi -= (radio_float_t)1.0;

	pObj->state.real = +(radio_float_t)cos(2.0*PI*pObj->phi + dPhi);
	pObj->state.imag = -(radio_float_t)sin(2.0*PI*pObj->phi + dPhi);
}

cpx_t NCO_MixRealComplexS(nco_t *pObj, radio_float_t src, radio_float_t gain)
{
	cpx_t dst;

	dst.real = gain*src;
	dst.imag = 0;

	return CpxMulS(pObj->state, dst);
}

radio_float_t NCO_MixComplexRealS(nco_t *pObj, cpx_t src, cpx_t gain)
{
	return gain.real*pObj->state.real*src.real + gain.imag*pObj->state.imag*src.imag;
}

cpx_t NCO_MixComplexS(nco_t *pObj, cpx_t src, cpx_t gain)
{
	return CpxMulS(pObj->state, CpxScaleComplexS(src, gain));
}

void NCO_MixRealComplexV(nco_t *pObj, cpx_t *pDst, radio_float_t *pSrc, radio_float_t gain, uint32_t len)
{

	while (len--)
	{

		*(pDst++) = NCO_MixRealComplexS(pObj, *(pSrc++), gain);

		// NCO update
		NCO_Process(pObj, 0, 0);
	}
}

void NCO_MixComplexRealV(nco_t *pObj, radio_float_t *pDst, cpx_t *pSrc, cpx_t gain, uint32_t len)
{
	while (len--)
	{

		*(pDst++) = NCO_MixComplexRealS(pObj, *(pSrc++), gain);

		// NCO update
		NCO_Process(pObj, 0, 0);
	}
}

void NCO_MixComplexV(nco_t *pObj, cpx_t *pSrcDst, cpx_t gain, uint32_t len)
{
	while (len--)
	{
		*(pSrcDst++) = NCO_MixComplexS(pObj, *(pSrcDst++), gain);

		// NCO update
		NCO_Process(pObj, 0, 0);
	}
}

// --------------------------------------------------------------