#include "biquad_standalone.h"
#include <math.h>
#include <limits.h>

// 常量定义
#define Q_factor 30
#define BOOST_BSHIFT 2

// 如果没有定义M_PI，则手动定义
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

static const float pi = (float)M_PI;
static const float log_2 = 0.69314718055f;
static const float db_2 = 6.02059991328f;  // 20*log10(2)

// 辅助函数实现
static inline int32_t _float2fixed(float x, int32_t q) {
    if (x < 0.0f) return (((float)(1u << q)) * x - 0.5f);
    if (x > 0.0f) return (((float)((1u << q) - 1)) * x + 0.5f);
    return 0;
}

static inline int32_t _float2fixed_assert(float x, int32_t q) {
    float max_val = (float)(1 << (31 - q));
    // 简单的断言检查，如果超出范围则返回边界值
    if (x > max_val) x = max_val;
    if (x < -max_val) x = -max_val;
    return _float2fixed(x, q);
}

// 简单的三角函数实现，使用标准库 - 重命名以避免冲突
static inline float biquad_sin(float x) {
    return sinf(x);
}

static inline float biquad_cos(float x) {
    return cosf(x);
}

static inline float _check_fc(float fc, float fs) {
    float fc_sat = fc;
    // saturate if > fs/2
    if (fc_sat >= fs / 2.0f) {
        fc_sat = fs / 2.0f;
    }
    return fc_sat;
}

static inline float _check_gain(float gain, float max_gain) {
    float gain_sat = gain;
    // saturate if > max_gain
    if (gain_sat >= max_gain) {
        gain_sat = max_gain;
    }
    return gain_sat;
}

void eq_biquad_bypass(q2_30 coeffs[5]) {
    coeffs[0] = 1 << Q_factor;
    coeffs[1] = 0;
    coeffs[2] = 0;
    coeffs[3] = 0;
    coeffs[4] = 0;
}

void eq_biquad_mute(q2_30 coeffs[5]) {
    coeffs[0] = 0;
    coeffs[1] = 0;
    coeffs[2] = 0;
    coeffs[3] = 0;
    coeffs[4] = 0;
}

left_shift_t eq_biquad_gain(q2_30 coeffs[5], const float gain_db) {
    float A = powf(10.0f, (gain_db * (1.0f / 20.0f)));

    coeffs[0] = _float2fixed_assert(A, Q_factor - BOOST_BSHIFT);
    coeffs[1] = 0;
    coeffs[2] = 0;
    coeffs[3] = 0;
    coeffs[4] = 0;

    return BOOST_BSHIFT;
}

void eq_biquad_lowpass(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q) {
    float fc_sat = _check_fc(fc, fs);

    // Compute common factors
    float K = tanf(pi * fc_sat / fs);
    float KK = K * K;
    float KQ = K / filter_Q;
    float norm = 1.0f / (1.0f + KQ + KK);

    // Compute coeffs
    float b0 = KK * norm;
    float b1 = 2.0f * b0;
    float b2 = b0;
    float a1 = 2.0f * (KK - 1.0f) * norm;
    float a2 = (1.0f - KQ + KK) * norm;

    //DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0, b1, b2, a1, a2);
    // Store as fixed-point values
    coeffs[0] = _float2fixed_assert(b0, Q_factor);
    coeffs[1] = _float2fixed_assert(b1, Q_factor);
    coeffs[2] = _float2fixed_assert(b2, Q_factor);
    coeffs[3] = _float2fixed_assert(-a1, Q_factor);
    coeffs[4] = _float2fixed_assert(-a2, Q_factor);
}

void eq_biquad_highpass(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q) {
    float fc_sat = _check_fc(fc, fs);

    // Compute common factors
    float K = tanf(pi * fc_sat / fs);
    float KK = K * K;
    float KQ = K / filter_Q;
    float norm = 1.0f / (1.0f + KQ + KK);

    // Compute coeffs
    float b0 = norm;
    float b1 = -2.0f * b0;
    float b2 = b0;
    float a1 = 2.0f * (KK - 1.0f) * norm;
    float a2 = (1.0f - KQ + KK) * norm;

    // Store as fixed-point values
    coeffs[0] = _float2fixed_assert(b0, Q_factor);
    coeffs[1] = _float2fixed_assert(b1, Q_factor);
    coeffs[2] = _float2fixed_assert(b2, Q_factor);
    coeffs[3] = _float2fixed_assert(-a1, Q_factor);
    coeffs[4] = _float2fixed_assert(-a2, Q_factor);
}

