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


//-----------------------------------------------------
// 定数定義
//-----------------------------------------------------
//サンプリング周波数
#define SAMPLEING_RATE 	88200
//円周率
#define PI		3.14159265358979
//ガボールフィルタの数
#define GABOR_CNT	8
//ファイルＩ／Ｏ時のブロック長
#define	FILE_IO_UNIT 	1024


//入力ファイル名
#define INPUT_FILE_NAME "input.pcm"
//出力ファイル名（フォーマット）
#define OUTPUT_FILE_NAME_FORM "out-filter%d.pcm"


//-----------------------------------------------------
// マクロ
//-----------------------------------------------------
//DELETE時にポインタをNULLにするマクロ
#define SAFE_DELETE(p) { if(p) { delete [] (p); (p)=NULL; } }

//-----------------------------------------------------
// 複素数クラス
//-----------------------------------------------------
struct Complex {
	double real;
	double image;
	
	Complex() {
		Clear();
	}

	Complex(double r,double i) {
		real = r;
		image = i;
	}

	Complex(double r) {
		real = r;
		image = 0;
	}

	//絶対値を返す
	double abs() {
		return sqrt(real * real + image * image);
	}
	//位相を返す
	double phase() {
		if (real== 0 && image == 0) return 0;
		return atan2(real, image);
	}


	Complex operator * (double opp) 
	{
		return Complex(real * opp,image * opp);
	}

	Complex operator / (double opp) 
	{
		return Complex(real / opp,image / opp);
	}

	Complex operator / (int opp) 
	{
		return Complex(real / opp,image / opp);
	}

	Complex& operator += (Complex& opp) 
	{
		real += opp.real;
		image += opp.image ;
		return *this;
	}

	Complex& operator -= (Complex& opp) 
	{
		real -= opp.real;
		image -= opp.image ;
		return *this;
	}
	Complex operator + (Complex& opp) 
	{
		return Complex(real + opp.real ,image + opp.image);
	}

	Complex operator - (Complex& opp) 
	{
		return Complex(real - opp.real ,image - opp.image);
	}

	void Clear() {
		real = 0;
		image = 0;
	}
};

//-----------------------------------------------------
// 擬似ガウス窓クラス
//-----------------------------------------------------
class Usouso_Gaussian {

	Complex	*pBuf;    //フィルタ用のバッファ
	int		cntState; //平均化フィルタの段数
    int     size[6];  //格段のバッファサイズ(段数は最大でも6つ程度と仮定）
	int		index[6]; //バッファインデックス
	Complex	integ[6]; //積分値

public:

	//コンストラクタ
	Usouso_Gaussian() {
		pBuf = NULL;
		cntState=0;
		for (int i=0;i<6;i++) {
			integ[i] = 0;
			size[i] = index[i] = 0;
		}
	}
	//デストラクタ
	~Usouso_Gaussian() {
		SAFE_DELETE(pBuf);
	}
	
	//データの確保
	void Alloc(int sample, int stage) 
	{

		//各ステージごとのサイズ（サンプル長）を決定する。
		int total_size, base, distrib;
		if (stage > 6) stage = 6; //最大６ステージ
		if (sample <= stage && sample > 1) stage = sample - 1; //無駄なステージは作らない
		cntState = stage;
		total_size = sample + stage - 1; //バッファの全長
		base = total_size / cntState;
		distrib = total_size % cntState;
		//各パラメータ初期化
		for (int i = 0; i < cntState; i++) {
			size[i] = base;
			if (i < distrib) size[i]++;
			index[i] = 0;
			integ[i] = 0;
		}
		//バッファの作成と初期化
		pBuf = new Complex[total_size];
		for (i = 0; i < total_size; i++) pBuf[i] = 0;

	}

	//窓関数をかける
	Complex WindowFunc(Complex input) 
	{
		int stage; //フィルタ段のインデックス
		Complex* pOffset;
		
		pOffset = pBuf;
		for (stage = 0; stage < cntState; stage++) {
			integ[stage] -= pOffset[index[stage]];
			pOffset[index[stage]] = input;
			integ[stage] += input;
			input = integ[stage] / size[stage];
			//インデックスの移動
			index[stage]++;
			if (index[stage] == size[stage]) index[stage] = 0; 
			//相対インデックスを次のバッファへ設定
			pOffset += size[stage]; 
		}
		return input;
	}
};


//-----------------------------------------------------
// ガボールフィルタクラス
//-----------------------------------------------------
class GaborFilter {
	
	Usouso_Gaussian gauss;
	double phase;
	double wavelength; //波長（サンプル数）
	double freq_per_sample;  //周波数（サンプリング周波数との比）
	int	   filter_len;


public:
	

	GaborFilter() {phase = wavelength = freq_per_sample = filter_len = 0;}
	~GaborFilter() {};

