/****************************************************************************

    DRC: Digital Room Correction
    Copyright (C) 2002-2004 Denis Sbragion

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

		You can contact the author on Internet at the following address:

				d.sbragion@infotecna.it

		This program uses the parsecfg library from Yuuki  NINOMIYA.  De
		tails  on  this  library  can be found in the parsecfg.c and par
		secfg.h files.  Many thanks to Yuuki NINOMIYA for this useful li
		brary.

		This program uses  also the FFT  routines from  Takuya Ooura and
		the GNU Scientific  Library (GSL).  Many thanks  to Takuya Ooura
		and the GSL developers for these efficient routines.

****************************************************************************/

/* Log sweep and inverse filter convolution program */

/* Includes */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "fftsg_h.h"

/* Output stringhe con sync output e parametro */
int sputsp(const char * s, const char * p)
	{
		int Res;
		if (p == NULL)
			Res = puts(s);
		else
			Res = printf("%s%s\n",s,p);
		fflush(stdout);
		return(Res);
	}

/* Output stringhe con sync output */
int sputs(const char * s)
	{
		return(sputsp(s, NULL));
	}

/* Determina la lunghezza di un file */
size_t FSize(char * FName)
	{
		long FS;
		FILE * F;

		if ((F = fopen(FName,"rb")) == NULL)
			{
				perror("Unable to open file");
				return 0;
			}

		fseek(F,0,SEEK_END);
		FS = ftell(F);
		fclose(F);
		return (size_t) FS;
	}

/* Calola il valore RMS del segnale Sig */
DLReal GetRMSLevel(const DLReal * Sig,const int SigLen)
	{
		DLReal RMS;
		int I;

		RMS = 0;
		for (I = 0; I < SigLen; I++)
			RMS += Sig[I] * Sig[I];

		return (DLReal) sqrt(RMS);
	}

/* Halfcomplex array convolution */
void hcconvolve(DLReal * A,const DLReal * B,const int N)
	{
		int R;
		int I;
		DLReal T1;
		DLReal T2;

		A[0] *= B[0];
		A[1] *= B[1];
		for (R = 2,I = 3;R < N;R += 2,I += 2)
			{
				T1 = A[R] * B[R];
				T2 = A[I] * B[I];
				A[I] = ((A[R] + A[I]) * (B[R] + B[I])) - (T1 + T2);
				A[R] = T1 - T2;
			}
	}

