/* beatdetect.c */

#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/audioio.h>
#include <fcntl.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdint.h>
#include <unistd.h>
#include <err.h>
#include <string.h>
#include <errno.h>

#include "fft.h"
#include "vm.h"

#define AUDIO_DEVICE "/dev/sound"

#define BUFSIZE 2048

#define CLIP 1

#define HISTSIZE 43
#define NOUTPUTS 10
#define BAR 1.5

#define MAXP 44
#define MINP 22

#define CHISTSIZE 512

#define SHISTSIZE 200

#define CLIPDELTA (20)
#define SMAXMIN (24576)
#define SMAXMAX (31129)
#define DOWNDELTA (3)
#define UPDELTA (1)
#define VOLMIN (1)
#define VOLMAX (255)

double outputslist[NOUTPUTS] = {
	1, 3, 7, 15, 31, 63, 127, 255, 511, 1023
};

#if 0
/*
double profile[NOUTPUTS] = {
	1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
};
*/
double profile[NOUTPUTS] = {
	1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0 
};

#else

double profile[NOUTPUTS] = {
	1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0
};

#endif

int audiofd;
#if 0
double lavg[NOUTPUTS];
double var[NOUTPUTS];
#endif
int lastmi = 0;
int lastmj = 0;
int lastmp = 0;
double lastmax = 0;
int minp, maxp;
double lastsum[NOUTPUTS];
double history[NOUTPUTS][HISTSIZE];
double chistory[NOUTPUTS][CHISTSIZE];
int shistory[SHISTSIZE];
int histptr;
int chistptr;
int shistptr;
char buffer[BUFSIZE];
char *bufptr;
int bufleft;
double phase;
double confidence;
int audio_initialised = 0;
int volume;

#define HISTORY(i, x) (history[i][((histptr+(x) >= HISTSIZE) ? histptr+(x)-HISTSIZE : histptr+(x))])

#define HISTWRITE(i, x) history[i][histptr] = (x)

#define HISTNEXT do {					\
		    histptr++;				\
		    if (histptr >= HISTSIZE)		\
			histptr = 0;			\
		 } while (0)

#define CHISTORY(i, x) (chistory[i][((chistptr+(x) >= CHISTSIZE) ? chistptr+(x)-CHISTSIZE : chistptr+(x))])

#define CHISTWRITE(i, x) chistory[i][chistptr] = (x)

#define CHISTNEXT do {					\
		    chistptr++;				\
		    if (chistptr >= CHISTSIZE)		\
			chistptr = 0;			\
		 } while (0)

#define SHISTORY(x) (shistory[((shistptr+(x) >= SHISTSIZE) ? shistptr+(x)-SHISTSIZE : shistptr+(x))])

#define SHISTWRITE(x) shistory[shistptr] = (x)

#define SHISTNEXT do {					\
		    shistptr++;				\
		    if (shistptr >= SHISTSIZE)		\
			shistptr = 0;			\
		 } while (0)

#define SUMSQ(a, b) ((a) * (a) + (b) * (b))
#define MAGSQ(i) SUMSQ(freqs[2*(i)], freqs[2*(i)+1])

void beatdetect_close(void)
{
	close(audiofd);
}

double beatdetect_getphase(void)
{
	return phase;
}

double beatdetect_getconfidence(void)
{
	return confidence;
}

int beatdetect_init(void)
{
	audio_info_t info, oinfo;

	audiofd = open(AUDIO_DEVICE, O_RDONLY | O_NONBLOCK, 0);
	if (audiofd < 0)
		err(1, "failed to open audio device");

	if (ioctl(audiofd, AUDIO_GETINFO, &oinfo) < 0)
		err(1, "failed to get audio info");

	AUDIO_INITINFO(&info);

	info.record.sample_rate = 44100;
	info.record.channels = 1;
	info.record.precision = 16;
	info.record.encoding = AUDIO_ENCODING_SLINEAR;
	volume = info.record.gain = oinfo.record.gain;
	info.record.port = oinfo.record.port;
	info.record.balance = oinfo.record.balance;

	info.monitor_gain = oinfo.monitor_gain;

	info.mode = AUMODE_RECORD;

	if (ioctl(audiofd, AUDIO_SETINFO, &info) < 0)
		err(1, "failed to set audio info");

	fft_init(BUFSIZE/2);

	bzero(history, sizeof(history));
	bzero(chistory, sizeof(chistory));
	histptr = 0;
	chistptr = 0;

	bufptr = buffer;
	bufleft = BUFSIZE;

	audio_initialised = 1;

	/* vm_register_blah */
	vm_register_signal_fd(audiofd, VM_BEATQ);

	return 1;
}

