k4/src/k3mix.c

#include"k3mix.h"

#include<vorbis/vorbisfile.h>
#include<string.h>
#include<stdlib.h>
#include<math.h>
#include<pthread.h>
#include<assert.h>

static uint32_t FinalSampleRate;
static uint8_t FinalChannels;

void k3MixInit(uint32_t sampleRate, uint8_t channels) {
	FinalSampleRate = sampleRate;
	FinalChannels = channels;
}

struct k3MixSource {
	char *filepath;
	OggVorbis_File vf;
	int bitstream;
};
struct k3MixSource *k3MixSourceFile(const char *path) {
	struct k3MixSource *ret = calloc(1, sizeof(*ret));
	ret->filepath = strdup(path);
	ov_fopen(path, &ret->vf);
	ret->bitstream = 0;
	return ret;
}
void k3MixSourceClose(struct k3MixSource *src) {
	ov_clear(&src->vf);
	free(src->filepath);
	free(src);
}

__attribute__((optimize("Ofast"))) static intmax_t ogg_read(struct k3MixWave *this, size_t sampleCount, float *data) {
	struct k3MixSource *od = this->data;
	
	float **ni;
	long totalRead = 0;
	do {
		long lastRead = ov_read_float(&od->vf, &ni, sampleCount, &od->bitstream);
		
		if(this->loop && lastRead == 0) {
			ov_pcm_seek(&od->vf, 0);
			continue;
		} else if(lastRead <= 0) {
			this->end = true;
			return totalRead;
		}
		
		for(long s = 0; s < lastRead; s++) {
			for(long c = 0; c < this->channels; c++) {
				*(data++) += ni[c][s] * this->volume;
			}
		}
		
		totalRead += lastRead;
		sampleCount -= lastRead;
	} while(sampleCount);
	
	return totalRead;
}
static void ogg_close(struct k3MixWave *this) {
	if(this->refs-- == 1) {
		k3MixSourceClose(this->data);
	}
}
static void ogg_clone(struct k3MixWave *this, struct k3MixWave *new) {
	new->refs = 1;
	new->sampleRate = this->sampleRate;
	new->channels = this->channels;
	new->data = k3MixSourceFile(((struct k3MixSource*) this->data)->filepath);
	new->read = ogg_read;
	new->clone = ogg_clone;
	new->close = ogg_close;
	
	new->loop = this->loop;
	new->dam = this->dam;
	new->volume = this->volume;
}
struct k3MixWave *k3MixSimple(struct k3MixSource *src) {
	vorbis_info *vi = ov_info(&src->vf, -1);
	
	struct k3MixWave *ret = calloc(1, sizeof(*ret));
	ret->refs = 1;
	ret->sampleRate = vi->rate;
	ret->channels = vi->channels;
	ret->data = k3MixSourceFile(src->filepath);
	
	ret->read = ogg_read;
	ret->clone = ogg_clone;
	ret->close = ogg_close;
	
	ret->loop = 0;
	ret->dam = 0;
	ret->volume = 1;
	
	return ret;
}
void k3MixSimpleSeek(struct k3MixWave *this, double t) {
	struct k3MixSource *od = this->data;
	ov_time_seek(&od->vf, t);
}

struct qdata {
	size_t ref;
	size_t count;
	struct k3MixWave *children;
	
