// Copyright 2025-2026 XMOS LIMITED.
// This Software is subject to the terms of the XMOS Public Licence: Version 1.
#include <string.h>
#include "downsample.h"
#include "xmath/filter.h"

#define CONVERT_FP(x)  ((int)((x) * (1<<30)))

#define DS_COEFFICIENTS  31
int32_t ds_coefficients[DS_COEFFICIENTS+1] = {
    CONVERT_FP(0.000012655763070268956),
    CONVERT_FP(0.0001144368956127061),
    CONVERT_FP(0.0005222575167540901),
    CONVERT_FP(0.0015503056579715265),
    CONVERT_FP(0.003206743194403911),
    CONVERT_FP(0.004441310178041441),
    CONVERT_FP(0.0027511574458392177),
    CONVERT_FP(-0.004624415198892449),
    CONVERT_FP(-0.0174382058234831),
    CONVERT_FP(-0.028890699902555182),
    CONVERT_FP(-0.025632812530463855),
    CONVERT_FP(0.005899831447759155),
    CONVERT_FP(0.06892545234870025),
    CONVERT_FP(0.14890438350333784),
    CONVERT_FP(0.2167916423815637),
    CONVERT_FP(0.24348744648423895),
    CONVERT_FP(0.2167916423815637),
    CONVERT_FP(0.14890438350333784),
    CONVERT_FP(0.06892545234870025),
    CONVERT_FP(0.005899831447759155),
    CONVERT_FP(-0.025632812530463855),
    CONVERT_FP(-0.028890699902555182),
    CONVERT_FP(-0.0174382058234831),
    CONVERT_FP(-0.004624415198892449),
    CONVERT_FP(0.0027511574458392177),
    CONVERT_FP(0.004441310178041441),
    CONVERT_FP(0.003206743194403911),
    CONVERT_FP(0.0015503056579715265),
    CONVERT_FP(0.0005222575167540901),
    CONVERT_FP(0.0001144368956127061),
    CONVERT_FP(0.000012655763070268956),
    0
};

// start:simple
int32_t ds_history[DS_COEFFICIENTS];

int ds_simple(int sample0, int sample1) {
    ds_history[DS_COEFFICIENTS-2] = sample0;
    ds_history[DS_COEFFICIENTS-1] = sample1;
    int64_t accumulator = 0;
    for(int i = 0; i < DS_COEFFICIENTS; i++) {
        accumulator += ((ds_coefficients[i] * (int64_t) ds_history[i]));
    }
    memmove(ds_history, ds_history + 2, (DS_COEFFICIENTS -2) * sizeof(int32_t));
    return (accumulator + (1<<29)) >> 30;
}
// end:simple

// start:lib_xcore_math
filter_fir_s32_t filter_fast;

int32_t fast_history[DS_COEFFICIENTS];

void ds_fast_init() {
    filter_fir_s32_init(&filter_fast, fast_history, DS_COEFFICIENTS, ds_coefficients, 0);
}

int ds_fast(int sample0, int sample1) {
    filter_fir_s32_add_sample(&filter_fast, sample0);
    return filter_fir_s32(&filter_fast, sample1);
}
// end:lib_xcore_math

// start:vpu
int32_t vpu_history[DS_COEFFICIENTS + 2];

extern int ds_vpu_asm(int32_t *c, int32_t *d);

int ds_vpu(int sample0, int sample1) {
    vpu_history[DS_COEFFICIENTS+0] = sample0;
    vpu_history[DS_COEFFICIENTS+1] = sample1;
    return ds_vpu_asm(ds_coefficients, vpu_history);
}
// end:vpu
