/* trisolve.h  -  triangular solver */ // -*- c++ -*-
/***************************************************************************
    begin                : 2006-12-11
    copyright            : (C) 2006 by Gunter Winkler
    email                : guwi17@gmx.de
***************************************************************************/

// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

namespace boost { namespace numeric { namespace ublas {

  /*********************************
   * PART 0: general documentation *
   *********************************/

  /** \brief solve lower triangular system Lx=b or (I+L)x=b.  
   *  - \param unit controls whether L is strict lower triangular 
        (\c true) or lower triangular (\c false)
      - \param MAT is the type of the matrix L
      - \param VEC is the type of the vector x

      On input \c x contains the right hand side, on output it
      contains the solution.

      The function returns \c true if the matrix is
      singular. Otherwise it returns \c false.

      In order to have an efficient implementation, the functions make
      no check whether L is really (strict) lower triangular. Thus the
      result is undefined if this requirement is not met. 

      You can force the necessary structure by using the \c triangular_adaptor
      with \c lower or \c strict_lower.

      \note The solution of (triangular) systems of equations is
      numerically unstable. You should expect large absolute errors if
      the matrix has a large condition number. The main reason is that
      small errors introduced in the first computed components of the
      solution add up to large errors in the final components if a)
      the matrix has many rows/columns and b) the sum of the
      off-diagonal entries of a row is larger than the value at the
      diagonal. You will find a detailed error analysis in any good
      text book about numerics, see, e.g., Golub/van Loan, "Matrix
      Computations."
  */
  template <bool unit, class MAT, class VEC>
  bool inplace_solve_lower(const matrix_expression<MAT>& L, vector_expression<VEC>& x);

  /** \brief solve lower triangular system LX=B or (I+L)X=B.  
   *  - \param unit controls whether L is expected to be strict lower
        triangular (true) or lower triangular (false)
      - \param MAT is the type of the matrix L
      - \param MATX is the type of the rhs-matrix X

      On input \c X contains the right hand sides, on output it
      contains the solutions.

      \sa inplace_solve_lower(const matrix_expression<MAT>& L, vector_expression<VEC>& x)
  */
  template <bool unit, class MAT, class MATX>
  bool inplace_solve_lower(const matrix_expression<MAT>& L, matrix_expression<MATX>& X);

  /** \brief solve upper triangular system Ux=b or (I+U)x=b.  
   *  - \param unit controls whether U is expected to be strict upper
        triangular (true) or upper triangular (false)
      - \param MAT is the type of the matrix U
      - \param VEC is the type of the vector x

      On input \c x contains the right hand side, on output it
      contains the solution.

      The function returns \c true if the matrix is
      singular. Otherwise it returns \c false.

      In order to have an efficient implementation, the functions make
      no check whether L is really (strict) upper triangular. Thus the
      result is undefined if this requirement is not met.  

      You can force the necessary structure by using the \c triangular_adaptor
      with \c upper or \c strict_upper.
  */
  template <bool unit, class MAT, class VEC>
  bool inplace_solve_upper(const matrix_expression<MAT>& U, vector_expression<VEC>& x);

  /** \brief solve upper triangular system UX=B or (I+U)X=B.  
   *  - \param unit controls whether U is expected to be strict upper
        triangular (true) or upper triangular (false)
      - \param MAT is the type of the matrix U
      - \param MATX is the type of the rhs-matrix X

      On input \c X contains the right hand sides, on output it
      contains the solutions.

      \sa inplace_solve_upper(const matrix_expression<MAT>& U, vector_expression<VEC>& x)
  */
  template <bool unit, class MAT, class MATX>
  bool inplace_solve_upper(const matrix_expression<MAT>& U, matrix_expression<MATX>& X);

  /*********************************************
   * PART 1: optimized lower and upper solvers *
   *********************************************/

