/*	anr.c

This file is part of a program that implements a Software-Defined Radio.

Copyright (C) 2012, 2013 Warren Pratt, NR0V

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

The author can be reached by email at

warren@wpratt.com

*/

#include "comm.h"

/* Filter output and tap-window energy over a unit-stride run of the delay line.

   The delay line is indexed (in_idx + j + delay) & mask, which wraps at most
   once across the tap window; xanr() splits the window at the wrap so both
   halves are contiguous here.  Four independent accumulator pairs keep the FMAs
   off a single dependency chain and let the vectorizer in -- an 'y += w[j]*x[j]'
   reduction cannot be reassociated without -ffast-math, which this library must
   not enable (it relies on IEEE semantics for 0/0 = NaN and x/0 = Inf). */
static inline void anr_dot (const double* WDSP_RESTRICT w,
	const double* WDSP_RESTRICT x, int n, double* py, double* psigma)
{
	double y0 = 0.0, y1 = 0.0, y2 = 0.0, y3 = 0.0;
	double s0 = 0.0, s1 = 0.0, s2 = 0.0, s3 = 0.0;
	int j = 0;
	for (; j <= n - 4; j += 4)
	{
		y0 += w[j + 0] * x[j + 0];	s0 += x[j + 0] * x[j + 0];
		y1 += w[j + 1] * x[j + 1];	s1 += x[j + 1] * x[j + 1];
		y2 += w[j + 2] * x[j + 2];	s2 += x[j + 2] * x[j + 2];
		y3 += w[j + 3] * x[j + 3];	s3 += x[j + 3] * x[j + 3];
	}
	for (; j < n; j++)
	{
		y0 += w[j] * x[j];
		s0 += x[j] * x[j];
	}
	*py		+= (y0 + y1) + (y2 + y3);
	*psigma += (s0 + s1) + (s2 + s3);
}

/* Leaky-LMS tap update over the same unit-stride run. */
static inline void anr_update (double* WDSP_RESTRICT w,
	const double* WDSP_RESTRICT x, int n, double c0, double c1)
{
	int j;
	for (j = 0; j < n; j++)
		w[j] = c0 * w[j] + c1 * x[j];
}

ANR create_anr	(
				int run,
				int position,
				int buff_size,
				double *in_buff,
				double *out_buff,
				int dline_size,
				int n_taps,
				int delay,
				double two_mu,
				double gamma,

				double lidx,
				double lidx_min,
				double lidx_max,
				double ngamma,
				double den_mult,
				double lincr,
				double ldecr
			)
{
	ANR a = (ANR) malloc0 (sizeof(anr));
	a->run = run;
	a->position = position;
	a->buff_size = buff_size;
	a->in_buff = in_buff;
	a->out_buff = out_buff;
	a->dline_size = dline_size;
	a->mask = dline_size - 1;
	a->n_taps = n_taps;
	a->delay = delay;
	a->two_mu = two_mu;
	a->gamma = gamma;
	a->in_idx = 0;
	a->lidx = lidx;
	a->lidx_min = lidx_min;
	a->lidx_max = lidx_max;
	a->ngamma = ngamma;
	a->den_mult = den_mult;
	a->lincr = lincr;
	a->ldecr = ldecr;
	
	memset (a->d, 0, sizeof(double) * ANR_DLINE_SIZE);
	memset (a->w, 0, sizeof(double) * ANR_DLINE_SIZE);
	
	return a;
}

void destroy_anr (ANR a)
{
	_aligned_free (a);
}