void eq_biquad_allpass(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q) {
    float fc_sat = _check_fc(fc, fs);

    float K = tanf(pi * fc_sat / fs);
    float norm = 1.0f / (1.0f + K / filter_Q + K * K);

    float b0 = (1.0f - K / filter_Q + K * K) * norm;
    float b1 = 2.0f * (K * K - 1.0f) * norm;
    float b2 = 1.0f;
    float a1 = b1;
    float a2 = b0;

    coeffs[0] = _float2fixed_assert(b0, Q_factor);
    coeffs[1] = _float2fixed_assert(b1, Q_factor);
    coeffs[2] = _float2fixed_assert(b2, Q_factor);
    coeffs[3] = _float2fixed_assert(-a1, Q_factor);
    coeffs[4] = _float2fixed_assert(-a2, Q_factor);
}

// 动态计算bshift的函数
static left_shift_t calculate_bshift(float b0, float b1, float b2) {
    float max_b = fabsf(b0);
    if (fabsf(b1) > max_b) max_b = fabsf(b1);
    if (fabsf(b2) > max_b) max_b = fabsf(b2);
    if (max_b == 0.0f) return 0;
    int bshift = 0;
    if (max_b >= 1.0f) {
        float temp = max_b;
        while (temp >= 2.0f) { temp /= 2.0f; bshift++; }
    } else {
        float temp = max_b;
        while (temp < 1.0f) { temp *= 2.0f; bshift--; }
        bshift--;
    }
    return (bshift >= 0) ? bshift : 0;
}

left_shift_t eq_biquad_peaking(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q, const float gain_db) {
    float gain_db_sat = _check_gain(gain_db, 20.0f * db_2);
    float fc_sat = _check_fc(fc, fs);
    float V = powf(10.0f, fabsf(gain_db_sat) / 20.0f);
    float K = tanf(pi * fc_sat / fs);
    float norm, b0, b1, b2, a1, a2;

    if (gain_db_sat >= 0) {
        norm = 1.0f / (1.0f + 1.0f/filter_Q * K + K * K);
        b0 = (1.0f + V/filter_Q * K + K * K) * norm;
        b1 = 2.0f * (K * K - 1.0f) * norm;
        b2 = (1.0f - V/filter_Q * K + K * K) * norm;
        a1 = b1;
        a2 = (1.0f - 1.0f/filter_Q * K + K * K) * norm;
    } else {
        norm = 1.0f / (1.0f + V/filter_Q * K + K * K);
        b0 = (1.0f + 1.0f/filter_Q * K + K * K) * norm;
        b1 = 2.0f * (K * K - 1.0f) * norm;
        b2 = (1.0f - 1.0f/filter_Q * K + K * K) * norm;
        a1 = b1;
        a2 = (1.0f - V/filter_Q * K + K * K) * norm;
    }

    left_shift_t bshift = calculate_bshift(b0, b1, b2);
    coeffs[0] = _float2fixed_assert(b0, Q_factor - bshift);
    coeffs[1] = _float2fixed_assert(b1, Q_factor - bshift);
    coeffs[2] = _float2fixed_assert(b2, Q_factor - bshift);
    coeffs[3] = _float2fixed_assert(-a1, Q_factor);
    coeffs[4] = _float2fixed_assert(-a2, Q_factor);
    return bshift;
}

