/* 
 * File:   Nco.hpp
 * Author: jens
 *
 * Created on 22. Dezember 2014, 12:51
 */

#ifndef _RADIO_NCO_HPP_
#define	_RADIO_NCO_HPP_

#include "Vector.hpp"

namespace Radio
{

class Nco
{
public:
	Nco(RealScalar omega=0, RealScalar phi=0)
	: m_omega(omega)
	, m_phi(phi)
	{
		
	}
	~Nco()
	{
		
	}
	void init(RealScalar omega=0, RealScalar phi=0)
	{
		m_omega = omega;
		m_phi = phi;
	}
	
	void setOmega(RealScalar omega)
	{
		m_omega = omega;
	}

	void setPhi(RealScalar phi)
	{
		m_phi = phi;
	}

	RealScalar getOmega()
	{
		return m_omega;
	}

	RealScalar getPhi()
	{
		return m_phi;
	}
	
	void process(RealScalar dOmega, RealScalar dPhi)
	{
		m_phi += (m_omega + dOmega);

		if (m_phi < -(radio_float_t)0.5)
			m_phi += (radio_float_t)1.0;

		if (m_phi >= +(radio_float_t)0.5)
			m_phi -= (radio_float_t)1.0;

		m_state = ComplexScalar((RealScalar)cos(2.0*PI*m_phi + dPhi), -(RealScalar)sin(2.0*PI*m_phi + dPhi));
	}

	ComplexScalar mixRealComplexS(RealScalar const &src, RealScalar const &gain)
	{
		ComplexScalar dst(gain*src, 0);

		return m_state * dst;
	}

	RealScalar mixComplexRealS(ComplexScalar const &src, ComplexScalar const &gain)
	{
		return gain.real()*m_state.real()*src.real() + gain.imag()*m_state.imag()*src.imag();
	}

	ComplexScalar mixComplexS(ComplexScalar const &src, ComplexScalar const &gain)
	{
		ComplexScalar y(src.real()*gain.real(), src.imag()*gain.imag());
		
		return y * m_state;
	}

	void mixRealComplexV(CVec &dst, RVec const &src, RealScalar gain, size_t len)
	{
		size_t i;
		
		i = 0;
		while (len--)
		{

			dst[i] = mixRealComplexS(src[i], gain);

			// NCO update
			process(0, 0);
			i++;
		}
	}

	void mixComplexRealV(RVec &dst, CVec const &src, ComplexScalar const &gain, size_t len)
	{
		size_t i;
		
		i = 0;
		while (len--)
		{

			dst[i] = mixComplexRealS(src[i], gain);

			// NCO update
			process(0, 0);
			i++;
		}
	}

	void mixComplexV(CVec &srcDst, ComplexScalar const &gain, size_t len)
	{
		size_t i;
		
		i = 0;
		while (len--)
		{
			srcDst[i] = mixComplexS(srcDst[i], gain);

			// NCO update
			process(0, 0);
			i++;
		}
	}	
	
private:
	RealScalar m_omega;
	RealScalar m_phi;
	ComplexScalar m_state;
};

} // ::Radio


#endif	/* _RADIO_NCO_HPP_ */