void xanr (ANR a, int position)
{
	int i;
	double c0, c1;
	double y, error, sigma, inv_sigp;
	double nel, nev;
	if (a->run && (a->position == position))
	{
		const int n_taps = a->n_taps;
		const int dline_size = a->dline_size;
		const int mask = a->mask;
		const int delay = a->delay;
		const int buff_size = a->buff_size;
		const double two_mu = a->two_mu;
		const double gamma = a->gamma;
		const double den_mult = a->den_mult;
		const double lincr = a->lincr;
		const double ldecr = a->ldecr;
		const double lidx_min = a->lidx_min;
		const double lidx_max = a->lidx_max;
		/* in_buff and out_buff are the same buffer in RXA, so neither may be
		   marked restrict; d and w are private to the struct. */
		const double* in_buff = a->in_buff;
		double* out_buff = a->out_buff;
		double* WDSP_RESTRICT d = a->d;
		double* WDSP_RESTRICT w = a->w;
		int in_idx = a->in_idx;
		double lidx = a->lidx;
		double ngamma = a->ngamma;

		for (i = 0; i < buff_size; i++)
		{
			double dsamp;
			int base, n1;

			dsamp = in_buff[2 * i + 0];
			d[in_idx] = dsamp;

			base = (in_idx + delay) & mask;
			if ((n1 = dline_size - base) > n_taps) n1 = n_taps;

			y = 0.0;
			sigma = 0.0;
			anr_dot (w, d + base, n1, &y, &sigma);
			if (n1 < n_taps)
				anr_dot (w + n1, d, n_taps - n1, &y, &sigma);

			inv_sigp = 1.0 / (sigma + 1e-10);
			error = dsamp - y;

			out_buff[2 * i + 0] = y;
			out_buff[2 * i + 1] = 0.0;

			if((nel = error * (1.0 - two_mu * sigma * inv_sigp)) < 0.0) nel = -nel;
			if((nev = dsamp - (1.0 - two_mu * ngamma) * y - two_mu * error * sigma * inv_sigp) < 0.0) nev = -nev;
			if (nev < nel)
			{
				if ((lidx += lincr) > lidx_max) lidx = lidx_max;
			}
			else
			{
				if ((lidx -= ldecr) < lidx_min) lidx = lidx_min;
			}
			ngamma = gamma * (lidx * lidx) * (lidx * lidx) * den_mult;

			c0 = 1.0 - two_mu * ngamma;
			c1 = two_mu * error * inv_sigp;

			anr_update (w, d + base, n1, c0, c1);
			if (n1 < n_taps)
				anr_update (w + n1, d, n_taps - n1, c0, c1);

			in_idx = (in_idx + mask) & mask;
		}
		a->in_idx = in_idx;
		a->lidx = lidx;
		a->ngamma = ngamma;
	}
	else if (a->in_buff != a->out_buff)
		memcpy (a->out_buff, a->in_buff, a->buff_size * sizeof (complex));
}

void flush_anr (ANR a)
{
	memset (a->d, 0, sizeof(double) * ANR_DLINE_SIZE);
	memset (a->w, 0, sizeof(double) * ANR_DLINE_SIZE);
	a->in_idx = 0;
}

void setBuffers_anr (ANR a, double* in, double* out)
{
	a->in_buff = in;
	a->out_buff = out;
}

void setSamplerate_anr (ANR a, int rate)
{
	flush_anr(a);
}

void setSize_anr (ANR a, int size)
{
	a->buff_size = size;
	flush_anr(a);
}

/********************************************************************************************************
*																										*
*											RXA Properties												*
*																										*
********************************************************************************************************/

PORT void
SetRXAANRRun (int channel, int run)
{
	ANR a = rxa[channel].anr.p;
	if (a->run != run)
	{
		RXAbp1Check (channel, rxa[channel].amd.p->run, rxa[channel].snba.p->run,
			rxa[channel].emnr.p->run, rxa[channel].anf.p->run, run,
			rxa[channel].rnnr.p->run, rxa[channel].sbnr.p->run);  // NR3 + NR4 support
		EnterCriticalSection (&ch[channel].csDSP);
		a->run = run;
		RXAbp1Set (channel);
		flush_anr (a);
		LeaveCriticalSection (&ch[channel].csDSP);
	}
}

PORT void
SetRXAANRVals (int channel, int taps, int delay, double gain, double leakage)
{
	EnterCriticalSection (&ch[channel].csDSP);
	rxa[channel].anr.p->n_taps = taps;
	rxa[channel].anr.p->delay = delay;
	rxa[channel].anr.p->two_mu = gain;
	rxa[channel].anr.p->gamma = leakage;
	flush_anr (rxa[channel].anr.p);
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT void
SetRXAANRTaps (int channel, int taps)
{
	EnterCriticalSection (&ch[channel].csDSP);
	rxa[channel].anr.p->n_taps = taps;
	flush_anr (rxa[channel].anr.p);
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT void
SetRXAANRDelay (int channel, int delay)
{
	EnterCriticalSection (&ch[channel].csDSP);
	rxa[channel].anr.p->delay = delay;
	flush_anr (rxa[channel].anr.p);
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT void
SetRXAANRGain (int channel, double gain)
{
	EnterCriticalSection (&ch[channel].csDSP);
	rxa[channel].anr.p->two_mu = gain;
	flush_anr (rxa[channel].anr.p);
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT void
SetRXAANRLeakage (int channel, double leakage)
{
	EnterCriticalSection (&ch[channel].csDSP);
	rxa[channel].anr.p->gamma = leakage;
	flush_anr (rxa[channel].anr.p);
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT void
SetRXAANRPosition (int channel, int position)
{
	EnterCriticalSection (&ch[channel].csDSP);
	rxa[channel].anr.p->position = position;
	rxa[channel].bp1.p->position = position;
	flush_anr (rxa[channel].anr.p);
	LeaveCriticalSection (&ch[channel].csDSP);
}