/*
 * @Author: mypx
 * @Date: 2025-08-21 10:11:11
 * @LastEditTime: 2025-08-27 17:20:20
 * @LastEditors: mypx mypx_coder@163.com
 * @Description: 
 * This module provides functions to detect zero-crossings in
 * sampled signals, with optional DC offset removal and linear
 * interpolation for more accurate crossing positions.
 *
 * Supports both floating-point and unsigned 16-bit integer input.
 */
#include "et_zero_cross.h"
#include <math.h>
#include <string.h>

static inline int et_float_equal(float a, float b)
{
    return fabs(a - b) < ET_ZERO_CROSS_EPSILON;
}

// Add float comparison function for qsort
static int compare_float(const void *a, const void *b)
{
    float fa = *(const float *)a;
    float fb = *(const float *)b;
    return (fa > fb) - (fa < fb);
}

/* helper: median calculation */
static float et_calc_median(float *arr, int n)
{
    if (n <= 0)
        return 0.0f;
    float *tmp = (float *)malloc(n * sizeof(float));
    if (!tmp)
        return 0.0f;

    memcpy(tmp, arr, n * sizeof(float));
    qsort(tmp, n, sizeof(float), compare_float);

    float median;
    if (n % 2 == 0)
        median = (tmp[n / 2 - 1] + tmp[n / 2]) * 0.5f;
    else
        median = tmp[n / 2];

    free(tmp);
    return median;
}

/**
 * @brief Count zero crossings (uint16_t input)
 * @param data   Pointer to input uint16_t array
 * @param length Number of samples
 * @param offset DC offset to subtract
 * @return int   Number of zero crossings
 */
uint32_t et_zero_cross_count_u16(const uint16_t *data, uint32_t length, uint16_t offset)
{
    if (data == NULL || length <= 1)
        return 0;

    uint32_t count = 0;
    int      prev  = (int)data[0] - (int)offset;

    for (uint32_t i = 1; i < length; i++)
    {
        int cur = (int)data[i] - (int)offset;
        if ((prev > 0 && cur <= 0) || (prev < 0 && cur >= 0))
        {
            count++;
        }
        prev = cur;
    }
    return count;
}

/**
 * @brief Detect zero-crossing positions with interpolation (uint16_t input)
 * @param zc     Pointer to et_zero_cross_t structure for storing results
 * @param data   Pointer to input uint16_t array
 * @param length Number of samples
 * @param offset DC offset to subtract
 * @return int   Number of zero crossings detected
 */
uint32_t et_zero_cross_pos_u16(et_zero_cross_t *zc, const uint16_t *data, uint32_t length, uint16_t offset)
{
    if (zc == NULL || data == NULL || length <= 1)
        return 0;

    uint32_t count = 0;
    int      prev  = (int)data[0] - (int)offset;

    for (uint32_t i = 1; i < length; i++)
    {
        int cur = (int)data[i] - (int)offset;
        // 只有当前后两点跨过零点时才认为是有效的零点
        if ((prev > 0 && cur <= 0) || (prev < 0 && cur >= 0))
        {
            if (zc && zc->pos_buf && count < zc->max_pos_size)
            {
                // 改进的插值计算
                float frac;
                if (fabsf((float)(prev - cur)) > 1e-6f)
                {
                    // 使用线性插值计算更精确的零点位置
                    frac = (float)prev / ((float)prev - (float)cur);
                    if (frac < 0.0f)
                        frac = 0.0f;
                    if (frac > 1.0f)
                        frac = 1.0f;
                }
                else
                {
                    // 如果两点非常接近，取中点
                    frac = 0.5f;
                }
                zc->pos_buf[count] = (float)(i - 1) + frac;
            }
            count++;
        }
        prev = cur;
    }

    zc->count = count;

    return count;
}

/**
 * @brief Estimate signal frequency using zero-crossing method (uint16_t input)
 * @param zc            Pointer to et_zero_cross_t structure for storing results
 * @param data          Pointer to input uint16_t array
 * @param length        Number of samples
 * @param sampling_rate Sampling rate in Hz
 * @param offset        DC offset to subtract
 * @return float        Estimated frequency in Hz
 */
