// MAKETSP.cpp 
//
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
/*---------------------------------------------------------
ＦＦＴのライブラリはこちらのサイト様のものを借用しました。
http://www.kurims.kyoto-u.ac.jp/~ooura/index-j.html      
---------------------------------------------------------  */
#include "fft4g.c"


//#define _DEBUG_FUNC


/************** 定数・マクロ定義 *************************************/
//TSPの全体のサンプル数
#define TSP_DEFAULT_TSP_SIZE		1 << 17
//正ＴＳＰ波形出力ファイル
#define TSP_DEFAULT_FILENAME		"TSP.pcm"
//逆ＴＳＰ波形出力ファイル
#define TSP_DEFAULT_FILENAME_INV	"TSP-INV.pcm"
//補正信号波形ファイル
#define ADJUST_DEFAULT_FILENAME		"ADJUST.pcm"
//ＴＳＰ長
#define TSP_LENGTH					40000
//振幅特性の補正を制限する閾値（中域）
#define MIN_AMP_ADJUST				-24
//振幅特性の補正を制限する閾値（高域・低域）
#define MIN_AMP_ADJUST_EDGE				-16
//振幅特性の補正で低域と扱われる最高周波数
#define MIN_AMP_ADJUST_LO_EDGE_SIZE		0.0005
//振幅特性の補正で高域と扱われる最低周波数
#define MIN_AMP_ADJUST_HI_EDGE_SIZE		0.6
//振幅特性の補正を制限する閾値:逆フィルタ側
#define MIN_AMP_ADJUST_I				-32
//特性補正用グラフの凹凸を平均化するフィルタ長
#define AMP_ADJUST_LPF_SIZE			2048

//円周率
#define PI							3.14159265358979
//許容誤差
#define GOOD_ERROR					((1.0 / (1 << 25)) / (1 << 25))
//二分法打ち切り処理回数
#define MAX_LOOP					4000

//デシベル変換マクロ
#define FROM_DB(x) (pow(10,x / 20.0))

/************** 構造体定義 *************************************/
//複素数
struct Complex {
	double real;
	double image;
};

/************** 関数プロトタイプ *************************************/
int getImpulse(Complex* pImpulse, int iTSPSize,char* pchFileName);
makeTSP(Complex* pFreqArea,double* pAdjustAmpGraph,double* pAdjustAmpGraph_I,double* pAdjustPhaseGraph_I,int iTSPSize,bool bInv) ;
int makeFile(Complex* pTsp, int iTSPSize,char* pchFileName);
int getAdjustGraph(double* pAdjustAmpGraph, double* pAdjustAmpGraph_I, double* pAdjustPhaseGraph_I, Complex* pImpulse, int iHalfTSPSize);
double LPF(double ix, double* pBuf);


/************** メイン関数 *************************************/
int main(int argc, char* argv[])
{
	char pchFileName[1024];//ファイル名
	int  iTSPSize;//TSP長
	int  iHalfTSPSize; //TSP長の半分
	int  ret;	//戻り値用
	double  *pAdjustAmpGraph;     //振幅特性補正量(振幅)格納配列
	double  *pAdjustAmpGraph_I;  //振幅特性補正量格納配列（逆ＴＳＰ側）
	double  *pAdjustPhaseGraph_I;//位相特性補正量格納配列（逆ＴＳＰ側）

	//データ初期化
	iTSPSize = TSP_DEFAULT_TSP_SIZE; 
	iHalfTSPSize = iTSPSize / 2;
	Complex* pTSP = new Complex[iTSPSize];
	Complex* pImp = new Complex[iTSPSize];



	//特性補正用インパルス応答データ取得
	strcpy(pchFileName,ADJUST_DEFAULT_FILENAME);
	ret = getImpulse(pImp , iTSPSize, pchFileName);

	//振幅特性補正グラフ生成
	pAdjustAmpGraph = NULL;
	if (ret == 0) {
		pAdjustAmpGraph = new double[(iHalfTSPSize+1) * 3];
		pAdjustAmpGraph_I = &(pAdjustAmpGraph[iHalfTSPSize+1]);
		pAdjustPhaseGraph_I = &(pAdjustAmpGraph_I[iHalfTSPSize+1]);
		getAdjustGraph(pAdjustAmpGraph, pAdjustAmpGraph_I,pAdjustPhaseGraph_I, pImp, iHalfTSPSize);
	}

	//正ＴＳＰ生成
	strcpy(pchFileName,TSP_DEFAULT_FILENAME);
	makeTSP(pTSP ,pAdjustAmpGraph, pAdjustAmpGraph_I,pAdjustPhaseGraph_I, iTSPSize, false);
	makeFile(pTSP, iTSPSize, pchFileName);

	//逆ＴＳＰ生成
	strcpy(pchFileName,TSP_DEFAULT_FILENAME_INV);
	makeTSP(pTSP, pAdjustAmpGraph, pAdjustAmpGraph_I,pAdjustPhaseGraph_I, iTSPSize, true);
	makeFile(pTSP, iTSPSize, pchFileName);
	
	if (pAdjustAmpGraph != NULL) delete [] pAdjustAmpGraph;
	delete [] pTSP;
	delete [] pImp;


	return 0;

}

