/* An answer tone detection algrotihm using Goertzel to detect level and phase
 *
 * The answer tone is described in ITU V.8 and is also used to disable echo
 * suppression and cancelation in the network. The detection uses phase to
 * detect frequencies that are too far off, as well a phase reversal, to
 * distinguish the answer tone from other continuous tones.
 *
 * (C) 2023 by Andreas Eversberg <andreas@eversberg.eu>
 * All Rights Reserved
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero 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 Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <errno.h>
#include "answertone.h"

#define ANS_FREQUENCY	2100.0
#define WINDOW_DURATION	0.010	/* 10 MS */
#define DETECT_DURATION	0.400	/* < 425 MS */
#define MIN_LEVEL	0.05	/* -26 dBm0 */
#define MAX_NOISE	0.5	/* -6 dB */
#define MAX_FREQUENCY	22	/* frequency error */
#define MIN_REVERSAL	2.967	/* about 10 degrees off 180 (PI) */

// #define DEBUG
// #define HEAVY_DEBUG

void goertzel_init(struct answer_tone *at, float frequency, float samplerate)
{
	float omega;

	omega = 2.0 * M_PI * frequency / samplerate;
	at->sine = sin(omega);
	at->cosine = cos(omega);
	at->coeff = 2.0 * at->cosine;
}

/* with rectangular window, the frequency response is:
 * if it matches the filter frequency: 0 dB
 * if it is 0.5/duration off the filter frequency: about -4 dB
 * if it is 1/duration off the filter frequency; -INF dB
 * if it is 1.5/duration off the filter frequency: about -13.5 dB
 */
void goertzel_calculate(struct answer_tone *at, float *data, int length, float *level_p, float *magnitude_p, float *phase_p)
{
	float q0 = 0, q1 = 0, q2 = 0;
	float real, imag;
	float scaling = (float)length / 2.0;
	float upper = 0, lower = 0;
	int i;

	upper = lower = *data;

	for (i = 0; i < length; i++) {
		if (*data > upper)
			upper = *data;
		else if (*data < lower)
			lower = *data;
		q0 = at->coeff * q1 - q2 + *data++;
		q2 = q1;
		q1 = q0;
	}

	real = (q1 * at->cosine - q2) / scaling;
	imag = (q1 * at->sine) / scaling;

	*level_p = (upper - lower) / 2.0;
	*magnitude_p = sqrtf(real*real + imag*imag);
	*phase_p = atan2(imag, real);
}

int answertone_init(struct answer_tone *at, int samplerate)
{
	memset(at, 0, sizeof(*at));

	/* use 10 ms as window size */
	at->buffer_size = samplerate * WINDOW_DURATION;
	at->buffer = calloc(at->buffer_size, sizeof(*at->buffer));
	if (!at->buffer)
		return -ENOMEM;

	goertzel_init(at, ANS_FREQUENCY, samplerate);

	return 0;
}

void answertone_reset(struct answer_tone *at)
{
	at->state = AT_STATE_NONE;
}

void answertone_exit(struct answer_tone *at)
{
	free(at->buffer);
}

enum at_fail answertone_check(struct answer_tone *at, float *phase_p, float *frequency_p)
{
	float level, magnitude, last_phase, shift;

	goertzel_calculate(at, at->buffer, at->buffer_size, &level, &magnitude, phase_p);
	last_phase = at->last_phase;
	at->last_phase = *phase_p;
	shift = (*phase_p - last_phase);
	if (shift > M_PI)
		shift -= M_PI * 2.0;
	else if (shift < -M_PI)
		shift += M_PI * 2.0;
	*frequency_p = shift / M_PI / 2.0 / WINDOW_DURATION;

#if HEAVY_DEBUG
	 printf("level=%6.3f magnitude=%6.3f snr=%6.3f phase=%9.3f frequency=%8.3f\n",
		level, magnitude, magnitude/level, *phase_p / M_PI * 180, *frequency_p);
#endif

	/* fails due to low level */
	if (magnitude < MIN_LEVEL)
		return AT_FAIL_MIN_LEVEL;

	/* fails due to bad SNR */
	if (1.0 - magnitude/level > MAX_NOISE)
		return AT_FAIL_MAX_NOISE;

	/* fails due to wrong frequency */
	if (fabsf(*frequency_p) > MAX_FREQUENCY)
		return AT_FAIL_MAX_FREQUENCY;

