function [x_sol,rel,matrix_size] = plu(A,b,threshold,d)
%%
% efficient Parker's algorithm for solving dense linera systems
%
% Based on paper  Mixed accelerated techniques for solving dense linear systems,A.S.Papadakis,E.Gallopoulos
%
% By: A.S.Papadakis 
% email: papadakis@ceid.upatras.gr
%
% INPUT:
%   A:         NxN square matrix
%   b:         Nx1 right hand vector
%   threshold: 
%              Parker's algorithm will be applied only if 
%           1. the position of the current pivot is multiple 
%              of threshold (virtual bad pivot).
%           2. the current pivot is under the threshold (uncomment line 43, comment line 42)
%   d:         depth of butterflies,(d={1,2} is only supported)
% 
% OUTPUT:
%   x_sol: solution of Ax=b
%   rel: relative error Ax_sol-b
%   matrix_size: size of matrix in which parker is applied
%
%  Copyright (C) 2012 A.S.Papadakis
% 
%  This program is free software; you can redistribute it and/or
%  modify it under the terms of the GNU General Public License
%  as published by the Free Software Foundation; either version 2
%  of the License, or (at your option) any later version.
% 
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
% 
L = eye(length(A));
U = A;
v=0;
matrix_size=0;
for (j = 1:length(A)-1)
    if(j==length(A)+1-threshold)
    %if (abs(U(j,j))<=threshold)
        matrix_size=length(A(j:end,j:end));
        parker(j,L,U)
        v=1;
        break;
    end
    L((j+1):end,j) = U((j+1):end,j)/U(j,j);
    U((j+1):end,j:end) = U((j+1):end,j:end) - L((j+1):end,j)*U(j,j:end);
end


if(v~=1)
   y=L\b;
   x=U\y;
   rel=norm(A*x-b);
   x_sol=x;
   k_pivot=0;
   return;
end

function out=parker(k_pivot,L,U)
    % to komati tou A pou den exei paragontopoih8ei einai
    A22 = U(k_pivot:end,k_pivot:end);
    U11 = U(1:k_pivot-1,1:k_pivot-1);
    U12 = U(1:k_pivot-1,k_pivot:end);
    L11=L(1:k_pivot-1,1:k_pivot-1);
    L22=L(k_pivot:end,k_pivot:end);
    L21=L(k_pivot:end,1:k_pivot-1);
    b2=b(k_pivot:end);
    b1=b(1:k_pivot-1);
    
    % upologismos butterflies
    n=length(A22)
    [G,A22] = makeButterfly(n,d,A22);
    H = makeButterfly(length(A22),d);
    if (G==0)
        return;
    end

    % bima 1
    %B22=G'*A22*H;
    B22=multi(G,A22,H);

    % bima 2
    %y=L\b;
    y1=L11\b1;
    y2=b2-L21*y1;
    y=[y1;y2];
    
    y1=y(1:k_pivot-1);
    y2=y(k_pivot:end);

    % bima 3
    if (length(A22)~=n)
        y2=[y2;zeros(length(A22)-n,1)];
    end

    L=eye(length(B22));
    U=B22;
    % z = B22\(G'*y2);
    
    
    for (j = 1:length(B22)-1)
        L((j+1):end,j) = U((j+1):end,j)/U(j,j);
        U((j+1):end,j:end) = U((j+1):end,j:end) - L((j+1):end,j)*U(j,j:end);
        %ploti(j)=U(j,j);
    end
    
    G=comp(G);
    H=comp(H);
    
    yy=L\G'*y2;
    z=U\yy;
    %semilogy(1:length(B22)-1,abs(ploti))

    % bima 4
    x2 = H*z;
    if (length(A22)~=n)
        x2=x2(1:n);
    end

    x_comp=x2;

    if (k_pivot>1)
        % bima 5
        x1 = U11\(y1 - U12*x2);

        % h lush einai
        x_comp = [x1;x2];
    end

    rel=norm(A*x_comp-b);
    x_sol=x_comp;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function B=comp(S)
    if(d==1)
       B11=diag(S(1:length(S)/2));
       B12=diag(S(length(S)/2+1:end));
       B21=B11;
       B22=-B12;
       B=1/sqrt(2)*[B11,B12;B21,B22];
    elseif(d==2)
        d=1;
        B0=comp(S(1:end,2));
        B1=comp(S(1:length(S)/2,1));
        B2=comp(S(length(S)/2+1:end,1));
        W=[B1,zeros(length(B1));zeros(length(B1)),B2];
        B=W*B0;
        d=2;
        
    else
        error('support only for depth=1,2');
    end
        
        
end


function B=multi(G,A,H)
    % input
    % G,H resourcive butterflies of compacted form
    % A matrix
    
    % output
    % B=G'AH
    z=length(A);
    
    if (d==1)
        B=kernel(G,H,A);
    elseif (d==2)
        A11=A(1:z/2,1:z/2);
        A12=A(1:z/2,z/2+1:end);
        A21=A(z/2+1:end,1:z/2);
        A22=A(z/2+1:end,z/2+1:end);
        
        g=G(1:z/2,1);
        h=H(1:z/2,1);
        K11=kernel(g,h,A11);
       
        g=G(1:z/2,1);
        h=H(z/2+1:end,1);
        K12=kernel(g,h,A12);
        
        g=G(z/2+1:end,1);
        h=H(1:z/2,1);
        K21=kernel(g,h,A21);
        
        g=G(z/2+1:end,1);
        h=H(z/2+1:end,1);
        K22=kernel(g,h,A22);
        
        K=[K11,K12;K21,K22];
        
        g=G(1:end,2);
        h=H(1:end,2);
        B=kernel(g,h,K);
    else
        error('Support only for depth=1,2');
    end
end

function B=kernel(g,h,A)
    z=length(A);
    A11=A(1:z/2,1:z/2);
    A12=A(1:z/2,z/2+1:end);
    A21=A(z/2+1:end,1:z/2);
    A22=A(z/2+1:end,z/2+1:end);
    
    C=[A11+A12+A21+A22,A11-A12+A21-A22;A11+A12-A21-A22,A11-A12-A21+A22];
    
    
    for i=1:z
        for j=1:z
            B(i,j)=1/2*g(i)*C(i,j)*h(j);
        end
    end
 %B=1/2*diag(g)*C*diag(h);

end



function [B,A]=makeButterfly(n,d,A)
    % B resourcive butterfly matrix
    % A augmented matrix with 1s
    % n size of A
    % d depth of B

    if (n<=2^(d-1))
       fprintf('depth of resursive matrix must be less than %d\n',log2(n)+1);
       B=0;
       A=0;
       return;
    end
    if (nargin == 3)
        % if n is not multiple of 2^(d) then we augmenting matrix A with additional 1's on the diagonal
        if(mod(n,2^(d))~=0)
            x= 2^(d) - mod(n,2^(d));
            A11=A;
            A22=diag(ones(x,1));
            A12=zeros(n,x);
            A21=zeros(x,n);

            A=[A11,A12;A21,A22];
            n=length(A);
        end
    end

    B=(1/sqrt(2))*exp((-1/2 + (1/2+1/2).*rand(n,d))/10);
end


end

