#include <stdio.h>
#include <complex.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <fftw3.h>
#include <unistd.h>
#include <err.h>
#include <assert.h>
#include <sndfile.h>
#include <sys/types.h>

#define MIN(a,b) ((a)<(b) ? (a) : (b))
#define MAX(a,b) ((a)>(b) ? (a) : (b))

void doFourier(double *input, int channels, sf_count_t len, fftw_complex *output)
{
	static sf_count_t len0;
	static int channels0;
	static double *windowedData;
	static fftw_plan planlos;
	static int planned = 0;
	if(!planned)
	{
		len0 = len;
		channels0 = channels;
		windowedData = fftw_malloc(sizeof(double) * channels * len);

		int l = len;
		fprintf(stderr, "Planning...\n");
		planlos = fftw_plan_many_dft_r2c(1, &l, channels,
			windowedData, NULL, channels, 1,
			output, NULL, channels, 1,
			FFTW_MEASURE | FFTW_PRESERVE_INPUT | FFTW_UNALIGNED);
		planned = 1;
	}
	assert(len0 == len);
	assert(channels0 == channels);
	
	sf_count_t nmax = channels * len;
	sf_count_t i;
	for(i = 0; i < nmax; ++i)
	{
		double ang = (i/channels) * 2 * M_PI / (len - 1);
		windowedData[i] = input[i] * (
			0.42
			- 0.5 * cos(ang)
			+ 0.08 * cos(2 * ang)
		); // Blackman
	}

	fftw_execute_dft_r2c(planlos, windowedData, output);
}

// return 1.0 for identical, 0.0 for different
#define VDIFF 1
#define LDIFF 255
double vectorSim(fftw_complex *v1, fftw_complex *v2, size_t length)
{
	size_t i;
	double sum = 0;
	double div = 0;
	for(i = 0; i < length; ++i)
	{
		fftw_complex a = v1[i];
		fftw_complex b = v2[i];

		div += cabs(a) + cabs(b);

		// primitives:
		//   cabs(a - b)             # 0 .. div
		//   fabs(cabs(a) - cabs(b)) # 0 .. div

		// component 1: vector diff
		sum += VDIFF * cabs(a - b); // can be at most 2*div

		// component 2: length diff
		sum += LDIFF * fabs(cabs(a) - cabs(b));
	}
	sum *= (1.0 / (VDIFF + LDIFF));
	if(div == 0)
		return -1;
	if(sum > div || sum < 0)
		fprintf(stderr, "%f %f\n", sum, div);
	return 1.0 - (sum / div);
}

sf_count_t findMaximumSingle(double (*func) (sf_count_t), sf_count_t x0, sf_count_t x1, sf_count_t step, double *val)
{
	sf_count_t bestpos = x1;
	double best = func(x1);

	sf_count_t i;

	for(i = x0; i < x1; i += step)
	{
		double cur = func(i);
		if(cur > best)
		{
			bestpos = i;
			best = cur;
		}
	}

	*val = best;

	return bestpos;
}

sf_count_t findMaximum(double (*func) (sf_count_t), sf_count_t x0, sf_count_t xg, sf_count_t xg2, sf_count_t x1, double *val)
{
	sf_count_t xg0, xg20;

	xg0 = xg = MAX(x0, MIN(xg, x1));
	xg20 = xg2 = MAX(x0, MIN(xg2, x1));

	fprintf(stderr, "min/max: %d %d\n", (int)xg, (int)xg2);

	for(;;)
	{
		sf_count_t size = xg2 - xg;
		if(size == 0)
			break;
		//fprintf(stderr, "round:\n");
		sf_count_t bestguess = findMaximumSingle(func, xg, xg2, size / 32 + 1, val);
		xg = MAX(xg0, bestguess - size / 3);
		xg2 = MIN(bestguess + size / 3, xg20);
	}

	fprintf(stderr, "guessed: %d\n", (int)xg);

	if(xg - xg0 < (xg20 - xg0) / 64)
		fprintf(stderr, "warning: best match very close to left margin, maybe decrease the guess?\n");

	if(xg20 - xg < (xg20 - xg0) / 64)
		fprintf(stderr, "warning: best match very close to right margin, maybe increase the guess?\n");

	return xg;
}

