#include"stdio.h"
#include"math.h"
#include"stdlib.h"
#include"brmul.c"
//#define B 0.23

int main()
{  
	FILE *fp1,*fp2,*fp3;
   static double u[650],e[650],z[650],x1[650];
  static double x[4][1],xx[1][4],w1[1][4],p[4][4],k[4][1],s[4][1],s1[4][1],ss[4][1];
  static double w1p[1][4],px[4][1],pp[4][4];
  double wpx[1],w1s[1],xxs[1],b,t=0.0,k1=0.0,err=0.0;
    int i,n,j,m;
if((fp1=fopen("M.dat","r"))==NULL)
{
printf("ERROR");
exit(1);
}

if((fp2=fopen("iv.dat","w"))==NULL)
{
printf("ERROR");
exit(1);
}

if((fp3=fopen("gauss.dat","r"))==NULL)
{
printf("ERROR");
exit(1);
}
/*
if((fp4=fopen("z1.dat","w"))==NULL)
{
printf("ERROR");
exit(1);
}
*/
for(i=0;i<650;i++)
{fscanf(fp1,"%lf",&u[i]);
 fscanf(fp3,"%lf",&e[i]);
//e[i]=B*e[i];
}



z[0]=e[0];
z[1]=0.5*e[0]+u[0]+e[1];//z[1]=v[1];

//for(i=0;i<2;i++)
//fprintf(fp4,"%lf\n",z[i]);

for(i=2;i<650;i++)
{z[i]=1.5*z[i-1]-0.7*z[i-2]+u[i-1]+0.5*u[i-2]+e[i]-1.0*e[i-1]+0.2*e[i-2];
//fprintf(fp4,"%lf\n",z[i]);
}
fclose(fp1);
fclose(fp3);
//printf("%lf\n",z[0]);
//fclose(fp4);

w1s[0]=0.0;
wpx[0]=0.0;
xxs[0]=0.0;
ss[0][0]=-1.5;
ss[1][0]=0.7;
ss[2][0]=1.0;
ss[3][0]=0.5;
for(i=0;i<4;i++)
p[i][i]=1.0e+6;
/*for(i=0;i<4;i++)
{for(j=0;j<4;j++)
printf("%lf  ",p[i][j]);
printf("\n");
}*/
w1[0][0]=-z[0];
w1[0][1]=0.0;
w1[0][2]=u[0];
w1[0][3]=0.0;

//f=1.0e-6;
n=0;
/*
printf("w1:\n");
for(i=0;i<4;i++)
{
printf("%lf  ",w1[0][i]);
printf("\n");}
*/


x[0][0]=0.0;
x[1][0]=0.0;
x[2][0]=u[0];
x[3][0]=0.0;
//printf("x\n");
/*
for(i=0;i<4;i++)
printf("%lf  ",x[i][0]);
printf("\n");
*/

for(m=0;m<650;m++)

{ 
   for(i=0;i<4;i++)
	s[i][0]=s1[i][0];
	for(i=0;i<4;i++)
 xx[0][i]=x[i][0];
	/*for(i=0;i<4;i++)
{
printf("%lf  ",w1[0][i]);
}*/
brmul(w1,p,1,4,4,w1p);
//printf("%lf\n",w1p[0][0]);
brmul(w1p,x,1,4,1,wpx);
//printf("%lf\n",wpx[0]);
k1=1.0/(wpx[0]+1.0);
brmul(p,x,4,4,1,px);
//printf("%lf",k1);
for(i=0;i<4;i++)
k[i][0]=px[i][0]*k1;
//printf("%lf\n",k[0][0]);

brmul(w1,s,1,4,1,w1s);
b=z[n+1]-w1s[0];
for(i=0;i<4;i++)
{s1[i][0]=s[i][0]+k[i][0]*b;
//printf("%lf   ",s1[i][0]);
}
//printf("\n");
//printf("\n");


brmul(px,w1p,4,1,4,pp);
for(i=0;i<4;i++)
for(j=0;j<4;j++)
p[i][j]=p[i][j]-pp[i][j]/(1.0+wpx[0]);


brmul(xx,s1,1,4,1,xxs);
x1[n+1]=xxs[0];
//printf("x1:%lf\n",x1[n+1]);

n=n+1;
//getch();


x[0][0]=-x1[n];
x[1][0]=-x1[n-1];
x[2][0]=u[n];
x[3][0]=u[n-1];

w1[0][0]=-z[n];
w1[0][1]=-z[n-1];
w1[0][2]=u[n];
w1[0][3]=u[n-1];

//for(i=0;i<4;i++)
//{c[i]=(s[i][0]-s1[i][0])/s[i][0];
// d[i]=fabs(c[i]);
//printf("##%lf  ",c[i][0]);
//printf("&&%lf  ",d[i]);
//}

/*t=d[0];
if(t<d[1])
t=d[1];
else if(t<d[2])
t=d[2];
else if(t<d[3])
t=d[3];
printf("\n%lf\n",t);
*/
}


for(i=0;i<4;i++)
{printf("%lf  ",s1[i][0]);
err=err+(s1[i][0]-ss[i][0])*(s1[i][0]-ss[i][0]);
}
printf("\n誤差平方和為：%lf",err);
fprintf(fp2,"遞推最小二乘方法參數估計結果：\n");

for(i=0;i<4;i++)
{if(i<2)
fprintf(fp2,"a%d=%lf  ",i+1,s1[i][0]);
else if(i<4)
fprintf(fp2,"b%d=%lf  ",i-1,s1[i][0]);
}
fclose(fp2);

  return 0;
}