//#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <math.h>
#include <lv2.h>
#include "ptap.h"

#define BUFFER_SECONDS 10

struct PTap
{
	double sample_rate;
	size_t buffer_max;

	/* Ports */
	struct {
		float *in[NUM_CHANNELS];
		float *out[NUM_CHANNELS];
		float *matrix[NUM_GAINS][NUM_GAINS];
		float *gain[NUM_GAINS];
		float *delay[NUM_TAPS];
	} ports;

	float *buffers[NUM_TAPS];           ///< Tap audio buffers
	const float *buffers_end[NUM_TAPS]; ///< Audio buffer end points
	float *wp[NUM_TAPS];                ///< Write pointers
};

static LV2_Handle ptap_instantiate(
	const LV2_Descriptor *descriptor,
	double sample_rate,
	const char *bundle_path,
	const LV2_Feature *const *host_features)
{
	/* Allocate local data */
	PTap *ptap = new PTap();
	ptap->sample_rate = sample_rate;
	ptap->buffer_max = ptap->sample_rate * BUFFER_SECONDS + 1;

	for (int i = 0; i < NUM_TAPS; i++) {
		ptap->buffers[i] = new float[ptap->buffer_max]();

		if (ptap->buffers[i] == NULL) {
			for (int j = 0; j < i; j++) {
				delete ptap->buffers[i];
			}
			delete ptap;

			printf("Failed to allocate buffers\n");
			return NULL;
		}

		ptap->buffers_end[i] = ptap->buffers[i] + ptap->buffer_max;
		ptap->wp[i] = ptap->buffers[i];
	}

	return (LV2_Handle)ptap;
}

static void ptap_connect_port(LV2_Handle lv2instance, uint32_t port, void *data)
{
	PTap *ptap = (PTap *)lv2instance;
	float *fdata = (float *)data;

	/* Audio ports */
	if (port >= TAP_INS && port < TAP_OUTS) {
		port -= TAP_INS;
		ptap->ports.in[port] = fdata;
		return;
	}
	
	if (port >= TAP_OUTS && port < TAP_MATRIX) {
		port -= TAP_OUTS;
		ptap->ports.out[port] = fdata;
		return;
	}

	if (port >= TAP_MATRIX && port < TAP_GAINS) {
		port -= TAP_MATRIX;
		ptap->ports.matrix[port / NUM_GAINS][port % NUM_GAINS] = fdata;
		return;
	}

	if (port >= TAP_GAINS && port < TAP_DELAYS) {
		port -= TAP_GAINS;
		ptap->ports.gain[port] = fdata;
		return;
	}

	if (port >= TAP_DELAYS && port < TAP_END) {
		port -= TAP_DELAYS;
		ptap->ports.delay[port] = fdata;
		return;
	}

	fprintf(stderr, "Unknown port index %u\n", port);
}

static void ptap_run(LV2_Handle lv2instance, uint32_t sample_count)
{
	PTap *ptap = (PTap *)lv2instance;

	const float *readp[NUM_GAINS];
	float gain[NUM_GAINS];
	bool check_overflow = false;

	/* Position read pointers behind write pointers */
	for (int i = 0; i < NUM_TAPS; i++) {
		int delay = *ptap->ports.delay[i] * ptap->sample_rate;
		/* Zero delay results in processing order dependencies. Tapiir doesn't permit it either... */
		if (delay < 1) delay = 1;
		readp[i] = ptap->wp[i] - delay;
		if (readp[i] < ptap->buffers[i]) {
			readp[i] += ptap->buffer_max;
		}
		if (ptap->wp[i] + sample_count >= ptap->buffers_end[i]) check_overflow = true;
		if (readp[i] + sample_count >= ptap->buffers_end[i]) check_overflow = true;
		gain[i] = *ptap->ports.gain[i];
	}

	float *out[NUM_CHANNELS];
	for (int i = 0; i < NUM_CHANNELS; i++) {
		out[i]              = ptap->ports.out[i];
		readp[NUM_TAPS + i] = ptap->ports.in[i];
		gain[NUM_TAPS + i]  = *ptap->ports.gain[NUM_TAPS + i];
	}

	while (sample_count--) {
		float rp[NUM_GAINS];
		float wp[NUM_GAINS];

		/* Read input to local buffers */
		for (int i = 0; i < NUM_GAINS; i++) rp[i] = *readp[i]++;

		/* Process delays */
		for (int i = 0; i < NUM_GAINS; i++) {
			float sample = 0;
			for (int j = 0; j < NUM_GAINS; j++) sample += rp[j] * *ptap->ports.matrix[i][j];
			wp[i] = sample * gain[i];
		}

		/* Write outputs */
		for (int i = 0; i < NUM_TAPS; i++) *ptap->wp[i]++ = wp[i];
		for (int i = 0; i < NUM_CHANNELS; i++) *out[i]++ = wp[NUM_TAPS + i];

		/* Check read/write pointers */
		if (check_overflow) {
			for (int i = 0; i < NUM_TAPS; i++) {
				if (ptap->wp[i] >= ptap->buffers_end[i]) ptap->wp[i] = ptap->buffers[i];
				if (readp[i] >= ptap->buffers_end[i]) readp[i] = ptap->buffers[i];
			}
		}
	}
}

static void ptap_cleanup(LV2_Handle lv2instance)
{
	PTap *ptap = (PTap *)lv2instance;

	for (int i = 0; i < NUM_TAPS; i++) {
		delete ptap->buffers[i];
	}

	delete ptap;
}

static const LV2_Descriptor s_lv2descriptor =
{
	PTAP_URI,
	&ptap_instantiate,
	&ptap_connect_port,
	NULL,
	&ptap_run,
	NULL,
	&ptap_cleanup,
	NULL,
};

const LV2_Descriptor *lv2_descriptor(uint32_t index)
{
	if (index == 0) {
		return &s_lv2descriptor;
	}

	return NULL;
}