/*
 *
 * Copyright (c) Kresimir Fresl 2002, 2003
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_CBLAS_LEVEL_2_HPP
#define BOOST_NUMERIC_BINDINGS_CBLAS_LEVEL_2_HPP

#include <cassert>

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/atlas/cblas2_overloads.hpp>
#include <boost/numeric/bindings/atlas/cblas_enum.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/same_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace atlas {

    // y <- alpha * op (A) * x + beta * y
    // op (A) == A || A^T || A^H
    template <typename T, typename Matr, typename VctX, typename VctY>
    inline
    void gemv (CBLAS_TRANSPOSE const TransA,
               T const& alpha, Matr const& a, VctX const& x,
               T const& beta, VctY& y
               )
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<Matr>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      assert (traits::vector_size (x) >= (TransA == CblasNoTrans ? n : m));
      assert (traits::vector_size (y) >= (TransA == CblasNoTrans ? m : n));
      // .. what about AtlasConj?

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<Matr>::ordering_type
#else
           typename Matr::orientation_category
#endif
         >::value);

      detail::gemv (stor_ord, TransA, m, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::matrix_storage (a),
#else
                    traits::matrix_storage_const (a),
#endif
                    traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
                    beta,
                    traits::vector_storage (y),
                    traits::vector_stride (y));
    }

    // y <- alpha * A * x + beta * y
    template <typename T, typename Matr, typename VctX, typename VctY>
    inline
    void gemv (T const& alpha, Matr const& a, VctX const& x,
               T const& beta, VctY& y) {
      gemv (CblasNoTrans, alpha, a, x, beta, y);
    }

    // y <- A * x
    template <typename Matr, typename VctX, typename VctY>
    inline
    void gemv (Matr const& a, VctX const& x, VctY& y) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<Matr>::value_type val_t;
#else
      typedef typename Matr::value_type val_t;
#endif
      gemv (CblasNoTrans, (val_t) 1, a, x, (val_t) 0, y);
    }

    // y <- alpha * A * x + beta * y
    // A real symmetric matrix (T == float | double)
    // [from 'dsymv.f':]
    /* [...] with UPLO = 'U' or 'u', the  leading n by n upper
     * triangular part of the array A must contain the upper triangular
     * part of the symmetric matrix and the strictly lower triangular
     * part of A is not referenced.
     * [...] with UPLO = 'L' or 'l', the leading n by n lower
     * triangular part of the array A must contain the lower triangular
     * part of the symmetric matrix and the strictly  upper
     * triangular part of A is not referenced.
     */
    template <typename T, typename SymmMatr, typename VctX, typename VctY>
    inline
    void symv (CBLAS_UPLO const uplo, T const& alpha, SymmMatr const& a,
               VctX const& x, T const& beta, VctY& y)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatr>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmMatr>::ordering_type
#else
           typename SymmMatr::orientation_category
#endif
         >::value);

      detail::symv (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::matrix_storage (a),
#else
                    traits::matrix_storage_const (a),
#endif
                    traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
                    beta,
                    traits::vector_storage (y),
                    traits::vector_stride (y));
    }

    template <typename T, typename SymmMatr, typename VctX, typename VctY>
    inline
    void symv (T const& alpha, SymmMatr const& a, VctX const& x,
               T const& beta, VctY& y)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatr>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmMatr>::ordering_type
#else
           typename SymmMatr::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmMatr>::uplo_type
#else
           typename SymmMatr::packed_category
#endif
         >::value);

      detail::symv (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::matrix_storage (a),
#else
                    traits::matrix_storage_const (a),
#endif
                    traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
                    beta,
                    traits::vector_storage (y),
                    traits::vector_stride (y));
    }

    // y <- A * x
    template <typename SymmMatr, typename VctX, typename VctY>
    inline
    void symv (SymmMatr const& a, VctX const& x, VctY& y) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmMatr>::value_type val_t;
#else
      typedef typename SymmMatr::value_type val_t;
#endif
      symv ((val_t) 1, a, x, (val_t) 0, y);
    }

    // y <- alpha * A * x + beta * y
    // A real symmetric matrix (T == float | double) in packed form
    // [from 'dspmv.f' (description assumes column major order):]
    /* A is an array of DIMENSION ( ( n*( n + 1 ) )/2 ).
     * Before entry with UPLO = 'U' or 'u', the array A must contain
     * the upper triangular  part of the symmetric matrix packed
     * sequentially, column by column, so that A( 1 ) contains a(1,1),
     * A( 2 ) and A( 3 ) contain a(1,2) and a(2,2) respectively, and
     * so on.
     * Before entry with UPLO = 'L' or 'l', the array A must contain
     * the lower triangular  part of the symmetric matrix packed
     * sequentially, column by column, so that A( 1 ) contains a(1,1),
     * A( 2 ) and A( 3 ) contain a(2,1) and a(3,1) respectively, and
     * so on.
     */
    template <typename T, typename SymmMatr, typename VctX, typename VctY>
    inline
    void spmv (T const& alpha, SymmMatr const& a, VctX const& x,
               T const& beta, VctY& y)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatr>::matrix_structure,
        traits::symmetric_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmMatr>::ordering_type
#else
           typename SymmMatr::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmMatr>::uplo_type
#else
           typename SymmMatr::packed_category
#endif
         >::value);

      detail::spmv (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::matrix_storage (a),
                    traits::vector_storage (x),
#else
                    traits::matrix_storage_const (a),
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
                    beta,
                    traits::vector_storage (y),
                    traits::vector_stride (y));
    }

    // y <- A * x
    template <typename SymmMatr, typename VctX, typename VctY>
    inline
    void spmv (SymmMatr const& a, VctX const& x, VctY& y) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmMatr>::value_type val_t;
#else
      typedef typename SymmMatr::value_type val_t;
#endif
      spmv ((val_t) 1, a, x, (val_t) 0, y);
    }

    // y <- alpha * A * x + beta * y
    // A complex hermitian matrix
    // (T == std::complex<float> | std::complex<double>)
    template <typename T, typename HermMatr, typename VctX, typename VctY>
    inline
    void hemv (CBLAS_UPLO const uplo, T const& alpha, HermMatr const& a,
               VctX const& x, T const& beta, VctY& y)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermMatr>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermMatr>::ordering_type
#else
           typename HermMatr::orientation_category
#endif
         >::value);

      detail::hemv (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::matrix_storage (a),
#else
                    traits::matrix_storage_const (a),
#endif
                    traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
                    beta,
                    traits::vector_storage (y),
                    traits::vector_stride (y));
    }

    template <typename T, typename HermMatr, typename VctX, typename VctY>
    inline
    void hemv (T const& alpha, HermMatr const& a, VctX const& x,
               T const& beta, VctY& y)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermMatr>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermMatr>::ordering_type
#else
           typename HermMatr::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermMatr>::uplo_type
#else
           typename HermMatr::packed_category
#endif
         >::value);

      detail::hemv (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::matrix_storage (a),
#else
                    traits::matrix_storage_const (a),
#endif
                    traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
                    beta,
                    traits::vector_storage (y),
                    traits::vector_stride (y));
    }

    // y <- A * x
    template <typename HermMatr, typename VctX, typename VctY>
    inline
    void hemv (HermMatr const& a, VctX const& x, VctY& y) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermMatr>::value_type val_t;
#else
      typedef typename HermMatr::value_type val_t;
#endif
      hemv ((val_t) 1, a, x, (val_t) 0, y);
    }

    // y <- alpha * A * x + beta * y
    // A complex hermitian matrix in packed form
    // (T == std::complex<float> | std::complex<double>)
    template <typename T, typename HermMatr, typename VctX, typename VctY>
    inline
    void hpmv (T const& alpha, HermMatr const& a, VctX const& x,
               T const& beta, VctY& y)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermMatr>::matrix_structure,
        traits::hermitian_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermMatr>::ordering_type
#else
           typename HermMatr::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermMatr>::uplo_type
#else
           typename HermMatr::packed_category
#endif
         >::value);

      detail::hpmv (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::matrix_storage (a),
                    traits::vector_storage (x),
#else
                    traits::matrix_storage_const (a),
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
                    beta,
                    traits::vector_storage (y),
                    traits::vector_stride (y));
    }

    // y <- A * x
    template <typename HermMatr, typename VctX, typename VctY>
    inline
    void hpmv (HermMatr const& a, VctX const& x, VctY& y) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermMatr>::value_type val_t;
#else
      typedef typename HermMatr::value_type val_t;
