#include <stdio.h>
#include <string.h>

#include "trx.h"
#include "throb.h"
#include "tab.h"
#include "filter.h"
#include "misc.h"
#include "fft.h"
#include "fftfilt.h"

static inline complex mixer(struct trx *trx, complex in)
{
	struct throb *s = (struct throb *) trx->modem;
	complex z;
	float f;

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

	z = cmul(z, in);

	f = trx->frequency + trx->bandwidth / 2.0;

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

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

	return z;
}

static void update_syncscope(struct throb *s)
{
	unsigned char data[SymbolLen1 / DownSample];
	unsigned int i;
	double ymax;

	ymax = 0.001;
	for (i = 0; i < s->symlen / DownSample; i++)
		if (cmod(s->pipe[i]) > ymax)
                        ymax = cmod(s->pipe[i]);

	for (i = 0; i < s->symlen / DownSample; i++)
		data[i] = (unsigned char) (255.0 * cmod(s->pipe[i]) / ymax);

        trx_put_syncscope(data, s->symlen / DownSample);
}

static int findtones(complex *word, int *tone1, int *tone2)
{
	double max1, max2;
	int maxtone, i;

	max1 = 0;
	*tone1 = 0;
	for (i = 0; i < NumTones; i++) {
		if (cmod(word[i]) > max1) {
			max1 = cmod(word[i]);
			*tone1 = i;
		}
	}

	maxtone = *tone1;

	max2 = 0;
	*tone2 = 0;
	for (i = 0; i < NumTones; i++) {
		if (i == *tone1)
			continue;
		if (cmod(word[i]) > max2) {
			max2 = cmod(word[i]);
			*tone2 = i;
		}
	}

	if (max1 > max2 * 2)
		*tone2 = *tone1;

	if (*tone1 > *tone2) {
		i = *tone1;
		*tone1 = *tone2;
		*tone2 = i;
	}

	return maxtone;
}

static void decodechar(struct trx *trx, int tone1, int tone2)
{
	struct throb *s = (struct throb *) trx->modem;
	int i;

	if (s->shift == TRUE) {
		if (tone1 == 0 && tone2 == 8)
			trx_put_rx_char('?');

		if (tone1 == 1 && tone2 == 7)
			trx_put_rx_char('@');

		if (tone1 == 2 && tone2 == 6)
			trx_put_rx_char('=');

		if (tone1 == 4 && tone2 == 4)
			trx_put_rx_char('\n');

		s->shift = FALSE;
		return;
	}

	if (tone1 == 3 && tone2 == 5) {
		s->shift = TRUE;
		return;
	}

	for (i = 0; i < NumChars; i++) {
		if (ThrobTonePairs[i][0] == tone1 + 1 && 
		    ThrobTonePairs[i][1] == tone2 + 1) {
			trx_put_rx_char(ThrobCharSet[i]);
			break;
		}
	}

	return;
}

static void rx(struct trx *trx, complex in)
{
	struct throb *s = (struct throb *) trx->modem;
	complex rxword[NumTones];
	int i, sync, len, t1, t2;

	len = s->symlen / DownSample;

	s->pipe[s->pipeptr++] = in;

	if (s->pipeptr < len * 2)
		return;

	update_syncscope(s);

	sync = ((int) ((trx->syncpos * len) + 0.5)) % len;

	for (i = 0; i < NumTones; i++)
		rxword[i] = cmac(s->rxtone[i], s->pipe + sync, len);

	i = findtones(rxword, &t1, &t2);

	decodechar(trx, t1, t2);

	if (trx->afcon == TRUE) {
		complex z1, z2;
		double f;

		if (sync == len - 1) {
			z1 = cmac(s->rxtone[i], s->pipe + sync - 1, len);
			z2 = rxword[i];
		} else {
			z1 = rxword[i];
			z2 = cmac(s->rxtone[i], s->pipe + sync + 1, len);
		}

		f = carg(ccor(z1, z2)) / (2 * 32 * M_PI / SampleRate);
		f -= s->freqs[i];

		trx_set_freq(trx->frequency + f / 8);
	}

	memcpy(s->pipe, s->pipe + len, len * sizeof(complex));
	s->pipeptr = len;
}

int throb_rxprocess(struct trx *trx, float *buf, int len)
{
	struct throb *s = (struct throb *) trx->modem;
	complex z, *zp;
	int i, n;

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

		/* shift down to 0 +- 32 (64) Hz */
		z = mixer(trx, z);

		/* low pass filter */
		n = fftfilt(s->fftfilt, z, &zp);

		/* downsample by 32 and push to the receiver */
		for (i = 0; i < n; i += DownSample)
			rx(trx, zp[i]);
	}

	return 0;
}
