// 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_DENSE2D_INCLUDE
#define MTL_DENSE2D_INCLUDE

#include <algorithm>
#include <boost/mpl/bool.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>
#include <boost/utility/enable_if.hpp>

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/matrix/crtp_base_matrix.hpp>
#include <boost/numeric/mtl/matrix/base_sub_matrix.hpp>
#include <boost/numeric/mtl/matrix/all_mat_expr.hpp>
#include <boost/numeric/mtl/matrix/operators.hpp>
#include <boost/numeric/mtl/detail/contiguous_memory_block.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/operation/compute_factors.hpp>
#include <boost/numeric/mtl/operation/clone.hpp>
#include <boost/numeric/mtl/operation/is_negative.hpp>
#include <boost/numeric/mtl/utility/common_include.hpp>
#include <boost/numeric/mtl/utility/assert.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/is_static.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/utility/dense_el_cursor.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>
#include <boost/numeric/mtl/utility/strided_dense_el_cursor.hpp>
#include <boost/numeric/mtl/utility/strided_dense_el_iterator.hpp>
#include <boost/numeric/mtl/utility/transposed_orientation.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>

#ifdef MTL_WITH_INITLIST
# include <initializer_list>
#endif
#include <algorithm>

// Forward declaration (for friend declaration)
namespace mtl { namespace traits { namespace detail {
    template <typename, typename, bool> struct dense2D_iterator_range_generator;
}}}

namespace mtl { namespace mat {

using std::size_t;

// Forward declarations
template <typename Value, typename Parameters> class dense2D;
class dense2D_indexer;

// Helper type
struct dense2D_sub_ctor {};

// Indexing for dense matrices
class dense2D_indexer
{
    // helpers for public functions
    size_t offset(size_t ldim, size_t r, size_t c, row_major) const
    {
	return r * ldim + c;
    }
    size_t offset(size_t ldim, size_t r, size_t c, col_major) const
    {
	return c * ldim + r;
    }

    size_t row(size_t offset, size_t ldim, row_major) const
    {
	return offset / ldim;
    }
    size_t row(size_t offset, size_t ldim, col_major) const
    {
	return offset % ldim;
    }

    size_t col(size_t offset, size_t ldim, row_major) const
    {
	return offset % ldim;
    }
    size_t col(size_t offset, size_t ldim, col_major) const
    {
	return offset / ldim;
    }

 public:
    template <typename Value, class Parameters>
    size_t operator() (const dense2D<Value, Parameters>& ma, size_t r, size_t c) const
    {
	typedef dense2D<Value, Parameters> matrix_type;
	// convert into c indices
	typename matrix_type::index_type my_index;
	size_t my_r= index::change_from(my_index, r);
	size_t my_c= index::change_from(my_index, c);
	return offset(ma.ldim, my_r, my_c, typename matrix_type::orientation());
    }

    template <typename Value, class Parameters>
    size_t row(const dense2D<Value, Parameters>& ma,
	       typename dense2D<Value, Parameters>::key_type key) const
    {
	typedef dense2D<Value, Parameters> matrix_type;
	// row with c-index for my orientation
	size_t r= row(ma.offset(key), ma.ldim, typename matrix_type::orientation());
	return index::change_to(typename matrix_type::index_type(), r);
    }

    template <typename Value, class Parameters>
    size_t col(const dense2D<Value, Parameters>& ma,
	       typename dense2D<Value, Parameters>::key_type key) const
    {
	typedef dense2D<Value, Parameters> matrix_type;
	// column with c-index for my orientation
	size_t c= col(ma.offset(key), ma.ldim, typename matrix_type::orientation());
	return index::change_to(typename matrix_type::index_type(), c);
    }
    template <typename, typename> friend class dense2D;
}; // dense2D_indexer

namespace detail
{

    // Compute required memory
    // Enabling mechanism to make sure that computation is valid
    template <typename Parameters, bool Enable>
    struct dense2D_array_size {
	static std::size_t const value= 0;
    };

    template <typename Parameters>
    struct dense2D_array_size<Parameters, true>
    {
	typedef typename Parameters::dimensions   dimensions;
	MTL_STATIC_ASSERT((dimensions::is_static), "Size must be known at compile time.");
	static std::size_t const value= dimensions::Num_Rows * dimensions::Num_Cols;
    };

    // return const-ref if matrix on stack and type itself if on heap
    template <typename Matrix, bool on_stack>
    struct ref_on_stack
    {
	typedef Matrix type;
    };

    template <typename Matrix>
    struct ref_on_stack<Matrix, true>
    {
	typedef const Matrix& type;
    };

} // namespace detail

/// Dense matrix type
template <typename Value, typename Parameters = parameters<> >
class dense2D
    : public base_sub_matrix<Value, Parameters>,
    public mtl::detail::contiguous_memory_block< Value, Parameters::on_stack,
    detail::dense2D_array_size<Parameters, Parameters::on_stack>::value >,
    public crtp_base_matrix< dense2D<Value, Parameters>, Value, std::size_t >,
    public mat_expr< dense2D<Value, Parameters> >
{
    typedef dense2D                                           self;
    typedef base_sub_matrix<Value, Parameters>                super;
    typedef mtl::detail::contiguous_memory_block<Value, Parameters::on_stack,
	detail::dense2D_array_size<Parameters, Parameters::on_stack>::value>     memory_base;
    typedef mat_expr< dense2D<Value, Parameters> >            expr_base;
    typedef crtp_base_matrix< self, Value, std::size_t >      crtp_base;
    typedef crtp_matrix_assign< self, Value, std::size_t >    assign_base;
public:
    typedef Parameters                        parameters;
    typedef typename Parameters::orientation  orientation;
    typedef typename Parameters::index        index_type;
    typedef typename Parameters::dimensions   dim_type;
    typedef Value                             value_type;
    typedef const value_type&                 const_reference;
    typedef value_type&                       reference;

    typedef const value_type*                 const_pointer_type;
    typedef const_pointer_type                key_type;
    typedef std::size_t                       size_type;
    typedef dense_el_cursor<Value>            el_cursor_type;
    typedef dense2D_indexer                   indexer_type;

    // Self-similar type unless dimension is fixed
    // Not supported for the moment
    typedef self                              sub_matrix_type;

protected:
    // Obviously, the next 3 functions must be called after setting dimensions
    void set_nnz() { this->my_nnz = this->num_rows() * this->num_cols(); }
    void set_ldim(row_major) { ldim = this->num_cols(); }
    void set_ldim(col_major) { ldim = this->num_rows(); }
    void set_ldim() { set_ldim(orientation()); }

    void init()
    {
	set_nnz(); set_ldim(); // set_to_zero(*this);
    }

public:
    /// Default constructor, if compile time matrix size allocate memory
    dense2D() : memory_base(dim_type().num_rows() * dim_type().num_cols())
    {
	init();
    }

    /// Constructor that only sets dimensions, only for run-time dimensions
    explicit dense2D(mtl::non_fixed::dimensions d)
	: super(d), memory_base(d.num_rows() * d.num_cols())
    {
	init();
    }

    /// Most common constructor from number of rows and columns
    explicit dense2D(size_type num_rows, size_type num_cols)
	: super(dim_type(num_rows, num_cols)),
	memory_base(num_rows * num_cols)
    {
	init();
    }

    /// Constructor that sets dimensions and pointer to external data
    explicit dense2D(mtl::non_fixed::dimensions d, value_type* a)
	: super(d), memory_base(a, d.num_rows() * d.num_cols())
    {
	init();
    }

    /// Constructor that sets dimensions and pointer to external data
    explicit dense2D(size_type num_rows, size_type num_cols, value_type* a)
	: super(mtl::non_fixed::dimensions(num_rows, num_cols)), memory_base(a, num_rows * num_cols)
    {
	init();
    }

    /// Constructor for compile time matrix size
    /** sets dimensions and pointer to external data **/
    explicit dense2D(value_type* a)
	: super(), memory_base(a, dim_type().num_rows() * dim_type().num_cols())
    {
	MTL_STATIC_ASSERT((dim_type::is_static), "Size must be known at compile time.");
	init();
    }

    /// Default copy constructor
    dense2D(const self& m)
	: super(dim_type(m.num_rows(), m.num_cols())),
	memory_base(m)
    {
	// In case of sub-matrices we need m's ldim -> init doesn't work
	this->my_nnz = m.my_nnz; ldim = m.ldim;
    }

    /// Clone constructor, copies every source including sub-matrices and other matrices with references
    explicit dense2D(const self& m, clone_ctor)
	: super(mtl::non_fixed::dimensions(m.num_rows(), m.num_cols())),
	memory_base(m, clone_ctor())
    {
	init();
	*this = m;
    }

    /// General copy constructor, uses functionality from CRTP base
    template <typename MatrixSrc>
    explicit dense2D(const MatrixSrc& src)
	: super(), memory_base(dim_type().num_rows() * dim_type().num_cols())
    {
	init();
	*this = src;
    }

#ifdef MTL_WITH_MOVE
    /// Move constructor
    dense2D(self&& src)
	: super(std::move(src)), memory_base(std::move(src)), ldim(src.ldim)
    {}
#endif

    /// Constructor for creating sub-matrices
    template <typename MatrixSrc>
    dense2D(MatrixSrc& matrix, dense2D_sub_ctor,
	    size_type begin_r, size_type end_r, size_type begin_c, size_type end_c)
      : super(mtl::non_fixed::dimensions(matrix.num_rows(), matrix.num_cols())),
        memory_base(matrix.data, (end_r - begin_r) * (end_c - begin_c), true)
    {
	sub_matrix_constructor(matrix, begin_r, end_r, begin_c, end_c, boost::mpl::bool_<memory_base::on_stack>());
    }

#if defined(MTL_WITH_INITLIST) && defined(MTL_WITH_AUTO) && defined(MTL_WITH_RANGEDFOR)
    /// Constructor for initializer list \p values
    template <typename Value2>
    dense2D(std::initializer_list<std::initializer_list<Value2> > values)
      : super(mtl::non_fixed::dimensions(values.size(), values.size()? values.begin()->size() : 0)),
    	memory_base(this->num_rows() * this->num_cols())
    {
    	init();
	*this= values;
    }
#endif

  private:
    template <typename MatrixSrc>
    void sub_matrix_constructor(MatrixSrc& matrix, size_type begin_r, size_type end_r,
				size_type begin_c, size_type end_c, boost::mpl::false_)
    {
	matrix.check_ranges(begin_r, end_r, begin_c, end_c);

	if(end_r <= begin_r || end_c <= begin_c)
	    set_ranges(0, 0);
	else {
	    // Leading dimension doesn't change
	    this->data += matrix.indexer(matrix, begin_r, begin_c);  // Takes care of indexing
	    set_ranges(end_r - begin_r, end_c - begin_c);
	}
	this->my_nnz= matrix.nnz(); ldim= matrix.get_ldim();
    }

    template <typename MatrixSrc>
    void sub_matrix_constructor(MatrixSrc&, size_type, size_type,
				size_type, size_type, boost::mpl::true_)
    {
	MTL_THROW(logic_error("Matrices cannot be used as sub-matrices!"));
    }

  public:

#ifdef MTL_WITH_MOVE
    /// Move assignment for data on heap
    self& operator=(self&& src)
    {	return self_assign(src, boost::mpl::bool_<memory_base::on_stack>());    }

    /// (Copy) Assignment
    self& operator=(const self& src)
    {	return self_assign(src, boost::mpl::true_());    }

#else
    /// (Copy) Assignment
    self& operator=(typename detail::ref_on_stack<self, memory_base::on_stack>::type src)
    {
	return self_assign(src, boost::mpl::bool_<memory_base::on_stack>());
    }
#endif

  private:
    // Already copied for lvalues -> data can be stolen (need non-const ref)
    self& self_assign(self& src, boost::mpl::false_)
    {
	// Self-copy would be an indication of an error
	assert(this != &src);
	// std::cout << "In move assignment: this* = \n" << *this << "src = \n" << src;

	this->checked_change_dim(src.num_rows(), src.num_cols());
	if (this->category == memory_base::view || src.category == memory_base::view)
	    matrix_copy(src, *this);
	else {
	    if (this->num_rows() != src.num_rows() || this->num_cols() != src.num_cols()) {
		super::change_dim(src.num_rows(), src.num_cols());
		init();
	    }
	    memory_base::move_assignment(src);
	}
	// std::cout << "End of move assignment: this* = \n" << *this;
	return *this;
    }

    // For matrices with data on stack (or lvalues in C++11)
    self& self_assign(const self& src, boost::mpl::true_)
    {
	if (this != &src) {
	    this->checked_change_dim(src.num_rows(), src.num_cols());
	    matrix_copy(src, *this);
	}
	return *this;
    }
  public:

    // import operators from CRTP base class
#if 0 // def __PGI
    using crtp_base::operator=;
#else
    using assign_base::operator=;
#endif

    /// Change dimension, can keep old data
    void change_dim(size_type r, size_type c, bool keep_data = false)
    {
	change_dim(r, c, keep_data, mtl::traits::is_static<self>());
    }

  private:
    void change_dim(size_type r, size_type c, bool keep_data, boost::mpl::false_)
    {
	if (r == this->num_rows() && c == this->num_cols())
	    return;

	self temp;
	if (keep_data) {
	    temp.super::change_dim(this->num_rows(), this->num_cols());
	    temp.init();
	    temp.memory_base::move_assignment(*this);
	}
	memory_base::realloc(r*c);
	super::change_dim(r, c);
	init();
	if (keep_data) {
	    if (r > temp.num_rows() || c > temp.num_cols()){
		set_to_zero(*this);
#if 0
		irange rr(0, std::min(r,temp.num_rows())), cr(0, std::min(c,temp.num_cols()));
		*this[rr][cr]= temp[rr][cr];
#endif
		sub_matrix(*this,0,std::min(r,temp.num_rows()),0,std::min(c,temp.num_cols()))
		    = sub_matrix(temp,0,std::min(r,temp.num_rows()),0,std::min(c,temp.num_cols()));
	    } else
		*this = temp[irange(0, r)][irange(0, c)];
	}
    }

    void change_dim(size_type MTL_DEBUG_ARG(r), size_type MTL_DEBUG_ARG(c), bool, boost::mpl::true_)
    {	assert(r == this->num_rows() && c == this->num_cols());    }

 public:
    /// Check whether indices r and c are in range
    bool check_indices(size_t r, size_t c) const
    {	return r >= this->begin_row() && r < this->end_row() && c >= this->begin_col() && c < this->end_col();    }

    /// Constant access to element
    const_reference operator() (size_t r, size_t c) const
    {
	MTL_CRASH_IF(is_negative(r) || r >= this->num_rows() || is_negative(c) || c >= this->num_cols(),
		     "Index out of range!");
        return this->data[indexer(*this, r, c)];
    }

    /// Mutable access to element
    value_type& operator() (size_t r, size_t c)
    {
	MTL_CRASH_IF(is_negative(r) || r >= this->num_rows() || is_negative(c) || c >= this->num_cols(),
		     "Index out of range!");
	return this->data[indexer(*this, r, c)];
    }

    // offset regarding c-style indices
    size_t c_offset(size_t r, size_t c) const
    {	return indexer.offset(ldim, r, c, orientation());    }

    /// Get lower dimension [advanced]
    size_type get_ldim() const
    {	return ldim;    }

    /// Swap two matrices
    friend void swap(self& matrix1, self& matrix2)
    {
	swap(static_cast<memory_base&>(matrix1), static_cast<memory_base&>(matrix2));
	swap(static_cast<super&>(matrix1), static_cast<super&>(matrix2));
	std::swap(matrix1.ldim, matrix2.ldim);
    }

    void crop() {} ///< Delete structural zeros; only dummy here

    /// Address of first data entry (mutable); to be used with care. [advanced]
    value_type* address_data() { return this->data; }
    /// Address of first data entry (constant); to be used with care. [advanced]
    const value_type* address_data() const { return this->data; }

    /// Whether data is stored in strides
    bool has_strided_data() const { return this->category != this->own; }

  protected:

    // Set ranges from begin_r to end_r and begin_c to end_c
    void set_ranges(size_type begin_r, size_type end_r, size_type begin_c, size_type end_c)
    {
	super::set_ranges(begin_r, end_r, begin_c, end_c);
	set_nnz();
    }

    // Set ranges to a num_row x num_col matrix, keeps indexing
    void set_ranges(size_type num_rows, size_type num_cols)
    {
	set_ranges(this->begin_row(), this->begin_row() + num_rows,
		   this->begin_col(), this->begin_col() + num_cols);
    }

  public:

    indexer_type  indexer;

    friend class dense2D_indexer;

#if !defined(_MSC_VER) || _MSC_VER != 1400 // Bug in MSVC 2005
    template <typename> friend struct sub_matrix_t;
    template <typename, typename> friend struct mtl::traits::range_generator;
    template <typename, typename, bool> friend struct mtl::traits::detail::dense2D_iterator_range_generator;

  protected:
#endif

    // Leading dimension is minor dimension in original matrix
    // Opposed to other dims doesn't change in sub-matrices
    size_type     ldim;

}; // dense2D

// ================
// Free functions
// ================

/// Number of rows
template <typename Value, typename Parameters>
typename dense2D<Value, Parameters>::size_type
inline num_rows(const dense2D<Value, Parameters>& matrix)
{
    return matrix.num_rows();
}

/// Number of columns
template <typename Value, typename Parameters>
typename dense2D<Value, Parameters>::size_type
inline num_cols(const dense2D<Value, Parameters>& matrix)
{
    return matrix.num_cols();
}

/// Size of the matrix, i.e. the number of row times columns
template <typename Value, typename Parameters>
typename dense2D<Value, Parameters>::size_type
inline size(const dense2D<Value, Parameters>& matrix)
{
    return matrix.num_cols() * matrix.num_rows();
}

}

using mat::dense2D;

} // namespace mtl::matrix