/* Main procedure */
int main(int argc, char * argv[])
	{
		/* Input parameters */
		char * SweepFile;
		char * InverseFile;
		char * OutFile;
		char * RefSweepFile;
		DLReal MinGain;
		DLReal DLStart;

		/* Convolution parameters */
		int SS;
		int IS;
		int RS;
		int CS;
		int CL;
		int I;
		int J;
		DLReal * Sweep;
		DLReal * Inverse;
		float RF;

		/* Dip limiting */
		DLReal RMSLevel;
		DLReal RMSMin;
		DLReal DLSLevel;
		DLReal DLLevel;
		DLReal DLGFactor;
		DLReal DLAbs;
		DLReal DLTheta;

		/* Peak position */
		int MP;
		DLReal AMax;

		/* Input/output file */
		FILE * IOF;

		/* Initial message */
		sputs("\nLSConv 1.1.0: log sweep and inverse filter convolution.");
		sputs("Copyright (C) 2002-2011 Denis Sbragion");
		#ifdef UseDouble
			sputs("\nCompiled with double precision arithmetic.");
		#else
			sputs("\nCompiled with single precision arithmetic.");
		#endif
		sputs("\nThis program may be freely redistributed under the terms of");
		sputs("the GNU GPL and is provided to you as is, without any warranty");
		sputs("of any kind. Please read the file \"COPYING\" for details.");

		/* Check program arguments */
		if (argc < 4 || (argc > 4 && argc < 6))
			{
				sputs("\nUsage: LSConv sweepfile inversefile outfile [refsweep mingain [dlstart]]");
				sputs("\nParameters:\n");
				sputs("  sweepfile: sweep file name");
				sputs("  inversefile: inverse sweep file name");
				sputs("  outfile: output impulse response file");
				sputs("  refsweep: reference channel sweep file name");
				sputs("  mingain: min gain for reference channel inversion");
				sputs("  dlstart: dip limiting start for reference channel inversion");
				sputs("\nExample: lsconv sweep.pcm inverse.pcm impulse.pcm refchannel.pcm 0.1 0.8\n");
				return 0;
			}

		/* Get the input parameters from the command line */
		sputs("\nCommand line parsing.");
		SweepFile = argv[1];
		InverseFile = argv[2];
		OutFile = argv[3];
		if (argc > 4)
			{
				RefSweepFile = argv[4];
				MinGain = (DLReal) atof(argv[5]);
			}
		else
			RefSweepFile = NULL;
		if (argc >= 7)
			DLStart = (DLReal) atof(argv[6]);
		else
			DLStart = (DLReal) 2.0;

		/* Computes the convolution length */
		sputs("Convolution length computation.");
		SS = FSize(SweepFile) / sizeof(float);
		IS = FSize(InverseFile) / sizeof(float);
		if (RefSweepFile != NULL)
			RS = FSize(RefSweepFile) / sizeof(float);
		else
			RS = 1;
		CL = SS + IS + RS - 2;
		for(CS = 1;CS < CL;CS <<= 1);

		/* Convolution arrays allocation */
		sputs("Convolution arrays allocation.");
		if ((Sweep = (DLReal *) malloc(sizeof(DLReal) * CS)) == NULL)
			{
				sputs("Memory allocation failure.");
				return 1;
			}
		if ((Inverse = (DLReal *) malloc(sizeof(DLReal) * CS)) == NULL)
			{
				sputs("Memory allocation failure.");
				free(Sweep);
				return 1;
			}

		/* Read the inverse file */
		sputsp("Reading inverse file: ",InverseFile);
		if ((IOF = fopen(InverseFile,"rb")) == NULL)
			{
				perror("Unable to open inverse file");
				return 1;
			}
		for(I = 0;I < IS;I++)
			{
				fread(&RF,sizeof(float),1,IOF);
				Inverse[I] = (DLReal) RF;
			}
		for (I = IS;I < CS;I++)
			Inverse[I] = (DLReal) 0.0;
		fclose(IOF);

		sputs("Inverse filter FFT...");
		rdft(CS,OouraRForward,Inverse);

		/* Check the reference file */
		if (RefSweepFile != NULL)
			{
				/* Read the reference file */
				sputsp("Reading reference file: ",RefSweepFile);
				if ((IOF = fopen(RefSweepFile,"rb")) == NULL)
					{
						perror("Unable to open reference file");
						return 1;
					}
				for (I = 0;I < CS;I++)
					Sweep[I] = (DLReal) 0.0;
				for(I = CS - IS,J = 0;J < RS;I++,J++)
					{
						fread(&RF,sizeof(float),1,IOF);
						Sweep[I % CS] = (DLReal) RF;
					}
				fclose(IOF);

				/* Convolving sweep and inverse */
				sputs("Reference inversion and convolution...");
				rdft(CS,OouraRForward,Sweep);
				hcconvolve(Sweep,Inverse,CS);

				/* Computes the RMS Level */
				RMSLevel = Sweep[0] * Sweep[0];
				for (I = 2,J = 3;I < CS;I += 2,J += 2)
					RMSLevel += Sweep[I] * Sweep[I] + Sweep[J] * Sweep[J];
				RMSLevel = (DLReal) sqrt(2.0 * RMSLevel / CS);
				RMSMin = MinGain * RMSLevel;

				/* Check the gain truncation type */
				if (DLStart >= (DLReal) 1.0)
					{
						/* Check starting components */
						if (Sweep[0] < RMSMin)
							Sweep[0] = ((DLReal) 1.0) / RMSMin;
						else
							Sweep[0] = ((DLReal) 1.0) / Sweep[0];
						if (Sweep[1] < RMSMin)
							Sweep[1] = ((DLReal) 1.0) / RMSMin;
						else
							Sweep[1] = ((DLReal) 1.0) / Sweep[1];

						/* Gain truncation scan */
						for (I = 2,J = 3;I < CS;I += 2,J += 2)
							{
								/* Gain computation */
								DLAbs = (DLReal) hypot(Sweep[I],Sweep[J]);

								/* Gain check and limitation */
								if (DLAbs < RMSMin)
									{
										/* Gain limited inversion */
										DLTheta = (DLReal) atan2(Sweep[J],Sweep[I]);
										DLAbs = ((DLReal) 1.0) / RMSMin;
										Sweep[I] = DLAbs * (DLReal) cos(-DLTheta);
										Sweep[J] = DLAbs * (DLReal) sin(-DLTheta);
									}
								else
									{
										/* Inversion and renormalization */
										DLAbs = ((DLReal) 1.0) / (DLAbs * DLAbs);
										Sweep[I] *= DLAbs;
										Sweep[J] *= -DLAbs;
									}
							}
					}
				else
					{
						/* Determina i fattori per la limitazione guadagno */
						DLSLevel = RMSMin / DLStart;
						DLGFactor = DLSLevel - RMSMin;

						/* Check starting components */
						if (Sweep[0] < DLSLevel)
							{
								/* Riassegna il guadagno del filtro */
								DLLevel = (DLSLevel - Sweep[0]) / DLGFactor;
								DLLevel = DLLevel / (((DLReal) 1.0) + DLLevel);
								Sweep[0] = ((DLReal) 1.0) / (DLSLevel - DLGFactor * DLLevel);
							}
						else
							Sweep[0] = ((DLReal) 1.0) / Sweep[0];
						if (Sweep[1] < DLSLevel)
							{
								/* Riassegna il guadagno del filtro */
								DLLevel = (DLSLevel - Sweep[1]) / DLGFactor;
								DLLevel = DLLevel / (((DLReal) 1.0) + DLLevel);
								Sweep[1] = ((DLReal) 1.0) / (DLSLevel - DLGFactor * DLLevel);
							}
						else
							Sweep[1] = ((DLReal) 1.0) / Sweep[1];

						/* Scansione per limitazione guadagno */
						for (I = 2,J = 3;I < CS;I += 2,J += 2)
							{
								DLAbs = (DLReal) hypot(Sweep[I],Sweep[J]);
								if (DLAbs <= (DLReal) 0.0)
									{
										Sweep[I] = ((DLReal) 1.0) / RMSMin;
										Sweep[J] = 0;
									}
								else
									if (DLAbs < DLSLevel)
										{
											/* Riassegna il guadagno del filtro */
											DLLevel = (DLSLevel - DLAbs) / DLGFactor;
											DLLevel = DLLevel / (((DLReal) 1.0) + DLLevel);
											DLAbs = ((DLReal) 1.0) / (DLSLevel - DLGFactor * DLLevel);

											/* Gain limited inversion */
											DLTheta = (DLReal) atan2(Sweep[J],Sweep[I]);
											Sweep[I] = DLAbs * (DLReal) cos(-DLTheta);
											Sweep[J] = DLAbs * (DLReal) sin(-DLTheta);
										}
									else
										{
											/* Inversion and renormalization */
											DLAbs = ((DLReal) 1.0) / (DLAbs * DLAbs);
											Sweep[I] *= DLAbs;
											Sweep[J] *= -DLAbs;
										}
							}
					}

				/* Inverse convolution */
				hcconvolve(Inverse,Sweep,CS);
			}

		/* Read the sweep file */
		sputsp("Reading sweep file: ",SweepFile);
		if ((IOF = fopen(SweepFile,"rb")) == NULL)
			{
				perror("Unable to open sweep file");
				return 1;
			}
		for(I = 0;I < SS;I++)
			{
				fread(&RF,sizeof(float),1,IOF);
				Sweep[I] = (DLReal) RF;
			}
		fclose(IOF);
		for (I = SS;I < CS;I++)
			Sweep[I] = (DLReal) 0.0;

		/* Convolving sweep and inverse */
		sputs("Sweep and inverse convolution...");
		rdft(CS,OouraRForward,Sweep);
		hcconvolve(Sweep,Inverse,CS);

		/* Impulse response recover */
		rdft(CS,OouraRBackward,Sweep);
		for (I = 0;I < CS;I++)
			Sweep[I] *= (DLReal) (2.0  / CS);

		/* Peak search */
		sputs("Finding impulse response peak value...");
		MP = 0;
		AMax = (DLReal) 0.0;
		for (I = 0;I < CS;I++)
			if ((DLReal) fabs(Sweep[I]) > AMax)
				{
					MP = I;
					AMax = (DLReal) fabs(Sweep[I]);
				}
		printf("Peak position: %d\n",MP);
		if (AMax > (DLReal) 0.0)
			printf("Peak value: %f (%f dB)\n",(double) AMax, (double) (20 * log10((double) AMax)));
		else
			printf("Peak value: %f (-inf dB)\n",(double) AMax);
		fflush(stdout);


		/* Writes output file */
		sputsp("Writing output file: ",OutFile);
		if ((IOF = fopen(OutFile,"wb")) == NULL)
			{
				perror("Unable to open output file");
				return 1;
			}
		for(I = 0;I < CL;I++)
			{
				RF = (float) Sweep[I];
				fwrite(&RF,sizeof(float),1,IOF);
			}
		fclose(IOF);

		/* Memory deallocation */
		free(Sweep);
		free(Inverse);

		/* Execution completed */
		sputs("Completed.");
		return 0;
	}