/*	
Updated/encapsulated LPC Coder/Decoder
Ryan Avery 
03/15/05

Based on the
fitlpc routine by Perry	R. Cook
Stanford CCRMA 1991.

Hacked in 1998 and 2000	for	CCRMA Summer DSP Workshop
Hacked in 1999 for new System ID of	Shaker Parameters.
(NSF CAREER	grant, Jan 1999	- Jan 2003)
Hacked in 2001 for Princeton COS/MUS 325 course

*/

#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <string.h>
#include <math.h>
#include <malloc.h>
#include <assert.h>
#include "sndfile.h"

#define	MAX_BLOCK 4096
#define	MAX_ORDER 100
#define	ONE_OVER_RANDLIMIT (1.0	/ 16384.0)
#define MAX_AMP14 16384
#define MAX_AMP15 32768
#define MAX_AMP16 65536


float myfrand()	{
	return ((float)	rand() - 16384)	* ONE_OVER_RANDLIMIT;
}

float autocorr(long size, float *data, float *result)
{
	long i,j,k;
	float temp,norm;

	for (i=0;i<size/2;i++)      {
		result[i] = 0.0;
		for (j=0;j<size-i-1;j++)	{
			result[i] += data[i+j] * data[j];
		}
	}
	temp = result[0];
	j = (long) size*0.02;
	while (result[j]<temp && j < size/2)	{
		temp = result[j];
		j += 1;
	}
	temp = 0.0;
	for (i=j;i<size*0.5;i++) {
		if (result[i]>temp) {
			j = i;
			temp = result[i];
		}
	}
	norm = 1.0 / size;
	k = size/2;
	for (i=0;i<size/2;i++)
		result[i] *=  (k - i) * norm;
	if (result[j] == 0) {
		j = 0;
	}
	else if ((result[j] / result[0]) < 0.1)
		j = 0;
	else if (j > size/2.5)
		j = 0;
	return (float) j;
}

long minvert(long size,float mat[][MAX_ORDER])
{
	long item,row,col,rank,t2;
	float temp,res[MAX_ORDER][MAX_ORDER];
	long ok,zerorow;

	for (row=1;row<=size;row++)     {
		for (col=1;col<=size;col++)	{
			//    printf(stdout," %f ",mat[row][col]);
			if (row==col) 
				res[row][col] = 1.0;
			else
				res[row][col] = 0.0;
		}
		//    fprintf(stdout,"\n");
	}
	for (item=1;item<=size;item++) {
		if (mat[item][item]==0)		{
			for (row=item;row<=size;row++)   {
				for (col=1;col<=size;col++)	{
					mat[item][col] = mat[item][col] + mat[row][col];
					res[item][col] = res[item][col] + res[row][col];
				}
			}
		} 
		for (row=item;row<=size;row++)  {
			temp=mat[row][item];
			if (temp!=0)	{
				for (col=1;col<=size;col++)	{
					mat[row][col] = mat[row][col] / temp;
					res[row][col] = res[row][col] / temp;
				}
			} 
		}
		if (item!=size)	{
			for (row=item+1;row<=size;row++)	{
				temp=mat[row][item];
				if (temp!=0)	{
					for (col=1;col<=size;col++)	{
						mat[row][col] = mat[row][col] - mat[item][col];
						res[row][col] = res[row][col] - res[item][col];
					}
				} 
			}
		} 
	}
	for (item=2;item<=size;item++)   {
		for (row=1;row<item;row++)	{
			temp = mat[row][item];
			for (col=1;col<=size;col++)	   {
				mat[row][col] = mat[row][col] - temp * mat[item][col];
				res[row][col] = res[row][col] - temp * res[item][col];
			}
		}
	}
	ok = 1;
	rank = 0; 
	for (row=1;row<=size;row++)	{
		zerorow = 1;
		for (col=1;col<=size;col++)	{
			if (mat[row][col]!=0) zerorow = 0;
			t2 = (mat[row][col] + 0.5);
			if (row==col&&t2!=1) ok = 0;
			t2 = fabs(mat[row][col]*100.0);
			if (row!=col&&t2!=0) ok = 0;
		}
		if (!zerorow) rank += 1;
	}
	if (!ok)	{
		fprintf(stdout,"Matrix Not Invertible\n");
		fprintf(stdout,"Rank is Only %i of %i\n",rank,size);
	}									
	for (row=1;row<=size;row++)	{
		for (col=1;col<=size;col++)	{
			mat[row][col] = res[row][col];
		}
	}	
	return 1;
}

