/*	resample.c

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

Copyright (C) 2013, 2025 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"

/************************************************************************************************
*																								*
*							  VERSION FOR COMPLEX DOUBLE-PRECISION								*
*																								*
************************************************************************************************/

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

   Four independent accumulator pairs are carried so the FMAs are not serialized
   on a single dependency chain, and so the compiler is free to vectorize: a
   plain '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).  Summation order therefore differs from a strict left-to-right
   reduction, at the usual pairwise-summation accuracy gain. */
static inline void resample_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 calc_resample (RESAMPLE a)
{
	int x, y, z;
	int i, j, k;
	int min_rate;
	double full_rate;
	double fc_norm_high, fc_norm_low;
	double* impulse;
	a->fc = a->fcin;
	a->ncoef = a->ncoefin;
	if ((x = a->in_rate)  <= 0) return;
	if ((y = a->out_rate) <= 0) return;
	while (y != 0)
	{
		z = y;
		y = x % y;
		x = z;
	}
	a->L = a->out_rate / x;
	a->M = a->in_rate / x;
	if (a->in_rate < a->out_rate) min_rate = a->in_rate;
	else min_rate = a->out_rate;
	if (a->fc == 0.0) a->fc = 0.45 * (double)min_rate;
	full_rate = (double)(a->in_rate * a->L);
	fc_norm_high = a->fc / full_rate;
	if (a->fc_low < 0.0)
		fc_norm_low = - fc_norm_high;
	else
		fc_norm_low = a->fc_low / full_rate;
	if (a->ncoef == 0) a->ncoef = (int)(140.0 * full_rate / min_rate);
	a->ncoef = (a->ncoef / a->L + 1) * a->L;
	a->cpp = a->ncoef / a->L;
	a->h = (double *)malloc0(a->ncoef * sizeof(double));
	impulse = fir_bandpass(a->ncoef, fc_norm_low, fc_norm_high, 1.0, 1, 0, a->gain * (double)a->L);
	i = 0;
	for (j = 0; j < a->L; j++)
		for (k = 0; k < a->ncoef; k += a->L)
			a->h[i++] = impulse[j + k];
	a->ringsize = a->cpp;
	a->ringI = (double *)malloc0(a->ringsize * sizeof(double));
	a->ringQ = (double *)malloc0(a->ringsize * sizeof(double));
	a->idx_in = a->ringsize - 1;
	a->phnum = 0;
	_aligned_free(impulse);
}

void decalc_resample (RESAMPLE a)
{
	_aligned_free(a->ringQ);
	_aligned_free(a->ringI);
	_aligned_free(a->h);
}

PORT
RESAMPLE create_resample ( int run, int size, double* in, double* out, int in_rate, int out_rate, double fc, int ncoef, double gain)
{
	RESAMPLE a = (RESAMPLE) malloc0 (sizeof (resample));
	
	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 = -1.0;		// could add to create_resample() parameters
	a->ncoefin = ncoef;
	a->gain = gain;
	calc_resample (a);
	return a;
}

PORT
void destroy_resample (RESAMPLE a)
{
	decalc_resample (a);
	_aligned_free (a);
}

PORT
void flush_resample (RESAMPLE a)
{
	memset (a->ringI, 0, a->ringsize * sizeof (double));
	memset (a->ringQ, 0, a->ringsize * sizeof (double));
	a->idx_in = a->ringsize - 1;
	a->phnum = 0;
}

