#ifndef FIR_FILTER_H
#define FIR_FILTER_H

#include <stdint.h>
#include <string.h>

/* This needs to be a power of two for the bitmask optimizations to work */
/* If a non power of two length is desired, change to modulo */
#define FIR_LEN (1u << 5) // = 32
_Static_assert((FIR_LEN & (FIR_LEN - 1u)) == 0, "FIR_LEN must be a power of two");

typedef struct {
    uint_fast8_t idx;
    float        buf[FIR_LEN];
    const float *coeffs;
} FIRFilter;

void  FIRFilter_init(FIRFilter *fir, const float *coef_array);
float FIRFilter_update(FIRFilter *fir, float inp);

#endif // FIR_FILTER_H

// #include "fir_filter.h"

void FIRFilter_init(FIRFilter *fir, const float *coeffs) {
    memset(fir, 0, sizeof(FIRFilter));
    fir->coeffs = coeffs;
}

float FIRFilter_update(FIRFilter *fir, float inp) {
    float out = 0.f;
    fir->buf[fir->idx] = inp;
    fir->idx = (fir->idx + 1) & (FIR_LEN - 1);

    // Compute the latest output sample by applying a convolution
    uint8_t sum_idx = fir->idx;

#ifndef CONFIG_FIR_UNROLL

    for (size_t i = 0; i < FIR_LEN; i++) {
        sum_idx = (sum_idx - 1) & (FIR_LEN - 1);
        out += fir->coeffs[i] * fir->buf[sum_idx];
    }

#else // CONFIG_FIR_UNROLL

    int          count = FIR_LEN / 8;
    int          leftover = FIR_LEN % 8;
    const float *c = fir->coeffs;

    switch (leftover) {
    case 0:
        do {
            sum_idx = (sum_idx - 1) & (FIR_LEN - 1);
            out += *c++ * fir->buf[sum_idx];
        case 7: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
        case 6: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
        case 5: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
        case 4: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
        case 3: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
        case 2: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
        case 1: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
        } while (--count > 0);
    }

#endif // CONFIG_FIR_UNROLL

    return out;
}

int main(void) {
}