void window(float* data, long size)
{	// Right now this is a Hamming window, but it will be variable in a future revision8
	int i;
	for(i = 0; i < size / 2.; i++)
	{
		data[i] *= 2.*i/size;
		data[size-i] *= 2.*i/size;
	}
}

float lpc_from_data(long order, long size, float *data, float *coeffs)
{
	float r_mat[MAX_ORDER][MAX_ORDER];
	long i,j;
	float pitch;
	float corr[MAX_BLOCK];
	float *winddata;
	winddata = (float *) malloc(size*sizeof(float));
	for(i = 0; i < size; i++)
	{
		winddata[i] = data[i];
	}
	window(winddata,size);
	pitch = autocorr(size, winddata,corr);
	for (i=1;i<order;i++) {
		for (j=1;j<order;j++)
			r_mat[i][j] = corr[abs(i-j)];
	}
	minvert(order-1,r_mat);
	for (i=0;i<order-1;i++)     {
		coeffs[i] = 0.0;
		for (j=0;j<order-1;j++)	{
			coeffs[i] += r_mat[i+1][j+1] * corr[1+j];
		}
	}
	return pitch;
}

float predict(long order,long length,float *data,float *coeffs, SNDFILE* resFile)
{
	long i,j;
	float power=0.0,error,tmp;
	static float Zs[MAX_ORDER] = {0.0};

	for (i=0;i<length;i++)     {         //  0 to hopsize??????????
		tmp = 0.0;
		for (j=0;j<order;j++)
			tmp += Zs[j]*coeffs[j];
		for (j=order-1;j>0;j--)
			Zs[j] = Zs[j-1];
		Zs[0] = data[i];
		error = data[i] - tmp;
		sf_write_float(resFile,&error,1);
		power += error * error;
	}
	return sqrt(power) / length;  
}

long check_stable(long order,float *coeffs)
{
	long i,j;
	float output,Zs[MAX_ORDER];
	for (i=0;i<order;i++) Zs[i] = 0.0;
	Zs[0] = 1.0;
	for (i=0;i<2*order*order;i++) {			// Until i = 2*order^2, test the reconstruction
		output = 0.0;
		for (j=0;j<order;j++)				// Calculate the next sample
			output += Zs[j]*coeffs[j];
		for (j=order-1;j>0;j--)				// Free up Zs[0] ..
			Zs[j] = Zs[j-1];
		Zs[0] = output;						// .. so we can put the new sample there
	}
	j = 1;
	if (output>1.0)
		j = 0;
	return j; 	
}

// Code written based on equation in
// http://www.amp.com/products/technology/5jot_5.pdf
double MeanSquareError(const double x1[], const double x2[], int N)
{
	int i;
	double diff = 0;
	double sum = 0;
	for (i = 0; i < N; ++i) {
		diff = x2[i] - x1[i];
		sum += diff*diff;
	}
	return (sum / (double) N);
}

// Taken from dot product code by P. Kabal in libtsp, modified to use double arrays
double VRfDotProd (const double x1[], const double x2[], int N)
{
	int i;
	double sum;

	sum = 0.0;
	for (i = 0; i < N; ++i)
		sum += (double) x1[i] * x2[i];

	return sum;
}

