Class to resample 1D signal in real time

Resampler.h

#ifndef RESAMPLER_H
#define RESAMPLER_H

#include <cmath>
#include <iostream>
#include <vector>


typedef float AudioSample;
typedef std::vector<AudioSample> AudioVector;
typedef AudioVector Coefficients;


std::vector<float> blackman_window(int m) {
    float a0 = 0.42659;
    float a1 = 0.49656;
    float a2 = 0.07685;    // Approximated...
    std::vector<float> values(m, 0.0);
    for (uint64_t i = 0; i < m; i++) {
        values[i] = a0 - a1 * cos(2.0 * M_PI * i / m) + a2 * cos(4.0 * M_PI * i / m);
    }
    return values;
}


std::vector<float> blackman_harrys_window(int m) {
    float a0 = 0.35875;
    float a1 = 0.48829;
    float a2 = 0.14128;
    float a3 = 0.01168;
    std::vector<float> values(m, 0.0);
    for (uint64_t i = 0; i < m; i++) {
        values[i] = a0 -
        a1 * cos(2.0 * M_PI * i / m) +
        a2 * cos(4.0 * M_PI * i / m) -
        a3 * cos(6.0 * M_PI * i / m);
    }
    return values;
}


std::vector<float> sinc(float f, int m) {
    float omega = 2.0 * M_PI * f;
    std::vector<float> values(m, 0.0);
    for (int64_t i = 1; i <= m / 2; i++) {
        float v = sin(omega * i) / (omega * i);
        values[m / 2 + i] = v;
        values[m / 2 - i] = v;
    }
    values[m / 2] = 1.0f;
    return values;
}


uint64_t get_taps_amount(float delta, float attenuation, uint64_t sample_rate) {
    float taps = (attenuation / 22.0f) * (sample_rate / delta);
    int taps_int = static_cast<int>(taps);
    if (taps - taps_int) {
        taps_int += 1;
    }
    if (!(taps_int % 2)) {
        taps_int += 1;
    }
    return taps_int;
}


Coefficients get_coefficients(float cutoff,
                              float delta,
                              float attenuation,
                              uint64_t sample_rate) {
    uint64_t taps = get_taps_amount(delta, attenuation, sample_rate);
    std::vector<float> values = sinc(cutoff / static_cast<float>(sample_rate), taps);
    std::vector<float> window = blackman_harrys_window(taps);
    assert(values.size() == window.size());
    for (uint64_t i = 0; i < values.size(); i++) {
        values[i] *= window[i];
    }
    return values;
}


class FIR {
public:
    FIR(Coefficients coefficients,  // Copy instead of referencing...
        uint64_t block_size):
    _block_size(block_size),
    _num_coefficients(coefficients.size()),
    _coefficients(coefficients),
    _history_size(_num_coefficients),
    _history(_history_size, 0.0),
    _history_windex() {
        
    }
    FIR(uint64_t block_size):
    _block_size(block_size),
    _num_coefficients(0),
    _coefficients(),
    _history_size(0),
    _history(),
    _history_windex(0) {
        
    }
    ~FIR() {
        
    }
    void set_coefficients(Coefficients coefficients) {
        _num_coefficients = coefficients.size();
        _coefficients = coefficients;
        _history_size = _num_coefficients;
        _history.resize(_history_size, 0.0);
    }
    void process(AudioVector& v_in, AudioVector& v_out) {
        assert(v_in.size() == _block_size); // TODO For debugging only...
        assert(v_out.size() == _block_size);
        
        for (uint64_t i = 0; i < _block_size; i++) {
            for (uint64_t j = 0; j < _num_coefficients; j++) {
                int64_t k = i - j;
                int64_t m = _num_coefficients - 1 - j;
                if (k < 0) {
                    int64_t n = _history_size + k;
                    v_out[i] += _history[n] * _coefficients[m];
                } else {
                    v_out[i] += v_in[k] * _coefficients[m];
                }
            }
        }
        // Update history vector...
        for (uint64_t i = 0; i < _history_size - _block_size; i++) {
            _history[i] = _history[i + _block_size];
        }
        for (uint64_t i = 0; i < _block_size; i++) {
            _history[i + _history_size - _block_size] = v_in[i];
        }
    }
    inline AudioSample process(AudioSample x) {
        _history[_history_windex] = x;
        AudioSample y = 0.0f;
        for (int i = 0; i < _history_size; i++) {
            int j = _history_windex - i;
            while (j < 0) {
                j += _history_size;
            }
            y += (_history[j] * _coefficients[i]);
        }
        _history_windex++;
        while (_history_windex >= _history_size) {
            _history_windex -= _history_size;
        }
        return y;
    }
private:
    uint64_t _block_size;
    uint64_t _num_coefficients;
    Coefficients _coefficients;
    uint64_t _history_size;
    AudioVector _history;
    uint64_t _history_windex;
};


