%AFQRRLS1 Problem 1.1.1.1.2.10
%
%   'ifile.mat' - input file containing:
%      I - members of ensemble
%      K - iterations
%      sigmax - standard deviation of input
%      Wo - coefficient vector of plant
%      sigman - standard deviation of measurement noise
%      lambda - forgetting factor
%      b - bits in decimal part
% 
%   'ofile.mat' - output file containing:
%      ind - sample indexes 
%      MSE - mean-square error

clear all	% clear memory
%load ifile;	% read input variables
L=length(Wo);		% plant and filter length
L1=L+1;			
L2=L1+1;		% auxiliary constants
N=L-1;			% plant and filter order
MSE=zeros(K,1);		% prepare to accumulate MSE*I

for i=1:I,		% ensemble
  X=zeros(L,1);		% initial memory
  x=randn(K,1)*sigmax;		% input 
  n=randn(K,1)*sigman;		% measurement noise for i=1:I,
  xq2=zeros(L,1);
  dq2=zeros(L,1);
  for l=1:L,
    Qthetafp(:,((1:L1)+(l-1)*L1))=sparse(eye(L1));
    Qtheta(:,((1:L1)+(l-1)*L1))=sparse(eye(L1));
  end
  normef=abs(x(1));
  gammap=1;
  r=zeros(L,1);
  for k=1:(K-1),	% iterations
    AUX=[x(k+1)
         lambda^(1/2)*xq2];
    for l=1:L,
      AUX=Qtheta(:,((1:L1)+(l-1)*L1))*AUX;
      AUX=qround(AUX,b);
    end
    efq1=AUX(1);
    xq2=AUX(2:L1);
    aux=normef;
    normef=qround(sqrt(qround((lambda*aux^2+efq1^2),b)),b);
    sinthetaf=efq1/normef;
    mu=normef;
    for l=1:L,
      aux=mu;
      mu=qround(sqrt(qround((aux^2+(xq2(l))^2),b)),b);
      costhetafp=aux/mu;
      sinthetafp=xq2(l)/mu;
      Qthetafp(l,l+(L-l)*L1)=costhetafp;
      Qthetafp(l,L1+(L-l)*L1)=-sinthetafp;
      Qthetafp(L1,l+(L-l)*L1)=sinthetafp;
      Qthetafp(L1,L1+(L-l)*L1)=costhetafp;
    end
    re=[r
        gammap*sinthetaf];
    for l=L:-1:1,
      re=Qthetafp(:,((1:L1)+(l-1)*L1))*re;
      re=qround(re,b);
    end
    r=re(2:L1);
    gammap=1;
    for l=1:L,
      aux=gammap;
      gammap2=qround((aux^2-(r(L1-l))^2),b);
      if gammap2<0,
        gammap2=0;
      end
      gammap=qround(sqrt(gammap2),b);
      aux=max(aux,2^(-b));
      costheta=gammap/aux;
      sintheta=r(L1-l)/aux;
      Qtheta(1,1+(l-1)*L1)=costheta;
      Qtheta(1,L2-l+(l-1)*L1)=-sintheta;
      Qtheta(L2-l,1+(l-1)*L1)=sintheta;
      Qtheta(L2-l,L2-l+(l-1)*L1)=costheta;
    end
    X=[x(k+1)
       X(1:N)];		% new input vector
    d=Wo'*X+n(k+1);	% noisy desired signal sample
    AUX=[d
         lambda^(1/2)*dq2];
    for l=1:L,
      AUX=Qtheta(:,((1:L1)+(l-1)*L1))*AUX;
      AUX=qround(AUX,b);
    end
    eq1=AUX(1);
    dq2=AUX(2:L1);
    ep=eq1/gammap;	% a priori error sample
    MSE(k+1)=MSE(k+1)+ep^2;	% accumulate MSE*I
  end
end

ind=0:(K-1);		% sample indexes
MSE=MSE/I;		% calculate MSE
save ofile ind MSE;	% write output variables