main(int argc,char *argv[])
{	// Variables for LPC conversion
	char *file_name_in,*file_name_out;
	float data[MAX_BLOCK],out_data[MAX_ORDER];
	double *all_data_res, *all_data_in, *all_data_out;
	short short_data[MAX_BLOCK], short_output;
	FILE *fd2;
	long i,n_read,time=0,block = 0;
	long block_size,hop_size,order;
	float pitch,power=0,total_power=0;
	// Variables for resynthesis
	SNDFILE	* file_in, * file_out, * file_res;
	SF_INFO	file_in_info, file_out_info, file_res_info;
	float coeffs[MAX_ORDER],Zs[MAX_ORDER],output,input;
	FILE *fileIn;
	long n_read2,n_write;
	long j,k=0,ticker;
	float last_pitch,pitchfactor=1.0;
	float amp_factor;
	double corr_ei, corr_oi, mse_ei, mse_oi, power_e, power_o, power_i;
	long length;
	int	sourceIn = 0;

		for (order = 100; order <= 100; order++) {
	time = 0; block = 0; power = 0; total_power = 0;
	k = 0; pitchfactor = 1.0; sourceIn = 0;
	if (argc>=6)  {
		//order =	atoi(argv[1]);
		if (order >= MAX_ORDER)	{
			order =	MAX_ORDER-1;
			printf("setting	order to %i\n",order);
		}
		block_size = atoi(argv[2]);
		if (block_size >= MAX_BLOCK) {
			block_size = MAX_BLOCK-1;
			printf("setting	block size to %i\n",block_size);
		}
		hop_size = atoi(argv[3]);
		if (hop_size>block_size)	{
			hop_size = block_size -	order;
			fprintf(stderr,"Setting	hop	size to	block size - order.\n");
		}
		file_name_in = argv[4];
		file_name_out =	"file.lpc";
		// fd =	fopen(file_name_in,"rb");
		file_in	= sf_open(file_name_in,	SFM_READ, &file_in_info);
		if (file_in)	 {

			fd2	= fopen(file_name_out,"wb"); //	And	write the output LPC header

			n_read = sf_read_float(file_in,data,block_size);

			file_res_info = file_in_info;
			file_res = sf_open("file-res.wav", SFM_WRITE, &file_res_info);
			while (n_read>0)	 { // For all input	file blocks...
				pitch =	lpc_from_data(order+1,block_size,data,out_data);
				if (!check_stable(order,out_data))	{
					fprintf(stderr,"This set is not stable!!\n");
				}
				power =	predict(order,n_read,&data[block_size-n_read],out_data,file_res);
				total_power	+= power;
				time+= hop_size;
				//				printf("time=%li pitch=%.1f pow=%.4f\ncoeffs = ",time,pitch,power);
				//				for	(i=0;i<order;i++)
				//					printf("%.4f, ",out_data[i]);
				//				printf("\n");

				//		getch();

				for	(i=0;i<block_size-hop_size;i++)
					data[i] = data[hop_size+i];
				n_read = sf_read_float(file_in,&data[block_size-hop_size],hop_size);

				if (n_read < hop_size) {
					for	(i=block_size-hop_size+n_read;i<block_size;i++)	{
						data[i]	= 0;
					}
				}
				fwrite(&pitch,4,1,fd2);	// Now write the pitch to the LPC header
				fwrite(&power,4,1,fd2);	// Now write the power to the LPC header
				fwrite(&out_data,4,order,fd2); // Now write	the	coeff data to the LPC file

				block += 1;
			}
			printf("\n");
			printf("%s error power = %f\n",file_name_in,total_power	/ block);
			sf_close(file_in);
			fclose(fd2);
			sf_close(file_res);
			//		getch();
		}
		else
			printf("I couldn't find	your input file!!!	%s\n",file_name_in);
	}
	else   {
		printf("Format is fitlpc order blocksize hopsize infile.wav	outfile.wav	[-p	pitchmult] [-s soundfile]\n");
	}

	/*	NOW	THE	FOLLOWING SECTION IS THE RESYNTHESIS FROM THE LPC FILE
	--------------------------------------------------------------
	*/
	pitch=100;
	file_res =	NULL;
	amp_factor = (float) hop_size / (float) block_size;

	if (argc>=6) {
		file_name_in = "file.lpc";
		file_name_out =	argv[5];
		i =	6;
		while (i < argc) {
			if (!strcmp(argv[i],"-p"))
				pitchfactor	= atof(argv[i+1]);
			if (!strcmp(argv[i],"-s"))
				sourceIn = 1;
			i += 1;
		}
		fileIn = fopen(file_name_in,"rb");
		if (sourceIn) {
			file_res =	sf_open("file-res.wav",SFM_READ,&file_res_info);
			if (!file_res)		{
				printf("Bogus Input	Excitation File\n");
				fclose(fileIn);
				exit(0);
			}
		}

		file_out_info = file_in_info;
		file_out =	 sf_open(file_name_out,	SFM_WRITE, &file_out_info) ;
		if (fileIn && file_out)	 {
			for	(i=0;i<order;i++)
				Zs[i] =	0.0;
			last_pitch = pitch;
			n_read = fread(&pitch,4,1,fileIn);
			n_read = fread(&input,4,1,fileIn);
			if (pitchfactor>0) 
				ticker = pitch / pitchfactor;
			else
				ticker = 100.0/	fabs(pitchfactor);
			n_read = fread(coeffs,4,order,fileIn);
			while (n_read>0)	 {
				for	(i=0;i<hop_size;i++) {
					output = 0.0;
					if (!sourceIn) {
						if ((pitch==0 && pitchfactor>0)	|| pitchfactor==0)
							output = input * 6.67 * myfrand();     // This block is a random signal
						else {
							ticker -= 1;
							if (ticker <= 0) {
								output = input * pitch;
								if (pitchfactor>0) 
									ticker = pitch / pitchfactor;
								else			{
									output = input * last_pitch;
									ticker = last_pitch	/ fabs(pitchfactor);
								}
							}
						}
					} else {
						n_read2	= sf_read_float(file_res,&output,1);
						if (!n_read2) {
							fprintf(stderr,"I'm	out	of input samples!!!\n");
							sourceIn = 0;
						}
					}
					for	(j=0;j<order;j++)
						output += Zs[j]*coeffs[j];
					for	(j=order-1;j>0;j--)
						Zs[j] =	Zs[j-1];
					Zs[0] =	output;
					if (output > 1.)
						output = output;
					n_write	= sf_write_float(file_out,&output,1);
				}
				k += hop_size;
				//				printf ("%i	 ... ",k);
				if (pitch>0)
					last_pitch	= pitch;
				n_read = fread(&pitch,4,1,fileIn);
				n_read = fread(&input,4,1,fileIn);
				n_read = fread(coeffs,4,order,fileIn);
			}
			printf("\nDone!!\n");
			fclose(fileIn);
			sf_close(file_out);
			if (file_res)
				sf_close(file_res);
		} else
			printf("I couldn't find	your input file!!!\n");
	}

	// Statistics of signals
	file_res = sf_open("file-res.wav",SFM_READ,&file_res_info);
	file_in	= sf_open(argv[4],	SFM_READ, &file_in_info);
	file_out = sf_open(file_name_out, SFM_READ, &file_out_info);
	all_data_res = (double *) malloc(file_in_info.frames*sizeof(double));
	all_data_in =  (double *) malloc(file_in_info.frames*sizeof(double));
	all_data_out = (double *) malloc(file_in_info.frames*sizeof(double));
	sf_read_double(file_res,all_data_res,file_res_info.frames);
	sf_read_double(file_in,all_data_in,file_in_info.frames);
	sf_read_double(file_out,all_data_out,file_out_info.frames);

	sf_close(file_res);
	sf_close(file_in);
	sf_close(file_out);

	length = (file_out_info.frames < file_in_info.frames)? file_out_info.frames : file_in_info.frames;

	corr_ei = VRfDotProd(all_data_res,all_data_in, length)/
		sqrt(VRfDotProd (all_data_res, all_data_res, length)
		*VRfDotProd (all_data_in, all_data_in, length));
	corr_oi = VRfDotProd(all_data_out,all_data_in, length)/
		sqrt(VRfDotProd (all_data_out, all_data_out, length)
		*VRfDotProd (all_data_in, all_data_in, length));
	mse_ei = MeanSquareError(all_data_res, all_data_in, length);
	mse_oi = MeanSquareError(all_data_out, all_data_in, length);
	power_e = VRfDotProd(all_data_res,all_data_res,length);
	power_i = VRfDotProd(all_data_in,all_data_in,length);
	power_o = VRfDotProd(all_data_out,all_data_out,length);

	printf("\n");
	printf("Order: %i\n", order);
	printf("Cross-correlation of residue and input: %g%%\n",100.*corr_ei);
	printf("Cross-correlation of output and input: %g%%\n",100.*corr_oi);
	printf("Mean-square error of residue: %g%%\n",mse_ei*100.);
	printf("Mean-square error of output: %g%%\n",mse_oi*100.);
	printf("Power of residue signal: %g\n",power_e);
	printf("Power of input signal:   %g\n",power_i);
	printf("Power of output signal:  %g\n",power_o);
	printf("\n");

	getch();
	}
	return 1;

}