
#include "config.h"

#include "splitter.h"

#include <algorithm>
#include <cmath>
#include <limits>
#include <numbers>

#include "alnumeric.h"
#include "gsl/gsl"


void BandSplitter::init(float const f0norm) noexcept NONBLOCKING
{
    auto const w = std::numbers::pi_v<float>*2.0f * std::min(f0norm, 0.49f);
    if(auto const cw = std::cos(w); cw > std::numeric_limits<float>::epsilon())
        mCoeff = (std::sin(w) - 1.0f) / cw;
    else
        mCoeff = cw * -0.5f;

    mLpZ1 = 0.0f;
    mLpZ2 = 0.0f;
    mApZ1 = 0.0f;
}

void BandSplitter::process(std::span<float const> const input, std::span<float> const hpout,
    std::span<float> const lpout) noexcept NONBLOCKING
{
    auto const ap_coeff = mCoeff;
    auto const lp_coeff = mCoeff*0.5f + 0.5f;
    auto lp_z1 = mLpZ1;
    auto lp_z2 = mLpZ2;
    auto ap_z1 = mApZ1;

    Expects(lpout.size() >= input.size());
    auto lpiter = lpout.begin();
    std::ranges::transform(input, hpout.begin(),
        [ap_coeff,lp_coeff,&lp_z1,&lp_z2,&ap_z1,&lpiter](float const in) noexcept -> float
    {
        /* Low-pass sample processing. */
        auto const d0 = (in - lp_z1) * lp_coeff;
        auto const lp_y0 = lp_z1 + d0;
        lp_z1 = lp_y0 + d0;

        auto const d1 = (lp_y0 - lp_z2) * lp_coeff;
        auto const lp_y1 = lp_z2 + d1;
        lp_z2 = lp_y1 + d1;

        *(lpiter++) = lp_y1;

        /* All-pass sample processing. */
        auto const ap_y = in*ap_coeff + ap_z1;
        ap_z1 = in - ap_y*ap_coeff;

        /* High-pass generated from removing low-passed output. */
        return ap_y - lp_y1;
    });
    mLpZ1 = lp_z1;
    mLpZ2 = lp_z2;
    mApZ1 = ap_z1;
}

void BandSplitter::processHfScale(std::span<float const> const input, std::span<float> const output,
    float const hfscale) noexcept NONBLOCKING
{
    auto const ap_coeff = mCoeff;
    auto const lp_coeff = mCoeff*0.5f + 0.5f;
    auto lp_z1 = mLpZ1;
    auto lp_z2 = mLpZ2;
    auto ap_z1 = mApZ1;
    std::ranges::transform(input, output.begin(),
        [hfscale,ap_coeff,lp_coeff,&lp_z1,&lp_z2,&ap_z1](float const in) noexcept -> float
    {
        /* Low-pass sample processing. */
        auto const d0 = (in - lp_z1) * lp_coeff;
        auto const lp_y0 = lp_z1 + d0;
        lp_z1 = lp_y0 + d0*lp_coeff;

        auto const d1 = (lp_y0 - lp_z2) * lp_coeff;
        auto const lp_y1 = lp_z2 + d1;
        lp_z2 = lp_y1 + d1;

        /* All-pass sample processing. */
        auto const ap_y = in*ap_coeff + ap_z1;
        ap_z1 = in - ap_y*ap_coeff;

        /* High-pass generated by removing the low-passed signal, which is then
         * scaled and added back to the low-passed signal.
         */
        return (ap_y-lp_y1)*hfscale + lp_y1;
    });
    mLpZ1 = lp_z1;
    mLpZ2 = lp_z2;
    mApZ1 = ap_z1;
}

void BandSplitter::processHfScale(std::span<float> const samples, float const hfscale) noexcept
    NONBLOCKING
{
    auto const ap_coeff = mCoeff;
    auto const lp_coeff = mCoeff*0.5f + 0.5f;
    auto lp_z1 = mLpZ1;
    auto lp_z2 = mLpZ2;
    auto ap_z1 = mApZ1;
    std::ranges::transform(samples, samples.begin(),
        [hfscale,ap_coeff,lp_coeff,&lp_z1,&lp_z2,&ap_z1](float const in) noexcept -> float
    {
        /* Low-pass sample processing. */
        auto const d0 = (in - lp_z1) * lp_coeff;
        auto const lp_y0 = lp_z1 + d0;
        lp_z1 = lp_y0 + d0;

        auto const d1 = (lp_y0 - lp_z2) * lp_coeff;
        auto const lp_y1 = lp_z2 + d1;
        lp_z2 = lp_y1 + d1;

        /* All-pass sample processing. */
        auto const ap_y = in*ap_coeff + ap_z1;
        ap_z1 = in - ap_y*ap_coeff;

        /* High-pass generated by removing the low-passed signal, which is then
         * scaled and added back to the low-passed signal.
         */
        return (ap_y-lp_y1)*hfscale + lp_y1;
    });
    mLpZ1 = lp_z1;
    mLpZ2 = lp_z2;
    mApZ1 = ap_z1;
}

void BandSplitter::processScale(std::span<float> const samples, float const hfscale,
    float const lfscale) noexcept NONBLOCKING
{
    auto const ap_coeff = mCoeff;
    auto const lp_coeff = mCoeff*0.5f + 0.5f;
    auto lp_z1 = mLpZ1;
    auto lp_z2 = mLpZ2;
    auto ap_z1 = mApZ1;
    std::ranges::transform(samples, samples.begin(),
        [hfscale,lfscale,ap_coeff,lp_coeff,&lp_z1,&lp_z2,&ap_z1](float const in) noexcept -> float
    {
        auto const d0 = (in - lp_z1) * lp_coeff;
        auto const lp_y0 = lp_z1 + d0;
        lp_z1 = lp_y0 + d0;

        auto const d1 = (lp_y0 - lp_z2) * lp_coeff;
        auto const lp_y1 = lp_z2 + d1;
        lp_z2 = lp_y1 + d1;

        auto const ap_y = in*ap_coeff + ap_z1;
        ap_z1 = in - ap_y*ap_coeff;

        /* Apply separate factors to the high and low frequencies. */
        return (ap_y-lp_y1)*hfscale + lp_y1*lfscale;
    });
    mLpZ1 = lp_z1;
    mLpZ2 = lp_z2;
    mApZ1 = ap_z1;
}

void BandSplitter::processAllPass(std::span<float> const samples) noexcept NONBLOCKING
{
    auto const coeff = mCoeff;
    auto z1 = mApZ1;
    std::ranges::transform(samples, samples.begin(), [coeff,&z1](float const x) noexcept -> float
    {
        auto const y = x*coeff + z1;
        z1 = x - y*coeff;
        return y;
    });
    mApZ1 = z1;
}
