#include <stdlib.h>
#include <stdio.h>

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

static void throb_txinit(struct trx *trx)
{
	struct throb *s = (struct throb *) trx->modem;
	s->preamble = 4;
}

static void throb_rxinit(struct trx *trx)
{
	struct throb *s = (struct throb *) trx->modem;
	s->shift = 0;
}

static void throb_free(struct throb *s)
{
	int i;

	if (s) {
		free(s->txpulse);

		clear_filter(s->hilbert);
		clear_fftfilt(s->fftfilt);

		for (i = 0; i < NumTones; i++) {
			free(s->rxtone[i]);
			free(s->tonelo[i]);
			free(s->tonehi[i]);
		}

		free(s);
	}
}

static void throb_destructor(struct trx *trx)
{
	throb_free((struct throb *) trx->modem);

	trx->modem = NULL;
	trx->txinit = NULL;
	trx->rxinit = NULL;
	trx->txprocess = NULL;
	trx->rxprocess = NULL;
	trx->destructor = NULL;
}

/*
 * Make a semi raised cosine pulse of length 'len'.
 */
static double *mk_semi_pulse(int len)
{
	double *pulse, x;
	int i, j;

	if ((pulse = calloc(len, sizeof(double))) == NULL)
		return NULL;

	for (i = 0; i < len; i++) {
		if (i < len / 5) {
			x = M_PI * i / (len / 5.0);
			pulse[i] = 0.5 * (1 - cos(x));
		}

		if (i >= len / 5 && i < len * 4 / 5)
			pulse[i] = 1.0;

		if (i >= len * 4 / 5) {
			j = i - len * 4 / 5;
			x = M_PI * j / (len / 5.0);
			pulse[i] = 0.5 * (1 + cos(x));
		}
	}

	return pulse;
}

/*
 * Make a full raised cosine pulse of length 'len'.
 */
static double *mk_full_pulse(int len)
{
	double *pulse;
	int i;

	if ((pulse = calloc(len, sizeof(double))) == NULL)
		return NULL;

	for (i = 0; i < len; i++)
		pulse[i] = 0.5 * (1 - cos(2 * M_PI * i / len));

	return pulse;
}

/*
 * Make a 32 times downsampled complex prototype tone for rx.
 */
static complex *mk_rxtone(double freq, double *pulse, int len)
{
	complex *tone;
	double x;
	int i;

	if ((tone = calloc(len / DownSample, sizeof(complex))) == NULL)
		return NULL;

	for (i = 0; i < len; i += DownSample) {
		x = -2.0 * M_PI * freq * i / SampleRate;
		tone[i / DownSample].re = pulse[i] * cos(x);
		tone[i / DownSample].im = pulse[i] * sin(x);
	}

	return tone;
}

void throb_init(struct trx *trx)
{
	struct throb *s;
	double bw;
	int i;

	if ((s = calloc(1, sizeof(struct throb))) == NULL)
		return;

	switch (trx->mode) {
	case MODE_THROB1:
		s->symlen = SymbolLen1;
		s->txpulse = mk_semi_pulse(SymbolLen1);
		for (i = 0; i < NumTones; i++)
			s->freqs[i] = ThrobToneFreqsNar[i];
		bw = 36.0 / SampleRate;
		break;

	case MODE_THROB2:
		s->symlen = SymbolLen2;
		s->txpulse = mk_semi_pulse(SymbolLen2);
		for (i = 0; i < NumTones; i++)
			s->freqs[i] = ThrobToneFreqsNar[i];
		bw = 36.0 / SampleRate;
		break;

	case MODE_THROB4:
		s->symlen = SymbolLen4;
		s->txpulse = mk_full_pulse(SymbolLen4);
		for (i = 0; i < NumTones; i++)
			s->freqs[i] = ThrobToneFreqsWid[i];
		bw = 72.0 / SampleRate;
		break;

	default:
		throb_free(s);
		return;
	}

	if (s->txpulse == NULL) {
		throb_free(s);
		return;
	}

	if ((s->hilbert = init_filter(0.05, 0.45)) == NULL) {
		throb_free(s);
		return;
	}

	if ((s->fftfilt = init_fftfilt(0, bw, FilterFFTLen)) == NULL) {
		throb_free(s);
		return;
	}

	for (i = 0; i < NumTones; i++) {
		s->rxtone[i] = mk_rxtone(s->freqs[i], s->txpulse, s->symlen);
		s->tonelo[i] = mk_rxtone(s->freqs[i] - 2, s->txpulse, s->symlen);
		s->tonehi[i] = mk_rxtone(s->freqs[i] + 2, s->txpulse, s->symlen);

		if (!s->rxtone[i] || !s->tonelo || !s->tonehi) {
			throb_free(s);
			return;
		}
	}

	trx->modem = s;

	trx->txinit = throb_txinit;
	trx->rxinit = throb_rxinit;

	trx->txprocess = throb_txprocess;
	trx->rxprocess = throb_rxprocess;

	trx->destructor = throb_destructor;

	trx->reverse = 0;
	trx->samplerate = SampleRate;
	trx->fragmentsize = s->symlen;
	trx->bandwidth = s->freqs[8] - s->freqs[0];
	trx->syncpos = 0.5;
}
