filter.h

/*
 * Copyright © 2015-2023 Synthstrom Audible Limited
 *
 * This file is part of The Synthstrom Audible Deluge Firmware.
 *
 * The Synthstrom Audible Deluge Firmware is free software: you can redistribute it and/or modify it under the
 * terms of the GNU General Public License as published by the Free Software Foundation,
 * either version 3 of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
 * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along with this program.
 * If not, see <https://www.gnu.org/licenses/>.
 */
 
#pragma once
 
#include "dsp_ng/core/types.hpp"
#include "model/mod_controllable/filters/filter_config.h"
#include "util/fixedpoint.h"
#include "util/functions.h"
#include <cstdint>
#include <ranges>
#include <span>
 
namespace deluge::dsp::filter {
constexpr int32_t ONE_Q16 = 134217728;
 
extern std::array<StereoSample<q31_t>, SSI_TX_BUFFER_NUM_SAMPLES> blendBuffer;
template <typename T>
class [[gnu::hot]] Filter {
public:
    Filter() = default;
    // returns a gain compensation value
    q31_t configure(q31_t frequency, q31_t resonance, FilterMode lpfMode, q31_t lpfMorph, q31_t filterGain) {
 
        // lpfmorph comes in q28 but we want q31
        return static_cast<T*>(this)->setConfig(frequency, resonance, lpfMode, lshiftAndSaturate<2>(lpfMorph),
                                                filterGain);
    }
    [[gnu::hot]] void filterMono(std::span<q31_t> buffer) {
        if (dryFade < 0.001) {
            static_cast<T*>(this)->doFilter(buffer);
            return;
        }
 
        memcpy(blendBuffer.data(), buffer.data(), buffer.size_bytes());
        static_cast<T*>(this)->doFilter(buffer);
 
        std::span mono_blend_buffer{reinterpret_cast<q31_t*>(blendBuffer.data()), buffer.size()};
 
        for (auto [dry, wet] : std::views::zip(mono_blend_buffer, buffer)) {
            wet = multiply_32x32_rshift32(wet, wetLevel);
            wet = multiply_accumulate_32x32_rshift32_rounded(wet, dry, ONE_Q31 - wetLevel) << 1;
            updateBlend(); // will go to one over 512 samples
        }
    }

    [[gnu::hot]] void filterStereo(StereoBuffer<q31_t> buffer) {
        if (dryFade < 0.001) {
            static_cast<T*>(this)->doFilterStereo(buffer);
            return;
        }
        memcpy(blendBuffer.data(), buffer.data(), buffer.size_bytes());
        static_cast<T*>(this)->doFilterStereo(buffer);
 
        for (auto [dry, wet] : std::views::zip(blendBuffer, buffer)) {
            wet.l = multiply_32x32_rshift32(wet.l, wetLevel);
            wet.l = multiply_accumulate_32x32_rshift32_rounded(wet.l, dry.l, ONE_Q31 - wetLevel) << 1;
            wet.r = multiply_32x32_rshift32(wet.r, wetLevel);
            wet.r = multiply_accumulate_32x32_rshift32_rounded(wet.r, dry.r, ONE_Q31 - wetLevel) << 1;
            updateBlend();
        }
    }

    void reset(bool fade = false) {
        static_cast<T*>(this)->resetFilter();
        if (fade) {
            dryFade = 1;
            wetLevel = 0;
        }
    }
 
    void updateBlend() {
        // fades over around 500 samples
        dryFade = dryFade * 0.99;
        wetLevel = (q31_t)(ONE_Q31 * (1 - dryFade));
    }

    void curveFrequency(q31_t frequency) {
        // Between 0 and 8, by my making. 1 represented by 268435456
        tannedFrequency = instantTan(lshiftAndSaturate<5>(frequency));
 
        // this is 1q31*1q16/(1q16+tan(f)/2)
        // tan(f) is q17
        divideBy1PlusTannedFrequency = (q31_t)(288230376151711744.0 / (double)(ONE_Q16 + (tannedFrequency >> 1)));
        fc = multiply_32x32_rshift32_rounded(tannedFrequency, divideBy1PlusTannedFrequency) << 4;
    }
    q31_t fc;
    float dryFade = 1;
    q31_t wetLevel = ONE_Q31;
    q31_t tannedFrequency;
    q31_t divideBy1PlusTannedFrequency;
};
 
} // namespace deluge::dsp::filter