/*
 * @Date: 2025-07-17 13:13:12
 * @Author: mypx
 * @LastEditors: mypx mypx_coder@163.com
 * @LastEditTime: 2025-08-29 10:03:35
 * @FilePath: et_goertzel.c
 * @Description:
 *   Goertzel algorithm implementation for power and phase detection.
 *   Copyright (c) 2025 by mypx, All Rights Reserved.
 */
#include "et_goertzel.h"

/**
 * @brief Initialize the Goertzel algorithm configuration.
 *
 * This function sets up the internal parameters needed by the Goertzel
 * algorithm based on the number of samples and the target frequency bin.
 *
 * @param cfg           Pointer to Goertzel configuration structure.
 * @param sample_count  Total number of input samples (N).
 * @param target_bin    Index of the frequency bin to detect (k).
 * @param dc_offset     DC offset value for ADC signal processing. For 12-bit ADC which is unipolar,
 *                      it cannot directly output negative values, so hardware design adds a DC bias
 *                      to shift the original AC signal (e.g. -V_ref/2 ~ +V_ref/2) to 0 ~ V_ref range,
 *                      corresponding to quantization values 0 ~ 4095:
 *                      Bias point 2048 corresponds to 0V (or zero point) of the original signal
 *                      Values less than 2048 correspond to negative half-cycle of the original signal
 *                      Values greater than 2048 correspond to positive half-cycle of the original signal
 * @return              0 on success, -1 for invalid parameters, -2 if target_bin is out of range [0, sample_count/2]
 */
int et_goertzel_init(et_goertzel_config_t *cfg, int sample_count, int target_bin, double dc_offset)
{
    // Parameter validation
    if (cfg == NULL || sample_count <= 0)
    {
        return -1; // Invalid parameters
    }

    // Check if target_bin is within the valid range [0, sample_count/2]
    int max_bin = sample_count / 2;
    if (target_bin < 0 || target_bin > max_bin)
    {
        return -2; // target_bin is out of valid range
    }

    cfg->sample_count = sample_count; // Store total number of samples
    cfg->target_bin   = target_bin;   // Store target DFT bin index
    cfg->dc_offset    = dc_offset;    // Store DC offset

    // Calculate the normalized angular frequency: 2*pi*k/N
    double omega = 2.0 * M_PI * target_bin / sample_count;

    // Precompute coefficient = 2 * cos(omega) for the recurrence
    cfg->coefficient = 2.0 * cos(omega);

    return 0; // Initialization successful
}

/**
 * @brief Compute the signal power at the target frequency bin.
 *
 * Uses the Goertzel recurrence to estimate the power in a single DFT bin.
 * This function accepts double precision floating point input data.
 *
 * @param cfg   Pointer to initialized Goertzel configuration.
 * @param data  Array of input samples (length = cfg->sample_count).
 * @return      Estimated power at the target frequency bin.
 */
double et_goertzel_compute_power_f64(const et_goertzel_config_t *cfg, const double *data)
{
    double s_prev  = 0.0; // y[n-1] in recurrence
    double s_prev2 = 0.0; // y[n-2] in recurrence
    double s;             // current y[n]

    // Iterate the Goertzel recurrence over all samples
    for (int i = 0; i < cfg->sample_count; i++)
    {
        s       = data[i] - cfg->dc_offset;
        s       = s + cfg->coefficient * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev  = s;
    }

    // Compute power: y[N-1]^2 + y[N-2]^2 - coeff * y[N-1] * y[N-2]
    double power = s_prev * s_prev + s_prev2 * s_prev2 - cfg->coefficient * s_prev * s_prev2;
    /* Corrected normalization: divide by (N²/2) to match the theoretical power (A²/2) for a sine wave.
    * The Goertzel result (before normalization) is proportional to |X[k]|^2.
    * For a sine wave with amplitude A, |X[k]|^2 = (A*N/2)^2 = A^2*N^2/4.
    * To get A^2/2 (power), we need to divide by N^2/2.
    */
    power = power * 2.0 / (cfg->sample_count * cfg->sample_count);

    return power;
}

