/*****************************************************************************/

/*
 *      audio.c  --  Audio processing for "virtual transceiver".
 *
 *      Copyright (C) 1999, 2001  Thomas Sailer (t.sailer@alumni.ethz.ch)
 *
 *      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., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *  Please note that the GPL allows you to use the driver, NOT the radio.
 *  In order to use the radio, you need a license from the communications
 *  authority of your country.
 *
 */

/*****************************************************************************/

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

/* AIX requires this to be the first thing in the file.  */
#ifndef __GNUC__
# if HAVE_ALLOCA_H
#  include <alloca.h>
# else
#  ifdef _AIX
#pragma alloca
#  else
#   ifndef alloca /* predefined by HP cc +Olibcalls */
char *alloca ();
#   endif
#  endif
# endif
#endif

#include <math.h>
#include "trx.h"
#include "simd.h"

/* ---------------------------------------------------------------------- */

#define PWRINTERVAL     4096         /* must be even */
#define PWRINTERVAL2      64         /* must be even */
#define PWRNOISECALIB   4096

#define RCOSALPHA 0.4
//#define FILTERRELAX 1.4
#define FILTERRELAX 1.0

#define MAXSAMPLERATE  0.5  /* relative to AUDIOSAMPLERATE */

/* --------------------------------------------------------------------- */

static inline double sinc(double x)
{
        double arg = x * M_PI;

        if (fabs(arg) < 1e-10)
                return 1;
        return sin(arg) / arg;
}

static inline double hamming(double x)
{
        return 0.54-0.46*cos(2*M_PI*x);
}

static void compute_filter(double tmul, int16_t filter[AUDIONUMFILTER][AUDIOFILTERLEN])
{
	int i, j;
        float f1, f2, time, alphatime;
        float c[AUDIONUMFILTER * AUDIOFILTERLEN];

	if (tmul > MAXSAMPLERATE)
		tmul = MAXSAMPLERATE;
#if 0
	tmul *= (FILTERRELAX / AUDIONUMFILTER);
        for (i = 0; i < AUDIONUMFILTER * AUDIOFILTERLEN; i++) {
                f1 = i - ((AUDIONUMFILTER * AUDIOFILTERLEN - 1.0) / 2.0);
                f1 *= tmul;
                f2 = i * (1.0 / (AUDIONUMFILTER * AUDIOFILTERLEN - 1.0));
                c[i] = sinc(f1) * hamming(f2);
        }
#else
	tmul *= (1.0 / AUDIONUMFILTER);
        for (i = 0; i < AUDIONUMFILTER * AUDIOFILTERLEN; i++) {
                time = i - ((AUDIONUMFILTER * AUDIOFILTERLEN - 1.0) / 2.0);
                time *= tmul;
                alphatime = time * RCOSALPHA;
                f1 = 1 - 4 * alphatime * alphatime;
                if (fabs(f1) < 1e-10)
                        f2 = M_PI * (1.0 / 8.0) * sin(M_PI * alphatime) / alphatime;
                else
                        f2 = cos(M_PI * alphatime) / f1;
                c[i] = sinc(time) * f2;
        }
#endif
        f1 = 0;
        for (i = 0; i < AUDIONUMFILTER; i++) {
                for (f2 = 0, j = i; j < AUDIONUMFILTER * AUDIOFILTERLEN; j += AUDIONUMFILTER)
                        f2 += fabs(c[j]);
                if (f2 > f1)
                        f1 = f2;
        }
        f1 = 32767.0 / f1;
        for (i = 0; i < AUDIONUMFILTER; i++)
                for (j = 0; j < AUDIOFILTERLEN; j++)
                        filter[i][AUDIOFILTERLEN-1-j] = f1 * c[j * AUDIONUMFILTER + i];
}

/* ---------------------------------------------------------------------- */

#if 0
static inline int16_t fir(const int16_t *p1, const int16_t *p2, int len)
{
        int32_t sum = 0;

        for(; len > 0; len--, p1++, p2++)
                sum += ((int32_t)*p1) * ((int32_t)*p2);
        return sum >> 16;
}
#endif