/**************************************************************
F
掃出線グラフ定義関数
x 　　... 時刻 (0<=x<=1)
戻り値...周波数
**************************************************************/
double F(double x) {

/*４点を通る線分の組み合わせ
	double t[4],hz[4];

	t[0]=0;   hz[0] = 0;
	t[1]=0.3; hz[1] = 220;
	t[2]=0.8; hz[2] = 3500;
	t[3]=1.0; hz[3] = 22050;
	
	for (int i=0;i<3 && !(t[i] <= x && t[i+1] > x) ;i++)
		;
	
	x= ((i==0) ? 
		(x * hz[1] / t[1]) : 
		(hz[i+1] - hz[i]) 
			/ (t[i+1] - t[i]) 
			* (x  -  t[i ]) + hz[i]);

	return x;    */   
	
/*????????????	return  x * x * x * x * 20 + x* x*x*x*x*x*x*x*100 + (1 - 1.0/(x*10+1));*/

/*指数関数グラフ*/
	return (pow(200,x) - 1 )  ; 

/*従来型
	return x;*/
/* 高次式　
	return x*x*x*x;*/
}


/**************************************************************
getImpulse  
補正信号の伝達関数グラフ取得関数
pImpulse   .. [OUT]  特性補正対象の伝達関数グラフ
iTSPSize   .. [IN]  ＴＳＰのサイズ
pchFileName.. [IN]  特性補正用のインパルス応答波形データのファイル名
**************************************************************/
int getImpulse(Complex* pImpulse, int iTSPSize,char* pchFileName) {

	FILE *fp;
	float data;

	fp = fopen(pchFileName,"rb");
	if (fp==NULL) return -1;

	int i;
	//インパルス応答取得
	for (i =0 ; i < iTSPSize ; i++) {
 		if (feof(fp) == 0) {
			fread(&data, sizeof(float), 1, fp);
		} else {
			data = 0;
		}
		pImpulse[i].real = data;
		pImpulse[i].image = 0;
	}
	fclose(fp);

	//フーリエ変換
	//時間領域に展開
	int *ip = new int[3+(int)sqrt(iTSPSize)]; //バタフライワーク
	double *table = new double[iTSPSize/2]; //サインワーク
	ip[0] = 0;
	//FFTライブラリ呼び出し
	cdft(iTSPSize * 2, 1, (double*)pImpulse, ip, table);

	delete [] ip;
	delete [] table;


	return 0;
}

