VIANC/pw_plugin/main.cc

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

constexpr int samplerate = 48000;

struct local_data {
	pw_main_loop* loop;
	pw_filter* filter;

	void* mic_port; // Input data from mic
	void* listen_port; // Input music port
	void* hp_port; // Output data to headphone

	// Hopefully we never have >100ms roundtrip delay
	float record_buffer[samplerate * 10];
	float mic_buffer[samplerate * 10];

	int32_t head_index = 0; // The head is where the NEXT sample is going to be saved

	/*double xcor_buffer[samplerate / 10] = {0};
	uint32_t sample_count = 0;*/
	uint32_t call_count = 0;
};

void on_process(local_data* ld, spa_io_position* position) {
	int32_t n_samples = (int32_t)position->clock.duration;

	float* mic_in = (float*)pw_filter_get_dsp_buffer(ld->mic_port, n_samples);
	float* listen_in = (float*)pw_filter_get_dsp_buffer(ld->listen_port, n_samples);
	float* hp_out = (float*)pw_filter_get_dsp_buffer(ld->hp_port, n_samples);

	if(mic_in == NULL || listen_in == NULL || hp_out == NULL) {return;}

	// Simply echo the outgoing data
	int copy_frames = std::min(samplerate*10 - ld->head_index, n_samples);

	std::memcpy(hp_out, listen_in, n_samples * sizeof(float));

	std::memcpy(&ld->record_buffer[ld->head_index], listen_in, copy_frames*sizeof(float));
	std::memcpy(&ld->mic_buffer[ld->head_index], mic_in, copy_frames*sizeof(float));

	ld->head_index += copy_frames;
	/*
	// Now, copy data to the circular buffer
	int32_t fst_cpy = std::min(n_samples, (int)(samplerate/10) - ld->head_index);
	std::memcpy(&ld->record_buffer[ld->head_index], listen_in, fst_cpy * sizeof(float));
	ld->head_index += fst_cpy;

	int32_t snd_cpy = std::max(0, n_samples - fst_cpy);
	std::memcpy(&ld->record_buffer[0], &listen_in[fst_cpy], snd_cpy * sizeof(float));
	if(snd_cpy != 0) { ld->head_index = snd_cpy; }
	*/

	// To save on computation time, we randomly sample only one of the mic_in
	// inputs.

	/*
	float input = mic_in[n_samples - 1];
	int32_t head_pos = ld->head_index - 1;
	if(head_pos == -1) {head_pos = samplerate/10 - 1;}

	// Update the xcorr
	for(int i = 0; i < samplerate / 10; ++i) {
		ld->xcor_buffer[i] = (1 - 1.0/512)*ld->xcor_buffer[i]
			+ 1.0/512 * input * ld->record_buffer[head_pos]; 
		head_pos--;
		if(head_pos == -1) {head_pos = samplerate/10 - 1;}
	}*/

	ld->call_count++;
	if(ld->head_index == samplerate*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 n_samps) {
	double acc = 0;
	for(int i = 0; i < n_samps; ++i) {
		acc += buffer[i] * buffer[i];
	}
	return (float)std::sqrt(acc/n_samps);
}

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

int main(int argc, char** argv) {
	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.mic_port = 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, "micinput",
			NULL),
		NULL, 0);

	local.hp_port = 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);

	local.listen_port = 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, "musinput",
			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();

	std::printf("Call count : %u\n", local.call_count);

	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], &local.mic_buffer[48000], delta, 48000*9);
		// Normalizing -> if h(n) = del_0(n) then correct_crosscor = del_0.
		std::printf("%e,", correct_crosscor);
	}
	std::printf("]\n");

	return 0;
}