/* ---------------------------------------------------------------------- */

void audioproc_init(struct trx_thread_state *state)
{
	if (state->cfg.adapt.rfsquelch <= RSSI_MIN)
		state->audioparam.rfsquelch = 0;
	else if (state->cfg.adapt.rfsquelch >= RSSI_MAX)
		state->audioparam.rfsquelch = 255;
	else
		state->audioparam.rfsquelch = (state->cfg.adapt.rfsquelch - RSSI_MIN) * (255.0/(RSSI_MAX-RSSI_MIN));
	/* init input sampling rate converter */
	memset(&state->du.a.isrc, 0, sizeof(state->du.a.isrc));
	state->du.a.isrc.phinc = ((1 << AUDIOSRCPHASEFRAC) * state->du.a.p.sratedspin + state->du.a.p.srateusb / 2) / state->du.a.p.srateusb;
	lprintf(2, "rec phase inc: 0x%05x\n", state->du.a.isrc.phinc);
	compute_filter(state->du.a.p.srateusb / (double)state->du.a.p.sratedspin, state->du.a.isrc.filter);
	/* init output sampling rate converter */
	memset(&state->du.a.osrc, 0, sizeof(state->du.a.osrc));
	state->du.a.osrc.phinc = ((1 << AUDIOSRCPHASEFRAC) * state->du.a.p.srateusb + state->du.a.p.sratedspout / 2) / state->du.a.p.sratedspout;
	lprintf(2, "play phase inc: 0x%05x\n", state->du.a.osrc.phinc);
	compute_filter(state->du.a.p.sratedspout / (double)state->du.a.p.srateusb, state->du.a.osrc.filter);
}

unsigned int audioproc_convertinput_isamples(struct trx_thread_state *state, unsigned int osamples)
{
	return (osamples * state->du.a.isrc.phinc + (state->du.a.isrc.ph & AUDIOSRCPHASEMASK)) >> AUDIOSRCPHASEFRAC;
}

void audioproc_convertinput(struct trx_thread_state *state, unsigned int inum, unsigned int onum, const int16_t *ibuf, signed char *obuf)
{
	int16_t *ib;
	unsigned int i;
	float pw;
	int s;
	
	/* handle overlap!! */
	for (i = 0; i < inum; i++) {
		state->du.a.isrc.pwracc += ibuf[i] * ibuf[i];
		if ((++state->du.a.isrc.pwrcnt) >= PWRINTERVAL) {
			pw = state->du.a.isrc.pwracc;
			state->du.a.isrc.pwracc = 0;
			state->du.a.isrc.pwrcnt = 0;
			pw *= (1.0 / PWRINTERVAL);
			if (pw < 1.6384)
				pw = -40;
			else
				pw = (10.0 / M_LN10) * log(pw) - 42.144;
			state->du.a.isrc.sigpwr = pw;
		}
	}
	/* handle overlap */
	ib = alloca((inum + AUDIOOVERLAP) * sizeof(ib[0]));
	memcpy(ib, state->du.a.isrc.sample, AUDIOOVERLAP * sizeof(ib[0]));
	memcpy(ib + AUDIOOVERLAP, ibuf, inum * sizeof(ib[0]));
	memcpy(state->du.a.isrc.sample, ib + inum, AUDIOOVERLAP * sizeof(ib[0]));
	/* output filtering */
	state->du.a.isrc.ph &= AUDIOSRCPHASEMASK;
	simdpreparefpu();
	for (i = 0; i < onum; i++) {
		s = simdfir16(state->du.a.isrc.filter[AUDIOFILTERIDX(state->du.a.isrc.ph)], ib+AUDIOFILTERSAMPLE(state->du.a.isrc.ph), AUDIOFILTERLEN) >> 16;
		s += SIN(state->du.a.isrc.ph0) >> 1;
		s += SIN(state->du.a.isrc.ph1) >> 1;
		s >>= 8;
		if (s > 127)
			s = 127;
		if (s < -128)
			s = -128;
		state->du.a.isrc.abuf[state->du.a.isrc.aptr] = s;
		state->du.a.isrc.aptr = (state->du.a.isrc.aptr + 1) % (sizeof(state->du.a.isrc.abuf) / sizeof(state->du.a.isrc.abuf[0]));
		obuf[i] = s;
		state->du.a.isrc.ph += state->du.a.isrc.phinc;
		state->du.a.isrc.ph0 += state->du.a.isrc.freq0;
		state->du.a.isrc.ph1 += state->du.a.isrc.freq1;
	}
	simdpreparefpu();
}