/**
 * @brief Compute the signal power at the target frequency bin.
 *
 * Uses the Goertzel recurrence to estimate the power in a single DFT bin.
 * This function accepts single precision floating point input data.
 *
 * @param cfg   Pointer to initialized Goertzel configuration.
 * @param data  Array of input samples (length = cfg->sample_count).
 * @return      Estimated power at the target frequency bin.
 */
double et_goertzel_compute_power_f32(const et_goertzel_config_t *cfg, const float *data)
{
    double s_prev  = 0.0; // y[n-1] in recurrence
    double s_prev2 = 0.0; // y[n-2] in recurrence
    double s;             // current y[n]

    // Iterate the Goertzel recurrence over all samples
    for (int i = 0; i < cfg->sample_count; i++)
    {
        s       = (double)data[i] - cfg->dc_offset;
        s       = s + cfg->coefficient * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev  = s;
    }

    // Compute power: y[N-1]^2 + y[N-2]^2 - coeff * y[N-1] * y[N-2]
    double power = s_prev * s_prev + s_prev2 * s_prev2 - cfg->coefficient * s_prev * s_prev2;
    /* Corrected normalization: divide by (N²/2) to match the theoretical power (A²/2) for a sine wave.
    * The Goertzel result (before normalization) is proportional to |X[k]|^2.
    * For a sine wave with amplitude A, |X[k]|^2 = (A*N/2)^2 = A^2*N^2/4.
    * To get A^2/2 (power), we need to divide by N^2/2.
    */
    power = power * 2.0 / (cfg->sample_count * cfg->sample_count);

    return power;
}

/**
 * @brief Compute the signal power at the target frequency bin.
 *
 * Uses the Goertzel recurrence to estimate the power in a single DFT bin.
 * This function accepts unsigned 16-bit integer input data.
 *
 * @param cfg   Pointer to initialized Goertzel configuration.
 * @param data  Array of input samples (length = cfg->sample_count).
 * @return      Estimated power at the target frequency bin.
 */
double et_goertzel_compute_power_u16(const et_goertzel_config_t *cfg, const uint16_t *data)
{
    double s_prev  = 0.0; // y[n-1] in recurrence
    double s_prev2 = 0.0; // y[n-2] in recurrence
    double s;             // current y[n]

    // Iterate the Goertzel recurrence over all samples
    for (int i = 0; i < cfg->sample_count; i++)
    {
        s       = (double)data[i] - cfg->dc_offset;
        s       = s + cfg->coefficient * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev  = s;
    }

    // Compute power: y[N-1]^2 + y[N-2]^2 - coeff * y[N-1] * y[N-2]
    double power = s_prev * s_prev + s_prev2 * s_prev2 - cfg->coefficient * s_prev * s_prev2;
    /* Corrected normalization: divide by (N²/2) to match the theoretical power (A²/2) for a sine wave.
    * The Goertzel result (before normalization) is proportional to |X[k]|^2.
    * For a sine wave with amplitude A, |X[k]|^2 = (A*N/2)^2 = A^2*N^2/4.
    * To get A^2/2 (power), we need to divide by N^2/2.
    */
    power = power * 2.0 / (cfg->sample_count * cfg->sample_count);

    return power;
}

double et_goertzel_compute_power_u32(const et_goertzel_config_t *cfg, const uint32_t *data)
{
    double s_prev  = 0.0;
    double s_prev2 = 0.0;
    double s;

    // Iterate the Goertzel recurrence over all samples
    for (int i = 0; i < cfg->sample_count; i++)
    {
        s       = ((double)data[i] - cfg->dc_offset) + cfg->coefficient * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev  = s;
    }

    // Compute power: y[N-1]^2 + y[N-2]^2 - coeff * y[N-1] * y[N-2]
    double power = s_prev * s_prev + s_prev2 * s_prev2 - cfg->coefficient * s_prev * s_prev2;
    power        = power * 2.0 / (cfg->sample_count * cfg->sample_count);

    return power;
}

/**
 * @brief Calculate the Goertzel bin index for a given frequency.
 *
 * Converts a target frequency in Hertz to the closest DFT bin index.
 * Ensures the bin index stays within [0, N/2].
 *
 * @param target_freq  Desired frequency to detect (Hz).
 * @param sample_rate  Sampling rate of the ADC (Hz).
 * @param num_samples  Total number of samples (N).
 * @return             Nearest bin index k (0 <= k <= N/2).
 *                     Returns -1 for invalid parameters.
 *                     Returns -2 if the target frequency violates the Nyquist theorem.
 */
