/* Orton's hum nulling software.
   Based on the LMS algorithm.
*/

#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <string.h>
#include <math.h>





/* The DSP routines that appear at the end of this file */

int initdsp(		/* Initialisation: */
	double sr,	/* The sample frequency being used */
	double mf	/* The mains frequency */
);			/* Return value is zero if supplied
			   frequencies are okay, one if not */

void dodsp(		/* Do actual DSP: */
	short *in,	/* Pointer to 16 bit input sample
			   buffer */
	short *out,	/* Pointer to 16 bit output sample
			   buffer */
	int n		/* Number of samples to process */
);			/* No return value */
	
void enddsp(		/* End DSP: */
);			/* No parameters, no return value */






/* This part is entirely concerned with parsing a .WAV file
   and creating a new one with identical format.

   The input file MUST be a 16 bit mono PCM file.
*/

#define BUFSZ	1024		/* Buffer size for file I/O */
#define	DCHNK	0x61746164	/* Data chunk ID */
#define	DFLTSR	16.0e3		/* Default sample rate */
#define	DFLTMF	60.0		/* Default mains frequency */

short insmpls[BUFSZ];	/* The input sample buffer */
short outsmpls[BUFSZ];	/* The output sample buffer */

void main()
{
	FILE *infp, *outfp;
	int nread, nwrite, fail, secs, oldsecs;
	long lsrate, lbuf[3], chnksz, progress;
	char infn[80], outfn[80], cbuf[100], c;
	double srate, mainsf;

	/* Print banner */
	printf("Off-line hum filtering utility\n\n");
	printf("At any time, press '.' to exit \n\n");
	
	/* Open input file */
	do
	{
		fail=0;
		printf("Enter input file name   [input.wav]: ");
		if(!gets(infn))
		{
			fail=1;
			continue;
		}
		if(infn[0]=='.')
		{
			printf("\nProgram aborted\n");
			exit(0);
		}
		if(!infn[0])
			strcpy(infn, "input.wav");
		else if(!strchr(infn, '.'))
			strcpy(infn+strlen(infn), ".wav");
		infp=fopen(infn, "rb");
		if(!infp)
		{
			printf("Cannot open %s\n", infn);
			fail=1;
		}
	}
	while(fail);

	/* Create output file */
	printf("\n");
	do
	{
		fail=0;
		printf("Enter output file name [output.wav]: ");
		if(!gets(outfn))
		{
			fail=1;
			continue;
		}
		if(outfn[0]=='.')
		{
			printf("\nProgram aborted\n");
			fclose(infp);
			exit(0);
		}
		if(!outfn[0])
			strcpy(outfn, "output.wav");
		else if(!strchr(outfn, '.'))
			strcpy(outfn+strlen(outfn), ".wav");
		outfp=fopen(outfn, "rb");
		if(outfp)
		{
			fclose(outfp);
			printf("%s exists - overwrite? [no]: ", outfn);
			if(!gets(cbuf))
			{
				fail=1;
				continue;
			}
			if(cbuf[0]=='.')
			{
				printf("\nProgram aborted\n");
				fclose(infp);
				exit(0);
			}
			if(!sscanf(cbuf, "%c", &c))
			{
				fail=1;
				continue;
			}
			if((c!='y')&&(c!='Y'))
			{
				fail=1;
				continue;
			}
		}
		outfp=fopen(outfn, "wb");
		if(!outfp)
		{
			printf("Cannot create %s\n", outfn);
			fail=1;
		}
	}
	while(fail);

	/* Get sample rate */
	printf("\n");
	do
	{
		fail=0;
		printf("Enter sampling frequency    [%g]: ", DFLTSR);
		if(!gets(cbuf))
		{
			fail=1;
			continue;
		}
		if(cbuf[0]=='.')
		{
			printf("\nProgram aborted\n");
			fclose(infp);
			fclose(outfp);
			remove(outfn);
			exit(0);
		}
		if(!cbuf[0])
			srate=DFLTSR;
		else if(!sscanf(cbuf, "%lf", &srate))
		{
			fail=1;
			continue;
		}
		if((srate<8000.0)||(srate>48000.0))
		{
			printf("Must be in range 8000..48000\n");
			fail=1;
		}
	}
	while(fail);
	lsrate=(long)srate;

	/* Get mains frequency */
	printf("\n");
	do
	{
		fail=0;
		printf("Enter mains frequency         [%g]: ", DFLTMF);
		if(!gets(cbuf))
		{
			fail=1;
			continue;
		}
		if(cbuf[0]=='.')
		{
			printf("\nProgram aborted\n");
			fclose(infp);
			fclose(outfp);
			remove(outfn);
			exit(0);
		}
		if(!cbuf[0])
			mainsf=DFLTMF;
		else if(!sscanf(cbuf, "%lf", &mainsf))
		{
			fail=1;
			continue;
		}
		if((mainsf<50.0)||(mainsf>60.0))
		{
			printf("Must be in range 50..60\n");
			fail=1;
			continue;
		}
	}
	while(fail);

	/* Read the WAV header and pass to output */
	printf("\n");
	nread=fread(lbuf, sizeof(long), 3, infp);
	nwrite=fwrite(lbuf, sizeof(long), 3, outfp);
	if((nread!=3)||(nwrite!=3))
	{
		printf("Header read failed\n");
		fclose(infp);
		fclose(outfp);
		remove(outfn);
		exit(0);
	}

	/* Skip chunks until 'data' chunk found */
	fail=0;
	do
	{
		nread=fread(lbuf, sizeof(long), 2, infp);
		nwrite=fwrite(lbuf, sizeof(long), 2, outfp);
		if((nread!=2)||(nwrite!=2))
		{
			fail=1;
			break;
		}
		chnksz=lbuf[1];
		if(lbuf[0]!=DCHNK)
		{
			/* Non-data chunk - pass to output file */
			nread=fread(cbuf, 1, chnksz, infp);
			nwrite=fwrite(cbuf, 1, chnksz, outfp);
			if((nread!=chnksz)||(nwrite!=chnksz))
			{
				fail=1;
				break;
			}
		}
	}
	while((!fail)&&(lbuf[0]!=DCHNK));
	if(fail)
	{
		printf("Header read failed\n");
		fclose(infp);
		fclose(outfp);
		remove(outfn);
		exit(0);
	}

	/* Inform DSP routines of sample rate, and mains freq */
	if(initdsp(srate, mainsf))
	{
		printf("DSP initialisation error\n");
		fclose(infp);
		fclose(outfp);
		remove(outfn);
		exit(0);
	}

	/* Main loop - keep reading buffer-fulls and process */
	printf("Processing:\n\n");
	progress=0L;
	secs=0;
	oldsecs=0;
	while(nread=fread((void *)insmpls, sizeof(short), BUFSZ, infp))
	{
		/* Process samples */
		dodsp(insmpls, outsmpls, nread);

		/* Write output samples to output file */
		nwrite=fwrite((void *)outsmpls, sizeof(short), nread, outfp);
		if(nwrite!=nread)
		{
			printf("\nData write failed\n");
			fclose(infp);
			fclose(outfp);
			remove(outfn);
			exit(0);
		}

		/* Report progress */
		progress+=(long)nread;
		secs=(int)(progress/lsrate);
		if(secs!=oldsecs)
		{
			printf("%d seconds completed\n", secs);
			oldsecs=secs;
		}

		/* Check if user wants to stop */
		if(kbhit()&&('.'==getch()))
		{
			printf("\nUser interrupted processing\n");
			fclose(infp);
			fclose(outfp);
			remove(outfn);
			exit(0);
		}
	}
	enddsp();
	fclose(infp);
	fclose(outfp);
	printf("\nProgram completed successfully\n");
}