class Buffer {
public:
    Buffer(uint64_t size = 0, std::string id = "undefined"):
    _size(size),
    _write_index(0),
    _read_index(0),
    _stack(0),
    _memory(size, 0.0),
    _id(id) {
        
    }
    ~Buffer() {
        
    }
    uint64_t size() const {
        return _size;
    }
    uint64_t stack() const {
        return _stack;
    }
    void reinit(uint64_t size) {
        _memory.resize(size, 0.0);
        _size = size;
        _write_index = 0;
        _read_index = 0;
        _stack = 0;
    }
    inline void append(AudioSample s) {
        _memory[_write_index] = s;
        _write_index++;
        _stack++;
        while (_write_index >= _size) {
            _write_index -= _size;
        }
    }
    void append(AudioVector& v) {
        for (uint64_t i = 0; i < v.size(); i++) {
            AudioSample s = v[i];
            append(s);
        }
    }
    inline int get(AudioSample& s) {
        if (!(_stack)) {
            return -1;
        }
        s = _memory[_read_index];
        _read_index++;
        _stack--;
        while (_read_index >= _size) {
            _read_index -= _size;
        }
        return 0;
    }
    int get(AudioVector& v) {
        if (_stack < v.size()) {
#ifdef DEBUG
            std::cout << "Not enough values to read" << '\n';
#endif
            return -1;
        }
        for (uint64_t i = 0; i < v.size(); i++) {
            AudioSample s = 0.0;
            int res = get(s);
            assert(!(res));
            v[i] = s;
        }
        return 0;
    }
private:
    uint64_t _size;
    uint64_t _write_index;
    uint64_t _read_index;
    uint64_t _stack;
    AudioVector _memory;
    std::string _id;
};


class PolyphaseResampler {
public:
    PolyphaseResampler(uint64_t up_ratio,
                       uint64_t down_ratio,
                       uint64_t sample_rate,
                       uint64_t block_size,
                       std::string id = "undefined"):
    _up_ratio(up_ratio),
    _down_ratio(down_ratio),
    _sample_rate(sample_rate),
    _block_size(block_size),
    _ring(0, id),
    _buffer_up(up_ratio * block_size),
    _buffer_down(down_ratio * block_size),
    _firs(),
    _id(id) {
        // Allocate ring buffer...
        int ring_size = std::max(down_ratio, up_ratio);
        ring_size *= ring_size;
        _ring.reinit(ring_size * _block_size);
        // Calculate coefficients...
        uint64_t upsample_rate = sample_rate * up_ratio;
        uint64_t downsample_rate = upsample_rate / down_ratio;
        Coefficients c = get_coefficients(downsample_rate * 0.5,
                                          downsample_rate * 0.1,
                                          60,
                                          upsample_rate);
        // Split coefficients...
        uint64_t subcoeff_size = c.size() % up_ratio ? c.size() / up_ratio + 1 : c.size() / up_ratio;
        std::vector<Coefficients> subcoeffs(up_ratio, Coefficients(subcoeff_size, 0.0));
        for (int i = 0; i < c.size(); i++) {
            int j = i % up_ratio;
            int k = i / up_ratio;
            subcoeffs[j][k] = c[i];
        }
        
        _firs.reserve(subcoeffs.size());
        for (int i = 0; i < subcoeffs.size(); i++) {
            FIR fir(subcoeffs[i], 1);
            _firs.push_back(fir);
        }
        std::cout << c.size() << " coefficients calculated." << '\n';
    }
    ~PolyphaseResampler() {
        
    }
    void process(AudioVector& v_in) {
#ifdef DEBUG
        std::cout << "process() - ID: " << _id << " - block size: " << _block_size << '\n';
#endif
        assert(v_in.size() == _block_size);
        // Upsample...
        for (uint64_t i = 0; i < _block_size; i++) {
            AudioSample s = v_in[i];
            for (uint64_t j = 0; j < _up_ratio; j++) {
                FIR& fir = _firs[j];
                // _buffer_up[i * _up_ratio + j] = fir.process(s);
                _ring.append(fir.process(s));
            }
        }
        // Append to circular buffer...
        // _ring.append(_buffer_up);
    }
    int fetch(AudioVector& v_out) {
#ifdef DEBUG
        std::cout << "fetch() - ID: " << _id << " - block size: " << _block_size << ' ';
#endif
        assert(v_out.size() == _block_size);
        // Downsample...
        // int res = _ring.get(_buffer_down);
        // if (res) {
        //     return -1;
        // }
        uint64_t samps_to_fetch = _block_size * _down_ratio;
        if (_ring.stack() < samps_to_fetch) {
#ifdef DEBUG
            std::cout << "Failed.\n";
#endif
            return -1;
        }
        for (uint64_t i = 0; i < _block_size; i++) {
            AudioSample s = 0.0;
            int res = _ring.get(s);
            assert(!(res));
            v_out[i] = s;
            for (uint64_t j = 0; j < _down_ratio - 1; j++) {    // One call to get() was already done!
                res = _ring.get(s);
            }
        }
#ifdef DEBUG
        std::cout << "Success.\n";
#endif
        return 0;
    }
private:
    uint64_t _up_ratio;
    uint64_t _down_ratio;
    uint64_t _sample_rate;
    uint64_t _block_size;
    Buffer _ring;
    AudioVector _buffer_up;
    AudioVector _buffer_down;
    std::vector<FIR> _firs;
    std::string _id;
};

#endif