/* DSP functions
 *
 * (C) 2020 by Andreas Eversberg <jolly@eversberg.eu>
 * All Rights Reserved
 *
 * 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 3 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, see <http://www.gnu.org/licenses/>.
 */

#define CHAN ((ss5_t *)(dsp->priv))->name

#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <errno.h>
#include <sys/types.h>
#include <arpa/inet.h>
#include "../libdebug/debug.h"
#include "ss5.h"

//#define DEBUG_DEMODULATOR

#define NUM_TONES 8
#define db2level(db)	pow(10, (double)(db) / 20.0)
#define level2db(level)	(20 * log10(level))

static double tone_dbm[NUM_TONES] =	{ -7,  -7,  -7,   -7,   -7,   -7,   -9,   -9 };
static double tone_freq[NUM_TONES] =	{ 700, 900, 1100, 1300, 1500, 1700, 2400, 2600 };
static double tone_width[NUM_TONES] =	{ 25,  25,  25,   25,   25,   25,   25,   25 };

static double tone_min_dbm[NUM_TONES] =	{ -14, -14, -14,  -14,  -14,  -14,  -16,  -16 };
static double tone_min_ampl_sq[NUM_TONES];
static double tone_diff_db[NUM_TONES] = { 4,   4,   4,    4,    4,    4,    5,    5 };
static double tone_diff_ampl_sq[NUM_TONES];

#define INTERRUPT_DURATION	0.015
#define SPLIT_DURATION		0.030
#define MF_RECOGNITION		0.025

int dsp_init_inst(dsp_t *dsp, void *priv, double samplerate, double sense_db)
{
	double tone_amplitude[NUM_TONES];
	int t;

	PDEBUG(DDSP, DEBUG_DEBUG, "Init DSP for SS5 instance.\n");

	memset(dsp, 0, sizeof(*dsp));
	dsp->priv = priv;
	dsp->samplerate = samplerate;
	dsp->interrupt_duration = (int)(1000.0 * INTERRUPT_DURATION);
	dsp->split_duration = (int)(1000.0 * SPLIT_DURATION);
	dsp->mf_detect_duration = (int)(1000.0 * MF_RECOGNITION);
	dsp->ms_per_sample = 1000.0 / samplerate;
	dsp->detect_tone = ' ';

	/* all levels are relative to 1mW */
	for (t = 0; t < NUM_TONES; t++) {
		tone_amplitude[t] = db2level(tone_dbm[t]);
		tone_min_ampl_sq[t] = pow(db2level(tone_min_dbm[t] - sense_db), 2);
		tone_diff_ampl_sq[t] = pow(db2level(tone_diff_db[t]), 2);
	}

	/* init MF modulator */
	dsp->mf_mod = mf_mod_init(samplerate, NUM_TONES, tone_freq, tone_amplitude);
	if (!dsp->mf_mod)
		return -EINVAL;

	/* init MF demodulator */
	dsp->mf_demod = mf_demod_init(samplerate, NUM_TONES, tone_freq, tone_width);
	if (!dsp->mf_mod)
		return -EINVAL;

	return 0;
}

void dsp_cleanup_inst(dsp_t *dsp)
{
	PDEBUG(DDSP, DEBUG_DEBUG, "Cleanup DSP of SS5 instance.\n");

	if (dsp->mf_mod) {
		mf_mod_exit(dsp->mf_mod);
		dsp->mf_mod = NULL;
	}

	if (dsp->mf_demod) {
		mf_demod_exit(dsp->mf_demod);
		dsp->mf_demod = NULL;
	}
}

/*
 * tone encoder
 */

