#include <stdlib.h>

#include "mfsk.h"
#include "trx.h"
#include "filter.h"
#include "sfft.h"
#include "viterbi.h"
#include "prefs.h"
#include "varicode.h"
#include "interleave.h"
#include "misc.h"

static void recvbit(struct mfsk *m, int bit)
{
	int c;

	m->datashreg = (m->datashreg << 1) | !!bit;

	/* search for "001" */
	if ((m->datashreg & 7) == 1) {
		/* the "1" belongs to the next symbol */
		c = varidec(m->datashreg >> 1);

		if (c != -1)
			trx_put_rx_char(c);

		/* we already received this */
		m->datashreg = 1;
	}
}

static void recvsymbol(struct trx *trx, complex *bins)
{
	struct mfsk *m = (struct mfsk *) trx->modem;
	float tone, sum, met, b[4];
	unsigned char symbols[4];
	int i, j, k;

	/* gray decode and form soft decision samples */
	sum = b[0] = b[1] = b[2] = b[3] = 0.0;
	for (i = 0; i < 16; i++) {
		j = graydecode(i);

		if (trx->reverse)
			k = (NumTones - 1) - i;
		else
			k = i;

		tone = cmod(bins[k + BaseTone]);

		b[0] += (j & 8) ? tone : -tone;
		b[1] += (j & 4) ? tone : -tone;
		b[2] += (j & 2) ? tone : -tone;
		b[3] += (j & 1) ? tone : -tone;

		sum += tone;
	}

	if (sum == 0.0) {
		/* avoid divide by zero */
		symbols[0] = 0.0;
		symbols[1] = 0.0;
		symbols[2] = 0.0;
		symbols[3] = 0.0;
	} else {
		/* shift to range 0...255 */
		symbols[0] = clamp(128.0 + (b[0] / sum * 128.0), 0, 255);
		symbols[1] = clamp(128.0 + (b[1] / sum * 128.0), 0, 255);
		symbols[2] = clamp(128.0 + (b[2] / sum * 128.0), 0, 255);
		symbols[3] = clamp(128.0 + (b[3] / sum * 128.0), 0, 255);
	}

	deinterleave(symbols);

	viterbi27(m->viterbi, symbols);

	if (m->viterbi->datalen > 0) {
		met = (m->viterbi->metric - m->prevmetric) / 20.0;
		m->prevmetric = m->viterbi->metric;

		if (met > 0) {
			met -= m->averagemetric;
			m->averagemetric += met / 8.0;

			trx->metric = m->averagemetric;
		}

		if (trx->squelchon && m->averagemetric < prefs.sqval)
			return;

		for (i = 0; i < m->viterbi->datalen; i++) {
			recvbit(m, m->viterbi->data[i] & 128);
			recvbit(m, m->viterbi->data[i] & 64);
			recvbit(m, m->viterbi->data[i] & 32);
			recvbit(m, m->viterbi->data[i] & 16);
			recvbit(m, m->viterbi->data[i] & 8);
			recvbit(m, m->viterbi->data[i] & 4);
			recvbit(m, m->viterbi->data[i] & 2);
			recvbit(m, m->viterbi->data[i] & 1);
		}
	}
}

static complex mixer(struct trx *trx, complex in)
{
	struct mfsk *m = (struct mfsk *) trx->modem;
	complex z;
	float f;

	f = trx->frequency - (float) BaseTone * SampleRate / SymbolLen;

	z.re = cos(m->phaseacc);
	z.im = sin(m->phaseacc);

	z = cmul(z, in);

	m->phaseacc -= 2.0 * M_PI * f / SampleRate;

	if (m->phaseacc > M_PI)
		m->phaseacc -= 2.0 * M_PI;
	else if (m->phaseacc < M_PI)
		m->phaseacc += 2.0 * M_PI;

	return z;
}

static int decodesymbol(complex *in)
{
	int i, symbol = 0;
	double x, max = 0.0;

	in += BaseTone;

	for (i = 0; i < NumTones; i++) {
		if ((x = cmod(in[i])) > max) {
			max = x;
			symbol = i;
		}
	}

	return symbol;
}

static void update_syncscope(struct mfsk *m)
{
	unsigned char data[PipeLen];
	double y[PipeLen], ymax;
	unsigned int i;

	ymax = 1e-10;

	for (i = 0; i < PipeLen; i++) {
		y[i] = cmod(m->pipe[i].vector[m->prev1symbol]);
		if (y[i] > ymax)
			ymax = y[i];
	}

	for (i = 0; i < PipeLen; i++)
		data[i] = (y[(i + m->pipeptr) % PipeLen] / ymax) * 255.0;

	trx_put_syncscope(data, PipeLen);
}

static void synchronize(struct mfsk *m)
{
	int i, j, syn = -1;
	float val, max = 0.0;

	if (m->currsymbol == m->prev1symbol)
		return;
	if (m->prev1symbol == m->prev2symbol)
		return;

	for (i = 0; i < PipeLen; i++) {
		j = (i + m->pipeptr) % PipeLen;
		val = cmod(m->pipe[j].vector[m->prev1symbol]);
		if (val > max) {
			syn = i;
			max = val;
		}
	}

	m->syncaverage += (syn - SymbolLen) / 16.0;

	if (m->syncaverage < 0.0)
		m->syncaverage += SymbolLen;
	if (m->syncaverage >= SymbolLen)
		m->syncaverage -= SymbolLen;

	m->symboltime = (unsigned int) floor(m->syncaverage);
}

static void afc(struct trx *trx)
{
	struct mfsk *m = (struct mfsk *) trx->modem;
	complex z;
	float x;

	if (trx->afcon == FALSE)
		return;

	if (m->currsymbol != m->prev1symbol)
		return;

	z = ccor(m->prev1vector, m->currvector);
	x = carg(z) / SymbolLen / (2.0 * M_PI / SampleRate);

	if (x > -ToneSpacing / 2.0 &&  x < ToneSpacing / 2.0)
		trx_set_freq(trx->frequency + (x / 8.0));
}

int mfsk_rxprocess(struct trx *trx, float *buf, int len)
{
	struct mfsk *m = (struct mfsk *) trx->modem;
	complex z, *bins;
	int i;

	while (len-- > 0) {
		/* create analytic signal... */
		z.re = z.im = *buf++;
		z = filter(m->hilbert, z);

		/* ...so it can be shifted in frequency */
		z = mixer(trx, z);

		/* feed it to the sliding FFT */
		bins = sfft(m->sfft, z);

		/* copy current vector to the pipe */
		for (i = 0; i < NumTones; i++)
			m->pipe[m->pipeptr].vector[i] = bins[i + BaseTone];

		if (m->synccounter > 0) {
			m->synccounter--;
		} else if (m->symbolphase == m->symboltime) {
			m->synccounter = SymbolLen / 2;

			m->currsymbol = decodesymbol(bins);
			m->currvector = bins[m->currsymbol + BaseTone];

			/* decode symbol */
			recvsymbol(trx, bins);

			/* update the scope */
			update_syncscope(m);

			/* symbol sync */
			synchronize(m);

			/* frequency tracking */
			afc(trx);

			m->prev2symbol = m->prev1symbol;
			m->prev2vector = m->prev1vector;
			m->prev1symbol = m->currsymbol;
			m->prev1vector = m->currvector;
		}

		m->pipeptr = (m->pipeptr + 1) % PipeLen;
		m->symbolphase = (m->symbolphase + 1) % SymbolLen;
	}

	return 0;
}