namespace mtl { namespace traits {

    // VC 8.0 finds ambiguity with mtl::tag::dense2D (I wonder why, especially here)
    using mtl::mat::dense2D;

    // ================
    // Range generators
    // For cursors
    // ================

    template <typename Value, typename Parameters>
    struct range_generator<glas::tag::all, dense2D<Value, Parameters> >
      : detail::dense_element_range_generator<dense2D<Value, Parameters>,
					      dense_el_cursor<Value>, complexity_classes::linear_cached>
    {};

    template <typename Value, typename Parameters>
    struct range_generator<glas::tag::nz, dense2D<Value, Parameters> >
      : detail::dense_element_range_generator<dense2D<Value, Parameters>,
					      dense_el_cursor<Value>, complexity_classes::linear_cached>
    {};

    namespace detail
    {
	// complexity of dense row cursor depends on storage scheme
	// if orientation is row_major then complexity is cached_linear, otherwise linear
	template <typename Orientation> struct dense2D_rc {};
	template<> struct dense2D_rc<row_major>
	{
	    typedef complexity_classes::linear_cached type;
	};
	template<> struct dense2D_rc<col_major>
	{
	    typedef complexity_classes::linear type;
	};

	// Complexity of column cursor is of course opposite
	template <typename Orientation> struct dense2D_cc
	    : dense2D_rc<typename mtl::traits::transposed_orientation<Orientation>::type>
	{};
    }

    template <typename Value, typename Parameters>
    struct range_generator<glas::tag::row, dense2D<Value, Parameters> >
	: detail::all_rows_range_generator<dense2D<Value, Parameters>,
					   typename detail::dense2D_rc<typename Parameters::orientation>::type>
    {};

    // For a cursor pointing to some row give the range of elements in this row
    template <typename Value, typename Parameters>
    struct range_generator<glas::tag::nz,
			   detail::sub_matrix_cursor<dense2D<Value, Parameters>, glas::tag::row, 2> >
    {
	typedef dense2D<Value, Parameters>                                            matrix;
	typedef typename matrix::size_type                                            size_type;
	typedef detail::sub_matrix_cursor<matrix, glas::tag::row, 2>               cursor;

	// linear for col_major and linear_cached for row_major
	typedef typename detail::dense2D_rc<typename Parameters::orientation>::type   complexity;
	static int const                                                              level = 1;

	typedef typename boost::mpl::if_<
	    boost::is_same<typename Parameters::orientation, row_major>
	  , dense_el_cursor<Value>
	  , strided_dense_el_cursor<Value>
	>::type type;

      private:

	type dispatch(cursor const& c, size_type col, row_major) const
	{
	    return type(c.ref, c.key, col);
	}
	type dispatch(cursor const& c, size_type col, col_major) const
	{
	    return type(c.ref, c.key, col, c.ref.ldim);
	}

      public:

	type begin(cursor const& c) const
	{
	    return dispatch(c, c.ref.begin_col(), typename matrix::orientation());
	}
	type end(cursor const& c) const
	{
	    return dispatch(c, c.ref.end_col(), typename matrix::orientation());
	}
	type lower_bound(cursor const& c, size_type position) const
	{
	    return dispatch(c, std::min(c.ref.end_col(), position), typename matrix::orientation());
	}
    };

    template <typename Value, typename Parameters>
    struct range_generator<glas::tag::all,
			   detail::sub_matrix_cursor<dense2D<Value, Parameters>, glas::tag::row, 2> >
        : range_generator<glas::tag::nz,
			  detail::sub_matrix_cursor<dense2D<Value, Parameters>, glas::tag::row, 2> >
    {};

    template <typename Value, typename Parameters>
    struct range_generator<glas::tag::col, dense2D<Value, Parameters> >
	: detail::all_cols_range_generator<dense2D<Value, Parameters>,
					   typename detail::dense2D_cc<typename Parameters::orientation>::type>
    {};

    // For a cursor pointing to some row give the range of elements in this row
    template <typename Value, typename Parameters>
    struct range_generator<glas::tag::nz,
			   detail::sub_matrix_cursor<dense2D<Value, Parameters>, glas::tag::col, 2> >
    {
	typedef dense2D<Value, Parameters>                                            matrix;
	typedef typename matrix::size_type                                            size_type;
	typedef detail::sub_matrix_cursor<matrix, glas::tag::col, 2>               cursor;
	typedef typename detail::dense2D_cc<typename Parameters::orientation>::type   complexity;
	static int const                                                              level = 1;

	typedef typename boost::mpl::if_<
	    boost::is_same<typename Parameters::orientation, col_major>
	  , dense_el_cursor<Value>
	  , strided_dense_el_cursor<Value>
	>::type type;

      private:
	type dispatch(cursor const& c, size_type row, col_major) const
	{
	    return type(c.ref, row, c.key);
	}
	type dispatch(cursor const& c, size_type row, row_major) const
	{
	    return type(c.ref, row, c.key, c.ref.ldim);
	}

      public:
	type begin(cursor const& c) const
	{
	    return dispatch(c, c.ref.begin_row(), typename matrix::orientation());
	}
	type end(cursor const& c) const
	{
	    return dispatch(c, c.ref.end_row(), typename matrix::orientation());
	}
	type lower_bound(cursor const& c, size_type position) const
	{
	    return dispatch(c, std::min(c.ref.end_row(), position), typename matrix::orientation());
	}
    };

    template <typename Value, typename Parameters>
    struct range_generator<glas::tag::all,
			   detail::sub_matrix_cursor<dense2D<Value, Parameters>, glas::tag::col, 2> >
      : public range_generator<glas::tag::nz,
			       detail::sub_matrix_cursor<dense2D<Value, Parameters>, glas::tag::col, 2> >
    {};

// =============
// For iterators
// =============

    namespace detail {

        // Traversal along major dimension first and then along minor
        template <typename OuterTag, typename Orientation>
        struct major_traversal
        {
	    static const bool value= false;
        };

        template <> struct major_traversal<glas::tag::row, row_major>
        {
	    static const bool value= true;
        };

        template <> struct major_traversal<glas::tag::col, col_major>
        {
	    static const bool value= true;
        };

        template <typename OuterTag, typename Matrix, bool is_const>
        struct dense2D_iterator_range_generator
        {
	    typedef Matrix                                                                matrix_type;
	    typedef typename matrix_type::size_type                                       size_type;
	    typedef typename matrix_type::value_type                                      value_type;
	    typedef typename matrix_type::parameters                                      parameters;
	    typedef detail::sub_matrix_cursor<matrix_type, OuterTag, 2>                   cursor;