int et_goertzel_calc_bin(double target_freq, double sample_rate, int num_samples)
{
    // Parameter validation: frequency cannot be negative, sample rate must be greater than 0, number of samples must be greater than 0
    if (target_freq < 0.0 || sample_rate <= 0.0 || num_samples <= 0)
    {
        return -1; // Invalid parameters
    }

    // Check if Nyquist theorem is satisfied: frequency cannot exceed half of the sample rate
    if (target_freq > sample_rate / 2.0)
    {
        return -2; // Violates Nyquist theorem
    }

    // Compute exact (floating-point) bin index: freq * N / sample_rate
    double exact_k = target_freq * num_samples / sample_rate;

    // Round to nearest integer bin
    int k = (int)round(exact_k);

    // Clamp k to valid range [0, N/2]
    int max_k = num_samples / 2;
    if (k < 0)
        k = 0;
    else if (k > max_k)
        k = max_k;

    return k;
}

/**
 * @brief Compute the phase at the target frequency bin.
 *
 * Runs the Goertzel recurrence to obtain the final state, then
 * reconstructs the real and imaginary parts of the DFT coefficient
 * to compute the phase angle.
 *
 * @param cfg   Pointer to initialized Goertzel configuration.
 * @param data  Array of input samples (length = cfg->sample_count).
 * @return      Estimated phase (radians) at the target bin.
 */
double et_goertzel_compute_phase_f64(const et_goertzel_config_t *cfg, const double *data)
{
    double s_prev  = 0.0; // y[n-1]
    double s_prev2 = 0.0; // y[n-2]
    double s;             // y[n]

    // Run recurrence over all samples
    for (int i = 0; i < cfg->sample_count; i++)
    {
        s       = data[i] - cfg->dc_offset + cfg->coefficient * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev  = s;
    }

    // Compute angular frequency for reconstruction: omega = 2*pi*k/N
    double omega = 2.0 * M_PI * cfg->target_bin / cfg->sample_count;

    // Real part of the DFT coefficient
    double real_part = s_prev - cos(omega) * s_prev2;

    // Imaginary part (note the sign flip to match DFT convention)
    double imag_part = -sin(omega) * s_prev2;

    // Compute raw phase angle
    double phase = atan2(imag_part, real_part);

    // Compensate for the extra omega step in the recurrence
    phase -= omega;

    return phase;
}

double et_goertzel_compute_phase_f32(const et_goertzel_config_t *cfg, const float *data)
{
    double s_prev  = 0.0;
    double s_prev2 = 0.0;
    double s;

    // Run recurrence over all samples
    for (int i = 0; i < cfg->sample_count; i++)
    {
        s       = (double)data[i] - cfg->dc_offset + cfg->coefficient * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev  = s;
    }

    double omega      = 2.0 * M_PI * cfg->target_bin / cfg->sample_count;
    double real_part  = s_prev - cos(omega) * s_prev2;
    double imag_part  = -sin(omega) * s_prev2;
    double phase      = atan2(imag_part, real_part);
    phase            -= omega;

    return phase;
}

double et_goertzel_compute_phase_u16(const et_goertzel_config_t *cfg, const uint16_t *data)
{
    double s_prev  = 0.0;
    double s_prev2 = 0.0;
    double s;

    // Run recurrence over all samples
    for (int i = 0; i < cfg->sample_count; i++)
    {
        s       = ((double)data[i] - cfg->dc_offset) + cfg->coefficient * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev  = s;
    }

    double omega      = 2.0 * M_PI * cfg->target_bin / cfg->sample_count;
    double real_part  = s_prev - cos(omega) * s_prev2;
    double imag_part  = -sin(omega) * s_prev2;
    double phase      = atan2(imag_part, real_part);
    phase            -= omega;

    return phase;
}

