% SVD5d  Laguerre approximation
% For two interactions we will find the best fit.
% 

close all;
clear all;
PLOTGRAPHS = 0;

points  = [[4 5 15 26 33 49 51 64 76 2]'[99 82 77 60 54 49 31 25 19 9]']
k1p = [0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 0.35];
k2p = [0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0.65];
p = [];
p2 = [];
for z=1:10,
    z   
	%read data from database
	k1 = k1p(z);
	k2 = k2p(z);
	[Resp1,RPos1] = LeDadoIt(points(z,1));
	[Resp2,RPos2] = LeDadoIt(points(z,2));
	
	Resp = k1*Resp1 + k2*Resp2;
	
	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);
	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:100,
        m
        %read data from database
        [Resp,RPos] = LeDadoIt(m);
        %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:100,
            if n~m;
                
                %read data from database
                [Resp,RPos] = LeDadoIt(n);
                %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)];
                
                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)];
                
                
                gt2=inv([gm';gn']*[gm,gn])*[gm';gn']*gs;
                mset2(m,n)=[gs-gt2(1)*gm-gt2(2)*gn]'*[gs-gt2(1)*gm-gt2(2)*gn];
                mset2p(z,m,n)=mset2(m,n);
                glaguere(z,m,n,1) = gt2(1);
                glaguere(z,m,n,2) = gt2(2);
                %mselaguerre(m) = sum(diag((Gains-Gainsnoisy)*(Gains-Gainsnoisy)'));
            end
        end
	end
	
	
	for j=1:4
        %find minumum on both matrixes
	%    [x, y] = find(mset<=min(min(mset)))
        [x2, y2] = find(mset2<=min(min(mset2)))
        
	%    mset(x,y) = 100;
        mset2(x2,y2) = 100;
        interaction(z,j,1) = x2(1,1);
        interaction(z,j,2) = y2(1,1);
        
        %Do it for the segment that collected charge
     %   [Resp,RPos] = LeDadoIt(x);
     %   p = [p;RPos];
     %   [Resp,RPos] = LeDadoIt(y);
     %   p = [p;RPos];
        %Do it for all segments
        [Resp,RPosx2] = LeDadoIt(x2);
        p2 = [p2;RPosx2];
        [Resp,RPosy2] = LeDadoIt(y2);
        p2 = [p2;RPosy2];
        d2(z,j,1) = min((RPos1-RPosy2)'*(RPos1-RPosy2),(RPos1-RPosx2)'*(RPos1-RPosx2));
        d2(z,j,2) = min((RPos2-RPosy2)'*(RPos2-RPosy2),(RPos2-RPosx2)'*(RPos2-RPosx2));
	end
end
	%[p p2]
	p2
	d2
	%figure, plot(d2);
	
	%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)
    for i=1:10
        [interaction(i,1,1) interaction(i,1,2) glaguere(i,interaction(i,1,1),interaction(i,1,2),1) glaguere(i,interaction(i,1,1),interaction(i,1,2),2)]
    end