% SVD6  Laguerre approximation
% Question: finding the MSE in time is the same as finding the
% MSE for the adjustable parameters????
% uses the samples of the segment that collects the charge,
% The "shape" has three signals: 2 induced and one collected charge
% The collected charge was upgrade to go to "zero" in the end with 
% an all pass-filter with minus input with 20 samples delay.  I added 
% 25 zeros samples after the original 50 samples to have
% orthonormal Laguerre functions.

clear all
hold off

load shape10   %has variable "shape"
s=75;
DL=s-3;

[C,R]= size(shape);
Y=shape(:,s+1-DL:s)';
Y=[Y; shape(:,2*s+1-DL:2*s)'];
Y=[Y; shape(:,3*s+1-DL:3*s)'];
[R,C]= size(Y);

%pole for induced charge: .25

T=.1; p = .45;     %find the outputs of the laguerre polynomial
K = sqrt((1-p^2)*T);
n = [0 K];               
d = [1 -1*p];
u = dimpulse(n,d,DL); 
nt = [-p 1];
dt = [1 -p];

%number parameter for induced charge: 19 + 1
Num_Lag_par = 25;   
for i=1:Num_Lag_par-1   % one parameter is found outside this loop
 u = [u dlsim(nt,dt,u(:,i))]; 
end
invutuut=inv(u'*u)*u';

t=zeros(3*Num_Lag_par,C);
y_hat=zeros(R,C);
for i=1:C
  t(1:Num_Lag_par,i)=invutuut*Y(1:DL,i);
  t(Num_Lag_par+1:2*Num_Lag_par,i)=invutuut*Y(DL+1:2*DL,i);
  t(2*Num_Lag_par+1:3*Num_Lag_par,i)=invutuut*Y(2*DL+1:3*DL,i);

  y_hat(1:DL,i)=u*t(1:Num_Lag_par,i);
  y_hat(DL+1:2*DL,i)=u*t(Num_Lag_par+1:2*Num_Lag_par,i);
  y_hat(2*DL+1:3*DL,i)=u*t(2*Num_Lag_par+1:3*Num_Lag_par,i);

  mse(i)=[Y(:,i)-y_hat(:,i)]'*[Y(:,i)-y_hat(:,i)];
%figure(1),plot(1:3*DL,Y(:,i),'r',1:3*DL,y_hat(:,i),'g')
%pause(1)
 end

sum(mse)

disp(1)

%simulate a measured signal
M=2*Y(:,1)+3*Y(:,29)+.05*randn(3*DL,1);

%   find mse for time sequence
mse=1000*ones(C,C);
for i=1:C
 for j=1:C
  if i~=j
   g=inv([Y(:,i)';Y(:,j)']*[Y(:,i),Y(:,j)])*[Y(:,i)';Y(:,j)']*M;  
   mse(i,j)=[M-g(1)*Y(:,i)-g(2)*Y(:,j)]'*[M-g(1)*Y(:,i)-g(2)*Y(:,j)];
%figure(1),plot(1:R,M,'g',1:R,g(1)*Y(:,i)+g(2)*Y(:,j),'r')
%pause(1)
  end
 end
end
disp(2)

%find the Laguerre parameters for the measured signal.
Mt=zeros(3*Num_Lag_par,1);
Mt(1:Num_Lag_par,1)=[invutuut]*M(1:DL,:);
Mt(Num_Lag_par+1:2*Num_Lag_par,1)=[invutuut]*M(DL+1:2*DL,:);
Mt(2*Num_Lag_par+1:3*Num_Lag_par,1)=[invutuut]*M(2*DL+1:3*DL,:);

M_hat=zeros(R,1);
M_hat(1:DL,1)=u*Mt(1:Num_Lag_par,1);
M_hat(DL+1:2*DL,1)=u*Mt(Num_Lag_par+1:2*Num_Lag_par,1);
M_hat(2*DL+1:3*DL,1)=u*Mt(2*Num_Lag_par+1:3*Num_Lag_par,1);
%mse=[M-M_hat]'*[M-M_hat]
%plot(1:DL,M,'r',1:DL,M_hat,'g')

%   find mse for Laguerre parameter sequence
v=[1:3*Num_Lag_par];   % v is used just for the plot
mset=1000*ones(C,C);
for i=1:C
 for j=1:C
  if i~=j
   g=inv([t(:,i)';t(:,j)']*[t(:,i),t(:,j)])*[t(:,i)';t(:,j)']*Mt;  
   mset(i,j)=[Mt-g(1)*t(:,i)-g(2)*t(:,j)]'*[Mt-g(1)*t(:,i)-g(2)*t(:,j)];
%figure(1),plot(v,Mt,'g',v,g(1)*t(:,i)+g(2)*t(:,j),'r')
%pause(1)
  end 
 end
end


[z,w]=min(mse)
[zt,wt]=min(mset)