	pthread_mutex_t damootecks;
};
static intmax_t queue_read(struct k3MixWave *this, size_t sampleCount, float *data) {
	struct qdata *qdat = this->data;
	
	pthread_mutex_lock(&qdat->damootecks);
	
		size_t totalRead = 0;
		
		while(sampleCount && qdat->count > 0) {
			intmax_t i = qdat->children[0].read(&qdat->children[0], sampleCount, data);
			
			if(i == 0) {
				qdat->children[0].close(&qdat->children[0]);
				memmove(&qdat->children[0], &qdat->children[1], sizeof(*qdat->children) * (qdat->count - 1));
				qdat->count--;
			} else {
				data += this->channels * i;
				totalRead += i;
				sampleCount -= i;
			}
		}
	
	pthread_mutex_unlock(&qdat->damootecks);
	
	return totalRead;
}
static void queue_close(struct k3MixWave *this) {
	if(this->refs-- == 1) {
		struct qdata *qdat = this->data;
		
		if(--qdat->ref == 0) {
			for(size_t i = 0; i < qdat->count; i++) {
				qdat->children[i].close(&qdat->children[i]);
			}
			
			free(qdat);
		}
	}
}
static void queue_clone(struct k3MixWave *this, struct k3MixWave *new) {
	struct qdata *data = this->data;
	
	data->ref++;
	
	new->refs = 1;
	new->sampleRate = this->sampleRate;
	new->channels = this->channels;
	new->data = data;
	new->read = queue_read;
	new->clone = queue_clone;
	new->close = queue_close;
	
	new->loop = this->loop;
	new->dam = this->dam;
	new->volume = this->volume;
}
struct k3MixWave *k3MixQueue() {
	struct k3MixWave *ret = malloc(sizeof(*ret));
	ret->refs = 1;
	ret->sampleRate = FinalSampleRate;
	ret->channels = FinalChannels;
	
	struct qdata *data = ret->data = calloc(1, sizeof(struct qdata));
	data->ref = 1;
	pthread_mutex_init(&data->damootecks, NULL);
	
	ret->read = queue_read;
	ret->clone = queue_clone;
	ret->close = queue_close;
	
	ret->loop = 0;
	ret->dam = 0;
	ret->volume = 1;
	
	return ret;
}
void k3MixQueueAdd(struct k3MixWave *this, struct k3MixWave *child) {
	struct qdata *qdat = this->data;
	
	pthread_mutex_lock(&qdat->damootecks);
		qdat->children = realloc(qdat->children, sizeof(*qdat->children) * (qdat->count + 1));
		child->clone(child, &qdat->children[qdat->count]);
		qdat->count++;
	pthread_mutex_unlock(&qdat->damootecks);
}
void k3MixQueueClear(struct k3MixWave *this) {
	struct qdata *qdat = this->data;
	
	pthread_mutex_lock(&qdat->damootecks);
		for(size_t i = 0; i < qdat->count; i++) {
			qdat->children[i].close(&qdat->children[i]);
		}
		qdat->count = 0;
		free(qdat->children);
		qdat->children = NULL;
	pthread_mutex_unlock(&qdat->damootecks);
}

struct powermeasurementdata {
	struct k3MixWave *child;
	
	size_t historySize;
	float *history;
};

static intmax_t power_measurement_read(struct k3MixWave *this, size_t sampleCount, float *data) {
	struct powermeasurementdata *dat = this->data;
	
	sampleCount = dat->child->read(dat->child, sampleCount, data);
	
	if(sampleCount * FinalChannels < dat->historySize) {
		memmove(dat->history, dat->history + sampleCount * FinalChannels, (dat->historySize - sampleCount * FinalChannels) * sizeof(*dat->history));
		memcpy(dat->history + (dat->historySize - sampleCount * FinalChannels), data, sampleCount * FinalChannels * sizeof(*dat->history));
	} else {
		memcpy(dat->history, data + sampleCount * FinalChannels - dat->historySize, dat->historySize * sizeof(*dat->history));
	}
	
	this->end = dat->child->end;
	
	return sampleCount;
}

static void power_measurement_close(struct k3MixWave *this) {
	if(this->refs-- == 1) {
		struct powermeasurementdata *dat = this->data;
		
		dat->child->close(dat->child);
		
		free(dat);
	}
}

static void power_measurement_clone(struct k3MixWave *this, struct k3MixWave *new) {
	struct powermeasurementdata *src = this->data;
	
	struct powermeasurementdata *dat = malloc(sizeof(*dat));
	dat->child = calloc(1, sizeof(*dat->child));
	src->child->clone(src->child, dat->child);
	dat->history = calloc(dat->historySize = src->historySize, sizeof(*dat->history));
	memcpy(dat->history, src->history, sizeof(*dat->history) * dat->historySize);
	
	new->refs = 1;
	new->sampleRate = FinalSampleRate;
	new->channels = FinalChannels;
	
	new->data = dat;
	
	new->read = power_measurement_read;
	new->clone = power_measurement_clone;
	new->close = power_measurement_close;
	
	new->loop = this->loop;
	new->dam = this->dam;
	new->volume = this->volume;
}

