#include"math.h"


#define IIRNUMBER 3 
#define PI 3.1415926

float InputWave();
float IIR_RY();

float fAn1_ry[IIRNUMBER]={1.0,2.0,1.0};
float fAn2_ry[IIRNUMBER]={1.0,-1.9648,0.9648};
float fBn1_ry[IIRNUMBER]={1.8615,-0.8808,0.0};
float fBn2_ry[IIRNUMBER]={1.6873,-0.6929,0.0};
float fXn1_ry[IIRNUMBER]={ 0.0 };
float fYn1_ry[IIRNUMBER]={ 0.0 };
float fXn2_ry[IIRNUMBER]={ 0.0 };
float fYn2_ry[IIRNUMBER]={ 0.0 };

float fInput,fOutput;
float fSignal1,fSignal2;
float fStepSignal1,fStepSignal2;
float f2PI;
float fIn[256],fOut[256];
int nIn,nOut;
float fSum1_ry,fSum2_ry;

main()
{
	nIn=0; nOut=0;
	f2PI=2*PI;
	fSignal1=0.0;    //起始相位
	fSignal2=PI*0.1;
	//fStepSignal1=2*PI/30;
	//fStepSignal2=2*PI*1.4;
	//fStepSignal1=2*PI*30*0.02/50;
	fStepSignal1=2*PI*100*0.02/128;
	fStepSignal2=2*PI*20*0.02/128;
	while ( 1 )
	{
		fInput=InputWave();
		fIn[nIn]=fInput;
		nIn++; nIn%=256;
		fOutput=IIR_RY();
		fOut[nOut]=fOutput;
		nOut++;				/* 請在此句上設置軟件斷點 */
		if ( nOut>=256 )
		{
			nOut=0;		
		}
	}
}

float InputWave()
{
	int i;
	for ( i=2;i>0;i-- )
	{
		fXn1_ry[i]=fXn1_ry[i-1];
		fYn1_ry[i]=fYn1_ry[i-1];
	}
	fXn1_ry[0]=sin(fSignal1)+sin(fSignal2)/3.0;//6.0;
	fYn1_ry[0]=fSum1_ry;
	fSignal1+=fStepSignal1; 
	if ( fSignal1>=f2PI )	fSignal1-=f2PI;
	fSignal2+=fStepSignal2;
	if ( fSignal2>=f2PI )	fSignal2-=f2PI;
	return(fXn1_ry[0]);
}


float IIR_RY()
{
    float fSumTmp=0;
	int i;
	fSum1_ry=0.0;
	fSum2_ry=0.0;
	/*	for ( i=2;i>0;i-- )               //參數移動,模擬人眼環節1
	{
		fXn1_ry[i]=fXn1_ry[i-1];
		fYn1_ry[i]=fYn1_ry[i-1];
	}*/
	fXn1_ry[0]=fXn1_ry[0]*0.0094;
    fYn1_ry[0]=fSum1_ry;
    for ( i=0;i<IIRNUMBER;i++ )        
	{
	   fSum1_ry+=(fXn1_ry[i]*fAn1_ry[i]);
	   fSum1_ry+=(fYn1_ry[i]*fBn1_ry[i]);
	}
		for ( i=2;i>0;i-- )               //參數移動,模擬人眼環節2
	{
		fXn2_ry[i]=fXn2_ry[i-1];
		fYn2_ry[i]=fYn2_ry[i-1];
	}
	fXn2_ry[0]=fSum1_ry;
    fYn2_ry[0]=fSum2_ry;
    for ( i=0;i<IIRNUMBER;i++ )        
	{
	   fSum2_ry+=(fXn2_ry[i]*fAn2_ry[i]);
	   fSum2_ry+=(fYn2_ry[i]*fBn2_ry[i]);
	}
    fSumTmp=fSum2_ry;
	return(fSumTmp);
}