% SVD5d  Laguerre approximation
% For two interactions we will find the best fit.
% Algortithm for time and Laguerre 

close all;
clear all;
PLOTGRAPHS = 0;

%read data from database
k1 = 0.75;
k2 = 0.25;
[Resp1,RPos1] = LeDadoIt(27);
[Resp2,RPos2] = LeDadoIt(15);

Resp = k1*Resp1 + k2*Resp2;
%super vector for time only
Resps = [Resp(:,1);Resp(:,2);Resp(:,3);Resp(:,4);Resp(:,5);Resp(:,6);Resp(:,7);Resp(:,8);Resp(:,9);Resp(:,10);Resp(:,11);Resp(:,12);Resp(:,13);Resp(:,14);Resp(:,15);Resp(:,16);Resp(:,17);Resp(:,18)];

DL = 47;
[a, b] = size(Resp');
%shapper for the signal that collects charge
Y=ones(a-4,1)-Resp(8,a+2-DL:a)';
Y=[zeros(4,1);Y];

%replace the contact that colected charge with the signal after going to the shapper
Resp(8,:)=Y';
Resp(:,1:DL)=Resp(:,a-DL+1:a);

%creates noise and add it to the input signal
noise = 1 * 0.00166 .* randn(size(Resp));
Respnoisy = Resp + noise;

%without noise
[Gains, yhat] = lagdecomp(Resp);
%with noise
[Gainsnoisy, yhatnoisy] = lagdecomp(Respnoisy);

%creates the super vector
gs = [Gainsnoisy(:,1);Gainsnoisy(:,2);Gainsnoisy(:,3);Gainsnoisy(:,4);Gainsnoisy(:,5);Gainsnoisy(:,6);Gainsnoisy(:,7);Gainsnoisy(:,8);Gainsnoisy(:,9);Gainsnoisy(:,10);Gainsnoisy(:,11);Gainsnoisy(:,12);Gainsnoisy(:,13);Gainsnoisy(:,14);Gainsnoisy(:,15);Gainsnoisy(:,16);Gainsnoisy(:,17);Gainsnoisy(:,18)];


% plots signal with and w/o noise
if PLOTGRAPHS == 1
    for p=1:b
        %without noise
        figure, plot(Resp(p,:))'
        hold;
        plot(yhat(p,:),'r');
        titletext = sprintf('signal %d of %d', p, b);
        title(titletext);
        %witnnoise
        figure, plot(Respnoisy(p,:))'
        hold;
        plot(yhatnoisy(p,:),'g');
        titletext = sprintf('signal with noise %d of %d', p, b);
        title(titletext);
    end
end

%calculates the mse for both case on time
mse = diag((Resp-yhat)*(Resp-yhat)');
msewnoise = diag((Respnoisy-yhatnoisy)*(Respnoisy-yhatnoisy)');


for m=1:40,
    m
    %read data from database
    [Resp,RPos] = LeDadoIt(m);
    %super vector for time only
    Respm = [Resp(:,1);Resp(:,2);Resp(:,3);Resp(:,4);Resp(:,5);Resp(:,6);Resp(:,7);Resp(:,8);Resp(:,9);Resp(:,10);Resp(:,11);Resp(:,12);Resp(:,13);Resp(:,14);Resp(:,15);Resp(:,16);Resp(:,17);Resp(:,18)];
    %shapper for the signal that collects charge
    Y=ones(a-4,1)-Resp(8,a+2-DL:a)';
    Y=[zeros(4,1);Y];
		
    %replace the contact that colected charge with the signal after going to the shapper
    Resp(8,:)=Y';
    Resp(:,1:DL)=Resp(:,a-DL+1:a);
		
    
    
    %without noise
    [Gains_m, yhat] = lagdecomp(Resp);
    gm = [Gains_m(:,1);Gains_m(:,2);Gains_m(:,3);Gains_m(:,4);Gains_m(:,5);Gains_m(:,6);Gains_m(:,7);Gains_m(:,8);Gains_m(:,9);Gains_m(:,10);Gains_m(:,11);Gains_m(:,12);Gains_m(:,13);Gains_m(:,14);Gains_m(:,15);Gains_m(:,16);Gains_m(:,17);Gains_m(:,18)];
    
    for n=1:40,
        if n~m;
            
            %read data from database
            [Resp,RPos] = LeDadoIt(n);
            %super vector for time only
            Respn = [Resp(:,1);Resp(:,2);Resp(:,3);Resp(:,4);Resp(:,5);Resp(:,6);Resp(:,7);Resp(:,8);Resp(:,9);Resp(:,10);Resp(:,11);Resp(:,12);Resp(:,13);Resp(:,14);Resp(:,15);Resp(:,16);Resp(:,17);Resp(:,18)];
            %shapper for the signal that collects charge
            Y=ones(a-4,1)-Resp(8,a+2-DL:a)';
            Y=[zeros(4,1);Y];
				
            %replace the contact that colected charge with the signal after going to the shapper
            Resp(8,:)=Y';
            Resp(:,1:DL)=Resp(:,a-DL+1:a);		
            
            %without noise
            [Gains_n, yhat] = lagdecomp(Resp);
                       
            %gt=inv([Gains_m(:,8)';Gains_n(:,8)']*[Gains_m(:,8),Gains_n(:,8)])*[Gains_m(:,8)';Gains_n(:,8)']*Gainsnoisy(:,8);
            %mset(m,n)=[Gainsnoisy(:,8)-gt(1)*Gains_m(:,8)-gt(2)*Gains_n(:,8)]'*[Gainsnoisy(:,8)-gt(1)*Gains_m(:,8)-gt(2)*Gains_n(:,8)];
            
            %super vector for laguerre
            gn = [Gains_n(:,1);Gains_n(:,2);Gains_n(:,3);Gains_n(:,4);Gains_n(:,5);Gains_n(:,6);Gains_n(:,7);Gains_n(:,8);Gains_n(:,9);Gains_n(:,10);Gains_n(:,11);Gains_n(:,12);Gains_n(:,13);Gains_n(:,14);Gains_n(:,15);Gains_n(:,16);Gains_n(:,17);Gains_n(:,18)];
                        
            %gain for laguerre
            gt2=inv([gm';gn']*[gm,gn])*[gm';gn']*gs;
            %gains for time only
            gtime=inv([Respm';Respn']*[Respm,Respn])*[Respm';Respn']*Resps;

            %mse for laguerre
            mset2(m,n)=[Resps-gt2(1)*Respm-gt2(2)*Respn]'*[Resps-gt2(1)*Respm-gt2(2)*Respn];
            %mse for time
            msetime(m,n)=[gs-gtime(1)*gm-gtime(2)*gn]'*[gs-gtime(1)*gm-gtime(2)*gn];
            %save gains for laguerre
            glaguere(m,n,1) = gt2(1);
            glaguere(m,n,2) = gt2(2);
            %save gains for laguerre
            gaintime(m,n,1) = gtime(1);
            gaintime(m,n,2) = gtime(2);
            %mselaguerre(m) = sum(diag((Gains-Gainsnoisy)*(Gains-Gainsnoisy)'));
        end
    end
end

%Saves the original table of mse to allow me to retrieve those values
mset2o    = mset2;
msetimeo  = msetime;

p = [];
p2 = [];
for j=1:40
    %find minumum on both matrixes
    [x2, y2] = find(mset2<=min(min(mset2)))
    
    %removes point from the list by making it very large
    mset2(x2,y2) = 100;
    
    %Finds the points that were found as best fit according to the algorithm
    [Resp,RPosx2] = LeDadoIt(x2);
    p2 = [p2;RPosx2];
    [Resp,RPosy2] = LeDadoIt(y2);
    p2 = [p2;RPosy2];
    %distance from the test point
    d2(j,1) = min((RPos1-RPosy2)'*(RPos1-RPosy2),(RPos1-RPosx2)'*(RPos1-RPosx2));
    d2(j,2) = min((RPos2-RPosy2)'*(RPos2-RPosy2),(RPos2-RPosx2)'*(RPos2-RPosx2));
end
%[p p2]
p2
d2
figure, plot(d2(:,1),'k-x');
hold;
plot(d2(:,2),'k-o');


%for time only
ptime = [];
ptime2 = [];
for j=1:40
    %find minumum on both matrixes
    [xtime2, ytime2] = find(msetime<=min(min(msetime)))
    
    %removes point from the list by making it very large
    msetime(xtime2,ytime2) = 100;
    
    %Finds the points that were found as best fit according to the algorithm
    [Resp,RPosx2] = LeDadoIt(xtime2);
    ptime2 = [p2;RPosx2];
    [Resp,RPosy2] = LeDadoIt(ytime2);
    ptime2 = [p2;RPosy2];
    %distance from the test point
    dtime2(j,1) = min((RPos1-RPosy2)'*(RPos1-RPosy2),(RPos1-RPosx2)'*(RPos1-RPosx2));
    dtime2(j,2) = min((RPos2-RPosy2)'*(RPos2-RPosy2),(RPos2-RPosx2)'*(RPos2-RPosx2));
end
%[p p2]
ptime2
dtime2
figure, plot(dtime2(:,1),'k-x');
hold;
plot(dtime2(:,2),'k-o');


%sorte mse for all elements
%sortedmselaguerre = sort(mselaguerre);
%figure, plot(sortedmselaguerre(1:5));
%sortedmselaguerre(1:5)

%find minimum error and where it happend
%noisemin = min(mselaguerre)
%find(mselaguerre<=noisemin)