static struct dsp_digits {
	char tone;
	uint32_t mask;
} dsp_digits[] = {
        { '1', 0x01 + 0x02 },
        { '2', 0x01 + 0x04 },
        { '3', 0x02 + 0x04 },
        { '4', 0x01 + 0x08 },
        { '5', 0x02 + 0x08 },
        { '6', 0x04 + 0x08 },
        { '7', 0x01 + 0x10 },
        { '8', 0x02 + 0x10 },
        { '9', 0x04 + 0x10 },
        { '0', 0x08 + 0x10 },
        { '*', 0x01 + 0x20 }, /* code 11 */
        { '#', 0x02 + 0x20 }, /* code 12 */
        { 'a', 0x04 + 0x20 }, /* KP1 */
        { 'b', 0x08 + 0x20 }, /* KP2 */
        { 'c', 0x10 + 0x20 }, /* ST */
        { 'A', 0x40        }, /* 2400 answer, acknowledge */
        { 'B', 0x80        }, /* 2600 busy */
        { 'C', 0x40 + 0x80 }, /* 2600+2400 clear forward */
        { ' ', 0 },           /* silence */
        { 0 , 0 },
};

#define KP_DIGIT_DURATION	0.100
#define OTHER_DIGIT_DURATION	0.055
#define DIGIT_PAUSE		0.055

/* set signaling tone duration threshold */
void set_sig_detect_duration(dsp_t *dsp, double duration_AB, double duration_C)
{
	dsp->detect_count = 0;
	dsp->sig_detect_duration_AB = (int)(1000.0 * duration_AB);
	dsp->sig_detect_duration_C = (int)(1000.0 * duration_C);
}

/* set given tone with duration (ms) or continuously (0) */
void set_tone(dsp_t *dsp, char tone, double duration)
{
	int i;

	dsp->tone = 0;

	if (!tone) {
		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Remove tone\n");
		return;
	}

	for (i = 0; dsp_digits[i].tone; i++) {
		if (dsp_digits[i].tone == tone) {
			dsp->tone_mask = dsp_digits[i].mask;
			dsp->tone = tone;
			dsp->tone_duration = (int)(dsp->samplerate * duration);
			PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Set tone=\'%c\' duration=%.0fms (mask = 0x%02x)\n", dsp->tone, 1000.0 * duration, dsp->tone_mask);
			return;
		}
	}
}

/* get next tone from dial string, if any */
static void get_tone_from_dial_string(dsp_t *dsp)
{
	char tone;
	double duration;

	dsp->tone = 0;

	if (dsp->dial_index == dsp->dial_length) {
		dsp->dial_length = 0;
		dialing_complete(dsp->priv);
		return;
	}

	/* get alternating tone/pause from dial string */
	if (!dsp->digit_pause) {
		/* digit on */
		tone = dsp->dial_string[dsp->dial_index++];
		dsp->digit_pause = 1;
		if (tone == 'a' || tone == 'b')
			duration = KP_DIGIT_DURATION;
		else
			duration = OTHER_DIGIT_DURATION;
		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Send digit \'%c\' from dial string\n", tone);
	} else {
		/* digit pause */
		tone = ' ';
		dsp->digit_pause = 0;
		duration = DIGIT_PAUSE;
		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Send pause after digit from dial string\n");
	}

	set_tone(dsp, tone, duration);
}

/* set given dial string */
void set_dial_string(dsp_t *dsp, const char *dial)
{
	dsp->digit_pause = 0;
	strncpy(dsp->dial_string, dial, sizeof(dsp->dial_string) - 1);
	dsp->dial_index = 0;
	dsp->dial_length = strlen(dsp->dial_string);
}

/* determine which tones to be modulated, get next tone, if elapsed */
static int assemble_tones(dsp_t *dsp, uint32_t *mask, int length)
{
	int i;

	for (i = 0; i < length; i++) {
		/* if tone was done, try to get next digit */
		if (!dsp->tone) {
			if (!dsp->dial_length)
				return i;
			get_tone_from_dial_string(dsp);
			if (!dsp->tone)
				return i;
		}
		*mask++ = dsp->tone_mask;
		if (dsp->tone_duration) {
			/* count down duration, if tones is not continuous */
			if (!(--dsp->tone_duration))
				dsp->tone = 0;
		}
	}

	return i;
}

/*
 * tone deencoder
 */

/* detection array for one frequency */
static char decode_one[8] =
	{ ' ', ' ', ' ', ' ', ' ', ' ', 'A', 'B' }; /* A = 2400, B = 2600 */