float et_zero_cross_freq_u16(et_zero_cross_t *zc, const uint16_t *data, uint32_t length, float sampling_rate,
                             uint16_t offset)
{
    if (data == NULL || length <= 1)
        return 0.0f;

    uint32_t zero_count = et_zero_cross_pos_u16(zc, data, length, offset);
    if (zero_count < 2)
        return 0.0f;

    float    total_interval = 0.0f;
    uint32_t valid          = 0;

    if (zc && zc->pos_buf)
    {
        for (uint32_t i = 1; i < zero_count && i < zc->max_pos_size; i++)
        {
            float interval = zc->pos_buf[i] - zc->pos_buf[i - 1];
            if (interval > 0.0f)
            {
                total_interval += interval;
                valid++;
            }
        }
    }

    if (valid == 0)
        return 0.0f;

    float avg_interval = total_interval / valid;
    // A complete cycle contains two zero crossings (positive-to-negative and negative-to-positive)
    float period_samples = avg_interval * 2.0f;
    // Avoid division by zero
    if (period_samples < 1e-10f)
    {
        return 0.0f;
    }
    return sampling_rate / period_samples;
}

/**
 * @brief Enhanced frequency estimation with median filtering and outlier rejection (uint16_t input)
 * @param zc            Pointer to et_zero_cross_t structure for storing results
 * @param data          Pointer to input uint16_t array
 * @param length        Number of samples
 * @param sampling_rate Sampling rate in Hz
 * @param offset        DC offset to subtract
 * @return float        Estimated frequency in Hz
 */
float et_zero_cross_freq_u16_ex(et_zero_cross_t *zc, const uint16_t *data, uint32_t length, float sampling_rate,
                                uint16_t offset)
{
    if (data == NULL || length <= 1 || !zc || !zc->pos_buf)
        return 0.0f;

    uint32_t zero_count = et_zero_cross_pos_u16(zc, data, length, offset);
    if (zero_count < 3)
        return 0.0f;

    uint32_t valid = 0;
    for (uint32_t i = 1; i < zero_count && i < zc->max_pos_size; i++)
    {
        float interval = zc->pos_buf[i] - zc->pos_buf[i - 1];
        if (interval > 0.0f)
        {
            zc->interval_buf[valid++] = interval;
        }
    }

    if (valid < 2)
        return 0.0f;

    float    median_interval = et_calc_median(zc->interval_buf, valid);
    float    total           = 0.0f;
    uint32_t count           = 0;

    for (uint32_t i = 0; i < valid; i++)
    {
        if (zc->interval_buf[i] > zc->thresh_low * median_interval
            && zc->interval_buf[i] < zc->thresh_high * median_interval)
        {
            total += zc->interval_buf[i];
            count++;
        }
    }

    if (count == 0)
        return 0.0f;

    float avg_interval = total / count;
    // A complete cycle contains two zero crossings (positive-to-negative and negative-to-positive)
    float period_samples = avg_interval * 2.0f;
    // Avoid division by zero
    if (period_samples < 1e-10f)
    {
        return 0.0f;
    }
    return sampling_rate / period_samples;
}

/**
 * @brief Count zero-crossings in floating-point data.
 * @param zc     Pointer to et_zero_cross_t structure for storing results
 * @param data   Input data array
 * @param length Number of samples
 * @param offset DC offset to subtract from each sample
 * @return int   Number of zero-crossings detected
 */
uint32_t et_zero_cross_count_f32(const float *data, uint32_t length, float offset)
{
    if (data == NULL || length <= 1)
        return 0;

    uint32_t count = 0;
    float    prev  = data[0] - offset;

    for (uint32_t i = 1; i < length; i++)
    {
        float cur = data[i] - offset;
        if ((prev > 0.0f && cur <= 0.0f) || (prev < 0.0f && cur >= 0.0f)) // Modified judgment condition
        {
            count++;
        }
        prev = cur;
    }
    return count;
}

/**
 * @brief Detect zero-crossing positions with interpolation (float input)
 * @param zc     Pointer to et_zero_cross_t structure for storing results
 * @param data   Pointer to input float array
 * @param length Number of samples
 * @param offset DC offset to subtract
 * @return int   Number of zero crossings detected
 */