	return AT_FAIL_NONE;
}

#if DEBUG
static const char *answertone_cause[] = {
	"ok",
        "level too low",
        "too noisy",
        "wrong frequency",
};
#endif

static enum at_state answertone_chunk(struct answer_tone *at, bool phase_reversal)
{
	float phase, frequency, shift;
	enum at_fail fail;
	int rc = 0;

	fail = answertone_check(at, &phase, &frequency);

	switch (at->state) {
	case AT_STATE_NONE:
		if (fail == AT_FAIL_NONE) {
#if DEBUG
			printf("DETECTED TONE\n");
#endif
			at->state = AT_STATE_DETECT;
			at->tone_duration = WINDOW_DURATION;
			/* count frequency values */
			at->frequency_sum = 0.0;
			at->count = 0;
		}
		break;
	case AT_STATE_DETECT:
		if (fail != AT_FAIL_NONE) {
#if DEBUG
			printf("LOST TONE AFTER %.3f SECONDS (because %s)\n", at->tone_duration, cause[fail]);
#endif
			at->state = AT_STATE_NONE;
			break;
		}
		at->frequency_sum += frequency;
		at->count++;
		at->tone_duration += WINDOW_DURATION;
		at->last2_valid_phase = at->last1_valid_phase;
		at->last1_valid_phase = phase;
		if (at->tone_duration >= DETECT_DURATION) {
#if DEBUG
			printf("LONG TONE VALID WITH FREQUENCY OFFSET %.3f\n", at->frequency_sum / at->count);
#endif
			at->state = AT_STATE_TONE;
		}
		break;
	case AT_STATE_TONE:
		if (fail != AT_FAIL_NONE) {
			if (!phase_reversal) {
#if DEBUG
				printf("LONG TONE LOST\n");
#endif
				at->state = AT_STATE_TONE;
				rc = 1;
				break;
			}
#if DEBUG
			printf("LONG TONE CHANGED, CHECK PHASE REVERSAL\n");
#endif
			at->last_valid_frequency = at->frequency_sum / at->count;
			at->state = AT_STATE_PAUSE;
			/* count pause chunks */
			at->count = 1;
			break;
		}
		at->frequency_sum += frequency;
		at->count++;
		at->tone_duration += WINDOW_DURATION;
		at->last2_valid_phase = at->last1_valid_phase;
		at->last1_valid_phase = phase;
		break;
	case AT_STATE_PAUSE:
		/* three pause chunks: two may be invalid due to phase reversal, one extra to get valid frequency */
		if (at->count++ < 3)
			break;
		/* phase change over 5 chunks */
		shift = (phase - at->last2_valid_phase);
		if (shift > M_PI)
			shift -= M_PI * 2.0;
		else if (shift < -M_PI)
			shift += M_PI * 2.0;
		/* substract phase change to be expected over 5 chunk */
		shift -= at->last_valid_frequency * WINDOW_DURATION * 5.0 * M_PI * 2.0;
		if (shift > M_PI)
			shift -= M_PI * 2.0;
		else if (shift < -M_PI)
			shift += M_PI * 2.0;
		/* if there is change of less than about 10 degrees off 180 */
		if (fabsf(shift) > MIN_REVERSAL) {
#if DEBUG
			printf("PHASE REVERSAL VALID (phase shift %.0f deg)\n", shift / M_PI * 180.0);
#endif
			at->frequency_sum = 0.0;
			at->count = 0;
			at->state = AT_STATE_DETECT;
			rc = 1;
		} else {
#if DEBUG
			printf("LOST TONE, NO VALID PHASE REVERSAL\n");
#endif
			at->state = AT_STATE_NONE;
		}
		break;
	}

	return rc;
}

/* convert stream of int16_t (ISDN) stream into chunks of float (dBm0); return 1, if there was a valid answer tone */
int answertone_process(struct answer_tone *at, int16_t *data, int length, bool phase_reversal)
{
	int rc = 0;

	while (length--) {
		/* convert ISDN data to float with dBm level (1 dBm = -3 dB below full int16_t scale) */
		at->buffer[at->buffer_pos++] = (float)(*data++) / 23170.0;
		if (at->buffer_pos == at->buffer_size) {
			rc |= answertone_chunk(at, phase_reversal);
			at->buffer_pos = 0;
		}
	}

	return rc;
}