/* DTMF coder
 *
 * (C) 2016 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/>.
 */

#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "../libsample/sample.h"
#include "../liblogging/logging.h"
#include "dtmf_decode.h"

//#define DEBUG_DTMF

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

#define DTMF_LOW_1	697.0
#define DTMF_LOW_2	770.0
#define DTMF_LOW_3	852.0
#define DTMF_LOW_4	941.0
#define DTMF_HIGH_1	1209.0
#define DTMF_HIGH_2	1336.0
#define DTMF_HIGH_3	1477.0
#define DTMF_HIGH_4	1633.0

static const char dtmf_digit[] = "     123A456B789C*0#D";

int dtmf_decode_init(dtmf_dec_t *dtmf, void *priv, void (*recv_digit)(void *priv, char digit, dtmf_meas_t *meas), int samplerate, double max_amplitude, double min_amplitude, double freq_margin)
{
	int rc;

	memset(dtmf, 0, sizeof(*dtmf));
	dtmf->priv = priv;
	dtmf->recv_digit = recv_digit;
	dtmf->samplerate = samplerate;
	dtmf->freq_margin = freq_margin;
	dtmf->max_amplitude = max_amplitude;
	dtmf->min_amplitude = min_amplitude;
	dtmf->forward_twist = db2level(4.0);
	dtmf->reverse_twist = db2level(8.0);
	dtmf->time_detect = (int)(0.025 * (double)samplerate);
	dtmf->time_meas = (int)(0.015 * (double)samplerate);
	dtmf->time_pause = (int)(0.010 * (double)samplerate);

	/* init fm demodulator */
	rc = fm_demod_init(&dtmf->demod_low, (double)samplerate, (DTMF_LOW_1 + DTMF_LOW_4) / 2.0, DTMF_LOW_4 - DTMF_LOW_1);
	if (rc < 0)
		goto error;
	rc = fm_demod_init(&dtmf->demod_high, (double)samplerate, (DTMF_HIGH_1 + DTMF_HIGH_4) / 2.0, DTMF_HIGH_4 - DTMF_HIGH_1);
	if (rc < 0)
		goto error;

	/* use fourth order (2 iter) filter, since it is as fast as second order (1 iter) filter */
	iir_lowpass_init(&dtmf->freq_lp[0], 100.0, samplerate, 2);
	iir_lowpass_init(&dtmf->freq_lp[1], 100.0, samplerate, 2);

	return 0;

error:
	dtmf_decode_exit(dtmf);
	return rc;
}

void dtmf_decode_exit(dtmf_dec_t *dtmf)
{
	fm_demod_exit(&dtmf->demod_low);
	fm_demod_exit(&dtmf->demod_high);
}

void dtmf_decode_reset(dtmf_dec_t *dtmf)
{
	dtmf->detected = 0;
	dtmf->count = 0;
}