int main(int argc, char **argv)
{
	int chans;
	SF_INFO infile_info;
	memset(&infile_info, 0, sizeof(infile_info));
	infile_info.format = 0;
	SNDFILE *infile = sf_open(argv[1], SFM_READ, &infile_info);
	if(!infile)
		err(1, "open");
	fprintf(stderr, "Input file has %ld frames, %d Hz, %d channels, format %08x, %d sections and is %s\n",
		(long) infile_info.frames,
		infile_info.samplerate,
		infile_info.channels,
		infile_info.format,
		infile_info.sections,
		infile_info.seekable ? "seekable" : "not seekable");
	chans = infile_info.channels;

	sf_count_t size = MIN(infile_info.frames, strtod(argv[3], NULL) * infile_info.samplerate);
	sf_count_t guess = strtod(argv[4], NULL) * infile_info.samplerate;
	sf_count_t guess2 = strtod(argv[5], NULL) * infile_info.samplerate;
	int channels = infile_info.channels;
	size_t fftsize = atoi(argv[2]);
	size_t ndata = channels * (fftsize/2 + 1);

	/*
	size_t ndata_lowpass = channels * (ndata / channels / 512);  // 43 Hz
	size_t ndata_highpass = channels * (ndata / channels / 4);   // 5512 Hz
	// simplifies the dot product and makes the target function more smooth
	*/
	size_t ndata_lowpass = 0;
	//size_t ndata_highpass = channels * (ndata / channels / 4);   // 5512 Hz
	size_t ndata_highpass = ndata;

	if(size < guess + 2 * (sf_count_t) fftsize)
		err(1, "sound file too small (maybe reduce fftsize?)");

	if(guess < fftsize)
		err(1, "guess too close to file start (maybe reduce fftsize?)");

	fprintf(stderr, "Using %ld frames of %d channels, guess at %ld, %d FFT size -> %d FFT result size\n", (long) size, channels, (long) guess, (int) fftsize, (int) ndata);

	fftw_complex *data_end = fftw_malloc(sizeof(fftw_complex) * ndata);
	fftw_complex *data_cur = fftw_malloc(sizeof(fftw_complex) * ndata);

	double *sbuf = malloc(sizeof(double) * (size * channels));
	if(size != sf_readf_double(infile, sbuf, size))
		errx(1, "sf_read_double");
	sf_close(infile);

	doFourier(sbuf + (size - fftsize) * chans, chans, fftsize, data_end);
	fprintf(stderr, "end transformed.\n");

	double similarityAt(sf_count_t i)
	{
		// A trampoline! Wheeeeeeeeeew!
		doFourier(sbuf + i * channels, channels, fftsize, data_cur);
		double v = vectorSim(data_end + ndata_lowpass, data_cur + ndata_lowpass, ndata_highpass - ndata_lowpass);
		//fprintf(stderr, "Evaluated at %.9f: %f\n", i / (double) infile_info.samplerate, v);
		return v;
	}

#if 0
	{
		sf_count_t i;
		for(i = guess; i < size - 2 * fftsize; ++i)
		{
			printf("%.9f %f\n", i / (double) infile_info.samplerate, similarityAt(i));
		}
		return 0;
	}
#endif

	double val = -1;
	sf_count_t best = findMaximum(similarityAt, 0, guess - fftsize, guess2 - fftsize, size - 2 * fftsize, &val);
	fprintf(stderr, "Result: %.9f (sample %ld) with quality %.9f\n", (best + fftsize) / (double) infile_info.samplerate, (long) (best + fftsize), val);

	// Now write it!

	// 1. Crossfading end to our start sample
	fprintf(stderr, "Crossfading...\n");
	sf_count_t i;
	int j;
	double p = 2;
	for(j = 0; j < channels; ++j)
		for(i = 0; i < fftsize; ++i)
		{
			double f1 = pow(fftsize - i, p);
			double f2 = pow(i, p);
			sbuf[(size - fftsize + i) * channels + j] = 
				(
					sbuf[(size - fftsize + i) * channels + j] * f1
					+
					sbuf[(best + i) * channels + j] * f2
				) / (f1 + f2);
		}

	// 2. Open sound file
	fprintf(stderr, "Opening...\n");
	SNDFILE *outfile = sf_open(argv[6], SFM_WRITE, &infile_info);
	if(!outfile)
		err(1, "open");

	fprintf(stderr, "Writing...\n");
	// 1. Write samples from start to just before start of our end FFT
	sf_writef_double(outfile, sbuf, size);
	
	fprintf(stderr, "Closing...\n");
	sf_close(outfile);

	printf("%ld %.9f\n", (long) (best + fftsize), (best + fftsize) / (double) infile_info.samplerate);

	return 0;
}