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

// --------------------------------------------------------------
// Clock generator
// --------------------------------------------------------------
void ClockInit(sample_clock_t *pObj, uint32_t divide)
{
	if (divide == 0)
		divide = 1;

	pObj->reload = divide;
	pObj->count = (int32_t)divide;
}

uint32_t ClockIsTick(sample_clock_t *pObj, uint32_t do_advance)
{
	uint32_t isTick;

	isTick = pObj->count == 0;

	if (do_advance)
	{
		if (isTick)
		{
			pObj->count = pObj->reload;
		}
		pObj->count--;
	}
	return isTick;
}

void ClockAdvance(sample_clock_t *pObj)
{
	if (pObj->count == 0)
	{
		pObj->count = pObj->reload;
	}
	pObj->count--;
}

void ClockSetPhase(sample_clock_t *pObj, int32_t phase)
{
	pObj->count = pObj->count - phase;
	if (pObj->count <= 0)
	{
		pObj->count += pObj->reload;
	}
	else if (pObj->count > (int32_t)pObj->reload)
	{
		pObj->count -= pObj->reload;
	}
}

// --------------------------------------------------------------
// Second-order Loop Filter
// --------------------------------------------------------------
void LeadLagInit(lead_lag_filter_t *pObj, radio_float_t state)
{
	pObj->lag_state = LEAD_LAG_SCALING*state;
};

void LeadLagSetCoeff(lead_lag_coeff_t *pCoeff, radio_float_t lead, radio_float_t lag)
{
	pCoeff->lead = lead;
	pCoeff->lag = lag;
}

radio_float_t LeadLagProcess(lead_lag_filter_t *pObj, lead_lag_coeff_t *pCoeff, radio_float_t x)
{
	pObj->lag_state += LEAD_LAG_SCALING*pCoeff->lag * x;

	return pObj->lag_state/LEAD_LAG_SCALING + pCoeff->lead * x;
}

radio_float_t LeadLagGetState(lead_lag_filter_t *pObj)
{
	return pObj->lag_state/LEAD_LAG_SCALING;
}

void LeadLagSetState(lead_lag_filter_t *pObj, radio_float_t state)
{
	pObj->lag_state = LEAD_LAG_SCALING*state;
}

// --------------------------------------------------------------
// Gardner Timing Error Detector
// --------------------------------------------------------------
void STRGardnerInit(str_t *pObj)
{
	memset(pObj->d, 0, sizeof(pObj->d));
}

radio_float_t STRGardnerProcess(str_t *pObj, cpx_t x)
{
	radio_float_t Vd;
	cpx_t t, *pD;

	pD = pObj->d;

	t.real = (pD[1].real - x.real) * pD[0].real;
	t.imag = (pD[1].imag - x.imag) * pD[0].imag;
	Vd = (t.real + t.imag);

	// Adjust delay line
	pD[1] = pD[0];
	pD[0] = x;

	return Vd;
}

// --------------------------------------------------------------
// Simple Phase detector for QPSK
// --------------------------------------------------------------
radio_float_t PhaseErrQPSK(cpx_t x)
{
	radio_float_t perr;
	perr = x.imag*dsign(x.real) - x.real*dsign(x.imag);

	return perr;
}

radio_float_t PhaseErr(cpx_t x, radio_float_t pstep, radio_float_t poffset)
{
	radio_float_t perr, phi, m;

	phi = (radio_float_t)CpxPhiS(x) + poffset;
	
	m = dmod(phi/(radio_float_t)PI,(radio_float_t)2.0/pstep);
	perr = (radio_float_t)1.0/pstep - m;

	return (radio_float_t)(PI*perr);
}

// --------------------------------------------------------------
void PfdInit(pfd_t *pObj)
{
	pObj->state_up = 0;
	pObj->state_down = 0;
}

radio_float_t PfdProcess(pfd_t *pObj, radio_float_t x, radio_float_t k)
{
	radio_float_t result;

	result = 0;
	if (x < 0)
	{
		if (pObj->state_down == 0)
		{
			pObj->state_up = 0;
			pObj->state_down = 1;
		}
		else
		{
			result = -k;
		}
	}
	else
	{
		if (pObj->state_up == 0)
		{
			pObj->state_down = 0;
			pObj->state_up = 1;
		}
		else
		{
			result = k;
		}
	}

	return result;
}

// --------------------------------------------------------------
void TimingCorrectorInit(timing_corrector_t *pObj, radio_float_t wrap_alpha)
{
	pObj->mu = 0;
	pObj->mu_filtered = 0;
	pObj->wrap_alpha = wrap_alpha;
	pObj->wrap_beta = (radio_float_t)1.0 - wrap_alpha;
	memset(pObj->skip, 0, sizeof(pObj->skip));
	memset(pObj->stuff, 0, sizeof(pObj->stuff));
}

radio_float_t TimingCorrectorProcess(timing_corrector_t *pObj, radio_float_t dMu)
{
	pObj->mu += dMu; 
	pObj->skip[1] = pObj->skip[0];
	pObj->stuff[1] = pObj->stuff[0];
	pObj->skip[0] = 0;
	pObj->stuff[0] = 0;
	pObj->mu_filtered = pObj->wrap_beta*pObj->mu_filtered + pObj->wrap_alpha*pObj->mu;

	if (pObj->mu_filtered < 0)
	{
		pObj->mu += (radio_float_t)1.0;
		pObj->stuff[0] = 1;
	}
	else if (pObj->mu_filtered >= (radio_float_t)1.0)
	{
		pObj->mu -= (radio_float_t)1.0;
		pObj->skip[0] = 1;
	}

	return pObj->mu;
}

radio_float_t TimingCorrectorGetMu(timing_corrector_t *pObj)
{
	return pObj->mu;
}

uint32_t TimingCorrectorIsSkip(timing_corrector_t *pObj)
{
	return pObj->skip[0];
}

uint32_t TimingCorrectorIsStuff(timing_corrector_t *pObj)
{
	return pObj->stuff[0];
}