roctave 0.0.1

data/ext/roctave/iir_filter.c

@@ -0,0 +1,462 @@
+/* Copyright (C) 2019 Théotime Bollengier <theotime.bollengier@gmail.com>
+ *
+ * This file is part of Roctave
+ * 
+ * Roctave 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.
+ * 
+ * Roctave 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 Roctave. If not, see <https://www.gnu.org/licenses/>.
+ */
+
+
+#include <ruby.h>
+#include "roctave.h"
+#include "iir_filter.h"
+
+VALUE c_IIR;
+
+static ID id_real;
+static ID id_imag;
+
+
+struct iir_filter {
+	double *b;
+	double *a;
+	double *state;
+	double *stateim;
+	int     nb_coefficients;
+	int     state_length;
+	VALUE   (*filter_one_sample_func)(struct iir_filter*, VALUE);
+};
+
+
+static void iir_filter_free(void *p)
+{
+	struct iir_filter *flt = (struct iir_filter*)p;
+
+	if (flt->b)
+		free(flt->b);
+
+	if (flt->a)
+		free(flt->a);
+
+	if (flt->state)
+		free(flt->state);
+
+	if (flt->stateim)
+		free(flt->stateim);
+
+	xfree(flt);
+}
+
+
+static size_t iir_filter_size(const void* data)
+{
+	const struct iir_filter *flt;
+	size_t res;
+
+	flt = (const struct iir_filter*)data;
+
+	res = sizeof(struct iir_filter);
+	res += 2*flt->nb_coefficients*sizeof(double);
+	res += flt->state_length*sizeof(double);
+	if (flt->stateim)
+		res += flt->state_length*sizeof(double);
+
+	return res;
+}
+
+
+static const rb_data_type_t iir_filter_type = {
+	.wrap_struct_name = "roctave_iir_filter_struct",
+	.function = {
+		.dmark = NULL,
+		.dfree = iir_filter_free,
+		.dsize = iir_filter_size,
+	},
+	.data = NULL,
+	.flags = RUBY_TYPED_FREE_IMMEDIATELY,
+};
+
+
+/* @return [Integer] the filter order
+ */
+static VALUE iir_filter_order(VALUE self)
+{
+	struct iir_filter *flt;
+
+	TypedData_Get_Struct(self, struct iir_filter, &iir_filter_type, flt);
+
+	return INT2NUM(flt->nb_coefficients - 1);
+}
+
+
+/* Set a numerator coefficient.
+ * @param index [Integer] the coefficient index
+ * @param coef  [Float] the coefficient value
+ * @return [Float, nil] the coefficient if the index is valid, or nil otherwise
+ */
+static VALUE iir_filter_set_b_coefficient_at(VALUE self, VALUE index, VALUE coef)
+{
+	struct iir_filter *flt;
+	int ind;
+	double val;
+	VALUE res = Qnil;
+
+	TypedData_Get_Struct(self, struct iir_filter, &iir_filter_type, flt);
+
+	ind = NUM2INT(index);
+	if (ind >= 0 && ind < flt->nb_coefficients) {
+		val = NUM2DBL(coef);
+		flt->b[ind] = val;
+		res = DBL2NUM(val);
+	}
+
+	return res;
+}
+
+
+/* Get a numerator coefficient.
+ * @param index [Integer] the coefficient index
+ * @return [Float] the coefficient at index
+ */
+static VALUE iir_filter_get_b_coefficient_at(VALUE self, VALUE index)
+{
+	struct iir_filter *flt;
+	int ind;
+	VALUE res = Qnil;
+
+	TypedData_Get_Struct(self, struct iir_filter, &iir_filter_type, flt);
+
+	ind = NUM2INT(index);
+	if (ind >= 0 && ind < flt->nb_coefficients)
+		res = DBL2NUM(flt->b[ind]);
+	else
+		res = DBL2NUM(0.0);
+
+	return res;
+}
+
+
+/* Set a denominator coefficient.
+ * @param index [Integer] the coefficient index, must be > 0
+ * @param coef  [Float] the coefficient value
+ * @return [Float, nil] the coefficient if the index is valid, or nil otherwise
+ */
+static VALUE iir_filter_set_a_coefficient_at(VALUE self, VALUE index, VALUE coef)
+{
+	struct iir_filter *flt;
+	int ind;
+	double val;
+	VALUE res = Qnil;
+
+	TypedData_Get_Struct(self, struct iir_filter, &iir_filter_type, flt);
+
+	ind = NUM2INT(index);
+	if (ind > 0 && ind < flt->nb_coefficients) {
+		val = NUM2DBL(coef);
+		flt->a[ind] = val;
+		res = DBL2NUM(val);
+	}
+
+	return res;
+}
+
+
+/* Get a denominator coefficient.
+ * @param index [Integer] the coefficient index
+ * @return [Float] the coefficient at index
+ */
+static VALUE iir_filter_get_a_coefficient_at(VALUE self, VALUE index)
+{
+	struct iir_filter *flt;
+	int ind;
+	VALUE res = Qnil;
+
+	TypedData_Get_Struct(self, struct iir_filter, &iir_filter_type, flt);
+
+	ind = NUM2INT(index);
+	if (ind >= 0 && ind < flt->nb_coefficients)
+		res = DBL2NUM(flt->a[ind]);
+	else
+		res = DBL2NUM(0.0);
+
+	return res;
+}
+
+
+/* Return a copy of the numerator coefficients as an array.
+ * @return [Array<Float>] the filter +b+ coefficients
+ */
+static VALUE iir_filter_b_coefficients(VALUE self)
+{
+	struct iir_filter *flt;
+	VALUE res;
+	int i;
+
+	TypedData_Get_Struct(self, struct iir_filter, &iir_filter_type, flt);
+	
+	res = rb_ary_new_capa(flt->nb_coefficients);
+	for (i = 0; i < flt->nb_coefficients; i++)
+		rb_ary_store(res, i, DBL2NUM(flt->b[i]));
+
+	return res;
+}
+
+
+/* Return a copy of the denominator coefficients as an array.
+ * @return [Array<Float>] the filter +a+ coefficients
+ */
+static VALUE iir_filter_a_coefficients(VALUE self)
+{
+	struct iir_filter *flt;
+	VALUE res;
+	int i;
+
+	TypedData_Get_Struct(self, struct iir_filter, &iir_filter_type, flt);
+	
+	res = rb_ary_new_capa(flt->nb_coefficients);
+	for (i = 0; i < flt->nb_coefficients; i++)
+		rb_ary_store(res, i, DBL2NUM(flt->a[i]));
+
+	return res;
+}
+
+
+static VALUE filter_one_sample_complex(struct iir_filter *flt, VALUE samp)
+{
+	const int flt_len = flt->nb_coefficients - 1;
+	double inr, ini, outr, outi;
+	int i;
+
+	inr = NUM2DBL(rb_funcallv(samp, id_real, 0, NULL));
+	ini = NUM2DBL(rb_funcallv(samp, id_imag, 0, NULL));
+	outr = flt->b[0] * inr + flt->state[0];
+	outi = flt->b[0] * ini + flt->stateim[0];
+	for (i = 1; i < flt_len; i++) {
+		flt->state[i-1]   = flt->b[i] * inr - flt->a[i] * outr + flt->state[i];
+		flt->stateim[i-1] = flt->b[i] * ini - flt->a[i] * outi + flt->stateim[i];
+	}
+	flt->state[i-1]   = flt->b[i] * inr - flt->a[i] * outr;
+	flt->stateim[i-1] = flt->b[i] * ini - flt->a[i] * outi;
+
+	return rb_complex_raw(DBL2NUM(outr), DBL2NUM(outi));
+}
+
+
+static void make_accept_complex(struct iir_filter *flt)
+{
+	if (!flt->stateim)
+		flt->stateim = calloc(flt->state_length, sizeof(double));
+	if (!flt->stateim)
+		rb_raise(rb_eRuntimeError, "Failed to allocate %lu bytes of state storage", flt->state_length*sizeof(double));
+	flt->filter_one_sample_func = &filter_one_sample_complex;
+}
+
+
+/* Not visible from the Ruby API.
+ * Process one Float sample, returning a Float sample.
+ * If the input sample is a Complex sample ane not a Float one,
+ * makes the filter accept Complex sample and returns filter_one_sample_complex().
+ * Once the filter has accepted one Complex, it will allways return complex samples.
+ */
+static VALUE filter_one_sample_float(struct iir_filter *flt, VALUE samp)
+{
+	const int flt_len = flt->nb_coefficients - 1;
+	double in, out;
+	int i;
+
+	if (rb_class_of(samp) == rb_cComplex) {
+		make_accept_complex(flt);
+		return filter_one_sample_complex(flt, samp);
+	}
+
+	/* Transposed direct form 2 */
+	in = NUM2DBL(samp);
+	out = flt->b[0] * in + flt->state[0];
+	for (i = 1; i < flt_len; i++)
+		flt->state[i-1] = flt->b[i] * in - flt->a[i] * out + flt->state[i];
+	flt->state[i-1] = flt->b[i] * in - flt->a[i] * out;
+
+	return DBL2NUM(out);
+}
+
+
+/* @param sample [Float, Array<Float>] a single sample or an array of samples to process
+ * @return [Float, Array<Float>] a single or an array of processed samples
+ */
+static VALUE iir_filter_filter(VALUE self, VALUE sample)
+{
+	struct iir_filter *flt;
+	VALUE res = Qnil;
+	int i, len;
+
+	TypedData_Get_Struct(self, struct iir_filter, &iir_filter_type, flt);
+
+	if (RB_FLOAT_TYPE_P(sample) || rb_obj_is_kind_of(sample, rb_cNumeric))
+		res = (*flt->filter_one_sample_func)(flt, sample);
+	else if (rb_class_of(sample) == rb_cArray) {
+		len = rb_array_len(sample);
+		res = rb_ary_new_capa(len);
+		for (i = 0; i < len; i++)
+			rb_ary_store(res, i, (*flt->filter_one_sample_func)(flt, rb_ary_entry(sample, i)));
+	}
+	else
+		rb_raise(rb_eArgError, "Expecting a single Numeric or an Array of Numeric");
+
+	return res;
+}
+
+
+static VALUE iir_filter_alloc(VALUE klass)
+{
+	VALUE obj;
+	struct iir_filter *flt;
+
+	flt = ZALLOC(struct iir_filter);
+
+	obj = TypedData_Wrap_Struct(klass, &iir_filter_type, flt);
+
+	flt->b = NULL;
+	flt->a = NULL;
+	flt->state = NULL;
+	flt->stateim = NULL;
+	flt->nb_coefficients = 0;
+	flt->state_length = 0;
+	flt->filter_one_sample_func = &filter_one_sample_float;
+
+	return obj;
+}
+
+
+/* Reset the filter state.
+ * @return self
+ */
+static VALUE iir_filter_reset(VALUE self)
+{
+	struct iir_filter *flt;
+	int i;
+
+	TypedData_Get_Struct(self, struct iir_filter, &iir_filter_type, flt);
+
+	for (i = 0; i < flt->state_length; i++)
+		flt->state[i] = 0.0f;
+
+	if (flt->stateim) {
+		for (i = 0; i < flt->state_length; i++)
+			flt->stateim[i] = 0.0f;
+	}
+
+	flt->filter_one_sample_func = &filter_one_sample_float;
+
+	return self;
+}
+
+
+/* @overload initialize(n)
+ *  @param n [Integer] the filter order
+ * @overload initialize(b, a)
+ *  @param b [Array<Float>] the array of numerator coefficients
+ *  @param a [Array<Float>] the array of denominator coefficients
+ */
+static VALUE iir_filter_initialize(int argc, VALUE *argv, VALUE self)
+{
+	struct iir_filter *flt;
+	VALUE b_or_n;
+	VALUE a;
+	double dv;
+	int i, lenb, lena = 0, len;
+
+	rb_scan_args(argc, argv, "11", &b_or_n, &a);
+
+	TypedData_Get_Struct(self, struct iir_filter, &iir_filter_type, flt);
+
+	if (rb_class_of(b_or_n) == rb_cInteger) {
+		if (argc > 1)
+			rb_raise(rb_eArgError, "If the order is given as the first argument, no other argument is expected");
+		flt->nb_coefficients = NUM2INT(b_or_n) + 1;
+		if (flt->nb_coefficients < 1)
+			rb_raise(rb_eArgError, "Order cannot be less than 0");
+		if (flt->nb_coefficients > 1048577)
+			rb_raise(rb_eArgError, "Order cannot be more than 1048576");
+		flt->b = calloc(flt->nb_coefficients, sizeof(double));
+		flt->a = calloc(flt->nb_coefficients, sizeof(double));
+		if (flt->b == NULL || flt->a == NULL)
+			rb_raise(rb_eRuntimeError, "Failed to allocate memory for coefficient storage");
+		flt->a[0] = 1.0;
+	}
+	else if (rb_class_of(b_or_n) == rb_cArray) {
+		lenb = rb_array_len(b_or_n);
+		len = lenb;
+		if (argc > 1) {
+			if (rb_class_of(a) == rb_cArray)
+				lena = rb_array_len(a);
+			else
+				rb_raise(rb_eArgError, "Expecting denominator coefficients (Array<Float>) as second argument");
+			if (len < lena)
+				len = lena;
+		}
+		if (len > 1048577)
+			rb_raise(rb_eArgError, "Cannot be more than 1048577 coefficients");
+		if (len < 1)
+			rb_raise(rb_eArgError, "Coefficient arrays are empty");
+		flt->nb_coefficients = len;
+		flt->b = calloc(flt->nb_coefficients, sizeof(double));
+		flt->a = calloc(flt->nb_coefficients, sizeof(double));
+		if (flt->b == NULL || flt->a == NULL)
+			rb_raise(rb_eRuntimeError, "Failed to allocate memory for coefficient storage");
+		flt->a[0] = 1.0;
+		if (lena > 1)
+			dv = NUM2DBL(rb_ary_entry(a, i));
+		else
+			dv = 1.0;
+		if (dv == 0.0)
+			rb_raise(rb_eArgError, "The first denominator coefficient may no be null");
+		for (i = 0; i < lenb; i++)
+			flt->b[i] = (double)(NUM2DBL(rb_ary_entry(b_or_n, i))) / dv;
+		for (i = 1; i < lena; i++)
+			flt->a[i] = (double)(NUM2DBL(rb_ary_entry(a, i))) / dv;
+	}
+	else
+		rb_raise(rb_eArgError, "Expecting the filter order (Integer) or the filter coefficients (two Array<Float>)");
+
+	flt->state_length = flt->nb_coefficients - 1;
+	flt->state = calloc(flt->state_length, sizeof(double));
+	if (!flt->state)
+		rb_raise(rb_eRuntimeError, "Failed to allocate %lu bytes of state storage", flt->state_length*sizeof(double));
+	flt->stateim = NULL;
+	flt->filter_one_sample_func = &filter_one_sample_float;
+
+	return self;
+}
+
+
+void Init_iir_filter()
+{
+	id_real = rb_intern("real");
+	id_imag = rb_intern("imag");
+
+	c_IIR = rb_define_class_under(m_Roctave, "IirFilter", rb_cData);
+
+	rb_define_alloc_func(c_IIR, iir_filter_alloc);
+	rb_define_method(c_IIR, "initialize",             iir_filter_initialize,          -1);
+	rb_define_method(c_IIR, "order",                  iir_filter_order,                0);
+	rb_define_method(c_IIR, "reset!",                 iir_filter_reset,                0);
+	rb_define_method(c_IIR, "get_num_coefficient_at", iir_filter_get_b_coefficient_at, 1);
+	rb_define_method(c_IIR, "set_num_coefficient_at", iir_filter_set_b_coefficient_at, 2);
+	rb_define_method(c_IIR, "get_den_coefficient_at", iir_filter_get_a_coefficient_at, 1);
+	rb_define_method(c_IIR, "set_den_coefficient_at", iir_filter_set_a_coefficient_at, 2);
+	rb_define_method(c_IIR, "numerator",              iir_filter_b_coefficients,       0);
+	rb_define_method(c_IIR, "denominator",            iir_filter_a_coefficients,       0);
+	rb_define_method(c_IIR, "filter",                 iir_filter_filter,               1);
+}
+
+