//
// Created by asem on 14/08/18.
//

#ifndef MARKOVIAN_FEATURES_DLIB_UTILITIES_HPP
#define MARKOVIAN_FEATURES_DLIB_UTILITIES_HPP

#include <dlib/matrix.h>

namespace dlib_utilities {

template<typename T>
struct column_matrix_like
{
    /*!
        This object defines a matrix expression that holds a reference to a std::vector<T>
        and makes it look like a column vector.  Thus it enables you to use a std::vector
        as if it was a dlib::matrix.
    !*/
    explicit column_matrix_like( std::vector<T> vect_ ) : _vect( std::move( vect_ ))
    {}

    template<typename MatrixExpression>
    explicit column_matrix_like( MatrixExpression exp )
    {
        _vect.reserve( exp.size());
        _vect.insert( _vect.end(), exp.begin(), exp.end());
    }

    template<typename MatrixExpression>
    column_matrix_like &operator=( MatrixExpression exp )
    {
        _vect.clear();
        _vect.reserve( exp.nr());
        _vect.insert( _vect.end(), exp.cbegin(), exp.cend());
        return *this;
    }

    // This expression wraps direct memory accesses so we use the lowest possible cost.
    const static long cost = 1;

    const static long NR = 0; // We don't know the length of the vector until runtime.  So we put 0 here.
    const static long NC = 1; // We do know that it only has one column (since it's a vector)
    typedef T type;
    // Since the std::vector doesn't use a dlib memory manager we list the default one here.
    typedef dlib::default_memory_manager mem_manager_type;
    // The layout type also doesn't really matter in this case.  So we list row_major_layout
    // since it is a good default.
    typedef dlib::row_major_layout layout_type;

    // Note that we define const_ret_type to be a reference type.  This way we can
    // return the contents of the std::vector by reference.
    typedef const T &const_ret_type;

    const_ret_type apply(
            long r,
            long
    ) const
    { return _vect[r]; }

    long nr() const
    { return _vect.size(); }

    long nc() const
    { return 1; }

    std::vector<T> steal_vector()
    {
        return std::move( _vect );
    }

    // This expression never aliases anything since it doesn't contain any matrix expression (it
    // contains only a std::vector which doesn't count since you can't assign a matrix expression
    // to a std::vector object).
    template<typename U>
    bool aliases( const dlib::matrix_exp<U> & ) const
    { return false; }

    template<typename U>
    bool destructively_aliases( const dlib::matrix_exp<U> & ) const
    { return false; }

private:
    std::vector<T> _vect;
};

template<typename T>
struct row_matrix_like
{
    /*!
        This object defines a matrix expression that holds a reference to a std::vector<T>
        and makes it look like a column vector.  Thus it enables you to use a std::vector
        as if it was a dlib::matrix.
    !*/
    explicit row_matrix_like( std::vector<T> vect_ ) : _vect( std::move( vect_ ))
    {}

    template<typename MatrixExpression>
    explicit row_matrix_like( MatrixExpression exp )
    {
        _vect.reserve( exp.size());
        _vect.insert( _vect.end(), exp.begin(), exp.end());
    }

    template<typename MatrixExpression>
    row_matrix_like &operator=( MatrixExpression exp )
    {
        _vect.clear();
        _vect.reserve( exp.size());
        _vect.insert( _vect.end(), exp.cbegin(), exp.cend());
        return *this;
    }

    // This expression wraps direct memory accesses so we use the lowest possible cost.
    const static long cost = 1;

    const static long NR = 1; // We don't know the length of the vector until runtime.  So we put 0 here.
    const static long NC = 0; // We do know that it only has one column (since it's a vector)
    typedef T type;
    // Since the std::vector doesn't use a dlib memory manager we list the default one here.
    typedef dlib::default_memory_manager mem_manager_type;
    // The layout type also doesn't really matter in this case.  So we list row_major_layout
    // since it is a good default.
    typedef dlib::row_major_layout layout_type;

    // Note that we define const_ret_type to be a reference type.  This way we can
    // return the contents of the std::vector by reference.
    typedef const T &const_ret_type;

    const_ret_type apply(
            long ,
            long c
    ) const
    { return _vect[c]; }

    long nr() const
    { return 1; }

    long nc() const
    { return _vect.size(); }

    std::vector<T> steal_vector()
    {
        return std::move( _vect );
    }

    // This expression never aliases anything since it doesn't contain any matrix expression (it
    // contains only a std::vector which doesn't count since you can't assign a matrix expression
    // to a std::vector object).
    template<typename U>
    bool aliases( const dlib::matrix_exp<U> & ) const
    { return false; }

    template<typename U>
    bool destructively_aliases( const dlib::matrix_exp<U> & ) const
    { return false; }

private:
    std::vector<T> _vect;
};

template<typename InputContainer>
auto vector_to_column_matrix_like( InputContainer &&container )
{
    using T = typename std::remove_reference_t<InputContainer>::value_type;
    using MatrixLike = column_matrix_like<T>;

    std::vector<T> delegate = std::forward<InputContainer>( container );
    return dlib::matrix_op<MatrixLike>( MatrixLike( std::move( delegate )));
}

template<typename InputContainer>
auto vector_to_row_matrix_like( InputContainer &&container )
{
    using T = typename std::remove_reference_t<InputContainer>::value_type;
    using MatrixLike = row_matrix_like<T>;

    std::vector<T> delegate = std::forward<InputContainer>( container );
    return dlib::matrix_op<MatrixLike>( MatrixLike( std::move( delegate )));
}

}
#endif //MARKOVIAN_FEATURES_DLIB_UTILITIES_HPP


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * 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 <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "SiconosVector.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"
#include "SiconosException.hpp"

using namespace Siconos;

//=============================
// Elements access (get or set)
//=============================

double SimpleMatrix::getValue(unsigned int row, unsigned int col) const
{
  if(row >= size(0) || col >= size(1))
    THROW_EXCEPTION("Index out of range");

  if(_num == Siconos::DENSE)
    return (*mat.Dense)(row, col);
  else if(_num == Siconos::TRIANGULAR)
    return (*mat.Triang)(row, col);
  else if(_num == Siconos::SYMMETRIC)
    return (*mat.Sym)(row, col);
  else if(_num == Siconos::SPARSE)
  {
    double * d = (*mat.Sparse).find_element(row, col);
    if(d)
      return *d;
    else
      return 0.0;
  }
  else if(_num == Siconos::SPARSE_COORDINATE)
  {
    double * d = (*mat.SparseCoordinate).find_element(row, col);
    if(d)
      return *d;
    else
      return 0.0;
  }
  else if(_num == Siconos::BANDED)
    return (*mat.Banded)(row, col);
  else if(_num == Siconos::ZERO)
    return 0;
  else //if (_num == Siconos::IDENTITY)
    return(row == col);
}

void SimpleMatrix::setValue(unsigned int row, unsigned int col, double value)
{
  if(row >= size(0) || col >= size(1))
    THROW_EXCEPTION("Index out of range");

  if(_num == Siconos::DENSE)
    (*mat.Dense)(row, col) = value;
  else if(_num == Siconos::TRIANGULAR)
    (*mat.Triang)(row, col) = value;
  else if(_num == Siconos::SYMMETRIC)
    (*mat.Sym)(row, col) = value ;
  else if(_num == Siconos::SPARSE)
  {
    double * d = (*mat.Sparse).find_element(row, col);
    if(d)
    {
      *d = value;
    }
    else
    {
      (*mat.Sparse).insert_element(row, col, value);
    }
  }
  else if(_num == Siconos::SPARSE_COORDINATE)
  {
    // double * d = (*mat.Sparse).find_element(row, col);
    // if (d)
    // {
    //   *d = value;
    // }
    // else
    // {
    (*mat.SparseCoordinate).insert_element(row, col, value);
    // }
  }

  else if(_num == Siconos::BANDED)
    (*mat.Banded)(row, col) = value;
  else if(_num == Siconos::ZERO || _num == Siconos::IDENTITY)
    THROW_EXCEPTION("orbidden for Identity or Zero type matrices.");
  resetFactorizationFlags();

}

//============================================
// Access (get or set) to blocks of elements
//============================================

void SimpleMatrix::setBlock(unsigned int row_min, unsigned int col_min, const SiconosMatrix& m)
{
  // Set current matrix elements, starting from row row_min and column col_min, with the values of the matrix m.
  // m may be a BlockMatrix.

  if(&m == this)
    THROW_EXCEPTION("m = this.");

  if(row_min >= size(0))
    THROW_EXCEPTION("row is out of range");

  if(col_min >= size(1))
    THROW_EXCEPTION("col is out of range");

  unsigned int row_max, col_max;
  row_max = m.size(0) + row_min;
  col_max = m.size(1) + col_min;

  if(row_max > size(0))
    THROW_EXCEPTION("row is out of range.");

  if(col_max > size(1))
    THROW_EXCEPTION("m.col + col is out of range.");

  Siconos::UBLAS_TYPE numM = m.num();

  if(numM == Siconos::BLOCK)  // if m is a block matrix ...
  {
    const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
    BlocksMat::const_iterator1 it;
    BlocksMat::const_iterator2 it2;
    unsigned int posRow = row_min;
    unsigned int posCol = col_min;

    for(it = mB._mat->begin1(); it != mB._mat->end1(); ++it)
    {
      for(it2 = it.begin(); it2 != it.end(); ++it2)
      {
        setBlock(posRow, posCol, **it2);
        posCol += (*it2)->size(1);
      }
      posRow += (*it)->size(0);
      posCol = col_min;
    }
  }
  else // if m is a SimpleMatrix
  {
    if(numM != _num)
      THROW_EXCEPTION("Inconsistent matrix types.");

    if(_num == Siconos::DENSE)
      noalias(ublas::subrange(*mat.Dense, row_min, row_max, col_min, col_max)) = *(m.dense());
    else if(_num == Siconos::TRIANGULAR)
      noalias(ublas::subrange(*mat.Triang, row_min, row_max, col_min, col_max)) = *(m.triang());
    else if(_num == Siconos::SYMMETRIC)
      noalias(ublas::subrange(*mat.Sym, row_min, row_max, col_min, col_max)) = *(m.sym());
    else if(_num == Siconos::SPARSE)
      noalias(ublas::subrange(*mat.Sparse, row_min, row_max, col_min, col_max)) = *(m.sparse());
    else if(_num == Siconos::BANDED)
      noalias(ublas::subrange(*mat.Banded, row_min, row_max, col_min, col_max)) = *(m.banded());
    else // if(_num == Siconos::ZERO) or _num == Siconos::IDENTITY nothing to do
    {}
    resetFactorizationFlags();
  }
}

