// FIRParamGen.cpp : Defines the entry point for the console application.
//

#include <math.h>
#include <stdio.h>	
#include <string.h>
#include <memory.h>
#include <stdlib.h>
#include <time.h>
#include <Share.h>



#include "stdafx.h"

#define PI			3.1415926535897932384626433
#define DEFFREQ		44100
#define DEFTAPS		65536		//must be even value. Freq/Taps = Filter Frequency accuracy.
#define DEFREDUCT	-200.0		//Default target reduction in dB.
#define DEFKISER	8
#define DEFLOW		100.0		//Default Low Freq
#define	DEFHIGH		1000.0		//Default High Freq

int		SampleFreq = 0;				//Sample Frequency. 44100/88200/48000/96000/176400/192000
int		Taps = 0;					//Filter Tap Length
int		alpha = 0;					//Kaiser Window Constant

int		TAPS_HALF;				//Variable but treated as constant in program
double	Pass_Freq_From = 0;			//Low Cut Freq. Frequency below this parameter is stopped.
double	Pass_Freq_To = 0;			//High Cut Freq. Frequency over this parameter is stopped.
		// If Pass_Freq_From = 0, Pass_Freq_To = 500,    it means  Low Pass Filter, Freq = 500Hz.
		// If Pass_Freq_From = 2000, Pass_Freq_To = 100000,   it means High Pass Filter, Freq = 2000Hz.
		// If Pass_Freq_From = 300, Pass_Freq_To = 3000, it means Band Pass Filter, 300 to 3000Hz.

double	EQFreqs[128];	// 128 EQ Points. Frequency.
double	EQGains[128];	// 128 EQ Gains. 

double	FreqResR[DEFTAPS];			//Freq Responce Real, Desired

double	Tempcoeff_r[DEFTAPS];	//Real,

double	Kwindow[DEFTAPS];		//Kaiser Window Parameter
double	FIRCoeff[DEFTAPS];		//Filter Coefficient

double	Freq[(DEFTAPS-1)/2];	//Result Responce:Frequency
double	Gain[(DEFTAPS-1)/2];	//Result Responce:Amplitude
double	Theta[(DEFTAPS-1)/2];	//Result Responce:Phase

void InitCoeffParams(void);	//0 Initialize for parameters
int ReadFreqs(_TCHAR*);		//Read desired Frequency
int ReadEQs(_TCHAR*);			//Read desired Room Correction EQ values
void CreateFres(void);		//Generate desired Frequency Responce
void CalcCoeffs(void);		//Generate FIR Coeffs by IDFT
void DoKaiser(int Taps, double alpha);	//Kaiser Window Function
void OutKaiser(_TCHAR*);
void OutCoeffs(_TCHAR*);		//Output FIR Coeff Parameter to File
void CalcFres(void);		//Calculate Frequency Responce
void OutFres(_TCHAR*);			//Output Frequency Responce to File
double Bessel(double alpha);//0 Bessel Function