	    // if traverse first along major dimension then memory access is contiguous (otherwise strided)
	    typedef typename boost::mpl::if_<
		major_traversal<OuterTag, typename parameters::orientation>
	      , complexity_classes::linear_cached
	      , complexity_classes::linear
	    >::type                                                                       complexity;
	    static int const                                                              level = 1;

	    // if traverse first along major dimension use pointer otherwise strided iterator
	    typedef typename boost::mpl::if_<
		major_traversal<OuterTag, typename parameters::orientation>
	      , typename boost::mpl::if_c<
    	            is_const
		  , const value_type*
    	          , value_type*
		>::type
	      , typename boost::mpl::if_c<
    	            is_const
		  , strided_dense_el_const_iterator<value_type>
    	          , strided_dense_el_iterator<value_type>
    	        >::type
	    >::type type;

        private:
	    // if traverse first along major dim. then return address as pointer
	    type dispatch(cursor const& c, size_type row, size_type col, complexity_classes::linear_cached) const
	    {
		matrix_type& ma= const_cast<matrix_type&>(c.ref);
		return ma.elements() + ma.indexer(ma, row, col); // &ref[row][col];
	    }

	    // otherwise strided
	    type dispatch(cursor const& c, size_type row, size_type col, complexity_classes::linear) const
	    {
		// cast const away (is dirty and should be improved later (cursors must distinct constness))
		matrix_type& ref= const_cast<matrix_type&>(c.ref);
		return type(ref, row, col, ref.ldim);
	    }

	    type begin_dispatch(cursor const& c, glas::tag::row) const
	    {
		return dispatch(c, c.key, c.ref.begin_col(), complexity());
	    }

	    type end_dispatch(cursor const& c, glas::tag::row) const
	    {
		return dispatch(c, c.key, c.ref.end_col(), complexity());
	    }

	    type begin_dispatch(cursor const& c, glas::tag::col) const
	    {
		return dispatch(c, c.ref.begin_row(), c.key, complexity());
	    }

	    type end_dispatch(cursor const& c, glas::tag::col) const
	    {
		return dispatch(c, c.ref.end_row(), c.key, complexity());
	    }

        public:

	    type begin(cursor const& c) const
	    {
		return begin_dispatch(c, OuterTag());
	    }

	    type end(cursor const& c) const
	    {
		return end_dispatch(c, OuterTag());
	    }
        };

    } // namespace detail

    template <typename Value, typename Parameters, typename OuterTag>
    struct range_generator<tag::iter::nz,
			   detail::sub_matrix_cursor<dense2D<Value, Parameters>, OuterTag, 2> >
      : public detail::dense2D_iterator_range_generator<OuterTag, dense2D<Value, Parameters>, false>
    {};

    template <typename Value, typename Parameters, typename OuterTag>
    struct range_generator<tag::iter::all,
			   detail::sub_matrix_cursor<dense2D<Value, Parameters>, OuterTag, 2> >
      : public detail::dense2D_iterator_range_generator<OuterTag, dense2D<Value, Parameters>, false>
    {};

    template <typename Value, typename Parameters, typename OuterTag>
    struct range_generator<tag::const_iter::nz,
			   detail::sub_matrix_cursor<dense2D<Value, Parameters>, OuterTag, 2> >
      : public detail::dense2D_iterator_range_generator<OuterTag, dense2D<Value, Parameters>, true>
    {};

    template <typename Value, typename Parameters, typename OuterTag>
    struct range_generator<tag::const_iter::all,
			   detail::sub_matrix_cursor<dense2D<Value, Parameters>, OuterTag, 2> >
      : public detail::dense2D_iterator_range_generator<OuterTag, dense2D<Value, Parameters>, true>
    {};

}} // namespace mtl::traits

namespace mtl { namespace mat {

    // ==========
    // Sub matrix
    // ==========

    template <typename Value, typename Parameters>
    struct sub_matrix_t<dense2D<Value, Parameters> >
    {
        typedef dense2D<Value, Parameters>      matrix_type;
	// copy orientation, ignore index, set dimension to non-fixed and on_stack to false
	typedef parameters<typename Parameters::orientation> para_type;

        typedef dense2D<Value, para_type>       sub_matrix_type;
        typedef sub_matrix_type const           const_sub_matrix_type;
        typedef typename matrix_type::size_type size_type;

        sub_matrix_type operator()(matrix_type& matrix, size_type begin_r, size_type end_r, size_type begin_c, size_type end_c)
        {
	    return sub_matrix_type(matrix, dense2D_sub_ctor(), begin_r, end_r, begin_c, end_c);
        }

        const_sub_matrix_type
        operator()(matrix_type const& matrix, size_type begin_r, size_type end_r, size_type begin_c, size_type end_c)
        {
	    // To minimize code duplication, we use the non-const version
	    sub_matrix_type tmp((*this)(const_cast<matrix_type&>(matrix), begin_r, end_r, begin_c, end_c));
	    return tmp;
        }
    };

}} // mtl::matrix

namespace mtl {

    // Enable cloning of dense matrices
    template <typename Value, typename Parameters>
    struct is_clonable< mtl::mat::dense2D<Value, Parameters> > : boost::mpl::true_ {};

} // namespace mtl

namespace math {

    // Multiplicative identities of matrices
    template <typename Value, typename Parameters>
    struct identity_t< mult<mtl::mat::dense2D<Value, Parameters> >, mtl::mat::dense2D<Value, Parameters> >
        : public std::binary_function< mult<mtl::mat::dense2D<Value, Parameters> >,
				       mtl::mat::dense2D<Value, Parameters>,
				       mtl::mat::dense2D<Value, Parameters> >
    {
        typedef mtl::mat::dense2D<Value, Parameters>  matrix_type;

        matrix_type operator() (const mult<matrix_type>&, const matrix_type& ref) const
        {
	    matrix_type tmp(ref);
	    tmp= one(typename matrix_type::value_type());
	    return tmp;
        }
    };

} // namespace math

#endif // MTL_DENSE2D_INCLUDE

/*
Limitations:
- with compile-time constant dimension, submatrices are not supported (would violate self-similarity)
- Element cursor doesn't work for sub-matrices (not contiguous)
*/


/**
 * @file
 * @brief Reads meshes containing second order elements and performs refinement
 * @author Anian Ruoss
 * @date   24.02.2019 18:24:17
 * @copyright MIT License
 */

#include <lf/base/base.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/refinement/mesh_hierarchy.h>
#include <boost/filesystem.hpp>
#include <iostream>

using lf::io::TikzOutputCtrl;

int main() {
  boost::filesystem::path file_path = __FILE__;

  for (const std::string& mesh_name :
       {"square_quads.msh", "square_trias.msh"}) {
    auto mesh_path = file_path.parent_path() / "meshes" / mesh_name;

    // read mesh from file
    auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    const lf::io::GmshReader reader(std::move(mesh_factory),
                                    mesh_path.string());

    // create mesh hierarchy from mesh for refinement
    lf::refinement::MeshHierarchy multi_mesh(
        std::const_pointer_cast<lf::mesh::Mesh>(reader.mesh()),
        std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2));

    int refinement_steps = 2;

    for (int step = 0; step < refinement_steps; ++step) {
      // refine mesh and store to TikZ
      multi_mesh.RefineRegular();
      auto mesh = multi_mesh.getMesh(multi_mesh.NumLevels() - 1);
      lf::io::writeTikZ(
          *mesh,
          mesh_name.substr(0, mesh_name.find_last_of('.')) + "_" +
              std::to_string(step) + ".tex",
          TikzOutputCtrl::RenderCells | TikzOutputCtrl::CellNumbering |
              TikzOutputCtrl::VerticeNumbering | TikzOutputCtrl::NodeNumbering |
              TikzOutputCtrl::EdgeNumbering);
    }
  }

  return 0;
}


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

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

#include "SiconosAlgebra.hpp"
// Useful function (print ...) from boost bindings examples.
#include "bindings_utils.hpp"

#include "Tools.hpp"

using namespace Siconos;

#include <boost/numeric/bindings/blas.hpp>
namespace siconosBindings = boost::numeric::bindings::blas;

// =================================================
//                CONSTRUCTORS
// =================================================

using std::cout;
using std::endl;

// Default (protected, used only for derived classes)
SimpleMatrix::SimpleMatrix(int i): SiconosMatrix(1), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  mat.Dense = new DenseMat(ublas::zero_matrix<double>());
}

SimpleMatrix::SimpleMatrix(): SiconosMatrix(1), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  mat.Dense = new DenseMat(ublas::zero_matrix<double>());
}

// parameters: dimensions and type.
SimpleMatrix::SimpleMatrix(unsigned int row, unsigned int col, UBLAS_TYPE typ, unsigned int upper, unsigned int lower):
  SiconosMatrix(1), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  if (typ == DENSE)
  {
    mat.Dense = new DenseMat(ublas::zero_matrix<double>(row, col));
    // _num = 1; default value
  }
  else if (typ == TRIANGULAR)
  {
    mat.Triang = new TriangMat(ublas::zero_matrix<double>(row, col));
    _num = 2;
  }
  else if (typ == SYMMETRIC)
  {
    mat.Sym = new SymMat(ublas::zero_matrix<double>(row, col));
    _num = 3;
  }
  else if (typ == SPARSE)
  {
    mat.Sparse = new SparseMat(row, col, upper);
    _num = 4;
    zero();
  }
  else if (typ == BANDED)
  {
    mat.Banded = new BandedMat(row, col, upper, lower);
    _num = 5;
    zero();
  }
  else if (typ == ZERO)
  {
    mat.Zero = new ZeroMat(row, col);
    _num = 6;
  }
  else if (typ == IDENTITY)
  {
    mat.Identity = new IdentityMat(row, col);
    _num = 7;
  }
  else
    SiconosMatrixException::selfThrow("SiconosMatrix::constructor(UBLAS_TYPE type, unsigned int row, unsigned int col): invalid type.");
}

// parameters: dimensions, input value and type
SimpleMatrix::SimpleMatrix(unsigned int row, unsigned int col, double inputValue, UBLAS_TYPE typ, unsigned int upper, unsigned int lower):
  SiconosMatrix(1), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  // This constructor has sense only for dense matrices ...
  if (typ == DENSE)
  {
    mat.Dense = new DenseMat(ublas::scalar_matrix<double>(row, col, inputValue));
    // _num = 1; default value
  }
  else
    SiconosMatrixException::selfThrow("SiconosMatrix::constructor(UBLAS_TYPE type, unsigned int row, unsigned int col, double fillInValue): invalid type.");
}

// // parameters: a vector (stl) of double and the type.
// SimpleMatrix::SimpleMatrix(const std::vector<double>& v, unsigned int row, unsigned int col, UBLAS_TYPE typ, unsigned int lower, unsigned int upper):
//   SiconosMatrix(1, row, col), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
// {
//   if( (  (v.size() != row*col) && (typ != SYMMETRIC && typ != BANDED) )
//       || (v.size() != row*row && typ == SYMMETRIC)
//       || (typ == BANDED && ( (v.size()) != (unsigned int)(std::max)(row, col)*(lower+1+upper) ) ))
//     SiconosMatrixException::selfThrow("constructor(UBLAS_TYPE, const std::vector<double>, int, int) : invalid vector size");

//   if(typ == DENSE)
//     {
//       mat.Dense = new DenseMat(row,col);
//       // _num = 1; default value
//     }
//   else if(typ == TRIANGULAR)
//     {
//       mat.Triang = new TriangMat(row,col);
//       _num = 2;
//     }
//   else if(typ == SYMMETRIC)
//     {
//       mat.Sym = new SymMat(row);
//       _num = 3;
//     }
//   else if(typ == SPARSE)
//     {
//       SiconosMatrixException::selfThrow("SimpleMatrix::constructor(UBLAS_TYPE, const std::vector<double>, int row, int col, int lower, int upper) : warning -- use constructor(const SparseMat &m) or constructor(UBLAS_TYPE, int row, int col) with UBLAS_TYPE = SPARSE");