float k3MixPowerMeasurementGetRMS(struct k3MixWave *this) {
	struct powermeasurementdata *dat = this->data;
	
	float sum = 0;
	
	for(int i = 0; i < dat->historySize; i++) {
		sum += pow(dat->history[i], 2);
	}
	
	return sqrtf(sum / dat->historySize);
}

struct k3MixWave *k3MixPowerMeasurement(struct k3MixWave *child) {
	child->refs++;
	
	struct powermeasurementdata *dat = malloc(sizeof(*dat));
	dat->child = child;
	
	dat->historySize = 8820;
	dat->history = calloc(dat->historySize, sizeof(*dat->history));
	
	struct k3MixWave *ret = malloc(sizeof(*ret));
	
	ret->refs = 1;
	ret->sampleRate = FinalSampleRate;
	ret->channels = FinalChannels;
	
	ret->data = dat;
	
	ret->read = power_measurement_read;
	ret->clone = power_measurement_clone;
	ret->close = power_measurement_close;
	
	ret->loop = 0;
	ret->dam = 0;
	ret->volume = 1;
	
	return ret;
}

static size_t playingCount, playingCapacity;
static struct k3MixWave **playings;

void k3MixStop(struct k3MixWave *wav) {
	for(size_t i = 0; i < playingCount; i++) {
		if(playings[i] == wav) {
			playings[i]->close(playings[i]);
			
			memmove(&playings[i], &playings[i + 1], sizeof(*playings) * (playingCount - i - 1));
			playingCount--;
		}
	}
}

struct k3MixWave *k3MixPlay(struct k3MixWave *wav) {
	for(size_t i = 0; i < playingCount; i++) {
		if(playings[i] == wav) {
			return NULL;
		}
	}
	
	k3MixPlayDirect(wav);
	
	return wav;
}

void k3MixPlayDirect(struct k3MixWave *w) {
	w->refs++;
	
	if(playingCount == playingCapacity) {
		playings = realloc(playings, sizeof(*playings) * (playingCapacity += 8));
	}
	playings[playingCount++] = w;
}

#include<xmmintrin.h>
__attribute__((optimize("Ofast"))) static void k3MixDoYourThang(size_t sampleCount, float *FinalData) {
	memset(FinalData, 0, sampleCount * FinalChannels * sizeof(*FinalData));
	for(size_t i = 0; i < playingCount;) {
		intmax_t read = playings[i]->read(playings[i], sampleCount, FinalData);
		
		if(playings[i]->end) {
			k3MixStop(playings[i]);
		} else i++;
	}
	
	for(size_t i = 0; i < sampleCount * FinalChannels; i += 4) {
		// Compute tanh approximation x*(27+x*x)/(27+9*x*x)
		
		float *ptr = FinalData + i;
		
		__m128 x = _mm_loadu_ps(ptr);
		__m128 xx = _mm_mul_ps(x, x);
		
		__m128 numer = _mm_mul_ps(x, _mm_add_ps(_mm_set1_ps(27), xx));
		__m128 denom = _mm_add_ps(_mm_set1_ps(27), _mm_mul_ps(_mm_set1_ps(9), xx));
		
		_mm_storeu_ps(ptr, _mm_div_ps(numer, denom));
	}
	
	// The accuracy isn't worth the function call per sample
	/*for(size_t i = 0; i < sampleCount * FinalChannels; i++) {
		*FinalData = tanhf(*FinalData);
		FinalData++;
	}*/
}

#include<portaudio.h>
#include<xmmintrin.h>
static PaStream *paStream;
static int pa_callback(const void *input, void *output, unsigned long frameCount, const PaStreamCallbackTimeInfo *timeInfo, PaStreamCallbackFlags statusFlags, void *ud) {
	float *out = output;
	
	static int done = 0;
	if(done == 0) {
		done = 1;
		_mm_setcsr(_mm_getcsr() | 0x8040);
	}
	
	k3MixDoYourThang(frameCount, out);
	
	return paContinue;
}
void k3MixPortAudio() {
	Pa_Initialize();
	Pa_OpenDefaultStream(&paStream, 0, FinalChannels, paFloat32, FinalSampleRate, 512, pa_callback, NULL);
	Pa_StartStream(paStream);
}