/* detection matrix for two frequencies */
static char decode_two[8][8] =
{
	{ ' ', '1', '2', '4', '7', '*', ' ', ' ' }, /* * = code 11 */
	{ '1', ' ', '3', '5', '8', '#', ' ', ' ' }, /* # = code 12 */
	{ '2', '3', ' ', '6', '9', 'a', ' ', ' ' }, /* a = KP1 */
	{ '4', '5', '6', ' ', '0', 'b', ' ', ' ' }, /* b = KP2 */
	{ '7', '8', '9', '0', ' ', 'c', ' ', ' ' }, /* c = ST */
	{ '*', '#', 'a', 'b', 'c', ' ', ' ', ' ' },
	{ ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'C' }, /* C = 2600+2400 */
	{ ' ', ' ', ' ', ' ', ' ', ' ', 'C', ' ' }
};

#define NONE_MIN_LEVEL_SQUARED	

/* determine which tone is played */
static void detect_tones(dsp_t *dsp, sample_t *samples, sample_t **levels_squared, int length, int incoming)
{
	int f1, f2;
	double f1_level_squared, f2_level_squared;
	char tone;
	int s, t;

	for (s = 0; s < length; s++) {
		/* mute if split duration reached */
		if (dsp->split_duration && dsp->split_count == dsp->split_duration)
			samples[s] = 0.0;
		/* only perform tone detection every millisecond */
		dsp->detect_interval += dsp->ms_per_sample;
		if (dsp->detect_interval < 1.0)
			continue;
		dsp->detect_interval -= 1.0;

		if (incoming) {
#ifdef DEBUG_DEMODULATOR
			for (t = 0; t < dsp->mf_demod->tones; t++) {
				char level[20];
				int db;
				memset(level, 32, sizeof(level));
				db = roundf(level2db(sqrt(levels_squared[t][s])) + 25);
				if (db >= 0 && db < (int)sizeof(level))
					level[db] = '*';
				level[sizeof(level)-1]=0;
				printf("%s|", level);
			}
			printf("\n");
#endif
		}

		/* find the tone which is the loudest */
		f1 = -1;
		f1_level_squared = -1.0;
		for (t = 0; t < dsp->mf_demod->tones; t++) {
			if (levels_squared[t][s] > f1_level_squared) {
				f1_level_squared = levels_squared[t][s];
				f1 = t;
			}
		}
		/* find the tone which is the second loudest */
		f2 = -1;
		f2_level_squared = -1.0;
		for (t = 0; t < dsp->mf_demod->tones; t++) {
			if (t == f1)
				continue;
			if (levels_squared[t][s] > f2_level_squared) {
				f2_level_squared = levels_squared[t][s];
				f2 = t;
			}
		}
		/* now check if the minimum level is reached */
		if (f1 >= 0 && f1_level_squared < tone_min_ampl_sq[f1])
			f1 = -1;
		if (f2 >= 0 && f2_level_squared < tone_min_ampl_sq[f2])
			f2 = -1;

//		printf("%s f1=%.0f (%.1f dBm)  f2=%.0f (%.1f dBm)\n", CHAN, (f1 >= 0) ? tone_freq[f1] : 0, level2db(sqrt(f1_level_squared)), (f2 >= 0) ? tone_freq[f2] : 0, level2db(sqrt(f2_level_squared)));
		/* check if no, one or two tones are detected */
		if (f1 < 0)
			tone = ' ';
		else if (f2 < 0)
			tone = decode_one[f1];
		else {
			if (f2_level_squared * tone_diff_ampl_sq[f2] < f1_level_squared)
				tone = ' ';
			else
				tone = decode_two[f1][f2];
		}
		//printf("tone=%c\n", tone);

		/* process interrupt counting, keep tone until interrupt counter expires */
		if (dsp->detect_tone != ' ' && tone != dsp->detect_tone) {
			if (dsp->interrupt_count < dsp->interrupt_duration) {
				dsp->interrupt_count++;
				tone = dsp->detect_tone;
			}
		} else
			dsp->interrupt_count = 0;

		/* split audio, after minimum duration of detecting a tone */
		if (tone >= 'A' && tone <= 'C') {
			if (dsp->split_count < dsp->split_duration)
				dsp->split_count++;
		} else
			dsp->split_count = 0;


		/* some change in tone */
		if (dsp->detect_tone != tone) {
			if (dsp->detect_count == 0)
				PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Detected new tone '%c' (%.1f dBm)\n", tone, level2db(sqrt(f1_level_squared)));
			switch (tone) {
			case 'A':
			case 'B':
				/* tone appears, wait some time */
				if (dsp->detect_count < dsp->sig_detect_duration_AB)
					dsp->detect_count++;
				else {
					/* sign tone detected */
					dsp->detect_count = 0;
					dsp->detect_tone = tone;
					receive_digit(dsp->priv, tone, level2db(sqrt(f1_level_squared)));
				}
				break;
			case 'C':
				/* tone appears, wait some time */
				if (dsp->detect_count < dsp->sig_detect_duration_C)
					dsp->detect_count++;
				else {
					/* sign tone detected */
					dsp->detect_count = 0;
					dsp->detect_tone = tone;
					receive_digit(dsp->priv, tone, level2db(sqrt(f1_level_squared)));
				}
				break;
			case ' ':
				/* tone appears or ceases */
				dsp->detect_count = 0;
				dsp->detect_tone = tone;
				receive_digit(dsp->priv, tone, 0.0);
				break;
			default:
				/* tone appears, wait some time */
				if (dsp->detect_count < dsp->mf_detect_duration)
					dsp->detect_count++;
				else {
					/* sign tone detected */
					dsp->detect_count = 0;
					dsp->detect_tone = tone;
					receive_digit(dsp->priv, tone, level2db(sqrt(f1_level_squared)));
				}
			}
		} else
			dsp->detect_count = 0;
	}
}