/* This is the DSP stuff. Essentially, we adapt a FIR filter, whose
   inputs are an epoch of the signal, so that the filter output is
   as close as possible to the same signal exactly one mains cycle
   later.

   In theory, if the mains cycle is an integer number of samples,
   then the filter should adapt to having 1.0 in one tap, and 0.0 in
   all others.

   If the mains cycle is not an integer number of samples (the more
   likely scenario) then the filter will adapt to perform the
   necessary sub-sample interoplation, perhaps using as many as eight
   samples either side of the pulse centre.

   The filter adapts on a continuous basis, and so will track changes
   in mains frequency.

   The first few seconds of the filtered audio may have significant
   hum as the filter adapts to the prevailing mains frequency.
*/

#define	FTOL	5.0		/* Mains frequency tolerance +/-
				   percent. Maximum value is 10.0 */
#define	MAXFSZ	1100		/* The maximum tap delay - good for
				   sample rates up to 48kHz */
#define	MAXCO	200		/* Maximum coefficients. We do not
				   need to implement the filter over
				   the entire mains cycle. Only
				   taps in the region 1 cycle +/-FTOL%
				   need be implemented. */
#define	GAIN	1.0e-10		/* Adaption gain. Change at your
				   peril. */
typedef double FPTYPE;		/* Floating point type used for DSP */