  /* if (unit) assume L is strict lower triangular
   * if (!unit) assume L is lower triangular and invertible
   */
  template < bool unit, class T, class Z, class D, size_t IB, class IA, class TA, class VEC >
  bool
  inplace_solve_lower(const compressed_matrix<T, basic_row_major<Z,D>, IB, IA, TA>& L, vector_expression<VEC>& ve)
  {
    if ( (!unit) && L.filled1() <= L.size1() ) return true;
    typedef typename IA::value_type size_type;
    size_type imax = L.filled1()-1;
    for (size_type i = 0; i < imax; ++i ) {
      size_type iptr_a = L.index1_data()[i];
      size_type iptr_b = L.index1_data()[i+1];
      if (!unit) { -- iptr_b; }
      T value = ve()(i);
      for (size_type k = iptr_a; k < iptr_b; ++k) {
        BOOST_UBLAS_CHECK (L.index2_data()[k] <= i, bad_index ());
        value -= L.value_data()[k] * ve()( L.index2_data()[k] );
      }
      if (unit) { 
        ve()(i) = value; 
      } else {
        ve()(i) = value / L.value_data()[iptr_b];
      }
    }
    return false;
  }

  /* if (unit) assume U is strict upper triangular
   * if (!unit) assume U is upper triangular and invertible
   */
  template < bool unit, class T, class Z, class D, size_t IB, class IA, class TA, class VEC >
  bool
  inplace_solve_upper(const compressed_matrix<T, basic_row_major<Z,D>, IB, IA, TA>& U, vector_expression<VEC>& ve)
  {
    if ( (!unit) && U.filled1() <= U.size1() ) return true;
    typedef typename IA::value_type size_type;
    size_type imax = U.filled1()-1;
    for (size_type i = imax; i > 0; --i ) {
      size_type iptr_a = U.index1_data()[i-1];
      size_type iptr_b = U.index1_data()[i];
      if (!unit) { ++ iptr_a; }
      T value = ve()(i-1);
      for (size_type k = iptr_a; k < iptr_b; ++k) {
        BOOST_UBLAS_CHECK (U.index2_data()[k] >= (i-1), bad_index ());
        value -= U.value_data()[k] * ve()( U.index2_data()[k] );
      }
      if (unit) { 
        ve()(i-1) = value; 
      } else {
        ve()(i-1) = value / U.value_data()[iptr_a-1];
      }
    }
    return false;
  }

  /*******************************************
   * PART 2: general lower and upper solvers *
   *******************************************/

  /* only the lower left or upper right triangle of A is accessed, 
   * the number of rows of A determines the (used) size of x,
   *
   * syntax 1:
   *   inplace_solve_lower<unit>(A,x)
   *   inplace_solve_upper<unit>(A,x)
   *
   * syntax 2:
   *   inplace_solve_lower<unit>(A,x,ORI)
   *   inplace_solve_upper<unit>(A,x,ORI)
   * where ORI is row_major_tag or column_major_tag
   * (here you can override the default choice of the orientation of the algorithm)
   */

  /** \brief Solve (column-by-column) Lx=b or (I+L)x=b where L is the (strict) lower triangle of A.
    
      This version uses the generic algorithm for dense matrices and
      thus only accesses the (strict) lower triangle of A. The result
      is even correct when the matrix A has non-zero value (at or)
      above the diagonal.
  */
  template <bool unit, class MAT, class VEC>
  bool inplace_solve_lower(const matrix_expression<MAT>& A, vector_expression<VEC>& x, column_major_tag)
  {
    typedef typename VEC::size_type size_type;
    typedef typename VEC::value_type value_type;

    BOOST_UBLAS_CHECK (A ().size2 () >= x ().size (), bad_size ());
    BOOST_UBLAS_CHECK (A ().size1 () <= x ().size (), bad_size ());
    
    size_type n = A().size1();
    if (n==0) return false;
   
    for (size_type j = 0; j < (n-1); ++j) {
      double t;
      if (!unit) {
        if (A()(j,j)==0.0) return true;
        t = ( x()(j) /= A()(j,j) );
      } else {
        t = x()(j);
      }
      if (t != value_type()) {
        noalias( project(x(), range(j+1,n)) ) -= t*project( column(A(), j), range(j+1,n) );
      }
    }
    if (!unit) {
        if (A()(n-1,n-1)==0.0) return true;
        x()(n-1) /= A()(n-1,n-1);
    }
    return false;
  }