#endif
      hpmv ((val_t) 1, a, x, (val_t) 0, y);
    }

    // A <- alpha * x * y^T + A
    // .. real & complex types
    template <typename T, typename Matr, typename VctX, typename VctY>
    inline
    void ger (T const& alpha, VctX const& x, VctY const& y, Matr& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<Matr>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      assert (traits::vector_size (x) >= m);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<Matr>::ordering_type
#else
           typename Matr::orientation_category
#endif
         >::value);

      detail::ger (stor_ord, m, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                   traits::vector_storage (x),
#else
                   traits::vector_storage_const (x),
#endif
                   traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                   traits::vector_storage (y),
#else
                   traits::vector_storage_const (y),
#endif
                   traits::vector_stride (y),
                   traits::matrix_storage (a),
                   traits::leading_dimension (a));
    }

    // A <- x * y^T + A
    template <typename Matr, typename VctX, typename VctY>
    inline
    void ger (VctX const& x, VctY const& y, Matr& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<Matr>::value_type val_t;
#else
      typedef typename Matr::value_type val_t;
#endif
      ger ((val_t) 1, x, y, a);
    }

    // A <- alpha * x * y^T + A
    // .. complex types only
    template <typename T, typename Matr, typename VctX, typename VctY>
    inline
    void geru (T const& alpha, VctX const& x, VctY const& y, Matr& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<Matr>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      assert (traits::vector_size (x) >= m);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<Matr>::ordering_type
#else
           typename Matr::orientation_category
#endif
         >::value);

      detail::geru (stor_ord, m, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    traits::matrix_storage (a),
                    traits::leading_dimension (a));
    }

    // A <- x * y^T + A
    template <typename Matr, typename VctX, typename VctY>
    inline
    void geru (VctX const& x, VctY const& y, Matr& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<Matr>::value_type val_t;
#else
      typedef typename Matr::value_type val_t;
#endif
      geru ((val_t) 1, x, y, a);
    }

    // A <- alpha * x * y^H + A
    // .. complex types only
    template <typename T, typename Matr, typename VctX, typename VctY>
    inline
    void gerc (T const& alpha, VctX const& x, VctY const& y, Matr& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<Matr>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      assert (traits::vector_size (x) >= m);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<Matr>::ordering_type
#else
           typename Matr::orientation_category
#endif
         >::value);

      detail::gerc (stor_ord, m, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    traits::matrix_storage (a),
                    traits::leading_dimension (a));
    }

    // A <- x * y^H + A
    template <typename Matr, typename VctX, typename VctY>
    inline
    void gerc (VctX const& x, VctY const& y, Matr& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<Matr>::value_type val_t;
#else
      typedef typename Matr::value_type val_t;
#endif
      gerc ((val_t) 1, x, y, a);
    }

    // A <- alpha * x * x^T + A
    // A real symmetric (see leading comments for 'symv()')
    template <typename T, typename SymmM, typename VctX>
    inline
    void syr (CBLAS_UPLO const uplo, T const& alpha, VctX const& x, SymmM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmM>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmM>::ordering_type
#else
           typename SymmM::orientation_category
#endif
         >::value);

      detail::syr (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                   traits::vector_storage (x),
#else
                   traits::vector_storage_const (x),
#endif
                   traits::vector_stride (x),
                   traits::matrix_storage (a),
                   traits::leading_dimension (a));
    }

    template <typename T, typename SymmM, typename VctX>
    inline
    void syr (T const& alpha, VctX const& x, SymmM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmM>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmM>::ordering_type
#else
           typename SymmM::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmM>::uplo_type
#else
           typename SymmM::packed_category
#endif
         >::value);

      detail::syr (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                   traits::vector_storage (x),
#else
                   traits::vector_storage_const (x),
#endif
                   traits::vector_stride (x),
                   traits::matrix_storage (a),
                   traits::leading_dimension (a));
    }

    // A <- x * x^T + A
    template <typename SymmM, typename VctX>
    inline
    void syr (VctX const& x, SymmM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmM>::value_type val_t;
#else
      typedef typename SymmM::value_type val_t;
#endif
      syr ((val_t) 1, x, a);
    }

    // A <- alpha * x * x^T + A
    // A real symmetric in packed form (see leading comments for 'spmv()')
    template <typename T, typename SymmM, typename VctX>
    inline
    void spr (T const& alpha, VctX const& x, SymmM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmM>::matrix_structure,
        traits::symmetric_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmM>::ordering_type
#else
           typename SymmM::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmM>::uplo_type
#else
           typename SymmM::packed_category
#endif
         >::value);

      detail::spr (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                   traits::vector_storage (x),
#else
                   traits::vector_storage_const (x),
#endif
                   traits::vector_stride (x),
                   traits::matrix_storage (a));
    }

    // A <- x * x^T + A
    template <typename SymmM, typename VctX>
    inline
    void spr (VctX const& x, SymmM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmM>::value_type val_t;
#else
      typedef typename SymmM::value_type val_t;
#endif
      spr ((val_t) 1, x, a);
    }

    // A <- alpha * x * y^T + alpha * y * x^T + A
    // A real symmetric (see leading comments for 'symv()')
    template <typename T, typename SymmM, typename VctX, typename VctY>
    inline
    void syr2 (CBLAS_UPLO const uplo, T const& alpha,
               VctX const& x, VctY const& y, SymmM& a)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmM>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmM>::ordering_type
#else
           typename SymmM::orientation_category
#endif
         >::value);

      detail::syr2 (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    traits::matrix_storage (a),
                    traits::leading_dimension (a));
    }

    template <typename T, typename SymmM, typename VctX, typename VctY>
    inline
    void syr2 (T const& alpha, VctX const& x, VctY const& y, SymmM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmM>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmM>::ordering_type
#else
           typename SymmM::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmM>::uplo_type
#else
           typename SymmM::packed_category
#endif
         >::value);

      detail::syr2 (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    traits::matrix_storage (a),
                    traits::leading_dimension (a));
    }

    // A <- x * y^T + y * x^T + A
    template <typename SymmM, typename VctX, typename VctY>
    inline
    void syr2 (VctX const& x, VctY const& y, SymmM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmM>::value_type val_t;
#else
      typedef typename SymmM::value_type val_t;
#endif
      syr2 ((val_t) 1, x, y, a);
    }

    // A <- alpha * x * y^T + alpha * y * x^T + A
    // A real symmetric in packed form (see leading comments for 'spmv()')
    template <typename T, typename SymmM, typename VctX, typename VctY>
    inline
    void spr2 (T const& alpha, VctX const& x, VctY const& y, SymmM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmM>::matrix_structure,
        traits::symmetric_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmM>::ordering_type
#else
           typename SymmM::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<SymmM>::uplo_type
#else
           typename SymmM::packed_category
#endif
         >::value);

      detail::spr2 (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    traits::matrix_storage (a));
    }

    // A <- x * y^T + y * x^T + A
    template <typename SymmM, typename VctX, typename VctY>
    inline
    void spr2 (VctX const& x, VctY const& y, SymmM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmM>::value_type val_t;
#else
      typedef typename SymmM::value_type val_t;
#endif
      spr2 ((val_t) 1, x, y, a);
    }

    // A <- alpha * x * x^H + A
    // A hermitian
    template <typename T, typename HermM, typename VctX>
    inline
    void her (CBLAS_UPLO const uplo, T const& alpha, VctX const& x, HermM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermM>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermM>::ordering_type
#else
           typename HermM::orientation_category
#endif
         >::value);

      detail::her (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                   traits::vector_storage (x),
#else
                   traits::vector_storage_const (x),
#endif
                   traits::vector_stride (x),
                   traits::matrix_storage (a),
                   traits::leading_dimension (a));
    }

    template <typename T, typename HermM, typename VctX>
    inline
    void her (T const& alpha, VctX const& x, HermM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermM>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermM>::ordering_type
#else
           typename HermM::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermM>::uplo_type
#else
           typename HermM::packed_category
#endif
         >::value);

      detail::her (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                   traits::vector_storage (x),
#else
                   traits::vector_storage_const (x),
#endif
                   traits::vector_stride (x),
                   traits::matrix_storage (a),
                   traits::leading_dimension (a));
    }

    // A <- x * x^H + A
    template <typename HermM, typename VctX>
    inline
    void her (VctX const& x, HermM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermM>::value_type val_t;
#else
      typedef typename HermM::value_type val_t;
#endif
      typedef typename traits::type_traits<val_t>::real_type real_t;
      her ((real_t) 1, x, a);
    }

    // A <- alpha * x * x^H + A
    // A hermitian in packed form
    template <typename T, typename HermM, typename VctX>
    inline
    void hpr (T const& alpha, VctX const& x, HermM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermM>::matrix_structure,
        traits::hermitian_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermM>::ordering_type
#else
           typename HermM::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermM>::uplo_type
#else
           typename HermM::packed_category
#endif
         >::value);

      detail::hpr (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                   traits::vector_storage (x),
#else
                   traits::vector_storage_const (x),
#endif
                   traits::vector_stride (x),
                   traits::matrix_storage (a));
    }

    // A <- x * x^H + A
    template <typename HermM, typename VctX>
    inline
    void hpr (VctX const& x, HermM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermM>::value_type val_t;
#else
      typedef typename HermM::value_type val_t;
#endif
      typedef typename traits::type_traits<val_t>::real_type real_t;
      hpr ((real_t) 1, x, a);
    }

    // A <- alpha * x * y^H + y * (alpha * x)^H + A
    // A hermitian
    template <typename T, typename HermM, typename VctX, typename VctY>
    inline
    void her2 (CBLAS_UPLO const uplo, T const& alpha,
               VctX const& x, VctY const& y, HermM& a)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermM>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermM>::ordering_type
#else
           typename HermM::orientation_category
#endif
         >::value);

      detail::her2 (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    traits::matrix_storage (a),
                    traits::leading_dimension (a));
    }

    template <typename T, typename HermM, typename VctX, typename VctY>
    inline
    void her2 (T const& alpha, VctX const& x, VctY const& y, HermM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermM>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermM>::ordering_type
#else
           typename HermM::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermM>::uplo_type
#else
           typename HermM::packed_category
#endif
         >::value);

      detail::her2 (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    traits::matrix_storage (a),
                    traits::leading_dimension (a));
    }

    // A <- x * y^H + y * x^H + A
    template <typename HermM, typename VctX, typename VctY>
    inline
    void her2 (VctX const& x, VctY const& y, HermM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermM>::value_type val_t;
#else
      typedef typename HermM::value_type val_t;
#endif
      her2 ((val_t) 1, x, y, a);
    }

    // A <- alpha * x * y^H + y * (alpha * x)^H + A
    // A hermitian in packed form
    template <typename T, typename HermM, typename VctX, typename VctY>
    inline
    void hpr2 (T const& alpha, VctX const& x, VctY const& y, HermM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermM>::matrix_structure,
        traits::hermitian_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (x) >= n);
      assert (traits::vector_size (y) >= n);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermM>::ordering_type
