-- LUD_package.ada  LU decomposition 
--                Decompose a matrix A into a Lower triangular matrix
--                times an Upper triangluar matrix  A = L * U
--                where the diagonal of L is all ones,
--                no element on the diagonal of U is zero for nonsingular A.
--                Works for positive definate matricies
--                Or more generally  P * A = L * U where P is a permutation
--                matrix (a permutation of the rows of the Identity matrix)
--                represented as an integer vector of subscripts PV=2,3,1,4
--                means a 1.0 in P at 1,2  2,3  3,1  4,4  (P**t = P**-1)
--                A = P**T * L * U
--
--  Methods:  from Introduction to Algorithms P 759
--
--            Array_Index_Error is raised if matrix not square and also if
--            A, L and U  row and column ranges are not the same.
--
-- basis of algorithm:
--
-- write out L U = A  and perform matrix multiply
--
-- | 1.0    0.0    0.0    0.0    |   | U(1,1) U(1,2) U(1,3) U(1,4) | 
-- | L(2,1) 1.0    0.0    0.0    | * | 0.0    U(2,2) U(2,3) U(2,4) | =
-- | L(3,1) L(3,2) 1.0    0.0    |   | 0.0    0.0    U(3,3) U(3,4) |
-- | L(4,1) L(4,2) L(4,3) 1.0    |   | 0.0    0.0    0.0    U(4,4) |
--
-- | A(1,1) A(1,2) A(1,3) A(1,4) |   | U(1,1)         U(1,2)
-- | A(2,1) A(2,2) A(2,3) A(2,4) | = | L(2,1)*U(1,1)  L(2,1)*U(1,2)+U(2,2)
-- | A(3,1) A(3,2) A(3,3) A(3,4) |   | L(3,1)*U(1,1)  L(3,1)*U(1,2)+L(3,2)*U(2,2)
-- | A(4,1) A(4,2) A(4,3) A(4,4) |   | L(4,1)*U(1,1)  L(4,1)*U(1,2)+L(4,2)*U(2,2)
--
-- U(1,3)
-- L(2,1)*U(1,3)+U(2,3)
-- L(3,1)*U(1,3)+L(3,2)*U(2,3)+U(3,3)
-- L(4,1)*U(1,3)+L(4,2)*U(2,3)+L(4,3)*U(3,3)
--
-- U(1,4)                                           |
-- L(2,1)*U(1,4)+U(2,4)                             |
-- L(3,1)*U(1,4)+L(3,2)*U(2,4)+U(3,4)               |
-- L(4,1)*U(1,4)+L(4,2)*U(2,4)+L(4,3)*U(3,4)+U(4,4) |
--
-- order of computation: (just solving each equation above)
--
-- U(1,1) := A(1,1)-Sum   Sum = 0.0
-- U(1,2) := A(1,2)-Sum   Sum = 0.0
-- U(1,3) := A(1,3)-Sum   Sum = 0.0
-- U(1,4) := A(1,4)-Sum   Sum = 0.0
--
-- L(2,1) := (A(2,1)-Sum)/U(1,1) Sum = 0.0
-- L(3,1) := (A(3,1)-Sum)/U(1,1) Sum = 0.0
-- L(4,1) := (A(4,1)-Sum)/U(1,1) Sum = 0.0
--
-- U(2,2) := A(2,2)-Sum          Sum = L(2,1)*U(1,2)
-- U(2,3) := A(2,3)-Sum          Sum = L(3,1)*U(1,2)
-- U(2,4) := A(2,4)-Sum          Sum = L(4,1)*U(1,2)
--
-- L(3,2) := (A(3,2)-Sum)/U(2,2) Sum = L(3,1)*U(1,2)
-- L(4,2) := (A(4,2)-Sum)/U(2,2) Sum = L(4,1)*U(1,2)
--
-- U(3,3) := A(3,3)-Sum          Sum = L(3,1)*U(1,3)+L(3,2)*U(2,3)
-- U(3,4) := A(3,4)-Sum          Sum = L(3,1)*U(1,4)+L(3,2)*U(2,4)
--
-- L(4,3) := (A(4,3)-Sum)/U(3,3) Sum = L(4,1)*U(1,3)+L(4,2)*U(2,3)
--
-- U(4,4) := A(4,4)-Sum     Sum = L(4,1)*U(1,4)+L(4,2)*U(2,4)+L(4,3)+U(3,4)
--
-- observations: 
--      since L is lower diagonal, the diagonal and upper L need never be used.
--      since U is upper diagonal and diagonal, lower U need never be used.
--      the algorithm can fail if any U(i,i) = 0.0, thus we must use permutation
--
-- programmed 3/5/94 by Jon S. Squire
-- repackaged 9/13/96

with Real_Arrays; use Real_Arrays;
with Integer_Arrays; use Integer_Arrays;

package LUD_Package is

  procedure LUD(A:Real_Matrix;
                L: in out Real_Matrix;
                U: in out Real_Matrix;
                P: in out Integer_Vector);

  function LUD_Solve(L:Real_Matrix;
                     U:Real_Matrix;
                     P:Integer_Vector;
                     Y:Real_Vector) return Real_Vector;

  function LUD_Inverse(L:Real_Matrix;
                       U:Real_Matrix;
                       P:Integer_Vector) return Real_Matrix;

  Array_Index_Error: exception renames Real_Arrays.Array_Index_Error;
  Singular_Error: exception ;

end LUD_Package;

