/*
 * dtmf.c — DTMF (touch-tone) detector using the Goertzel algorithm.
 *
 * Detection pipeline per 20 ms frame (320 samples @ 16 kHz):
 *   1. Compute Goertzel power for 8 DTMF frequencies (4 low + 4 high groups).
 *   2. Find the strongest frequency in each group (best_low, best_high).
 *   3. Apply three guards:
 *        a) Absolute energy threshold — both must exceed DTMF_ENERGY_THRESH.
 *        b) Group dominance ratio    — winner must be > DTMF_DOMINANT_RATIO×
 *                                      times the second-best in the same group.
 *        c) Twist guard             — energy ratio (low/high) must be within
 *                                      [1/DTMF_TWIST_MAX, DTMF_TWIST_MAX].
 *   4. Debounce:
 *        - Require DTMF_CONFIRM_FRAMES consecutive matching frames to emit.
 *        - Require DTMF_RELEASE_FRAMES of silence/mismatch before re-arming.
 *
 * Threshold rationale:
 *   DTMF_ENERGY_THRESH  = 4000000
 *     Goertzel power is mean-squared × N².  A -30 dBFS sine at 16-bit PCM
 *     (amplitude ≈ 1000 LSB) gives power ≈ (1000²/2) × 320² / 320 ≈ 1.6e8.
 *     -50 dBFS (amplitude ≈ 100) gives ≈ 1.6e6.  We set the floor at 4e6
 *     (≈ -47 dBFS) to reject noise while allowing quiet handset microphones.
 *
 *   DTMF_DOMINANT_RATIO = 4.0f  (≈ 6 dB separation within a group)
 *     A real DTMF tone drives exactly one row and one column.  If two
 *     frequencies in the same group are within 6 dB of each other, it is
 *     more likely noise or voice than a keypad press.
 *
 *   DTMF_TWIST_MAX = 8.0f  (≈ 9 dB)
 *     ITU-T Q.24 allows up to ±8 dB twist between low/high group.  We use
 *     8× power ratio which corresponds to ≈ 9 dB — slightly relaxed to
 *     accommodate the varied mic responses of vintage telephone handsets.
 *
 *   DTMF_CONFIRM_FRAMES = 2  (2 × 20 ms = 40 ms minimum tone duration)
 *     ITU-T Q.24 specifies ≥ 40 ms tone duration for valid DTMF.
 *
 *   DTMF_RELEASE_FRAMES = 1  (≥ 20 ms inter-digit silence required)
 *     Prevents a single sustained keypress from re-triggering.
 */

/* sdkconfig.h must be included before any CONFIG_* test so the preprocessor
 * has the symbol defined when the #if guard below is evaluated. */
#include "sdkconfig.h"

#if CONFIG_PLIP_DIAL_DTMF

#include "dtmf.h"
#include "audio.h"
#include "dialer.h"

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"

#include <math.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>

#define TAG "dtmf"

/* -------------------------------------------------------------------------
 * Detection thresholds (see rationale in file header)
 * ---------------------------------------------------------------------- */

/* Minimum Goertzel power (mean-squared × N) for a frequency to be considered
 * "present".  Both the row and column candidate must exceed this. */
#define DTMF_ENERGY_THRESH    4000000LL

/* Minimum ratio of best/second-best power within a group.  Below this the
 * group is ambiguous (noise / voice) and the frame is rejected. */
#define DTMF_DOMINANT_RATIO   4.0f

/* Maximum ratio of low-group / high-group power (and its inverse).
 * Exceeding this means one group is far stronger than expected for DTMF. */
#define DTMF_TWIST_MAX        8.0f

/* Consecutive frames required before a digit is reported. */
#define DTMF_CONFIRM_FRAMES   2

/* Frames of "no valid tone" required between two reports. */
#define DTMF_RELEASE_FRAMES   1

/* -------------------------------------------------------------------------
 * DTMF frequency table
 * ---------------------------------------------------------------------- */

#define NUM_LOW   4
#define NUM_HIGH  4

static const float LOW_FREQS[NUM_LOW]  = { 697.0f, 770.0f, 852.0f, 941.0f };
static const float HIGH_FREQS[NUM_HIGH] = { 1209.0f, 1336.0f, 1477.0f, 1633.0f };