  /** \brief Solve (row-by-row) Lx=b or (I+L)x=b where L is the (strict) lower triangle of A.
    
      This version uses the generic algorithm for dense matrices and
      thus only accesses the (strict) lower triangle of A. The result
      is even correct when the matrix A has non-zero value (at or)
      above the diagonal.
  */
  template <bool unit, class MAT, class VEC>
  bool inplace_solve_lower(const matrix_expression<MAT>& A, vector_expression<VEC>& x, row_major_tag)
  {
    typedef typename VEC::size_type size_type;
    typedef typename VEC::value_type value_type;

    BOOST_UBLAS_CHECK (A ().size2 () >= x ().size (), bad_size ());
    BOOST_UBLAS_CHECK (A ().size1 () <= x ().size (), bad_size ());
    
    size_type n = A().size1();
    if (n==0) return false;
    if (!unit) {
        if (A()(0,0)==0.0) return true;
        x()(0) /= A()(0,0);
    }
   
    for (size_type j = 1; j < n; ++j) {
      double t;
      t = inner_prod( project(x(), range(0,j)), project( row(A(), j), range(0,j) ) );
      if (!unit) {
        if (A()(j,j)==0.0) return true;
        x()(j) = (x()(j) - t)/A()(j,j);
      } else {
        x()(j) -= t;
      }
    }
    return false;
  }

  /** \brief Solve (column-by-column) Ux=b or (I+U)x=b where U is the (strict) upper triangle of A.
    
      This version uses the generic algorithm for dense matrices and
      thus only accesses the (strict) upper triangle of A. The result
      is even correct when the matrix A has non-zero value (at or)
      below the diagonal.
  */
  template <bool unit, class MAT, class VEC>
  bool inplace_solve_upper(const matrix_expression<MAT>& A, vector_expression<VEC>& x, column_major_tag)
  {
    typedef typename VEC::size_type size_type;
    typedef typename VEC::value_type value_type;

    BOOST_UBLAS_CHECK (A ().size2 () >= x ().size (), bad_size ());
    BOOST_UBLAS_CHECK (A ().size1 () <= x ().size (), bad_size ());
    
    size_type n = A().size1();
    if (n==0) return false;
   
    for (size_type j = n-1; j > 0; --j) {
      double t;
      if (!unit) {
        if (A()(j,j)==0.0) return true;
        t = ( x()(j) /= A()(j,j) );
      } else {
        t = x()(j);
      }
      if (t != value_type()) {
        noalias( project(x(), range(0,j)) ) -= t*project( column(A(), j), range(0,j) );
      }
    }
    if (!unit) {
        if (A()(0,0)==0.0) return true;
        x()(0) /= A()(0,0);
    }
    return false;
  }

  /** \brief Solve (row-by-row) Ux=b or (I+U)x=b where U is the (strict) upper triangle of A.
    
      This version uses the generic algorithm for dense matrices and
      thus only accesses the (strict) upper triangle of A. The result
      is even correct when the matrix A has non-zero value (at or)
      below the diagonal.
  */
  template <bool unit, class MAT, class VEC>
  bool inplace_solve_upper(const matrix_expression<MAT>& A, vector_expression<VEC>& x, row_major_tag)
  {
    typedef typename VEC::size_type size_type;
    typedef typename VEC::value_type value_type;

    BOOST_UBLAS_CHECK (A ().size2 () >= x ().size (), bad_size ());
    BOOST_UBLAS_CHECK (A ().size1 () <= x ().size (), bad_size ());
    
    size_type n = A().size1();
    if (n==0) return false;
    if (!unit) {
        x()(n-1) /= A()(n-1,n-1);
    }
   
    for (size_type j = n-2; /* at end */ ; --j) {
      double t;
      t = inner_prod( project(x(), range(j+1,n)), project( row(A(), j), range(j+1,n) ) );
      if (!unit) {
        if (A()(j,j)==0.0) return true;
        x()(j) = (x()(j) - t)/A()(j,j);
      } else {
        x()(j) -= t;
      }
      if (j==0) break;
    }
    return false;
  }