#else
           typename HermM::orientation_category
#endif
         >::value);

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<HermM>::uplo_type
#else
           typename HermM::packed_category
#endif
         >::value);

      detail::hpr2 (stor_ord, uplo, n, alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    traits::matrix_storage (a));
    }

    // A <- x * y^H + y * x^H + A
    template <typename HermM, typename VctX, typename VctY>
    inline
    void hpr2 (VctX const& x, VctY const& y, HermM& a) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermM>::value_type val_t;
#else
      typedef typename HermM::value_type val_t;
#endif
      hpr2 ((val_t) 1, x, y, a);
    }

  } // namespace atlas

}}}

#endif // BOOST_NUMERIC_BINDINGS_CBLAS_LEVEL_2_HPP


/* The Image Registration Toolkit (IRTK)
 *
 * Copyright 2008-2015 Imperial College London
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include <irtkTransformation.h>

#include <boost/cstdint.hpp>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/math/tools/tuple.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <algorithm>

// =============================================================================
// Newton-Raphson solver
// =============================================================================

namespace boost { namespace math { namespace tools {

namespace detail {

template<class T>
bool invert_matrix(const boost::numeric::ublas::matrix<T> & operand,
  boost::numeric::ublas::matrix<T> & result)
{
  namespace ublas = boost::numeric::ublas;
  typedef ublas::matrix<T> matrix_type;
  typedef typename matrix_type::size_type size_type;
  typedef typename matrix_type::value_type value_type;
  typedef ublas::permutation_matrix<size_type> permutation_matrix_type;
  typedef ublas::identity_matrix<value_type> identity_matrix_type;

  // Create temporary copy of operand.
  matrix_type A(operand);
  // Create a permutation matrix for the LU-factorization.
  permutation_matrix_type P(A.size1());
  // Perform LU-factorization.
  size_type singular = lu_factorize(A, P);
  // Set inverse to identity.
  result.assign(identity_matrix_type(A.size1()));
  // Substitute back to compute the inverse.
  lu_substitute(A, P, result);

  return (singular == 0);
}

} // end namespace detail

template<class F>
bool multivariate_newton_raphson_iterate(F f,
  typename F::vector_type & x, int digits,
  boost::uintmax_t max_iter=100)
{
  typedef typename F::vector_type vector_type;
  typedef typename F::matrix_type matrix_type;
  typedef typename vector_type::value_type value_type;
  typedef typename vector_type::size_type size_type;

  value_type factor = static_cast<value_type>(ldexp(1.0, 1-digits));
  size_type n = x.size();
  vector_type d_x(n), e_x(n), f0(n);
  matrix_type f1(n, n), f1_inv(n, n);
  boost::uintmax_t iter=1;

  do
  {
    boost::math::tie(f0, f1) = f(x);
    if(!detail::invert_matrix<value_type>(f1, f1_inv))
      return false;
    d_x = boost::numeric::ublas::prod(f1_inv, f0);
    x -= d_x;
    for(size_type ni = 0; ni < n; ni++)
      // FIXME: use zip iterator.
      e_x(ni) = fabs(d_x(ni)) - fabs(x(ni) * factor);
  }
  while((iter++ < max_iter) &&
         std::any_of(e_x.begin(), e_x.end(), [](value_type val){return val > 0;}));

  return true;
}

}}} // namespace boost::math::tools

// =============================================================================
// Inverse global transformation
// =============================================================================

struct GlobalInverseTransformFunctor
{
  typedef boost::numeric::ublas::vector<double> vector_type;
  typedef boost::numeric::ublas::matrix<double> matrix_type;
  typedef boost::math::tuple<vector_type, matrix_type> return_type;

  GlobalInverseTransformFunctor(const irtkTransformation * transformation,
    double x, double y, double z, double t, double t0)
    : transformation_(transformation), x_(x), y_(y), z_(z), t_(t), t0_(t0)
  {
  }

  return_type operator()(const vector_type & x)
  {
    typedef typename vector_type::value_type value_type;
    typedef typename vector_type::size_type size_type;

    value_type x1 = x(0), x2 = x(1), x3 = x(2);
    transformation_->GlobalTransform(x1, x2, x3, t_, t0_);
    vector_type f0(3);
    f0(0) = x1 - x_;
    f0(1) = x2 - y_;
    f0(2) = x3 - z_;

    irtkMatrix J(3, 3);
    transformation_->GlobalJacobian(J, x(0), x(1), x(2), t_, t0_);
    matrix_type f1(3, 3);
    for(size_type icol = 0; icol < 3; icol++)
      for(size_type irow = 0; irow < 3; irow++)
        f1(irow, icol) = J(irow, icol);

    return boost::math::make_tuple(f0, f1);
  }

  const irtkTransformation * transformation_;
  double x_;
  double y_;
  double z_;
  double t_;
  double t0_;
};

// -----------------------------------------------------------------------------
bool EvaluateGlobalInverse(const irtkTransformation *T,
                           double &x, double &y, double &z, double t, double t0)
{
  using namespace boost::math::tools;
  typedef GlobalInverseTransformFunctor functor_type;
  typedef typename functor_type::vector_type vector_type;

  functor_type fun(T, x, y, z, t, t0);
  vector_type sol(3); sol(0) = x;  sol(1) = y;  sol(2) = z;
  bool success = multivariate_newton_raphson_iterate<functor_type>(fun, sol, 6);
  x = sol(0); y = sol(1); z = sol(2);
  return success;
}

// =============================================================================
// Inverse local transformation
// =============================================================================

struct LocalInverseTransformFunctor
{
  typedef boost::numeric::ublas::vector<double> vector_type;
  typedef boost::numeric::ublas::matrix<double> matrix_type;
  typedef boost::math::tuple<vector_type, matrix_type> return_type;

  LocalInverseTransformFunctor(const irtkTransformation * transformation,
    double x, double y, double z, double t, double t0)
    : transformation_(transformation), x_(x), y_(y), z_(z), t_(t), t0_(t0)
  {
  }

  return_type operator()(vector_type x)
  {
    typedef typename vector_type::value_type value_type;
    typedef typename vector_type::size_type size_type;

    value_type x1 = x(0), x2 = x(1), x3 = x(2);
    transformation_->LocalTransform(x1, x2, x3, t_, t0_);
    vector_type f0(3);
    f0(0) = x1 - x_;
    f0(1) = x2 - y_;
    f0(2) = x3 - z_;

    irtkMatrix J(3, 3);
    transformation_->LocalJacobian(J, x(0), x(1), x(2), t_, t0_);
    matrix_type f1(3, 3);
    for(size_type icol = 0; icol < 3; icol++)
      for(size_type irow = 0; irow < 3; irow++)
        f1(irow, icol) = J(irow, icol);

    return boost::math::make_tuple(f0, f1);
  }

  const irtkTransformation * transformation_;
  double x_;
  double y_;
  double z_;
  double t_;
  double t0_;
};

// -----------------------------------------------------------------------------
bool EvaluateLocalInverse(const irtkTransformation *T,
                          double &x, double &y, double &z, double t, double t0)
{
  using namespace boost::math::tools;
  typedef LocalInverseTransformFunctor functor_type;
  typedef typename functor_type::vector_type vector_type;

  functor_type fun(T, x, y, z, t, t0);
  vector_type sol(3); sol(0) = x;  sol(1) = y;  sol(2) = z;
  bool success = multivariate_newton_raphson_iterate<functor_type>(fun, sol, 6);
  x = sol(0); y = sol(1); z = sol(2);
  return success;
}

// =============================================================================
// Inverse transformation
// =============================================================================

struct InverseTransformFunctor
{
  typedef boost::numeric::ublas::vector<double> vector_type;
  typedef boost::numeric::ublas::matrix<double> matrix_type;
  typedef boost::math::tuple<vector_type, matrix_type> return_type;

  InverseTransformFunctor(const irtkTransformation * transformation,
    double x, double y, double z, double t, double t0)
    : transformation_(transformation), x_(x), y_(y), z_(z), t_(t), t0_(t0)
  {
  }

  return_type operator()(vector_type x)
  {
    typedef typename vector_type::value_type value_type;
    typedef typename vector_type::size_type size_type;

    value_type x1 = x(0), x2 = x(1), x3 = x(2);
    transformation_->Transform(x1, x2, x3, t_, t0_);
    vector_type f0(3);
    f0(0) = x1 - x_;
    f0(1) = x2 - y_;
    f0(2) = x3 - z_;

    irtkMatrix J(3, 3);
    transformation_->Jacobian(J, x(0), x(1), x(2), t_, t0_);
    matrix_type f1(3, 3);
    for(size_type icol = 0; icol < 3; icol++)
      for(size_type irow = 0; irow < 3; irow++)
        f1(irow, icol) = J(irow, icol);

    return boost::math::make_tuple(f0, f1);
  }

  const irtkTransformation * transformation_;
  double x_;
  double y_;
  double z_;
  double t_;
  double t0_;
};

// -----------------------------------------------------------------------------
bool EvaluateInverse(const irtkTransformation *T,
                     double &x, double &y, double &z, double t, double t0)
{
  using namespace boost::math::tools;
  typedef InverseTransformFunctor functor_type;
  typedef typename functor_type::vector_type vector_type;

  functor_type fun(T, x, y, z, t, t0);
  vector_type sol(3); sol(0) = x;  sol(1) = y;  sol(2) = z;
  bool success = multivariate_newton_raphson_iterate<functor_type>(fun, sol, 6);
  x = sol(0); y = sol(1); z = sol(2);
  return success;
}

// =============================================================================
// Inverse multi-level transformation
// =============================================================================

struct InverseMultiLevelTransformFunctor
{
  typedef boost::numeric::ublas::vector<double> vector_type;
  typedef boost::numeric::ublas::matrix<double> matrix_type;
  typedef boost::math::tuple<vector_type, matrix_type> return_type;

  InverseMultiLevelTransformFunctor(const irtkMultiLevelTransformation * transformation,
    double x, double y, double z, int m, int n, double t, double t0)
    : transformation_(transformation), x_(x), y_(y), z_(z), m_(m), n_(n), t_(t), t0_(t0)
  {
  }

  return_type operator()(vector_type x)
  {
    typedef typename vector_type::value_type value_type;
    typedef typename vector_type::size_type size_type;

    value_type x1 = x(0), x2 = x(1), x3 = x(2);
    transformation_->Transform(m_, n_, x1, x2, x3, t_, t0_);
    vector_type f0(3);
    f0(0) = x1 - x_;
    f0(1) = x2 - y_;
    f0(2) = x3 - z_;

    irtkMatrix J(3, 3);
    transformation_->Jacobian(m_, n_, J, x(0), x(1), x(2), t_, t0_);
    matrix_type f1(3, 3);
    for(size_type icol = 0; icol < 3; icol++)
      for(size_type irow = 0; irow < 3; irow++)
        f1(irow, icol) = J(irow, icol);

    return boost::math::make_tuple(f0, f1);
  }

  const irtkMultiLevelTransformation * transformation_;
  double x_;
  double y_;
  double z_;
  int m_;
  int n_;
  double t_;
  double t0_;
};

// -----------------------------------------------------------------------------
bool EvaluateInverse(const irtkMultiLevelTransformation *T, int m, int n,
                     double &x, double &y, double &z, double t, double t0)
{
  using namespace boost::math::tools;
  typedef InverseMultiLevelTransformFunctor functor_type;
  typedef typename functor_type::vector_type vector_type;

  functor_type fun(T, x, y, z, m, n, t, t0);
  vector_type sol(3); sol(0) = x;  sol(1) = y;  sol(2) = z;
  bool success = multivariate_newton_raphson_iterate<functor_type>(fun, sol, 6);
  x = sol(0); y = sol(1); z = sol(2);
  return success;
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_EXP10_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_EXP10_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-exponential
    This function object returns the base ten exponential of its argument: \f$10^{x}\f$

    @par Header <boost/simd/function/exp10.hpp>

    @par Note:

    provisions are made for obtaining a flint result from a flint argument.

    @see exp2, exp, pow

    @par Example:

      @snippet exp10.cpp exp10

    @par Possible output:

      @snippet exp10.txt exp10

  **/
  IEEEValue exp10(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/exp10.hpp>
#include <boost/simd/function/simd/exp10.hpp>

#endif


#include "heg_system.h"

#include <array>
#include <boost/functional/hash.hpp>
#include <climits>
#include <cmath>
#include <exception>
#include <unordered_map>
#include "../array_math.h"
#include "../injector.h"
#include "../solver/spin_det_util.h"
#include "k_points_util.h"
#include "omp.h"

class HEGSystemImpl : public HEGSystem {
 public:
  HEGSystemImpl(Session* const session);

  void setup(const double rcut) override;

  int get_n_orbitals(const double rcut) const override;

  double hamiltonian(
      const data::Determinant* const det_pq,
      const data::Determinant* const det_rs) const override;

  void find_connected_dets(
      const data::Determinant* const det,
      const double eps,
      const std::function<void(const data::Determinant* const)>&
          connected_det_handler) override;

 private:
  double rcut = 0.0;

  double r_s = 0.0;

  int n_up = 0;

  int n_dn = 0;

  double k_unit = 0.0;

  double H_unit = 0.0;

  double max_abs_H = 0.0;

  double energy_hf = 0.0;

  bool verbose = false;

  std::vector<std::array<int8_t, 3>> k_points;

  std::unordered_map<
      std::array<int8_t, 3>,
      int,
      boost::hash<std::array<int8_t, 3>>>
      k_lut;

  std::unordered_map<
      std::array<int8_t, 3>,
      std::vector<std::pair<std::array<int8_t, 3>, double>>,
      boost::hash<std::array<int8_t, 3>>>
      same_spin_hci_queue;  // O(k_points^2).

  std::vector<std::pair<std::array<int8_t, 3>, double>>
      opposite_spin_hci_queue;  // O(k_points).

  std::vector<data::Determinant> tmp_dets;

  void setup_hci_queue();

  void evaluate_energy_hf();

  double hamiltonian_diagonal(const data::Determinant* const det) const;
};

HEGSystemImpl::HEGSystemImpl(Session* const session) : HEGSystem(session) {
  Config* const config = session->get_config();
  Parallel* const parallel = session->get_parallel();
  verbose = parallel->is_master();
  tmp_dets.resize(parallel->get_n_threads());
  r_s = config->get_double("r_s");
  n_up = config->get_int("n_up");
  n_dn = config->get_int("n_dn");
}

void HEGSystemImpl::setup(const double rcut) {
  assert(rcut > 0 && rcut < 32);
  this->rcut = rcut;

  constexpr double PI = 3.14159265358979323846;

  // Setup basic units.
  const double density = 3.0 / (4.0 * PI * std::pow(r_s, 3));
  const double cell_length = std::pow((n_up + n_dn) / density, 1.0 / 3);
  k_unit = 2 * PI / cell_length;
  H_unit = 1.0 / (PI * cell_length);

  // Setup K points and its look up table.
  k_points = KPointsUtil::generate_k_points(rcut);
  const int n_k_points = k_points.size();
  if (verbose) printf("Number of orbitals: %'d\n", n_k_points * 2);
  k_lut.clear();
  for (int i = 0; i < n_k_points; i++) k_lut[k_points[i]] = i;

  // Setup HCI queue.
  setup_hci_queue();

  // Evaluate HF energy.
  evaluate_energy_hf();
}

int HEGSystemImpl::get_n_orbitals(const double rcut) const {
  return KPointsUtil::get_n_k_points(rcut) * 2;
}

double HEGSystemImpl::hamiltonian(
    const data::Determinant* const det_pq,
    const data::Determinant* const det_rs) const {
  if (det_pq == det_rs) {
    return hamiltonian_diagonal(det_pq);
  } else {
    const auto& eor_up = SpinDetUtil::get_eor(det_pq->up(), det_rs->up());
    const int n_eor_up = eor_up.size();
    if (n_eor_up > 4) return 0.0;
    const auto& eor_dn = SpinDetUtil::get_eor(det_pq->dn(), det_rs->dn());
    const int n_eor_dn = eor_dn.size();
    if (n_eor_up == 0 && n_eor_dn == 0) return hamiltonian_diagonal(det_pq);
    if (n_eor_up + n_eor_dn != 4) return 0.0;

    // Obtain p, q, s.
    bool k_p_set = false, k_r_set = false;
    int orb_p = 0, orb_r = 0, orb_s = 0;
    std::array<int8_t, 3> k_change;
    k_change.fill(0);
    for (const auto orb_i : eor_up) {
      if (SpinDetUtil::is_occupied(det_pq->up(), orb_i)) {
        k_change -= k_points[orb_i];
        if (!k_p_set) {
          orb_p = orb_i;
          k_p_set = true;
        }
      } else {
        k_change += k_points[orb_i];
        if (!k_r_set) {
          orb_r = orb_i;
          k_r_set = true;
        } else {
          orb_s = orb_i;
        }
      }
    }
    for (const auto orb_i : eor_dn) {
      if (SpinDetUtil::is_occupied(det_pq->dn(), orb_i)) {
        k_change -= k_points[orb_i];
        if (!k_p_set) {
          orb_p = orb_i;
          k_p_set = true;
        }
      } else {
        k_change += k_points[orb_i];
        if (!k_r_set) {
          orb_r = orb_i;
          k_r_set = true;
        } else {
          orb_s = orb_i;
        }
      }
    }

    // Check for momentum conservation.
    if (k_change != 0) return 0.0;

    double H = H_unit / squared_norm(k_points[orb_p] - k_points[orb_r]);
    if (n_eor_up != 2) {
      H -= H_unit / squared_norm(k_points[orb_p] - k_points[orb_s]);
    }

    const auto& get_gamma_exp = [&](const data::SpinDeterminant& spin_det,
                                    const std::vector<int>& eor) {
      int res = 0;
      for (const int orb : eor) {
        res += SpinDetUtil::get_n_lower_elecs(spin_det, orb);
      }
      return res;
    };

    const int gamma_exp = get_gamma_exp(det_pq->up(), eor_up) +
                          get_gamma_exp(det_pq->dn(), eor_dn) +
                          get_gamma_exp(det_rs->up(), eor_up) +
                          get_gamma_exp(det_rs->dn(), eor_dn);
    if ((gamma_exp & 1) == 1) H = -H;

    return H;
  }
};

double HEGSystemImpl::hamiltonian_diagonal(
    const data::Determinant* const det) const {
  double H = energy_hf;

  const auto& spin_correct = [&](const data::SpinDeterminant& spin_det) {
    const int n_hf_elecs = spin_det.n_hf_elecs();
    const int n_v_holes = spin_det.v_holes_size();
    const int n_c_elecs = spin_det.c_elecs_size();

    // One electron operators correction.
    for (const int p : spin_det.v_holes())
      H -= squared_norm(k_points[p] * k_unit) * 0.5;
    for (const int p : spin_det.c_elecs())
      H += squared_norm(k_points[p] * k_unit) * 0.5;

    // Two electron operators correction.
    std::vector<bool> hf_orbs(n_hf_elecs, true);
    for (const int p : spin_det.v_holes()) hf_orbs[p] = false;
    for (int p = 0; p < n_hf_elecs; p++) {
      if (!hf_orbs[p]) continue;
      const auto& k_p = k_points[p];
      for (const int q : spin_det.v_holes()) {
        const auto& k_q = k_points[q];
        H += H_unit / squared_norm(k_p - k_q);
      }
      for (const int q : spin_det.c_elecs()) {
        const auto& k_q = k_points[q];
        H -= H_unit / squared_norm(k_p - k_q);
      }
    }
    for (int ip = 0; ip < n_v_holes; ip++) {
      const auto& k_p = k_points[spin_det.v_holes(ip)];
      for (int iq = ip + 1; iq < n_v_holes; iq++) {
        const auto& k_q = k_points[spin_det.v_holes(iq)];
        H += H_unit / squared_norm(k_p - k_q);
      }
    }
    for (int ip = 0; ip < n_c_elecs; ip++) {
      const auto& k_p = k_points[spin_det.c_elecs(ip)];
      for (int iq = ip + 1; iq < n_c_elecs; iq++) {
        const auto& k_q = k_points[spin_det.c_elecs(iq)];
        H -= H_unit / squared_norm(k_p - k_q);
      }
    }
  };

  spin_correct(det->up());
  spin_correct(det->dn());

  return H;
}

void HEGSystemImpl::find_connected_dets(
    const data::Determinant* const det,
    const double eps,
    const std::function<void(const data::Determinant* const)>&
        connected_det_handler) {
  if (max_abs_H < eps) return;

  const int thread_id = omp_get_thread_num();
  auto& new_det = tmp_dets[thread_id];
  new_det = *det;

  const int dn_offset = k_points.size();

  const auto& is_occupied = [&](const data::Determinant* const target_det,
                                const int orb) {
    if (orb < dn_offset) {
      return SpinDetUtil::is_occupied(target_det->up(), orb);
    } else {
      return SpinDetUtil::is_occupied(target_det->dn(), orb - dn_offset);
    }
  };

  const auto& set_occupation =
      [&](data::Determinant* const target_det, const int orb, const bool occ) {
        if (orb < dn_offset) {
          SpinDetUtil::set_occupation(target_det->mutable_up(), orb, occ);
        } else {
          SpinDetUtil::set_occupation(
              target_det->mutable_dn(), orb - dn_offset, occ);
        }
      };

  const auto& pq_handler = [&](const int p, const int q) {
    int pp = p, qq = q;
    if (p >= dn_offset && q >= dn_offset) {
      pp -= dn_offset;
      qq -= dn_offset;
    } else if (p < dn_offset && q >= dn_offset && p > q - dn_offset) {
      pp = q - dn_offset;
      qq = p + dn_offset;
    }
    bool same_spin = false;
    const std::vector<std::pair<std::array<int8_t, 3>, double>>* items_ptr;
    if (pp < dn_offset && qq < dn_offset) {
      same_spin = true;
      const auto& diff_pq = k_points[qq] - k_points[pp];
      items_ptr = &(same_spin_hci_queue.find(diff_pq)->second);
    } else {
      items_ptr = &(opposite_spin_hci_queue);
    }
    const auto& items = *items_ptr;
    int qs_offset = 0;
    if (!same_spin) qs_offset = dn_offset;

    for (const auto& item : items) {
      if (item.second < eps) break;
      const auto& diff_pr = item.first;
      const auto& it_r = k_lut.find(diff_pr + k_points[pp]);
      if (it_r == k_lut.end()) continue;
      int r = it_r->second;
      const auto& it_s =
          k_lut.find(k_points[pp] + k_points[qq - qs_offset] - k_points[r]);
      if (it_s == k_lut.end()) continue;
      int s = it_s->second;
      if (same_spin && s < r) continue;
      s += qs_offset;
      if (p >= dn_offset && q >= dn_offset) {
        r += dn_offset;
        s += dn_offset;
      } else if (p < dn_offset && q >= dn_offset && p > q - dn_offset) {
        const int tmp = s;
        s = r + dn_offset;
        r = tmp - dn_offset;
      }

      if (!is_occupied(det, r) && !is_occupied(det, s)) {
        set_occupation(&new_det, p, false);
        set_occupation(&new_det, q, false);
        set_occupation(&new_det, r, true);
        set_occupation(&new_det, s, true);
        connected_det_handler(&new_det);
        set_occupation(&new_det, p, true);
        set_occupation(&new_det, q, true);
        set_occupation(&new_det, r, false);
        set_occupation(&new_det, s, false);
      }
    }
  };

  const auto& occ_up = SpinDetUtil::get_occupied_orbitals(det->up());
  const auto& occ_dn = SpinDetUtil::get_occupied_orbitals(det->dn());

  for (int i = 0; i < n_up; i++) {
    for (int j = i + 1; j < n_up; j++) {
      pq_handler(occ_up[i], occ_up[j]);
    }
  }
  for (int i = 0; i < n_dn; i++) {
    for (int j = i + 1; j < n_dn; j++) {
      pq_handler(occ_dn[i] + dn_offset, occ_dn[j] + dn_offset);
    }
  }
  for (int i = 0; i < n_up; i++) {
    for (int j = 0; j < n_dn; j++) {
      pq_handler(occ_up[i], occ_dn[j] + dn_offset);
    }
  }
};

void HEGSystemImpl::setup_hci_queue() {
  same_spin_hci_queue.clear();
  opposite_spin_hci_queue.clear();
  max_abs_H = 0.0;

  // Common dependencies.
  const auto& k_diffs = KPointsUtil::get_k_diffs(k_points);
  const auto& sort_comparison = [](const auto& a, const auto& b) -> bool {
    return a.second > b.second;
  };

  // Same spin.
  const double diff_max_squared = 4.0 * rcut * rcut;
  for (const auto& diff_pq : k_diffs) {
    for (const auto& diff_pr : k_diffs) {
      const auto& diff_sr =
          diff_pr + diff_pr - diff_pq;  // Momentum conservation.
      if (diff_sr == 0 || squared_norm(diff_sr) > diff_max_squared) continue;
      const auto& diff_ps = diff_pr - diff_sr;
      if (diff_ps == 0) continue;
      if (std::abs(squared_norm(diff_pr) - squared_norm(diff_ps)) <
          std::numeric_limits<double>::epsilon()) {
        continue;
      }
      const double abs_H =
          std::abs(1.0 / squared_norm(diff_pr) - 1.0 / squared_norm(diff_ps));
      if (abs_H < std::numeric_limits<double>::epsilon()) continue;
      const auto& item = std::make_pair(diff_pr, abs_H * H_unit);
      same_spin_hci_queue[diff_pq].push_back(item);
    }
  }
  unsigned long long n_same_spin = 0;
  for (auto& kv : same_spin_hci_queue) {
    auto& items = kv.second;
    std::stable_sort(items.begin(), items.end(), sort_comparison);
    max_abs_H = std::max(max_abs_H, items.front().second);
    n_same_spin += items.size();
  }
  if (verbose) {
    printf("Number of same spin hci queue items: %'llu\n", n_same_spin);
  }

  // Opposite spin.
  for (const auto& diff_pr : k_diffs) {
    const double abs_H = 1.0 / squared_norm(diff_pr);
    if (abs_H < std::numeric_limits<double>::epsilon()) continue;
    const auto& item = std::make_pair(diff_pr, abs_H * H_unit);
    opposite_spin_hci_queue.push_back(item);
  }
  std::stable_sort(
      opposite_spin_hci_queue.begin(),
      opposite_spin_hci_queue.end(),
      sort_comparison);
  max_abs_H = std::max(max_abs_H, opposite_spin_hci_queue.front().second);
  if (verbose) {
    unsigned long long n_opposite_spin = opposite_spin_hci_queue.size();
    printf("Number of opposite spin hci queue items: %'llu\n", n_opposite_spin);
  }
}

void HEGSystemImpl::evaluate_energy_hf() {
  double H = 0.0;

  // One electron operator.
  for (int p = 0; p < n_up; p++) H += squared_norm(k_points[p] * k_unit) * 0.5;
  for (int p = 0; p < n_dn; p++) H += squared_norm(k_points[p] * k_unit) * 0.5;

  // Two electrons operator.
  for (int p = 0; p < n_up; p++) {
    const auto& k_p = k_points[p];
    for (int q = p + 1; q < n_up; q++) {
      const auto& k_q = k_points[q];
      H -= H_unit / squared_norm(k_p - k_q);
    }
  }
  for (int p = 0; p < n_dn; p++) {
    const auto& k_p = k_points[p];
    for (int q = p + 1; q < n_dn; q++) {
      const auto& k_q = k_points[q];
      H -= H_unit / squared_norm(k_p - k_q);
    }
  }

  energy_hf = H;
  if (verbose) printf("HF energy: %.12f Ha\n", energy_hf);
}

HEGSystem* Injector::new_heg_system(Session* const session) {
  return new HEGSystemImpl(session);
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/concept/group.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/group.hpp>
using namespace boost::hana;

int main() {
    test::TestGroup<int>{make<tuple_tag>(0,1,2,3,4,5)};
    test::TestGroup<long>{make<tuple_tag>(0l,1l,2l,3l,4l,5l)};

    // minus
    static_assert(minus(6, 4) == 6 - 4, "");

    // negate
    static_assert(negate(6) == -6, "");
}


#ifndef Integration_detail_ExponentialIntegral_hpp
#define Integration_detail_ExponentialIntegral_hpp

#include <boost/math/special_functions/expint.hpp>
#include <iostream>

/** @file ExponentialIntegral.hpp
  * @brief Contains ArrheniusIntegral class specialization for trapezoid method.
  * @author C.D. Clark III
  * @date 06/26/17
  *
  * NOTE: This file is expected to be included from the ArrheniusIntegral.hpp
  * It does not include any of the headers that are already included there, which
  * only works if the contents of ArrheniusIntegral.hpp is included first.
  */

namespace libArrhenius {

template <typename Real>
class ArrheniusIntegral<Real,ExponentialIntegral> : public ArrheniusIntegralBase<Real>
{
  protected:
    // this will keep up from having to use 'this->' to access these.
    using ArrheniusIntegralBase<Real>::Ea;
    using ArrheniusIntegralBase<Real>::A;
    using ArrheniusIntegralBase<Real>::parallel_threshold;

  public:
    ArrheniusIntegral( Real A_, Real Ea_ )
    {
      this->setA(A_);
      this->setEa(Ea_);
    }

    Real operator()( std::size_t N, Real const *t, Real const *T ) const
    {
      Real sum = 0;
      Real alpha = Ea/Constants::MKS::R;
      Real tolerance = 0.001/alpha;
      // we need to have some way for each thread to tell if they have cached the first
      // calculation or not.
      Real quadrature_last;
      bool have_last = false;

#define LOOP \
      for(size_t i = 1; i < N; ++i) \
      { \
        Real quadrature_now = T[i]*boost::math::expint(2,alpha/T[i] ); \
        if(!have_last) \
        { \
          quadrature_last = quadrature_now; \
          have_last = true; \
          continue; \
        } \
        if( abs(1/T[i] - 1/T[i-1]) > tolerance ) \
        { \
          sum += (quadrature_now - quadrature_last)*(t[i]-t[i-1])/(T[i] - T[i-1]); \
          quadrature_last = quadrature_now; \
        } \
        else \
        { \
          sum += exp(-alpha/T[i])*(t[i] - t[i-1]); \
        } \
      } \

      if(N < parallel_threshold)
      {
        LOOP
      }
      else
      {
        // we have to use a special reduction function here
        #pragma omp declare reduction( add:Real:omp_out=Integration::detail::add(omp_out, omp_in) )initializer(omp_priv=0)
        #pragma omp parallel for schedule(static) reduction(add:sum) firstprivate(have_last) private(quadrature_last)
        LOOP
      }
      sum *= A;
#undef LOOP
      return sum;
    }

};

}

#endif // include protector


#include <df/optimization/nonrigidIcp.h>

#include <df/optimization/deformationGraphRegularization.h>
#include <df/util/dualQuaternion.h>
#include <df/util/typeList.h>

#include <sophus/se3.hpp>

#include <Eigen/CholmodSupport>

#include <fstream> // TODO

namespace df {

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT>
Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> solveSparseLinearSystem(const NonrigidTransformer<Scalar,TransformT> & transformer,
                                                                  const Eigen::SparseMatrix<ScalarOpt> & JTJ,
                                                                  const Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> & JTr,
                                                                  const Scalar diagonalRegularization) {

    typedef Eigen::SparseMatrix<ScalarOpt> SparseMat;

    static Eigen::CholmodSimplicialLDLT<SparseMat, Eigen::Upper> solver;

    static uint lastTransformerSerialNumber = std::numeric_limits<uint>::max();

    const uint currentTransformerSerialNumber = transformer.serialNumber();

//    std::cout << "summed: " << std::endl;
//    std::cout << "max: " << JTJ.toDense().maxCoeff() << std::endl;
//    std::cout << "min: " << JTJ.toDense().minCoeff() << std::endl;

//    std::cout << JTJ.rows() << " x " << JTJ.cols() << std::endl;
//    std::cout << JTr.rows() << " x " << JTr.cols() << std::endl;

//    std::cout << "max: " << JTr.maxCoeff() << std::endl;
//    std::cout << "min: " << JTr.minCoeff() << std::endl;

    for (int k=0; k<JTJ.outerSize(); ++k) {
        for (typename SparseMat::InnerIterator it(JTJ,k); it; ++it) {
//            std::cout << it.row() << ", " << it.col() << std::endl;
            if (!std::isfinite(it.value())) {
                throw std::runtime_error(std::to_string(it.row()) + "," + std::to_string(it.col()) + " not finite (" + std::to_string(it.value()) + ")");
            }
        }
    }

    for (int r = 0; r < JTr.rows(); ++r) {
        if (!std::isfinite(JTr(r))) {
            throw std::runtime_error("row " + std::to_string(r) + " not finite (" + std::to_string(JTr(r)) + ")");
        }
    }

    // check if the transformer's serial number has changed.
    // if so, the deformation graph has been updated and we should
    // redo the pattern analysis.
    if (currentTransformerSerialNumber != lastTransformerSerialNumber) {

        solver.analyzePattern(JTJ);

        lastTransformerSerialNumber = currentTransformerSerialNumber;

    }

    solver.setShift(diagonalRegularization);

    solver.factorize(JTJ);

    return -solver.solve(JTr);

}

//template Eigen::VectorXd solveSparseLinearSystem(const NonrigidTransformer<double,DualQuaternion> &,
//                                                 const Eigen::SparseMatrix<double> &,
//                                                 const Eigen::VectorXd &,
//                                                 const double);

//template Eigen::VectorXd solveSparseLinearSystem(const NonrigidTransformer<double,Sophus::SE3Group> &,
//                                                 const Eigen::SparseMatrix<double> &,
//                                                 const Eigen::VectorXd &,
//                                                 const double);

template <typename Scalar, int Options, template <typename,int...> class TransformT>
inline TransformT<Scalar,Options> applyUpdate(TransformT<Scalar,Options> & transform,
                                      const TransformT<Scalar,Options> & update,
                                      const IntToType<internal::TransformUpdateLeftMultiply>) {

//    std::cout << "left update" << std::endl;
    return update*transform;

}

template <typename Scalar, int Options, template <typename,int...> class TransformT>
inline TransformT<Scalar,Options> applyUpdate(TransformT<Scalar,Options> & transform,
                                      const TransformT<Scalar,Options> & update,
                                      const IntToType<internal::TransformUpdateRightMultiply>) {

//    std::cout << "right update" << std::endl;
    return transform*update;

}

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT, internal::TransformUpdateMethod U = internal::TransformUpdateLeftMultiply>
void updateDeformationGraphTransforms(NonrigidTransformer<Scalar,TransformT> & transformer,
                                      const Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> & vectorizedUpdate) {

    typedef TransformT<Scalar> Transform;

    static constexpr uint BlockDim = 6;

    assert(vectorizedUpdate.rows() == BlockDim * transformer.numVerticesTotal());

    uint runningVertexCount = 0;

    for (uint level = 0; level < transformer.numRegularizationTreeLevels(); ++level) {

//        std::cout << "updating level " << level << std::endl;

        for (uint index = 0; index < transformer.numVerticesAtLevel(level); ++index) {

//            std::cout << "updating vertex " << index << std::endl;

//            std::cout << vectorizedUpdate.template block<6,1>((runningVertexCount + index)*BlockDim,0).transpose() << std::endl;

            // TODO: investigate an exponential mappint directly into the DualQuaternion space
            Transform update = Sophus::SE3Group<Scalar>::exp(vectorizedUpdate.template block<6,1>((runningVertexCount + index)*BlockDim,0).template cast<Scalar>());

            Transform & transform = transformer.transforms(level)[index];

            transform = applyUpdate(transform,update,IntToType<U>());

        }

        runningVertexCount += transformer.numVerticesAtLevel(level);

    }

}

//template void updateDeformationGraphTransforms(NonrigidTransformer<float,Sophus::SE3Group> &,
//                                               const Eigen::VectorXf &);

//template void updateDeformationGraphTransforms(NonrigidTransformer<float,DualQuaternion> &,
//                                               const Eigen::VectorXf &);

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT, internal::TransformUpdateMethod U = internal::TransformUpdateLeftMultiply>
void icpNonrigid(NonrigidTransformer<Scalar,TransformT> & transformer,
                 Eigen::SparseMatrix<ScalarOpt> & dataJTJ,
                 Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> & dataJTr,
                 const Scalar diagonalRegularization,
                 const Scalar regularizationWeight) {

    Eigen::SparseMatrix<ScalarOpt> regularizerJTJ(6*transformer.numVerticesTotal(),6*transformer.numVerticesTotal());
    Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> regularizerJTr(6*transformer.numVerticesTotal());

    std::cout << "compute regularizer normal equations:" << std::endl;
    computeRegularizerNormalEquations<Scalar,ScalarOpt,TransformT,U>(transformer,regularizerJTJ,regularizerJTr);

//    std::cout << regularizerJTJ.toDense() << std::endl;

    std::cout << "solve sparse linear system" << std::endl;
    const Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> solution = solveSparseLinearSystem<Scalar,ScalarOpt,TransformT>(transformer,
//                                                                                dataJTJ,
//                                                                                dataJTr,
                                                                                regularizationWeight*regularizerJTJ + dataJTJ,
                                                                                regularizationWeight*regularizerJTr + dataJTr,
//                                                                                regularizerJTJ,
//                                                                                regularizerJTr,
                                                                                diagonalRegularization);
    std::cout << "update deformation graph transforms" << std::endl;
    updateDeformationGraphTransforms<Scalar,ScalarOpt,TransformT,U>(transformer,solution);

}

template void icpNonrigid<float,double,Sophus::SE3Group,internal::TransformUpdateLeftMultiply>
    (NonrigidTransformer<float,Sophus::SE3Group> &,
     Eigen::SparseMatrix<double> &,
     Eigen::VectorXd &, float, float);

template void icpNonrigid<float,double,Sophus::SE3Group,internal::TransformUpdateRightMultiply>
    (NonrigidTransformer<float,Sophus::SE3Group> &,
     Eigen::SparseMatrix<double> &,
     Eigen::VectorXd &, float, float);

template void icpNonrigid<float,double,DualQuaternion,internal::TransformUpdateLeftMultiply>
    (NonrigidTransformer<float,DualQuaternion> &,
     Eigen::SparseMatrix<double> &,
     Eigen::VectorXd &, float, float);

template void icpNonrigid<float,double,DualQuaternion,internal::TransformUpdateRightMultiply>
    (NonrigidTransformer<float,DualQuaternion> &,
     Eigen::SparseMatrix<double> &,
     Eigen::VectorXd &, float, float);

template <typename Scalar, typename ScalarOpt, typename CameraModelT,
          template <typename,int...> class TransformT, int K,
          internal::TransformUpdateMethod U = internal::TransformUpdateLeftMultiply,
          typename ... DebugArgTs>
void nonrigidICP(const DeviceTensor2<Eigen::UnalignedVec3<Scalar> > & liveVertices,
                 DeviceTensor2<Eigen::UnalignedVec3<Scalar> > & predictedWarpedVertices,
                 DeviceTensor2<Eigen::UnalignedVec3<Scalar> > & predictedWarpedNormals,
                 const DeviceTensor2<Eigen::UnalignedVec4<Scalar> > & predictedCanonicalVertices,
                 const DeviceTensor2<Eigen::UnalignedVec4<Scalar> > & predictedCanonicalNormals,
                 const CameraModelT & cameraModel,
                 NonrigidTransformer<Scalar,TransformT> & transformer,
                 const Sophus::SE3Group<Scalar> & updatePredictionToLive,
                 const Eigen::Matrix<Scalar,2,1> & depthRange,
                 const uint numIterations,
                 const Scalar diagonalRegularization,
                 const Scalar regularizationWeight,
                 DebugArgTs ... debugArgs) {

    typedef Eigen::Triplet<ScalarOpt> Triplet;
    typedef Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> VecX;

    static constexpr int ModelDim = 6;

    const int numBaseLevelVertices = transformer.numVerticesAtLevel(0);

    const int numVerticesTotal = transformer.numVerticesTotal();

    for (uint iteration = 0; iteration < numIterations; ++iteration) {

        // -=-=-=- update prediction -=-=-=-
        transformer.template warpMesh<K,2,4>(predictedWarpedVertices, predictedWarpedNormals,
                                             predictedCanonicalVertices, predictedCanonicalNormals);

        // -=-=-=- data term -=-=-=-
        std::vector<Triplet> JTJTriplets;
        VecX dataJTr;

        internal::computeDataNormalEquations<Scalar,ScalarOpt,CameraModelT,TransformT,K,U,DebugArgTs...>(
                    liveVertices,
                    predictedWarpedVertices, predictedWarpedNormals,
                    predictedCanonicalVertices, predictedCanonicalNormals,
                    cameraModel, transformer, updatePredictionToLive, depthRange,
                    JTJTriplets, dataJTr, debugArgs...);

        Eigen::SparseMatrix<ScalarOpt> dataJTJ(numVerticesTotal * ModelDim, numVerticesTotal * ModelDim);

        dataJTJ.setFromTriplets(JTJTriplets.begin(), JTJTriplets.end());

        dataJTJ.finalize();

        //    std::cout << "data: " << std::endl;

        //    std::ofstream dataJTJStream("/tmp/dataJTJ.txt");
        //    dataJTJStream << dataJTJ.toDense() << std::endl;
        //    std::cout << "max: " << dataJTJ.toDense().maxCoeff() << std::endl;
        //    std::cout << "min: " << dataJTJ.toDense().minCoeff() << std::endl;

        //    std::cout << dataJTJ.coeffRef(588,588) << std::endl;

        // -=-=-=- regularization term -=-=-=-

        Eigen::SparseMatrix<ScalarOpt> regularizerJTJ(ModelDim * numVerticesTotal,ModelDim * numVerticesTotal);
        Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> regularizerJTr(ModelDim * numVerticesTotal);

        computeRegularizerNormalEquations<Scalar,ScalarOpt,TransformT,U>(transformer,regularizerJTJ,regularizerJTr);

        //    std::cout << "reg: " << std::endl;

        //    std::ofstream regJTJStream("/tmp/regJTJ.txt");
        //    regJTJStream << regularizerJTJ.toDense() << std::endl;

        //    std::cout << "max: " << regularizerJTJ.toDense().maxCoeff() << std::endl;
        //    std::cout << "min: " << regularizerJTJ.toDense().minCoeff() << std::endl;

        //    std::cout << regularizerJTJ.coeffRef(588,588) << std::endl;

        // -=-=-=- solution -=-=-=-
        Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> fullDataJTr(ModelDim * numVerticesTotal);

        fullDataJTr.head(ModelDim*numBaseLevelVertices) = dataJTr;

        fullDataJTr.tail(ModelDim*(numVerticesTotal - numBaseLevelVertices)) = Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1>::Zero( ModelDim*(numVerticesTotal - numBaseLevelVertices),1);

        const Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> solution = solveSparseLinearSystem<Scalar,ScalarOpt,TransformT>(transformer,
                                                                                regularizationWeight*regularizerJTJ + dataJTJ,
                                                                                regularizationWeight*regularizerJTr + fullDataJTr,
                                                                                diagonalRegularization);

        // -=-=-=- solution -=-=-=-
        updateDeformationGraphTransforms<Scalar,ScalarOpt,TransformT,U>(transformer,solution);

    }

}

#define NONRIGID_ICP_EXPLICIT_INSTANTIATION(type, opt_type, camera, transform, K, update)                                       \
template void nonrigidICP<type,opt_type,camera##CameraModel<type>,transform, K, internal::TransformUpdate##update##Multiply>(   \
    const DeviceTensor2<Eigen::UnalignedVec3<type>  > &,                                                                        \
    DeviceTensor2<Eigen::UnalignedVec3<type> > &,                                                                               \
    DeviceTensor2<Eigen::UnalignedVec3<type> > &,                                                                               \
    const DeviceTensor2<Eigen::UnalignedVec4<type>  > &,                                                                        \
    const DeviceTensor2<Eigen::UnalignedVec4<type>  > &,                                                                        \
    const camera##CameraModel<type> &,                                                                                \
    NonrigidTransformer<type,transform> &,                                                                                \
    const Sophus::SE3Group<type> &,                                                                                \
    const Eigen::Matrix<type,2,1> &,                                                                                \
    const uint, const type, const type);                                                                                \
                                                                                \
template void nonrigidICP<type,opt_type,camera##CameraModel<type>,transform, K, internal::TransformUpdate##update##Multiply, DeviceTensor2<Eigen::UnalignedVec4<uchar> > >(   \
    const DeviceTensor2<Eigen::UnalignedVec3<type>  > &,                                                                        \
    DeviceTensor2<Eigen::UnalignedVec3<type> > &,                                                                               \
    DeviceTensor2<Eigen::UnalignedVec3<type> > &,                                                                               \
    const DeviceTensor2<Eigen::UnalignedVec4<type>  > &,                                                                        \
    const DeviceTensor2<Eigen::UnalignedVec4<type>  > &,                                                                        \
    const camera##CameraModel<type> &,                                                                                \
    NonrigidTransformer<type,transform> &,                                                                                \
    const Sophus::SE3Group<type> &,                                                                                \
    const Eigen::Matrix<type,2,1> &,                                                                                \
    const uint, const type, const type,                                                                                \
    DeviceTensor2<Eigen::UnalignedVec4<uchar> > )

NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Poly3, DualQuaternion, 4, Left);
NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Poly3, DualQuaternion, 4, Right);

NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Poly3, Sophus::SE3Group, 4, Left);
NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Poly3, Sophus::SE3Group, 4, Right);

NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Linear, DualQuaternion, 4, Left);
NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Linear, DualQuaternion, 4, Right);

NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Linear, Sophus::SE3Group, 4, Left);
NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Linear, Sophus::SE3Group, 4, Right);

} // namespace df


#include "truncated-svd-solver/marginalization.h"

#include <cmath>

#include <cholmod.h>
#include <Eigen/Dense>
#include <glog/logging.h>
#include <spqr.hpp>
#include <SuiteSparseQR.hpp>

#include "truncated-svd-solver/linear-algebra-helpers.h"

namespace truncated_svd_solver {

Eigen::MatrixXd marginalJacobian(cholmod_sparse* J_x, cholmod_sparse*
    J_thetat, cholmod_common* cholmod) {
  // Compute the QR factorization of J_x.
  SuiteSparseQR_factorization<double>* QR = SuiteSparseQR_factorize<double>(
    SPQR_ORDERING_BEST, SPQR_DEFAULT_TOL, J_x, cholmod);
  CHECK(QR != nullptr) << "SuiteSparseQR_factorize failed.";

  // Compute the Jacobian of the reduced system.
  cholmod_sparse* J_thetatQFull = SuiteSparseQR_qmult<double>(SPQR_XQ, QR,
    J_thetat, cholmod);
  CHECK(QR != nullptr) << "SuiteSparseQR_qmult failed.";

  std::ptrdiff_t* col_indices = new std::ptrdiff_t[J_x->ncol];
  for (size_t i = 0; i < J_x->ncol; ++i)
   col_indices[i] = i;
  cholmod_sparse* J_thetatQ = cholmod_l_submatrix(J_thetatQFull, NULL, -1,
    col_indices, J_x->ncol, 1, 0, cholmod);
  delete [] col_indices;
  CHECK(J_thetatQ != nullptr) << "cholmod_l_submatrix failed.";

  cholmod_sparse* J_thetat2 = cholmod_l_aat(J_thetat, NULL, 0, 1, cholmod);
  CHECK(J_thetat2 != nullptr) << "cholmod_l_aat failed.";

  cholmod_sparse* J_thetatQ2 = cholmod_l_aat(J_thetatQ, NULL, 0, 1,
    cholmod);
  CHECK(J_thetatQ2 != nullptr) << "cholmod_l_aat failed.";

  double alpha[2];
  alpha[0] = 1;
  double beta[2];
  beta[0] = -1;
  cholmod_sparse* Omega = cholmod_l_add(J_thetat2, J_thetatQ2, alpha, beta,
    1, 0, cholmod);
  CHECK(Omega != nullptr) << "cholmod_l_add failed.";

  Eigen::MatrixXd OmegaDense;
  cholmodSparseToEigenDenseCopy(Omega, OmegaDense);

  // Clean allocated memory.
  SuiteSparseQR_free(&QR, cholmod);
  cholmod_l_free_sparse(&J_thetatQFull, cholmod);
  cholmod_l_free_sparse(&J_thetatQ, cholmod);
  cholmod_l_free_sparse(&J_thetat2, cholmod);
  cholmod_l_free_sparse(&J_thetatQ2, cholmod);
  cholmod_l_free_sparse(&Omega, cholmod);

  return OmegaDense;
}

double marginalize(cholmod_sparse* Jt, size_t j, Eigen::MatrixXd& NS,
                   Eigen::MatrixXd& CS, Eigen::MatrixXd& Sigma,
                   Eigen::MatrixXd& SigmaP, Eigen::MatrixXd& Omega,
                   double norm_tol, double eps_tol) {
  CHECK_NOTNULL(Jt);

  // Initialize cholmod.
  cholmod_common cholmod;
  cholmod_l_start(&cholmod);

  // Convert to cholmod_sparse.
  cholmod_sparse* J = cholmod_l_transpose(Jt, 1, &cholmod);
  CHECK(J != nullptr) << "cholmod_l_transpose failed.";

  // Extract the part corresponding to the state/landmarks/...
  std::ptrdiff_t* col_indices = new std::ptrdiff_t[j];
  for (size_t i = 0; i < j; ++i)
   col_indices[i] = i;
  cholmod_sparse* J_x = cholmod_l_submatrix(J, NULL, -1, col_indices, j, 1,
    0, &cholmod);
  delete [] col_indices;
  CHECK(J_x != nullptr) << "cholmod_l_submatrix failed.";

  // Extract the part corresponding to the calibration parameters.
  col_indices = new std::ptrdiff_t[J->ncol - j];
  for (size_t i = j; i < J->ncol; ++i)
   col_indices[i - j] = i;
  cholmod_sparse* J_theta = cholmod_l_submatrix(J, NULL, -1, col_indices,
    J->ncol - j, 1, 0, &cholmod);
  delete [] col_indices;
  CHECK(J_theta != nullptr) << "cholmod_l_submatrix failed.";

  cholmod_sparse* J_thetat = cholmod_l_transpose(J_theta, 1, &cholmod);
  CHECK(J_thetat != nullptr) << "cholmod_l_transpose failed.";

  // Compute the marginal Jacobian.
  Omega = marginalJacobian(J_x, J_thetat, &cholmod);

  // Scale J_x.
  cholmod_dense* G_x = cholmod_l_allocate_dense(J_x->ncol, 1, J_x->ncol,
    CHOLMOD_REAL, &cholmod);
  CHECK(G_x != nullptr) << "cholmod_l_allocate_dense failed.";
  double* values = reinterpret_cast<double*>(G_x->x);

  for (size_t j = 0; j < J_x->ncol; ++j) {
    const double norm_col = colNorm(J_x, j);
    if (norm_col < norm_tol)
      values[j] = 0.0;
    else
      values[j] = 1.0 / norm_col;
  }

  bool success = cholmod_l_scale(G_x, CHOLMOD_COL, J_x, &cholmod);
  CHECK(success) << "cholmod_l_scale failed.";
  cholmod_l_free_dense(&G_x, &cholmod) ;

  // Scale J_thetat.
  cholmod_dense* G_theta = cholmod_l_allocate_dense(J_theta->ncol, 1,
    J_theta->ncol, CHOLMOD_REAL, &cholmod);
  CHECK(G_theta != nullptr) << "cholmod_l_allocate_dense failed.";
  values = reinterpret_cast<double*>(G_theta->x);

  for (size_t j = 0; j < J_theta->ncol; ++j) {
    const double normCol = colNorm(J_theta, j);
    if (normCol < norm_tol)
      values[j] = 0.0;
    else
      values[j] = 1.0 / normCol;
  }

  success = cholmod_l_scale(G_theta, CHOLMOD_ROW, J_thetat, &cholmod);
  CHECK(success) << "cholmod_l_scale failed.";
  cholmod_l_free_dense(&G_theta, &cholmod);

  // Compute the scaled marginal Jacobian.
  Eigen::MatrixXd OmegaScaled;
  OmegaScaled = marginalJacobian(J_x, J_thetat, &cholmod);

  // Cleanup cholmod.
  cholmod_l_free_sparse(&J, &cholmod);
  cholmod_l_free_sparse(&J_x, &cholmod);
  cholmod_l_free_sparse(&J_theta, &cholmod);
  cholmod_l_free_sparse(&J_thetat, &cholmod);
  cholmod_l_finish(&cholmod);

  // Compute the thin SVD of OmegaScaled.
  const Eigen::JacobiSVD<Eigen::MatrixXd> svd_scaled(OmegaScaled,
    Eigen::ComputeThinU | Eigen::ComputeThinV);

  // Compute the numerical rank.
  size_t nrank = OmegaScaled.cols();
  const Eigen::VectorXd& SScaled = svd_scaled.singularValues();
  const double tol = OmegaScaled.rows() * SScaled(0) * eps_tol;
  for (std::ptrdiff_t i = OmegaScaled.cols() - 1; i > 0; --i) {
    if (SScaled(i) > tol) {
      break;
    } else {
      nrank--;
    }
  }

  // Compute the thin SVD of Omega.
  const Eigen::JacobiSVD<Eigen::MatrixXd> svd(Omega,
    Eigen::ComputeThinU | Eigen::ComputeThinV);
  const Eigen::MatrixXd& V = svd.matrixV();

  // compute the numerical column space
  CS = V.block(0, 0, V.rows(), nrank);

  // compute the numerical null space
  NS = V.block(0, nrank, V.rows(), Omega.cols() - nrank);

  // compute the projected covariance matrix
  Eigen::MatrixXd inv_S(Eigen::MatrixXd::Zero(Omega.cols(), Omega.cols()));
  SigmaP = Eigen::MatrixXd::Zero(nrank, nrank);
  double sv_log_sum = 0.0;
  const Eigen::VectorXd& S = svd.singularValues();
  for (size_t i = 0u; i < nrank; ++i) {
    SigmaP(i, i) = 1.0 / S(i);
    sv_log_sum = sv_log_sum + log2(S(i));
    inv_S(i, i) = SigmaP(i, i);
  }

  // compute the covariance matrix
  Sigma = V * inv_S * V.transpose();
  return sv_log_sum;
}

}  // namespace truncated_svd_solver


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_FREXP_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_FREXP_HPP_INCLUDED