package body LUD_Package is

  procedure LUD(A:Real_Matrix;
                L: in out Real_Matrix;
                U: in out Real_Matrix;
                P: in out Integer_Vector) is
    subtype Real is Float;
    AA:Real_Matrix(A'range(1),A'range(2)) := A;
    Itemp:Integer;
    Kk:Integer;
    Temp:Real;
    Big:Float;
    Sca:Float:=0.0;
  begin
    if A'length(1)/=A'length(2) or A'length(1)/=L'length(1) or
       A'length(1)/=L'length(2) or A'length(1)/=U'length(1) or
       A'length(1)/=U'length(2) or A'length(1)/=P'length or
       A'first(1)/=A'first(2) or  -- remove later by offsetting
       A'first(1)/=L'first(1) or A'first(1)/=L'first(2) or
       A'first(1)/=U'first(1) or A'first(1)/=U'first(2) or A'first(1)/=P'first
    then
      raise Array_Index_Error;
    end if ;
    L := (others =>(others =>0.0));
    U := (others =>(others =>0.0));
    for I in P'range loop
      Sca := Sca + abs A(A'first(1),I); -- could be zero if singular
      P(I) := I;
      L(I,I) := 1.0;
    end loop ;
    for K in A'first(1)..A'last(1)-1 loop
      Big := 0.0;
      for I in K..A'last(1) loop
        if abs AA(I,K)>Big then
          Big := abs AA(I,K);
          Kk := I;
        end if ;
      end loop ;
      if Big < 1.0E-5*Sca then
        raise Singular_Error;
      end if ;
      Itemp := P(K);
      P(K) := P(Kk);
      P(Kk) := Itemp;
      for I in A'range(1) loop
        Temp := AA(K,I);
        AA(K,I) := AA(Kk,I);
        AA(Kk,I) := Temp;
      end loop ;
      for I in K+1..A'last(1) loop
        AA(I,K) := AA(I,K)/AA(K,K);
        for J in K+1..A'last(1) loop
          AA(I,J) := AA(I,J)-AA(I,K)*AA(K,J);
        end loop ;
      end loop ;
    end loop ;
    Big := abs AA(A'Last(1),A'last(2));
    if Big < 1.0E-5*Sca then
      raise Singular_Error;
    end if ;

    for I in A'range(1) loop  -- copy from A to U
      for J in I..A'last(1) loop
        U(I,J) := AA(I,J);
      end loop ;
    end loop ;
    for I in A'first(1)+1..A'last(1) loop  -- copy from A to L
      for J in 1..I-1 loop
        L(I,J) := AA(I,J);
      end loop ;
    end loop ;
  end LUD;

  function LUD_Solve(L:Real_Matrix;
                     U:Real_Matrix;
                     P:Integer_Vector;
                     Y:Real_Vector) return Real_Vector is
    subtype Real is Float;
    X:Real_Vector(Y'range);  -- result of  A = P**T * L * U,  A * x = y
    B:Real_Vector(Y'range);  -- temporary
    Sum:Real;
    Array_Index_Error: exception renames Real_Arrays.Array_Index_Error;
  begin
    if L'length(1)/=L'length(2) or L'length(1)/=U'length(1) or
       L'length(1)/=U'length(2) or L'length(1)/=P'length or
       L'length(1)/=Y'length or L'first(1)/=L'first(2) or
       -- remove later by offsetting
       L'first(1)/=U'first(1) or L'first(1)/=U'first(2) or L'first(1)/=P'first
       or L'first(1)/=Y'first then
      raise Array_Index_Error;
    end if ;
    for I in Y'range loop  --   do i=1,n
      Sum := 0.0;
      for J in Y'first..I-1 loop  --     do j=1,i-1
        Sum := Sum+L(I,J)*B(J);
      end loop ;
      B(I) := Y(P(I))-Sum;
    end loop ;
    for I in reverse Y'range loop  --   do i=n,1,-1
      Sum := 0.0;
      for J in I+1..Y'last loop  --     do j=i+1,n
        Sum := Sum+U(I,J)*X(J);
      end loop ;
      X(I) := (B(I)-Sum)/U(I,I);
    end loop ;
    return X;
  end LUD_Solve;

  function LUD_Inverse(L:Real_Matrix;
                       U:Real_Matrix;
                       P:Integer_Vector) return Real_Matrix is
    subtype Real is Float;
    Y:Real_Vector(L'range(1));  -- unit vectors
    X:Real_Vector(Y'range);  -- column of inverse(A)
    B:Real_Vector(Y'range);  -- temporary
    Ai:Real_Matrix(L'range(1),L'range(2));  -- inverse(A)
    Sum:Real;
    Array_Index_Error: exception renames Real_Arrays.Array_Index_Error;
  begin
    if L'length(1)/=L'length(2) or L'length(1)/=U'length(1) or
       L'length(1)/=U'length(2) or L'length(1)/=P'length or
       L'length(1)/=Y'length or L'first(1)/=L'first(2) or
       -- remove later by offsetting
       L'first(1)/=U'first(1) or L'first(1)/=U'first(2) or L'first(1)/=P'first
       or L'first(1)/=Y'first then
      raise Array_Index_Error;
    end if ;
    for K in L'range(1) loop
      Y := Unit_Vector(K,L'length(1),L'first(1));
      for I in Y'range loop
        Sum := 0.0;
        for J in B'first..I-1 loop
          Sum := Sum+L(I,J)*B(J);
        end loop ;
        B(I) := Y(P(I))-Sum;
      end loop ;
      for I in reverse Y'range loop
        Sum := 0.0;
        for J in I+1..Y'last loop
          Sum := Sum+U(I,J)*X(J);
        end loop ;
        X(I) := (B(I)-Sum)/U(I,I);
      end loop ;
      for I in Y'range loop
        Ai(I,K) := X(I);
      end loop ;
    end loop ;
    return Ai;
  end LUD_Inverse;

end LUD_Package;