/**************************************************************
makeTSP  
ＴＳＰ生成関数
pFreqArea  .. [OUT] 実時間波形を返すバュファ
pAdjustAmpGraph     .. [IN]  振幅特性補正グラフ
pAdjustAmpGraph_I   .. [IN]  逆ＴＳＰの振幅特性補正グラフ
pAdjustPhaseGraph_I .. [IN]  逆ＴＳＰの位相特性補正グラフ
iTSPSize   .. [IN]  ＴＳＰのサイズ
bInv       .. [IN]  falseならば正ＴＳＰ  trueならば逆ＴＳＰ
**************************************************************/
int makeTSP(
	Complex* pFreqArea,
	double* pAdjustAmpGraph, 
	double* pAdjustAmpGraph_I,
	double* pAdjustPhaseGraph_I, 
	int iTSPSize, 
	bool bInv) 
{
	
	int		i;
	double	Q ;	//ＴＳＰサイズ
	double	A;//振幅重み係数
	int		iHalfTSPSize;	//ＴＳＰサイズの半分
	double	iHalfTSPSizeInv;//iHalfTSPSizeの逆数
	double	direction;		//位相関数内の変数の符号
	double	*pPhaseGraph;	//位相回転量格納配列
	double	*pAmpGraph;		//振幅格納配列
	double  maxF;			//F(1)の値
	double	x_y,bef_ix, a, b, ix, nx, //逆関数計算用
			lpf_ix,bef_lpf_ix,pLPFBuf[5];//フィルタ用
	double  phaseShift; //位相補正量

	iHalfTSPSize = iTSPSize / 2;
	iHalfTSPSizeInv = 1.0 / iHalfTSPSize; 

	//ワークエリア確保
	pPhaseGraph = new double[iHalfTSPSize+1];
	pAmpGraph = new double[iHalfTSPSize+1];

	//ローパスフィルタ用バッファ初期化
	bef_ix = ix =lpf_ix=bef_lpf_ix=0;
	for (i = 0;i < 4;i++) pLPFBuf[i] = 0;
	pLPFBuf[i] = 1;

	/*********************************************************
	* 0 ≦ n ≦ 1におけるF()の逆関数の積分関数を求める
	* F()は原点を通る単調増加する連続関数とする
	*********************************************************/
	maxF = F(1);
	pPhaseGraph[0] = 0; 
	for (i=1; i <= iHalfTSPSize;i++) {

		x_y = i * iHalfTSPSizeInv;

		nx = x_y - F(x_y) / maxF;

		//ixが定まっていない時は仮にx_yと同じとする
		if (ix == 0) ix = x_y;
		
		if (nx != 0) { 
			//初期値の推定（単調増加なので、かならず1つ前のループの値よりも大きくなると仮定）
			if (nx > 0) {
				for (a = b = ix; F(b) / maxF < x_y; b =b * 1.1 + .01)
					a = b;
			}
			else  {
				a = bef_ix;
				b = x_y;
			}
			int cnt = 0;
			//二分法によりixを収束させる
			while (fabs(nx) > GOOD_ERROR && cnt++ < MAX_LOOP) {
				ix = (a + b) * 0.5;
				nx = x_y - F(ix) / maxF;

				if (nx == 0.0) break;
				if (nx > 0.0) {
					a = ix;
				} else {
					b = ix;
				}			
			}	
		} else {
			ix = x_y;
		}


		lpf_ix = LPF(ix, pLPFBuf);

		//位相シフト量グラフ
		if (pAdjustAmpGraph == NULL) {	
		//補正なし
			pPhaseGraph[i] = pPhaseGraph[i-1] + lpf_ix;
		} else {
		//補正あり
			//まずは２つのグラフの微分値同士を掛け合わせる
			pPhaseGraph[i] = lpf_ix - bef_lpf_ix;
			pPhaseGraph[i] *= pAdjustAmpGraph[i] * pAdjustAmpGraph[i];
			if (i==1) pPhaseGraph[0] = lpf_ix * pAdjustAmpGraph[0] * pAdjustAmpGraph[0]; //[0]の時は[1]の値と同じ値を設定して近似させる
			//結果を積分する
			pPhaseGraph[i] += pPhaseGraph[i-1];
		}

		//増幅量グラフ
		pAmpGraph[i] = lpf_ix - bef_lpf_ix;
		if (i==1) pAmpGraph[0] = lpf_ix; //[0]の時は[1]の値と同じ値を設定して近似させる

		bef_lpf_ix = lpf_ix;
		bef_ix = ix;

#ifdef _DEBUG_FUNC
//	pPhaseGraph[i] = F(x_y) / maxF;
//	pAmpGraph[i]= ix;

#endif
	}

	if (pAdjustAmpGraph != NULL) {
		//位相シフト量グラフを積分して群遅延グラフを得る
		for (i=1; i <= iHalfTSPSize;i++) {
			pPhaseGraph[i] += pPhaseGraph[i-1];
		}
		//原点を通るように修正
		for (i=0; i <= iHalfTSPSize;i++) {
			pPhaseGraph[i] -= pPhaseGraph[0];
		}
	}

	//F()は単調増加するのでpPhaseGraph[iHalfTSPSize]が最大値
	Q = 2 * PI *  TSP_LENGTH  / ( pPhaseGraph[iHalfTSPSize]);

	direction = bInv ? 1.0 : -1.0;

	//周波数領域の設定
	for (i = 0; i <= iHalfTSPSize; i++) {

#ifndef _DEBUG_FUNC

		//0 ~ iHalfTSPSize
		double jexp; 

		jexp =  pPhaseGraph[i]  * Q;

		//振幅はf()の微分の平方根
		A = sqrt(pAmpGraph[i]);

		if (bInv) A = 1.0 / A; //逆ＴＳＰの場合
		
		//振幅特性の補正
		if (pAdjustAmpGraph != NULL) {
			if (bInv) {
				A *= pAdjustAmpGraph_I[i];
			} else {
				A *= pAdjustAmpGraph[i];
			}
		}
		
		//位相関数
		phaseShift = (bInv && pAdjustAmpGraph != NULL) ? -pAdjustPhaseGraph_I[i]  : 0;
		pFreqArea[i].real = cos( direction * jexp + phaseShift) * A;
		pFreqArea[i].image = -cos( PI / 2.0 + direction * jexp + phaseShift) * A;

		//ナイキスト周波数より上の帯域の設定
		if (i > 0 ) {  
			pFreqArea[iTSPSize - i].real  =   pFreqArea[i].real;
			pFreqArea[iTSPSize - i].image = - pFreqArea[i].image;	
		}


#else 
	pFreqArea[i].real = pPhaseGraph[i];
	pFreqArea[iHalfTSPSize + i - 1].real = pAmpGraph[i];
#endif

	}
	
	delete [] pPhaseGraph;
	delete [] pAmpGraph;

#ifndef _DEBUG_FUNC

	//時間領域に展開
	int *ip = new int[3+(int)sqrt(iTSPSize)]; //バタフライワーク
	double *table = new double[iTSPSize/2]; //サインワーク
	ip[0] = 0;
	//FFTライブラリ呼び出し
	cdft(iTSPSize * 2, 1, (double*)pFreqArea, ip, table);

	delete [] ip;
	delete [] table;

#endif
	return 0;
}