#include <boost/simd/ieee/functions/frexp.hpp>
#include <boost/simd/include/functions/scalar/is_invalid.hpp>
#include <boost/simd/include/functions/scalar/bitwise_notand.hpp>
#include <boost/simd/include/functions/scalar/bitwise_and.hpp>
#include <boost/simd/include/functions/scalar/bitwise_or.hpp>
#include <boost/simd/include/functions/scalar/shri.hpp>
#include <boost/simd/include/functions/scalar/minus.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::frexp_
                                    , tag::cpu_
                                    , (A0)(A1)
                                    , (scalar_< floating_<A0> >)
                                      (scalar_< floating_<A0> >)
                                      (scalar_< integer_<A1>  >)
                                    )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0,A0 & r0,A1& r1) const
    {
      if (a0 == 0 || is_invalid(a0))
      {
        r0 = a0;
        r1 = Zero<A1>();
      }
      else
      {
        typedef typename dispatch::meta::as_integer<A0, signed>::type int_type;
        const int_type me = Maxexponent<A0>()-1;
        const int_type nmb= Nbmantissabits<A0>();
        const int_type n1 = ((2*me+3)<<nmb);
        const int_type n2 = me<<nmb;

        A0 ci = simd::bitwise_cast<A0>(n1);
           r1 = simd::bitwise_cast<int_type>(b_and(ci, a0));  // extract exp.
        A0 x  = b_notand(ci, a0);                             // clear exp. in a0
           r1 = sub(shri(r1,nmb), me);                        // compute exp.
           r0 = b_or(x,n2);                                   // insert exp.+1 in x

        if (r1 > me)
        {
          r1   = 0;
          r0  += a0;
        }
      }
    }
  };
} } }

#endif