/*	varsamp.c

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

Copyright (C) 2017 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"

void calc_varsamp (VARSAMP a)
{
	double min_rate, norm_rate;
	// double max_rate;
	double fc_norm_high, fc_norm_low;
	a->nom_ratio = (double)a->out_rate / (double)a->in_rate;
	a->cvar = a->var * a->nom_ratio;
	a->inv_cvar = 1.0 / a->cvar;
	a->old_inv_cvar = a->inv_cvar;
	a->dicvar = 0.0;
	a->delta = fabs (1.0 / a->cvar - 1.0);
	a->fc = a->fcin;
	if (a->out_rate >= a->in_rate)
	{
		min_rate = (double)a->in_rate;
		// max_rate = (double)a->out_rate;
		norm_rate = min_rate;
	}
	else
	{
		min_rate = (double)a->out_rate;
		// max_rate = (double)a->in_rate;
		// norm_rate = max_rate;
		norm_rate = (double)a->in_rate;
	}
	if (a->fc == 0.0) a->fc = 0.95 * 0.45 * min_rate;
	fc_norm_high = a->fc / norm_rate;
	if (a->fc_low < 0.0)
		fc_norm_low = - fc_norm_high;
	else
		fc_norm_low = a->fc_low / norm_rate;
	a->rsize = (int)(140.0 * norm_rate / min_rate);
	a->ncoef = a->rsize + 1;
	a->ncoef += (a->R - 1) * (a->ncoef - 1);		// = R * rsize + 1
	{
		/* Store the coefficients transposed into phases.  hshift() walks
		   h[hidx + m*R] for m = 0..rsize-1, which strides by R doubles -- 8 KB
		   at R = 1024 -- over a 1.1 MB table, so every tap is its own cache
		   line.  Transposing makes each phase contiguous; hshift() interpolates
		   between phases hidx and hidx+1, hence R+1 of them. */
		int p, m;
		const int R = a->R, rsize = a->rsize;
		double* h = fir_bandpass(a->ncoef, fc_norm_low, fc_norm_high, (double)R, 1, 0, (double)R * a->gain);
		// every element is written below, so skip malloc0()'s memset of ~1 MB
		a->hp = (double *)_aligned_malloc ((size_t)(R + 1) * rsize * sizeof (double), 16);
		// walk h forward (p is its fast axis) so the prefetcher sees a linear
		// stream; h is cold here, straight from fir_bandpass()
		for (m = 0; m < rsize; m++)
			for (p = 0; p <= R; p++)
				a->hp[(size_t)p * rsize + m] = h[p + (size_t)m * R];
		_aligned_free (h);
	}
	a->ringI = (double *)malloc0(a->rsize * sizeof(double));
	a->ringQ = (double *)malloc0(a->rsize * sizeof(double));
	a->idx_in = a->rsize - 1;
	a->h_offset = 0.0;
	a->hs = (double *)malloc0 (a->rsize * sizeof (double));
	a->isamps = 0.0;
}

void decalc_varsamp (VARSAMP a)
{
	_aligned_free (a->hs);
	_aligned_free (a->ringQ);
	_aligned_free (a->ringI);
	_aligned_free (a->hp);
}

VARSAMP create_varsamp ( int run, int size, double* in, double* out,
	int in_rate, int out_rate, double fc, double fc_low, int R, double gain, double var, int varmode)
{
	VARSAMP a = (VARSAMP) malloc0 (sizeof (varsamp));
	
	a->run = run;
	a->size = size;
	a->in = in;
	a->out = out;
	a->in_rate = in_rate;
	a->out_rate = out_rate;
	a->fcin = fc;
	a->fc_low = fc_low;
	a->R = R;
	a->gain = gain;
	a->var = var;
	a->varmode = varmode;
	calc_varsamp (a);
	return a;
}

void destroy_varsamp (VARSAMP a)
{
	decalc_varsamp (a);
	_aligned_free (a);
}

void flush_varsamp (VARSAMP a)
{
	memset (a->ringI, 0, a->rsize * sizeof (double));
	memset (a->ringQ, 0, a->rsize * sizeof (double));
	a->idx_in = a->rsize - 1;
	a->h_offset = 0.0;
	a->isamps = 0.0;
}

/* Accumulate n taps of a unit-stride complex dot product into *pI / *pQ.

   Four independent accumulator pairs keep the FMAs off a single dependency
   chain and let the vectorizer in: an 'I += h[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 varsamp_dot (const double* WDSP_RESTRICT hp,
	const double* WDSP_RESTRICT xI, const double* WDSP_RESTRICT xQ,
	int n, double* pI, double* pQ)
{
	double i0 = 0.0, i1 = 0.0, i2 = 0.0, i3 = 0.0;
	double q0 = 0.0, q1 = 0.0, q2 = 0.0, q3 = 0.0;
	int j = 0;
	for (; j <= n - 4; j += 4)
	{
		i0 += hp[j + 0] * xI[j + 0];	q0 += hp[j + 0] * xQ[j + 0];
		i1 += hp[j + 1] * xI[j + 1];	q1 += hp[j + 1] * xQ[j + 1];
		i2 += hp[j + 2] * xI[j + 2];	q2 += hp[j + 2] * xQ[j + 2];
		i3 += hp[j + 3] * xI[j + 3];	q3 += hp[j + 3] * xQ[j + 3];
	}
	for (; j < n; j++)
	{
		i0 += hp[j] * xI[j];
		q0 += hp[j] * xQ[j];
	}
	*pI += (i0 + i1) + (i2 + i3);
	*pQ += (q0 + q1) + (q2 + q3);
}

void hshift (VARSAMP a)
{
	int m;
	int hidx;
	double frac, pos;
	const int rsize = a->rsize;
	const double* WDSP_RESTRICT h0;
	const double* WDSP_RESTRICT h1;
	double* WDSP_RESTRICT hs = a->hs;
	/* h_offset is normalized to [0,1) by the caller, so hidx is in [0, R-1]
	   and phase hidx+1 <= R exists. */
	pos = (double)a->R * a->h_offset;
	hidx = (int)(pos);
	frac = pos - (double)hidx;
	h0 = a->hp + (size_t)hidx * rsize;
	h1 = h0 + rsize;
	for (m = 0; m < rsize; m++)
		hs[rsize - 1 - m] = h0[m] + frac * (h1[m] - h0[m]);
}