uint32_t et_zero_cross_pos_f32(et_zero_cross_t *zc, const float *data, uint32_t length, float offset)
{
    if (data == NULL || length <= 1)
        return 0;

    uint32_t count = 0;
    float    prev  = data[0] - offset;

    for (uint32_t i = 1; i < length; i++)
    {
        float cur = data[i] - offset;
        // 使用更严格的零点判断条件
        if ((prev > ET_ZERO_CROSS_EPSILON && cur <= -ET_ZERO_CROSS_EPSILON)
            || (prev < -ET_ZERO_CROSS_EPSILON && cur >= ET_ZERO_CROSS_EPSILON))
        {
            if (zc && zc->pos_buf && count < zc->max_pos_size)
            {
                float frac;
                float denominator = prev - cur;
                if (fabsf(denominator) > ET_ZERO_CROSS_EPSILON)
                {
                    // 改进的线性插值计算
                    frac = prev / denominator;
                    // 确保插值结果在有效范围内
                    if (frac < 0.0f)
                        frac = 0.0f;
                    if (frac > 1.0f)
                        frac = 1.0f;
                }
                else
                {
                    // 数值太接近时取中点
                    frac = 0.5f;
                }
                zc->pos_buf[count] = (float)(i - 1) + frac;
            }
            count++;
        }
        prev = cur;
    }

    if (zc != NULL)
    {
        zc->count = count;
    }

    return count;
}

/**
 * @brief Estimate signal frequency using zero-crossing method (float input)
 * @param zc            Pointer to et_zero_cross_t structure for storing results
 * @param data          Pointer to input float array
 * @param length        Number of samples
 * @param sampling_rate Sampling rate in Hz
 * @param offset        DC offset to subtract
 * @return float        Estimated frequency in Hz
 */
float et_zero_cross_freq_f32(et_zero_cross_t *zc, const float *data, uint32_t length, float sampling_rate, float offset)
{
    if (data == NULL || length <= 1)
        return 0.0f;

    uint32_t zero_count = et_zero_cross_pos_f32(zc, data, length, offset);
    if (zero_count < 2)
        return 0.0f;

    float    total_interval = 0.0f;
    uint32_t valid          = 0;

    if (zc && zc->pos_buf)
    {
        for (uint32_t i = 1; i < zero_count && i < zc->max_pos_size; i++)
        {
            float interval = zc->pos_buf[i] - zc->pos_buf[i - 1];
            if (interval > 0.0f)
            {
                total_interval += interval;
                valid++;
            }
        }
    }

    if (valid == 0)
        return 0.0f;

    float avg_interval = total_interval / valid;
    // A complete cycle contains two zero crossings (positive-to-negative and negative-to-positive)
    float period_samples = avg_interval * 2.0f;
    // Avoid division by zero
    if (period_samples < 1e-10f)
    {
        return 0.0f;
    }
    return sampling_rate / period_samples;
}

/**
 * @brief Enhanced frequency estimation with median filtering and outlier rejection (float input)
 * @param zc            Pointer to et_zero_cross_t structure for storing results
 * @param data          Pointer to input float array
 * @param length        Number of samples
 * @param sampling_rate Sampling rate in Hz
 * @param offset        DC offset to subtract
 * @return float        Estimated frequency in Hz
 */
float et_zero_cross_freq_f32_ex(et_zero_cross_t *zc, const float *data, uint32_t length, float sampling_rate,
                                float offset)
{
    if (data == NULL || length <= 1 || !zc || !zc->pos_buf)
        return 0.0f;

    uint32_t zero_count = et_zero_cross_pos_f32(zc, data, length, offset);
    if (zero_count < 3)
        return 0.0f;

    // Calculate and store intervals in the first half of the buffer
    uint32_t valid = 0;
    for (uint32_t i = 1; i < zero_count && i < zc->max_pos_size; i++)
    {
        float interval = zc->pos_buf[i] - zc->pos_buf[i - 1];
        if (interval > 0.0f)
        {
            zc->interval_buf[valid++] = interval;
        }
    }

    if (valid < 2)
        return 0.0f;

    float    median_interval = et_calc_median(zc->interval_buf, valid);
    float    total           = 0.0f;
    uint32_t count           = 0;

    for (uint32_t i = 0; i < valid; i++)
    {
        if (zc->interval_buf[i] > zc->thresh_low * median_interval
            && zc->interval_buf[i] < zc->thresh_high * median_interval)
        {
            total += zc->interval_buf[i];
            count++;
        }
    }

    if (count == 0)
        return 0.0f;

    float avg_interval = total / count;
    // A complete cycle contains two zero crossings (positive-to-negative and negative-to-positive)
    float period_samples = avg_interval * 2.0f;
    // Avoid division by zero
    if (et_float_equal(period_samples, 0.0f))
    {
        return 0.0f;
    }
    return sampling_rate / period_samples;
}

