#include "stdlib.h"
  #include "math.h"
  #include "stdio.h"
  void brmul(a,b,m,n,k,c)
  int m,n,k;
  double a[],b[],c[];
  { int i,j,l,u;
    for (i=0; i<=m-1; i++)
    for (j=0; j<=k-1; j++)
      { u=i*k+j; c[u]=0.0;
        for (l=0; l<=n-1; l++)
          c[u]=c[u]+a[i*n+l]*b[l*k+j];
      }
  }
  int brinv(double a[],int n)
  { int *is,*js,i,j,k,l,u,v;
    double d,p;
    is=malloc(n*sizeof(int));
    js=malloc(n*sizeof(int));
    for (k=0; k<=n-1; k++)
      { d=0.0;
        for (i=k; i<=n-1; i++)
        for (j=k; j<=n-1; j++)
          { l=i*n+j; p=fabs(a[l]);
            if (p>d) { d=p; is[k]=i; js[k]=j;}
          }
        if (d+1.0==1.0)
          { free(is); free(js); printf("err**not inv\n");
            return(0);
          }
        if (is[k]!=k)
          for (j=0; j<=n-1; j++)
            { u=k*n+j; v=is[k]*n+j;
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
        if (js[k]!=k)
          for (i=0; i<=n-1; i++)
            { u=i*n+k; v=i*n+js[k];
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
        l=k*n+k;
        a[l]=1.0/a[l];
        for (j=0; j<=n-1; j++)
          if (j!=k)
            { u=k*n+j; a[u]=a[u]*a[l];}
        for (i=0; i<=n-1; i++)
          if (i!=k)
            for (j=0; j<=n-1; j++)
              if (j!=k)
                { u=i*n+j;
                  a[u]=a[u]-a[i*n+k]*a[k*n+j];
                }
        for (i=0; i<=n-1; i++)
          if (i!=k)
            { u=i*n+k; a[u]=-a[u]*a[l];}
      }
    for (k=n-1; k>=0; k--)
      { if (js[k]!=k)
          for (j=0; j<=n-1; j++)
            { u=k*n+j; v=js[k]*n+j;
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
        if (is[k]!=k)
          for (i=0; i<=n-1; i++)
            { u=i*n+k; v=i*n+is[k];
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
      }
    free(is); free(js);
    return(1);
  }

void main()
{

	double z[607],zz[607][1],w[4][1],ww[1][4],p[4][4],I[4][4],k[4][1],kk[4][1],ss[4][1],pp[4][4],X[4][1],XX[1][4],x1[807];
	double s[4][1],a[4][1],b[1],c[1][1],f[4][4],g[4][4],x2[1][1];//,ee[1][1]
    double u1,v1,d,q;
	int i,j,N;
	double u[607],e[607],v[607];

    FILE *fp1,*fp2,*fp3,*fp;

	if((fp1=fopen("m.txt","r"))==NULL)
    {
      printf("ERROR");exit(1);
    }
    if((fp2=fopen("wnoise.txt","r"))==NULL)
    {
     printf("ERROR");exit(1);
    }
    if((fp3=fopen("IV.txt","w"))==NULL)
    {
     printf("ERROR");exit(1);
    }
	if((fp=fopen("result.txt","w"))==NULL)
    {
     printf("ERROR");exit(1);
    }
    for(i=0;i<607;i++)
    {
     fscanf(fp1,"%lf ",&u1);
     u[i]=u1;
     fscanf(fp2,"%lf ",&v1);
     e[i]=v1;
    }

    e[-1]=e[-2]=0.0;
	for(i=0;i<607;i++)
		v[i]=e[i]-1.0*e[i-1]+0.2*e[i-2];  //產生有色噪聲

	z[0]=v[0];
	z[1]=1.5*z[0]+u[0]+v[1]; 
	for(i=2;i<607;i++)
	z[i]=1.5*z[i-1]-0.7*z[i-2]+u[i-1]+0.5*u[i-2]+v[i];


	for(i=0;i<607;i++)
	{
		zz[i][0]=z[i];
	
	}
	for(i=0;i<4;i++)
	{
		s[i][0]=0.0;
	for(j=0;j<4;j++)
	{
		if(i==j)
		{
		p[i][j]=1000000;
		I[i][j]=1.0;
		}
		else 
		{
			p[i][j]=0.0;
		    I[i][j]=0.0;
		}

	}
	}
     for(i=0;i<6;i++)   x1[i]=0.0;
	//開始遞推
    fprintf(fp3,"N     a1         a2          b1         b2        \n");    
	for(N=0;N<600;N++)
	{
		for(j=0;j<4;j++)
		{
			if (j<2) w[j][0]=-z[N+5-j];
	       	else if(j<4)  w[j][0]=u[N+7-j];
		}

	for(i=0;i<4;i++)
			ww[0][i]=w[i][0];  //轉置
	for(j=0;j<4;j++)
			{
				if (j<2) X[j][0]=-x1[N+5-j];
	       		else if(j<4)  X[j][0]=u[N+7-j];
			}

	for(i=0;i<4;i++)
		XX[0][i]=X[i][0];  //轉置

	//開始求k
	brmul(p,X,4,4,1,a);
    brmul(ww,a,1,4,1,b);
	for(i=0;i<4;i++)
		k[i][0]=a[i][0]/(1+b[0]);
	//求s
	brmul(ww,s,1,4,1,c);
	 d=zz[N+6][0]-c[0][0];
	 for(i=0;i<4;i++)
		 kk[i][0]=k[i][0]*d;
   fprintf(fp3,"N:%d",N);
	 for(i=0;i<4;i++)
	 {
		 ss[i][0]=s[i][0];
		 s[i][0]=ss[i][0]+kk[i][0];
		 fprintf(fp3," %lf  ",s[i][0]);
	    if(i==3)
	    fprintf(fp3,"\n");
		 q=fabs((ss[i][0]-s[i][0])/ss[i][0]);
	     if (q<0.0000000001&&q>0) break;
	}
	  //遞推p
    brmul(k,ww,4,1,4,f);


	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
		{
			pp[i][j]=I[i][j]-f[i][j];
		}
    brmul(pp,p,4,4,4,g);
	for(i=0;i<4;i++)
			for(j=0;j<4;j++)
				p[i][j]=g[i][j];
   //遞推X
    brmul(XX,s,1,4,1,x2);
	x1[N+6]=x2[0][0];

	}


	for(i=0;i<4;i++)
	{
		printf("%f\n",s[i][0]);
		fprintf(fp,"%lf\n",s[i][0]);
	}
	fclose(fp1);
	fclose(fp2);
	fclose(fp3);
	fclose(fp);
}