//     }
//   else if(typ == BANDED)
//     {
//       mat.Banded = new BandedMat(row, col, lower, upper);
//       _num = 5;
//     }
//   else
//     SiconosMatrixException::selfThrow("constructor(UBLAS_TYPE, const std::vector<double>, int, int) : invalid type of matrix given");

//   std::copy(v.begin(), v.end(), (vect.Dense)->begin());

// }

// Copy constructors
SimpleMatrix::SimpleMatrix(const SimpleMatrix &smat): SiconosMatrix(smat.num()), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  if (_num == 1)
  {
    mat.Dense = new DenseMat(smat.size(0), smat.size(1));
    noalias(*mat.Dense) = (*smat.dense());
  }
  //   mat.Dense = new DenseMat(*smat.dense());

  else if (_num == 2)
    mat.Triang = new TriangMat(*smat.triang());

  else if (_num == 3)

    mat.Sym = new SymMat(*smat.sym());

  else if (_num == 4)
    mat.Sparse = new SparseMat(*smat.sparse());

  else if (_num == 5)
    mat.Banded = new BandedMat(*smat.banded());

  else if (_num == 6)
    mat.Zero = new ZeroMat(smat.size(0), smat.size(1));

  else// if(_num == 7)
    mat.Identity = new IdentityMat(smat.size(0), smat.size(1));
}

/** copy constructor of a block given by the coord = [r0A r1A c0A c1A]
 *  \param A the matrix for extracting the block
 */
SimpleMatrix::SimpleMatrix(const SimpleMatrix& A , const Index& coord ):  SiconosMatrix(A.num()), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  if (coord[0]>=coord[1])
    SiconosMatrixException::selfThrow("SimpleMatrix::SimpleMatrix(const SimpleMatrix& A , const Index& coord ). Empty row range coord[0]>= coord[1]");
  if (coord[2]>=coord[3])
    SiconosMatrixException::selfThrow("SimpleMatrix::SimpleMatrix(const SimpleMatrix& A , const Index& coord ). Empty column range coord[2]>= coord[3]");
  if (coord[1] > A.size(0) )
    SiconosMatrixException::selfThrow("SimpleMatrix::SimpleMatrix(const SimpleMatrix& A , const Index& coord ). row index too large.");
  if (coord[3] > A.size(1) )
    SiconosMatrixException::selfThrow("SimpleMatrix::SimpleMatrix(const SimpleMatrix& A , const Index& coord ). column index too large.");

  if (_num== 1)
  {
    ublas::matrix_range<DenseMat> subA(*A.dense(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.Dense=new DenseMat(subA);
  }
  else if (_num == 2)
  {
    ublas::matrix_range<TriangMat> subA(*A.triang(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.Triang=new TriangMat(subA);
  }
  else if (_num == 3)
  {
    ublas::matrix_range<SymMat> subA(*A.sym(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.Sym=new SymMat(subA);
  }
  else if (_num == 4)
  {
    ublas::matrix_range<SparseMat> subA(*A.sparse(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.Sparse=new SparseMat(subA);
  }
  else if (_num == 5)
  {
    ublas::matrix_range<BandedMat> subA(*A.banded(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.Banded=new BandedMat(subA);
  }
  else if (_num == 6)
  {
    mat.Zero = new ZeroMat(coord[1]-coord[0], coord[3]-coord[2]);
  }
  else// if(_num == 7)
    mat.Identity = new IdentityMat(coord[1]-coord[0], coord[3]-coord[2] );
}

SimpleMatrix::SimpleMatrix(const SiconosMatrix &m): SiconosMatrix(m.num()), _isPLUFactorized(), _isQRFactorized(false), _isPLUInversed(false)
{
  // _num is set in SiconosMatrix constructor with m.num() ... must be changed if m is Block
  unsigned int numM = m.num();

  _isPLUFactorized= m.isPLUFactorized();
  _isPLUInversed= m.isPLUInversed();

  if (m.ipiv())
    _ipiv.reset(new VInt(*(m.ipiv())));

  if (numM == 0) // ie if m is Block, this matrix is set to a dense.
  {
    const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
    _num = 1;
    // get number of blocks in a row/col of m.
    mat.Dense = new DenseMat(m.size(0), m.size(1));
    ConstBlocksIterator1 it;
    ConstBlocksIterator2 it2;
    unsigned int posRow = 0;
    unsigned int posCol = 0;

    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 = 0;
    }
  }
  else if (_num == 1)
  {
    mat.Dense = new DenseMat(m.size(0), m.size(1));
    noalias(*mat.Dense) = (*m.dense());
  }

  else if (_num == 2)
    mat.Triang = new TriangMat(*m.triang());

  else if (_num == 3)
    mat.Sym = new SymMat(*m.sym());

  else if (_num == 4)
    mat.Sparse = new SparseMat(*m.sparse());

  else if (_num == 5)
    mat.Banded = new BandedMat(*m.banded());

  else if (_num == 6)
    mat.Zero = new ZeroMat(m.size(0), m.size(1));

  else // if(_num == 7)
    mat.Identity = new IdentityMat(m.size(0), m.size(1));
}

SimpleMatrix::SimpleMatrix(const DenseMat& m): SiconosMatrix(1), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  mat.Dense = new DenseMat(m);
}

SimpleMatrix::SimpleMatrix(const TriangMat& m): SiconosMatrix(2), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  mat.Triang = new TriangMat(m);
}

SimpleMatrix::SimpleMatrix(const SymMat& m): SiconosMatrix(3), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  mat.Sym = new SymMat(m);
}

SimpleMatrix::SimpleMatrix(const SparseMat& m): SiconosMatrix(4), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  mat.Sparse = new SparseMat(m);
}

SimpleMatrix::SimpleMatrix(const BandedMat& m): SiconosMatrix(5), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  mat.Banded = new BandedMat(m);
}

SimpleMatrix::SimpleMatrix(const ZeroMat& m): SiconosMatrix(6), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  mat.Zero = new ZeroMat(m);
}

SimpleMatrix::SimpleMatrix(const IdentityMat& m): SiconosMatrix(7), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  mat.Identity = new IdentityMat(m);
}

SimpleMatrix::SimpleMatrix(const std::string &file, bool ascii): SiconosMatrix(1), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
  mat.Dense = new DenseMat();
  if (ascii)
  {
    ioMatrix::read(file, "ascii", *this);
  }
  else
  {
    ioMatrix::read(file, "binary", *this);
  }
}

SimpleMatrix::~SimpleMatrix()
{
  if (_num == 1)
    delete(mat.Dense);
  else if (_num == 2)
    delete(mat.Triang);
  else if (_num == 3)
    delete(mat.Sym);
  else if (_num == 4)
    delete(mat.Sparse);
  else if (_num == 5)
    delete(mat.Banded);
  else if (_num == 6)
    delete(mat.Zero);
  else if (_num == 7)
    delete(mat.Identity);
}

bool SimpleMatrix::isSymmetric(double tol) const
{
  SP::SimpleMatrix  m_trans (new SimpleMatrix(*this));
  m_trans->trans();
  double err = (*this-*m_trans).normInf();
  if ((*m_trans).normInf() > 0.0 )
  {
    err /= (*m_trans).normInf();
  }
  // std::cout << "err_rel  ="<< err <<std::endl;
  return (err < tol);
}
//======================================
// get Ublas component (dense, sym ...)
//======================================

const DenseMat SimpleMatrix::getDense(unsigned int, unsigned int) const
{
  if (_num != 1)
    SiconosMatrixException::selfThrow("SimpleMatrix::getDense(): the current matrix is not a Dense matrix");

  return *mat.Dense;
}

const TriangMat SimpleMatrix::getTriang(unsigned int, unsigned int) const
{
  if (_num != 2)
    SiconosMatrixException::selfThrow("TriangMat SimpleMatrix::getTriang(): the current matrix is not a Triangular matrix");

  return *mat.Triang;
}

const SymMat SimpleMatrix::getSym(unsigned int, unsigned int) const
{
  if (_num != 3)
    SiconosMatrixException::selfThrow("SymMat SimpleMatrix::getSym(): the current matrix is not a Symmetric matrix");

  return *mat.Sym;
}

const SparseMat SimpleMatrix::getSparse(unsigned int, unsigned int) const
{
  if (_num != 4)
    SiconosMatrixException::selfThrow("SparseMat SimpleMatrix::getSparse(): the current matrix is not a Sparse matrix");

  return *mat.Sparse;
}

const BandedMat SimpleMatrix::getBanded(unsigned int, unsigned int) const
{
  if (_num != 5)
    SiconosMatrixException::selfThrow("BandedMat SimpleMatrix::getBanded(): the current matrix is not a Banded matrix");

  return *mat.Banded;
}

const ZeroMat SimpleMatrix::getZero(unsigned int, unsigned int) const
{
  if (_num != 6)
    SiconosMatrixException::selfThrow("ZeroMat SimpleMatrix::getZero(): the current matrix is not a Zero matrix");

  return *mat.Zero;
}

const IdentityMat SimpleMatrix::getIdentity(unsigned int, unsigned int) const
{
  if (_num != 7)
    SiconosMatrixException::selfThrow("IdentityMat SimpleMatrix::getIdentity(): the current matrix is not a Identity matrix");

  return *mat.Identity;
}

DenseMat* SimpleMatrix::dense(unsigned int, unsigned int) const
{
  if (_num != 1)
    SiconosMatrixException::selfThrow("DenseMat* SimpleMatrix::dense(): the current matrix is not a Dense matrix");

  return mat.Dense;
}

TriangMat* SimpleMatrix::triang(unsigned int, unsigned int) const
{
  if (_num != 2)
    SiconosMatrixException::selfThrow("TriangMat* SimpleMatrix::triang(): the current matrix is not a Triangular matrix");

  return mat.Triang;
}

SymMat* SimpleMatrix::sym(unsigned int, unsigned int) const
{
  if (_num != 3)
    SiconosMatrixException::selfThrow("SymMat* SimpleMatrix::sym(): the current matrix is not a Symmetric matrix");

  return mat.Sym;
}

SparseMat* SimpleMatrix::sparse(unsigned int, unsigned int) const
{
  if (_num != 4)
    SiconosMatrixException::selfThrow("SparseMat* SimpleMatrix::sparse(): the current matrix is not a Sparse matrix");

  return mat.Sparse;
}

BandedMat* SimpleMatrix::banded(unsigned int, unsigned int) const
{
  if (_num != 5)
    SiconosMatrixException::selfThrow("BandedMat* SimpleMatrix::banded(): the current matrix is not a Banded matrix");

  return mat.Banded;
}

ZeroMat* SimpleMatrix::zero_mat(unsigned int, unsigned int) const
{
  if (_num != 6)
    SiconosMatrixException::selfThrow("ZeroMat* SimpleMatrix::zero_mat(): the current matrix is not a Zero matrix");

  return mat.Zero;
}

IdentityMat* SimpleMatrix::identity(unsigned int, unsigned int) const
{
  if (_num != 7)
    SiconosMatrixException::selfThrow("IdentityMat* SimpleMatrix::identity(): the current matrix is not a Identity matrix");

  return mat.Identity;
}

double* SimpleMatrix::getArray(unsigned int, unsigned int) const
{
  if (_num == 4)
    SiconosMatrixException::selfThrow("SimpleMatrix::getArray(): not yet implemented for sparse matrix.");

  if (_num == 1)
    return (((*mat.Dense).data()).data());
  else if (_num == 2)
    return &(((*mat.Triang).data())[0]);
  else if (_num == 3)
    return &(((*mat.Sym).data())[0]);
  else if (_num == 6)
  {
    ZeroMat::iterator1 it = (*mat.Zero).begin1();
    return const_cast<double*>(&(*it));
  }
  else if (_num == 7)
  {
    IdentityMat::iterator1 it = (*mat.Identity).begin1();
    return const_cast<double*>(&(*it));
  }
  else
    return &(((*mat.Banded).data())[0]);
}

// ===========================
//       fill matrix
// ===========================