int et_zero_cross_set_threshold(et_zero_cross_t *zc, float thresh_low, float thresh_high)
{
    if (!zc || thresh_low < ET_ZERO_CROSS_MIN_THRESH || thresh_high > ET_ZERO_CROSS_MAX_THRESH
        || thresh_high <= thresh_low)
    {
        return -1;
    }

    zc->thresh_low  = thresh_low;
    zc->thresh_high = thresh_high;
    return 0;
}

int et_zero_cross_create(et_zero_cross_t *zc, uint32_t max_pos)
{
    if (zc == NULL || max_pos == 0)
        return -1;

    memset(zc, 0, sizeof(et_zero_cross_t));

    zc->pos_buf = (float *)malloc(max_pos * sizeof(float));
    if (zc->pos_buf == NULL)
        return -1;
    zc->interval_buf = (float *)malloc(max_pos * sizeof(float));
    if (zc->interval_buf == NULL)
        return -1;

    zc->max_pos_size = max_pos;
    zc->thresh_low   = ET_ZERO_CROSS_DEFAULT_LOW_THRESH;
    zc->thresh_high  = ET_ZERO_CROSS_DEFAULT_HIGH_THRESH;
    return 0;
}

void et_zero_cross_destroy(et_zero_cross_t *zc)
{
    if (zc == NULL)
        return;

    if (zc->pos_buf)
    {
        free(zc->pos_buf);
        zc->pos_buf = NULL;
    }
    if(zc->interval_buf)
    {
        free(zc->interval_buf);
        zc->interval_buf = NULL;
    }
    memset(zc, 0, sizeof(et_zero_cross_t));
}

/**
 * @brief Count zero crossings (uint32_t input)
 * @param data   Pointer to input uint32_t array
 * @param length Number of samples
 * @param offset DC offset to subtract
 * @return int   Number of zero crossings
 */
uint32_t et_zero_cross_count_u32(const uint32_t *data, uint32_t length, uint32_t offset)
{
    if (data == NULL || length <= 1)
        return 0;

    uint32_t count = 0;
    int64_t  prev  = (int64_t)data[0] - (int64_t)offset;

    for (uint32_t i = 1; i < length; i++)
    {
        int64_t cur = (int64_t)data[i] - (int64_t)offset;
        if ((prev > 0 && cur <= 0) || (prev < 0 && cur >= 0))
        {
            count++;
        }
        prev = cur;
    }
    return count;
}

/**
 * @brief Detect zero-crossing positions with interpolation (uint32_t input)
 * @param zc     Pointer to et_zero_cross_t structure for storing results
 * @param data   Pointer to input uint32_t array
 * @param length Number of samples
 * @param offset DC offset to subtract
 * @return int   Number of zero crossings detected
 */
uint32_t et_zero_cross_pos_u32(et_zero_cross_t *zc, const uint32_t *data, uint32_t length, uint32_t offset)
{
    if (data == NULL || length <= 1)
        return 0;

    uint32_t count = 0;
    int64_t  prev  = (int64_t)data[0] - (int64_t)offset;

    for (uint32_t i = 1; i < length; i++)
    {
        int64_t cur = (int64_t)data[i] - (int64_t)offset;
        if ((prev > 0 && cur <= 0) || (prev < 0 && cur >= 0))
        {
            if (zc && zc->pos_buf && count < zc->max_pos_size)
            {
                float   frac = 0.5f; // default fraction
                int64_t diff = prev - cur;
                if (diff != 0)
                {
                    frac = (float)prev / (float)diff; // linear interpolation
                }
                zc->pos_buf[count] = (float)(i - 1) + frac;
            }
            count++;
        }
        prev = cur;
    }

    if (zc != NULL)
    {
        zc->count = count;
    }

    return count;
}

/**
 * @brief Estimate signal frequency using zero-crossing method (uint32_t input)
 * @param zc            Pointer to et_zero_cross_t structure for storing results
 * @param data          Pointer to input uint32_t array
 * @param length        Number of samples
 * @param sampling_rate Sampling rate in Hz
 * @param offset        DC offset to subtract
 * @return float        Estimated frequency in Hz
 */