int audioproc_adjustinput(struct trx_thread_state *state, unsigned int usbdelay)
{
	if (usbdelay > SNDLATENCY) {
		state->du.a.isrc.phinc++;
		state->du.a.isrc.ph += AUDIOSRCPHASEMASK >> 2;
		if (state->du.a.isrc.ph > AUDIOSRCPHASEMASK)
			state->du.a.isrc.ph = AUDIOSRCPHASEMASK;
	} else if (usbdelay < SNDLATENCY) {
		state->du.a.isrc.phinc--;
		state->du.a.isrc.ph -= AUDIOSRCPHASEMASK >> 2;
		if (state->du.a.isrc.ph > AUDIOSRCPHASEMASK)
			state->du.a.isrc.ph = 0;
	}
	if (state->du.a.isrc.phinc < AUDIOSRCMINPHINC) {
		lprintf(0, "phinc (0x%05x) out of range\n", state->du.a.isrc.phinc);
		state->du.a.isrc.phinc = AUDIOSRCMINPHINC;
	} else if (state->du.a.isrc.phinc > AUDIOSRCMAXPHINC) {
		lprintf(0, "phinc (0x%05x) out of range\n", state->du.a.isrc.phinc);
		state->du.a.isrc.phinc = AUDIOSRCMAXPHINC;
	}
	lprintf(20, "in: usbdelay %d phase increment: 0x%05x\n", usbdelay, state->du.a.isrc.phinc);
	return 0;
}