/**********************************************************************
LPF  
ローパスフィルタ関数
ix         .. [IN]      フィルタ処理前の波形
iTSPSize   .. [IN/OUT]  フィルタ用バッファ
戻り値     .. フィルタ処理後の波形
***********************************************************************/
double LPF(double ix, double* pBuf) {
	
	return ix;
		
	//ローパスフィルタ
	if (pBuf[0] > 1) {
		pBuf[1] += ( ix - pBuf[1]) * pBuf[4];
		pBuf[2] += ( pBuf[1] - pBuf[2]) * pBuf[4];
		pBuf[3] += ( pBuf[2] - pBuf[3]) * pBuf[4];
		//フィルタの強さの設定（０Ｈｚ付近はフィルタをかけない）
		if (pBuf[4] > 0.005) pBuf[4] *= .99;
	} else
	{
		pBuf[1] = pBuf[2] = pBuf[3] = ix;
		pBuf[0]++;
	}
	return pBuf[3];
		
}


/**********************************************************************
getAdjustGraph  
振幅特性補正グラフ生成関数
pAdjustAmpGraph     .. [OUT]  振幅特性補正グラフ
pAdjustAmpGraph_I   .. [OUT]  逆ＴＳＰの振幅特性補正グラフ
pAdjustPhaseGraph_I .. [OUT]  逆ＴＳＰの位相特性補正グラフ
pImpulse   .. [IN/OUT]  特性補正対象の伝達関数グラフ
iHalfTSPSize   .. [IN]  TSP長の半分
戻り値     ..  0-成功 1-エラー
***********************************************************************/
int getAdjustGraph(double* pAdjustAmpGraph, double* pAdjustAmpGraph_I, double* pAdjustPhaseGraph_I, Complex* pImpulse, int iHalfTSPSize) {

	int i,j,k;
	double max, max2; //振幅の最大値
	double adjust_min_amp;//特性補正の下限振幅
	double adjust_min_amp_edge;//特性補正の下限振幅(高域・低域）
	double *pLPFBuf;//ローパスフィルタのグラフ
	double *pAdjust2; //ローパスフィルタ処理用の中間データ
	double lpfout; //ローパスフィルタ後の値
	int iHalfLPFSize;//ローパスフィルタ長の半分

	//振幅特性の二乗を得る
	max=0;
	pAdjust2 = new double[iHalfTSPSize+1];
	for (i=0;i<=iHalfTSPSize;i++) {
		//振幅特性を得る
		pAdjust2[i] = 
			sqrt(pImpulse[i].real * pImpulse[i].real + pImpulse[i].image * pImpulse[i].image);
		//逆ＴＳＰの補正用
		pAdjustAmpGraph_I[i] = pAdjust2[i]; 
		//位相特性（の負数）を得る
		pAdjustPhaseGraph_I[i] = atan2(pImpulse[i].real, pImpulse[i].image) - PI / 2;
		//振幅の最大値を取得
		if (max < pAdjust2[i]) max = pAdjust2[i];
	}

	//小さな振幅を大きな振幅に制限する（過剰な補正を避けるため）
	adjust_min_amp = max * FROM_DB(MIN_AMP_ADJUST); 
	adjust_min_amp_edge = max * FROM_DB(MIN_AMP_ADJUST_EDGE); 

	for (i=0;i<=iHalfTSPSize;i++) {
		if (i >= MIN_AMP_ADJUST_LO_EDGE_SIZE * iHalfTSPSize && 
			i <= MIN_AMP_ADJUST_HI_EDGE_SIZE * iHalfTSPSize) {

			if (pAdjust2[i]  < adjust_min_amp) pAdjust2[i] = adjust_min_amp;

		} else {

			if (pAdjust2[i]  < adjust_min_amp_edge) pAdjust2[i] = adjust_min_amp_edge;

		}

	}	

	//ローパスフィルタのグラフ生成（ハミング窓）
	pLPFBuf = new double[AMP_ADJUST_LPF_SIZE];

	for (i=0;i < AMP_ADJUST_LPF_SIZE;i++) {
		pLPFBuf[i] = 1.0 - cos(i * PI * 2.0  / AMP_ADJUST_LPF_SIZE) ;
	}
	
	//ローパスフィルタをかけて、振幅の細かな変動を平均化する
	//加えて補正用の振幅特性グラフを生成する
	iHalfLPFSize = AMP_ADJUST_LPF_SIZE / 2;
	max = 0;
	max2 = 0;
	for (i=0;i<=iHalfTSPSize;i++) {
		lpfout = 0;
		for (j = 0; j <  AMP_ADJUST_LPF_SIZE; j++) {
			k = i - iHalfLPFSize + j;
			if (k < 0) k = 0;
			if (k > iHalfTSPSize) k = iHalfTSPSize;
			lpfout += pAdjust2[k] * pLPFBuf[j];
		}

		//正ＴＳＰ用補正振幅特性
		pAdjustAmpGraph[i] = 1 / lpfout;
		if (max2 < pAdjustAmpGraph[i]) max2 = pAdjustAmpGraph[i];

		//逆ＴＳＰ用補正振幅特性
		pAdjustAmpGraph_I[i] /=  lpfout;
		if (max < pAdjustAmpGraph_I[i]) max = pAdjustAmpGraph_I[i];
		
	}

	//小さな振幅を大きな振幅に制限する（逆TSP側）
	adjust_min_amp = max * FROM_DB(MIN_AMP_ADJUST_I); 
	for (i=0;i<=iHalfTSPSize;i++) {
		if (pAdjustAmpGraph_I[i]  < adjust_min_amp) pAdjustAmpGraph_I[i] = adjust_min_amp;
	}	

	
	//ローパスフィルタをかけて、逆ＴＳＰ用振幅特性の細かな変動を平均化する
	max=0;
	for (i=0;i<=iHalfTSPSize;i++) {
		lpfout = 0;
		//正ＴＳＰ用と比べてローパスフィルタ長を1/8とする
		for (j = 0; j <  AMP_ADJUST_LPF_SIZE; j+=8) {
			k = i + ( j -  iHalfLPFSize ) / 8;
			if (k < 0) k = 0;
			if (k > iHalfTSPSize) k = iHalfTSPSize;
			lpfout += pAdjustAmpGraph_I[k] * pLPFBuf[j];
		}

		//逆ＴＳＰ用補正振幅特性
		pAdjust2[i] =  1 / lpfout;
		if (max < pAdjust2[i]) max = pAdjust2[i];
	}
	
	//ノーマライズ
	for (i=0;i<=iHalfTSPSize;i++) {
		pAdjustAmpGraph[i] = pAdjustAmpGraph[i] / max2;
		pAdjustAmpGraph_I[i] = pAdjust2[i] / max;
	}

	delete [] pLPFBuf;
	delete [] pAdjust2;

	return 0;
}

int makeFile(Complex* pTsp, int iTSPSize,char* pchFileName) {

	FILE *fp;
	float data;
	fp = fopen(pchFileName,"wb");

	int i;
	
	for (i =0 ; i < iTSPSize ; i++) {
		int shift;

		shift =(i +  (iTSPSize - TSP_LENGTH)  / 2) % iTSPSize;
		data = (float)pTsp[i].real;
		fwrite(&data, sizeof(float), 1, fp);
	}

	fclose(fp);


	return 0;
}