float et_zero_cross_freq_u32(et_zero_cross_t *zc, const uint32_t *data, uint32_t length, float sampling_rate,
                             uint32_t offset)
{
    if (data == NULL || length <= 1)
        return 0.0f;

    uint32_t zero_count = et_zero_cross_pos_u32(zc, data, length, offset);
    if (zero_count < 2)
        return 0.0f;

    float    total_interval = 0.0f;
    uint32_t valid          = 0;

    if (zc && zc->pos_buf)
    {
        for (uint32_t i = 1; i < zero_count && i < zc->max_pos_size; i++)
        {
            float interval = zc->pos_buf[i] - zc->pos_buf[i - 1];
            if (interval > 0.0f)
            {
                total_interval += interval;
                valid++;
            }
        }
    }

    if (valid == 0)
        return 0.0f;

    float avg_interval = total_interval / valid;
    // A complete cycle contains two zero crossings (positive-to-negative and negative-to-positive)
    float period_samples = avg_interval * 2.0f;
    // Avoid division by zero
    if (period_samples < 1e-10f)
    {
        return 0.0f;
    }
    return sampling_rate / period_samples;
}

/**
 * @brief Enhanced frequency estimation with median filtering and outlier rejection (uint32_t input)
 * @param zc            Pointer to et_zero_cross_t structure for storing results
 * @param data          Pointer to input uint32_t array
 * @param length        Number of samples
 * @param sampling_rate Sampling rate in Hz
 * @param offset        DC offset to subtract
 * @return float        Estimated frequency in Hz
 */
float et_zero_cross_freq_u32_ex(et_zero_cross_t *zc, const uint32_t *data, uint32_t length, float sampling_rate,
                                uint32_t offset)
{
    if (data == NULL || length <= 1 || !zc || !zc->pos_buf)
        return 0.0f;

    uint32_t zero_count = et_zero_cross_pos_u32(zc, data, length, offset);
    if (zero_count < 3)
        return 0.0f;

    uint32_t valid = 0;
    for (uint32_t i = 1; i < zero_count && i < zc->max_pos_size; i++)
    {
        float interval = zc->pos_buf[i] - zc->pos_buf[i - 1];
        if (interval > 0.0f)
        {
            zc->interval_buf[valid++] = interval;
        }
    }

    if (valid < 2)
        return 0.0f;

    float    median_interval = et_calc_median(zc->interval_buf, valid);
    float    total           = 0.0f;
    uint32_t count           = 0;

    for (uint32_t i = 0; i < valid; i++)
    {
        if (zc->interval_buf[i] > zc->thresh_low * median_interval
            && zc->interval_buf[i] < zc->thresh_high * median_interval)
        {
            total += zc->interval_buf[i];
            count++;
        }
    }

    if (count == 0)
        return 0.0f;

    float avg_interval = total / count;
    // A complete cycle contains two zero crossings (positive-to-negative and negative-to-positive)
    float period_samples = avg_interval * 2.0f;
    // Avoid division by zero
    if (period_samples < 1e-10f)
    {
        return 0.0f;
    }
    return sampling_rate / period_samples;
}

uint32_t et_zero_cross_count_point(const et_zc_point_t *point, uint32_t length, uint16_t offset)
{
    if (point == NULL || length <= 1)
        return 0;

    uint32_t count = 0;
    int      prev  = (int)point[0].val - (int)offset;

    for (uint32_t i = 1; i < length; i++)
    {
        int cur = (int)point[i].val - (int)offset;
        if ((prev > 0 && cur <= 0) || (prev < 0 && cur >= 0))
        {
            count++;
        }
        prev = cur;
    }
    return count;
}

uint32_t et_zero_cross_pos_point(et_zero_cross_t *zc, const et_zc_point_t *point, uint32_t length, uint16_t offset)
{
    if (point == NULL || length <= 1)
        return 0;

    uint32_t count = 0;
    int      prev  = (int)point[0].val - (int)offset;

    for (uint32_t i = 1; i < length; i++)
    {
        int cur = (int)point[i].val - (int)offset;
        if ((prev > 0 && cur <= 0) || (prev < 0 && cur >= 0))
        {
            if (zc && zc->pos_buf && count < zc->max_pos_size)
            {
                float frac = 0.5f; // default fraction
                int   diff = prev - cur;
                if (diff != 0)
                {
                    frac = (float)prev / (float)diff; // linear interpolation
                }
                zc->pos_buf[count] = (float)(i - 1) + frac;
            }
            count++;
        }
        prev = cur;
    }

    if (zc != NULL)
    {
        zc->count = count;
    }

    return count;
}