unsigned int audioproc_convertoutput(struct trx_thread_state *state, unsigned int inum, const signed char *ibuf, int16_t *obuf)
{
	unsigned int cnt = 0;
	unsigned int i;
	float pw1, pw2;
	int s;
	int16_t *ib;

	if (!inum)
		return 0;
	if (!state->audioparam.audiosquelch)
		state->dcd = state->rssi >= state->audioparam.rfsquelch;
	ib = alloca((inum + AUDIOOVERLAP) * sizeof(ib[0]));
	memcpy(ib, state->du.a.osrc.sample, AUDIOOVERLAP * sizeof(ib[0]));
	for (i = 0; i < inum; i++) {
		s = state->du.a.osrc.abuf[state->du.a.osrc.aptr] = ibuf[i];
		state->du.a.osrc.aptr = (state->du.a.osrc.aptr + 1) % (sizeof(state->du.a.osrc.abuf) / sizeof(state->du.a.osrc.abuf[0]));
		ib[i + AUDIOOVERLAP] = s << 8;
		state->du.a.osrc.pwracc1 += s * s;
		switch (state->du.a.osrc.pwrcnt2 & 3) {
		case 0:
			state->du.a.osrc.pwracc2 -= s;
			break;

		case 1:
			state->du.a.osrc.pwracc3 -= s;
			break;

		case 2:
			state->du.a.osrc.pwracc2 += s;
			break;
			
		default:
			state->du.a.osrc.pwracc3 += s;
			break;
		}
		if ((++state->du.a.osrc.pwrcnt2) >= PWRINTERVAL2) {
			state->du.a.osrc.pwracc4 += state->du.a.osrc.pwracc2 * state->du.a.osrc.pwracc2 + state->du.a.osrc.pwracc3 * state->du.a.osrc.pwracc3;
			state->du.a.osrc.pwracc2 = 0;
			state->du.a.osrc.pwracc3 = 0;
			state->du.a.osrc.pwrcnt2 = 0;			
		}
		if ((++state->du.a.osrc.pwrcnt) >= PWRINTERVAL) {
			pw1 = state->du.a.osrc.pwracc1;
			pw2 = state->du.a.osrc.pwracc4;
			state->du.a.osrc.pwracc1 = 0;
			state->du.a.osrc.pwracc4 = 0;
			state->du.a.osrc.pwrcnt = 0;
			pw1 *= (1.0 / PWRINTERVAL);
			pw2 *= (1.0 / PWRINTERVAL / PWRINTERVAL) * PWRNOISECALIB;
			if (pw1 < 1.6384)
				pw1 = -40;
			else
				pw1 = (10.0 / M_LN10) * log(pw1) - 42.144;
			if (pw2 < 1.6384)
				pw2 = -40;
			else
				pw2 = (10.0 / M_LN10) * log(pw2) - 42.144;
			if (pw2 > 0)
				pw2 = 0;
			state->du.a.osrc.sigpwr = pw1;
			state->du.a.osrc.noisepwr = pw2;
			if (state->audioparam.audiosquelch)
				state->dcd = (pw1 > -30) && (pw2 < (pw1 / 2 - 20));
		}
	}
	state->du.a.osrc.ph &= AUDIOSRCPHASEMASK;
	memcpy(state->du.a.osrc.sample, ib + inum, AUDIOOVERLAP * sizeof(ib[0]));
	simdpreparefpu();
	while (state->du.a.osrc.ph < (inum << AUDIOSRCPHASEFRAC)) {
		obuf[cnt] = simdfir16(state->du.a.osrc.filter[AUDIOFILTERIDX(state->du.a.osrc.ph)], ib+AUDIOFILTERSAMPLE(state->du.a.osrc.ph), AUDIOFILTERLEN) >> 16;
		state->du.a.osrc.ph += state->du.a.osrc.phinc;
		cnt++;
	}
	simdpreparefpu();
#if 0
	printf("audiosq %u rfsq %u rssi %u dcd %u\n", state->audioparam.audiosquelch, state->audioparam.rfsquelch, state->rssi, state->dcd);
#endif
	if (!state->dcd)
		memset(obuf, 0, 2*cnt);
	return cnt;
}

int audioproc_adjustoutput(struct trx_thread_state *state, unsigned int dspdelay)
{
	state->du.a.osrc.ph &= AUDIOSRCPHASEMASK;
	if (dspdelay > SNDLATENCY) {
		state->du.a.osrc.phinc++;
		state->du.a.osrc.ph += AUDIOSRCPHASEMASK >> 2;
		if (state->du.a.osrc.ph > AUDIOSRCPHASEMASK)
			state->du.a.osrc.ph = AUDIOSRCPHASEMASK;
	} else if (dspdelay < SNDLATENCY) {
		state->du.a.osrc.phinc--;
		state->du.a.osrc.ph -= AUDIOSRCPHASEMASK >> 2;
		if (state->du.a.osrc.ph > AUDIOSRCPHASEMASK)
			state->du.a.osrc.ph = 0;
	}
	if (state->du.a.osrc.phinc < AUDIOSRCMINPHINC) {
		lprintf(0, "phinc (0x%05x) out of range\n", state->du.a.osrc.phinc);
		state->du.a.osrc.phinc = AUDIOSRCMINPHINC;
	} else if (state->du.a.osrc.phinc > AUDIOSRCMAXPHINC) {
		lprintf(0, "phinc (0x%05x) out of range\n", state->du.a.osrc.phinc);
		state->du.a.osrc.phinc = AUDIOSRCMAXPHINC;
	}
	lprintf(20, "out: dspdelay %d phase increment: 0x%05x\n", dspdelay, state->du.a.osrc.phinc);
	return 0;
}

/* ---------------------------------------------------------------------- */