int firsz;			/* Actual filter size */
int nco;			/* Number of coefficients */ 
int prechg;			/* Counter for pre-charging filter */
FPTYPE *firp;			/* Input pointer to circular FIR data
				   buffer */
FPTYPE *firend;			/* Points to end of filter - used for
				   circularising pointers */
FPTYPE firco[MAXCO];		/* The coefficient vector */
FPTYPE firbuf[MAXFSZ];		/* The circular data vector */



/* Initialise DSP */
int initdsp(double sr, double mf)
{
	int i, j;
	FPTYPE *p1;

	/* Check for sensible parameters */
	if((sr<8.0e3)||(sr>48.0e3))
		return(1);
	if((mf<50.0)||(mf>60.0))
		return(1);

	/* Calculate required filter size and coefficients */
	i=(int)(sr/mf+0.5);
	j=(int)((FTOL*0.01*sr/mf)+1.0);
	firsz=i+j+1;
	nco=2*j+1;

	/* Initialise coefficients assuming mains is
	   spot on frequency - our best guess */
	p1=firco;
	for(i=nco; i>0; i--)
		*p1++=0.0;
	firco[j]=1.0;

	/* Don't attempt to filter until we have filled it */
	prechg=firsz-1;

	/* Initialise input pointer, and end limit pointer */
	firp=firbuf;
	firend=firbuf+firsz;
	return(0);
}

void dodsp(short *in, short *out, int n)
{
	short *p1, *p2;
	FPTYPE *p3, *p4, *p5, *p6, x;
	int i, j, k;

	/* Process the samples provided */
	p1=in;
	p2=out;
	p3=firp;
	p4=firend;
	i=n;
	j=prechg;
	while(i--)
	{
		if(j)
		{
			/* Precharging - don't filter */
			*p3++=(FPTYPE)(*p1++);
			*p2++=0;
			j--;
		}
		else
		{
			/* Filtering - move in new sample */
			x=(FPTYPE)(*p1++);
			*p3++=x;

			/* Circularise in pointer */
			if(p3>=p4)
				p3=firbuf;

			/* Perform convolution */
			p5=p3;
			p6=firco;
			for(k=nco; k>0; k--)
			{
				x-=((*p5++)*(*p6++));
				if(p5>=p4)
					p5=firbuf;
			}

			/* Deliver output sample */
			*p2++=(short)x;

			/* Adapt coefficients */
			x*=GAIN;
			p5=p3;
			p6=firco;
			for(k=nco; k>0; k--)
			{
				(*p6++)+=x*(*p5++);
				if(p5>=p4)
					p5=firbuf;
			}
		}
	}
	firp=p3;
	prechg=j;
}



/* Tidy up. Nothing to do actually! */
void enddsp()
{
}