int _tmain(int argc, _TCHAR* argv[])
{
	//Process arguments

	//Check Arguments Count
	if (argc != 9)
	{
		printf("requires 8 arguments.\n");
		printf("FIRParamGen Sample_Rate Taps Kaiser FreqFile EQFile FreqOutFile CoeffOutFile KaiserOutFile\n");
		printf("FIRParamGen 44100 8192 8 \"C:\\\\Temp\\\\Freq01.txt\" \"C:\\\\Temp\\\\EQ01.txt\" \"C:\\\\Temp\\\\Out_Freq01.txt\" \"C:\\\\Temp\\\\Out_Coeff01.txt\" \"C:\\\\Temp\\\\Out_Kaiser01.txt\" \n");
		return -1;
	}
	printf("FIRParamGen Start....\n");

	//get each arguments
	SampleFreq = _tstoi(argv[1]);
	switch(SampleFreq)
	{
	case 44100: 		break;
	case 48000: 		break;
	case 88200: 		break;
	case 96000: 		break;
	case 176400: 		break;
	case 192000: 		break;
	default:
		printf("Error:Unsupported Frequency, %dHz",SampleFreq);
		return -1;
	}
	printf("Sample Frequency    = %dHz\n",SampleFreq);

	Taps = _tstoi(argv[2]);
	if ((Taps < 128) || (Taps > 65536))
	{
		printf("Error:Unsupported Taps, Taps should be 128 to 65536, %d\n",Taps);
		return -1;
	}
	if (( Taps % 2) != 0)
	{
		printf("Error:odd Taps, Taps should be even, %d\n",Taps);
		return -1;
	}
	printf("FIR TAPS            = %d\n",Taps);

	alpha = _tstoi(argv[3]);
	if ((alpha < 2) || (alpha > 16))
	{
		printf("Error:Unsupported alpha, alpha should be 2 to 16, %d\n",alpha);
		return -1;
	}
	printf("Kaiser Window alpha = %d\n",alpha);

	_TCHAR	FreqFile[256];
	wcsncpy_s(FreqFile, 256, argv[4], wcslen(argv[4]));
	_tprintf(L"Freq Parameter file = %s\n",FreqFile);

	_TCHAR	EQFile[256];
	wcsncpy_s(EQFile, 256, argv[5], wcslen(argv[5]));
	_tprintf(L"EQ   Parameter file = %s\n",EQFile);

	_TCHAR	OFreqFile[256];
	wcsncpy_s(OFreqFile, 256, argv[6], wcslen(argv[6]));
	_tprintf(L"Freq Output File    = %s\n",OFreqFile);

	_TCHAR	OCoeffFile[256];
	wcsncpy_s(OCoeffFile, 256, argv[7], wcslen(argv[7]));
	_tprintf(L"Coeff Output File   = %s\n",OCoeffFile);

	_TCHAR	OKaiserFile[256];
	wcsncpy_s(OKaiserFile, 256, argv[8], wcslen(argv[8]));
	_tprintf(L"Kaiser Output File  = %s\n",OKaiserFile);


	//Initialize to safety
	InitCoeffParams();

	int ret;
	ret = ReadFreqs(FreqFile);
	if(ret<0)
	{
		_tprintf(L"Error:Can not open Frequency Parameter File. %s\n",FreqFile);
		return -1;
	}
	_tprintf(L"Freq Low, High      = %f,%f\n",Pass_Freq_From,Pass_Freq_To);

	ret = ReadEQs(EQFile);
	if(ret<0)
	{
		_tprintf(L"Error:Can not open Equalizer Parameter File. %s\n",EQFile);
		return -1;
	}
	_tprintf(L"EQFreq[1],EQGain[1] = %f,%f\n",EQFreqs[1],EQGains[1]);


	_tprintf(L"Creating Frequency Response.\n");
	CreateFres();

	_tprintf(L"Calculating Coeffs.\n");
	CalcCoeffs();
	_tprintf(L"Applying Kaiser Window.\n");
	DoKaiser(Taps, alpha);
	_tprintf(L"Output Kaiser Window data.\n");
	OutKaiser(OKaiserFile);
	_tprintf(L"Output Coeffs data.\n");
	OutCoeffs(OCoeffFile);
	_tprintf(L"Calculating Freq Responce.\n");
	CalcFres();
	_tprintf(L"Output Freq Responce.\n");
	OutFres(OFreqFile);

	return 0;
}

void InitCoeffParams(void)
{

	int i;
	for(i = 0; i < DEFTAPS; i++ ) 
	{
		FIRCoeff[i] = 0.0;
	}
	if (SampleFreq == 0)
		SampleFreq = DEFFREQ;

	if (Taps == 0)
	{
		Taps = DEFTAPS;
		TAPS_HALF = (DEFTAPS - 1)/2;
	}
	else
	{
		TAPS_HALF = Taps/2;
	}

	if (alpha == 0)
	{
		alpha = DEFKISER;
	}

	Pass_Freq_From = DEFLOW;
	Pass_Freq_To = DEFHIGH;
	
	EQFreqs[0] = 0.0;
	EQGains[0] = 0.0;
	for(i = 1; i < 128; i++)
	{
		EQFreqs[i] = (double)SampleFreq;	//Fill by Max Freq
		EQGains[i] = 0.0;
	}
}