void SimpleMatrix::zero()
{
  unsigned int size1 = size(0);
  unsigned int size2 = size(1);
  if (_num == 1)
    *mat.Dense = ublas::zero_matrix<double>(size1, size2);
  else if (_num == 2)
    *mat.Triang = ublas::zero_matrix<double>(size1, size2);

  else if (_num == 3)
    *mat.Sym = ublas::zero_matrix<double>(size1, size2);

  else if (_num == 4)
    *mat.Sparse = ublas::zero_matrix<double>(size1, size2);

  else if (_num == 5)
    *mat.Banded = ublas::zero_matrix<double>(size1, size2);

  else if (_num == 7)
    SiconosMatrixException::selfThrow("SimpleMatrix::zero(): you can not set to zero a matrix of type Identity!.");
  resetLU();
  // if _num == 6: nothing
}

void SimpleMatrix::randomize()
{
  if (_num == 1)
    Siconos::algebra::fill(*mat.Dense);
  else
    SiconosMatrixException::selfThrow("SimpleMatrix::randomize(): only implemented for dense matrices.");
  resetLU();
}

void SimpleMatrix::randomize_sym()
{
  if (_num == 1)
    Siconos::algebra::fill_sym(*mat.Dense);
  else
    SiconosMatrixException::selfThrow("SimpleMatrix::randomize_sym(): only implemented for dense matrices.");
  resetLU();
}

void SimpleMatrix::eye()
{
  unsigned int size1 = size(0);
  unsigned int size2 = size(1);
  if (_num == 1)
    *mat.Dense = ublas::identity_matrix<double>(size1, size2);

  else if (_num == 2)
    *mat.Triang = ublas::identity_matrix<double>(size1, size2);

  else if (_num == 3)
    *mat.Sym = ublas::identity_matrix<double>(size1, size2);

  else if (_num == 4)
    *mat.Sparse = ublas::identity_matrix<double>(size1, size2);

  else if (_num == 5)
    *mat.Banded = ublas::identity_matrix<double>(size1, size2);

  else if (_num == 6)
    SiconosMatrixException::selfThrow("SimpleMatrix::eye(): you can not set to identity a matrix of type Zero!.");
  resetLU();
}

unsigned int SimpleMatrix::size(unsigned int index) const
{
  if (_num == 1)
  {
    if (index == 0) return (*mat.Dense).size1();
    else  return (*mat.Dense).size2();
  }
  else if (_num == 2)
  {
   if (index == 0) return (*mat.Triang).size1();
   else return (*mat.Triang).size2();
  }
  else if (_num == 3)
  {
   if (index == 0) return (*mat.Sym).size1();
   else  return (*mat.Sym).size2();
  }
  else if (_num == 4)
  {
   if (index == 0) return (*mat.Sparse).size1();
   else return (*mat.Sparse).size2();
  }
  else if (_num == 5)
  {
  if (index == 0) return  (*mat.Banded).size1();
  else  return  (*mat.Banded).size2();
  }
  else if (_num == 6)
  {
  if (index == 0) return (*mat.Zero).size1();
  else  return (*mat.Zero).size2();
  }
  else if (_num == 7)
  {
   if (index == 0) return (*mat.Identity).size1();
   else  return (*mat.Identity).size2();
  }
  else return 0;

};

//=======================
// set matrix dimension
//=======================

void SimpleMatrix::resize(unsigned int row, unsigned int col, unsigned int lower, unsigned int upper, bool preserve)
{

  if (_num == 1)
  {
    (*mat.Dense).resize(row, col, preserve);
  }
  else if (_num == 2)
  {
    (*mat.Triang).resize(row, col, preserve);
  }
  else if (_num == 3)
  {
    (*mat.Sym).resize(row, col, preserve);
  }
  else if (_num == 4)
  {
    (*mat.Sparse).resize(row, col, preserve);
  }
  else if (_num == 5)
  {
    (*mat.Banded).resize(row, col, lower, upper, preserve);
  }
  else if (_num == 6)
  {
    (*mat.Zero).resize(row, col, preserve);
  }
  else if (_num == 7)
  {
    (*mat.Identity).resize(row, col, preserve);
  }
  resetLU();
 }

//=====================
// screen display
//=====================

void SimpleMatrix::display() const
{
  std::cout.setf(std::ios::scientific);
  std::cout.precision(6);

  if (size(0) == 0 || size(1) ==0)
  {
    std::cout << "SimpleMatrix::display(): empty matrix" << std::endl;
  }
  std::cout << "SimpleMatrix storage type - num = " << _num << "\n";
  if (_num == 1)
  {
    Siconos::algebra::print_m(*mat.Dense);
    //std::cout << *mat.Dense << std::endl;
  }
  else if (_num == 2)
    std::cout << *mat.Triang << std::endl;
  else if (_num == 3)
    std::cout << *mat.Sym << std::endl;
  else if (_num == 4)
    std::cout << *mat.Sparse << std::endl;
  else if (_num == 5)
    std::cout << *mat.Banded << std::endl;
  else if (_num == 6)
    std::cout << *mat.Zero << std::endl;
  else if (_num == 7)
    std::cout << *mat.Identity << std::endl;
}

//=====================
// convert to a string
//=====================

std::string SimpleMatrix::toString() const
{
  return ::toString(*this);
}

//=====================
// convert to an ostream
//=====================

std::ostream& operator<<(std::ostream& os, const SimpleMatrix& sm)
{
  if (sm._num == 1)
    os << *sm.mat.Dense;
  else if (sm._num == 2)
    os << *sm.mat.Triang;
  else if (sm._num == 3)
    os << *sm.mat.Sym;
  else if (sm._num == 4)
    os << *sm.mat.Sparse;
  else if (sm._num == 5)
    os << *sm.mat.Banded;
  else if (sm._num == 6)
    os << *sm.mat.Zero;
  else if (sm._num == 7)
    os << *sm.mat.Identity;
  return os;
}

void prod(const SiconosMatrix& A, const BlockVector& x, SiconosVector& y, bool init)
{

  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if (init)
    y.zero();
  unsigned int startRow = 0;
  unsigned int startCol = 0;
  // In private_addprod, the sum of all blocks of x, x[i], is computed: y = Sum_i (subA x[i]), with subA a submatrix of A,
  // starting from position startRow in rows and startCol in columns.
  // private_prod takes also into account the fact that each block of x can also be a block.
  VectorOfVectors::const_iterator it;
  for (it = x.begin(); it != x.end(); ++it)
  {
    private_addprod(A, startRow, startCol, **it, y);
    startCol += (*it)->size();
  }
}

void prod(const SiconosMatrix& A, const SiconosVector& x, BlockVector& y, bool init)
{
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  unsigned int startRow = 0;
  VectorOfVectors::const_iterator it;
  // For Each subvector of y, y[i], private_prod computes y[i] = subA x, subA being a submatrix of A corresponding to y[i] position.
  //       // private_prod takes into account the fact that x and y[i] may be block vectors.
  for (it = y.begin(); it != y.end(); ++it)
  {
    private_prod(A, startRow, x, **it, init);
    startRow += (*it)->size();
  }

}

void subprod(const SiconosMatrix& A, const BlockVector& x, SiconosVector& y, const Index& coord, bool init)
{
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  // Number of the subvector of x that handles element at position coord[4]
  std::size_t firstBlockNum = x.getNumVectorAtPos(coord[4]);
  // Number of the subvector of x that handles element at position coord[5]
  unsigned int lastBlockNum = x.getNumVectorAtPos(coord[5]);
  Index subCoord = coord;
  SPC::SiconosVector  tmp = x[firstBlockNum];
  std::size_t subSize =  tmp->size(); // Size of the sub-vector
  const SP::Index xTab = x.tabIndex();
  if (firstBlockNum != 0)
  {
    subCoord[4] -= (*xTab)[firstBlockNum - 1];
    subCoord[5] =  std::min(coord[5] - (*xTab)[firstBlockNum - 1], subSize);
  }
  else
    subCoord[5] =  std::min(coord[5], subSize);

  if (firstBlockNum == lastBlockNum)
  {
    subprod(A, *tmp, y, subCoord, init);
  }
  else
  {
    unsigned int xPos = 0 ; // Position in x of the current sub-vector of x
    bool firstLoop = true;
    subCoord[3] = coord[2] + subCoord[5] - subCoord[4];
    for (VectorOfVectors::const_iterator it = x.begin(); it != x.end(); ++it)
    {
      if ((*it)->num() == 0)
        SiconosMatrixException::selfThrow("subprod(A,x,y) error: not yet implemented for x block of blocks ...");
      if (xPos >= firstBlockNum && xPos <= lastBlockNum)
      {
        tmp = x[xPos];
        if (firstLoop)
        {
          subprod(A, *tmp, y, subCoord, init);
          firstLoop = false;
        }
        else
        {
          subCoord[2] += subCoord[5] - subCoord[4]; // !! old values for 4 and 5
          subSize = tmp->size();
          subCoord[4] = 0;
          subCoord[5] = std::min(coord[5] - (*xTab)[xPos - 1], subSize);
          subCoord[3] = subCoord[2] + subCoord[5] - subCoord[4];
          subprod(A, *tmp, y, subCoord, false);
        }
      }
      xPos++;
    }
  }
}

void prod(const SiconosVector& x, const SiconosMatrix& A, BlockVector& y, bool init)
{

  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if (A.size(0) != x.size())
    SiconosMatrixException::selfThrow("prod(x,A,y) error: inconsistent sizes between A and x.");

  if (A.size(1) != y.size())
    SiconosMatrixException::selfThrow("prod(x,A,y) error: inconsistent sizes between A and y.");
  unsigned int startRow = 0;
  VectorOfVectors::const_iterator it;
  // For Each subvector of y, y[i], private_prod computes y[i] = subA x, subA being a submatrix of A corresponding to y[i] position.
  // private_prod takes into account the fact that x and y[i] may be block vectors.
  for (it = y.begin(); it != y.end(); ++it)
  {
    private_prod(createSPtrConstSiconosVector(x), createSPtrConstSiconosMatrix(A), startRow, *it, init);
    startRow += (*it)->size();
  }
}