void beatdetect_volume(int vol)
{
	audio_info_t info;

	if (audiofd < 0)
		return;
	if (ioctl(audiofd, AUDIO_GETINFO, &info) < 0) {
		printf("ERROR: can't get audio info\n");
		return;
	}
	info.record.gain = vol;
/*	printf("Set record gain to %d\n", vol); */
	if (ioctl(audiofd, AUDIO_SETINFO, &info) < 0) {
		printf("ERROR: can't set audio info\n");
		return;
	}
}

int beatdetect_read(void)
{
#if 0
	double elem;
#endif
	int i, j, n, p;
	int rv;
	int clip;
	int smax, smaxh;
	int changevol;
	double localsum[NOUTPUTS];
	fft_type *freqs;
	int count;
	int nitems;
	double mean, variance, sd;
	int beat;
	double r[NOUTPUTS][MAXP][MAXP-MINP];
	double max;
	double cmax;
	int mi, mj, mp;
	int cmi, cmj, cmp;
	int ovolume;

	if (!audio_initialised)
		return 0;

	while (1) {
		rv = read(audiofd, bufptr, bufleft);
		if (rv == -1) {
			if (errno == EAGAIN) {
				return 0;
			}
			printf("audio read failed\n");
			audio_initialised = 0;
			return 0;
		}

		if (rv == 0)
			return 0;

		bufptr += rv;
		bufleft -= rv;

		if (bufleft != 0) {
//			printf("audio: short read (read %d)\n", rv);
			if (bufleft < 0) {
				printf("audio: oversize read\n");
				bufptr = buffer;
				bufleft = BUFSIZE;
			}
			return 0;
		}

		bufptr = buffer;
		bufleft = BUFSIZE;

		fft_data_signed16((int16_t *)buffer);

		clip = 0;
		smax = 0;

		/* Check for clip and compute rms */
		for (i = 0; i < BUFSIZE/2; i++) {
			int sample = ((int16_t *)buffer)[i];
			if (abs(sample) > smax)
				smax = abs(sample);
			if ((sample == INT16_MAX) || (sample == (-1 - INT16_MAX)))
				clip = 1;
		}

		SHISTWRITE(smax);
		SHISTNEXT;

		smaxh = 0;
		changevol = 0;

		for (i = 0; i < SHISTSIZE; i++) {
			if (SHISTORY(i) > smaxh)
				smaxh = SHISTORY(i);
		}

		/* Ideal smax range is between 75% and 95% of full signal */
		
		ovolume = volume;

		if (clip) {
			volume = volume - CLIPDELTA;
			changevol = 1;
		} else {
			if (smax > SMAXMAX) {
				volume = volume - DOWNDELTA;
				changevol = 1;
			}
			if (smaxh < SMAXMIN) {
				volume = volume + UPDELTA;
				changevol = 1;
			}
		}	

		if (changevol) {
			if (volume < VOLMIN)
				volume = 1;
			if (volume > VOLMAX)
				volume = VOLMAX;
			if (volume != ovolume)
				beatdetect_volume(volume);
		}

		fft_window();
		fft_compute();
		freqs = fft_getresult();

		n = 0;
		for (i = 0; i < NOUTPUTS; i++) {
			double output = 0;
			for (j = 0; i+j < outputslist[n]; j++)
				output += MAGSQ(i+j);
			output = output / j;
			localsum[i] = output;
			HISTWRITE(i, output);
			n++;
		}

		HISTNEXT;

#if 0
		for (i = 0; i < NOUTPUTS; i++) {
			lavg[i] = 0;
			for (j = 0; j < HISTSIZE; j++) {
				lavg[i] += HISTORY(i, j);
			}
			lavg[i] = lavg[i] / (double)HISTSIZE;
		}

		for (i = 0; i < NOUTPUTS; i++) {
			var[i] = 0;
			for (j = 0; j < HISTSIZE; j++) {
				elem = HISTORY(i, j) - lavg[i];
				var[i] += elem * elem;
			}
			var[i] = var[i] / (double)HISTSIZE;
		}
#endif

#if CLIP
		if (clip)
			printf("C");
		else
			printf(" ");
#endif

		for (i = 0; i < NOUTPUTS; i++) {
			CHISTWRITE(i, localsum[i] - lastsum[i]);
			lastsum[i] = localsum[i];
		}
		CHISTNEXT;

		max = 0;
		mi = 0;
		mp = 0;
		mj = 0;
		mean = 0;
		nitems = 0;

		for (i = 0; i < NOUTPUTS; i++) {
			for (p = MINP; p < MAXP; p++) {
				for (j = 0; j < p; j++) {
					r[i][j][p-MINP] =
					    (CHISTORY(i, CHISTSIZE-j-1) +
					    CHISTORY(i, CHISTSIZE-(p+j)-1) +
					    CHISTORY(i, CHISTSIZE-(2*p+j)-1) +
					    CHISTORY(i, CHISTSIZE-(3*p+j)-1) +
					    CHISTORY(i, CHISTSIZE-(4*p+j)-1) +
					    CHISTORY(i, CHISTSIZE-(5*p+j)-1) +
					    CHISTORY(i, CHISTSIZE-(6*p+j)-1) +
					    CHISTORY(i, CHISTSIZE-(7*p+j)-1))
					    * profile[i];
//					printf("i j p = %d %d %d\n", i, j, p);
					if (r[i][j][p-MINP] < 0)
						r[i][j][p-MINP] = 0;
					if (r[i][j][p-MINP] > max) {
						max = r[i][j][p-MINP];
						mi = i;
						mp = p;
						mj = j;
					}
					mean += r[i][j][p-MINP];
					nitems++;
				}
			}
		}
		mean /= nitems;

		cmax = 0;
		cmi = 0;
		cmp = 0;
		cmj = 0;

		minp = lastmp - 1;
		if (minp < MINP)
			minp = MINP;
		maxp = lastmp + 1;
		if (maxp >= MAXP)
			maxp = MAXP-1;

		for (i = 0; i < NOUTPUTS; i++) {
			for (p = minp; p <= maxp; p++) {
				for (j = lastmj; j <= lastmj+2; j++) {
					int nj = j;
					if (nj >= p)
						nj -= p;
					if (r[i][nj][p-MINP] > cmax) {
						cmax = r[i][nj][p-MINP];
						cmi = i;
						cmj = nj;
						cmp = p;
					}
				}
			}
		}

		count = 0;
		variance = 0;
		for (i = 0; i < NOUTPUTS; i++)
			for (p = MINP; p < MAXP; p++)
				for (j = 0; j < p; j++) {
					double val;
					if (r[i][j][p-MINP] > max / 1.2)
						count++;
					val = r[i][j][p-MINP] - mean;
					variance += val * val;
				}

		variance /= nitems;
		sd = sqrt(variance);

		if (cmax > (max / 1.2)) {
			max = cmax;
			mi = cmi;
			mj = cmj;
			mp = cmp;
		}

		phase = (((double)mj) / ((double)mp));
		confidence = sd;
		beat = 0;
		if (sd > /*200*/ 10)
			if (mj < lastmj) {
				beat = 1;
			}

		lastmax = max;
		lastmj = mj;
		lastmi = mi;
		lastmp = mp;

		if (beat)
			return 1;
	}
}