/* process audio from one link (source) to another (destination) */
static void process_audio(ss5_t *ss5_a, ss5_t *ss5_b, int length)
{
	sample_t samples[2][length], s;
	sample_t b1[length], b2[length], b3[length], b4[length], b5[length], b6[length], b7[length], b8[length];
	sample_t *levels_squared[NUM_TONES] = { b1, b2, b3, b4, b5, b6, b7, b8 };
	uint32_t mask[length];
	int16_t data[160];
	int count1, count2;
	int i;

	/* trigger reception of RTP stuff */
	if (ss5_a->cc_session)
		osmo_cc_session_handle(ss5_a->cc_session);
	if (ss5_b->cc_session)
		osmo_cc_session_handle(ss5_b->cc_session);

	/* get audio from jitter buffer */
	jitter_load(&ss5_a->tx_dejitter, samples[0], length);
	jitter_load(&ss5_b->tx_dejitter, samples[1], length);

	/* optionally add comfort noise */
	if (!ss5_a->cc_callref && ss5_a->ss5_ep->comfort_noise) {
		for (i = 0; i < length; i++)
			samples[0][i] += (double)((int8_t)random()) / 2000.0;
	}
	if (!ss5_b->cc_callref && ss5_b->ss5_ep->comfort_noise) {
		for (i = 0; i < length; i++)
			samples[1][i] += (double)((int8_t)random()) / 2000.0;
	}

	/* modulate tone/digit. if no tone has to be played (or it stopped), count is less than length */
	count1 = assemble_tones(&ss5_a->dsp, mask, length);
	mf_mod(ss5_a->dsp.mf_mod, mask, samples[0], count1);
	count2 = assemble_tones(&ss5_b->dsp, mask, length);
	mf_mod(ss5_b->dsp.mf_mod, mask, samples[1], count2);

	/* optionally add some crosstalk */
	if (ss5_a->ss5_ep->crosstalk) {
		/* use count, since it carries number of samples with signalling */
		for (i = 0; i < count1; i++)
			samples[1][i] += samples[0][i] / 70.0;
	}
	if (ss5_b->ss5_ep->crosstalk) {
		/* use count, since it carries number of samples with signalling */
		for (i = 0; i < count2; i++)
			samples[0][i] += samples[1][i] / 70.0;
	}

	/* ! here is the bridge from a to b and from b to a ! */

	/* optionally add one way delay */
	if (ss5_b->delay_buffer) {
		for (i = 0; i < length; i++) {
			s = ss5_b->delay_buffer[ss5_b->delay_index];
			ss5_b->delay_buffer[ss5_b->delay_index] = samples[0][i];
			if (++(ss5_b->delay_index) == ss5_b->delay_length)
				ss5_b->delay_index = 0;
			samples[0][i] = s;
		}
	}
	if (ss5_a->delay_buffer) {
		for (i = 0; i < length; i++) {
			s = ss5_a->delay_buffer[ss5_a->delay_index];
			ss5_a->delay_buffer[ss5_a->delay_index] = samples[1][i];
			if (++(ss5_a->delay_index) == ss5_a->delay_length)
				ss5_a->delay_index = 0;
			samples[1][i] = s;
		}
	}

	/* demodulate and call tone detector */
	mf_demod(ss5_b->dsp.mf_demod, samples[0], length, levels_squared);
	detect_tones(&ss5_b->dsp, samples[0], levels_squared, length, 1);
	mf_demod(ss5_a->dsp.mf_demod, samples[1], length, levels_squared);
	detect_tones(&ss5_a->dsp, samples[1], levels_squared, length, 0);

	/* forward audio to CC if call exists */
	if (ss5_b->cc_callref && ss5_b->codec) {
		samples_to_int16_1mw(data, samples[0], length);
		osmo_cc_rtp_send(ss5_b->codec, (uint8_t *)data, length * 2, 0, 1, length);
	}
	if (ss5_a->cc_callref && ss5_a->codec) {
		samples_to_int16_1mw(data, samples[1], length);
		osmo_cc_rtp_send(ss5_a->codec, (uint8_t *)data, length * 2, 0, 1, length);
	}
}