  /* only the lower left or upper right triangle of A is accessed, 
   * the number of rows of A determines the (used) size of X,
   *
   * syntaX 1:
   *   inplace_solve_lower<unit>(A,X)
   *   inplace_solve_upper<unit>(A,X)
   *
   * syntaX 2:
   *   inplace_solve_lower<unit>(A,X,ORI)
   *   inplace_solve_upper<unit>(A,X,ORI)
   * where ORI is row_major_tag or column_major_tag
   * (here you can override the default choice of the orientation of the algorithm)
   */

  /** \brief Solve (column-by-column) LX=b or (I+L)X=b where L is the (strict) lower triangle of A.
    
      This version uses the generic algorithm for dense matrices and
      thus only accesses the (strict) lower triangle of A. The result
      is even correct when the matriX A has non-zero value (at or)
      above the diagonal.
  */
  template <bool unit, class MAT, class VEC>
  bool inplace_solve_lower(const matrix_expression<MAT>& A, matrix_expression<VEC>& X, column_major_tag)
  {
    typedef typename VEC::size_type size_type;
    typedef typename VEC::value_type value_type;

    BOOST_UBLAS_CHECK (A ().size2 () >= X ().size1 (), bad_size ());
    BOOST_UBLAS_CHECK (A ().size1 () <= X ().size1 (), bad_size ());

    size_type nrhs = X().size2();
    
    size_type n = A().size1();
    if (n==0) return false;
   
    for (size_type j = 0; j < (n-1); ++j) {
      if (!unit) {
        if (A()(j,j)==0.0) return true;
        row(X(),j) /= A()(j,j) ;
      }
      noalias( project(X(), range(j+1,n), range(0, nrhs) ) )
               -= outer_prod(project( column(A(), j), range(j+1,n) ), row(X(),j));
    }
    if (!unit) {
        if (A()(n-1,n-1)==0.0) return true;
        row(X(),n-1) /= A()(n-1,n-1);
    }
    return false;
  }

  /** \brief Solve (row-by-row) LX=b or (I+L)X=b where L is the (strict) lower triangle of A.
    
      This version uses the generic algorithm for dense matrices and
      thus only accesses the (strict) lower triangle of A. The result
      is even correct when the matriX A has non-zero value (at or)
      above the diagonal.
  */
  template <bool unit, class MAT, class VEC>
  bool inplace_solve_lower(const matrix_expression<MAT>& A, matrix_expression<VEC>& X, row_major_tag)
  {
    typedef typename VEC::size_type size_type;
    typedef typename VEC::value_type value_type;

    BOOST_UBLAS_CHECK (A ().size2 () >= X ().size1 (), bad_size ());
    BOOST_UBLAS_CHECK (A ().size1 () <= X ().size1 (), bad_size ());
    
    size_type nrhs = X().size2();
    
    size_type n = A().size1();
    if (n==0) return false;
    if (!unit) {
        if (A()(0,0)==0.0) return true;
        row(X(),0) /= A()(0,0);
    }
   
    for (size_type j = 1; j < n; ++j) {
      if (!unit) {
        if (A()(j,j)==0.0) return true;
        noalias( row(X(),j) )
          = ( row(X(),j) - prod( project( row(A(), j), range(0,j) ),
                                 project(X(), range(0,j), range(0,nrhs)) ) )
          / A()(j,j);
      } else {
        noalias( row(X(),j) ) 
          -= prod( project( row(A(), j), range(0,j) ),
                   project(X(), range(0,j), range(0,nrhs)) );
      }
    }
    return false;
  }

  /** \brief Solve (column-by-column) UX=b or (I+U)X=b where U is the (strict) upper triangle of A.
    
      This version uses the generic algorithm for dense matrices and
      thus only accesses the (strict) upper triangle of A. The result
      is even correct when the matriX A has non-zero value (at or)
      below the diagonal.
  */
  template <bool unit, class MAT, class VEC>
  bool inplace_solve_upper(const matrix_expression<MAT>& A, matrix_expression<VEC>& X, column_major_tag)
  {
    typedef typename VEC::size_type size_type;
    typedef typename VEC::value_type value_type;

    BOOST_UBLAS_CHECK (A ().size2 () >= X ().size1 (), bad_size ());
    BOOST_UBLAS_CHECK (A ().size1 () <= X ().size1 (), bad_size ());
    
    size_type nrhs = X().size2();
    
    size_type n = A().size1();
    if (n==0) return false;
   
    for (size_type j = n-1; j > 0; --j) {
      if (!unit) {
        if (A()(j,j)==0.0) return true;
        row(X(),j) /= A()(j,j);
      }
      noalias( project(X(), range(0,j), range(0,nrhs) ) )
               -= outer_prod( project( column(A(), j), range(0,j) ),
                              row(X(),j) );
    }
    if (!unit) {
        if (A()(0,0)==0.0) return true;
        row(X(),0) /= A()(0,0);
    }
    return false;
  }