left_shift_t eq_biquad_lowshelf(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q, const float gain_db) {
    float gain_db_sat = _check_gain(gain_db, 20.0f * db_2);
    float fc_sat = _check_fc(fc, fs);

    // 使用与Python端BiquadFilterCalculator.calculate("lowshelf")完全一致的算法
    float V = powf(10.0f, fabsf(gain_db_sat) / 20.0f);  // 与Python端一致
    float K = tanf(pi * fc_sat / fs);  // 与Python端一致：K = tan(π * Fc / Fs)
    float norm = 0.0f;

    // 根据增益正负选择不同的计算方式，与Python端完全一致
    if (gain_db_sat >= 0) {
        norm = 1.0f / (1.0f + sqrtf(2.0f) * K + K * K);
        float b0 = (1.0f + sqrtf(2.0f * V) * K + V * K * K) * norm;
        float b1 = 2.0f * (V * K * K - 1.0f) * norm;
        float b2 = (1.0f - sqrtf(2.0f * V) * K + V * K * K) * norm;
        float a1 = 2.0f * (K * K - 1.0f) * norm;
        float a2 = (1.0f - sqrtf(2.0f) * K + K * K) * norm;

       // DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0, b1, b2, a1, a2);
        
        // 动态计算bshift
        left_shift_t bshift = calculate_bshift(b0, b1, b2);
        
        // Store as fixed-point values
        coeffs[0] = _float2fixed_assert(b0, Q_factor - bshift);
        coeffs[1] = _float2fixed_assert(b1, Q_factor - bshift);
        coeffs[2] = _float2fixed_assert(b2, Q_factor - bshift);
        coeffs[3] = _float2fixed_assert(-a1, Q_factor);
        coeffs[4] = _float2fixed_assert(-a2, Q_factor);

        return bshift;
    } else {
        norm = 1.0f / (1.0f + sqrtf(2.0f * V) * K + V * K * K);
        float b0 = (1.0f + sqrtf(2.0f) * K + K * K) * norm;
        float b1 = 2.0f * (K * K - 1.0f) * norm;
        float b2 = (1.0f - sqrtf(2.0f) * K + K * K) * norm;
        float a1 = 2.0f * (V * K * K - 1.0f) * norm;
        float a2 = (1.0f - sqrtf(2.0f * V) * K + V * K * K) * norm;

       // DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0, b1, b2, a1, a2);
        
        // 动态计算bshift
        left_shift_t bshift = calculate_bshift(b0, b1, b2);
        
        // Store as fixed-point values
        coeffs[0] = _float2fixed_assert(b0, Q_factor - bshift);
        coeffs[1] = _float2fixed_assert(b1, Q_factor - bshift);
        coeffs[2] = _float2fixed_assert(b2, Q_factor - bshift);
        coeffs[3] = _float2fixed_assert(-a1, Q_factor);
        coeffs[4] = _float2fixed_assert(-a2, Q_factor);

        return bshift;
    }
}

left_shift_t eq_biquad_highshelf(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q, const float gain_db) {
    float gain_db_sat = _check_gain(gain_db, 20.0f * db_2);
    float fc_sat = _check_fc(fc, fs);

    // 使用与Python端BiquadFilterCalculator.calculate("highshelf")完全一致的算法
    float V = powf(10.0f, fabsf(gain_db_sat) / 20.0f);  // 与Python端一致
    float K = tanf(pi * fc_sat / fs);  // 与Python端一致：K = tan(π * Fc / Fs)
    float norm = 0.0f;

    // 根据增益正负选择不同的计算方式，与Python端完全一致
    if (gain_db_sat >= 0) {
        norm = 1.0f / (1.0f + sqrtf(2.0f) * K + K * K);
        float b0 = (V + sqrtf(2.0f * V) * K + K * K) * norm;
        float b1 = 2.0f * (K * K - V) * norm;
        float b2 = (V - sqrtf(2.0f * V) * K + K * K) * norm;
        float a1 = 2.0f * (K * K - 1.0f) * norm;
        float a2 = (1.0f - sqrtf(2.0f) * K + K * K) * norm;

       // DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0, b1, b2, a1, a2);

        // 动态计算bshift
        left_shift_t bshift = calculate_bshift(b0, b1, b2);

        // Store as fixed-point values
        coeffs[0] = _float2fixed_assert(b0, Q_factor - bshift);
        coeffs[1] = _float2fixed_assert(b1, Q_factor - bshift);
        coeffs[2] = _float2fixed_assert(b2, Q_factor - bshift);
        coeffs[3] = _float2fixed_assert(-a1, Q_factor);
        coeffs[4] = _float2fixed_assert(-a2, Q_factor);
        
        return bshift;
    } else {
        norm = 1.0f / (V + sqrtf(2.0f * V) * K + K * K);
        float b0 = (1.0f + sqrtf(2.0f) * K + K * K) * norm;
        float b1 = 2.0f * (K * K - 1.0f) * norm;
        float b2 = (1.0f - sqrtf(2.0f) * K + K * K) * norm;
        float a1 = 2.0f * (K * K - V) * norm;
        float a2 = (V - sqrtf(2.0f * V) * K + K * K) * norm;

       // DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0, b1, b2, a1, a2);

        // 动态计算bshift
        left_shift_t bshift = calculate_bshift(b0, b1, b2);

        // Store as fixed-point values
        coeffs[0] = _float2fixed_assert(b0, Q_factor - bshift);
        coeffs[1] = _float2fixed_assert(b1, Q_factor - bshift);
        coeffs[2] = _float2fixed_assert(b2, Q_factor - bshift);
        coeffs[3] = _float2fixed_assert(-a1, Q_factor);
        coeffs[4] = _float2fixed_assert(-a2, Q_factor);
        
        return bshift;
    }
}

/* Removed unused filters: bandpass, bandstop, notch, const_q, linkwitz */