int xvarsamp (VARSAMP a, double var)
{
	int outsamps = 0;
	uint64_t* picvar;
	uint64_t N;
	a->var = var;
	a->old_inv_cvar = a->inv_cvar;
	a->cvar = a->var * a->nom_ratio;
	a->inv_cvar = 1.0 / a->cvar;
	if (a->varmode)
	{
		a->dicvar = (a->inv_cvar - a->old_inv_cvar) / (double)a->size;
		a->inv_cvar = a->old_inv_cvar;
	}
	else			a->dicvar = 0.0;
	if (a->run)
	{
		int i, n1;
		double I, Q;
		const int rsize = a->rsize;
		/* a->hs is rewritten by hshift() on every output sample, so it must not
		   be hoisted behind a restrict pointer here; varsamp_dot() re-reads it. */
		const double* in = a->in;
		double* out = a->out;
		double* WDSP_RESTRICT ringI = a->ringI;
		double* WDSP_RESTRICT ringQ = a->ringQ;
		int idx_in = a->idx_in;

		for (i = 0; i < a->size; i++)
		{
			ringI[idx_in] = in[2 * i + 0];
			ringQ[idx_in] = in[2 * i + 1];
			a->inv_cvar += a->dicvar;
			picvar = (uint64_t*)(&a->inv_cvar);
			N = *picvar & 0xffffffffffff0000;
			a->inv_cvar = *((double *)&N);
			a->delta = 1.0 - a->inv_cvar;
			while (a->isamps < 1.0)
			{
				I = 0.0;
				Q = 0.0;
				hshift (a);
				a->h_offset += a->delta;
				while (a->h_offset >= 1.0) a->h_offset -= 1.0;
				while (a->h_offset <  0.0) a->h_offset += 1.0;
				/* the ring wraps at most once over rsize taps; split it so both
				   halves are unit-stride */
				n1 = rsize - idx_in;
				varsamp_dot (a->hs, ringI + idx_in, ringQ + idx_in, n1, &I, &Q);
				if (n1 < rsize)
					varsamp_dot (a->hs + n1, ringI, ringQ, rsize - n1, &I, &Q);
				out[2 * outsamps + 0] = I;
				out[2 * outsamps + 1] = Q;
				outsamps++;
				a->isamps += a->inv_cvar;
			}
			a->isamps -= 1.0;
			if (--idx_in < 0) idx_in = rsize - 1;
		}
		a->idx_in = idx_in;
	}
	else if (a->in != a->out)
		memcpy (a->out, a->in, a->size * sizeof (complex));
	return outsamps;
}

void setBuffers_varsamp (VARSAMP a, double* in, double* out)
{
	a->in = in;
	a->out = out;
}

void setSize_varsamp (VARSAMP a, int size)
{
	a->size = size;
	flush_varsamp (a);
}

void setInRate_varsamp (VARSAMP a, int rate)
{
	decalc_varsamp (a);
	a->in_rate = rate;
	calc_varsamp (a);
}

void setOutRate_varsamp (VARSAMP a, int rate)
{
	decalc_varsamp (a);
	a->out_rate = rate;
	calc_varsamp (a);
}

void setFCLow_varsamp (VARSAMP a, double fc_low)
{
	if (fc_low != a->fc_low)
	{
		decalc_varsamp (a);
		a->fc_low = fc_low;
		calc_varsamp (a);
	}
}

void setBandwidth_varsamp (VARSAMP a, double fc_low, double fc_high)
{
	if (fc_low != a->fc_low || fc_high != a->fcin)
	{
		decalc_varsamp (a);
		a->fc_low = fc_low;
		a->fcin = fc_high;
		calc_varsamp (a);
	}
}

// exported calls

PORT
void* create_varsampV (int in_rate, int out_rate, int R)
{
	return (void *)create_varsamp (1, 0, 0, 0, in_rate, out_rate, 0.0, -1.0, R, 1.0, 1.0, 1);
}

PORT
void xvarsampV (double* input, double* output, int numsamps, double var, int* outsamps, void* ptr)
{
	VARSAMP a = (VARSAMP)ptr;
	a->in = input;
	a->out = output;
	a->size = numsamps;
	*outsamps = xvarsamp(a, var);
}

PORT
void destroy_varsampV (void* ptr)
{
	destroy_varsamp ( (VARSAMP)ptr );
}