float et_zero_cross_freq_point(et_zero_cross_t *zc, const et_zc_point_t *point, uint32_t length, uint16_t offset)
{
    if (point == NULL || length <= 1)
        return 0.0f;

    // First, get the zero crossing positions
    uint32_t zero_count = et_zero_cross_pos_point(zc, point, length, offset);
    if (zero_count < 2)
        return 0.0f;

    // Use timestamps to calculate actual time intervals
    double   total_interval = 0.0;
    uint32_t valid          = 0;

    if (zc && zc->pos_buf)
    {
        for (uint32_t i = 1; i < zero_count && i < zc->max_pos_size; i++)
        {
            // Get the index positions of two zero crossing points
            uint32_t idx1 = (uint32_t)zc->pos_buf[i - 1];
            uint32_t idx2 = (uint32_t)zc->pos_buf[i];

            // Ensure indices are within valid range
            if (idx1 < length && idx2 < length)
            {
                // Calculate actual time interval using timestamps
                double interval = (double)point[idx2].time - (double)point[idx1].time;
                if (interval > 0.0)
                {
                    total_interval += interval;
                    valid++;
                }
            }
        }
    }

    if (valid == 0)
        return 0.0f;

    // Calculate average time interval (unit could be seconds, milliseconds, etc., depending on time field)
    double avg_interval = total_interval / valid;

    // A complete cycle contains two zero crossings (positive-to-negative and negative-to-positive)
    double period_time = avg_interval * 2.0;

    // Avoid division by zero
    if (period_time < 1e-10)
    {
        return 0.0f;
    }

    // Calculate frequency: convert microseconds to seconds, then freq = 1.0 / period_time
    // time field is in microseconds, so we need to convert to seconds
    double period_seconds = period_time / 1000000.0; // Convert from microseconds to seconds
    return (float)(1.0 / period_seconds);
}

/**
 * @brief Enhanced frequency estimation with median filtering and outlier rejection (et_zc_point_t input)
 * @param zc            Pointer to et_zero_cross_t structure for storing results
 * @param point         Pointer to input et_zc_point_t array
 * @param length        Number of samples
 * @param offset        DC offset to subtract
 * @return float        Estimated frequency in Hz
 */
float et_zero_cross_freq_point_ex(et_zero_cross_t *zc, const et_zc_point_t *point, uint32_t length, uint16_t offset)
{
    if (point == NULL || length <= 1 || !zc || !zc->pos_buf || !zc->interval_buf)
        return 0.0f;

    // First, get the zero crossing positions
    uint32_t zero_count = et_zero_cross_pos_point(zc, point, length, offset);
    if (zero_count < 3)
        return 0.0f;

    // Calculate and store time intervals in the buffer
    uint32_t valid = 0;
    for (uint32_t i = 1; i < zero_count && i < zc->max_pos_size; i++)
    {
        // Get the index positions of two zero crossing points
        uint32_t idx1 = (uint32_t)zc->pos_buf[i - 1];
        uint32_t idx2 = (uint32_t)zc->pos_buf[i];

        // Ensure indices are within valid range
        if (idx1 < length && idx2 < length)
        {
            // Calculate actual time interval using timestamps
            double interval = (double)point[idx2].time - (double)point[idx1].time;
            if (interval > 0.0)
            {
                // Reuse buffer for storing intervals
                zc->interval_buf[valid++] = (float)interval;
            }
        }
    }

    if (valid < 2)
        return 0.0f;

    // Calculate median interval for outlier rejection
    float    median_interval = et_calc_median(zc->pos_buf, valid);
    float    total           = 0.0f;
    uint32_t count           = 0;

    // Filter outliers based on threshold multipliers
    for (uint32_t i = 0; i < valid; i++)
    {
        if (zc->interval_buf[i] > zc->thresh_low * median_interval
            && zc->interval_buf[i] < zc->thresh_high * median_interval)
        {
            total += zc->interval_buf[i];
            count++;
        }
    }

    if (count == 0)
        return 0.0f;

    // Calculate average interval from filtered data
    float avg_interval = total / count;

    // A complete cycle contains two zero crossings (positive-to-negative and negative-to-positive)
    double period_time = avg_interval * 2.0;

    // Avoid division by zero
    if (period_time < 1e-10)
    {
        return 0.0f;
    }

    // Calculate frequency: convert microseconds to seconds, then freq = 1.0 / period_time
    // time field is in microseconds, so we need to convert to seconds
    double period_seconds = period_time / 1000000.0; // Convert from microseconds to seconds
    return (float)(1.0 / period_seconds);
}