void private_addprod(const SiconosMatrix& A, unsigned startRow, unsigned int startCol, const SiconosVector& x, SiconosVector& y)
{
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");
  assert(!A.isBlock() && "private_addprod(A,start,x,y) error: not yet implemented for block matrix.");

  // we take a submatrix subA of A, starting from row startRow to row (startRow+sizeY) and between columns startCol and (startCol+sizeX).
  // Then computation of y = subA*x + y.
  unsigned int numA = A.num();
  unsigned int numY = y.num();
  unsigned int numX = x.num();
  unsigned int sizeX = x.size();
  unsigned int sizeY = y.size();

  assert(numX == numY && "private_addprod(A,start,x,y) error: not yet implemented for x and y of different types.");

  if (numY == 1 && numX == 1)
  {

    assert(y.dense() != x.dense());

    if (numA == 1)
      noalias(*y.dense()) += prod(ublas::subrange(*A.dense(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.dense());
    else if (numA == 2)
      noalias(*y.dense()) += prod(ublas::subrange(*A.triang(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.dense());
    else if (numA == 3)
      noalias(*y.dense()) += prod(ublas::subrange(*A.sym(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.dense());
    else if (numA == 4)
      noalias(*y.dense()) += prod(ublas::subrange(*A.sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.dense());
    else //if(numA==5)
      noalias(*y.dense()) += prod(ublas::subrange(*A.banded(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.dense());
  }
  else // x and y sparse
  {
    if (numA == 4)
      *y.sparse() += prod(ublas::subrange(*A.sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.sparse());
    else
      SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for x, y  sparse and A not sparse.");
  }
}

void private_addprod(const SiconosMatrix& A, unsigned int startRow, unsigned int startCol, const BlockVector& x, SiconosVector& y)
{
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  assert(!A.isBlock() && "private_addprod(A,start,x,y) error: not yet implemented for block matrix.");

  VectorOfVectors::const_iterator it;
  unsigned int startColBis = startCol;
  for (it = x.begin(); it != x.end(); ++it)
  {
    private_addprod(A, startRow, startColBis, **it, y);
    startColBis += (*it)->size();
  }

}

// x block, y siconos
void private_prod(const SiconosMatrix& A, unsigned int startRow, const BlockVector& x, SiconosVector& y, bool init)
{
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  // Computes y = subA *x (or += if init = false), subA being a sub-matrix of A, between el. of index (row) startRow and startRow + sizeY

  if (init) // y = subA * x , else y += subA * x
    y.zero();
  private_addprod(A, startRow, 0, x, y);
}

// x and y blocks
void private_prod(SPC::SiconosMatrix A, const unsigned int startRow, SPC::BlockVector x, SP::BlockVector y, bool init)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  unsigned int row = startRow;
  VectorOfVectors::const_iterator it;
  for (it = y->begin(); it != y->end(); ++it)
  {
    private_prod(*A, row, *x, **it, init);
    row += (*it)->size();
  }
}

// x block, y siconos
void private_prod(const SiconosMatrix& A, unsigned int startRow, const SiconosVector& x, SiconosVector& y, bool init)
{
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  // Computes y = subA *x (or += if init = false), subA being a sub-matrix of A, between el. of index (row) startRow and startRow + sizeY

  if (init) // y = subA * x , else y += subA * x
    y.zero();
  private_addprod(A, startRow, 0, x, y);
}

// x and y blocks
void private_prod(SPC::SiconosMatrix A, const unsigned int startRow, SPC::SiconosVector x, SP::BlockVector y, bool init)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  unsigned int row = startRow;
  VectorOfVectors::const_iterator it;
  for (it = y->begin(); it != y->end(); ++it)
  {
    private_prod(*A, row, *x, **it, init);
    row += (*it)->size();
  }
}
// With trans(A) ...
void private_addprod(SPC::SiconosVector x, SPC::SiconosMatrix A, unsigned int startRow, unsigned int startCol, SP::SiconosVector y)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if (A->isBlock())
    SiconosMatrixException::selfThrow("private_addprod(x,A,start,y) error: not yet implemented for block matrix.");

  // we take a submatrix subA of A, starting from row startRow to row (startRow+sizeY) and between columns startCol and (startCol+sizeX).
  // Then computation of y = subA*x + y.
  unsigned int numA = A->num();
  unsigned int numY = y->num();
  unsigned int numX = x->num();
  unsigned int sizeX = x->size();
  unsigned int sizeY = y->size();

  if (numX != numY)
    SiconosMatrixException::selfThrow("private_addprod(x,A,start,y) error: not yet implemented for x and y of different types.");

  if (numY == 1 && numX == 1)
  {

    assert(y->dense() != x->dense());

    if (numA == 1)
      noalias(*y->dense()) += prod(ublas::subrange(trans(*A->dense()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if (numA == 2)
      noalias(*y->dense()) += prod(ublas::subrange(trans(*A->triang()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if (numA == 3)
      noalias(*y->dense()) += prod(ublas::subrange(trans(*A->sym()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if (numA == 4)
      noalias(*y->dense()) += prod(ublas::subrange(trans(*A->sparse()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else //if(numA==5)
      noalias(*y->dense()) += prod(ublas::subrange(trans(*A->banded()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
  }
  else // x and y sparse
  {
    if (numA == 4)
      *y->sparse() += prod(ublas::subrange(trans(*A->sparse()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->sparse());
    else
      SiconosMatrixException::selfThrow("private_addprod(x,A,start,y) error: not yet implemented for x, y  sparse and A not sparse.");
  }
}

void private_addprod(SPC::BlockVector x, SPC::SiconosMatrix A, unsigned int startRow, unsigned int startCol, SP::SiconosVector y)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  VectorOfVectors::const_iterator it;
  unsigned int startColBis = startCol;
  for (it = x->begin(); it != x->end(); ++it)
  {
    private_addprod((*it), A, startRow, startColBis, y);
    startColBis += (*it)->size();
  }

}

void private_prod(SPC::SiconosVector x, SPC::SiconosMatrix A, unsigned int startCol, SP::SiconosVector  y, bool init)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  // Computes y = subA *x (or += if init = false), subA being a sub-matrix of trans(A), between el. of A of index (col) startCol and startCol + sizeY
  if (init) // y = subA * x , else y += subA * x
    y->zero();
  private_addprod(x, A, startCol, 0 , y);

}

void private_prod(SPC::SiconosVector x, SPC::SiconosMatrix A, unsigned int startCol, SP::BlockVector  y, bool init)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  unsigned int col = startCol;
  VectorOfVectors::const_iterator it;
  for (it = y->begin(); it != y->end(); ++it)
  {
    private_prod(x, A, col, *it, init);
    col += (*it)->size();
  }
}

void private_prod(SPC::BlockVector x, SPC::SiconosMatrix A, unsigned int startCol, SP::SiconosVector  y, bool init)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  // Computes y = subA *x (or += if init = false), subA being a sub-matrix of trans(A), between el. of A of index (col) startCol and startCol + sizeY
  if (init) // y = subA * x , else y += subA * x
    y->zero();
  private_addprod(x, A, startCol, 0 , y);

}

void private_prod(SPC::BlockVector x, SPC::SiconosMatrix A, unsigned int startCol, SP::BlockVector  y, bool init)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  unsigned int col = startCol;
  VectorOfVectors::const_iterator it;
  for (it = y->begin(); it != y->end(); ++it)
  {
    private_prod(x, A, col, *it, init);
    col += (*it)->size();
  }
}

void private_addprod(double a, SPC::SiconosMatrix A, unsigned int startRow, unsigned int startCol, SPC::SiconosVector x, SP::SiconosVector y)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if (A->isBlock())
    SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for block matrix.");

  // we take a submatrix subA of A, starting from row startRow to row (startRow+sizeY) and between columns startCol and (startCol+sizeX).
  // Then computation of y = subA*x + y.
  unsigned int numA = A->num();
  unsigned int numY = y->num();
  unsigned int numX = x->num();
  unsigned int sizeX = x->size();
  unsigned int sizeY = y->size();

  if (numX != numY)
    SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for x and y of different types.");

  if (numY == 1 && numX == 1)
  {

    assert(y->dense() != x->dense());

    if (numA == 1)
      noalias(*y->dense()) += a * prod(ublas::subrange(*A->dense(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if (numA == 2)
      noalias(*y->dense()) += a * prod(ublas::subrange(*A->triang(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if (numA == 3)
      noalias(*y->dense()) += a * prod(ublas::subrange(*A->sym(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if (numA == 4)
      noalias(*y->dense()) += a * prod(ublas::subrange(*A->sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else //if(numA==5)
      noalias(*y->dense()) += a * prod(ublas::subrange(*A->banded(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
  }
  else // x and y sparse
  {
    if (numA == 4)
      *y->sparse() += a * prod(ublas::subrange(*A->sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->sparse());
    else
      SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for x, y  sparse and A not sparse.");
  }

}

void private_prod(double a, SPC::SiconosMatrix A, unsigned int startRow, SPC::SiconosVector x, SP::SiconosVector  y, bool init)
{
  assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");

  // Computes y = subA *x (or += if init = false), subA being a sub-matrix of A, between el. of index (row) startRow and startRow + sizeY

  if (init) // y = subA * x , else y += subA * x
    y->zero();
  private_addprod(a, A, startRow, 0, x, y);

}

unsigned SimpleMatrix::copyData(double* data) const
{
  assert((_num == 1) && "SiconosMatrix::copyData : forbidden: the current matrix is not dense.");

  unsigned size = mat.Dense->size1() * mat.Dense->size2();
  siconosBindings::detail::copy(size, getArray(), 1, data, 1);
  return size;
}


#pragma once
#include <memory>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>

#include "operators.hpp"

namespace yavque
{
class Variable;

class Circuit
{
private:
	uint32_t dim_;

	// Operators
	std::vector<std::unique_ptr<Operator>> ops_;

	// States evaluated from the left
	mutable std::vector<std::shared_ptr<const Eigen::VectorXcd>> states_from_left_ = {};
	mutable uint32_t states_updated_to_ = 0;

	void add_op_right(std::unique_ptr<Operator> op) { ops_.emplace_back(std::move(op)); }

	void add_op_left(std::unique_ptr<Operator> op)
	{
		states_updated_to_ = 0;
		ops_.emplace(ops_.begin(), std::move(op));
	}

protected:
	template<typename ConstIterator>
	Circuit(uint32_t dim, const ConstIterator& begin, const ConstIterator& end)
		: dim_{dim}
	{
		// check all ops has the same dim
		for(auto iter = begin; iter != end; ++iter)
		{
			ops_.push_back((*iter)->clone());
		}
	}

public:
	explicit Circuit(uint32_t dim) : dim_{dim} { }

	Circuit(const Circuit& rhs); // Defined in cpp
	Circuit(Circuit&& rhs) = default;

	Circuit& operator=(const Circuit& rhs); // Defined in cpp
	Circuit& operator=(Circuit&& rhs) = default;

	~Circuit() = default;

	uint32_t get_dim() const { return dim_; }

	bool is_evaluated() const { return (states_updated_to_ == ops_.size() + 1); }

	// evaluate
	void evaluate() const
	{
		// Initial state hasn't been set
		if(states_updated_to_ == 0)
		{
			return;
		}

		// All states are evaluated
		if(states_updated_to_ == ops_.size() + 1)
		{
			return;
		}

		for(auto d = states_from_left_.size() - 1; d < ops_.size(); ++d)
		{
			states_from_left_.emplace_back(std::make_shared<Eigen::VectorXcd>(
				ops_[d]->apply_right(*states_from_left_[d])));
		}
		states_updated_to_ = ops_.size() + 1;
	}

	void clear_evaluated()
	{
		states_updated_to_ = 1;
		states_from_left_.resize(1);
	}

	std::size_t num_operators() const { return ops_.size(); }

	const std::unique_ptr<Operator>& operator_at(std::size_t idx) const
	{
		return ops_[idx];
	}

	template<typename T, typename... Args> void add_op_right(Args&&... args)
	{
		static_assert(std::is_convertible_v<T*, Operator*>,
		              "T must be a subclass of yavque::Operator.");
		ops_.emplace_back(std::make_unique<T>(std::forward<Args>(args)...));
	}

	template<typename T, typename... Args> void add_op_left(Args&&... args)
	{
		static_assert(std::is_convertible_v<T*, Operator*>,
		              "T must be a subclass of yavque::Operator.");
		states_updated_to_ = 0;
		ops_.emplace(ops_.begin(), std::make_unique<T>(std::forward<Args>(args)...));
	}

	/**
	 * evaluate |st> - C
	 * When C = U_N \cdots U_1, it computes U_N \cdots U_1 | st \rangle
	 * */
	void set_input(const Eigen::VectorXcd& st)
	{
		states_updated_to_ = 1;
		states_from_left_.clear();
		states_from_left_.push_back(std::make_shared<Eigen::VectorXcd>(st));
	}

	/**
	 * subcircuit from 0 to d [0,d)
	 */
	Circuit to(uint32_t d) const
	{
		Circuit circ(dim_, ops_.begin(), ops_.begin() + d);
		if(states_updated_to_ != 0)
		{
			circ.states_from_left_ = std::vector<std::shared_ptr<const Eigen::VectorXcd>>(
				states_from_left_.begin(),
				states_from_left_.begin() + std::min(d + 1, states_updated_to_));
			circ.states_updated_to_ = std::min(d + 1, states_updated_to_);
		}
		return circ;
	}

	/**
	 * subcircuit from d to end [d,end)
	 */
	Circuit from(uint32_t d) const
	{
		Circuit circ(dim_, ops_.begin() + d, ops_.end());
		if(states_updated_to_ > d)
		{
			circ.states_from_left_ = std::vector<std::shared_ptr<const Eigen::VectorXcd>>(
				states_from_left_.begin() + d,
				states_from_left_.begin() + states_updated_to_);
			circ.states_updated_to_ = states_updated_to_ - d;
		}

		return circ;
	}

	Circuit& operator|=(const Circuit& b)
	{
		using std::begin;
		using std::end;
		assert(dim_ == b.dim_);
		for(const auto& op : b.ops_)
		{
			ops_.push_back(op->clone());
		}
		return *this;
	}

	Circuit& operator|=(Circuit&& b)
	{
		using std::begin;
		using std::end;
		assert(dim_ == b.dim_);
		for(auto&& op : b.ops_)
		{
			ops_.emplace_back(std::move(op));
		}
		b.states_updated_to_ = 0;
		b.states_from_left_.resize(0);
		b.ops_.resize(0);
		return *this;
	}

	Circuit dagger() const
	{
		Circuit res(dim_, ops_.crbegin(), ops_.crend());
		for(std::size_t idx = 0; idx < ops_.size(); ++idx)
		{
			res.ops_[idx]->dagger_in_place();
		}
		return res;
	}

	std::vector<Variable> variables() const;

	void derivs() const;

	std::shared_ptr<const Eigen::VectorXcd> state_at(uint32_t idx) const
	{
		// May throw exception instead
		if(idx > ops_.size())
		{
			return nullptr;
		}

		// If not all stastes are evaluated
		if(idx > states_updated_to_)
		{
			evaluate();
		}

		return states_from_left_[idx];
	}

	std::shared_ptr<const Eigen::VectorXcd> output() const
	{
		if(!is_evaluated())
		{
			evaluate();
		}
		return states_from_left_.back();
	}

	[[nodiscard]] std::string desc() const
	{
		// change to format in C++20
		std::ostringstream ss;
		for(uint32_t d = 0; d < ops_.size(); ++d)
		{
			ss << "layer" << d << ": [" << ops_[d]->desc() << "]" << std::endl;
		}
		return ss.str();
	}
};

Circuit operator|(Circuit a, const Circuit& b);
Circuit operator|(Circuit a, Circuit&& b);

} // namespace yavque


// Filename: CirculationModel_aorta.cpp
// Created on 20 Aug 2007 by Boyce Griffith

// Modified 2019, Alexander D. Kaiser

#include "CirculationModel_aorta.h"
#include "pnpoly.h"
/////////////////////////////// INCLUDES /////////////////////////////////////

#ifndef included_IBAMR_config
#include <IBAMR_config.h>
#define included_IBAMR_config
#endif

#ifndef included_SAMRAI_config
#include <SAMRAI_config.h>
#define included_SAMRAI_config
#endif

// SAMRAI INCLUDES
#include <CartesianGridGeometry.h>
#include <CartesianPatchGeometry.h>
#include <PatchLevel.h>
#include <SideData.h>
#include <tbox/RestartManager.h>
#include <tbox/SAMRAI_MPI.h>
#include <tbox/Utilities.h>

// C++ STDLIB INCLUDES
#include <cassert>

#include <Eigen/Dense>
using namespace Eigen;

namespace
{
// Name of output file.
static const string DATA_FILE_NAME = "bc_data.m";

}

/////////////////////////////// NAMESPACE ////////////////////////////////////

/////////////////////////////// STATIC ///////////////////////////////////////

/////////////////////////////// PUBLIC ///////////////////////////////////////

CirculationModel_aorta::CirculationModel_aorta(Pointer<Database> input_db,
                                               const fourier_series_data *fourier_ventricle,
                                               string ventricle_vertices_file_name,
                                               string aorta_vertices_file_name,
                                               const double  cycle_duration,
                                               const double  t_offset_bcs_unscaled,
                                               const double  initial_time,
                                               double P_initial_aorta,
                                               bool rcr_bcs_on,
                                               bool P_initial_aorta_equal_to_ventricle,
                                               double rcr_on_time)
    :
      d_object_name("circ_model_aorta"),  // constant name here
      d_registered_for_restart(true),      // always true
      d_fourier_ventricle(fourier_ventricle),
      d_cycle_duration(cycle_duration),
      d_t_offset_bcs_unscaled(t_offset_bcs_unscaled),
      d_current_idx_series(0),
      d_Q_ventricle(0.0),
      d_Q_aorta(0.0),
      d_time(initial_time),
      d_aorta_P_Wk(P_initial_aorta),
      d_p_extender_mean(0.0),
      d_p_extender_point(0.0),
      d_area_ventricle(0.0),
      d_area_aorta(0.0),
      d_area_initialized(false),
      d_rcr_bcs_on(rcr_bcs_on),
      d_P_initial_aorta_equal_to_ventricle(P_initial_aorta_equal_to_ventricle),
      d_rcr_on_time(rcr_on_time)
{

    if (d_registered_for_restart)
    {
        RestartManager::getManager()->registerRestartItem(d_object_name, this);
    }

    // Initialize object with data read from the input and restart databases.
    const bool from_restart = RestartManager::getManager()->isFromRestart();
    if (from_restart)
    {
        getFromRestart();
    }

    if (d_rcr_bcs_on){
        if (input_db){
            // left and right equal for now
            d_aorta_R_proximal = input_db->getDouble("R_proximal");
            d_aorta_R_distal   = input_db->getDouble("R_distal");
            d_aorta_C          = input_db->getDouble("C");

            std::cout << "input db got values:\n";
            std::cout << "input db got values R_proximal = " << d_aorta_R_proximal << "\tR_distal = " << d_aorta_R_distal << "\tC = " << d_aorta_C << "\n";
        }
        else {
            TBOX_ERROR("Must provide valid input_db");
        }
    }

    double x,x_prev,y,y_prev,z,z_prev;
    double tol = 1.0e-2;

    // read vertices from file
    ifstream ventricle_file(ventricle_vertices_file_name.c_str(), ios::in);

    if(!ventricle_file){
        TBOX_ERROR("Aorta file not found\n");
    }

    ventricle_file >> d_n_pts_ventricle;

    d_ventricle_points_idx1 = new double[d_n_pts_ventricle];
    d_ventricle_points_idx2 = new double[d_n_pts_ventricle];

    for (int i=0; i<d_n_pts_ventricle; i++){
        ventricle_file >> x;
        ventricle_file >> d_ventricle_points_idx1[i];
        ventricle_file >> d_ventricle_points_idx2[i];

        if (i>0){
            if (fabs(x_prev - x) > tol){
                TBOX_ERROR("x coordinates must be consistent\n");
            }
        }
        x_prev = x;

    }
    pout << "to ventricle file close\n";
    ventricle_file.close();
    // hardcode to top
    d_ventricle_axis = 0;
    d_ventricle_side = 1;

    // read vertices from file
    ifstream aorta_file(aorta_vertices_file_name.c_str(), ios::in);

    if(!aorta_file){
        TBOX_ERROR("Aorta file not found\n");
    }

    aorta_file >> d_n_pts_aorta;

    d_aorta_points_idx1 = new double[d_n_pts_aorta];
    d_aorta_points_idx2 = new double[d_n_pts_aorta];

    for (int i=0; i<d_n_pts_aorta; i++){
        aorta_file >> d_aorta_points_idx1[i];
        aorta_file >> d_aorta_points_idx2[i];
        aorta_file >> z;

        if (i>0){
            if (fabs(z_prev - z) > tol){
                TBOX_ERROR("z coordinates must be consistent\n");
            }
        }
        z_prev = z;

    }
    pout << "to aorta file close\n";
    aorta_file.close();
    d_aorta_axis = 2;
    d_aorta_side = 1;

    if (!from_restart){
        if (d_P_initial_aorta_equal_to_ventricle){
            d_aorta_P = MMHG_TO_CGS * this->d_fourier_ventricle->values[0];
        }
        else{
            d_aorta_P = P_initial_aorta;
        }
    }

    pout << "passed contstructor\n";

    pout << "initial aorta pressure = " << P_initial_aorta << ", P_wk = " << d_aorta_P << "\n";

    return;
} // CirculationModel

CirculationModel_aorta::~CirculationModel_aorta()
{
    return;
} // ~CirculationModel_aorta

void CirculationModel_aorta::advanceTimeDependentData(const double dt,
                                                        const Pointer<PatchHierarchy<NDIM> > hierarchy,
                                                        const int U_idx,
                                                        const int /*P_idx*/,
                                                        const int /*wgt_cc_idx*/,
                                                        const int wgt_sc_idx)
{
    // Compute the mean flow rates in the vicinity of the inflow and outflow
    // boundaries.

    double Q_ventricle_local = 0.0;
    double Q_aorta_local = 0.0;

    double area_ventricle_local = 0.0;
    double area_aorta_local = 0.0;

    for (int ln = 0; ln <= hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = hierarchy->getPatchLevel(ln);
        for (PatchLevel<NDIM>::Iterator p(level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            Pointer<CartesianPatchGeometry<NDIM> > pgeom = patch->getPatchGeometry();
            if (pgeom->getTouchesRegularBoundary())
            {
                Pointer<SideData<NDIM, double> > U_data = patch->getPatchData(U_idx);
                Pointer<SideData<NDIM, double> > wgt_sc_data = patch->getPatchData(wgt_sc_idx);
                const Box<NDIM>& patch_box = patch->getBox();
                const double* const x_lower = pgeom->getXLower();
                const double* const dx = pgeom->getDx();
                double dV = 1.0;
                for (int d = 0; d < NDIM; ++d)
                {
                    dV *= dx[d];
                }

                for(int axis=0; axis<3; axis++)
                {
                    for(int side=0; side<2; side++)
                    {
                        const bool is_lower = (side == 0);
                        if (pgeom->getTouchesRegularBoundary(axis, side))
                        {

                            Vector n;
                            for (int d = 0; d < NDIM; ++d)
                            {
                                n[d] = axis == d ? (is_lower ? -1.0 : +1.0) : 0.0;
                            }
                            Box<NDIM> side_box = patch_box;
                            if (is_lower)
                            {
                                side_box.lower(axis) = patch_box.lower(axis);
                                side_box.upper(axis) = patch_box.lower(axis);
                            }
                            else
                            {
                                side_box.lower(axis) = patch_box.upper(axis) + 1;
                                side_box.upper(axis) = patch_box.upper(axis) + 1;
                            }
                            for (Box<NDIM>::Iterator b(side_box); b; b++)
                            {
                                const Index<NDIM>& i = b();

                                double X[NDIM];
                                for (int d = 0; d < NDIM; ++d)
                                {
                                    X[d] = x_lower[d] + dx[d] * (double(i(d) - patch_box.lower(d)) + (d == axis ? 0.0 : 0.5));
                                }

                                double X_in_plane_1 = 0.0;
                                double X_in_plane_2 = 0.0;
                                if (axis == 0)
                                {
                                    X_in_plane_1 = X[1];
                                    X_in_plane_2 = X[2];
                                }
                                else if (axis == 1)
                                {
                                    X_in_plane_1 = X[0];
                                    X_in_plane_2 = X[2];
                                }
                                else if (axis == 2)
                                {
                                    X_in_plane_1 = X[0];
                                    X_in_plane_2 = X[1];
                                }
                                else{
                                    TBOX_ERROR("Invalid value of axis\n");
                                }

                                const int in_ventricle  = this->point_in_ventricle(X_in_plane_1, X_in_plane_2, axis, side);
                                const int in_aorta      = this->point_in_aorta    (X_in_plane_1, X_in_plane_2, axis, side);

                                if (in_ventricle && in_aorta){
                                    TBOX_ERROR("Position is within two inlets and outlets, should be impossible\n");
                                }

                                if (in_ventricle)
                                {
                                    const SideIndex<NDIM> i_s(i, axis, SideIndex<NDIM>::Lower);
                                    if ((*wgt_sc_data)(i_s) > std::numeric_limits<double>::epsilon())
                                    {
                                        double dA = dV / dx[axis];
                                        Q_ventricle_local += (*U_data)(i_s)* n[axis] * dA;

                                        if (!d_area_initialized){
                                            area_ventricle_local += dA;
                                        }

                                    }
                                }

                                if (in_aorta)
                                {
                                    const SideIndex<NDIM> i_s(i, axis, SideIndex<NDIM>::Lower);
                                    if ((*wgt_sc_data)(i_s) > std::numeric_limits<double>::epsilon())
                                    {
                                        double dA = dV / dx[axis];
                                        Q_aorta_local += (*U_data)(i_s) * n[axis] * dA;

                                        if (!d_area_initialized){
                                            area_aorta_local += dA;
                                        }

                                    }
                                }

                            }
                        }
                    }
                }
            }
        }
    }

    d_Q_ventricle = SAMRAI_MPI::sumReduction(Q_ventricle_local);
    d_Q_aorta        = SAMRAI_MPI::sumReduction(Q_aorta_local);

    if (!d_area_initialized){
        d_area_ventricle = SAMRAI_MPI::sumReduction(area_ventricle_local);
        d_area_aorta        = SAMRAI_MPI::sumReduction(area_aorta_local);
        d_area_initialized = true;
    }

    if (d_rcr_bcs_on){
        // The downstream pressure is determined by a three-element Windkessel model.

        if ((d_P_initial_aorta_equal_to_ventricle) && (d_time < d_rcr_on_time)){
            // linear interpolation
            d_aorta_P = (1 - d_time/d_rcr_on_time) * MMHG_TO_CGS * this->d_fourier_ventricle->values[0] +
                        (    d_time/d_rcr_on_time) * d_aorta_P_Wk; // wk pressure is the end pressure for the interpolation
        }
        else{
            d_aorta_P_Wk = ((d_aorta_C / dt) * d_aorta_P_Wk + d_Q_aorta) / (d_aorta_C / dt + 1.0 / d_aorta_R_distal);
            d_aorta_P = d_aorta_P_Wk + d_aorta_R_proximal * d_Q_aorta;
        }
    }

    // print_summary();

    // bool debug_out_areas = false;
    // if (debug_out_areas){
    //     pout << "d_area_ventricle = " << d_area_ventricle << "\n";
    //     pout << "d_area_aorta = " << d_area_aorta << "\n";
    // }

    d_time += dt;

    // compute which index in the Fourier series we need here
    // always use a time in current cycle
    double t_reduced = d_time - d_cycle_duration * floor(d_time/d_cycle_duration);

    // fourier series has its own period, scale to that
    double t_scaled = t_reduced * (d_fourier_ventricle->L  / d_cycle_duration);

    // start offset some arbitrary time in the cardiac cycle, but this is relative to the series length
    double t_scaled_offset = t_scaled + d_t_offset_bcs_unscaled;

    // Fourier data here
    // index without periodicity
    unsigned int k = (unsigned int) floor(t_scaled_offset / (d_fourier_ventricle->dt));

    // // take periodic reduction
    d_current_idx_series = k % (d_fourier_ventricle->N_times);

    // bool debug_out = false;
    // if (debug_out){
    //     pout << "circ mode: d_time = " << d_time << ", d_current_idx_series = " << d_current_idx_series << "\n";
    //     pout << "t_reduced = " << t_reduced << " t_scaled = " << t_scaled << " t_scaled_offset = " << t_scaled_offset << "\n";
    //     pout << "k (unreduced idx) = " << k << " d_current_idx_series = " << d_current_idx_series << "\n\n";
    // }

    writeDataFile();

} // advanceTimeDependentData

void CirculationModel_aorta::set_Q_valve(double Q_valve){
    d_Q_valve = Q_valve;
}

void CirculationModel_aorta::set_extender_pressures(double p_extender_mean, double p_extender_point){
    d_p_extender_mean  = p_extender_mean;
    d_p_extender_point = p_extender_point;
}

void
CirculationModel_aorta::putToDatabase(Pointer<Database> db)
{

    db->putInteger("d_current_idx_series", d_current_idx_series);
    db->putDouble("d_Q_ventricle", d_Q_ventricle);
    db->putDouble("d_Q_aorta", d_Q_aorta);
    db->putDouble("d_Q_valve", d_Q_valve);
    db->putDouble("d_aorta_P", d_aorta_P);
    db->putDouble("d_aorta_P_Wk", d_aorta_P_Wk);
    db->putDouble("d_p_extender_mean", d_p_extender_mean);
    db->putDouble("d_p_extender_point", d_p_extender_point);
    db->putDouble("d_time", d_time);
    db->putBool("d_rcr_bcs_on", d_rcr_bcs_on);
    return;
} // putToDatabase

void CirculationModel_aorta::print_summary(){

    double P_ventricle = d_fourier_ventricle->values[d_current_idx_series];
    double P_aorta = d_aorta_P / MMHG_TO_CGS;

    if (!d_rcr_bcs_on){
        TBOX_ERROR("Not implemented\n");
        // P_aorta        = d_fourier_aorta->values[d_current_idx_series];
    }

    pout << "rcr_bcs_on = " << d_rcr_bcs_on << "\n";
    pout << "% time \t       P_ventricle (mmHg)\t   P_aorta (mmHg)\t  Q_ventricle (ml/s)\t    d_Q_aorta (ml/s)\t  d_Q_valve (ml/s)\t  Q_current_idx_series \t idx" ;
    pout << "\t aorta_P_Wk \t p_extender_mean \t p_extender_point ";
    pout << "\n";
    pout << d_time << " " << P_ventricle <<  " " << P_aorta << " " << d_Q_ventricle << " " << d_Q_aorta << " " << d_Q_valve << " " << d_current_idx_series;
    pout  << " " << d_aorta_P_Wk << " " << d_p_extender_mean/MMHG_TO_CGS << " " << d_p_extender_point/MMHG_TO_CGS;
    pout << "\n";

}

int CirculationModel_aorta::point_in_ventricle(double testx, double testy, int axis, int side){
    // checks whether given point is in right ventricle

    // quick exit for correct side and axis
    if ((axis != d_ventricle_axis) || (side != d_ventricle_side))
        return 0;

    return pnpoly(d_n_pts_ventricle, d_ventricle_points_idx1, d_ventricle_points_idx2, testx, testy);
}

int CirculationModel_aorta::point_in_aorta(double testx, double testy, int axis, int side){
    // checks whether given point is in right ventricle

    // quick exit for correct side and axis
    if ((axis != d_aorta_axis) || (side != d_aorta_side))
        return 0;

    return pnpoly(d_n_pts_aorta, d_aorta_points_idx1, d_aorta_points_idx2, testx, testy);
}

    void CirculationModel_aorta::write_plot_code()
{

    static const int mpi_root = 0;
    if (SAMRAI_MPI::getRank() == mpi_root)
    {
        ofstream fout(DATA_FILE_NAME.c_str(), ios::app);
        fout.setf(ios_base::scientific);
        fout.setf(ios_base::showpos);
        fout.precision(10);
        fout << "];\n";
        fout << "fig = figure;\n";
        fout << "times            =  bc_vals(:,1);\n";
        fout << "p_lv             =  bc_vals(:,2);\n";
        fout << "p_aorta          =  bc_vals(:,3); \n";
        fout << "q_ventricle      = -bc_vals(:,4);\n";
        fout << "q_aorta          =  bc_vals(:,5);\n";
        fout << "q_valve          =  bc_vals(:,6);\n";
        fout << "p_wk             =  bc_vals(:,7);\n";
        fout << "p_extender_mean  =  bc_vals(:,8);\n";
        fout << "p_extender_point =  bc_vals(:,9);\n";
        fout << "subplot(2,1,1)\n";
        fout << "plot(times, p_aorta, 'k')\n";
        fout << "hold on\n";
        fout << "plot(times, p_wk, ':k')\n";
        fout << "plot(times, p_lv, '--k')\n";
        fout << "%plot(times, p_extender_mean)\n";
        fout << "plot(times, p_extender_point)\n";
        fout << "%legend('P_{Ao}', 'P_{Wk}', 'P_{LV}', 'P extender mean', 'P extender point', 'Location','NorthEastOutside');\n";
        fout << "%legend('P_{Ao}', 'P_{Wk}', 'P_{LV}', 'P extender point', 'Location','NorthEastOutside');\n";
        fout << "xlabel('t (s)');\n";
        fout << "ylabel('P (mmHg)');\n";
        fout << "subplot(2,1,2)\n";
        fout << "plot(times, q_aorta, 'k')\n";
        fout << "hold on\n";
        fout << "dt = times(2,1) - times(1);\n";
        fout << "net_flux = dt*cumsum(q_aorta);\n";
        fout << "plot(times, net_flux, '--k')\n";
        fout << "plot(times, q_ventricle)\n";
        fout << "% plot(times, q_valve)\n";
        fout << "plot(bc_vals(:,1), 0*net_flux, ':k')\n";
        fout << "%legend('Q', 'net Q', 'Q ventricle', 'Q valve', 'Location','NorthEastOutside')\n";
        fout << "legend('Q', 'net Q', 'Q ventricle', 'Location','NorthEastOutside')\n";
        fout << "xlabel('t (s)')\n";
        fout << "ylabel('Flow (ml/s), Net Flow (ml)')\n";
        fout << "set(fig, 'Position', [100, 100, 1000, 750])\n";
        fout << "set(fig,'PaperPositionMode','auto')\n";
        fout << "printfig(fig, 'bc_model_variables')\n";
        fout << "diary notes.txt\n";
        fout << "min_p_aorta_after_first_beat = min(p_aorta(floor(end/3):end))\n";
        fout << "max_p_aorta_after_first_beat = max(p_aorta(floor(end/3):end))\n";
        fout << "mean_p_aorta = mean(p_aorta)\n";
        fout << "mean_p_wk    = mean(p_wk)\n";
        fout << "max_p_exender_mean = max(p_extender_mean)\n";
        fout << "max_p_exender_point = max(p_extender_point)\n";
        fout << "mean_p_lv    = mean(p_lv)\n";
        fout << "end_flux    = net_flux(end)\n";
        fout << "max_flux    = max(net_flux)\n";
        fout << "diary off\n";

    }
    return;

}

/////////////////////////////// PROTECTED ////////////////////////////////////

/////////////////////////////// PRIVATE //////////////////////////////////////

void
    CirculationModel_aorta::writeDataFile() const
{
    static const int mpi_root = 0;
    if (SAMRAI_MPI::getRank() == mpi_root)
    {
        static bool file_initialized = false;
        const bool from_restart = RestartManager::getManager()->isFromRestart();
        if (!from_restart && !file_initialized)
        {
            ofstream fout(DATA_FILE_NAME.c_str(), ios::out);
            fout << "% time \t P_ventricle (mmHg)\t P_aorta (mmHg)\t d_Q_ventricle (ml/s)\t d_Q_aorta (ml/s)\t d_Q_valve (ml/s)\t"
                 << "d_aorta_P_Wk (mmHg) \t d_p_extender_mean (mmHg) \t d_p_extender_point (mmHg)"
                 << "\n"
                 << "bc_vals = [";
            file_initialized = true;
        }

        ofstream fout(DATA_FILE_NAME.c_str(), ios::app);

        fout << d_time;
        fout.setf(ios_base::scientific);
        fout.setf(ios_base::showpos);
        fout.precision(10);

        double P_ventricle = d_fourier_ventricle->values[d_current_idx_series];

        double P_aorta = 0.0;

        if (d_rcr_bcs_on){
            P_aorta        = d_aorta_P/MMHG_TO_CGS;
        }
        else{
            TBOX_ERROR("not implemented\n");
            // P_aorta        = d_fourier_aorta->values[d_current_idx_series];
        }

        fout << " " << P_ventricle <<  " " << P_aorta;
        fout << " " << d_Q_ventricle << " " << d_Q_aorta << " " << d_Q_valve;
        fout << " " << d_aorta_P_Wk/MMHG_TO_CGS;
        fout << " " << d_p_extender_mean/MMHG_TO_CGS;
        fout << " " << d_p_extender_point/MMHG_TO_CGS;
        fout << "; \n";

    }

    return;
} // writeDataFile

void
CirculationModel_aorta::getFromRestart()
{
    Pointer<Database> restart_db = RestartManager::getManager()->getRootDatabase();
    Pointer<Database> db;
    if (restart_db->isDatabase(d_object_name))
    {
        db = restart_db->getDatabase(d_object_name);
    }
    else
    {
        TBOX_ERROR("Restart database corresponding to " << d_object_name << " not found in restart file.");
    }

    d_current_idx_series = db->getInteger("d_current_idx_series");
    d_Q_ventricle        = db->getDouble("d_Q_ventricle");
    d_Q_aorta            = db->getDouble("d_Q_aorta");
    d_Q_valve            = db->getDouble("d_Q_valve");
    d_aorta_P            = db->getDouble("d_aorta_P");
    d_aorta_P_Wk         = db->getDouble("d_aorta_P_Wk");
    d_p_extender_mean    = db->getDouble("d_p_extender_mean");
    d_p_extender_point   = db->getDouble("d_p_extender_point");
    d_time               = db->getDouble("d_time");
    d_rcr_bcs_on         = db->getBool("d_rcr_bcs_on");
    return;
} // getFromRestart

/////////////////////////////// NAMESPACE ////////////////////////////////////

/////////////////////////////// TEMPLATE INSTANTIATION ///////////////////////

//////////////////////////////////////////////////////////////////////////////