void SimpleMatrix::getRow(unsigned int r, SiconosVector &vOut) const
{
  // Get row number r of current matrix and copy it into vOut.
  if(r >= size(0))
    THROW_EXCEPTION("row is out of range");

  if(vOut.size() != size(1))
    THROW_EXCEPTION("inconsistent sizes between this and v.");

  if(_num == Siconos::IDENTITY)  // identity matrix
  {
    vOut.zero();
    vOut(r) = 1.0;
  }
  else if(_num == Siconos::ZERO)  // Zero matrix
    vOut.zero();
  else
  {
    Siconos::UBLAS_TYPE numV = vOut.num();
    if(numV == Siconos::DENSE)
    {
      if(_num == Siconos::DENSE)
      {
        noalias(*(vOut.dense())) = ublas::row(*mat.Dense, r);
      }
      else if(_num == Siconos::TRIANGULAR)
      {
        noalias(*(vOut.dense())) = ublas::row(*mat.Triang, r);
      }
      else if(_num == Siconos::SYMMETRIC)
      {
        noalias(*(vOut.dense())) = ublas::row(*mat.Sym, r);
      }
      else if(_num == Siconos::SPARSE)
      {
        noalias(*(vOut.dense())) = ublas::row(*mat.Sparse, r);
      }
      else //if(_num == Siconos::BANDED){
        noalias(*(vOut.dense())) = ublas::row(*mat.Banded, r);
    }
    else // if numV == Siconos::SPARSE
    {
      if(_num == Siconos::SPARSE)
      {
        noalias(*(vOut.sparse())) = ublas::row(*mat.Sparse, r);
      }
      else
        THROW_EXCEPTION("inconsistent types between this (not sparse) and v (sparse).");
    }
  }
}

void SimpleMatrix::setRow(unsigned int r, const SiconosVector& vIn)
{
  // Set row number r of current matrix with vIn.
  Siconos::UBLAS_TYPE numV = vIn.num();
  if(r >= size(0))
    THROW_EXCEPTION("row is out of range");

  if(vIn.size() != size(1))
    THROW_EXCEPTION("inconsistent sizes between this and v.");

  if(_num == Siconos::ZERO || _num == Siconos::IDENTITY)
    THROW_EXCEPTION("current matrix is read-only (zero or identity).");

  {
    if(_num == Siconos::DENSE)
    {
      if(numV == Siconos::DENSE)
      {
        noalias(ublas::row(*mat.Dense, r)) = *vIn.dense();
      }
      else if(numV == Siconos::SPARSE)
      {
        noalias(ublas::row(*mat.Dense, r)) = *vIn.sparse();
      }
    }
    else if(_num == Siconos::SPARSE && numV == Siconos::SPARSE)
      noalias(ublas::row(*mat.Sparse, r)) = *vIn.sparse();
    else
      THROW_EXCEPTION("inconsistent types between current matrix and v.");
  }

  resetFactorizationFlags();
}

void SimpleMatrix::getCol(unsigned int r, SiconosVector &vOut)const
{
  // Get column number r of current matrix and copy it into vOut.
  if(r >= size(1))
    THROW_EXCEPTION("col is out of range");

  if(vOut.size() != size(0))
    THROW_EXCEPTION("inconsistent sizes between this and v.");

  if(_num == Siconos::IDENTITY)  // identity matrix
  {
    vOut.zero();
    vOut(r) = 1.0;
  }
  else if(_num == Siconos::ZERO)  // Zero matrix
    vOut.zero();
  else
  {
    Siconos::UBLAS_TYPE numV = vOut.num();

    if(numV == Siconos::DENSE)
    {

      if(_num == Siconos::DENSE)
      {
        noalias(*(vOut.dense())) = ublas::column(*mat.Dense, r);
      }
      else if(_num == Siconos::TRIANGULAR)
      {
        noalias(*(vOut.dense())) = ublas::column(*mat.Triang, r);
      }
      else if(_num == Siconos::SYMMETRIC)
      {
        noalias(*(vOut.dense())) = ublas::column(*mat.Sym, r);
      }
      else if(_num == Siconos::SPARSE)
      {
        noalias(*(vOut.dense())) = ublas::column(*mat.Sparse, r);
      }
      else //if(_num == Siconos:BANDED){
        noalias(*(vOut.dense())) = ublas::column(*mat.Banded, r);
    }
    else // if _numV == Siconos::SPARSE
    {
      if(_num == Siconos::SPARSE)
      {
        noalias(*(vOut.sparse())) = ublas::column(*mat.Sparse, r);
      }
      else
        THROW_EXCEPTION("inconsistent types between this (not sparse) and v (sparse).");
    }
  }
}

void SimpleMatrix::setCol(unsigned int r, const SiconosVector &vIn)
{
  // Set column number r of current matrix with vIn.
  Siconos::UBLAS_TYPE numV = vIn.num();
  if(r >= size(1))
    THROW_EXCEPTION("col is out of range");

  if(vIn.size() != size(0))
    THROW_EXCEPTION("inconsistent sizes between this and v.");

  if(_num == Siconos::ZERO || _num == Siconos::IDENTITY)
    THROW_EXCEPTION("current matrix is read-only (zero or identity).");

  {
    if(_num == Siconos::DENSE)
    {
      if(numV == Siconos::DENSE)
      {
        noalias(ublas::column(*mat.Dense, r)) = *vIn.dense();
      }
      else if(numV == Siconos::SPARSE)
      {
        noalias(ublas::column(*mat.Dense, r)) = *vIn.sparse();
      }
    }
    else if(_num == Siconos::SPARSE && numV == Siconos::SPARSE)
      noalias(ublas::column(*mat.Sparse, r)) = *vIn.sparse();
    else
      THROW_EXCEPTION("nconsistent types between current matrix and v.");
  }

  resetFactorizationFlags();
}

void SimpleMatrix::getSubRow(unsigned int r, unsigned int pos, SP::SiconosVector vOut) const
{
  // Get row number r of current matrix, starting from element at position pos, and copy it into vOut.
  if(r >= size(0))
    THROW_EXCEPTION("row is out of range");

  if(vOut->size() > size(1) - pos)
    THROW_EXCEPTION("inconsistent sizes between this and v.");

  if(_num == Siconos::IDENTITY)  // identity matrix
  {
    vOut->zero();
    if(r >= pos)
      (*vOut)(r - pos) = 1.0;
  }
  else if(_num == Siconos::ZERO)  // Zero matrix
    vOut->zero();
  else
  {
    Siconos::UBLAS_TYPE numV = vOut->num();
    unsigned int nbEl = vOut->size();

    if(numV == Siconos::DENSE)
    {
      if(_num == Siconos::DENSE)
      {
        //      noalias(*(vOut->dense())) = ublas::row(ublas::subrange(*mat.Dense, r, r+1,pos, endPos),0);
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
      }
      else if(_num == Siconos::TRIANGULAR)
      {
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<TriangMat >(*mat.Triang, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
      }
      else if(_num == Siconos::SYMMETRIC)
      {
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SymMat >(*mat.Sym, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
      }
      else if(_num == Siconos::SPARSE)
      {
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
      }
      else //if(_num == Siconos::BANDED){
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<BandedMat >(*mat.Banded, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
    }
    else // if numV == Siconos::SPARSE
    {
      if(_num == Siconos::SPARSE)
      {
#ifdef BOOST_LIMITATION
        THROW_EXCEPTION("ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
#else
        noalias(*(vOut->sparse())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
#endif
      }
      else
        THROW_EXCEPTION("inconsistent types between this (not sparse) and v (sparse).");
    }
  }

}

void SimpleMatrix::setSubRow(unsigned int r, unsigned int pos, SP::SiconosVector vIn)
{
  // Set row number r, starting from element at position pos, of current matrix with vIn.
  Siconos::UBLAS_TYPE numV = vIn->num();
  if(r >= size(0))
    THROW_EXCEPTION("row is out of range");

  if(vIn->size() > size(1) - pos)
    THROW_EXCEPTION("inconsistent sizes between this and v.");

  if(_num == Siconos::ZERO || _num == Siconos::IDENTITY)
    THROW_EXCEPTION("current matrix is read-only (zero or identity).");

  {
    unsigned int nbEl = vIn->size();
    if(_num == Siconos::DENSE)
    {
      if(numV == Siconos::DENSE)
      {
        noalias(ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl))) = *vIn->dense();
      }
      else if(numV == Siconos::SPARSE)
      {
        ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl)) = *vIn->sparse();
      }
    }
    else if(_num == Siconos::SPARSE && numV == Siconos::SPARSE)
#ifdef BOOST_LIMITATION
      THROW_EXCEPTION("ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
#else
      ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl)) = *vIn->sparse();
#endif
    else
      THROW_EXCEPTION("inconsistent types between current matrix and v.");
    resetFactorizationFlags();
  }

}

