/*	wfmd.c

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

Copyright (C) 2013, 2023 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_wfmd (WFMD a)
{
	// discriminator
	a->pre_i = 0.0;
	a->pre_q = 0.0;
	a->again = a->rate / (a->deviation * TWOPI);
	// dc removal
	a->mtau = exp(-1.0 / (a->rate * a->tau));
	a->onem_mtau = 1.0 - a->mtau;
	a->fmdc = 0.0;
	// de-emphasis
	if (a->tau_de > 0.0) a->mde = exp(-1.0 / (a->rate * a->tau_de));
	else				 a->mde = 0.0;
	a->onem_mde = 1.0 - a->mde;
	a->deemph_z = 0.0;
	// detector limiter
	a->plim = create_wcpagc (
		1,											// run - always ON
		5,											// mode
		1,											// 0 for max(I,Q), 1 for envelope
		a->out,										// input buff pointer
		a->out,										// output buff pointer
		a->size,									// io_buffsize
		(int)a->rate,								// sample rate
		0.001,										// tau_attack
		0.008,										// tau_decay
		4,											// n_tau
		a->lim_gain,								// max_gain (sets threshold, initial value)
		1.0,										// var_gain / slope
		1.0,										// fixed_gain
		1.0,										// max_input
		0.9,										// out_targ
		0.250,										// tau_fast_backaverage
		0.004,										// tau_fast_decay
		4.0,										// pop_ratio
		0,											// hang_enable
		0.500,										// tau_hang_backmult
		0.500,										// hangtime
		2.000,										// hang_thresh
		0.100);										// tau_hang_decay
}

void decalc_wfmd (WFMD a)
{
	destroy_wcpagc(a->plim);
}

WFMD create_wfmd (int run, int size, double* in, double* out, int rate, double deviation, double f_low, double f_high,
	double tau, int deemph_run, double tau_de, double afgain, int nc_aud, int mp_aud)
{
	WFMD a = (WFMD) malloc0 (sizeof (wfmd));
	double* impulse;
	a->run = run;
	a->size = size;
	a->in = in;
	a->out = out;
	a->rate = (double)rate;
	a->deviation = deviation;
	a->f_low = f_low;
	a->f_high = f_high;
	a->tau = tau;
	a->deemph_run = deemph_run;
	a->tau_de = tau_de;
	a->afgain = afgain;
	a->nc_aud = nc_aud;
	a->mp_aud = mp_aud;
	a->lim_run = 0;
	a->lim_pre_gain = 0.4;
	a->lim_gain = 2.5;
	calc_wfmd (a);
	a->audio = (double *) malloc0 (a->size * sizeof (complex));
	// audio filter
	impulse = fir_bandpass(a->nc_aud, 0.8 * a->f_low, 1.1 * a->f_high, a->rate, 0, 1, a->afgain / (2.0 * a->size));
	a->paud = create_fircore (a->size, a->audio, a->out, a->nc_aud, a->mp_aud, impulse);
	_aligned_free (impulse);
	return a;
}

void destroy_wfmd (WFMD a)
{
	destroy_fircore (a->paud);
	_aligned_free (a->audio);
	decalc_wfmd (a);
	_aligned_free (a);
}

void flush_wfmd (WFMD a)
{
	memset (a->audio, 0, a->size * sizeof (complex));
	flush_fircore (a->paud);
	a->pre_i = 0.0;
	a->pre_q = 0.0;
	a->fmdc = 0.0;
	a->deemph_z = 0.0;
	flush_wcpagc (a->plim);
}

void xwfmd (WFMD a)
{
	if (a->run)
	{
		int i;
		double si, sq, cr, ci, det, aud;
		for (i = 0; i < a->size; i++)
		{
			// quadrature discriminator:  det = arg (x[n] * conj (x[n-1]))
			si = a->in[2 * i + 0];
			sq = a->in[2 * i + 1];
			cr = + si * a->pre_i + sq * a->pre_q;
			ci = - si * a->pre_q + sq * a->pre_i;
			a->pre_i = si;
			a->pre_q = sq;
			det = atan2 (ci, cr);
			// dc removal, gain, & demod output
			a->fmdc = a->mtau * a->fmdc + a->onem_mtau * det;
			aud = a->again * (det - a->fmdc);
			// de-emphasis
			if (a->deemph_run)
			{
				a->deemph_z = a->mde * a->deemph_z + a->onem_mde * aud;
				aud = a->deemph_z;
			}
			a->audio[2 * i + 0] = aud;
			a->audio[2 * i + 1] = aud;
		}
		// audio filter
		xfircore (a->paud);
		if (a->lim_run)
		{
			for (i = 0; i < 2 * a->size; i++)
				a->out[i] *= a->lim_pre_gain;
			xwcpagc (a->plim);
		}
	}
	else if (a->in != a->out)
		memcpy (a->out, a->in, a->size * sizeof (complex));
}

void setBuffers_wfmd (WFMD a, double* in, double* out)
{
	decalc_wfmd (a);
	a->in = in;
	a->out = out;
	calc_wfmd (a);
	setBuffers_fircore (a->paud, a->audio, a->out);
	setBuffers_wcpagc (a->plim, a->out, a->out);
}

void setSamplerate_wfmd (WFMD a, int rate)
{
	double* impulse;
	decalc_wfmd (a);
	a->rate = rate;
	calc_wfmd (a);
	// audio filter
	impulse = fir_bandpass(a->nc_aud, 0.8 * a->f_low, 1.1 * a->f_high, a->rate, 0, 1, a->afgain / (2.0 * a->size));
	setImpulse_fircore (a->paud, impulse, 1);
	_aligned_free (impulse);
	setSamplerate_wcpagc (a->plim, (int)a->rate);
}

void setSize_wfmd (WFMD a, int size)
{
	double* impulse;
	decalc_wfmd (a);
	_aligned_free (a->audio);
	a->size = size;
	calc_wfmd (a);
	a->audio = (double *) malloc0 (a->size * sizeof (complex));
	// audio filter
	destroy_fircore (a->paud);
	impulse = fir_bandpass(a->nc_aud, 0.8 * a->f_low, 1.1 * a->f_high, a->rate, 0, 1, a->afgain / (2.0 * a->size));
	a->paud = create_fircore (a->size, a->audio, a->out, a->nc_aud, a->mp_aud, impulse);
	_aligned_free (impulse);
	setSize_wcpagc (a->plim, a->size);
}

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

PORT
void SetRXAWFMDeviation (int channel, double deviation)
{
	WFMD a;
	EnterCriticalSection (&ch[channel].csDSP);
	a = rxa[channel].wfmd.p;
	a->deviation = deviation;
	a->again = a->rate / (a->deviation * TWOPI);
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void SetRXAWFMNCaud (int channel, int nc)
{
	WFMD a;
	double* impulse;
	EnterCriticalSection (&ch[channel].csDSP);
	a = rxa[channel].wfmd.p;
	if (a->nc_aud != nc)
	{
		a->nc_aud = nc;
		impulse = fir_bandpass(a->nc_aud, 0.8 * a->f_low, 1.1 * a->f_high, a->rate, 0, 1, a->afgain / (2.0 * a->size));
		setNc_fircore (a->paud, a->nc_aud, impulse);
		_aligned_free (impulse);
	}
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void SetRXAWFMMPaud (int channel, int mp)
{
	WFMD a;
	a = rxa[channel].wfmd.p;
	if (a->mp_aud != mp)
	{
		a->mp_aud = mp;
		setMp_fircore (a->paud, a->mp_aud);
	}
}

PORT
void SetRXAWFMAFFilter (int channel, double low, double high)
{
	WFMD a = rxa[channel].wfmd.p;
	double* impulse;
	EnterCriticalSection (&ch[channel].csDSP);
	if (a->f_low != low || a->f_high != high)
	{
		a->f_low = low;
		a->f_high = high;
		impulse = fir_bandpass (a->nc_aud, 0.8 * a->f_low, 1.1 * a->f_high, a->rate, 0, 1, a->afgain / (2.0 * a->size));
		setImpulse_fircore (a->paud, impulse, 1);
		_aligned_free (impulse);
	}
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void SetRXAWFMDeemphRun (int channel, int run)
{
	WFMD a = rxa[channel].wfmd.p;
	EnterCriticalSection (&ch[channel].csDSP);
	if (a->deemph_run != run)
	{
		a->deemph_run = run;
		a->deemph_z = 0.0;
	}
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void SetRXAWFMDeemphTau (int channel, double tau)
{
	WFMD a = rxa[channel].wfmd.p;
	EnterCriticalSection (&ch[channel].csDSP);
	if (a->tau_de != tau && tau > 0.0)
	{
		a->tau_de = tau;
		a->mde = exp(-1.0 / (a->rate * a->tau_de));
		a->onem_mde = 1.0 - a->mde;
		a->deemph_z = 0.0;
	}
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void SetRXAWFMLimRun (int channel, int run)
{
	WFMD a = rxa[channel].wfmd.p;
	EnterCriticalSection (&ch[channel].csDSP);
	if (a->lim_run != run)
	{
		a->lim_run = run;
	}
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void SetRXAWFMLimGain (int channel, double gaindB)
{
	double gain = pow(10.0, gaindB / 20.0);
	WFMD a = rxa[channel].wfmd.p;
	EnterCriticalSection (&ch[channel].csDSP);
	if (a->lim_gain != gain)
	{
		decalc_wfmd (a);
		a->lim_gain = gain;
		calc_wfmd (a);
	}
	LeaveCriticalSection (&ch[channel].csDSP);
}