/* DTMF matrix: [low_idx][high_idx] → character.
 * Column 3 (1633 Hz) maps to ABCD which are unused on standard phones → '\0'. */
static const char DTMF_MATRIX[NUM_LOW][NUM_HIGH] = {
    { '1', '2', '3', '\0' },  /* 697 Hz row */
    { '4', '5', '6', '\0' },  /* 770 Hz row */
    { '7', '8', '9', '\0' },  /* 852 Hz row */
    { '*', '0', '#', '\0' },  /* 941 Hz row */
};

/* -------------------------------------------------------------------------
 * Goertzel coefficient cache (precomputed at first call)
 * ---------------------------------------------------------------------- */

#define FS     16000  /* sample rate */
#define N_SAMP 320    /* frame size  */

static bool  s_coeff_ready = false;
static float s_low_coeff[NUM_LOW];
static float s_high_coeff[NUM_HIGH];

static void precompute_coeffs(void)
{
    for (int i = 0; i < NUM_LOW; i++) {
        float k = (float)N_SAMP * LOW_FREQS[i] / (float)FS;
        s_low_coeff[i] = 2.0f * cosf(2.0f * (float)M_PI * k / (float)N_SAMP);
    }
    for (int i = 0; i < NUM_HIGH; i++) {
        float k = (float)N_SAMP * HIGH_FREQS[i] / (float)FS;
        s_high_coeff[i] = 2.0f * cosf(2.0f * (float)M_PI * k / (float)N_SAMP);
    }
    s_coeff_ready = true;
}

/* -------------------------------------------------------------------------
 * Goertzel power for a single frequency
 * Power = Q1² + Q2² − Q1·Q2·coeff  (unnormalised, proportional to amplitude²)
 * ---------------------------------------------------------------------- */

static float goertzel_power(const int16_t *samples, int n, float coeff)
{
    float q1 = 0.0f, q2 = 0.0f;
    for (int i = 0; i < n; i++) {
        float q0 = coeff * q1 - q2 + (float)samples[i];
        q2 = q1;
        q1 = q0;
    }
    return q1 * q1 + q2 * q2 - q1 * q2 * coeff;
}

/* -------------------------------------------------------------------------
 * Public API: dtmf_detect_frame
 * ---------------------------------------------------------------------- */

char dtmf_detect_frame(const int16_t *mono, int n)
{
    if (n <= 0 || !mono) return '\0';

    if (!s_coeff_ready) precompute_coeffs();

    /* Compute Goertzel power for all 8 frequencies */
    float low_pow[NUM_LOW], high_pow[NUM_HIGH];
    for (int i = 0; i < NUM_LOW; i++)
        low_pow[i]  = goertzel_power(mono, n, s_low_coeff[i]);
    for (int i = 0; i < NUM_HIGH; i++)
        high_pow[i] = goertzel_power(mono, n, s_high_coeff[i]);

    /* Find strongest in each group */
    int   best_low = 0, best_high = 0;
    float max_low  = low_pow[0], max_high = high_pow[0];
    for (int i = 1; i < NUM_LOW; i++) {
        if (low_pow[i] > max_low)  { max_low  = low_pow[i];  best_low  = i; }
    }
    for (int i = 1; i < NUM_HIGH; i++) {
        if (high_pow[i] > max_high) { max_high = high_pow[i]; best_high = i; }
    }

    /* Guard (a): absolute energy threshold */
    if ((int64_t)max_low < DTMF_ENERGY_THRESH || (int64_t)max_high < DTMF_ENERGY_THRESH)
        goto no_tone;

    /* Guard (b): group dominance — find second-best in each group */
    {
        float second_low = 0.0f, second_high = 0.0f;
        for (int i = 0; i < NUM_LOW; i++) {
            if (i != best_low && low_pow[i] > second_low)
                second_low = low_pow[i];
        }
        for (int i = 0; i < NUM_HIGH; i++) {
            if (i != best_high && high_pow[i] > second_high)
                second_high = high_pow[i];
        }
        /* If second-best is within DTMF_DOMINANT_RATIO of best, ambiguous */
        if (second_low  > 0.0f && max_low  < DTMF_DOMINANT_RATIO * second_low)
            goto no_tone;
        if (second_high > 0.0f && max_high < DTMF_DOMINANT_RATIO * second_high)
            goto no_tone;
    }

    /* Guard (c): twist — power ratio must be within [1/TWIST_MAX, TWIST_MAX] */
    {
        float ratio = max_low / max_high;
        if (ratio > DTMF_TWIST_MAX || ratio < (1.0f / DTMF_TWIST_MAX))
            goto no_tone;
    }

    /* --- Debounce state (static) --- */
    {
        static char  s_candidate      = '\0';
        static int   s_confirm_count  = 0;
        static int   s_release_count  = 0;
        static bool  s_armed          = true;  /* true = ready to report */

        char sym = DTMF_MATRIX[best_low][best_high];
        if (sym == '\0') goto no_tone;  /* 1633 Hz column — ignored */

        /* Reset release counter: we have a tone */
        s_release_count = 0;

        if (!s_armed) {
            /* Waiting for silence/release before accepting next press */
            return '\0';
        }

        if (sym == s_candidate) {
            s_confirm_count++;
        } else {
            s_candidate     = sym;
            s_confirm_count = 1;
        }

        if (s_confirm_count >= DTMF_CONFIRM_FRAMES) {
            /* Confirmed — report and disarm until release */
            s_confirm_count = 0;
            s_candidate     = '\0';
            s_armed         = false;
            return sym;
        }
        return '\0';

no_tone:
        /* No valid tone detected: advance release counter */
        ;  /* label must precede a statement */
        s_confirm_count = 0;
        s_candidate     = '\0';
        if (!s_armed) {
            s_release_count++;
            if (s_release_count >= DTMF_RELEASE_FRAMES) {
                s_armed         = true;
                s_release_count = 0;
            }
        }
        return '\0';
    }
}