int ReadFreqs(_TCHAR* filename)
{
	FILE * f;
	if ((f = _wfsopen(filename,L"r",_SH_DENYWR)) != NULL)
	{
		fscanf_s(f, "%lf,%lf",&Pass_Freq_From,&Pass_Freq_To);
		fclose(f);
	}
	else
	{
		return -1;
	}
	return 0;
}


int ReadEQs(_TCHAR* filename)
{
	int i;
	FILE * f;
	if ((f = _wfsopen(filename,L"r",_SH_DENYWR)) != NULL)
	{
		i = 1;	//Keep EQFreqs[0] = 0
		while (!feof(f))
		{
			fscanf_s(f, "%lf,%lf",&EQFreqs[i],&EQGains[i]);
			i += 1;
			if ( i > 126) break;
		}
		fclose(f);	
	}
	else
	{
		return -1;
	}

	return 0;
}
void CreateFres(void)
{
	// gain = pow(10.0, desired_dB / 20.0);
	// There are folding at Fs/2. _____L~~~~~H_____|_____H'~~~L'____. |=Fs/2

	int i;
	double gain;

	for (i = 0; i < TAPS_HALF; i++)
	{
		double CurrentFreq = SampleFreq * (i + 1)/Taps;
		if (CurrentFreq < Pass_Freq_From)
		{
			//Stop Area 1
			gain = pow(10.0, DEFREDUCT / 20.0);
		}
		else if (CurrentFreq < Pass_Freq_To) 
		{
			//Pass Area 1
			//Initialize to 0 dB
			gain = pow(10.0, 0.0);

			//Find EQ freq and Value. If There are no EQ, all gains will be 0.
			for (int k = 0; k < 126; k++)
			{
				if ((EQFreqs[k] <= CurrentFreq) && (CurrentFreq < EQFreqs[k+1]))
				{
					//Linear interpolation
					gain = pow(10.0, (EQGains[k]+ (EQGains[k+1]-EQGains[k])*(CurrentFreq - EQFreqs[k])/(EQFreqs[k+1]-EQFreqs[k])) /20.0);
					break;
				}
			}
		}
		else if (CurrentFreq <= SampleFreq / 2)
		{
			//Stop Area 2
			gain = pow(10.0, DEFREDUCT / 20.0);
		}

		FreqResR[i] = gain;
	}
	for (i = TAPS_HALF; i < Taps; i++)
	{
		//Mirror Freq Responce
		FreqResR[i] = FreqResR[Taps - i];
	}

}
void OutFres(_TCHAR* filename)
{
	//PROTOTYPE: Output to File
	FILE * f;
	_wfopen_s(&f, filename, L"w");
	fprintf(f, "Freq[Hz],Gain[dB],Theta[rad]\n");
	for (int i = 0 ; i < TAPS_HALF; i++)
	{
		fprintf(f, "%6.1lf,%6.15lf,%10.15lf\n",Freq[i],Gain[i],Theta[i]);
	}
	fclose(f);

}

void CalcCoeffs(void)
{
	// Calculate Inverse DFT for desired Frequency Responces.
	// Result is Filter Coeffs (not windowed yet)
	int i,k;
	double omega;

	for (i = 0; i < Taps; i++) 
	{
		Tempcoeff_r[i] = 0.0;	//Initialize Coeffs to zero
		for (k = 0; k < Taps; k++) 
		{
			//Calculate Coeffs by sum up 0 to Tap Count
			omega = 2.0 * PI * k * i / Taps; 
			Tempcoeff_r[i] = Tempcoeff_r[i] + FreqResR[k] * cos(omega);
		}
	}
	for (i = 0; i < Taps; i++) 
	{
		Tempcoeff_r[i] = Tempcoeff_r[i] / Taps; 
	}
	//TempCoeff is starting from 0. should be shifted half taps.
	for (i = 0; i < TAPS_HALF; i++) 
	{
		FIRCoeff[i] = Tempcoeff_r[TAPS_HALF-i];
	}
	for (i = TAPS_HALF; i < 2*TAPS_HALF; i++) 
	{
		FIRCoeff[i] = Tempcoeff_r[i-TAPS_HALF];
	}
}

