virtual_audio_delay.c

/*-
 * Copyright (c) 2014 Hans Petter Selasky
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <err.h>
#include <math.h>
#include <sysexits.h>

#include <sys/queue.h>

#include "virtual_int.h"

#define	REF_FREQ 500	/* HZ */

uint32_t voss_ad_last_delay;
uint8_t voss_ad_enabled;
uint8_t voss_ad_output_signal;
uint8_t voss_ad_input_channel;
uint8_t voss_ad_output_channel;

static struct voss_ad {
	double *wave;

	double *sin_a;
	double *cos_a;

	double *sin_b;
	double *cos_b;

	double *buf_a;
	double *buf_b;

	double sum_sin_a;
	double sum_cos_a;

	double sum_sin_b;
	double sum_cos_b;

	uint32_t len_a;
	uint32_t len_b;

	uint32_t offset_a;
	uint32_t offset_b;
} voss_ad;

void
voss_ad_reset(void)
{
	uint32_t x;

	for (x = 0; x != voss_ad.len_a; x++)
		voss_ad.buf_a[x] = 0;

	for (x = 0; x != voss_ad.len_b; x++)
		voss_ad.buf_b[x] = 0;

	voss_ad.sum_sin_a = 0;
	voss_ad.sum_cos_a = 0;
	voss_ad.sum_sin_b = 0;
	voss_ad.sum_cos_b = 0;

	voss_ad.offset_a = 0;
	voss_ad.offset_b = 0;

	voss_ad_last_delay = 0;
}

void
voss_ad_init(uint32_t rate)
{
	double freq;
	int samples;
	int len;
	int x;

	len = sqrt(rate);

	samples = len * len;

	voss_ad.wave = malloc(sizeof(voss_ad.wave[0]) * samples);

	voss_ad.sin_a = malloc(sizeof(voss_ad.sin_a[0]) * len);
	voss_ad.cos_a = malloc(sizeof(voss_ad.cos_a[0]) * len);
	voss_ad.buf_a = malloc(sizeof(voss_ad.buf_a[0]) * len);
	voss_ad.len_a = len;

	voss_ad.sin_b = malloc(sizeof(voss_ad.sin_b[0]) * samples);
	voss_ad.cos_b = malloc(sizeof(voss_ad.cos_b[0]) * samples);
	voss_ad.buf_b = malloc(sizeof(voss_ad.buf_b[0]) * samples);
	voss_ad.len_b = samples;

	if (voss_ad.sin_a == NULL || voss_ad.cos_a == NULL ||
	    voss_ad.sin_b == NULL || voss_ad.cos_b == NULL ||
	    voss_ad.buf_a == NULL || voss_ad.buf_b == NULL)
		errx(EX_SOFTWARE, "Out of memory");

	freq = 1.0;

	while (1) {
		double temp = freq * ((double)rate) / ((double)len);
		if (temp >= REF_FREQ)
			break;
		freq += 1.0;
	}

	for (x = 0; x != len; x++) {
		voss_ad.sin_a[x] = sin(freq * 2.0 * M_PI * ((double)x) / ((double)len));
		voss_ad.cos_a[x] = cos(freq * 2.0 * M_PI * ((double)x) / ((double)len));
		voss_ad.buf_a[x] = 0;
	}

	for (x = 0; x != samples; x++) {

		voss_ad.wave[x] = sin(freq * 2.0 * M_PI * ((double)x) / ((double)len)) *
		  (1.0 + sin(2.0 * M_PI * ((double)x) / ((double)samples))) / 2.0;

		voss_ad.sin_b[x] = sin(2.0 * M_PI * ((double)x) / ((double)samples));
		voss_ad.cos_b[x] = cos(2.0 * M_PI * ((double)x) / ((double)samples));
		voss_ad.buf_b[x] = 0;
	}
}

static double
voss_add_decode_offset(double x /* cos */, double y /* sin */)
{
	uint32_t v;
	double r;

	r = sqrt((x * x) + (y * y));

	if (r == 0.0)
		return (0);

	x /= r;
	y /= r;

	v = 0;

	if (y < 0) {
		v |= 1;
		y = -y;
	}
	if (x < 0) {
		v |= 2;
		x = -x;
	}

	if (y < x) {
		r = acos(y);
	} else {
		r = asin(x);
	}

	switch (v) {
	case 0:
		r = (2.0 * M_PI) - r;
		break;
	case 1:
		r = M_PI + r;
		break;
	case 3:
		r = M_PI - r;
		break;
	default:
		break;
	}
	return (r);
}

double
voss_ad_getput_sample(double sample)
{
	double retval;
	double phase;
	uint32_t xa;
	uint32_t xb;

	xa = voss_ad.offset_a;
	xb = voss_ad.offset_b;
	retval = voss_ad.wave[xb];

	sample -= voss_ad.buf_a[xa];
	voss_ad.sum_sin_a += voss_ad.sin_a[xa] * sample;
	voss_ad.sum_cos_a += voss_ad.cos_a[xa] * sample;
	voss_ad.buf_a[xa] += sample;

	sample = sqrt((voss_ad.sum_sin_a * voss_ad.sum_sin_a) +
	    (voss_ad.sum_cos_a * voss_ad.sum_cos_a));

	sample -= voss_ad.buf_b[xb];
	voss_ad.sum_sin_b += voss_ad.sin_b[xb] * sample;
	voss_ad.sum_cos_b += voss_ad.cos_b[xb] * sample;
	voss_ad.buf_b[xb] += sample;

	if (++xa == voss_ad.len_a)
		xa = 0;

	if (++xb == voss_ad.len_b) {
		xb = 0;

		phase = voss_add_decode_offset(
		  voss_ad.sum_cos_b, voss_ad.sum_sin_b);

		voss_ad_last_delay = (uint32_t)(phase * (double)(voss_ad.len_b) / (2.0 * M_PI)) - (voss_ad.len_a / 2);
		if (voss_ad_last_delay > voss_ad.len_b)
			voss_ad_last_delay = voss_ad.len_b;
	}
	voss_ad.offset_a = xa;
	voss_ad.offset_b = xb;

	return (retval * (1LL << voss_ad_output_signal));
}