void SimpleMatrix::getSubCol(unsigned int r, unsigned int pos, SP::SiconosVector vOut) const
{
  // Get col _number r of current matrix, starting from element at position pos, and copy it into vOut.
  if(r >= size(1))
    THROW_EXCEPTION("col is out of range");

  if(vOut->size() > size(0) - pos)
    THROW_EXCEPTION("inconsistent sizes between this and v.");

  if(_num == Siconos::IDENTITY)  // identity matrix
  {
    vOut->zero();
    if(r >= pos)
      (*vOut)(r - pos) = 1.0;
  }
  else if(_num == Siconos::ZERO)  // Zero matrix
    vOut->zero();
  else
  {
    Siconos::UBLAS_TYPE numV = vOut->num();
    unsigned int nbEl = vOut->size();

    if(numV == Siconos::DENSE)
    {
      if(_num == Siconos::DENSE)
      {
        //      noalias(*(vOut->dense())) = ublas::row(ublas::subrange(*mat.Dense, r, r+1,pos, endPos),0);
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
      }
      else if(_num == Siconos::TRIANGULAR)
      {
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<TriangMat >(*mat.Triang, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
      }
      else if(_num == Siconos::SYMMETRIC)
      {
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SymMat >(*mat.Sym, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
      }
      else if(_num == Siconos::SPARSE)
      {
#ifdef BOOST_LIMITATION
        THROW_EXCEPTION("ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
#else
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
#endif
      }
      else //if(_num == Siconos::BANDED){
        noalias(*(vOut->dense())) = ublas::matrix_vector_slice<BandedMat >(*mat.Banded, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
    }
    else // if numV == Siconos::SPARSE
    {
      if(_num == Siconos::SPARSE)
      {
#ifdef BOOST_LIMITATION
        THROW_EXCEPTION("ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
#else
        noalias(*(vOut->sparse())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
#endif
      }
      else
        THROW_EXCEPTION("inconsistent types between this (not sparse) and v (sparse).");
    }
  }

}

void SimpleMatrix::setSubCol(unsigned int r, unsigned int pos, SP::SiconosVector vIn)
{
  // Set column number r, starting from element at position pos, of current matrix with vIn.
  Siconos::UBLAS_TYPE numV = vIn->num();
  if(r >= size(1))
    THROW_EXCEPTION("col is out of range");

  if(vIn->size() > size(0) - pos)
    THROW_EXCEPTION("inconsistent sizes between this and v.");

  if(_num == Siconos::ZERO || _num == Siconos::IDENTITY)
    THROW_EXCEPTION("current matrix is read-only (zero or identity).");

  {
    unsigned int nbEl = vIn->size();
    if(_num == Siconos::DENSE)
    {
      if(numV == Siconos::DENSE)
      {
        noalias(ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl))) = *vIn->dense();
      }
      else if(numV == Siconos::SPARSE)
      {
        ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl)) = *vIn->sparse();
      }
    }
    else if(_num == Siconos::SPARSE && numV == Siconos::SPARSE)
#ifdef BOOST_LIMITATION
      THROW_EXCEPTION("ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
#else
      ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl)) = *vIn->sparse();
#endif
    else
      THROW_EXCEPTION("inconsistent types between current matrix and v.");
    resetFactorizationFlags();
  }
}


#include <armadillo>
#include <fstream>
#include "mcpdft.h"
#include "HDF5_Read_Contiguous.h"
#include "hdf5.h"
#include <assert.h>

#define OPDM_H5FILE  "opdm.h5"
#define TPDM_H5FILE  "tpdm.h5"
#define D1A_DATASET  "/D1a/D1a_DataSet"
#define D1B_DATASET  "/D1b/D1b_DataSet"
#define D2AB_DATASET "/D2ab/D2ab_DataSet"
#define RANK 2

namespace mcpdft {

   void HDF5ReadContiguous::read_rdms(arma::mat &D1a,
                          arma::mat &D1b,
			  arma::mat &D2ab) {
      read_opdm(D1a, D1b);
      read_tpdm(D2ab);
   }

   void HDF5ReadContiguous::read_opdm(arma::mat &D1a,
		          arma::mat &D1b) {
      std::ifstream file(OPDM_H5FILE);
      if( !file.good() )
	throw "\n  Warning: No accessible HDF5 file by the name \"opdm.h5\" exists!\n";
        file.close();

      assert( D1a.n_cols == D1a.n_rows );
      assert( D1b.n_cols == D1b.n_rows );
      size_t dim = D1a.n_cols;

      /* file indentifiers and handles */
      hid_t file_id;
      hid_t dcpl_D1a, dcpl_D1b;
      hid_t D1a_dst_id, D1b_dst_id;
      herr_t status;

      /* Open the existing HDF5 file in the read-only mode */
      file_id = H5Fopen(OPDM_H5FILE, H5F_ACC_RDONLY, H5P_DEFAULT);

      /* Open the existing HDF5 dataset in the read-only mode */
      D1a_dst_id = H5Dopen2(file_id, D1A_DATASET, H5P_DEFAULT);

      /* Open the existing HDF5 dataset in the read-only mode */
      D1b_dst_id = H5Dopen2(file_id, D1B_DATASET, H5P_DEFAULT);

      /*
       * Retrieve the dataset creation property list for opdms
       * and print their storage layout.
       */
      dcpl_D1a = H5Dget_create_plist (D1a_dst_id);
      dcpl_D1b = H5Dget_create_plist (D1b_dst_id);
      H5D_layout_t layout[] = { H5Pget_layout (dcpl_D1a), H5Pget_layout (dcpl_D1b) };
      std::string  dataset_names[] = { D1A_DATASET, D1B_DATASET };

      printf ("-------------------------------------------------------------\n");
      printf ("          HDF5 OPDM & TPDM FILES LAYOUT SUMMARY              \n");
      printf ("-------------------------------------------------------------\n");
      for (int i = 0; i < 2; i++) {
         printf ("  Storage layout for %s is   : ", dataset_names[i].c_str());
         switch (layout[i]) {
             case H5D_COMPACT:
                 printf ("H5D_COMPACT\n");
                 break;
             case H5D_CONTIGUOUS:
                 printf ("H5D_CONTIGUOUS\n");
                 break;
             case H5D_CHUNKED:
                 printf ("H5D_CHUNKED\n");
                 break;
             case H5D_VIRTUAL:
                 printf ("H5D_VIRTUAL\n");
                 break;
             case H5D_LAYOUT_ERROR:
             case H5D_NLAYOUTS:
                 printf ("H5D_LAYOUT_ERROR\n");
         }
      }

      /* Read the D1a_DataSet */
      status = H5Dread(D1a_dst_id, H5T_NATIVE_DOUBLE,
                       H5S_ALL, H5S_ALL, H5P_DEFAULT, D1a.memptr());

      /* Read the D1a_DataSet */
      status = H5Dread(D1b_dst_id, H5T_NATIVE_DOUBLE,
                       H5S_ALL, H5S_ALL, H5P_DEFAULT, D1b.memptr());

      /* Close property lists */
      status = H5Pclose(dcpl_D1a);
      status = H5Pclose(dcpl_D1b);

      /* Close datasets */
      status = H5Dclose(D1a_dst_id);
      status = H5Dclose(D1b_dst_id);

      /* Close the file */
      status = H5Fclose(file_id);
   }

   void HDF5ReadContiguous::read_tpdm(arma::mat &D2ab) {
      std::ifstream file(TPDM_H5FILE);
      if( !file.good() )
	throw "\n  Warning: No accessible HDF5 file by the name \"tpdm.h5\" exists!\n";
        file.close();

      assert( D2ab.n_cols == D2ab.n_rows );
      size_t dim = D2ab.n_cols;

      /* file indentifiers and handles */
      hid_t file_id;
      hid_t dcpl_D2ab;
      hid_t D2ab_dst_id;
      herr_t status;

      /* Open the existing HDF5 file in the read-only mode */
      file_id = H5Fopen(TPDM_H5FILE, H5F_ACC_RDONLY, H5P_DEFAULT);

      /* Open the existing HDF5 dataset in the read-only mode */
      D2ab_dst_id = H5Dopen2(file_id, D2AB_DATASET, H5P_DEFAULT);

      /*
       * Retrieve the dataset creation property list for opdms
       * and print their storage layout
       */
      dcpl_D2ab = H5Dget_create_plist (D2ab_dst_id);
      H5D_layout_t layout = H5Pget_layout (dcpl_D2ab);

      printf ("  Storage layout for %s is : ", D2AB_DATASET);
      switch (layout) {
          case H5D_COMPACT:
              printf ("H5D_COMPACT\n");
              break;
          case H5D_CONTIGUOUS:
              printf ("H5D_CONTIGUOUS\n");
              break;
          case H5D_CHUNKED:
              printf ("H5D_CHUNKED\n");
              break;
          case H5D_VIRTUAL:
              printf ("H5D_VIRTUAL\n");
              break;
          case H5D_LAYOUT_ERROR:
          case H5D_NLAYOUTS:
              printf ("H5D_LAYOUT_ERROR\n");
      }
      printf ("-------------------------------------------------------------\n\n" );

      /* Read the D2ab_DataSet */
      status = H5Dread(D2ab_dst_id, H5T_NATIVE_DOUBLE,
                       H5S_ALL, H5S_ALL, H5P_DEFAULT, D2ab.memptr());

      /* Close property list */
      status = H5Pclose(dcpl_D2ab);

      /* Close dataset */
      status = H5Dclose(D2ab_dst_id);

      /* Close the file */
      status = H5Fclose(file_id);
   }
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__MANIFOLD_HPP_
#define SMOOTH__MANIFOLD_HPP_

#include <Eigen/Core>

#include <concepts>

/**
 * @file manifold.hpp Manifold interface and free Manifold functions.
 */

namespace smooth {

namespace traits {

/**
 * @brief Trait class for making a class a Manifold instance via specialization.
 */
template<typename T>
struct man;

}  // namespace traits

// clang-format off

/**
 * @brief Class-external Manifold interface defined through the traits::man trait class.
 */
template<typename M>
concept Manifold =
requires  (Eigen::Index dof) {
  // Underlying scalar type
  typename traits::man<M>::Scalar;
  // Default representation
  typename traits::man<M>::PlainObject;
  // Compile-time degrees of freedom (tangent space dimension). Can be dynamic (equal to -1)
  {traits::man<M>::Dof}->std::convertible_to<Eigen::Index>;
  // A default-initialized Manifold object (if Dof > 0 then dof = Dof can be assumed)
  {traits::man<M>::Default(dof)}->std::convertible_to<typename traits::man<M>::PlainObject>;
} &&
requires(const M & m1, const M & m2, const Eigen::Vector<typename traits::man<M>::Scalar, traits::man<M>::Dof> & a) {
  // Run-time degrees of freedom (tangent space dimension)
  {traits::man<M>::dof(m1)}->std::convertible_to<Eigen::Index>;
  // Right-plus: add a tangent vector to a Manifold element to obtain a new Manifold element
  {traits::man<M>::rplus(m1, a)}->std::convertible_to<typename traits::man<M>::PlainObject>;
  // Right-minus: subtract a Manifold element from another to obtain the difference tangent vector
  {traits::man<M>::rminus(m1, m2)}->std::convertible_to<Eigen::Vector<typename traits::man<M>::Scalar, traits::man<M>::Dof>>;
} && (
  !std::is_convertible_v<typename traits::man<M>::Scalar, double> ||
  requires (const M & m) {
    {traits::man<M>::template cast<double>(m)}->std::convertible_to<typename traits::man<M>::template CastT<double>>;
  }
) && (
  !std::is_convertible_v<typename traits::man<M>::Scalar, float> ||
  requires (const M & m) {
    {traits::man<M>::template cast<float>(m)}->std::convertible_to<typename traits::man<M>::template CastT<float>>;
  }
) &&
// PlainObject must be default-constructible
std::is_default_constructible_v<typename traits::man<M>::PlainObject> &&
std::is_copy_constructible_v<typename traits::man<M>::PlainObject> &&
// PlainObject must be assignable from M
std::is_assignable_v<typename traits::man<M>::PlainObject &, M>;

// clang-format on

namespace traits {

/**
 * @brief Concept to identify Eigen column vectors
 */
template<typename G>
concept RnType = std::is_base_of_v<Eigen::MatrixBase<G>, G> && G::ColsAtCompileTime == 1;

/**
 * @brief Manifold interface for RnType
 */
template<RnType M>
struct man<M>
{
  // \cond
  static constexpr int Dof = M::SizeAtCompileTime;

  using Scalar      = typename M::Scalar;
  using PlainObject = Eigen::Vector<Scalar, Dof>;
  template<typename NewScalar>
  using CastT = Eigen::Vector<NewScalar, M::SizeAtCompileTime>;

  static inline PlainObject Default(Eigen::Index dof) { return M::Zero(dof); }

  static inline Eigen::Index dof(const M & m) { return m.size(); }

  template<typename NewScalar>
  static inline CastT<NewScalar> cast(const M & m)
  {
    return m.template cast<NewScalar>();
  }

  template<typename Derived>
  static inline PlainObject rplus(const M & g, const Eigen::MatrixBase<Derived> & a)
  {
    return g + a;
  }

  template<typename Derived>
  static inline Eigen::Vector<Scalar, M::SizeAtCompileTime>
  rminus(const M & m1, const Eigen::MatrixBase<Derived> & m2)
  {
    return m1 - m2;
  }
  // \endcond
};

/**
 * @brief Trait class to mark external types as scalars.
 */
template<typename T>
struct scalar_trait
{
  /// @brief true if type T is a scalar
  static constexpr bool value = false;
};

/**
 * @brief Concept to identify built-in scalars
 */
template<typename T>
concept ScalarType = std::is_floating_point_v<T> || traits::scalar_trait<T>::value;

/**
 * @brief Manifold interface for ScalarType
 */

template<ScalarType M>
struct man<M>
{
  // \cond
  static constexpr int Dof = 1;

  using Scalar      = M;
  using PlainObject = M;
  template<typename NewScalar>
  using CastT = NewScalar;

  static inline PlainObject Default(Eigen::Index) { return M(0); }

  static inline Eigen::Index dof(const M &) { return 1; }

  template<typename NewScalar>
  static inline CastT<NewScalar> cast(const M & m)
  {
    return static_cast<NewScalar>(m);
  }

  template<typename Derived>
  static inline PlainObject rplus(const M & g, const Eigen::MatrixBase<Derived> & a)
  {
    return g + a.x();
  }

  static inline Eigen::Vector<Scalar, Dof> rminus(const M & m1, const M & m2)
  {
    return Eigen::Vector<Scalar, Dof>(m1 - m2);
  }
  // \endcond
};

}  // namespace traits

////////////////////////////////////////////////
//// Free functions that dispatch to traits::man<M> ////
////////////////////////////////////////////////

// Static constants

/**
 * @brief Manifold degrees of freedom (tangent space dimension)
 *
 * @note Equal to -1 for a dynamically sized Manifold
 */
template<Manifold M>
static inline constexpr Eigen::Index Dof = traits::man<M>::Dof;

// Types

/**
 * @brief Manifold scalar type
 */
template<Manifold M>
using Scalar = typename traits::man<M>::Scalar;

/**
 * @brief Manifold default type
 */
template<Manifold M>
using PlainObject = typename traits::man<M>::PlainObject;

/**
 * @brief Cast'ed type
 */
template<typename NewScalar, Manifold M>
using CastT = typename traits::man<M>::template CastT<NewScalar>;

/**
 * @brief Tangent as a Dof-length vector
 */
template<Manifold M>
using Tangent = Eigen::Vector<Scalar<M>, Dof<M>>;

/**
 * @brief Matrix of size Dof x Dof
 */
template<Manifold M>
using TangentMap = Eigen::Matrix<Scalar<M>, Dof<M>, Dof<M>>;

// Functions

/**
 * @brief Default-initialized Manifold
 */
template<Manifold M>
inline PlainObject<M> Default(Eigen::Index dof)
{
  return traits::man<M>::Default(dof);
}

/**
 * @brief Default-initialized Manifold with static dof
 */
template<Manifold M>
inline PlainObject<M> Default() requires(Dof<M> > 0)
{
  return traits::man<M>::Default(Dof<M>);
}

/**
 * @brief Manifold degrees of freedom (tangent space dimension)
 */
template<Manifold M>
inline Eigen::Index dof(const M & m)
{
  return traits::man<M>::dof(m);
}

/**
 * @brief Cast to different scalar type
 */
template<typename NewScalar, Manifold M>
inline CastT<NewScalar, M> cast(const M & m)
{
  return traits::man<M>::template cast<NewScalar>(m);
}

/**
 * @brief Manifold right-plus
 */
template<Manifold M, typename Derived>
inline PlainObject<M> rplus(const M & m, const Eigen::MatrixBase<Derived> & a)
{
  return traits::man<M>::rplus(m, a);
}

/**
 * @brief Manifold right-minus
 */
template<Manifold M, Manifold Mo>
inline Tangent<M> rminus(const M & g1, const Mo & g2)
{
  return traits::man<M>::rminus(g1, g2);
}

}  // namespace smooth

#endif


#ifndef KA_FUNCTIONAL_HPP
#define KA_FUNCTIONAL_HPP
#pragma once
#include <boost/config.hpp>
#include <boost/optional.hpp>
#include "integersequence.hpp"
#include "macro.hpp"
#include "macroregular.hpp"
#include "memory.hpp"
#include "src.hpp"
#include "typetraits.hpp"
#include "utility.hpp"

/// @file
/// Contains functional tools: function composition, constant function, identity
/// function, lock-and-call wrapper, etc.

namespace ka {

  /// Polymorphic function that maps any input to the same output.
  ///
  /// Copiable Ret
  template<typename Ret>
  struct constant_function_t {
    Ret ret;
  // Regular:
    KA_GENERATE_FRIEND_REGULAR_OPS_1(constant_function_t, ret)
  // PolymorphicFunction<Ret (Args...)>:
    template<typename... Args>
    Ret operator()(Args const&...) const {
      return ret;
    }
  };

  KA_DERIVE_CTOR_FUNCTION_TEMPLATE(constant_function)

  /// `void` specialization
  template<>
  struct constant_function_t<void> {
  // Regular:
    KA_GENERATE_FRIEND_REGULAR_OPS_0(constant_function_t)
  // PolymorphicFunction<void (Args...)>:
    template<typename... Args>
    void operator()(Args const&...) const {
    }
  };

  KA_DERIVE_CTOR_FUNCTION_TEMPLATE_VOID(constant_function)

  /// A polymorphic transformation that takes a value and returns it as-is.
  ///
  /// A transformation is a unary function from a type to itself.
  struct id_transfo_t {
  // Regular:
    KA_GENERATE_FRIEND_REGULAR_OPS_0(id_transfo_t)
  // PolymorphicTransformation:
    template<typename T>
    inline T operator()(T const& t) const {
      return t;
    }
  // Isomorphism:
    friend id_transfo_t retract(id_transfo_t x) {
      return x;
    }
  };

  KA_DERIVE_CTOR_FUNCTION(id_transfo)

  /// A polymorphic action that does not modify its argument.
  ///
  /// See concept `Action`.
  struct id_action_t {
  // Regular:
    KA_GENERATE_FRIEND_REGULAR_OPS_0(id_action_t)
  // PolymorphicAction:
    template<typename T>
    inline void operator()(T&) const {
    }
  };

  KA_DERIVE_CTOR_FUNCTION(id_action)

// Mainly useful because of the redundancy of trailing return types.
//
// TODO: Remove this when get rid of C++11.
#define KA_FWD_ARGS fwd<Args>(args)...

  namespace detail {
    template<typename Ret>
    struct composition_t {
      /// TODO: Remove the trailing return type when get rid of C++11.
      template<typename G, typename F, typename... Args>
      auto operator()(G& g, F& f, Args&&... args) const -> decltype(g(f(KA_FWD_ARGS))) {
        return g(f(KA_FWD_ARGS));
      }
    };

    template<>
    struct composition_t<void> {
      /// TODO: Remove the trailing return type when get rid of C++11.
      template<typename G, typename F, typename... Args>
      auto operator()(G& g, F& f, Args&&... args) const -> decltype(g()) {
        return f(KA_FWD_ARGS), g();
      }
    };
  } // namespace detail

  /// Composes two procedures, handling the void case.
  ///
  /// In C++, `g(f(x))` is not possible if `f` returns `void` and `g` takes `void`
  /// (no argument). This case is handled as `f(x), g()`.
  ///
  /// Procedure<C (B)> G, Procedure<B (A...)> F
  template<typename G, typename F>
  struct composition_t {
    G g;
    F f;
  // Regular (if G and F are):
    KA_GENERATE_FRIEND_REGULAR_OPS_2(composition_t, g, f)
  // Procedure<C (A...)>:
    /// TODO: Remove the trailing return type when get rid of C++11.
    template<typename... Args>
    auto operator()(Args&&... args)
        -> decltype(detail::composition_t<decltype(f(KA_FWD_ARGS))>{}(g, f, KA_FWD_ARGS)) {
      return detail::composition_t<decltype(f(KA_FWD_ARGS))>{}(g, f, KA_FWD_ARGS);
    }

    template<typename... Args>
    auto operator()(Args&&... args) const
        -> decltype(detail::composition_t<decltype(f(KA_FWD_ARGS))>{}(g, f, KA_FWD_ARGS)) {
      return detail::composition_t<decltype(f(KA_FWD_ARGS))>{}(g, f, KA_FWD_ARGS);
    }
  // RetractableFunction (if `F` and `G` are):
    // TODO: Add it when switching to C++14.
    // implementation is: x -> compose(retract(x.f), retract(x.g))
  };

#undef KA_FWD_ARGS

  namespace detail {
    template<typename G, typename F>
    using IsCompositionIdentity = Conjunction<
      std::is_empty<G>,
      ka::IsRetract<G, F>>;
  } // namespace detail

  /// Performs a mathematical function composition.
  ///
  /// The composition is smart enough so that if you compose a function and its
  /// retraction, it will return the identity function.
  ///
  /// Example: Composing with a non-void function
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// // `motor` takes a `string` and returns the properties of a motor.
  /// // `heat` takes the properties of a motor and returns an `int`.
  /// // `motor_names` is a container of string.
  /// auto heat_of_motor = compose(heat, motor);
  /// auto max_heat = std::max_element(motor_names.begin(), motor_names.end(), heat_of_motor);
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Example: Composing with a void function
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// // `log_name` and `log_heat` take no input (void) and returns no ouput (void).
  /// auto log_name_then_heat = compose(log_heat, log_name);
  /// auto fut = strand.async(log_name_then_heat);
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Remark: In the future, add a SFINAE guard if there is a name clash.
  ///   Alternatively, rename this function in `fun_compose`.
  ///
  /// Remark: Operator `*` is proposed as an opt-in alternative to `compose` in
  ///   the `functional_ops` namespace.
  ///
  /// Procedure<C (B)> G,
  /// Procedure<B (Args...)> F

// TODO: Remove this when get rid of VS2013.
#if KA_COMPILER_VS2013_OR_BELOW
  // Poor-man version that does not perform  simplifications.
  template<typename G, typename F>
  composition_t<Decay<G>, Decay<F>> compose(G&& g, F&& f) {
    return {fwd<G>(g), fwd<F>(f)};
  }
#else
  template<typename G, typename F, typename =
    EnableIf<!detail::IsCompositionIdentity<Decay<G>, Decay<F>>::value>>
  composition_t<Decay<G>, Decay<F>> compose(G&& g, F&& f) {
    return {fwd<G>(g), fwd<F>(f)};
  }

  template<typename F, typename = EnableIf<std::is_empty<F>::value>>
  id_transfo_t compose(Retract<Decay<F>> const&, F const&) {
    return {};
  }
#endif

  template<typename F>
  F&& compose(id_transfo_t, F&& f) {
    return fwd<F>(f);
  }

  template<typename F>
  F&& compose(F&& f, id_transfo_t) {
    return fwd<F>(f);
  }

  // This one could be theoretically handled by the simplifying overload
  // (because `id_transfo_t` is its own retraction/inverse). But removing this
  // overload causes overload resolution to fail because of ambiguities.
  inline id_transfo_t compose(id_transfo_t, id_transfo_t) {
    return {};
  }

  /// Composes two accumulations.
  ///
  /// An `Accumulation` has the signature `void (T&, Args...)`, meaning it modifies
  /// its argument in-place. There is always a semantically identical `Function`
  /// of the form `T (T)`.
  ///
  /// Composing two accumulations `g` and `f` is equivalent to perform
  /// ```
  /// f(args...);
  /// g(args...);
  /// ```
  /// This implies that both accumulations must have the same signature to be
  /// composable.
  ///
  /// Example: Arithmetic actions
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// using M = matrix_t<int, 10000, 10000>;
  ///
  /// // We use actions to avoid the expensive copies that would happen with the
  /// // equivalent transformations (at least without compiler optimizations).
  /// incr_t<M> incr;   // signature: void (M&)
  /// twice_t<M> twice; // signature: void (M&)
  ///
  /// // Using `compose` wouldn't work here because we don't pass the output of
  /// // `incr` to the input of `twice`.
  ///
  /// auto twice_of_incr = compose_accu(twice, incr); // signature: void (M&)
  ///
  /// M m = compute_matrix();
  /// twice_of_incr(m); // Apply `incr`, then `twice`.
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Remark: Operator `*=` is proposed as an opt-in alternative to
  ///   `compose_accu` in the `functional_ops` namespace.
  ///
  /// Accumulation<T, Args...> G, Accumulation<T, Args...> F
  template<typename G, typename F>
  struct composition_accu_t {
    G g;
    F f;
  // Regular (if G and F are):
    KA_GENERATE_FRIEND_REGULAR_OPS_2(composition_accu_t, g, f)
  // Accumulation<T, Args...>:
    template<typename T, typename... Args>
    void operator()(T& t, Args const&... args) {
      f(t, args...);
      g(t, args...);
    }

    template<typename T, typename... Args>
    void operator()(T& t, Args const&... args) const {
      f(t, args...);
      g(t, args...);
    }
  // IsomorphismAccu (if `F` and `G` are):
    // TODO: Add it when switching to C++14.
    // implementation is: x -> compose_accu(retract(x.f), retract(x.g))
  };

  /// Helper-function to deduce types for `composition_accu_t`.
  ///
  /// This function will detect retractions and optimise the composition. This
  /// means that if you compose an action with its retraction (i.e. an action
  /// that undoes it), the identity action will be returned.
  ///
  /// Accumulation<T...> G, Accumulation<T...> F
  template<typename G, typename F, typename = EnableIf<
    !detail::IsCompositionIdentity<Decay<G>, Decay<F>>::value>>
  composition_accu_t<Decay<G>, Decay<F>> compose_accu(G&& g, F&& f) {
    return {fwd<G>(g), fwd<F>(f)};
  }

  template<typename F, typename = EnableIf<std::is_empty<F>::value>>
  id_action_t compose_accu(Retract<F> const&, F const&) {
    return {};
  }

  template<typename F>
  F&& compose_accu(id_action_t, F&& f) {
    return fwd<F>(f);
  }

  template<typename F>
  F&& compose_accu(F&& f, id_action_t) {
    return fwd<F>(f);
  }

  // This one could be theoretically handled by the simplifying overload
  // (because `id_action_t` is its own inverse). But removing this overload causes
  // overload resolution to fail because of ambiguities.
  inline id_action_t compose_accu(id_action_t, id_action_t) {
    return {};
  }

  namespace functional_ops {
    /// Performs a mathematical function composition.
    ///
    /// See `compose()`.
    ///
    /// `*` is used because in mathematics multiplication denotes an
    /// associative operation (not required to be commutative).
    /// Function composition being associative, the composition of `g` with `f`
    /// is often abbreviated as `gf`, reusing the product notation.
    ///
    /// Furthermore `id_transfo_t` being polymorphic, it denotes the identity
    /// functions for all types, so the function composition together with
    /// `id_transfo_t` forms a category.
    ///
    /// Therefore, the following invariants hold:
    ///
    /// ```
    /// // `f` is an arbitrary function.
    /// id_transfo_t _1;
    ///
    /// assert(f * _1 == f);
    /// assert(_1 * f == f);
    /// assert(_1 * _1 == _1);
    /// ```
    template<typename G, typename F>
    auto operator*(G&& g, F&& f) -> decltype(compose(fwd<G>(g), fwd<F>(f))) {
      return compose(fwd<G>(g), fwd<F>(f));
    }

    /// Performs a composition of accumulation procedures.
    ///
    /// See `compose_accu()`.
    ///
    /// `*=` is used as a variant of `*` (the function composition operator).
    /// The `=` sign is used to emphasize the notion of accumulation.
    ///
    /// This naming follows the fact that in the language, binary operations
    /// typically have a semantically equivalent accumulation counterpart.
    /// For example for a type `T`, `+: T x T -> T` is a binary operation and
    /// has for counterpart the semantically equivalent accumulation
    /// `+=: T& x T -> void`.
    ///
    /// So composing functions (i.e. regular procedures with signature
    /// `A0 x A1 x ... -> B`) is done with `*` and composing accumulations (i.e.
    /// procedures with signature `A0& x A1 x ... -> void)` is done with `*=`.
    ///
    /// Note: Symmetrically to `compose` with `id_transfo_t`, `compose_accu` with
    ///   `id_action_t` forms a category. Therefore, the same kind of invariants
    ///   hold:
    ///
    /// ```
    /// // `a` is an arbitrary accumulation.
    /// id_action_t _1;
    ///
    /// assert((a *= _1) == a);
    /// assert((_1 *= a) == a);
    /// assert((_1 *= _1) == _1);
    /// ```
    template<typename G, typename F>
    auto operator*=(G&& g, F&& f) -> decltype(compose_accu(fwd<G>(g), fwd<F>(f))) {
      return compose_accu(fwd<G>(g), fwd<F>(f));
    }
  }

  /// Function that transforms the codomain (return type) of the given procedure
  /// into an "enriched" type.
  ///
  /// This is related to monad theory (see below).
  ///
  /// Example: Transforming a procedure of type (A -> B) in a procedure of type (A -> Future<B>):
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// // `proc` takes an `int` and returns a `bool`
  /// auto f = semilift(proc, UnitFuture{});
  /// Future<bool> fut = f(i);
  /// // ...
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// # Background on monads
  ///
  /// A monad can be viewed as an "enriched" type together with functions to go
  /// from and to the corresponding "base" type.
  /// In the above example, `int` is the base type and `Future<int>` is the
  /// enriched, monadic, type.
  ///
  /// To lift is to map a function on base types to a function on the
  /// corresponding enriched types:
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// enriched "level":  X<int> ---> X<bool>
  ///                            ^
  ///                            | lift
  ///                            |
  /// base "level":      int    ---> bool
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// In the above case `X` is `Future`.
  ///
  /// Other examples for X are: std::optional, std::vector, etc.
  ///
  /// Generally speaking, the enriched type (e.g. X<int>) gives access to a value
  /// of an underlying type (e.g. int). Thus, it can be viewed as a "container".
  ///
  /// In our case, we have:
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// enriched "level":       X<bool>
  ///                         ^    ^
  ///   semilift(proc, unit) /     | unit
  ///                       /      |
  /// base "level":      int ---> bool
  ///                        proc
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// This diagram commutes: all paths are equivalent.
  ///
  /// That is, we have for all x:
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// semilift(proc, unit)(x) == compose(unit, proc)(x)
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// where `compose` denotes the function composition.
  ///
  /// `unit` is a function that sends a "base" value to an "enriched", monadic value.
  /// Its name comes from the fact that it is a "unit" or "neutral element" for
  /// monadic composition.
  ///
  /// This function is named `semilift` because it partially "lifts" the procedure:
  /// only the codomain is enriched.
  ///
  /// Procedure Proc
  template<typename Proc, typename F>
  auto semilift(Proc&& p, F&& unit) -> decltype(compose(fwd<F>(unit), fwd<Proc>(p))) {
    return compose(fwd<F>(unit), fwd<Proc>(p));
  }

  /// Bind a data to a procedure without changing its semantics.
  /// One use is to bind a shared pointer to extend the lifetime of some data
  /// related to the procedure.
  ///
  /// This type is a useful equivalent to a variadic generic lambda in C++14:
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// T data;
  /// auto data_bound_proc = [data](auto&&... args) {
  ///   // ...
  /// };
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Warning: This type is not strictly equivalent to a lambda (C++14 or even C++17),
  ///   as it is Regular and lambdas are not.
  ///
  /// Procedure Proc
  template<typename Proc, typename T>
  struct data_bound_proc_t {
    Proc proc;
    T data;
  // Regular (if T is regular):
    KA_GENERATE_FRIEND_REGULAR_OPS_2(data_bound_proc_t, proc, data)
  // Procedure:
    template<typename... Args>
    auto operator()(Args&&... args) -> decltype(proc(fwd<Args>(args)...)) {
      return proc(fwd<Args>(args)...);
    }

    template<typename... Args>
    auto operator()(Args&&... args) const -> decltype(proc(fwd<Args>(args)...)) {
      return proc(fwd<Args>(args)...);
    }
  };

  KA_DERIVE_CTOR_FUNCTION_TEMPLATE(data_bound_proc)

  /// A polymorphic transformation that takes a procedure and returns
  /// an equivalent one that is bound to an arbitrary data.
  /// The bound data can be a shared pointer to extend the lifetime of an object
  /// the procedure is dependent on.
  ///
  /// Example: extending instance lifetime by binding a shared pointer
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// // Network N
  /// template<typename N>
  /// struct X : std::enable_shared_from_this<X> {
  ///   SendMessageEnqueue<N, SocketPtr<N>> sendMsg;
  ///
  ///   void doStuff() {
  ///     // ...
  ///     auto onSent = [](...) {
  ///        // use X's members here
  ///     };
  ///     // sendMsg will wrap any asynchronous task by binding it with a
  ///     // shared pointer on this instance.
  ///     sendMsg(msg, ssl, onSent,
  ///       data_bound_transfo(shared_from_this()) // lifetimeTransfo
  ///     );
  ///   }
  /// };
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Regular T
  template<typename T>
  struct data_bound_transfo_t {
    T data;
  // Regular (if T is regular):
    KA_GENERATE_FRIEND_REGULAR_OPS_1(data_bound_transfo_t, data)
  // PolymorphicTransformation:
    template<typename Proc>
    data_bound_proc_t<Decay<Proc>, T> operator()(Proc&& p) {
      return {fwd<Proc>(p), data};
    }

    template<typename Proc>
    data_bound_proc_t<Decay<Proc>, T> operator()(Proc&& p) const {
      return {fwd<Proc>(p), data};
    }
  };

  KA_DERIVE_CTOR_FUNCTION_TEMPLATE(data_bound_transfo)

  /// Action that move-assigns a stored value on call.
  ///
  /// Warning: Can be called only once because the value assigned from is moved.
  ///
  /// With Src s, Dest d, the following is valid:
  ///   d = std::move(s);
  template<typename Dest, typename Src>
  struct move_assign_t {
    Src s;
  // Action<T>:
    /// Precondition: it is the first time this operator is called on this instance
    void operator()(Dest& d) {
      d = std::move(s);
    }

    // TODO: Remove this when get rid of VS2013.
    move_assign_t(Src&& s)
      : s(std::move(s)) {
    }

    // TODO: Remove this when get rid of VS2013.
    move_assign_t(Src const& s)
      : s(s) {
    }

    // TODO: Remove this when get rid of VS2013.
    move_assign_t(move_assign_t const& other) = default;

    // TODO: Remove this when get rid of VS2013.
    move_assign_t(move_assign_t&& x)
      : s(std::move(x.s)) {
    }
  };

  /// Helper function that performs type deduction for `MoveAssign`.
  template<typename Dest, typename Src>
  move_assign_t<Dest, Decay<Src>> move_assign(Src&& s) {
    return std::move(move_assign_t<Dest, Decay<Src>>(fwd<Src>(s)));
  }

  template<typename T>
  struct incr_mono_t;

  /// Isomorphic action that decrements its parameter.
  ///
  /// See `incr_mono_t` for a use example.
  ///
  /// The inverse action is `incr_mono_t<T>`
  ///
  /// (Arithmetic || BidirectionalIterator) T
  template<typename T>
  struct decr_mono_t {
  // Regular:
    KA_GENERATE_FRIEND_REGULAR_OPS_0(decr_mono_t)
  // Action<Arithmetic || BidirectionalIterator>:
    // TODO: Remove this when get rid of VS2013.
    using retract_type = incr_mono_t<T>;

    void operator()(T& x) const {
      --x;
    }
  // IsomorphicAction<Integral || BidirectionalIterator>:
    template<typename U>
    friend incr_mono_t<U> retract(decr_mono_t<U>);
  };

  /// Isomorphic action that increments its parameter.
  ///
  /// Example: Using in conjunction with scoped tools
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// void pretty_print(doc_t const& doc, std::ostream& o) {
  ///   // `depth` is an `int` with a wider scope.
  ///   auto _ = scoped_apply_and_retract(depth, incr_mono_t<int>{});
  ///   // here, the depth has been incremented
  ///   ...
  /// }
  /// // here, the depth has been decremented
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// The inverse action is `decr_mono_t<T>`.
  ///
  /// (Arithmetic || InputIterator) T
  template<typename T>
  struct incr_mono_t {
  // Regular:
    KA_GENERATE_FRIEND_REGULAR_OPS_0(incr_mono_t)
  // Action<Arithmetic || InputIterator>:
    void operator()(T& x) const {
      ++x;
    }
  // IsomorphicAction<Integral || BidirectionalIterator>:
    // TODO: Remove this when get rid of VS2013.
    using retract_type = decr_mono_t<T>;

    friend decr_mono_t<T> retract(incr_mono_t) {
      return {};
    }
  };

  template<typename T>
  incr_mono_t<T> retract(decr_mono_t<T>) {
    return {};
  }

  struct incr_t;

  struct decr_t {
  // Regular:
    KA_GENERATE_FRIEND_REGULAR_OPS_0(decr_t)
  // Action<Arithmetic || BidirectionalIterator>:
    // TODO: Remove this when get rid of VS2013.
    using retract_type = incr_t;

    template<typename T>
    inline void operator()(T& x) const {
      --x;
    }
  // IsomorphicAction<Integral || BidirectionalIterator>:
    inline friend incr_t retract(decr_t);
  };

  KA_DERIVE_CTOR_FUNCTION(decr)

  struct incr_t {
  // Regular:
    KA_GENERATE_FRIEND_REGULAR_OPS_0(incr_t)
  // Action<Arithmetic || InputIterator>:
    template<typename T>
    inline void operator()(T& x) const {
      ++x;
    }
  // IsomorphicAction<Integral || BidirectionalIterator>:
    // TODO: Remove this when get rid of VS2013.
    using retract_type = decr_t;

    inline friend decr_t retract(incr_t) {
      return {};
    }
  };

  inline incr_t retract(decr_t) {
    return {};
  }

  KA_DERIVE_CTOR_FUNCTION(incr)

  namespace detail {
    template<typename Proc, typename Args, std::size_t... I>
    BOOST_CONSTEXPR auto apply_impl(Proc&& proc, Args&& args, index_sequence<I...>)
      // TODO: Guess what, remove this when c++14 is available.
        -> decltype(proc(std::get<I>(fwd<Args>(args))...)) {
      return proc(std::get<I>(fwd<Args>(args))...);
    }
  }

  /// Applies the procedure after unpacking the arguments.
  ///
  /// Besides std::tuple, arguments can be stored as std::pair, std::array, or
  /// any type modeling the `Tuple` concept (see concept.hpp).
  ///
  /// Note: This is roughly equivalent to C++17's `std::apply`. The main difference
  /// is that this version use function call syntax instead of C++17's `std::invoke`.
  ///
  /// TODO: Replace this by `std::apply` when C++17 is available.
  ///
  /// Example: using a `std::tuple` as a container for arguments
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// int f(int i, char c, float f) {
  ///   ...
  /// }
  ///
  /// // ...
  /// auto args = std::make_tuple(5, 'a', 3.14f);
  /// int res = apply(f, args);
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Example: using a `std::pair` as a container for arguments
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// int f(int i, char c) {
  ///   ...
  /// }
  ///
  /// // ...
  /// auto args = std::make_pair(5, 'a');
  /// int res = apply(f, args);
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Example: using a `std::array` as a container for arguments
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// int f(int i, int j, int k, int l) {
  ///   ...
  /// }
  ///
  /// // ...
  /// std::array<int, 4> args = {13, 2, 9874, 42};
  /// int res = apply(f, args);
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Example: using a custom type as a container for arguments
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// namespace test {
  ///   template<std::size_t N>
  ///   using index = std::integral_constant<std::size_t, N>;
  ///
  ///   template<typename A, typename B, typename C>
  ///   struct X {
  ///     A a;
  ///     B b;
  ///     C c;
  ///
  ///     bool operator==(X const& y) const {
  ///       return a == y.a && b == y.b && c == y.c;
  ///     }
  ///
  ///     A& get(index<0>) {return a;}
  ///     B& get(index<1>) {return b;}
  ///     C& get(index<2>) {return c;}
  ///     A const& get(index<0>) const {return a;}
  ///     B const& get(index<1>) const {return b;}
  ///     C const& get(index<2>) const {return c;}
  ///   };
  /// } // namespace test
  ///
  /// namespace std {
  ///   template<typename A, typename B, typename C>
  ///   struct tuple_size<test::X<A, B, C>> : integral_constant<size_t, 3> {
  ///   };
  ///
  ///   template<size_t I, typename A, typename B, typename C>
  ///   BOOST_CONSTEXPR auto get(test::X<A, B, C>& x)
  ///     // TODO: replace the trailing return by a `decltype(auto)` when c++14 is available
  ///       -> decltype(x.get(integral_constant<size_t, I>{})) {
  ///     return x.get(integral_constant<size_t, I>{});
  ///   }
  ///
  ///   template<size_t I, typename A, typename B, typename C>
  ///   BOOST_CONSTEXPR auto get(test::X<A, B, C> const& x)
  ///     // TODO: replace the trailing return by a `decltype(auto)` when c++14 is available
  ///       -> decltype(x.get(integral_constant<size_t, I>{})) {
  ///     return x.get(integral_constant<size_t, I>{});
  ///   }
  /// } // namespace std
  ///
  /// int f(int i, char c, float f) {
  ///   ...
  /// }
  /// // ...
  ///
  /// X const args{5, 'a', 3.14f};
  /// int res = apply(f, args);
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// See `apply_t` to transform a function into an equivalent tuple-accepting function.
  ///
  /// Procedure Proc, Tuple Args
  template<typename Proc, typename Args>
  BOOST_CONSTEXPR auto apply(Proc&& proc, Args&& args)
    // TODO: replace the trailing return by a `decltype(auto)` when c++14 is available
      -> decltype(detail::apply_impl(fwd<Proc>(proc), fwd<Args>(args),
        make_index_sequence<std::tuple_size<Decay<Args>>::value>{})) {
    return detail::apply_impl(fwd<Proc>(proc), fwd<Args>(args),
      make_index_sequence<std::tuple_size<Decay<Args>>::value>{});
  }

  /// Procedure accepting a tuple of arguments and unpacking them for the underlying
  /// procedure.
  ///
  /// Example: transforming a "n-argument function" into a "tuple function"
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// int f(int i, int j) {
  ///   ...
  /// };
  ///
  /// // ...
  /// auto f2 = apply(f); // helper function that returns a `apply_t<Decay<decltype(f)>>`
  /// int res0 = f2(std::make_tuple(4454, 7));
  /// int res1 = f2(std::make_pair(5, 12));
  /// int res2 = f2(std::array<int, 2>{98, 99});
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Note: See `apply` for an example on how to use a custom type.
  ///
  /// See `apply` for more examples.
  ///
  /// Procedure Proc
  template<typename Proc>
  struct apply_t {
    Proc proc;
  // Regular:
    KA_GENERATE_FRIEND_REGULAR_OPS_1(apply_t, proc)
  // Procedure:
    template<typename Args>
    auto operator()(Args&& args) KA_NOEXCEPT_EXPR(apply(proc, fwd<Args>(args)))
        // TODO: replace the trailing return by a `decltype(auto)` when c++14 is available
        -> decltype(apply(proc, fwd<Args>(args))) {
      return apply(proc, fwd<Args>(args));
    }

    template<typename Args>
    auto operator()(Args&& args) const KA_NOEXCEPT_EXPR(apply(proc, fwd<Args>(args)))
        // TODO: replace the trailing return by a `decltype(auto)` when c++14 is available
        -> decltype(apply(proc, fwd<Args>(args))) {
      return apply(proc, fwd<Args>(args));
    }
  };

  /// Helper function to enable type deduction for `apply_t`.
  ///
  /// Note: An overload taking the procedure _and_ the arguments also exists.
  /// This allows for currying.
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// int f(int i, int j) {
  ///   ...
  /// };
  ///
  /// // ...
  /// auto args = std::make_pair(34, 45);
  ///
  /// // immediate call version
  /// auto res0 = apply(f, args);
  ///
  /// // currified version (this version)
  /// auto f2 = apply(f);
  /// auto res1 = f2(args);
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Procedure Proc
  template<typename Proc>
  apply_t<Decay<Proc>> apply(Proc&& proc) {
    return {fwd<Proc>(proc)};
  }

  namespace detail {
    template<typename Ret>
    struct scope_lock_proc_t {
      template<typename Proc, typename L, typename... Args>
      boost::optional<Ret> operator()(Proc& proc, L& lockable, Args&&... args) const {
        if (auto lock = scopelock(lockable)) {
          return proc(fwd<Args>(args)...);
        }
        return {};
      }
    };

    template<>
    struct scope_lock_proc_t<void> {
      template<typename Proc, typename L, typename... Args>
      void operator()(Proc& proc, L& lockable, Args&&... args) const {
        if (auto lock = scopelock(lockable)) {
          proc(fwd<Args>(args)...);
        }
      }
    };
  } // namespace detail

  /// Procedure wrapper that calls its underlying procedure only if the associated
  /// lockable could be locked.
  ///
  /// Example: weak_ptr as a lockable to perform lifetime protection
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// // std::shared_ptr<T> p;
  /// async(scope_lock_proc(
  ///   [](int i) mutable { // procedure to be called
  ///     return p->do_stuff(i);
  ///   },
  ///   mutable_store(std::weak_ptr<T>{p}) // lockable: will create a non-null shared_ptr on success
  ///   )
  /// );
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Procedure<T (...)> Proc,
  /// Mutable<ScopeLockable> M
  template<typename Proc, typename M>
  struct scope_lock_proc_t {
    Proc proc;
    M mut_lockable;
  // Regular (if members are):
    KA_GENERATE_FRIEND_REGULAR_OPS_2(scope_lock_proc_t, proc, mut_lockable)
  // Procedure:
    template<typename... Args>
    auto operator()(Args&&... args)
      -> decltype(detail::scope_lock_proc_t<Decay<decltype(proc(fwd<Args>(args)...))>>{}
                    (proc, src(mut_lockable), fwd<Args>(args)...)) { // TODO: Remove this when we can use C++14
      return detail::scope_lock_proc_t<Decay<decltype(proc(fwd<Args>(args)...))>>{}
        (proc, src(mut_lockable), fwd<Args>(args)...);
    }

    template<typename... Args>
    auto operator()(Args&&... args) const
      -> decltype(detail::scope_lock_proc_t<Decay<decltype(proc(fwd<Args>(args)...))>>{}
                    (proc, src(mut_lockable), fwd<Args>(args)...)) { // TODO: Remove this when we can use C++14
      return detail::scope_lock_proc_t<Decay<decltype(proc(fwd<Args>(args)...))>>{}
        (proc, src(mut_lockable), fwd<Args>(args)...);
    }
  };

  KA_DERIVE_CTOR_FUNCTION_TEMPLATE(scope_lock_proc)

  /// A polymorphic transformation constructed from an instance `l` of a
  /// ScopeLockable type that takes a procedure and returns an equivalent one
  /// that calls that procedure only if l was successfully locked and keeps the
  /// lock alive until the procedure returns.
  ///
  /// Example: mutex as a lockable to protect a asynchronous function call
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /// int global_count = 0;
  /// void increment_global_count() {
  ///   ++global_count;
  /// }
  ///
  /// template<typename P>
  /// void async_increment_ten_times(P sync_transfo) {
  ///   for (int i = 0; i < 10; ++i)
  ///     my::async(sync_transfo(increment_global_count));
  /// }
  ///
  /// void do_stuff() {
  ///   static std::mutex m;
  ///   async_increment_ten_times(scope_lock_transfo(&m));
  /// }
  /// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  ///
  /// Mutable<ScopeLockable> M
  template<typename M>
  struct scope_lock_transfo_t {
    M mut_lockable;
  // Regular (if members are regular):
    KA_GENERATE_FRIEND_REGULAR_OPS_1(scope_lock_transfo_t, mut_lockable)
  // PolymorphicTransformation:
    /// Procedure<T (...)> Proc0
    template<typename Proc>
    scope_lock_proc_t<Decay<Proc>, M> operator()(Proc&& p) {
      return { fwd<Proc>(p), mut_lockable };
    }

    /// Procedure<T (...)> Proc0
    template<typename Proc>
    scope_lock_proc_t<Decay<Proc>, M> operator()(Proc&& p) const {
      return { fwd<Proc>(p), mut_lockable };
    }
  };

  KA_DERIVE_CTOR_FUNCTION_TEMPLATE(scope_lock_transfo)
} // namespace ka

#endif // KA_FUNCTIONAL_HPP


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

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

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#include "dataset.hpp"

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

namespace bayesopt
{

  Dataset::Dataset(): mMinIndex(0), mMaxIndex(0) {};

  Dataset::Dataset(const matrixd& x, const vectord& y):
    mMinIndex(0), mMaxIndex(0)
  {
    setSamples(x,y);
  };

  Dataset::~Dataset(){};

  void Dataset::setSamples(const matrixd &x, const vectord &y)
  {
    // WARNING: It assumes mX is empty
    mY = y;
    for (size_t i=0; i<x.size1(); ++i)
      {
	mX.push_back(row(x,i));
	updateMinMax(i);
      }
  };

  void Dataset::setSamples(const vectord &y)
  {
    mY = y;
    for (size_t i=0; i<y.size(); ++i)
      {
	updateMinMax(i);
      }
  };

  void Dataset::setSamples(const matrixd &x)
  {
    for (size_t i=0; i<x.size1(); ++i)
      {
	mX.push_back(row(x,i));
      }
  };

  void Dataset::plotData(TLogLevel level)
  {
    // For logging purpose
    FILE_LOG(level) << "Initial points:" ;
    for(size_t i = 0; i < mY.size(); i++)
      {
	FILE_LOG(level) << "X:" << mX[i]
			<< "|Y:" << mY(i);
      }
    const double yPoint = getValueAtMinimum();
    const vectord xPoint = getPointAtMinimum();
    FILE_LOG(level) << "Best point so far:" ;
    FILE_LOG(level) << "X:" << xPoint
		    << "|Y:" << yPoint;

  } // plotData

} //namespace bayesopt


#ifndef PARSER_REGULAR_GRAMMAR_HPP
#define PARSER_REGULAR_GRAMMAR_HPP

#include <iostream>
#include <boost/spirit/home/x3.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>

#include <boost/spirit/include/support_istream_iterator.hpp>
#include <iostream>
#include <sstream>
#include <fstream>

#include <formal_languages/devices/Grammar.hpp>

namespace formal_device
{
namespace grammar
{

using string_type = Regular::symbol_type::string_type;

namespace ast
{
struct Production
{
    string_type m_production;
};

struct Line
{
    string_type m_symbol;
    std::vector<Production> m_productions;
};

struct Document
{
    std::vector<Line> m_lines;
};
}   // namespace ast
}   // namespace grammar
}   // namespace formal_device

BOOST_FUSION_ADAPT_STRUCT(formal_device::grammar::ast::Production, m_production)
BOOST_FUSION_ADAPT_STRUCT(formal_device::grammar::ast::Line, m_symbol, m_productions)
BOOST_FUSION_ADAPT_STRUCT(formal_device::grammar::ast::Document, m_lines)

namespace formal_device
{
namespace grammar
{
namespace parser
{
    Regular make_regular_grammar(const string_type & file);

    namespace x3    = boost::spirit::x3;
    namespace ascii = x3::ascii;

    x3::rule<class production_, ast::Production> production{"production"};
    x3::rule<class line_, ast::Line>             line{"line"};
    x3::rule<class document_, ast::Document>     document{"document"};

    const auto identifier     = x3::lexeme[+x3::char_("a-zA-Z")];
    const auto production_def = identifier;
    const auto line_def       = identifier >> x3::lit("->") >> production % "|";
    const auto document_def   = line % x3::eol;

    BOOST_SPIRIT_DEFINE(production, line, document);
}   // namespace ast
}   // namespace grammar
}   // namespace formal_devices

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_LOWER_TRISOLVE_INCLUDE
#define MTL_LOWER_TRISOLVE_INCLUDE

#include <boost/mpl/int.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>

#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace mat {

namespace detail {

    /// Class that implements lower trisolver
    /** DiaTag can be tag::regular_diagonal, tag::unit_diagonal, or tag::inverse_diagonal.
	CompactStorage means that matrix contains only lower entries (strict lower when DiaTag == unit_diagonal). \sa \ref trisolve_object **/
    template <typename Matrix, typename DiaTag, bool CompactStorage= false>
    struct lower_trisolve_t
    {
	MTL_STATIC_ASSERT((boost::is_same<DiaTag, tag::regular_diagonal>::value
			   || boost::is_same<DiaTag, tag::unit_diagonal>::value
			   || boost::is_same<DiaTag, tag::inverse_diagonal>::value),
			  "DiaTag must be either tag::regular_diagonal, tag::unit_diagonal, or tag::inverse_diagonal.");

	typedef typename Collection<Matrix>::value_type         	       	    value_type;
	typedef typename Collection<Matrix>::size_type          	       	    size_type;
	typedef typename OrientedCollection<Matrix>::orientation	       	    my_orientation;
	typedef typename mtl::traits::category<Matrix>::type    	       	    my_category;
	typedef typename mtl::traits::range_generator<tag::major, Matrix>::type     a_cur_type; // row or col depending on Matrix
	typedef typename mtl::traits::range_generator<tag::nz, a_cur_type>::type    a_icur_type;

	/// Construction from matrix \p A
	lower_trisolve_t(const Matrix& A) : A(A), value_a(A), col_a(A), row_a(A)
	{    MTL_THROW_IF(num_rows(A) != num_cols(A), matrix_not_square());	}

	template <typename M, typename D, bool C>
	struct generic_version
	  : boost::mpl::int_<(mtl::traits::is_row_major<M>::value ? 0 : 2)
	                     + (boost::is_same<D, tag::unit_diagonal>::value ? 1 : 2)
                            > {};

	template <typename M, typename D, bool C>
	struct version
	  : generic_version<M, D, C> {};

	template <typename Value, typename Para, typename D>
	struct version<compressed2D<Value, Para>, D, true>
	  : boost::mpl::if_<mtl::traits::is_row_major<Para>,
			    typename boost::mpl::if_<boost::is_same<D, tag::unit_diagonal>,
						     boost::mpl::int_<5>,
						     boost::mpl::int_<6> >::type,
			    generic_version<compressed2D<Value, Para>, D, true>
	                   >::type {};

	/// Solve \p w = A * \p v
	template <typename VectorIn, typename VectorOut>
	void operator()(const VectorIn& v, VectorOut& w) const
	{   vampir_trace<5022> tracer; apply(v, w, version<Matrix, DiaTag, CompactStorage>()); }

      private:
	template <typename Value>
	Value inline lower_trisolve_diavalue(const Value& v, tag::regular_diagonal) const
	{   using math::reciprocal; return reciprocal(v); }

	template <typename Value>
	Value lower_trisolve_diavalue(const Value& v, tag::inverse_diagonal) const
	{  return v;	}

	template <typename Tag> int dia_inc(Tag) { return 0; }
	int dia_inc(tag::unit_diagonal) { return 1; }

	// Generic row-major unit_diagonal
	template <typename VectorIn, typename VectorOut>
	void apply(const VectorIn& v, VectorOut& w, boost::mpl::int_<1>) const
	{
	    using namespace tag;
	    a_cur_type ac= begin<row>(A), aend= end<row>(A);
	    for (size_type r= 0; ac != aend; ++r, ++ac) {
		a_icur_type aic= begin<nz>(ac), aiend= CompactStorage ? end<nz>(ac) : lower_bound<nz>(ac, r);
		typename Collection<VectorOut>::value_type rr= v[r];
		for (; aic != aiend; ++aic) {
		    MTL_DEBUG_THROW_IF(col_a(*aic) >= r, logic_error("Matrix entries must be sorted for this."));
		    rr-= value_a(*aic) * w[col_a(*aic)];
		}
		w[r]= rr;
	    }
	}

	// Generic row-major not unit_diagonal
	template <typename VectorIn, typename VectorOut>
	void apply(const VectorIn& v, VectorOut& w, boost::mpl::int_<2>) const
	{
	    using namespace tag;
	    a_cur_type ac= begin<row>(A), aend= end<row>(A);
	    for (size_type r= 0; ac != aend; ++r, ++ac) {
		a_icur_type aic= begin<nz>(ac), aiend= CompactStorage ? end<nz>(ac) : lower_bound<nz>(ac, r+1);
		MTL_THROW_IF(aic == aiend, missing_diagonal());
		--aiend;
		MTL_THROW_IF(col_a(*aiend) != r, missing_diagonal());

		value_type dia= value_a(*aiend);
		typename Collection<VectorOut>::value_type rr= v[r];

		for (; aic != aiend; ++aic) {
		    MTL_DEBUG_THROW_IF(col_a(*aic) >= r, logic_error("Matrix entries must be sorted for this."));
		    rr-= value_a(*aic) * w[col_a(*aic)];
		}
		w[r]= rr * lower_trisolve_diavalue(dia, DiaTag());
	    }
	}

	// Generic column-major unit_diagonal
	template <typename VectorIn, typename VectorOut>
	void apply(const VectorIn& v, VectorOut& w, boost::mpl::int_<3>) const
	{
	    using namespace tag;
	    w= v;
	    a_cur_type ac= begin<col>(A), aend= end<col>(A);
	    for (size_type r= 0; ac != aend; ++r, ++ac) {
		a_icur_type aic= CompactStorage ? begin<nz>(ac) : lower_bound<nz>(ac, r+1), aiend= end<nz>(ac);
		typename Collection<VectorOut>::value_type rr= w[r];

		for (; aic != aiend; ++aic) {
		    MTL_DEBUG_THROW_IF(row_a(*aic) <= r, logic_error("Matrix entries must be sorted for this."));
		    w[row_a(*aic)]-= value_a(*aic) * rr;
		}
	    }
	}

	// Generic column-major not unit_diagonal
	template <typename VectorIn, typename VectorOut>
	void apply(const VectorIn& v, VectorOut& w, boost::mpl::int_<4>) const
	{
	    using namespace tag;
	    w= v;
	    a_cur_type ac= begin<col>(A), aend= end<col>(A);
	    for (size_type r= 0; ac != aend; ++r, ++ac) {
		a_icur_type aic= CompactStorage ? begin<nz>(ac) : lower_bound<nz>(ac, r), aiend= end<nz>(ac);
		MTL_DEBUG_THROW_IF(aic == aiend || row_a(*aic) != r, missing_diagonal());
		typename Collection<VectorOut>::value_type rr= w[r]*= lower_trisolve_diavalue(value_a(*aic), DiaTag());

		for (++aic; aic != aiend; ++aic) {
		    MTL_DEBUG_THROW_IF(row_a(*aic) <= r, logic_error("Matrix entries must be sorted for this."));
		    w[row_a(*aic)]-= value_a(*aic) * rr;
		}
	    }
	}

	// Tuning for IC_0 and similar using compressed2D row-major compact with implicit unit diagonal
	template <typename VectorIn, typename VectorOut>
	void apply(const VectorIn& v, VectorOut& w, boost::mpl::int_<5>) const
	{
	    vampir_trace<5048> tracer;
	    if (num_rows(A) == 0) return;
	    size_type j1= A.ref_major()[1];
	    for (size_type r= 0, rend= num_rows(A); r != rend; ++r) {
		size_type j0= j1;
		j1= A.ref_major()[r+1];
		typename Collection<VectorOut>::value_type rr= v[r];
		for (; j0 != j1; ++j0) {
		    MTL_DEBUG_THROW_IF(A.ref_minor()[j0] > r, logic_error("Matrix entries from U in lower triangular."));
		    rr-= A.data[j0] * w[A.ref_minor()[j0]];
		}
		w[r]= rr;
	    }
	}

	// Tuning for IC_0 and similar using compressed2D row-major compact with explicitly stored diagonal (possibly already inverted)
	template <typename VectorIn, typename VectorOut>
	void apply(const VectorIn& v, VectorOut& w, boost::mpl::int_<6>) const
	{
	    vampir_trace<5047> tracer;
	    for (size_type r= 0, rend= num_rows(A); r != rend; ++r) {
		size_type j0= A.ref_major()[r], j1= A.ref_major()[r+1];
		MTL_THROW_IF(j0 == j1, missing_diagonal());
		--j1;
		MTL_THROW_IF(A.ref_minor()[j1] != r, missing_diagonal());
		value_type dia= A.data[j1];
		typename Collection<VectorOut>::value_type rr= v[r];
		for (; j0 != j1; ++j0) {
		    MTL_DEBUG_THROW_IF(A.ref_minor()[j0] > r, logic_error("Matrix entries from U in lower triangular."));
		    rr-= A.data[j0] * w[A.ref_minor()[j0]];
		}
		w[r]= rr * lower_trisolve_diavalue(dia, DiaTag());
	    }
	}

	const Matrix&                                    A;
	typename mtl::traits::const_value<Matrix>::type  value_a;
	typename mtl::traits::col<Matrix>::type          col_a;
	typename mtl::traits::row<Matrix>::type          row_a;
    };

}  // detail

/// Solves the lower triangular matrix A  with the rhs v and returns the solution vector
template <typename Matrix, typename Vector>
Vector inline lower_trisolve(const Matrix& A, const Vector& v)
{
    Vector w(resource(v));
    detail::lower_trisolve_t<Matrix, tag::regular_diagonal> solver(A);
    solver(v, w);
    return w;
}

/// Solves the lower triangular matrix A  with the rhs v
template <typename Matrix, typename VectorIn, typename VectorOut>
inline void lower_trisolve(const Matrix& A, const VectorIn& v, VectorOut& w)
{
    detail::lower_trisolve_t<Matrix, tag::regular_diagonal> solver(A);
    solver(v, w);
}

/// Solves the lower triangular matrix A (only one's in the diagonal) with the rhs v and returns the solution vector
template <typename Matrix, typename Vector>
Vector inline unit_lower_trisolve(const Matrix& A, const Vector& v)
{
    Vector w(resource(v));
    detail::lower_trisolve_t<Matrix, tag::unit_diagonal> solver(A);
    solver(v, w);
    return w;
}

/// Solves the lower triangular matrix A (only one's in the diagonal) with the rhs v and returns the solution vector
template <typename Matrix, typename VectorIn, typename VectorOut>
inline void unit_lower_trisolve(const Matrix& A, const VectorIn& v, VectorOut& w)
{
    detail::lower_trisolve_t<Matrix, tag::unit_diagonal> solver(A);
    solver(v, w);
}

/// Solves the lower triangular matrix A (inverse the diagonal) with the rhs v and returns the solution vector
template <typename Matrix, typename Vector>
Vector inline inverse_lower_trisolve(const Matrix& A, const Vector& v)
{
    Vector w(resource(v));
    detail::lower_trisolve_t<Matrix, tag::inverse_diagonal> solver(A);
    solver(v, w);
    return w;
}

/// Solves the lower triangular matrix A (inverse the diagonal) with the rhs v and returns the solution vector
template <typename Matrix, typename VectorIn, typename VectorOut>
inline void inverse_lower_trisolve(const Matrix& A, const VectorIn& v, VectorOut& w)
{
    detail::lower_trisolve_t<Matrix, tag::inverse_diagonal> solver(A);
    solver(v, w);
}

template <typename Matrix, typename Vector, typename DiaTag>
Vector inline lower_trisolve(const Matrix& A, const Vector& v, DiaTag)
{
    Vector w(resource(v));
    detail::lower_trisolve_t<Matrix, DiaTag> solver(A);
    solver(v, w);
    return w;
}

template <typename Matrix, typename VectorIn, typename VectorOut, typename DiaTag>
inline void lower_trisolve(const Matrix& A, const VectorIn& v, VectorOut& w, DiaTag)
{
    detail::lower_trisolve_t<Matrix, DiaTag> solver(A);
    solver(v, w);
}

}} // namespace mtl::matrix

#endif // MTL_LOWER_TRISOLVE_INCLUDE