/** -*- c++ -*- \file cholesky.hpp \brief cholesky decomposition */
/*
 -   begin                : 2005-08-24
 -   copyright            : (C) 2005 by Gunter Winkler, Konstantin Kutzkow
 -   email                : guwi17@gmx.de

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library 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
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

*/

#ifndef _H_CHOLESKY_HPP_
#define _H_CHOLESKY_HPP_

#include <cassert>

/*
 * Workaround: UBLAS has not been updated to reflect changes to Boost serialization
 * in Boost 1.64.  Including array_wrapper.hpp fixes the problem.  However,
 * including array_wrapper.hpp with Boost < 1.64 also causes compilation
 * errors, so the #include must be conditional on BOOST_VERSION.
 *
 * See: https://stackoverflow.com/questions/44534516/error-make-array-is-not-a-member-of-boostserialization
 * And also: https://github.com/uBLAS/ublas/pull/55#issuecomment-316985022
 */
#if BOOST_VERSION >= 106400
#include <boost/serialization/array_wrapper.hpp>
#endif

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>

#include <boost/numeric/ublas/triangular.hpp>

namespace ublas = boost::numeric::ublas;


/** \brief decompose the symmetric positive definit matrix A into product L L^T.
 *
 * \param MATRIX type of input matrix
 * \param TRIA type of lower triangular output matrix
 * \param A square symmetric positive definite input matrix (only the lower triangle is accessed)
 * \param L lower triangular output matrix
 * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
 */
template < class MATRIX, class TRIA >
size_t cholesky_decompose(const MATRIX& A, TRIA& L)
{
  using namespace ublas;

  assert( A.size1() == A.size2() );
  assert( A.size1() == L.size1() );
  assert( A.size2() == L.size2() );

  const size_t n = A.size1();

  for (size_t k=0 ; k < n; k++) {

    double qL_kk = A(k,k) - inner_prod( project( row(L, k), range(0, k) ),
                                        project( row(L, k), range(0, k) ) );

    if (qL_kk <= 0) {
      return 1 + k;
    } else {
      double L_kk = sqrt( qL_kk );
      L(k,k) = L_kk;

      matrix_column<TRIA> cLk(L, k);
      project( cLk, range(k+1, n) )
        = ( project( column(A, k), range(k+1, n) )
            - prod( project(L, range(k+1, n), range(0, k)),
                    project(row(L, k), range(0, k) ) ) ) / L_kk;
    }
  }
  return 0;
}


/** \brief decompose the symmetric positive definit matrix A into product L L^T.
 *
 * \param MATRIX type of matrix A
 * \param A input: square symmetric positive definite matrix (only the lower triangle is accessed)
 * \param A output: the lower triangle of A is replaced by the cholesky factor
 * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
 */
template < class MATRIX >
size_t cholesky_decompose(MATRIX& A)
{
  using namespace ublas;

  const MATRIX& A_c(A);

  const size_t n = A.size1();

  for (size_t k=0 ; k < n; k++) {

    double qL_kk = A_c(k,k) - inner_prod( project( row(A_c, k), range(0, k) ),
                                          project( row(A_c, k), range(0, k) ) );

    if (qL_kk <= 0) {
      return 1 + k;
    } else {
      double L_kk = sqrt( qL_kk );

      matrix_column<MATRIX> cLk(A, k);
      project( cLk, range(k+1, n) )
        = ( project( column(A_c, k), range(k+1, n) )
            - prod( project(A_c, range(k+1, n), range(0, k)),
                    project(row(A_c, k), range(0, k) ) ) ) / L_kk;
      A(k,k) = L_kk;
    }
  }
  return 0;
}

#if 0
  using namespace ublas;

    // Operations:
    //  n * (n - 1) / 2 + n = n * (n + 1) / 2 multiplications,
    //  n * (n - 1) / 2 additions

    // Dense (proxy) case
    template<class E1, class E2>
    void inplace_solve (const matrix_expression<E1> &e1, vector_expression<E2> &e2,
                        lower_tag, column_major_tag) {
      std::cout << " is_lc ";
        typedef typename E2::size_type size_type;
        typedef typename E2::difference_type difference_type;
        typedef typename E2::value_type value_type;

        BOOST_UBLAS_CHECK (e1 ().size1 () == e1 ().size2 (), bad_size ());
        BOOST_UBLAS_CHECK (e1 ().size2 () == e2 ().size (), bad_size ());
        size_type size = e2 ().size ();
        for (size_type n = 0; n < size; ++ n) {
#ifndef BOOST_UBLAS_SINGULAR_CHECK
            BOOST_UBLAS_CHECK (e1 () (n, n) != value_type/*zero*/(), singular ());
#else
            if (e1 () (n, n) == value_type/*zero*/())
                singular ().raise ();
#endif
            value_type t = e2 () (n) / e1 () (n, n);
            e2 () (n) = t;
            if (t != value_type/*zero*/()) {
              project( e2 (), range(n+1, size) )
                .minus_assign( t * project( column( e1 (), n), range(n+1, size) ) );
            }
        }
    }
#endif





/** \brief decompose the symmetric positive definit matrix A into product L L^T.
 *
 * \param MATRIX type of matrix A
 * \param A input: square symmetric positive definite matrix (only the lower triangle is accessed)
 * \param A output: the lower triangle of A is replaced by the cholesky factor
 * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
 */
template < class MATRIX >
size_t incomplete_cholesky_decompose(MATRIX& A)
{
  using namespace ublas;

  typedef typename MATRIX::value_type T;

  // read access to a const matrix is faster
  const MATRIX& A_c(A);

  const size_t n = A.size1();

  for (size_t k=0 ; k < n; k++) {

    double qL_kk = A_c(k,k) - inner_prod( project( row( A_c, k ), range(0, k) ),
                                          project( row( A_c, k ), range(0, k) ) );

    if (qL_kk <= 0) {
      return 1 + k;
    } else {
      double L_kk = sqrt( qL_kk );

      // aktualisieren
      for (size_t i = k+1; i < A.size1(); ++i) {
        T* Aik = A.find_element(i, k);

        if (Aik != 0) {
          *Aik = ( *Aik - inner_prod( project( row( A_c, k ), range(0, k) ),
                                      project( row( A_c, i ), range(0, k) ) ) ) / L_kk;
        }
      }

      A(k,k) = L_kk;
    }
  }

  return 0;
}




/** \brief solve system L L^T x = b inplace
 *
 * \param L a triangular matrix
 * \param x input: right hand side b; output: solution x
 */
template < class TRIA, class VEC >
void
cholesky_solve(const TRIA& L, VEC& x, ublas::lower)
{
  using namespace ublas;
//   ::inplace_solve(L, x, lower_tag(), typename TRIA::orientation_category () );
  inplace_solve(L, x, lower_tag() );
  inplace_solve(trans(L), x, upper_tag());
}


#endif