PORT
int xresample (RESAMPLE a)
{
	int outsamps = 0;
	if (a->run)
	{
		int i, n1;
		double I, Q;

		const int cpp = a->cpp;
		const int ringsize = a->ringsize;
		const int L = a->L;
		const int M = a->M;
		const int size = a->size;
		const double* WDSP_RESTRICT h = a->h;
		const double* WDSP_RESTRICT in = a->in;
		double* WDSP_RESTRICT ringI = a->ringI;
		double* WDSP_RESTRICT ringQ = a->ringQ;
		double* WDSP_RESTRICT out = a->out;
		int idx_in = a->idx_in;
		int phnum = a->phnum;

		for (i = 0; i < size; i++)
		{
			ringI[idx_in] = in[2 * i + 0];
			ringQ[idx_in] = in[2 * i + 1];
			while (phnum < L)
			{
				const double* WDSP_RESTRICT hp = h + cpp * phnum;
				/* The tap loop walks the ring forward from idx_in and wraps at
				   most once.  Split it at the wrap point so both halves are
				   unit-stride: the wrap test that used to sit inside the loop
				   made the address non-affine and blocked vectorization. */
				if ((n1 = ringsize - idx_in) > cpp) n1 = cpp;
				I = 0.0;
				Q = 0.0;
				resample_dot (hp, ringI + idx_in, ringQ + idx_in, n1, &I, &Q);
				if (n1 < cpp)
					resample_dot (hp + n1, ringI, ringQ, cpp - n1, &I, &Q);
				out[2 * outsamps + 0] = I;
				out[2 * outsamps + 1] = Q;
				outsamps++;
				phnum += M;
			}
			phnum -= L;
			if (--idx_in < 0) idx_in = ringsize - 1;
		}
		a->idx_in = idx_in;
		a->phnum = phnum;
	}
	else if (a->in != a->out)
		memcpy (a->out, a->in, a->size * sizeof (complex));
	return outsamps;
}

void setBuffers_resample(RESAMPLE a, double* in, double* out)
{
	a->in = in;
	a->out = out;
}

void setSize_resample(RESAMPLE a, int size)
{
	a->size = size;
	flush_resample (a);
}

void setInRate_resample(RESAMPLE a, int rate)
{
	decalc_resample (a);
	a->in_rate = rate;
	calc_resample (a);
}

void setOutRate_resample(RESAMPLE a, int rate)
{
	decalc_resample (a);
	a->out_rate = rate;
	calc_resample (a);
}

void setFCLow_resample (RESAMPLE a, double fc_low)
{
	if (fc_low != a->fc_low)
	{
		decalc_resample (a);
		a->fc_low = fc_low;
		calc_resample (a);
	}
}

void setBandwidth_resample (RESAMPLE a, double fc_low, double fc_high)
{
	if (fc_low != a->fc_low || fc_high != a->fcin)
	{
		decalc_resample (a);
		a->fc_low = fc_low;
		a->fcin = fc_high;
		calc_resample (a);
	}
}

// exported calls

PORT
void* create_resampleV (int in_rate, int out_rate)
{
	return (void *)create_resample (1, 0, 0, 0, in_rate, out_rate, 0.0, 0, 1.0);
}

PORT
void xresampleV (double* input, double* output, int numsamps, int* outsamps, void* ptr)
{
	RESAMPLE a = (RESAMPLE)ptr;
	a->in = input;
	a->out = output;
	a->size = numsamps;
	*outsamps = xresample(a);
}

PORT
void destroy_resampleV (void* ptr)
{
	destroy_resample ( (RESAMPLE)ptr );
}

/************************************************************************************************
*																								*
*							  VERSION FOR NON-COMPLEX FLOATS									*
*																								*
************************************************************************************************/

/* Real-valued counterpart of resample_dot(). */
static inline void resampleF_dot (const double* WDSP_RESTRICT hp,
	const double* WDSP_RESTRICT x, int n, double* pI)
{
	double i0 = 0.0, i1 = 0.0, i2 = 0.0, i3 = 0.0;
	int j = 0;
	for (; j <= n - 4; j += 4)
	{
		i0 += hp[j + 0] * x[j + 0];
		i1 += hp[j + 1] * x[j + 1];
		i2 += hp[j + 2] * x[j + 2];
		i3 += hp[j + 3] * x[j + 3];
	}
	for (; j < n; j++)
		i0 += hp[j] * x[j];
	*pI += (i0 + i1) + (i2 + i3);
}

