//this progarm is for test correlation filter
#include <stdio.h>
#include <math.h>

const int N=62;//采樣點數
const double PI=3.1415;
const double T=64;
double dSrcData[N];
double dDestData[N];
void Correlator(const double dSrcData[],double dDestData[]);
void main(void)
{
	int i;
	//printf("輸入的數據(有直流分量，有高頻噪聲)：\n");
	for(i=0;i<N;i++)
	{
		dSrcData[i]=1.5+2.5*sin(2.0*PI*(1/T)*i+PI/2)+cos(3200*2.0*PI*(1/T)*i)+sin(2425*2.0*PI*(1/T)*i)-0.3*cos(25000.0*2.0*PI*(1/T)*i)+0.25*sin(10200.0*2.0*PI*(1/T)*i);
		//printf("%f\n",dSrcData[i]);
	}
	Correlator(dSrcData,dDestData);//相關濾波
	printf("輸出的數據應該為：\n");
	for(i=0;i<N;i++)
	{
		printf("%f\n",(2.5*sin(2.0*PI*(1/T)*i+PI/2)));
	}

	printf("\n輸出的數據：\n");
	for(i=0;i<N;i++)
	{
		printf("%f\n",dDestData[i]);
	}
}

void Correlator(const double dSrcData[],double dDestData[])
{
	int i=0;
	double dFilterX[N];
	double dFilterY[N];
	double dResultX=0.0;
	double dResultY=0.0;
	double dAmplitude=0.0;
	double dPhase=0.0;
	for(i=0;i<N;i++)//定義濾波器
	{
		dFilterX[i]=sin(2.0*PI*(1/T)*i);
		dFilterY[i]=cos(2.0*PI*(1/T)*i);
	}
	for(i=0;i<N;i++)
	{
		dResultX=dResultX+dSrcData[i]*dFilterX[i];
		dResultY=dResultY+dSrcData[i]*dFilterY[i];
	}
	dResultX=dResultX*2/(T*N/T);
	dResultY=dResultY*2/(T*N/T);
	dAmplitude=sqrt(dResultX*dResultX+dResultY*dResultY);
	dPhase=atan(dResultY/dResultX);
	for(i=0;i<N;i++)
	{
		dDestData[i]=dAmplitude*sin(2.0*PI*(1/T)*i+dPhase);
	}
	//幅值和相位：
	printf("幅值：%f\n",dAmplitude);
	printf("相位: %f\n",(dPhase*180/PI));
}