/* clock is called every given number of samples (20ms) */
void audio_clock(ss5_endpoint_t *ss5_ep_sunset, ss5_endpoint_t *ss5_ep_sunrise, int len)
{
	ss5_t *ss5_a, *ss5_b;

	if (!ss5_ep_sunset)
		return;

	if (!ss5_ep_sunrise) {
		/* each pair of links on the same endpoint are bridged */
		for (ss5_b = ss5_ep_sunset->link_list; ss5_b; ss5_b = ss5_b->next) {
			ss5_a = ss5_b;
			ss5_b = ss5_b->next;
			if (!ss5_b)
				break;
			process_audio(ss5_a, ss5_b, len);
		}
	} else {
		/* each link on two endpoints are bridged */
		for (ss5_a = ss5_ep_sunset->link_list, ss5_b = ss5_ep_sunrise->link_list; ss5_a && ss5_b; ss5_a = ss5_a->next, ss5_b = ss5_b->next) {
			process_audio(ss5_a, ss5_b, len);
		}
	}
}

/* take audio from CC and store in jitter buffer */
void down_audio(struct osmo_cc_session_codec *codec, uint8_t __attribute__((unused)) marker, uint16_t sequence, uint32_t timestamp, uint32_t ssrc, uint8_t *data, int len)
{
        ss5_t *ss5 = codec->media->session->priv;
	int count = len/2;
	sample_t samples[count];

	int16_to_samples_1mw(samples, (int16_t *)data, count);
	jitter_save(&ss5->tx_dejitter, samples, count, 1, sequence, timestamp, ssrc);
}

void encode_l16(uint8_t *src_data, int src_len, uint8_t **dst_data, int *dst_len)
{
	uint16_t *src = (uint16_t *)src_data, *dst;
	int len = src_len / 2, i;

	dst = malloc(len * 2);
	if (!dst)
		return;
	for (i = 0; i < len; i++)
		dst[i] = htons(src[i]);
	*dst_data = (uint8_t *)dst;
	*dst_len = len * 2;
}

void decode_l16(uint8_t *src_data, int src_len, uint8_t **dst_data, int *dst_len)
{
	uint16_t *src = (uint16_t *)src_data, *dst;
	int len = src_len / 2, i;

	dst = malloc(len * 2);
	if (!dst)
		return;
	for (i = 0; i < len; i++)
		dst[i] = ntohs(src[i]);
	*dst_data = (uint8_t *)dst;
	*dst_len = len * 2;
}