	void Prepare(double freq, int stage, int times) {
		
		freq_per_sample  = freq / SAMPLEING_RATE;
		wavelength = 1.0 / freq_per_sample;
		phase = 0.0;

		//Usouso_Gaussianの設定
		filter_len = floor(wavelength * times + 0.5); //波長の何倍のサンプル数か？
		gauss.Alloc(filter_len, stage);
	}
	
	//実際のフィルタ処理を行う
	Complex Filter(double in) {
		Complex c_in;
		//回転因子を掛け合わせる
		c_in.real =  cos(phase     ) * in;
		c_in.image = - cos(phase + PI / 2.0) * in;

		//位相を進める
		phase += freq_per_sample * 2 * PI;
		if (phase > PI) phase -= PI * 2.0;

		//フィルタを掛けた結果を返す
		return gauss.WindowFunc(c_in);

	}
	
	//現在の位相
	double Phase() {return phase;}
	//最大振幅部の位相
	double CenterPhase() {return phase;}
	//フィルタ長
	int FilterLen() {return filter_len;}
	//
	double GetFreqency() {return freq_per_sample * SAMPLEING_RATE;}

};


//-----------------------------------------------------
// 主処理
//-----------------------------------------------------
int main(int argc, char* argv[])
{

	int i, j;
	GaborFilter gabor[GABOR_CNT];
	Complex out;
	FILE *fp_in, *fp_out[GABOR_CNT];
	float	fdata[FILE_IO_UNIT], 
			fout[FILE_IO_UNIT][2];
	int read_size;

	
    //------------各ガボールフィルタ初期化-------------------------
	for (i=0;i<GABOR_CNT;i++) {
			
		gabor[i].Prepare(
			(i+1) * (i+1) * 200,	//周波数
			5,						//段数
			8						//フィルタ内のサイクル数
		);	
		
		//出力ファイル名
		char outfname[128];
		sprintf(outfname, OUTPUT_FILE_NAME_FORM, i + 1);
		fp_out[i] = fopen(outfname,	 "wb");

		if (fp_out[i] == NULL) {
				printf("%sが開けません",outfname);
				return -1;
		}


	}

	//入力ファイルを開く
	//入力形式
	//32BitFPのモノラルPCM形式
	fp_in = fopen(INPUT_FILE_NAME, "rb");
	if (fp_in == NULL) {
			printf("%sが開けません", INPUT_FILE_NAME);
			return -1;
	}
	
	int pos = 0;
	//入力ファイルを読み込むループ
	while (!feof(fp_in)) 
	{
		//データブロック読み込み
		read_size = fread((void*)fdata, sizeof(float), FILE_IO_UNIT,  fp_in);
		
		//各ガボールフィルタのループ
		for (j = 0; j < GABOR_CNT; j++)
		{
			//データブロック内のループ
			for (i = 0; i < read_size ; i++) {
				
				out= gabor[j].Filter(fdata[i]);
				
				fout[i][0] = out.abs(); //振幅
				
				//位相
				double phase;
				phase = out.phase() - gabor[j].Phase();
				if (phase < - PI) phase += 2 * PI;
				fout[i][1] = phase; 

			}
			int latency;
			latency = gabor[j].FilterLen() / 2;
			if (latency < pos) {
				//データ書き出し
				//出力形式は
				//32BitFPの2chPCM形式とし、ch1に振幅、ch2に位相
				fwrite(
						(void*)fout, 
						sizeof(float) * 2,
						read_size, 
						fp_out[j]
					);
				if (feof(fp_in)) {
					//入力を０としてレイテンシ部分を書き出す
					i = 0;
					while ( latency < 0 ) {
						int loop_len;
						loop_len = __min(latency, FILE_IO_UNIT);
						for (i=0;i < loop_len; i++ ) {
							out= gabor[j].Filter(0.0);
							
							fout[i][0] = out.abs(); //振幅
							//位相
							double phase;
							phase = out.phase() - gabor[j].Phase();
							if (phase < - PI) phase += 2 * PI;
							fout[i][1] = phase; 
						}

						fwrite(
								(void*)fout, 
								sizeof(float) * 2,
								loop_len, 
								fp_out[j]
							);

						latency -= FILE_IO_UNIT;
					}
				}


			}
			else if ( pos <= latency && latency <= pos + read_size) { 
				
				//データ書き出し
				//出力形式は
				//32BitFPの2chPCM形式とし、ch1に振幅、ch2に位相
				fwrite(
						(void*)&fout[latency - pos], 
						sizeof(float) * 2,
						(read_size + pos - latency), 
						fp_out[j]
					);
				
			}
		}

		pos += read_size;
	}      

	//後始末
	fclose(fp_in);
	for (i=0;i<GABOR_CNT;i++) {
		fclose(fp_out[i]);
	}

	return 0;
}