  /** \brief Solve (row-by-row) UX=b or (I+U)X=b where U is the (strict) upper triangle of A.
    
      This version uses the generic algorithm for dense matrices and
      thus only accesses the (strict) upper triangle of A. The result
      is even correct when the matriX A has non-zero value (at or)
      below the diagonal.
  */
  template <bool unit, class MAT, class VEC>
  bool inplace_solve_upper(const matrix_expression<MAT>& A, matrix_expression<VEC>& X, row_major_tag)
  {
    typedef typename VEC::size_type size_type;
    typedef typename VEC::value_type value_type;

    BOOST_UBLAS_CHECK (A ().size2 () >= X ().size1 (), bad_size ());
    BOOST_UBLAS_CHECK (A ().size1 () <= X ().size1 (), bad_size ());
    
    size_type nrhs = X().size2();
    
    size_type n = A().size1();
    if (n==0) return false;
    if (!unit) {
        row(X(),n-1) /= A()(n-1,n-1);
    }
   
    for (size_type j = n-2; /* at end */ ; --j) {
      if (!unit) {
        if (A()(j,j)==0.0) return true;
        noalias( row(X(),j) )
          = ( row(X(),j) - prod( project( row(A(), j), range(j+1,n) ),
                                 project(X(), range(j+1,n), range(0,nrhs)) ) )
          / A()(j,j);
      } else {
        noalias( row(X(),j) )
          -= prod( project( row(A(), j), range(j+1,n) ),
                   project(X(), range(j+1,n), range(0,nrhs)) );
      }
      if (j==0) break;
    }
    return false;
  }

  // Dispatcher for default orientation 

  template <bool unit, class MAT, class VEC>
  bool inplace_solve_lower(const matrix_expression<MAT>& A, vector_expression<VEC>& x)
  {
    return inplace_solve_lower<unit>(A, x, typename MAT::orientation_category() );
  }

  template <bool unit, class MAT, class VEC>
  bool inplace_solve_upper(const matrix_expression<MAT>& A, vector_expression<VEC>& x)
  {
    return inplace_solve_upper<unit>(A, x, typename MAT::orientation_category() );
  }

  template <bool unit, class MAT, class MATX>
  bool inplace_solve_lower(const matrix_expression<MAT>& A, matrix_expression<MATX>& X)
  {
    return inplace_solve_lower<unit>(A, X, typename MAT::orientation_category() );
  }

  template <bool unit, class MAT, class MATX>
  bool inplace_solve_upper(const matrix_expression<MAT>& A, matrix_expression<MATX>& X)
  {
    return inplace_solve_upper<unit>(A, X, typename MAT::orientation_category() );
  }

  /***********************************
   * PART 3: upper/lower dispatchers *
   ***********************************/

  // note: the dispatcher even work when VEC is a matrix expression.

  template < class MAT, class VEC >
  bool
  inplace_solve( const MAT& L, VEC& x, unit_lower_tag )
  {
    return inplace_solve_lower<true>(L, x);
  }

  template < class MAT, class VEC >
  bool
  inplace_solve( const MAT& L, VEC& x, lower_tag )
  {
    return inplace_solve_lower<false>(L, x);
  }

  template < class MAT, class VEC >
  bool
  inplace_solve( const MAT& U, VEC& x, unit_upper_tag )
  {
    return inplace_solve_upper<true>(U, x);
  }

  template < class MAT, class VEC >
  bool
  inplace_solve( const MAT& U, VEC& x, upper_tag )
  {
    return inplace_solve_upper<false>(U, x);
  }

}}}