/* Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include <stdlib.h>
#include "eq.h"

struct eq {
	int n;
	struct biquad biquad[MAX_BIQUADS_PER_EQ];
};

struct eq *eq_new()
{
	struct eq *eq = (struct eq *)calloc(1, sizeof(*eq));
	return eq;
}

void eq_free(struct eq *eq)
{
	free(eq);
}

int eq_append_biquad(struct eq *eq, enum biquad_type type, float freq, float Q,
		      float gain)
{
	if (eq->n >= MAX_BIQUADS_PER_EQ)
		return -1;
	biquad_set(&eq->biquad[eq->n++], type, freq, Q, gain);
	return 0;
}

int eq_append_biquad_direct(struct eq *eq, const struct biquad *biquad)
{
	if (eq->n >= MAX_BIQUADS_PER_EQ)
		return -1;
	eq->biquad[eq->n++] = *biquad;
	return 0;
}

/* This is the prototype of the processing loop. */
void eq_process1(struct eq *eq, float *data, int count)
{
	int i, j;
	for (i = 0; i < eq->n; i++) {
		struct biquad *q = &eq->biquad[i];
		float x1 = q->x1;
		float x2 = q->x2;
		float y1 = q->y1;
		float y2 = q->y2;
		float b0 = q->b0;
		float b1 = q->b1;
		float b2 = q->b2;
		float a1 = q->a1;
		float a2 = q->a2;
		for (j = 0; j < count; j++) {
			float x = data[j];
			float y = b0*x
				+ b1*x1 + b2*x2
				- a1*y1 - a2*y2;
			data[j] = y;
			x2 = x1;
			x1 = x;
			y2 = y1;
			y1 = y;
		}
		q->x1 = x1;
		q->x2 = x2;
		q->y1 = y1;
		q->y2 = y2;
	}
}

/* This is the actual processing loop used. It is the unrolled version of the
 * above prototype. */
void eq_process(struct eq *eq, float *data, int count)
{
	int i, j;
	for (i = 0; i < eq->n; i += 2) {
		if (i + 1 == eq->n) {
			struct biquad *q = &eq->biquad[i];
			float x1 = q->x1;
			float x2 = q->x2;
			float y1 = q->y1;
			float y2 = q->y2;
			float b0 = q->b0;
			float b1 = q->b1;
			float b2 = q->b2;
			float a1 = q->a1;
			float a2 = q->a2;
			for (j = 0; j < count; j++) {
				float x = data[j];
				float y = b0*x
					+ b1*x1 + b2*x2
					- a1*y1 - a2*y2;
				data[j] = y;
				x2 = x1;
				x1 = x;
				y2 = y1;
				y1 = y;
			}
			q->x1 = x1;
			q->x2 = x2;
			q->y1 = y1;
			q->y2 = y2;
		} else {
			struct biquad *q = &eq->biquad[i];
			struct biquad *r = &eq->biquad[i+1];
			float x1 = q->x1;
			float x2 = q->x2;
			float y1 = q->y1;
			float y2 = q->y2;
			float qb0 = q->b0;
			float qb1 = q->b1;
			float qb2 = q->b2;
			float qa1 = q->a1;
			float qa2 = q->a2;

			float z1 = r->y1;
			float z2 = r->y2;
			float rb0 = r->b0;
			float rb1 = r->b1;
			float rb2 = r->b2;
			float ra1 = r->a1;
			float ra2 = r->a2;

			for (j = 0; j < count; j++) {
				float x = data[j];
				float y = qb0*x
					+ qb1*x1 + qb2*x2
					- qa1*y1 - qa2*y2;
				float z = rb0*y
					+ rb1*y1 + rb2*y2
					- ra1*z1 - ra2*z2;
				data[j] = z;
				x2 = x1;
				x1 = x;
				y2 = y1;
				y1 = y;
				z2 = z1;
				z1 = z;
			}
			q->x1 = x1;
			q->x2 = x2;
			q->y1 = y1;
			q->y2 = y2;
			r->y1 = z1;
			r->y2 = z2;
		}
	}
}