VIANC/pw_plugin/td_xcorr.cc

#include <random>
#include <cstdio>
#include <cmath>
#include <iostream>
#include "pipewire/pipewire.h"
#include "spa/param/audio/format-utils.h"
#include "spa/param/latency-utils.h"

struct local_data {
	pw_main_loop* loop;
	pw_filter* filter;

	void* iport;
	void* oport;

	std::random_device gen;
	std::normal_distribution<float> nd{0, 0.3};

	uint32_t buffer_index = 0;
	float record_buffer[48000 * 10][2]; // Forçage (to R.), retour (mic L)
};

void on_process(local_data* ld, spa_io_position* position) {
	float* in; float* out;
	uint32_t n_samples = position->clock.duration;

	in = (float*)pw_filter_get_dsp_buffer(ld->iport, n_samples);
	out = (float*)pw_filter_get_dsp_buffer(ld->oport, n_samples);

	if(in == NULL || out == NULL) {return;}
	
	for(uint32_t i = 0; i < n_samples; ++i) {
		auto val = ld->nd(ld->gen);
		if(ld->buffer_index < 48000 * 10) {
			ld->record_buffer[ld->buffer_index][0] = val;
			ld->record_buffer[ld->buffer_index][1] = *in++;
		}

		*out++ = val;

		ld->buffer_index+=1;
	}

	if(ld->buffer_index >= 48000 * 10) {
		pw_main_loop_quit(ld->loop);
	}
}

const struct pw_filter_events filter_events {
	PW_VERSION_FILTER_EVENTS,
	.process = (void(*)(void*, spa_io_position*))on_process,
};

float compute_RMS_power(float* buffer, int stride, int n_samps) {
	double acc = 0;
	for(int i = 0; i < n_samps; ++i) {
		acc += buffer[stride*i] * buffer[stride*i];
	}
	return (float)std::sqrt(acc/n_samps);
}

// Delta positif -> retard du micro par rapport au HP -> physique.
float avg_correlation(float buffer[][2], int delta, int n_samps) {
	double acc = 0;
	for(int i = delta; i < n_samps; ++i) {
		acc += buffer[i - delta][0] * buffer[i][1];
	}
	return acc / (n_samps - delta);
}

int main(int argc, char** argv) {
	// We do NOT seed the MT engine -> the time series will be predictable!
	local_data local{nullptr,nullptr,};

	pw_init(&argc, &argv);

	local.loop = pw_main_loop_new(NULL);
	if(local.loop == NULL) {
		std::cerr << "Could not create loop!\n";
		return 1;
	}

	local.filter = pw_filter_new_simple(
		pw_main_loop_get_loop(local.loop),
		"audio-filter",
		pw_properties_new(
			PW_KEY_MEDIA_TYPE, "Audio",
			PW_KEY_MEDIA_CATEGORY, "Filter",
			PW_KEY_MEDIA_ROLE, "DSP",
			NULL),
		&filter_events,
		&local);

	local.iport = pw_filter_add_port(local.filter,
		PW_DIRECTION_INPUT,
		PW_FILTER_PORT_FLAG_MAP_BUFFERS,
		sizeof(void*),
		pw_properties_new(
			PW_KEY_FORMAT_DSP, "32 bit float mono audio",
			PW_KEY_PORT_NAME, "input",
			NULL),
		NULL, 0);

	local.oport = pw_filter_add_port(local.filter,
		PW_DIRECTION_OUTPUT,
		PW_FILTER_PORT_FLAG_MAP_BUFFERS,
		sizeof(void*),
		pw_properties_new(
			PW_KEY_FORMAT_DSP, "32 bit float mono audio",
			PW_KEY_PORT_NAME, "output",
			NULL),
		NULL, 0);

	const spa_pod* params[2];
	uint8_t buffer[1024];
	spa_pod_builder b = SPA_POD_BUILDER_INIT(buffer, sizeof(buffer));

	auto latinfo = SPA_PROCESS_LATENCY_INFO_INIT(.ns = 10 * SPA_NSEC_PER_MSEC);
	auto formatinfo = SPA_AUDIO_INFO_RAW_INIT(.format = SPA_AUDIO_FORMAT_DSP_F32,
		.rate = 48000,
		.channels = 1);

	params[0] = spa_process_latency_build(&b, SPA_PARAM_ProcessLatency, &latinfo);
	params[1] = spa_format_audio_raw_build(&b, SPA_PARAM_EnumFormat, &formatinfo);

	if(pw_filter_connect(local.filter,
		PW_FILTER_FLAG_RT_PROCESS,
		params, 1) < 0) {
		std::fprintf(stderr, "Cannot connect\n");
		return -1;
	}

	std::printf("Waiting for connection\n");
	pw_main_loop_run(local.loop);
	std::printf("Successfully recorded 10s\n");

	pw_filter_destroy(local.filter);
	pw_main_loop_destroy(local.loop);
	pw_deinit();

	float power_input = compute_RMS_power(&local.record_buffer[0][0], 2, 48000*10);
	float power_mic = compute_RMS_power(&local.record_buffer[0][1], 2, 48000*10);

	std::printf("RMS Powers : Input %f, Mic. %f\n",
		power_input,
		power_mic);

	std::printf("XCor = [\n");
	// Look at the first 200ms...
	for(int delta = 0; delta < 48000 / 5; ++delta) {
		float correct_crosscor = avg_correlation(&local.record_buffer[48000], delta, 48000*9);
		correct_crosscor /= power_input * power_input;
		// Normalizing -> if h(n) = del_0(n) then correct_crosscor = del_0.
		std::printf("%e,", correct_crosscor);
	}
	std::printf("]\n");
	return 0;
}