/* -------------------------------------------------------------------------
 * Background capture task
 * ---------------------------------------------------------------------- */

#define DTMF_TASK_STACK  4096
#define DTMF_TASK_PRIO   3
#define DTMF_FRAME_SIZE  320

static volatile bool s_armed_flag = false;  /* true = task should process frames */
static TaskHandle_t  s_task_handle = NULL;

static void dtmf_task(void *arg)
{
    (void)arg;
    int16_t  frame[DTMF_FRAME_SIZE];
    int64_t  rms_sq;

    ESP_LOGI(TAG, "dtmf_task started");

    /* Open the capture stream once and keep it open.
     * Full-duplex: TX (speaker) stays active; RX is already enabled at boot.
     * We pass generous max_ms / silence_ms since we never call capture_end
     * while the call is in progress — dtmf_stop() just clears the armed flag. */
    if (audio_capture_begin(3600000, 3600000) != 0) {
        ESP_LOGE(TAG, "dtmf_task: capture_begin failed — task exits");
        s_task_handle = NULL;
        vTaskDelete(NULL);
        return;
    }

    for (;;) {
        if (!s_armed_flag) {
            /* Disarmed: drain frames slowly so the RX FIFO doesn't overflow */
            audio_capture_read_frame(frame, DTMF_FRAME_SIZE, &rms_sq);
            vTaskDelay(pdMS_TO_TICKS(20));
            continue;
        }

        int got = audio_capture_read_frame(frame, DTMF_FRAME_SIZE, &rms_sq);
        if (got <= 0) continue;

        char sym = dtmf_detect_frame(frame, got);
        if (sym == '\0') continue;

        ESP_LOGI(TAG, "DTMF detected: '%c'", sym);
        if (sym >= '0' && sym <= '9') {
            dialer_push_digit(sym - '0');
        }
        /* '*' and '#' are logged only — no dialer push for now */
    }
}

void dtmf_start(void)
{
    if (!s_task_handle) {
        /* Create the task once */
        BaseType_t ok = xTaskCreatePinnedToCore(
            dtmf_task, "dtmf", DTMF_TASK_STACK, NULL, DTMF_TASK_PRIO,
            &s_task_handle, 1);
        if (ok != pdPASS) {
            ESP_LOGE(TAG, "dtmf_start: xTaskCreate failed");
            return;
        }
    }
    s_armed_flag = true;
    ESP_LOGI(TAG, "dtmf_start: DTMF detection armed");
}

void dtmf_stop(void)
{
    s_armed_flag = false;
    ESP_LOGI(TAG, "dtmf_stop: DTMF detection disarmed");
}

#endif /* CONFIG_PLIP_DIAL_DTMF */