void DoKaiser(int Taps, double K_alpha)
{
	int i;
	double Numer, Denom;
	double center,Kg,Kd;
	
	Denom = Bessel(K_alpha);	

	center = (double)(Taps - 1)/2;
	for (i = 0; i < Taps; i++)
	{
		Kg = (i*1.0 - center) / center;		//Regulated Distance, -1 to 1
		Kd = (K_alpha * sqrt(1.0 - Kg*Kg));	//kaiser Distance
		Numer = Bessel(Kd);
		Kwindow[i] = Numer / Denom;
		//PROTOTYPE: Actually don't have to store each Kaiser Value.
	}

	for(i = 0; i < Taps; i++) 
	{
		FIRCoeff[i] = FIRCoeff[i] * Kwindow[i];
	}
}

void OutKaiser(_TCHAR* filename)
{
	//PROTOTYPE: Output to File
	FILE * f;
	_wfopen_s(&f, filename, L"w");
	for (int i = 0 ; i < Taps; i++)
	{
		fprintf(f, "%3.10lf\n",Kwindow[i]);
	}
	fclose(f);
}

double Bessel(double alpha)
{
	double delta = 1e-14;
	double BesselValue;
	double Term, k, F;
                                          
	k = 0.0;
	BesselValue = 0.0;
	F = 1.0;
	Term = 0.5;
	while( (Term < -delta) || ( delta < Term) )
	{
		k = k + 1.0;						// k = step 1,2,3,4,5
		F = F * ((alpha / 2)/k);			// f(k+1) = f(k)*(c/k),f(0)=1. c is alpha/2. c/k -> 0
		Term = F*F;							// Current term value
		BesselValue = BesselValue + Term;	// Sum(f(k)^2)
	}
	return BesselValue;
}

void OutCoeffs(_TCHAR* filename)
{
	//PROTOTYPE: Output to File
	FILE * f;
	_wfopen_s(&f, filename, L"w");
	for (int i = 0 ; i < Taps; i++)
	{
		fprintf(f, "%3.15lf\n",FIRCoeff[i]);
	}
	fclose(f);
}
void CalcFres(void)
{

	int i, k;
	double	omega;		//angular frequency
	double	Resp_Real;	//Frequency Responce, Real
	double	Resp_Image;	//Frequency Responce, imaginary
	double	AmpABS2;	//square of amplitude characteristic
	double	Resp_Phase;	//phase characteristic

	for(i = 0; i < TAPS_HALF; i++) 
	{
		//omega = 0 to 2Pi (rad)
		omega = (i / (Taps/360.0) * 2.0 * PI  ) / 360.0;

		//initialize
		Resp_Real = 0.00; 
		Resp_Image = 0.00;
		
		//accumulate for all TAP of FIR
		for(k =	0; k < Taps; k++) 
		{
			Resp_Real = Resp_Real + FIRCoeff[k] * cos(k * omega);
			Resp_Image = Resp_Image + FIRCoeff[k] * sin(k * omega);
		}
		//calculate square of amplitude characteristic
		AmpABS2 = Resp_Real * Resp_Real + Resp_Image * Resp_Image;

		//calculate phase characteristic
		if (Resp_Real != 0.0)
		{
			Resp_Phase = (-1) * atan(Resp_Image / Resp_Real);
		}
		else
		{
			Resp_Phase = (-1) * 0.5 * PI;
		}

		//get dB value for amplitude characteristic
		Gain[i] = 10 * log10(AmpABS2);
		Theta[i] = Resp_Phase;
		Freq[i] = SampleFreq / Taps * (i+1);
	}

}