void dtmf_decode_filter(dtmf_dec_t *dtmf, sample_t *samples, int length, sample_t *frequency_low, sample_t *frequency_high, sample_t *amplitude_low, sample_t *amplitude_high)
{
	sample_t I_low[length], Q_low[length];
	sample_t I_high[length], Q_high[length];
	int i;

	fm_demodulate_real(&dtmf->demod_low, frequency_low, length, samples, I_low, Q_low);
	fm_demodulate_real(&dtmf->demod_high, frequency_high, length, samples, I_high, Q_high);
	/* peak amplitude is the length of I/Q vector
	 * since we filter out the unwanted modulation product, the vector is only half of length */
	for (i = 0; i < length; i++) {
		amplitude_low[i] = sqrt(I_low[i] * I_low[i] + Q_low[i] * Q_low[i]) * 2.0;
		amplitude_high[i] = sqrt(I_high[i] * I_high[i] + Q_high[i] * Q_high[i]) * 2.0;
	}
	iir_process(&dtmf->freq_lp[0], frequency_low, length);
	iir_process(&dtmf->freq_lp[1], frequency_high, length);
}
void dtmf_decode(dtmf_dec_t *dtmf, sample_t *samples, int length)
{
	sample_t frequency_low[length], amplitude_low[length];
	sample_t frequency_high[length], amplitude_high[length];
	double margin, min_amplitude, max_amplitude, forward_twist, reverse_twist, f1, f2;
	int time_detect, time_meas, time_pause;
	int low = 0, high = 0;
	char detected, digit;
	int count;
	int amplitude_ok, twist_ok;
	int i;

	margin = dtmf->freq_margin / 100.0 + 1.0;
	min_amplitude = dtmf->min_amplitude;
	max_amplitude = dtmf->max_amplitude;
	forward_twist = dtmf->forward_twist;
	reverse_twist = dtmf->reverse_twist;
	time_detect = dtmf->time_detect;
	time_meas = dtmf->time_meas;
	time_pause = dtmf->time_pause;
	detected = dtmf->detected;
	count = dtmf->count;

	/* FM/AM demod */
	dtmf_decode_filter(dtmf, samples, length, frequency_low, frequency_high, amplitude_low, amplitude_high);

	for (i = 0; i < length; i++) {
#ifdef DEBUG_DTMF
//		printf("%s %.5f\n", debug_amplitude(samples[i]/2.0), samples[i]/2.0);
#endif
		/* get frequency of low frequencies, correct amplitude drop at cutoff point */
		f1 = frequency_low[i] + (DTMF_LOW_1 + DTMF_LOW_4) / 2.0;
		if (f1 >= DTMF_LOW_1 / margin && f1 <= DTMF_LOW_1 * margin) {
			/* cutoff point */
			amplitude_low[i] /= 0.7071;
			low = 1;
			f1 -= DTMF_LOW_1;
		} else
		if (f1 >= DTMF_LOW_2 / margin && f1 <= DTMF_LOW_2 * margin) {
			amplitude_low[i] /= 1.0734;
			low = 2;
			f1 -= DTMF_LOW_2;
		} else
		if (f1 >= DTMF_LOW_3 / margin && f1 <= DTMF_LOW_3 * margin) {
			amplitude_low[i] /= 1.0389;
			low = 3;
			f1 -= DTMF_LOW_3;
		} else
		if (f1 >= DTMF_LOW_4 / margin && f1 <= DTMF_LOW_4 * margin) {
			/* cutoff point */
			amplitude_low[i] /= 0.7071;
			low = 4;
			f1 -= DTMF_LOW_4;
		} else
			low = 0;
		/* get frequency of high frequencies, correct amplitude drop at cutoff point */
		f2 = frequency_high[i] + (DTMF_HIGH_1 + DTMF_HIGH_4) / 2.0;
		if (f2 >= DTMF_HIGH_1 / margin && f2 <= DTMF_HIGH_1 * margin) {
			/* cutoff point */
			amplitude_high[i] /= 0.7071;
			high = 1;
			f2 -= DTMF_HIGH_1;
		} else
		if (f2 >= DTMF_HIGH_2 / margin && f2 <= DTMF_HIGH_2 * margin) {
			amplitude_high[i] /= 1.0731;
			high = 2;
			f2 -= DTMF_HIGH_2;
		} else
		if (f2 >= DTMF_HIGH_3 / margin && f2 <= DTMF_HIGH_3 * margin) {
			amplitude_high[i] /= 1.0372;
			high = 3;
			f2 -= DTMF_HIGH_3;
		} else
		if (f2 >= DTMF_HIGH_4 / margin && f2 <= DTMF_HIGH_4 * margin) {
			/* cutoff point */
			amplitude_high[i] /= 0.7071;
			high = 4;
			f2 -= DTMF_HIGH_4;
		} else
			high = 0;
		digit = 0;
		amplitude_ok = 0;
		twist_ok = 0;
		if (low && high) {
			digit = dtmf_digit[low*4+high];
			/* check for limits */
			if (amplitude_low[i] <= max_amplitude && amplitude_low[i] >= min_amplitude && amplitude_high[i] <= max_amplitude && amplitude_high[i] >= min_amplitude) {
				amplitude_ok = 1;
#ifdef DEBUG_DTMF
				printf("%.1f %.1f (limits %.1f .. %.1f) %.1f\n", level2db(amplitude_low[i]), level2db(amplitude_high[i]), level2db(min_amplitude), level2db(max_amplitude), level2db(amplitude_high[i] / amplitude_low[i]));
#endif
				if (amplitude_high[i] / amplitude_low[i] <= forward_twist && amplitude_low[i] / amplitude_high[i] <= reverse_twist)
					twist_ok = 1;
			}
		}

		if (!detected) {
			if (digit && amplitude_ok && twist_ok) {
				if (count == 0) {
					memset(&dtmf->meas, 0, sizeof(dtmf->meas));
				}
				if (count >= time_meas) {
					dtmf->meas.frequency_low += f1;
					dtmf->meas.frequency_high += f2;
					dtmf->meas.amplitude_low += amplitude_low[i];
					dtmf->meas.amplitude_high += amplitude_high[i];
					dtmf->meas.count++;
				}
				count++;
				if (count >= time_detect) {
					detected = digit;
					dtmf->meas.frequency_low /= dtmf->meas.count;
					dtmf->meas.frequency_high /= dtmf->meas.count;
					dtmf->meas.amplitude_low /= dtmf->meas.count;
					dtmf->meas.amplitude_high /= dtmf->meas.count;
					dtmf->meas.count = 1;
					dtmf->recv_digit(dtmf->priv, digit, &dtmf->meas);
				}
			} else
				count = 0;
		} else {
			if (!digit || digit != detected || !amplitude_ok || !twist_ok) {
				count++;
				if (count >= time_pause) {
					detected = 0;
#ifdef DEBUG_DTMF
					printf("lost!\n");
#endif
				}
			} else
				count = 0;
		}
#ifdef DEBUG_DTMF
		if (digit)
			printf("DTMF tone='%c' diff frequency=%.1f %.1f amplitude=%.1f %.1f dB (%s) twist=%.1f dB (%s)\n", digit, f1, f2, level2db(amplitude_low[i]), level2db(amplitude_high[i]), (amplitude_ok) ? "OK" : "nok", level2db(amplitude_high[i] / amplitude_low[i]), (twist_ok) ? "OK" : "nok");
#endif

		dtmf->detected = detected;
		dtmf->count = count;
	}
}