RESAMPLEF create_resampleF ( int run, int size, float* in, float* out, int in_rate, int out_rate)
{
	RESAMPLEF a = (RESAMPLEF) malloc0 (sizeof (resampleF));
	int x, y, z;
	int i, j, k;
	int min_rate;
	double full_rate;
	double fc;
	double fc_norm;
	double* impulse;
	a->run = run;
	a->size = size;
	a->in = in;
	a->out = out;
	if ((x = in_rate)  <= 0) return 0;
	if ((y = out_rate) <= 0) return 0;
	while (y != 0)
	{
		z = y;
		y = x % y;
		x = z;
	}
	a->L = out_rate / x;
	a->M = in_rate / x;
	if (in_rate < out_rate) min_rate = in_rate;
	else min_rate = out_rate;
	fc = 0.45 * (double)min_rate;
	full_rate = (double)(in_rate * a->L);
	fc_norm = fc / full_rate;
	a->ncoef = (int)(60.0 / fc_norm);
	a->ncoef = (a->ncoef / a->L + 1) * a->L;
	a->cpp = a->ncoef / a->L;
	a->h = (double *) malloc0 (a->ncoef * sizeof (double));
	impulse = fir_bandpass (a->ncoef, -fc_norm, +fc_norm, 1.0, 1, 0, (double)a->L);
	i = 0;
	for (j = 0; j < a->L; j ++)
		for (k = 0; k < a->ncoef; k += a->L)
			a->h[i++] = impulse[j + k];
	a->ringsize = a->cpp;
	a->ring = (double *) malloc0 (a->ringsize * sizeof (double));
	a->idx_in = a->ringsize - 1;
	a->phnum = 0;
	_aligned_free (impulse);
	return a;
}

void destroy_resampleF (RESAMPLEF a)
{
	_aligned_free (a->ring);
	_aligned_free (a->h);
	_aligned_free (a);
}

void flush_resampleF (RESAMPLEF a)
{
	memset (a->ring, 0, a->ringsize * sizeof (double));
	a->idx_in = a->ringsize - 1;
	a->phnum = 0;
}

int xresampleF (RESAMPLEF a)
{
	int outsamps = 0;
	if (a->run)
	{
		int i;
		double I;

		const int cpp = a->cpp;
		const int ringsize = a->ringsize;
		const int L = a->L;
		const int M = a->M;
		const int size = a->size;
		const double* WDSP_RESTRICT h = a->h;
		const float* WDSP_RESTRICT in = a->in;
		double* WDSP_RESTRICT ring = a->ring;
		float* WDSP_RESTRICT out = a->out;
		int idx_in = a->idx_in;
		int phnum = a->phnum;
		int n1;

		for (i = 0; i < size; i++)
		{
			ring[idx_in] = (double)in[i];

			while (phnum < L)
			{
				const double* WDSP_RESTRICT hp = h + cpp * phnum;
				/* see resample_dot(): split at the ring wrap so both halves are
				   unit-stride, and carry independent accumulators */
				if ((n1 = ringsize - idx_in) > cpp) n1 = cpp;
				I = 0.0;
				resampleF_dot (hp, ring + idx_in, n1, &I);
				if (n1 < cpp)
					resampleF_dot (hp + n1, ring, cpp - n1, &I);
				out[outsamps] = (float)I;

				outsamps++;
				phnum += M;
			}
			phnum -= L;
			if (--idx_in < 0) idx_in = ringsize - 1;
		}
		a->idx_in = idx_in;
		a->phnum = phnum;
	}
	else if (a->in != a->out)
		memcpy (a->out, a->in, a->size * sizeof (float));
	return outsamps;
}

// Exported calls

PORT
void* create_resampleFV (int in_rate, int out_rate)
{
	return (void *)create_resampleF (1, 0, 0, 0, in_rate, out_rate);
}

PORT
void xresampleFV (float* input, float* output, int numsamps, int* outsamps, void* ptr)
{
	RESAMPLEF a = (RESAMPLEF)ptr;
	a->in = input;
	a->out = output;
	a->size = numsamps;
	*outsamps = xresampleF(a);
}

PORT
void destroy_resampleFV (void* ptr)
{
	destroy_resampleF ( (RESAMPLEF)ptr );
}