#include "core/instrument/engine/filter/filter_coeffs.h"

#include <cmath>

namespace reasampler::instrument::engine::filter {
namespace {

// M_PI is not standard C++ and is absent on MSVC without _USE_MATH_DEFINES.
constexpr double kPi = 3.14159265358979323846;

double clampd(double v, double lo, double hi) { return v < lo ? lo : (v > hi ? hi : v); }

}  // namespace

SvfCoeffs svfCoeffs(float cutoffHz, float q, double sampleRate) {
    const double qq = clampd(q, kFilterQMin, kFilterQMax);
    const double k = 1.0 / qq;

    double g = 0.0;
    if (sampleRate > 0.0) {
        const double fc = clampd(cutoffHz, kFilterCutoffMinHz, kFilterNyquistFraction * sampleRate);
        g = std::tan(kPi * fc / sampleRate);
    }

    // Solved in double and narrowed once. The intermediate g*(g+k) is the term that carries the
    // pole proximity, so forming it in float would throw away the conditioning TPT just bought.
    const double a1 = 1.0 / (1.0 + g * (g + k));
    const double a2 = g * a1;
    const double a3 = g * a2;

    SvfCoeffs c;
    c.g = static_cast<float>(g);
    c.k = static_cast<float>(k);
    c.a1 = static_cast<float>(a1);
    c.a2 = static_cast<float>(a2);
    c.a3 = static_cast<float>(a3);
    return c;
}

}  // namespace reasampler::instrument::engine::filter
