Balance matrix, i.e. finds such similarity transformation of the original
    matrix A:  A = T * bA * T^{-1}, where norms of columns of bA and of rows of bA
    are as close as possible. It is usually used as a preprocessing step in
    eigenvalue calculation routine. It is useful also for State-Space models.

    See also:
        [1] Beresford N. Parlett and Christian Reinsch (1969). Balancing
            a matrix for calculation of eigenvalues and eigenvectors.
            Numerische Mathematik, 13(4): 293-304.

    Input:
    ----------------------
    A: square matrix
        Matrix to be balanced

    Output:
    ----------------
        bA: matrix
            Balanced matrix

        T: matrix
            Left part of the similarity transformation

        T_inv: matrix
            Right part of the similarity transformation.

def balance_matrix(A):
    """
    Balance matrix, i.e. finds such similarity transformation of the original
    matrix A:  A = T * bA * T^{-1}, where norms of columns of bA and of rows of bA
    are as close as possible. It is usually used as a preprocessing step in
    eigenvalue calculation routine. It is useful also for State-Space models.

    See also:
        [1] Beresford N. Parlett and Christian Reinsch (1969). Balancing
            a matrix for calculation of eigenvalues and eigenvectors.
            Numerische Mathematik, 13(4): 293-304.

    Input:
    ----------------------
    A: square matrix
        Matrix to be balanced

    Output:
    ----------------
        bA: matrix
            Balanced matrix

        T: matrix
            Left part of the similarity transformation

        T_inv: matrix
            Right part of the similarity transformation.
    """

    if len(A.shape) != 2 or (A.shape[0] != A.shape[1]):
        raise ValueError('balance_matrix: Expecting square matrix')

    N = A.shape[0] # matrix size

    gebal = sp.linalg.lapack.get_lapack_funcs('gebal',(A,))
    bA, lo, hi, pivscale, info = gebal(A, permute=True, scale=True,overwrite_a=False)
    if info < 0:
        raise ValueError('balance_matrix: Illegal value in %d-th argument of internal gebal ' % -info)
    # calculating the similarity transforamtion:
    def perm_matr(D, c1,c2):
        """
        Function creates the permutation matrix which swaps columns c1 and c2.

        Input:
        --------------
        D: int
            Size of the permutation matrix
        c1: int
            Column 1. Numeration starts from 1...D
        c2: int
            Column 2. Numeration starts from 1...D
        """
        i1 = c1-1; i2 = c2-1 # indices
        P = np.eye(D);
        P[i1,i1] = 0.0; P[i2,i2] = 0.0; # nullify diagonal elements
        P[i1,i2] = 1.0; P[i2,i1] = 1.0

        return P

    P = np.eye(N) # permutation matrix
    if (hi != N-1): # there are row permutations
        for k in range(N-1,hi,-1):
            new_perm = perm_matr(N, k+1, pivscale[k])
            P = np.dot(P,new_perm)
    if (lo != 0):
        for k in range(0,lo,1):
            new_perm = perm_matr(N, k+1, pivscale[k])
            P = np.dot(P,new_perm)
    D = pivscale.copy()
    D[0:lo] = 1.0; D[hi+1:N] = 1.0 # thesee scaling factors must be set to one.
    #D = np.diag(D) # make a diagonal matrix

    T = np.dot(P,np.diag(D)) # similarity transformation in question
    T_inv = np.dot(np.diag(D**(-1)),P.T)

    #print( np.max(A - np.dot(T, np.dot(bA, T_inv) )) )
    return bA.copy(), T, T_inv