double et_goertzel_compute_phase_u32(const et_goertzel_config_t *cfg, const uint32_t *data)
{
    double s_prev  = 0.0;
    double s_prev2 = 0.0;
    double s;

    // Run recurrence over all samples
    for (int i = 0; i < cfg->sample_count; i++)
    {
        s       = ((double)data[i] - cfg->dc_offset) + cfg->coefficient * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev  = s;
    }

    double omega      = 2.0 * M_PI * cfg->target_bin / cfg->sample_count;
    double real_part  = s_prev - cos(omega) * s_prev2;
    double imag_part  = -sin(omega) * s_prev2;
    double phase      = atan2(imag_part, real_part);
    phase            -= omega;

    return phase;
}

/**
 * @brief Find the dominant (center) frequency using Goertzel scanning.
 *
 * This function scans a frequency range (e.g., 20Hz–100Hz) and returns the frequency
 * with the maximum energy response. Suitable for real-time frequency tracking.
 *
 * @param signal        Input signal array (length = num_samples)
 * @param num_samples   Number of samples in signal
 * @param sample_rate   Sampling rate in Hz
 * @param f_min         Minimum frequency to scan (e.g., 20.0)
 * @param f_max         Maximum frequency to scan (e.g., 100.0)
 * @param f_step        Frequency resolution (e.g., 1.0 Hz)
 * @param dc_offset     DC offset value for ADC signal processing
 * @return              Frequency (Hz) with strongest energy (center freq)
 */
double et_goertzel_find_center_frequency_f64(const double *signal, int num_samples, double sample_rate, double f_min,
                                             double f_max, double f_step, double dc_offset)
{
    double max_power = 0.0;
    double best_freq = 0.0;

    et_goertzel_config_t cfg;

    for (double freq = f_min; freq <= f_max; freq += f_step)
    {
        int bin = et_goertzel_calc_bin(freq, sample_rate, num_samples);
        et_goertzel_init(&cfg, num_samples, bin, dc_offset);
        double power = et_goertzel_compute_power_f64(&cfg, signal);

        if (power > max_power)
        {
            max_power = power;
            best_freq = freq;
        }
    }

    return best_freq;
}

double et_goertzel_find_center_frequency_f32(const float *signal, int num_samples, double sample_rate, double f_min,
                                             double f_max, double f_step, double dc_offset)
{
    double               max_power = 0.0;
    double               best_freq = 0.0;
    et_goertzel_config_t cfg;

    for (double freq = f_min; freq <= f_max; freq += f_step)
    {
        int bin = et_goertzel_calc_bin(freq, sample_rate, num_samples);
        et_goertzel_init(&cfg, num_samples, bin, dc_offset);
        double power = et_goertzel_compute_power_f32(&cfg, signal);
        if (power > max_power)
        {
            max_power = power;
            best_freq = freq;
        }
    }

    return best_freq;
}

double et_goertzel_find_center_frequency_u16(const uint16_t *signal, int num_samples, double sample_rate, double f_min,
                                             double f_max, double f_step, double dc_offset)
{
    double               max_power = 0.0;
    double               best_freq = 0.0;
    et_goertzel_config_t cfg;

    for (double freq = f_min; freq <= f_max; freq += f_step)
    {
        int bin = et_goertzel_calc_bin(freq, sample_rate, num_samples);
        et_goertzel_init(&cfg, num_samples, bin, dc_offset);
        double power = et_goertzel_compute_power_u16(&cfg, signal);

        if (power > max_power)
        {
            max_power = power;
            best_freq = freq;
        }
    }

    return best_freq;
}

double et_goertzel_find_center_frequency_u32(const uint32_t *signal, int num_samples, double sample_rate, double f_min,
                                             double f_max, double f_step, double dc_offset)
{
    double               max_power = 0.0;
    double               best_freq = 0.0;
    et_goertzel_config_t cfg;

    for (double freq = f_min; freq <= f_max; freq += f_step)
    {
        int bin = et_goertzel_calc_bin(freq, sample_rate, num_samples);
        et_goertzel_init(&cfg, num_samples, bin, dc_offset);
        double power = et_goertzel_compute_power_u32(&cfg, signal);
        if (power > max_power)
        {
            max_power = power;
            best_freq = freq;
        }
    }

    return best_freq;
}


