#define _CRT_SECURE_NO_WARNINGS
#include <stdio.h>
#include <stdlib.h>
#include <memory.h>
#include <math.h>

typedef short sample_t;
typedef __int64 err_t;

#define DESIRED_RMS	10000	//can be tweaked slightly for better utilization of dynamic range depending on material
#define BPS	1
#define DELTA 10			//try table values +- this every pass
							//higher values = more exhaustive search
//#define PRESHAPE
#define POSTSHAPE
//#define VERBOSE

static void normalize(sample_t *data, int n) {
	float rms = 0;
	int x;

	for (x = 0; x < n; x++)
		rms += data[x]*data[x];

	rms = sqrtf(rms / n);
	printf("RMS amplitude prior to normalization: %f\n", rms);

	for (x = 0; x < n; x++) {
		int value = data[x] * DESIRED_RMS / rms;
		if (value < -32768) value = -32768;
		if (value > 32767)  value = 32767;
		data[x] = value;
	}
}

static void quantize(const sample_t *input, sample_t *output, int n) {
	int x, last_error = 0;
	for (x = 0; x < n; x++) {
#ifdef PRESHAPE
		//shape using feedback. not sure how correct this is,
		//but quiet parts appear to receive less noise
		output[x] = ((input[x] + 3 * last_error / 4) / 2048) + 15;
#else
		output[x] = (input[x] / 2048) + 15;
#endif

		if (output[x] < 0)
			output[x] = 0;
		if (output[x] >= 31)
			output[x] = 30;

		last_error = (output[x] - 15) * 2048 - input[x];
	}
}

//table is k*31 entries, where k=2^N
static err_t table_adpcm_work(sample_t *data, int n, int *table, int k, int do_output) {
	int x, last = 0, last_error = 0;
	err_t ret = 0;

	for (x = 0; x < n; x++) {
		int y;
		int best, best_error, best_value;

		for (y = 0; y < k; y++) {
			int value = table[y + last*k];
			int error;

#ifdef POSTSHAPE
			//again, may not be entirely correct,
			//but the output sounds roughly right
			error = value - data[x] - last_error / 2;
#else
			error = value - data[x];
#endif
			error *= error;

			if (y == 0 || error < best_error) {
				best = y;
				best_error = error;
				best_value = value;
			}
		}

		last_error = best_value - data[x];
		last = best_value;
		ret += best_error;

		if (do_output)
			data[x] = best_value;
	}

	return ret;
}

static void table_adpcm(sample_t *data, int n, int bps) {
	int k = 1 << bps;
	int x, y;
	int entries = 31 * k;
	int *table = malloc(entries*sizeof(int));
	int *best_table = malloc(entries*sizeof(int));
	err_t e, ebest;
	int pass, changes;
	char table_name[32];
	FILE *f;

	//speed up processing by only looking at the first tenth of the file
	//this seems to work well enough
	int n2 = n / 10;

	//initialize table with reasonable values
	for (y = 0; y < 31; y++)
		for (x = 0; x < k; x++) {
			if (bps == 1)
				table[x + y*k] = y + 7*(x - k/2) + 4;
			else
				table[x + y*k] = y + 3*(x - k/2) + 3;

			if (table[x + y*k] < 0)  table[x + y*k] = 0;
			if (table[x + y*k] > 30) table[x + y*k] = 30;
		}

	ebest = table_adpcm_work(data, n2, table, k, 0);
	memcpy(best_table, table, entries*sizeof(int));

	printf("initial error: %li\n", ebest);
	printf("initial rms: %.2f\n", sqrtf((float)ebest/n2));

	changes = 1;
	for (pass = 1; changes; pass++) {
		changes = 0;

		for (y = 0; y < entries; y++) {
			int delta = DELTA;
			int min = best_table[y] - delta, max = best_table[y] + delta;
			memcpy(table, best_table, entries*sizeof(int));

#ifdef VERBOSE
			printf("table[% 4i/% 4i] = % 3i\n", y, entries, best_table[y]);
#endif

			if (min < 0)
				min = 0;
			if (max > 30)	//31 isn't a legal value
				max = 30;

			for (x = min; x < max; x++) {
				table[y] = x;
				e = table_adpcm_work(data, n2, table, k, 0);

				if (e < ebest) {
					float rms = sqrtf((float)ebest/n2);
					memcpy(best_table, table, entries*sizeof(int));
					ebest = e;
					changes++;

#ifdef VERBOSE
					printf("                -> % 3i -> %li ", x, ebest);
					//for some reason printf()ing rms above doesn't work..
					printf("(%.2f)\n", rms);
#endif
				}
			}
		}

		printf("pass %i: %i changes, rms = %.2f\n", pass, changes, sqrtf((float)ebest/n2));
	}

	e = table_adpcm_work(data, n, best_table, k, 1);
	printf("final rms: %.2f\n", sqrtf((float)e/n));

	sprintf(table_name, "table_%ibps.asm", bps);
	f = fopen(table_name, "w");

	fprintf(f, ";%i entries\n", entries);
	fprintf(f, ";%i bits per sample\n", bps);
	fprintf(f, ";rms = %.2f\n", sqrtf((float)e/n));
	fprintf(f, "ADPCMTable\n");
	for (x = 0; x < entries; x++)
		fprintf(f, "\t.byte %i\n", best_table[x]);

	free(table);
	free(best_table);
	fclose(f);
}

static void dequantize(sample_t *data, int n) {
	int x;
	for (x = 0; x < n; x++) {
		//driver will output 15 if given 31
		if (data[x] == 31)
			data[x] = 15;

		data[x] = (data[x] - 15) * 2048;
	}
}

static void process(const char *filename, const char *outname) {
	FILE *f = fopen(filename, "rb"), *of;
	unsigned char header[44];
	int size, rate, samples;
	sample_t *input, *output;
	int bps;

	if (!f)
		return;

	of = fopen(outname, "wb");

	fread(header, 44, 1, f);
	rate = *(int*)&header[24];
	size = *(int*)&header[40];
	samples = size / 2;

	printf("rate: %i\n", rate);
	printf("size: %i = %i samples = %f seconds\n", size, samples, (float)samples/rate);

	input  = malloc(samples*sizeof(sample_t));
	output = malloc(samples*sizeof(sample_t));

	fread(input, size, 1, f);

	bps = BPS;

	normalize(input, samples);
	quantize(input, output, samples);
	table_adpcm(output, samples, bps);
	dequantize(output, samples);

	fwrite(header, 44, 1, of);
	fwrite(output, size, 1, of);

	printf("encoded size: %.2f KiB (%i bps)\n", (samples * bps) / 8192.f, bps);

	free(input);
	free(output);
	fclose(f);
	fclose(of);
}

int main() {
	process("bloom.wav", "output.wav");
	return 0;
}