/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2013-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef NUMAMATRIX_HH
#define NUMAMATRIX_HH

#include <algorithm>
#include <cassert>
#include <vector>

#include <boost/iterator/iterator_facade.hpp>

#include "linalg/dynamicMatrix.hh"
#include "utilities/threading.hh"

namespace Kaskade {

  // forward declaration
  template <class Entry> class NumaDenseMatrix;

  /**
   * \cond internals
   */
  namespace NumaDenseMatrixDetail
  {

    template <class Entry>
    class ColumnIterator: public boost::iterator_facade<ColumnIterator<Entry>,Entry,boost::random_access_traversal_tag>
    {
    public:
      ColumnIterator() = default;

      ColumnIterator(Entry* base_, size_t stride_, size_t idx_=0)
      : base(base_), stride(stride_), idx(idx_) {}

      size_t index() const { return idx; }

    private:
      friend class boost::iterator_core_access;

      bool equal(ColumnIterator<Entry> const& p) const
      {
        return idx==p.idx;
      }

      void increment() { ++idx; }
      void decrement() { --idx; }
      void advance(typename ColumnIterator<Entry>::difference_type n) { idx += n; }
      template <class E>
      typename ColumnIterator<Entry>::difference_type distance_to(ColumnIterator<E> const& i) const { return i.idx - idx; }

      Entry& dereference() const { return base[idx*stride]; }

      Entry* base;
      size_t stride;
      size_t idx;
    };

    // ----------------------------------------------------------------------------------------------

    template <class Entry>
    class Row
    {
    public:
      typedef ColumnIterator<Entry> iterator;
      typedef ColumnIterator<Entry const> const_iterator;

      Row() = default;

      Row(Entry* first_, size_t stride_, std::size_t cols_)
      : first(first_), stride(stride_), cols(cols_) {}

      iterator begin() { return iterator(first,stride,0); }
      iterator end()   { return iterator(first,stride,cols); }
      const_iterator begin() const { return const_iterator(first,stride,0); }
      const_iterator end()   const { return const_iterator(first,stride,cols); }

      Entry&       operator[](size_t c)       { return first[stride*c]; }
      Entry const& operator[](size_t c) const { return first[stride*c]; }

    private:
      Entry* first;
      size_t stride;
      size_t cols;
    };

    // ----------------------------------------------------------------------------------------------

    template <class Entry>
    class RowIterator: public boost::iterator_facade<RowIterator<Entry>,
                                                     typename std::conditional<std::is_const<Entry>::value,
                                                                               Row<typename std::remove_const<Entry>::type> const,
                                                                               Row<Entry>>::type,
                                                     boost::random_access_traversal_tag>
    {
      typedef typename std::remove_const<Entry>::type NonConstEntry;

      typedef typename std::conditional<std::is_const<Entry>::value,
                                        NumaDenseMatrix<NonConstEntry> const,
                                        NumaDenseMatrix<Entry>>::type Matrix;

      typedef boost::iterator_facade<RowIterator<Entry>,
                                    typename std::conditional<std::is_const<Entry>::value,
                                                              Row<NonConstEntry> const,
                                                              Row<Entry>>::type,
                                    boost::random_access_traversal_tag> Base;

    public:
      RowIterator() = default;

      RowIterator(Matrix& mat_, size_t idx_)
      : mat(&mat_)
      {
        update(idx_);
      }

      size_t index() const { return idx; }

    private:
      friend class boost::iterator_core_access;
      using DifferenceType = Base::difference_type;

      bool equal(RowIterator<Entry> const& other) const { return idx==other.idx;}
      void increment() { update(idx+1); }
      void decrement() { update(idx-1); }
      void advance(typename RowIterator<Entry>::difference_type n) { update(idx+n); }
      template <class E> DifferenceType distance_to(RowIterator<E> const& i) const { return static_cast<DifferenceType>(i.idx) - idx; }
      typename Base::reference dereference() const { return row; }

      // moves row as to represent row with index newidx.
      void update(size_t newidx)
      {
        idx = newidx;
        if (idx<mat->N()) // to not trip over the edge when arriving at end()
        {
          size_t chunk = uniformWeightRange(idx,mat->chunks.size(),mat->N());
          size_t rowStart = uniformWeightRangeStart(chunk,mat->chunks.size(),mat->N());
          size_t stride = uniformWeightRangeStart(chunk+1,mat->chunks.size(),mat->N()) - rowStart; // that's "LDA"
          row = typename Base::value_type(const_cast<NonConstEntry*>(&mat->chunks[chunk][idx-rowStart]),stride,mat->M());
        }
      }

      Matrix* mat;
      size_t idx; // row index
      mutable typename Base::value_type row; // dereference is const method, but can "point to" mutable row value type..
    };

    // ----------------------------------------------------------------------------------------------

    template <class T, int n>
    typename Dune::FieldTraits<T>::field_type frobenius_product(Dune::FieldVector<T,n> const& x, Dune::FieldVector<T,n> const& y)
    {
      typename Dune::FieldTraits<T>::field_type result = 0;
      for (int i=0; i<n; ++i)
        result += frobenius_product(x[i],y[i]);
      return result;
    }

    template <class T, int n, int m>
    typename Dune::FieldTraits<T>::field_type frobenius_product(Dune::FieldMatrix<T,n,m> const& x, Dune::FieldMatrix<T,n,m> const& y)
    {
      typename Dune::FieldTraits<T>::field_type result = 0;
      for (int i=0; i<n; ++i)
        for (int j=0; j<m; ++j)
          result += frobenius_product(x[i][j],y[i][j]);
      return result;
    }

    double frobenius_product(double x, double y) { return x*y; }
    float  frobenius_product(float x, float y)  { return x*y; }

    template <class T, int n>
    typename Dune::FieldTraits<typename Dune::FieldTraits<T>::field_type>::real_type frobenius_norm2(Dune::FieldVector<T,n> const& x) { return x.two_norm2(); }

    template <class T, int n, int m>
    typename Dune::FieldTraits<typename Dune::FieldTraits<T>::field_type>::real_type frobenius_norm2(Dune::FieldMatrix<T,n,m> const& x) { return x.frobenius_norm2(); }

    double frobenius_norm2(double x) { return x*x; }
    float  frobenius_norm2(float x)  { return x*x; }

    template <class T, int n>
    typename Dune::FieldTraits<typename Dune::FieldTraits<T>::field_type>::real_type one_norm(Dune::FieldVector<T,n> const& x) { return x.one_norm(); }

    template <class T, int n, int m>
    typename Dune::FieldTraits<typename Dune::FieldTraits<T>::field_type>::real_type one_norm(Dune::FieldMatrix<T,n,m> const& x) { return x.one_norm(); }

    double one_norm(double x) { return std::abs(x); }
    float  one_norm(float x)  { return std::abs(x); }

    template <class T, int n>
    void axpy(Dune::FieldVector<T,n>& y, typename Dune::FieldTraits<T>::field_type const& a, Dune::FieldVector<T,n> const& x) { y.axpy(a,x); }

    template <class T, int n, int m>
    void axpy(Dune::FieldMatrix<T,n,m>& y, typename Dune::FieldTraits<T>::field_type const& a, Dune::FieldMatrix<T,n,m> const& x) { y += a*x; }

    void axpy(double& y, double a, double x) { y += a*x; }
    void axpy(float& y,  float a,  float x) { y += a*x; }

    // ----------------------------------------------------------------------------------------------

    // forward declaration
    template <class Entry> class VectorIterator;

    /**
     * \ingroup linalgbasic
     * \brief A base class for dense matrix and vector classes.
     *
     * This distributes the entries across several NUMA nodes by storing a contiguous block of rows (chunk) on each node.
     * In each chunk, the entries are stored column-major, i.e. as a LAPACK dense matrix.
     */
    template <class Entry>
    class NumaDenseBase
    {
    protected:
      typedef std::vector<Entry,NumaAllocator<Entry>> Chunk;  // the actual data storage type
      typedef NumaDenseBase<Entry> Self;

    public:
      typedef size_t      size_type;
      typedef Entry       value_type;

      /**
       * \brief the type of the scalar field.
       */
      typedef typename Dune::FieldTraits<Entry>::field_type     field_type;
      typedef typename Dune::FieldTraits<field_type>::real_type real_type;

      /**
       * \brief the type of matrix entries.
       */
      typedef Entry block_type;

      /**
       * \name Construction and assignment
       * @{
       */

      /**
       * \brief Creates a 0 x 0 matrix.
       */
      NumaDenseBase (): NumaDenseBase(0,0)
      {
      }

      /**
       * \brief Copy constructor.
       */
      NumaDenseBase (NumaDenseBase const& other)
      : rows(other.rows), cols(other.cols), chunks(other.chunks.size())
      {
        parallelForNodes([&](int i, int n) // perform data copy on the respective
        {                                  // NUMA node
          chunks[i] = other.chunks[i];
        });
      }

      /**
       * \brief Move constructor.
       */
      NumaDenseBase(NumaDenseBase&& a) = default;

      /**
       * \brief Creates an r x c matrix with given entries.
       */
      NumaDenseBase (size_type r, size_type c, value_type v = value_type()) :
      rows(r), cols(c)
      {
        size_type n = NumaThreadPool::instance().nodes();
        chunks.reserve(n);
        for (size_type i=0; i<n; ++i)
          chunks.emplace_back((uniformWeightRangeStart(i+1,n,r)-uniformWeightRangeStart(i,n,r))*c,v,NumaAllocator<Entry>(i));
      }

      /**
       * \brief Copy assignment.
       */
      NumaDenseBase& operator=(NumaDenseBase const& other)
      {
        assert(chunks.size() == other.chunks.size()); // this can only be violated if the number of NUMA nodes changes...
        rows = other.rows;
        cols = other.cols;
        parallelForNodes([&](int i, int n) // perform data copy on the respective
        {                                  // NUMA node
          chunks[i] = other.chunks[i];
        });
      }

      /**
       * \brief Assignment from scalar.
       */
      Self& operator=(field_type const& a)
      {
        for_each([&](Entry& e) { e = a; });
      }

      /// @}

    protected:
      /**
       * \brief Resizes the matrix to r x c, leaving the entries in an undefined state.
       *
       * On resize, all row objects and iterators are invalidated. This method adheres to the Dune::DynamicMatrix interface.
       */
      void resize (size_type r, size_type c)
      {
        // Check whether we need to change anything. Note that due to row blocking,
        // the check rows*cols==n*m is NOT sufficient (the distribution of rows on
        // NUMA nodes may change nevertheless).
        if (rows==r && cols==c)
          return;

        rows = r;
        cols = c;
        for (int i=0; i<chunks.size(); ++i) // TODO: parallelize
          chunks[i].resize((uniformWeightRangeStart(i+1,chunks.size(),r)-uniformWeightRangeStart(i,chunks.size(),r))*c);
      }

    public:
      /**
       * \brief Return the number of rows.
       */
      size_type N() const { return rows; }

    protected:
      /**
       * \brief Return the number of rows.
       */
      size_type M() const { return cols; }

      /**
       * \brief scalar product
       */
      field_type dot(Self const& other) const
      {
        assert(rows==other.rows && cols==other.cols);
        std::vector<field_type> dots(chunks.size());
        parallelForNodes([&](int i, int n)
        {
          assert(n==chunks.size());
          field_type sum = 0;
          for (size_t k=0; k<chunks[i].size(); ++k)
            sum += NumaDenseMatrixDetail::frobenius_product(chunks[i][k],other.chunks[i][k]);
          dots[i] = sum;
        });
        return std::accumulate(dots.begin(),dots.end(),0);
      }

      /**
       * \brief Performs a (parallel) accumulation procedure.
       *
       * Note that this groups the operations in a different way than the std::accumulate, and hence
       * gives only the same results if the operation F is associative.
       */
      template <class Result, class F>
      Result accumulate(F f, Result const& init) const
      {
        std::vector<Result> rs(chunks.size());
        parallelForNodes([&](int i, int n)
        {
          assert(n==this->chunks.size());
          Result r = init;
          for (auto const& e: chunks[i])
            r = f(r,e);
          rs[i] = r;
        });
        return std::accumulate(rs.begin()+1,rs.end(),rs[0],f);
      }

      /**
       * \brief Applies the given functor to all matrix entries.
       */
      template <class F>
      void for_each(F const& f)
      {
        parallelForNodes([&](int i, int n)
        {
          assert(n==this->chunks.size());
          std::for_each(begin(chunks[i]),end(chunks[i]),f);
        });
      }

      /**
       * \brief Applies the given functor to pairs of corresponding matrix elements.
       */
      template <class F>
      void for_each2(F const& f, Self const& other)
      {
        assert(rows==other.rows && cols==other.cols);
        parallelForNodes([&](int i, int n)
        {
          assert(n==this->chunks.size());
          for (size_t j=0; j<chunks[i].size(); ++j)
            f(chunks[i][j],other.chunks[i][j]);
        });
      }

    private:
      friend class NumaDenseMatrixDetail::RowIterator<Entry>;
      friend class NumaDenseMatrixDetail::RowIterator<Entry const>;
      friend class NumaDenseMatrixDetail::VectorIterator<Entry>;
      friend class NumaDenseMatrixDetail::VectorIterator<Entry const>;

      size_type rows, cols;      // the matrix dimensions
      std::vector<Chunk> chunks; // for each NUMA node a separate array to store the entries
    };

  }
  /**
   * \endcond
   */

  //-------------------------------------------------------------------------------------------

  /**
   * \ingroup linalgbasic
   * \brief A dense matrix class tailored towards NUMA architectures.
   *
   * The class satisfies the Dune ISTL recursive matrix interface similar to Dune::Matrix. The data storage, however,
   * is organized in such a way as to benefit from NUMA architectures: the rows of the matrix are distributed evenly
   * over the nodes. This implies that a good load balancing can only be achieved for matrices with (significantly)
   * more rows than NUMA nodes. Avoid flat, long matrices.
   *
   * There is no guarantee about placement or ordering of matrix elements, in particular none about the
   * contiguity of element storage.
   *
   * \tparam Entry the entry type (usually a Dune::FieldMatrix<n,m,double>)
   *
   * \TODO: Currently, the implementation is mainly intended to gather a bag of coefficient vectors in one place,
   * rather than to provide a fully-fledged matrix class.
   */
  template <class Entry>
  class NumaDenseMatrix: public NumaDenseMatrixDetail::NumaDenseBase<Entry>
  {
    typedef NumaDenseMatrix<Entry> Self;
    typedef NumaDenseMatrixDetail::NumaDenseBase<Entry> Base;

  public:
    typedef size_t      size_type;
    typedef Entry       value_type;

    /**
     * \brief the type of the scalar field.
     */
    typedef typename Dune::FieldTraits<Entry>::field_type         field_type;
    typedef typename Dune::FieldTraits<field_type>::real_type     real_type;

    typedef NumaDenseMatrixDetail::RowIterator<Entry>       RowIterator;
    typedef NumaDenseMatrixDetail::RowIterator<Entry const> ConstRowIterator;

    typedef typename RowIterator::value_type      row_type;
    typedef typename ConstRowIterator::value_type const_row_type;

    /**
     * \name Construction, assignment, and resize.
     * @{
     */

    /**
     * \brief Creates a 0 x 0 matrix.
     */
    NumaDenseMatrix () = default;

    /**
     * \brief Copy constructor.
     */
    NumaDenseMatrix (NumaDenseMatrix const& a) = default;

    /**
     * \brief Move constructor.
     */
    NumaDenseMatrix (NumaDenseMatrix&& a) = default;

    /**
     * \brief Creates an r x c matrix with given entries.
     */
    NumaDenseMatrix (size_type r, size_type c, value_type v = value_type())
    : Base(r,c,v)
    {}

    /**
     * \brief Copy assignment.
     */
    NumaDenseMatrix& operator=(NumaDenseMatrix const& a) = default;

    /**
     * \brief Assignment from scalar.
     */
    Self& operator=(field_type const& a)
    {
      Base::operator=(a);
      return *this;
    }

    /**
     * \brief Resizes the matrix to r x c, leaving the entries in an undefined state.
     *
     * On resize, all row objects and iterators are invalidated. This method adheres to the Dune::DynamicMatrix interface.
     */
    void resize (size_type r, size_type c)
    {
      Base::resize(r,c);
    }

    /**
     * \brief Resizes the matrix to r x c, leaving the entries in an undefined state.
     *
     * On resize, all row objects and iterators are invalidated. This method adheres to the Dune::Matrix interface.
     */
    void setSize (size_type r, size_type c)
    {
      resize(r,c);
    }

    /// @}

    /**
     * \name Entry access
     * @{
     */

    /**
     * \brief Get iterator to first row.
     */
    RowIterator begin() { return RowIterator(*this,0); }

    /**
     * \brief Get iterator to one beyond last row.
     */
    RowIterator end() { return RowIterator(*this,this->N()); }

    /**
     * \brief Get const iterator to first row.
     */
    ConstRowIterator begin() const { return ConstRowIterator(*this,0); }

    /**
     * \brief Get const iterator to one beyond last row.
     */
    ConstRowIterator end() const { return ConstRowIterator(*this,this->N()); }

    /**
     * \brief The subscript operator.
     *
     * Rows do not exist as persistent data structures in the matrix. Hence the subscript operator
     * returns a row object (in fact a view onto data held by the matrix) by value.
     */
    row_type  operator[] (size_type row) { return *RowIterator(*this,row); }

    /**
     * \brief The const subscript operator.
     *
     * Rows do not exist as persistent data structures in the matrix. Hence the subscript operator
     * returns a row object (in fact a view onto data held by the matrix) by value.
     */
    const_row_type const operator[] (size_type row) const { return *ConstRowIterator(*this,row); }

    /// @}

    /**
     * \brief Return the number of columns.
     */
    size_type M() const { return Base::M(); }

    /**
     * \name Linear algebra operations.
     * @{
     */

    /**
     * \brief Multiplication with a scalar.
     */
    Self& operator*= (field_type const& a)
    {
      this->for_each([&](Entry& e) { e *= a; });
      return *this;
    }

    /**
     * \brief Division by a scalar.
     */
    Self& operator/= (field_type const& scalar)
    {
      return (*this) *= (1/scalar);
    }

    /**
     * \brief Matrix addition.
     * Both matrices have to have the same sizes.
     */
    Self& operator+= (Self const& b)
    {
      this->for_each2([](Entry& y, Entry const& x) { y += x; },b);
      return *this;
    }

    /**
     * \brief Matrix subtraction.
     */
    Self& operator-= (Self const& b)
    {
      this->for_each2([](Entry& y, Entry const& x) { y -= x; },b);
      return *this;
    }

    /// @}

    /**
     * \brief frobenius norm: sqrt(sum over squared frobenius norms of entries)
     */
    real_type frobenius_norm () const { return std::sqrt(frobenius_norm2()); }

    /**
     * \brief frobenius norm squared: sum over squared frobenius norms of entries
     */
    real_type frobenius_norm2 () const
    {
      return this->accumulate([](real_type s, Entry const& e) { return s+NumaDenseMatrixDetail::frobenius_norm2(e); }, real_type(0));
    }
  };

  /**
   * \relates NumaDenseMatrix
   */
  template <class Entry>
  std::ostream& operator<<(std::ostream& out, NumaDenseMatrix<Entry> const& mat)
  {
    for (auto ri=mat.begin(); ri!=mat.end(); ++ri)
    {
      std::copy(ri->begin(),ri->end(),std::ostream_iterator<Entry>(out," "));
      out << '\n';
    }
    return out;
  }

  // --------------------------------------------------------------------------

  // forward declaration
  template <class Entry> class NumaVector;

  /**
   * \cond internals
   */
  namespace NumaDenseMatrixDetail
  {

    template <class Entry>
    class VectorIterator: public boost::iterator_facade<VectorIterator<Entry>,Entry,boost::random_access_traversal_tag>
    {
      typedef typename std::remove_const<Entry>::type NonConstEntry;

      typedef typename std::conditional<std::is_const<Entry>::value,
                                        NumaVector<NonConstEntry> const,
                                        NumaVector<Entry>>::type Vector;
    public:
      VectorIterator() = default;

      VectorIterator(Vector& vec_, typename Vector::size_type idx_)
      : vec(&vec_), idx(idx_) {}

      size_t index() const { return idx; }

    private:
      friend class boost::iterator_core_access;

      bool equal(VectorIterator<Entry> const& p) const
      {
        return idx==p.idx;
      }

      void increment() { ++idx; }
      void decrement() { --idx; }
      void advance(typename VectorIterator<Entry>::difference_type n) { idx += n; }
      template <class E>
      typename VectorIterator<Entry>::difference_type distance_to(VectorIterator<E> const& i) const { return i.idx - idx; }
      Entry& dereference() const
      {
        size_t chunk = uniformWeightRange(idx,vec->chunks.size(),vec->N());
        size_t chunkStart = uniformWeightRangeStart(chunk,vec->chunks.size(),vec->N());
        return vec->chunks[chunk][idx-chunkStart];
      }

      Vector* vec;
      typename Vector::size_type idx;
    };
  }
  /**
   * \endcond
   */

  /**
   * \ingroup linalgbasic
   * \brief A vector class tailored towards NUMA architectures.
   * This vector distributes its entries in blocks of approximately equal size to the different nodes of NUMA machines
   * in order to exploit the larger accumulated memory bandwidth for fast vector operations. This is most beneficial
   * for large vectors exceeding the cache of the CPUs. Modern processors (2015-01-01) have up to 6MB 3rd level cache,
   * so this is intended for vectors of more than 50,000 doubles, say.
   *
   * \tparam Entry the type of vector entries (usually Dune::FieldVector<double,n>)
   */
  template <class Entry>
  class NumaVector: public NumaDenseMatrixDetail::NumaDenseBase<Entry>
  {
    typedef NumaVector<Entry> Self;
    typedef NumaDenseMatrixDetail::NumaDenseBase<Entry> Base;

   public:
    typedef typename Dune::FieldTraits<Entry>::field_type         field_type;
    typedef typename Dune::FieldTraits<field_type>::real_type     real_type;
    typedef typename Base::size_type                              size_type;
    typedef typename Base::value_type                             value_type;
    typedef NumaDenseMatrixDetail::VectorIterator<Entry>          iterator;
    typedef NumaDenseMatrixDetail::VectorIterator<Entry const>    const_iterator;

    /**
     * \name Construction, assignment, and shape changes.
     * @{
     */

    /**
     * \brief Creates a 0 vector.
     */
    NumaVector(): Base(0,1)
    {}

    /**
     * \brief Copy constructor.
     */
    NumaVector(NumaVector const& a) = default;

    /**
     * \brief Move constructor.
     */
    NumaVector (NumaVector&& a) = default;

    /**
     * \brief Creates an r vector with given entries.
     */
    NumaVector (size_type r, value_type v = value_type()) :
      Base(r,1,v)
    {}

    /**
     * \brief Copy assignment.
     */
    NumaVector& operator=(NumaVector const& a) = default;

    /**
     * \brief Resizes the vector to r entries, leaving the entries in an undefined state.
     *
     * On resize, all iterators are invalidated. This method adheres to the Dune::BlockVector interface.
     */
    void resize (size_type r) { Base::resize(r,1); }

    /// @}

    /**
     * \name Entry access
     * @{
     */
    /**
     * \brief iterator to first entry.
     */
    iterator begin() { return iterator(*this,0); }

    /**
     * \brief iterator to one beyond last entry.
     */
    iterator end() { return iterator(*this,this->N()); }

    /**
     * \brief const iterator to first row.
     */
    const_iterator begin() const { return const_iterator(*this,0); }

    /**
     * \brief const iterator to one beyond last entry.
     */
    const_iterator end() const { return const_iterator(*this,this->N()); }

    /**
     * \brief The subscript operator.
     */
    Entry&  operator[] (size_type row) { return *iterator(*this,row); }

    /**
     * \brief The const subscript operator.
     */
    Entry const& operator[] (size_type row) const { return *const_iterator(*this,row); }

    /// @}

    /**
     * \name Linear algebra opterations.
     * The computations are performed in parallel on the NUMA nodes.
     * @{
     */

    /**
     * \brief \f$ y \leftarrow a y \f$
     */
    Self& operator*= (field_type const& a)
    {
      this->for_each([&](Entry& e) { e *= a; });
      return *this;
    }

    /**
     * \brief \f$ y \leftarrow \frac{1}{a} y \f$
     */
    Self& operator/= (field_type const& a)
    {
      return (*this) *= (1/a);
    }

    /**
     * \brief \f$ y \leftarrow y+x \f$
     *
     * Both vectors have to have the same size.
     */
    Self& operator+= (Self const& x)
    {
      this->for_each2([](Entry& yi, Entry const& xi) { yi += xi; },x);
      return *this;
    }

    /**
     * \brief \f$ y \leftarrow y - x \f$
     *
     * Both vectors have to have the same size.
     */
    Self& operator-= (Self const& x)
    {
      this->for_each2([](Entry& yi, Entry const& xi) { yi -= xi; },x);
      return *this;
    }

    /**
     * \brief \f$ y \leftarrow ax + y \f$
     */
    Self& axpy(field_type const& a, Self const& x)
    {
      this->for_each2([&](Entry& yi, Entry const& xi) { NumaDenseMatrixDetail::axpy(yi,a,xi); },x);
      return *this;
    }

    /// @}

    /**
     * \name Norms and scalar product.
     *
     * The computations are performed in parallel on the NUMA nodes.
     * @{
     */

    /**
     * \brief euclidean norm: sqrt(sum over squared euclidean norms of entries)
     */
    real_type two_norm () const { return std::sqrt(two_norm2()); }

    /**
     * \brief euclidean norm squared: sum over squared euclidean norms of entries
     */
    real_type two_norm2 () const
    {
      return this->accumulate([](real_type s, Entry const& e) { return s+NumaDenseMatrixDetail::frobenius_norm2(e); }, real_type(0));
    }

    /**
     * \brief one-norm: sum over absolute values of entries
     */
    real_type one_norm() const
    {
      return this->accumulate([](real_type s, Entry const& e) { return s+NumaDenseMatrixDetail::one_norm(e); }, real_type(0));
    }

    /**
     * \brief
     */
    field_type dot(Self const& other) const { return Base::dot(other); }

    /// @}
  };

  /**
   * \relates NumaVector
   */
  template <class Entry>
  std::ostream& operator<<(std::ostream& out, NumaVector<Entry> const& vec)
  {
    using namespace std;
    copy(begin(vec),end(vec),ostream_iterator<Entry>(out," "));
    return out;
  }
}

#endif


#pragma once

#include <cmr/config.h>
#include <cmr/export.h>

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

#include "matrix_transposed.hpp"
#include <iomanip>

namespace tu
{
  namespace detail
  {

    template <typename Iterator>
    class Range
    {
    public:
      Range(Iterator begin, Iterator end)
        : _begin(begin), _end(end)
      {

      }

      Range(const Range<Iterator>& other)
        : _begin(other._begin), _end(other._end)
      {

      }

      Iterator begin()
      {
        return _begin;
      }

      Iterator end()
      {
        return _end;
      }

    private:
      Iterator _begin;
      Iterator _end;
    };

  } /* namespace tu::detail */

  /**
   * \brief Base class for matrices whose entries have type \p V.
   */

  template <typename V>
  class Matrix
  {
  public:
    typedef V Value; /// Type of entries
    typedef std::size_t Index; /// Row/column index type

    struct Nonzero
    {
      Index row;
      Index column;
      const Value& value;

      Nonzero(Index r, Index c, const Value& v)
        : row(r), column(c), value(v)
      {

      }
    };
  };

  /**
   * \brief Dense matrix with entries of type \p V.
   */

  template <typename V>
  class DenseMatrix : public Matrix<V>
  {
  public:
    typedef typename Matrix<V>::Value Value;
    typedef typename Matrix<V>::Index Index;
    typedef typename Matrix<V>::Nonzero Nonzero;

    template <bool Row>
    class NonzeroIterator
    {
    public:
      NonzeroIterator(const DenseMatrix<V>& matrix, Index major)
        : _matrix(matrix), _major(major), _minor(0)
      {
        if (Row)
        {
          while (_minor < _matrix.numColumns() && _matrix.get(_major, _minor) == 0)
            ++_minor;
        }
        else
        {
          while (_minor < _matrix.numRows() && _matrix.get(_minor, _major) == 0)
            ++_minor;
        }
      }

      NonzeroIterator(const DenseMatrix<V>& matrix, Index major, int dummy)
        : _matrix(matrix), _major(major)
      {
        if (Row)
          _minor = _matrix.numColumns();
        else
          _minor = _matrix.numRows();
      }

      NonzeroIterator<Row>& operator++()
      {
        ++_minor;
        if (Row)
        {
          while (_minor < _matrix.numColumns() && _matrix.get(_major, _minor) == 0)
            ++_minor;
        }
        else
        {
          while (_minor < _matrix.numRows() && _matrix.get(_minor, _major) == 0)
            ++_minor;
        }
      }

      Nonzero operator*() const
      {
        if (Row)
        {
          assert(_major < _matrix.numRows());
          assert(_minor < _matrix.numColumns());
          return Nonzero(_major, _minor, _matrix.get(_major, _minor));
        }
        else
        {
          assert(_minor < _matrix.numRows());
          assert(_major < _matrix.numColumns());
          return Nonzero(_minor, _major, _matrix.get(_minor, _major));
        }
      }

      bool operator!=(const NonzeroIterator<Row>& other) const
      {
        assert(&_matrix == &other._matrix);
        assert(_major == other._major);
        return _minor != other._minor;
      }

    private:
      const DenseMatrix<V>& _matrix;
      std::size_t _major, _minor;
    };

    typedef detail::Range<NonzeroIterator<true>> NonzeroRowRange;
    typedef detail::Range<NonzeroIterator<false>> NonzeroColumnRange;

    /**
     * \brief Default constructor for 0x0 matrix.
     */

    DenseMatrix()
      : _data(nullptr), _numRows(0), _numColumns(0)
    {

    }

    /**
     * \brief Move constructor that steals the data from \p other.
     */

    DenseMatrix(DenseMatrix<V>&& other)
      : _data(other._data), _numRows(other._numRows), _numColumns(other._numColumns)
    {
      other._data = nullptr;
      other._numRows = 0;
      other._numColumns = 0;
    }

    /**
     * \brief Copy constructor.
     */

    template <typename V2>
    DenseMatrix(const DenseMatrix<V2>& other)
      : _numRows(other._numRows), _numColumns(other._numColumns)
    {
      std::size_t size = other._numRows * other._numColumns;
      _data = new Value[size];
      for (std::size_t i = 0; i < size; ++i)
        _data[i] = other._data[i];
    }

    /**
     * \brief Move assignment operator that steals the data from \p other.
     */

    DenseMatrix<V>& operator=(DenseMatrix<V>&& other)
    {
      _data = other._data;
      _numRows = other._numRows;
      _numColumns = other._numColumns;
      other._data = nullptr;
      other._numRows = 0;
      other._numColumns = 0;
      return *this;
    }

    /**
     * \brief Assignment operator.
     */

    template <typename V2>
    DenseMatrix<V>& operator=(const DenseMatrix<V2>& other)
    {
      _numRows = other._numRows;
      _numColumns = other._numColumns;
      std::size_t size = other._numRows * other._numColumns;
      _data = new Value[size];
      for (std::size_t i = 0; i < size; ++i)
        _data[i] = other._data[i];
      return *this;
    }

    /**
     * \brief Destructor.
     */

    ~DenseMatrix()
    {
      if (_data != nullptr)
        delete[] _data;
    }

    /**
     * \brief Constructs zero matrix of given size.
     */

    DenseMatrix(Index numRows, Index numColumns)
      : _numRows(numRows), _numColumns(numColumns)
    {
      std::size_t size = numRows * numColumns;
      _data = new Value[size];
      for (std::size_t i = 0; i < size; ++i)
        _data[i] = 0;
    }

    /**
     * \brief Returns the number of rows.
     */

    Index numRows() const
    {
      return _numRows;
    }

    /**
     * \brief Returns the number of columns.
     */

    Index numColumns() const
    {
      return _numColumns;
    }

    /**
     * \brief Returns entry at \p row, \p column.
     */

    const Value& get(Index row, Index column) const
    {
      assert(row < _numRows);
      assert(column < _numColumns);
      return _data[_numColumns * row + column];
    }

    /**
     * \brief Sets entry at \p row, \p column to copy of \p value.
     */

    template <typename V2>
    void set(Index row, Index column, const V2& value)
    {
      assert(row < _numRows);
      assert(column < _numColumns);
      _data[_numColumns * row + column] = value;
    }

    /**
     * \brief Returns the number of nonzeros in \p row.
     */

    std::size_t countRowNonzeros(Index row) const
    {
      std::size_t begin = _numColumns * row;
      std::size_t end = _numColumns * (row + 1);
      std::size_t count = 0;
      for (std::size_t i = begin; i < end; ++i)
      {
        if (_data[i] != 0)
          ++count;
      }
      return count;
    }

    /**
     * \brief Returns the number of nonzeros in \p column.
     */

    std::size_t countColumnNonzeros(Index column) const
    {
      std::size_t begin = column;
      std::size_t end = _numRows * _numColumns;
      std::size_t count = 0;
      for (std::size_t i = begin; i < end; i += _numColumns)
      {
        if (_data[i] != 0)
          ++count;
      }
      return count;
    }

    /**
     * \brief Returns a range for iterating over the nonzeros of \p row.
     */

    NonzeroRowRange iterateRowNonzeros(Index row) const
    {
      return NonzeroRowRange(NonzeroIterator<true>(*this, row),
        NonzeroIterator<true>(*this, row, 0));
    }

    /**
     * \brief Returns a range for iterating over the nonzeros of \p column.
     */

    NonzeroColumnRange iterateColumnNonzeros(Index column) const
    {
      return NonzeroColumnRange(NonzeroIterator<false>(*this, column),
        NonzeroIterator<false>(*this, column, 0));
    }

    /**
     * \brief Checks for consistency, raising a \ref std::runtime_error if inconsistent.
     */

    void ensureConsistency() const
    {

    }

  protected:
    Value* _data; /// Matrix entries with row-major ordering.
    Index _numRows; /// Number of rows.
    Index _numColumns; /// Number of columns.
  };

  /**
   * \brief Sparse matrix with entries of type \p V.
   */

  template<typename V>
  class SparseMatrix : public Matrix<V>
  {
  public:
    typedef typename Matrix<V>::Value Value;
    typedef typename Matrix<V>::Index Index;
    typedef typename Matrix<V>::Nonzero Nonzero;

    /**
     * \brief Matrix data, row-wise or column-wise.
     *
     * Matrix data, row-wise or column-wise. The term major refers to the rows if it is a row-wise
     * view and to columns otherwise. The term minor refers to the opposite.
     */

    struct Data
    {
      /// Array that maps a major to its nonzero range specified by first and beyond entries.
      std::vector<std::pair<Index, Index>> range;
      /// Array that maps the nonzero indices to pairs of minor index and value.
      std::vector<std::pair<Index, Value>> entries;
      /// Whether the data is sorted by minor.
      bool isSorted;

      /**
       * \brief Default constructor.
       */

      Data()
        : isSorted(true)
      {

      }

      /**
       * \brief Move constructor.
       */

      Data(Data&& other)
        : range(std::move(other.range)), entries(std::move(other.entries)), isSorted(other.isSorted)
      {

      }

      /**
       * \brief Copy constructor.
       */

      Data(const Data& other)
        : range(other.range), entries(other.entries), isSorted(other.isSorted)
      {

      }

      /**
       * \brief Move operator.
       */

      Data& operator=(Data&& other)
      {
        range = std::move(other.range);
        entries = std::move(other.entries);
        isSorted = other.isSorted;
        return *this;
      }

      /**
       * \brief Assignment operator.
       */

      Data& operator=(const Data& other)
      {
        range = other.range;
        entries = other.entries;
        isSorted = other.isSorted;
        return *this;
      }

      /**
       * \brief Destructor.
       */

      ~Data() = default;

      /**
       * \brief Computes the transpose version of \p data. \p numMinor is the new number of minor
       * indices.
       */

      void constructFromTransposed(const Data& transposedData, Index numMinor)
      {
        // Iterate over entries and count how many in each major.
        range.resize(numMinor);
        for (auto& r : range)
          r.second = 0;

        for (Index i = 0; i < Index(transposedData.range.size()); ++i)
        {
          for (Index j = transposedData.range[i].first; j < transposedData.range[i].second; ++j)
          {
            ++range[transposedData.entries[j].first].second;
          }
        }

        // Initialize start of each major. This will be incremented when copying.
        range[0].first = 0;
        for (Index i = 1; i < Index(range.size()); ++i)
        {
          range[i].first = range[i-1].first + range[i-1].second;
          range[i-1].second = range[i].first;
        }
        range.back().second = transposedData.entries.size();

        // Copy entries major-wise into slots indicated by first[i], incrementing the latter.
        entries.resize(transposedData.entries.size());
        for (Index transposedMajor = 0; transposedMajor < Index(transposedData.range.size());
          ++transposedMajor)
        {
          for (Index j = transposedData.range[transposedMajor].first;
            j < transposedData.range[transposedMajor].second; ++j)
          {
            auto transposedEntry = transposedData.entries[j];
            entries[range[transposedEntry.first].first] = std::make_pair(transposedMajor,
              transposedEntry.second);
            ++range[transposedEntry.first].first;
          }
        }

        // Re-initialize start of each major.
        range[0].first = 0;
        for (Index i = 1; i < Index(range.size()); ++i)
          range[i].first = range[i-1].second;

        isSorted = true;
      }

      /**
       * \brief Checks for consistency, raising a \ref std::runtime_error if inconsistent.
       */

      void ensureConsistency(Index numMinor = std::numeric_limits<Index>::max()) const
      {
        for (Index i = 0; i < Index(range.size()); ++i)
        {
          if (range[i].first > entries.size())
            throw std::runtime_error(
                "Inconsistent SparseMatrix::Data: range.first contains index out of range.");
          if (range[i].second > entries.size())
            throw std::runtime_error(
                "Inconsistent SparseMatrix::Data: range.second contains index out of range.");
          if (range[i].first > range[i].second)
            throw std::runtime_error(
                "Inconsistent SparseMatrix::Data: range.first > range.beyond.");
        }
        for (Index i = 0; i < Index(entries.size()); ++i)
        {
          if (entries[i].first >= numMinor)
            throw std::runtime_error("Inconsistent SparseMatrix::Data: minor entry exceeds bound.");
          if (entries[i].second == 0)
            throw std::runtime_error("Inconsistent SparseMatrix::Data: zero entry found.");
        }
        for (Index i = 0; i < Index(range.size()); ++i)
        {
          for (Index j = range[i].first + 1; j < range[i].second; ++j)
          {
            if (entries[j-1].first == entries[j].first)
            {
              std::stringstream ss;
              ss << "Inconsistent SparseMatrix::Data: minor entries of major " << i
                << " contain duplicate indices " << (j-1) << "->" << entries[j-1].first << " and "
                << j << "->" << entries[j].first << ".";
              throw std::runtime_error(ss.str());
            }
            else if (entries[j-1].first > entries[j].first && isSorted)
            {
              std::stringstream ss;
              ss << "Inconsistent SparseMatrix::Data: minor entries of major " << i
                << " are not sorted for indices " << (j-1) << "->" << entries[j-1].first << " and "
                << j << "->" << entries[j].first << ".";
              throw std::runtime_error(ss.str());
            }
          }
        }
      }

      /**
       * \brief Swaps data with \p other.
       */

      void swap(Data& other)
      {
        range.swap(other.range);
        entries.swap(other.entries);
        std::swap(isSorted, other.isSorted);
      }

      /**
       * \brief Finds entry \p major,\p minor.
       *
       * Find entry \p major,\p minor. If it exists, the index of the entry is returned, and
       * \c std::numeric_limits<std::size_t>::max() otherwise.
       */

      std::size_t find(Index major, Index minor) const
      {
        if (isSorted)
        {
          // Binary search on interval [lower, upper)

          std::size_t lower = range[major].first;
          std::size_t upper = range[major].second;
          std::size_t mid;
          while (lower < upper)
          {
            mid = (lower + upper) / 2;
            if (entries[mid].first < minor)
              lower = mid + 1;
            else if (entries[mid].first > minor)
              upper = mid;
            else
              return mid;
          }
        }
        else
        {
          // Linear scan.

          for (std::size_t i = range[major].first; i < range[major].second; ++i)
          {
            if (entries[i].first == minor)
              return i;
          }
        }

        return std::numeric_limits<std::size_t>::max();
      }

      /**
       * \brief Sort data by minor.
       */

      void sort()
      {
        if (!isSorted)
          return;

        for (std::size_t i = 0; i < entries.size(); ++i)
        {
          auto compare = [] (const std::pair<Index, Value>& a,
            const std::pair<Index, Value>& b)
            {
              return a.first < b.first;
            };

          std::sort(entries.begin() + range[i].first, entries.begin() + range[i].second, compare);
        }

        isSorted = true;
      }
    };

    /**
     * \brief Iterator for row- or column-wise iteration over the entries.
     */

    template <bool Row>
    struct NonzeroIterator
    {
    public:
      NonzeroIterator(const SparseMatrix<V>& matrix, Index major)
        : _data(Row ? matrix._rowData : matrix._columnData), _major(major),
        _index(_data.range[major].first)
      {

      }

      NonzeroIterator(const SparseMatrix<V>& matrix, Index major, int dummy)
        : _data(Row ? matrix._rowData : matrix._columnData), _major(major),
        _index(_data.range[major].second)
      {

      }

      /**
       * \brief Copy constructor.
       */

      NonzeroIterator(const NonzeroIterator& other)
        : _data(other._data), _major(other._major), _index(other._index)
      {

      }

      /**
       * \brief Returns the current entry.
       */

      inline const Nonzero operator*() const
      {
        // Statically known, so one branch will be optimized away.
        if (Row)
          return Nonzero(_major, _data.entries[_index].first, _data.entries[_index].second);
        else
          return Nonzero(_data.entries[_index].first, _major, _data.entries[_index].second);
      }

      /**
       * \brief Advances to the next entry in the same major.
       */

      inline void operator++()
      {
        ++_index;
        assert(_index <= _data.range[_major].second);
      }

      /**
       * \brief Compares two iterators for being not equal.
       */

      inline bool operator!=(const NonzeroIterator<Row>& other) const
      {
        assert(&_data == &other._data);
        return _major != other._major || _index != other._index;
      }

      private:
        /// Reference to the matrix data.
        const Data& _data;
        /// Major index.
        Index _major;
        /// Index of the entry relative to this major.
        Value _index;
    };

    typedef detail::Range<NonzeroIterator<true>> NonzeroRowRange;
    typedef detail::Range<NonzeroIterator<false>> NonzeroColumnRange;

    /**
     * \brief Constructs a 0x0 matrix.
     */

    SparseMatrix()
      : _zero(0)
    {

    }

    /**
     * \brief Move constructor.
     */

    SparseMatrix(SparseMatrix&& other)
      : _rowData(std::move(other._rowData)), _columnData(std::move(other._columnData)), _zero(0)
    {

    }

    /**
     * \brief Move assignment operator.
     */

    SparseMatrix& operator=(SparseMatrix&& other)
    {
      _rowData = std::move(other._rowData);
      _columnData = std::move(other._columnData);
      return *this;
    }

    /**
     * \brief Constructs a matrix.
     *
     * Constructs a matrix. Given entries may contain zeros, but they are filtered.
     *
     * \param majorRow Indicates if data is row-wise.
     * \param numMajor Number of rows (resp. columns) of the matrix.
     * \param numMinor Number of columns (resp. rows) of the matrix.
     * \param numEntries Number of provided entries.
     * \param first Array of size \p numMajor with first index of each row (resp. column).
     * \param beyond Array of size \p numMajor with beyond index of each row (resp. column).
     *   May be \c NULL in which case the first-entry of the next major is considered, i.e.,
     *   the rows (resp. columns) need to be given in an ordered way.
     * \param entryMinors Array of size \p numEntries with column (resp. row) of each entry.
     * \param entryValues Array of size \p numEntries with value of each entry.
     * \param mayContainZeros Whether we have to check for zero entries.
     * \param isSorted Whether the entries of each row (resp. column) are already sorted.
     */

    SparseMatrix(bool majorRow, Index numMajor, Index numMinor, Index numEntries, Index* first,
      Index* beyond, Index* entryMinors, Value* entryValues, bool mayContainZeros = true,
      bool isSorted = false)
      : _zero(0)
    {
      set(majorRow, numMajor, numMinor, numEntries, first, beyond, entryMinors, entryValues,
        mayContainZeros, isSorted);
    }

    /**
     * \brief Constructs a matrix.
     *
     * Constructs a matrix from a prepared data structure.
     *
     * \param majorRow Indicates if data is row-wise.
     * \param majorIsSorted Whether the data of each major is sorted by minor.
     * \param data Matrix data.
     */

    SparseMatrix(bool majorRow, const Data& data, Index numMinor, bool majorIsSorted = false)
      : _zero(0)
    {
      set(majorRow, data, numMinor, majorIsSorted);
    }

    /**
     * \brief Constructs a matrix.
     *
     * Constructs a matrix from a prepared data structure.
     *
     * \param majorRow Indicates if data is row-wise.
     * \param data Matrix data.
     */

    SparseMatrix(bool majorRow, Data&& data, Index numMinor)
      : _zero(0)
    {
      set(majorRow, data, numMinor);
    }

    /**
     * \brief Constructs a matrix.
     *
     * Constructs a matrix from two prepared data structures.
     *
     * \param rowData Matrix row data.
     * \param columnData Matrix column data.
     */

    SparseMatrix(const Data& rowData, const Data& columnData)
      : _zero(0)
    {
      set(rowData, columnData);
    }

    /**
     * \brief Constructs a matrix.
     *
     * Constructs a matrix from a two prepared data structures.
     *
     * \param rowData Matrix row data.
     * \param columnData Matrix column data.
     */

    SparseMatrix(Data&& rowData, Data&& columnData)
      : _zero(0)
    {
      set(std::move(rowData), std::move(columnData));
    }

    /**
     * \brief Sets the contents of the matrix.
     *
     * Sets the contents of the matrix. Given entries may contain zeros, but they are filtered.
     *
     * \param majorRow Indicates if data is row-wise.
     * \param numMajor Number of rows (resp. columns) of the matrix.
     * \param numMinor Number of columns (resp. rows) of the matrix.
     * \param numEntries Number of provided entries.
     * \param first Array of size \p numMajor with first index of each row (resp. column).
     * \param beyond Array of size \p numMajor with beyond index of each row (resp. column).
     *   May be \c nullptr in which case the first-entry of the next major is considered, i.e.,
     *   the rows (resp. columns) need to be ordered.
     * \param entryMinors Array of size \p numEntries with column (resp. row) of each entry.
     * \param entryValues Array of size \p numEntries with value of each entry.
     * \param mayContainZeros Whether zero entries shall be skipped.
     * \param isSorted Whether the entries of each major are sorted by minor.
     */

    void set(bool majorRow, Index numMajor, Index numMinor, Index numEntries, const Index* first,
      const Index* beyond, const Index* entryMinors, const Value* entryValues,
      bool mayContainZeros = true, bool isSorted = false)
    {
      if (mayContainZeros)
      {
        // Use first run over entries to count nonzeros in each row (resp. column).
        _rowData.range.resize(numMajor);
        for (auto& range : _rowData.range)
          range.first = 0;
        Index current = 0;
        for (Index i = 0; i < numMajor; ++i)
        {
          _rowData.range[i].first = current;
          Index b;
          if (beyond != nullptr)
            b = beyond[i];
          else if (i + 1 < numMajor)
            b = first[i+1];
          else
            b = numEntries;
          for (Index j = first[i]; j < b; ++j)
          {
            if (entryValues[j] != 0)
              ++current;
          }
          _rowData.range[i].second = current;
        }

        // Use second run to copy the nonzeros.
        _rowData.entries.resize(current);
        current = 0;
        for (Index i = 0; i < numMajor; ++i)
        {
          Index b;
          if (beyond != nullptr)
            b = beyond[i];
          else if (i + 1 < numMajor)
            b = first[i+1];
          else
            b = numEntries;
          for (Index j = first[i]; j < b; ++j)
          {
            if (entryValues[j] != 0)
            {
              _rowData.entries[current] = std::make_pair(entryMinors[j], entryValues[j]);
              ++current;
            }
          }
        }
      }
      else
      {
        // If we can trust the input then we can copy directly.
        _rowData.range.resize(numMajor);
        if (beyond != nullptr)
        {
          for (Index i = 0; i < numMajor; ++i)
            _rowData.range[i] = std::make_pair(first[i], beyond[i]);
        }
        else
        {
          for (Index i = 0; i + 1 < numMajor; ++i)
            _rowData.range[i] = std::make_pair(first[i], first[i+1]);
          _rowData.range.back() = std::make_pair(first[numMajor - 1], Index(numEntries));
        }
        _rowData.entries.resize(numEntries);
        for (Index i = 0; i < numEntries; ++i)
          _rowData.entries[i] = std::make_pair(entryMinors[i], entryValues[i]);
      }

      _rowData.isSorted = isSorted;

      // Construct column-wise (resp. row-wise) representation.
      _columnData.constructFromTransposed(_rowData, numMinor);

      if (!majorRow)
        _rowData.swap(_columnData);

  #if !defined(NDEBUG)
      ensureConsistency();
  #endif /* !NDEBUG */
    }

    /**
     * \brief Sets the contents of the matrix.
     *
     * Sets the contents of the matrix from a prepared data structure.
     *
     * \param majorRow Indicates if data is row-wise.
     * \param data Matrix data.
     * \param numMinor Number of columns (resp. rows).
     */

    void set(bool majorRow, const Data& data, Index numMinor)
    {
      // We can trust the input and thus we can copy directly.
      _rowData.range = data.range;
      _rowData.entries = data.entries;
      _rowData.isSorted = data.isSorted;

      // Construct column-wise (resp. row-wise) representation.
      _columnData.constructFromTransposed(_rowData, numMinor);

      if (!majorRow)
        _rowData.swap(_columnData);

  #if !defined(NDEBUG)
      ensureConsistency();
  #endif /* !NDEBUG */
    }

    /**
     * \brief Sets the contents of the matrix.
     *
     * Sets the contents of the matrix from a prepared data structure.
     *
     * \param majorRow Indicates if data is row-wise.
     * \param data Matrix data.
     * \param numMinor Number of columns (resp. rows).
     */

    void set(bool majorRow, Data&& data, Index numMinor)
    {
      _rowData = std::move(data);
      _columnData.constructFromTransposed(_rowData, numMinor);
      if (!majorRow)
        _rowData.swap(_columnData);

#if !defined(NDEBUG)
      ensureConsistency();
#endif /* !NDEBUG */
    }

    /**
     * \brief Sets the contents of the matrix.
     *
     * Sets the contents of the matrix from a prepared data structure.
     *
     * \param rowData Matrix row data.
     * \param columnData Matrix column data.
     */

    void set(const Data& rowData, const Data& columnData)
    {
      _rowData.range = rowData.range;
      _rowData.entries = rowData.entries;
      _rowData.isSorted = rowData.isSorted;
      _rowData.ensureConsistency();

      _columnData.range = columnData.range;
      _columnData.entries = columnData.entries;
      _columnData.isSorted = columnData.isSorted;
      _columnData.ensureConsistency();

#if !defined(NDEBUG)
      ensureConsistency();
#endif /* !NDEBUG */
    }

    /**
     * \brief Sets the contents of the matrix.
     *
     * Sets the contents of the matrix from a prepared data structure.
     *
     * \param rowData Matrix row data.
     * \param columnData Matrix column data.
     */

    void set(Data&& rowData, Data&& columnData)
    {
      _rowData = std::move(rowData);
      _columnData = std::move(columnData);
#if !defined(NDEBUG)
      ensureConsistency();
#endif /* !NDEBUG */
    }

    /**
     * \brief Destructor.
     */

    ~SparseMatrix() = default;

    /**
     * \brief Returns the number of rows.
     */

    std::size_t numRows() const
    {
      return _rowData.range.size();
    }

    /**
     * \brief Returns the number of columns.
     */

    std::size_t numColumns() const
    {
      return _columnData.range.size();
    }

    /**
     * \brief Indicates if the row data is sorted by column.
     */

    bool hasSortedRows() const
    {
      return _rowData.isSorted;
    }

    /**
     * \brief Indicates if the column data is sorted by row.
     */

    bool hasSortedColumns() const
    {
      return _columnData.isSorted;
    }

    /**
     * \brief Returns entry at \p row, \p column.
     *
     * Returns entry at \p row, \p column. If the row or column data is sorted (resp. not sorted)
     * then the time is logarithmic (resp. linear) in the number of nonzeros of the row or column.
     */

    const Value& get(Index row, Index column) const
    {
      assert(row < numRows());
      assert(column < numColumns());

      if (_rowData.isSorted)
      {
        std::size_t columnIndex = _rowData.find(row, column);
        if (columnIndex < std::numeric_limits<std::size_t>::max())
          return _rowData.entries[columnIndex].second;
      }
      else
      {
        std::size_t rowIndex = _columnData.find(column, row);
        if (rowIndex < std::numeric_limits<std::size_t>::max())
          return _columnData.entries[rowIndex].second;
      }

      return _zero;
    }

    /**
     * \brief Returns the number of nonzeros.
     */

    std::size_t numNonzeros() const
    {
      return _rowData.entries.size();
    }

    /**
     * \brief Returns the number of nonzeros in \p row.
     */

    std::size_t countRowNonzeros(Index row) const
    {
      return _rowData.range[row].second - _rowData.range[row].first;
    }

    /**
     * \brief Returns the number of nonzeros in \p column.
     */

    std::size_t countColumnNonzeros(Index column) const
    {
      return _columnData.range[column].second - _columnData.range[column].first;
    }

    /**
     * \brief Returns a range for iterating over the nonzeros of \p row.
     */

    NonzeroRowRange iterateRowNonzeros(Index row) const
    {
      return NonzeroRowRange(NonzeroIterator<true>(*this, row),
        NonzeroIterator<true>(*this, row, 0));
    }

    /**
     * \brief Returns a range for iterating over the nonzeros of \p column.
     */

    NonzeroColumnRange iterateColumnNonzeros(Index column) const
    {
      return NonzeroColumnRange(NonzeroIterator<false>(*this, column),
        NonzeroIterator<false>(*this, column, 0));
    }

    /**
     * \brief Returns a copy of the matrix' transpose.
     */

    SparseMatrix transposed() const
    {
      return SparseMatrix(_columnData, _rowData);
    }

    /**
     * \brief Checks for consistency, raising a \ref std::runtime_error if inconsistent.
     *
     * Checks for consistency of row- and column-data, which includes that all entries must be
     * nonzeros and that entries are sorted and free of duplicates (if stated so).
     */

    void ensureConsistency() const
    {
      // First ensure individual consistency of row and column data.
      _rowData.ensureConsistency();
      _columnData.ensureConsistency();

      // Copy of a nonzero.
      struct NZ
      {
        Index row;
        Index column;
        Value value;
      };

      // Comparison for entries.
      auto compare = [](const NZ& a, const NZ& b)
      {
        if (a.row != b.row)
          return a.row < b.row;
        if (a.column != b.column)
          return a.column < b.column;
        return a.value < b.value;
      };

      // Construct sorted vector of entries from row data.
      std::vector<NZ> rowNonzeros;
      for (Index i = 0; i < Index(_rowData.range.size()); ++i)
      {
        for (Index j = _rowData.range[i].first; j < _rowData.range[i].second; ++j)
        {
          NZ nz = { i, _rowData.entries[j].first, _rowData.entries[j].second };
          rowNonzeros.push_back(nz);
        }
      }
      std::sort(rowNonzeros.begin(), rowNonzeros.end(), compare);

      // Construct sorted vector of entries from column data.
      std::vector<NZ> columnNonzeros;
      for (Index i = 0; i < Index(_columnData.range.size()); ++i)
      {
        for (Index j = _columnData.range[i].first; j < _columnData.range[i].second; ++j)
        {
          NZ nz = { _columnData.entries[j].first, i, _columnData.entries[j].second };
          columnNonzeros.push_back(nz);
        }
      }
      std::sort(columnNonzeros.begin(), columnNonzeros.end(), compare);

      for (std::size_t i = 0; i < rowNonzeros.size(); ++i)
      {
        if (rowNonzeros[i].row != columnNonzeros[i].row
          || rowNonzeros[i].column != columnNonzeros[i].column
          || rowNonzeros[i].value != columnNonzeros[i].value)
        {
          throw std::runtime_error("Inconsistent Matrix: row and column data differ.");
        }
      }
    }

    /**
     * \brief Transposes the matrix.
     *
     * Transposes the matrix in constant time.
     */

    void transpose()
    {
      _rowData.swap(_columnData);
    }

    /**
     * \brief Sort row data (by column).
     */

    void sortRows()
    {
      _rowData.sort();
    }

    /**
     * \brief Sort column data (by row).
     */

    void sortColumns()
    {
      _columnData.sort();
    }

    /**
     * \brief Sort row and column data (by column and row, respectively).
     */

    void sort()
    {
      sortRows();
      sortColumns();
    }

  private:
    /// Data for row-wise access.
    Data _rowData;
    /// Data for column-wise access.
    Data _columnData;
    /// Zero value.
    Value _zero;
    /// Whether the row and column data is sorted (by columns and rows, respectively).
    bool _isSorted;
  };

  /**
   * \brief Matrix proxy for the transpose of a matrix.
   *
   * Matrix proxy for the transpose of a matrix. It references another matrix and does not maintain
   * matrix data by itself.
   */

  template <typename M>
  class TransposedMatrix : public Matrix<typename M::Value>
  {
  public:
    typedef typename M::Value Value;
    typedef typename M::Index Index;
    typedef typename M::Nonzero Nonzero;

    template <bool Row>
    class NonzeroIterator
    {
    public:
      NonzeroIterator(const TransposedMatrix<M>& matrix, Index major)
        : _matrix(matrix._matrix), _iterator(typename M::template NonzeroIterator<!Row>(matrix._matrix, major))
      {

      }

      NonzeroIterator(const TransposedMatrix<M>& matrix, Index major, int dummy)
        : _matrix(matrix._matrix), _iterator(typename M::template NonzeroIterator<!Row>(matrix._matrix, major, 0))
      {

      }

      NonzeroIterator<Row>& operator++()
      {
        ++_iterator;
      }

      Nonzero operator*() const
      {
        Nonzero nz = *_iterator;
        std::swap(nz.row, nz.column);
        return nz;
      }

      bool operator!=(const NonzeroIterator<Row>& other) const
      {
        assert(&_matrix == &other._matrix);
        return _iterator != other._iterator;
      }

    private:
      const M& _matrix;
      typename M::template NonzeroIterator<!Row> _iterator;
    };

    typedef detail::Range<NonzeroIterator<true>> NonzeroRowRange;
    typedef detail::Range<NonzeroIterator<false>> NonzeroColumnRange;

    /**
     * \brief Constructs from a matrix of type \p M.
     */

    TransposedMatrix(M& matrix)
      : _matrix(matrix)
    {

    }

    /**
     * \brief Move constructor.
     */

    TransposedMatrix(TransposedMatrix&& other)
      : _matrix(other._matrix)
    {

    }

    /**
     * \brief Returns the number of rows.
     */

    std::size_t numRows() const
    {
      return _matrix.numColumns();
    }

    /**
     * \brief Returns the number of columns.
     */

    std::size_t numColumns() const
    {
      return _matrix.numRows();
    }

    /**
     * \brief Indicates if the row data is sorted by column.
     */

    bool hasSortedRows() const
    {
      return _matrix.hasSortedColumns();
    }

    /**
     * \brief Indicates if the column data is sorted by row.
     */

    bool hasSortedColumns() const
    {
      return _matrix.hasSortedRows();
    }

    /**
     * \brief Returns entry at \p row, \p column.
     *
     * Returns entry at \p row, \p column. If the row or column data is sorted (resp. not sorted)
     * then the time is logarithmic (resp. linear) in the number of nonzeros of the row or column.
     */

    const Value& get(Index row, Index column) const
    {
      assert(row < numRows());
      assert(column < numColumns());

      return _matrix.get(column, row);
    }

    /**
     * \brief Returns the number of nonzeros.
     */

    std::size_t numNonzeros() const
    {
      return _matrix.numNonzeros();
    }

    /**
     * \brief Returns the number of nonzeros in \p row.
     */

    std::size_t countRowNonzeros(Index row) const
    {
      return _matrix.countColumnNonzeros(row);
    }

    /**
     * \brief Returns the number of nonzeros in \p column.
     */

    std::size_t countColumnNonzeros(Index column) const
    {
      return _matrix.countRowNonzeros(column);
    }

    /**
     * \brief Returns a range for iterating over the nonzeros of \p row.
     */

    NonzeroRowRange iterateRowNonzeros(Index row) const
    {
      return NonzeroRowRange(NonzeroIterator<true>(_matrix, row),
        NonzeroIterator<true>(_matrix, row, 0));
    }

    /**
     * \brief Returns a range for iterating over the nonzeros of \p column.
     */

    NonzeroColumnRange iterateColumnNonzeros(Index column) const
    {
      return NonzeroColumnRange(NonzeroIterator<false>(_matrix, column),
        NonzeroIterator<false>(_matrix, column, 0));
    }

    /**
     * \brief Checks for consistency, raising a \ref std::runtime_error if inconsistent.
     */

    void ensureConsistency() const
    {
      _matrix.ensureConsistency();
    }

  protected:
    M& _matrix;
  };

  /**
   * \brief Returns a \ref TransposedMatrix for \p matrix.
   */

  template <typename M>
  TransposedMatrix<M> transposedMatrix(M& matrix)
  {
    return TransposedMatrix<M>(matrix);
  }

  /**
   * \brief Indexes a submatrix.
   */

  class SubmatrixIndices
  {
  public:
    /**
     * \brief Copy constructor.
     */

    CMR_EXPORT
    SubmatrixIndices(const SubmatrixIndices& other);

    /**
     * \brief Move constructor.
     */

    CMR_EXPORT
    SubmatrixIndices(SubmatrixIndices&& other);

    /**
     * \brief Constructor from arrays \p rows and \p columns.
     */

    CMR_EXPORT
    SubmatrixIndices(const std::vector<std::size_t>& rows, const std::vector<std::size_t>& columns);

    /**
     * \brief Constructor for a 1x1 submatrix.
     *
     * Constructor for a 1x1 submatrix.
     */

    CMR_EXPORT
    SubmatrixIndices(std::size_t row, std::size_t column);

    /**
     * \brief Returns the number of row indices.
     */

    inline
    std::size_t numRows() const
    {
      return _rows.size();
    }

    /**
     * \brief Returns the number of column indices.
     */

    inline
    std::size_t numColumns() const
    {
      return _columns.size();
    }

    /**
     * \brief Returns the vector of row indices.
     */

    inline
    const std::vector<std::size_t>& rows() const
    {
      return _rows;
    }

    /**
     * \brief Returns the vector of column indices.
     */

    inline
    const std::vector<std::size_t>& columns() const
    {
      return _columns;
    }

  private:
    /// Array of row indices.
    std::vector<std::size_t> _rows;

    /// Array of column indices.
    std::vector<std::size_t> _columns;
  };

  template <typename M>
  class Submatrix : public Matrix<typename M::Value>
  {
  public:
    typedef typename M::Index Index;
    typedef typename M::Value Value;
    typedef typename M::Nonzero Nonzero;

    /**
     * \brief Constructs submatrix of \p matrix indexed by \p indices.
     */

    Submatrix(const M& matrix, const SubmatrixIndices& indices)
      : _matrix(matrix), _indices(indices)
    {

    }

    /**
     * \brief Move constructor.
     */

    Submatrix(Submatrix && other)
      : _matrix(other._matrix), _indices(other._indices)
    {

    }

    /**
     * \brief Returns the number of rows.
     */

    std::size_t numRows() const
    {
      return _indices.numRows();
    }

    /**
     * \brief Returns the number of columns.
     */

    std::size_t numColumns() const
    {
      return _indices.numColumns();
    }

    /**
     * \brief Indicates if the row data is sorted by column.
     */

    bool hasSortedRows() const
    {
      return false;
    }

    /**
     * \brief Indicates if the column data is sorted by row.
     */

    bool hasSortedColumns() const
    {
      return false;
    }

    /**
     * \brief Returns entry at \p row, \p column.
     *
     * Returns entry at \p row, \p column. If the row or column data is sorted (resp. not sorted)
     * then the time is logarithmic (resp. linear) in the number of nonzeros of the row or column.
     */

    const Value& get(Index row, Index column) const
    {
      assert(row < numRows());
      assert(column < numColumns());

      return _matrix.get(_indices.rows()[row], _indices.columns()[column]);
    }

    /**
     * \brief Checks for consistency, raising a \ref std::runtime_error if inconsistent.
     */

    void ensureConsistency() const
    {
      for (auto row : _indices.rows())
      {
        if (row >= _matrix.numRows())
          throw std::runtime_error("Inconsistent Submatrix: row index too large.");
      }
      for (auto column : _indices.columns())
      {
        if (column >= _matrix.numColumns())
          throw std::runtime_error("Inconsistent Submatrix: column index too large.");
      }
      _matrix.ensureConsistency();
    }

  private:
    const M& _matrix;
    const SubmatrixIndices& _indices;
  };

  template <typename Matrix>
  bool find_smallest_nonzero_matrix_entry(const Matrix& matrix, size_t row_first, size_t row_beyond, size_t column_first, size_t column_beyond,
      size_t& row, size_t& column)
  {
    bool result = false;
    int current_value = 0;
    for (size_t r = row_first; r != row_beyond; ++r)
    {
      for (size_t c = column_first; c != column_beyond; ++c)
      {
        int value = matrix(r, c);
        if (value == 0)
          continue;

        value = value >= 0 ? value : -value;

        if (!result || value < current_value)
        {
          result = true;
          row = r;
          column = c;
          current_value = value;
        }
      }
    }
    return result;
  }

  template <typename Matrix>
  bool matrix_row_zero(const Matrix& matrix, size_t row, size_t column_first, size_t column_beyond)
  {
    for (size_t c = column_first; c != column_beyond; ++c)
      if (matrix(row, c) != 0)
        return false;
    return true;
  }

  template <typename Matrix>
  bool matrix_column_zero(const Matrix& matrix, size_t column, size_t row_first, size_t row_beyond)
  {
    return matrix_row_zero(make_transposed_matrix(matrix), column, row_first, row_beyond);
  }

  template <typename Matrix1, typename Matrix2>
  bool equals(const Matrix1& first, const Matrix2& second)
  {
    if (first.size1() != second.size1())
      return false;
    if (first.size2() != second.size2())
      return false;
    for (size_t r = 0; r < first.size1(); ++r)
    {
      for (size_t c = 0; c < first.size2(); ++c)
      {
        if (first(r, c) != second(r, c))
          return false;
      }
    }
    return true;
  }

  /**
   * Free function to permute two rows of a permuted matrix.
   *
   * @param matrix The permuted matrix
   * @param index1 First index
   * @param index2 Second index
   */

  template <typename MatrixType>
  inline void matrix_permute1(MatrixType& matrix, size_t index1, size_t index2)
  {
    for (size_t index = 0; index < matrix.size2(); ++index)
    {
      std::swap(matrix(index1, index), matrix(index2, index));
    }
  }

  /**
   * Free function to permute two columns of a permuted matrix.
   *
   * @param matrix The permuted matrix
   * @param index1 First index
   * @param index2 Second index
   */

  template <typename MatrixType>
  inline void matrix_permute2(MatrixType& matrix, size_t index1, size_t index2)
  {
    for (size_t index = 0; index < matrix.size1(); ++index)
    {
      std::swap(matrix(index, index1), matrix(index, index2));
    }
  }

} /* namespace tu */


/**
 *  @file    mdm_AIF.cxx
 *  @brief   Implementation of mdm_AIF class
 */

#ifndef MDM_API_EXPORTS
#define MDM_API_EXPORTS
#endif // !MDM_API_EXPORTS

#include "mdm_AIF.h"

#include <cstdio>
#include <cassert>
#include <iostream>
#include <cmath>
#include <istream>

#include <madym/mdm_exception.h>
#include <madym/mdm_ProgramLogger.h>
#include <boost/format.hpp>

//
MDM_API mdm_AIF::mdm_AIF()
	:
  AIFtype_(mdm_AIF::AIF_TYPE::AIF_UNDEFINED),
  PIFtype_(mdm_AIF::PIF_TYPE::PIF_UNDEFINED),
  AIFTimes_(0),
	base_AIF_(0),
	resampled_AIF_(0),
  base_PIF_(0),
  resampled_PIF_(0),
	Hct_(0.42),
	prebolus_(8),
	dose_(0.1)
{

}

//
MDM_API mdm_AIF::~mdm_AIF()
{

}

//
MDM_API void mdm_AIF::readAIF(const std::string &full_AIF_filename, const size_t nDynamics)
{
  try {
    readIFFromFile(base_AIF_, full_AIF_filename, nDynamics);
  }
	catch (mdm_exception &e)
  {
    e.append("Unable to read AIF");
    throw;
  }
  setAIFType(AIF_FILE);
}

//
MDM_API void mdm_AIF::readPIF(const std::string &full_PIF_filename, const size_t nDynamics)
{
  try {
    readIFFromFile(base_PIF_, full_PIF_filename, nDynamics);
  }
  catch (mdm_exception &e)
  {
    e.append("Unable to read PIF");
    throw;
  }
  setPIFType(PIF_FILE);

}

//
MDM_API void mdm_AIF::writeAIF(const std::string &filename)
{
	if (base_AIF_.empty())
		base_AIF_ = resampled_AIF_;

  try { writeIFToFile(base_AIF_, filename); }
  catch (mdm_exception &e)
  {
    e.append("Unable to write AIF");
    throw;
  }
}

//
MDM_API void mdm_AIF::writePIF(const std::string &filename)
{
	if (base_PIF_.empty())
		base_PIF_ = resampled_PIF_;

  try { writeIFToFile(base_PIF_, filename); }
  catch (mdm_exception &e)
  {
    e.append("Unable to write PIF");
    throw;
  }
}

//
MDM_API void mdm_AIF::setBaseAIF(const std::vector<double> &aifVals)
{
  if (aifVals.size() != AIFTimes_.size())
    throw mdm_exception(__func__, boost::format(
      "Size of input AIF values (%1%) does not match number of times (%2%)")
      % aifVals.size() % AIFTimes_.size());

  base_AIF_ = aifVals;
}

//
MDM_API const std::vector<double>& mdm_AIF::AIF() const
{
	return resampled_AIF_;
}

//
MDM_API const std::vector<double>& mdm_AIF::PIF() const
{
	return resampled_PIF_;
}

//
MDM_API void mdm_AIF::resample_AIF(double tOffset)
{
	//Important this is only called after AIFTimes has been set
	const auto nTimes = AIFTimes_.size();

	switch (AIFtype_)
	{
	case AIF_STD:
		aifWeinman(nTimes, tOffset);
		break;
	case AIF_FILE:
  case AIF_MAP:
		aifFromBase(nTimes, tOffset);
		break;
	case AIF_POP:
		aifPopGJMP(nTimes, tOffset);
		break;

	case AIF_UNDEFINED:
	default:
		// Add warning, quit here
    throw "Tried to resample undefined AIF";
		break;
	}
}

//
MDM_API void mdm_AIF::resample_PIF(double tOffset, bool offsetAIF/* = true*/, bool resampleIRF/* = true*/)
{
	//Important this is only called after AIFTimes has been set
	const auto nTimes = AIFTimes_.size();

	switch (PIFtype_)
	{

	case PIF_FILE:
		pifFromBase(nTimes, tOffset);
		break;

	case PIF_POP:
		aifPopHepaticAB(nTimes, tOffset, offsetAIF, resampleIRF);
		break;
	case PIF_UNDEFINED:
	default:
		// Add warning, quit here
    throw "Tried to resample undefined AIF";
		break;
	}
}

//
MDM_API bool mdm_AIF::setAIFType(AIF_TYPE value)
{
  bool setOK;

  switch (value)
  {
    case AIF_STD:
    case AIF_FILE:
    case AIF_POP:
    case AIF_MAP:
      AIFtype_ = value;
      setOK = true;
      break;
    default:
      AIFtype_ = AIF_UNDEFINED;
      setOK = false;
      throw "AIF type not recognised";
      break;
  }

  return setOK;
}

//
MDM_API bool mdm_AIF::setPIFType(PIF_TYPE value)
{
	bool setOK;

	switch (value)
	{
	case PIF_FILE:
	case PIF_POP:
		PIFtype_ = value;
		setOK = true;
		break;
	default:
		PIFtype_ = PIF_UNDEFINED;
		setOK = false;
    throw "AIF type not recognised";
		break;
	}

	return setOK;
}

//
MDM_API mdm_AIF::AIF_TYPE mdm_AIF::AIFType() const
{
  return AIFtype_;
}

//
MDM_API mdm_AIF::PIF_TYPE mdm_AIF::PIFType() const
{
	return PIFtype_;
}

//
MDM_API const std::vector<double>& mdm_AIF::AIFTimes() const
{
	return AIFTimes_;
}

//
MDM_API double mdm_AIF::AIFTime(size_t i) const
{
  try { return AIFTimes_[i]; }
  catch (std::out_of_range &e)
  {
    mdm_exception em(__func__, e.what());
    em.append(boost::format(
      "Attempting to access timepoint %1% when there are only %2% times")
      % i % AIFTimes_.size());
    throw em;
  }
}

//
MDM_API void mdm_AIF::setAIFTimes(const std::vector<double> times)
{
	const auto nTimes = times.size();
	AIFTimes_.resize(nTimes);
	for (size_t i = 0; i < nTimes; i++)
		AIFTimes_[i] = times[i] - times[0];
}

//
MDM_API void  mdm_AIF::setPrebolus(size_t p)
{
	prebolus_ = p;
}

//
MDM_API void  mdm_AIF::setHct(double h)
{
	Hct_ = h;
}

//
MDM_API void  mdm_AIF::setDose(double d)
{
	dose_ = d;
}

//
MDM_API size_t mdm_AIF::prebolus() const
{
	return prebolus_;
}

//
MDM_API double mdm_AIF::Hct() const
{
	return Hct_;
}

//
MDM_API double mdm_AIF::dose() const
{
	return dose_;
}

//**************************************************************************
// Private functions
//**************************************************************************

//
void mdm_AIF::aifPopGJMP(size_t nTimes, double tOffset)
{
  double gaussian1, gaussian2, sigmoid;
  std::vector<double> offsetTimes(nTimes);

  // These parameters are from Parker et al MRM 56:993(2006)
  // For gaussian1
  const double kA1     = 5.7326;
  const double kMu1    = 0.17046;
  const double kSigma1 = 0.0563;
  // For gaussian2
  const double kA2     = 0.9974;
  const double kMu2    = 0.365;
  const double kSigma2 = 0.132;
  // For sigmoid
  const double kAlpha  = 1.050;
  const double kBeta   = 0.1685;
  const double kS      = 38.078;
  const double kTau    = 0.483;

  // Get AIF timing data
  for (size_t i = 0; i < nTimes; i++)
    offsetTimes[i] = (double) (AIFTimes_[i] - AIFTimes_[0] + tOffset);

  // TODO Trying out Anita's pb - 1 instead of 2 in GJMP AIF
	resampled_AIF_.resize(nTimes);
  for (int i = 0; i < nTimes; i++)
  {
    gaussian1 = kA1 * exp(-1.0 * ((double) AIFTimes_[i] - kMu1 - offsetTimes[(int) prebolus_ - 1])
                               * ((double) AIFTimes_[i] - kMu1 - offsetTimes[(int) prebolus_ - 1])
                               / (2.0 * kSigma1 * kSigma1));
    gaussian2 = kA2 * exp(-1.0 * ((double) AIFTimes_[i] - kMu2 - offsetTimes[(int) prebolus_ - 1])
                               * ((double) AIFTimes_[i] - kMu2 - offsetTimes[(int) prebolus_ - 1])
                               / (2.0 * kSigma2 * kSigma2));
    sigmoid   = kAlpha * exp(-kBeta * ((double) AIFTimes_[i] - offsetTimes[(int) prebolus_ - 1]))
                       / (1 + exp(-kS * ((double) AIFTimes_[i] - kTau - offsetTimes[(int) prebolus_ - 1])));
    resampled_AIF_[i] = (double) (((dose_ / 0.1) * (gaussian1 + gaussian2 + sigmoid))
    		                    / (1.0 - Hct_));
  }
}

void mdm_AIF::aifPopHepaticAB(size_t nTimes, double tOffset, bool offsetAIF, bool resampleIRF)
{
	//If we've got an offset, make sure AIF has been resampled
	if (offsetAIF || resampled_AIF_.size() != nTimes)
		resample_AIF( tOffset);

	//generate a population IRF according to Anita's model
	if (resampleIRF || PIF_IRF_.size() != nTimes || std::isnan(PIF_IRF_[0]))
	{
		PIF_IRF_.resize(nTimes);
		double irf_sum = 0.0;
		for (size_t i_t = 0; i_t < nTimes; i_t++)
		{
			const double &t = AIFTimes_[i_t] - tOffset;
			if (t < 0.08)
        PIF_IRF_[i_t] = 0;//This might have been set to NaN, so make sure we set back to zero
			else if (t < 0.17)
				PIF_IRF_[i_t] = 24.16*t - 2.01;
			else
				PIF_IRF_[i_t] = 2.83*exp(-10.80*t) + 2.12*exp(-1.82*t);

			irf_sum += PIF_IRF_[i_t];
		}
		for (int i_t = 0; i_t < nTimes; i_t++)
			PIF_IRF_[i_t] /= irf_sum;
	}

	//Convolve the AIF with the IRF to generate the PIF
	resampled_PIF_.resize(nTimes);

	//literal convolution operation
	for (int i_t = 0; i_t < nTimes; i_t++)
	{
		double pif_sum = 0.0;
		for (int j_t = 0, k_t = i_t; j_t <= i_t; j_t++, k_t--)
		{
			pif_sum += resampled_AIF_[j_t] * PIF_IRF_[k_t];
		}
		resampled_PIF_[i_t] = pif_sum;
	}
}

//
void mdm_AIF::aifWeinman(size_t nTimes, double tOffset)
{
  std::vector<double> AIF(nTimes), offsetTimes(nTimes);
  double delta_t, remainder;

  /* From original paper TODO get Weinman ref */
  const double kAlpha1 = 3.99;
  const double kBeta1  = 0.144;
  const double kAlpha2 = 4.78;
  const double kBeta2  = 0.0111;

  /* Get AIF timing data */
  for (size_t i = 0; i < nTimes; i++)
    offsetTimes[i] = (double) (AIFTimes_[i] - AIFTimes_[0] + tOffset);

  AIF[0] = 0.0;
  for (size_t i = 1; i < nTimes; i++)
  {
    if (i < prebolus_)
      AIF[i] = 0.0;
    else
      AIF[i] = dose_ * (kAlpha1 * exp(-kBeta1 * (AIFTimes_[i - 1]))
                             + kAlpha2 * exp(-kBeta2 * (AIFTimes_[i - 1])));
  }

	//Linear resample AIF to shifted time points
  resampled_AIF_[0] = 0.0;
  for (size_t i = 1; i < nTimes; i++)
  {
    if (AIFTimes_[i] <= offsetTimes[0])
      resampled_AIF_[i] = 0.0;
    for (size_t j = 1; j < nTimes; j++)
      // find where in the AIF time series the current tissue time point falls
      if (AIFTimes_[i] > offsetTimes[j - 1] && AIFTimes_[i] <= offsetTimes[j])
      {
        delta_t = offsetTimes[j] - offsetTimes[j - 1];
        remainder = AIFTimes_[i] - offsetTimes[j - 1];
        resampled_AIF_[i] = remainder / delta_t * AIF[j]
                           + (1.0 - remainder / delta_t) * AIF[j - 1];
      }
  }
}

//
void mdm_AIF::aifFromBase(size_t nTimes, double tOffset)
{
	resampleBase(resampled_AIF_, base_AIF_,
		nTimes, tOffset);
}

//
//Load an hepatic portal vein from file
void mdm_AIF::pifFromBase(size_t nTimes, double tOffset)
{
	resampleBase(resampled_PIF_, base_PIF_,
		nTimes, tOffset);
}

//Load input funtion previously loaded from file
void mdm_AIF::resampleBase(std::vector<double> &resampled_if, const std::vector<double> &loaded_if,
  size_t nTimes, double tOffset)
{
	std::vector<double> offsetTimes(nTimes);
	double delta_t, remainder;

	// Get AIF timing data
	for (size_t i = 0; i < nTimes; i++)
		offsetTimes[i] = AIFTimes_[i] + tOffset;

	// Linear resample AIF to shifted time points
	resampled_if.resize(nTimes);
	for (size_t i = 1; i < nTimes; i++)
	{
		for (size_t j = 1; j < nTimes; j++)
		{
			// find where in the AIF time series the current tissue time point falls
			if (AIFTimes_[i] > offsetTimes[j - 1] && AIFTimes_[i] <= offsetTimes[j])
			{
				delta_t = offsetTimes[j] - offsetTimes[j - 1];
				remainder = AIFTimes_[i] - offsetTimes[j - 1];
				resampled_if[i] = ((remainder / delta_t) * loaded_if[j]
					+ (1.0 - remainder / delta_t) * loaded_if[j - 1])
					/ (1.0 - Hct_);

				break;
			}
		}
	}
}

//Load an AIF from file
void mdm_AIF::readIFFromFile(std::vector<double> &loaded_if,
  const std::string &filename, const size_t nDynamics)
{
	std::vector<double>  timesFromFile(nDynamics, 0);
	loaded_if.resize(nDynamics);

	std::ifstream aifStream(filename, std::ios::in);

	if (!aifStream.is_open())
    throw mdm_exception(__func__,
      boost::format( "IF file %2% not found") % filename);

	//Load times and values from file - MB added check we don't reach EOF
	//If we do this suggests the AIF file does not have sufficient time points -
	//most likely we've loaded a file with data organised in the wrong format
	for (size_t i = 0; i < nDynamics; i++)
	{
		if (aifStream.eof())
		{
      aifStream.close();

      throw mdm_exception(__func__, boost::format(
        "IF does not contain sufficient time points. EOF reached after %1% points. "
        "Expected %2% ") %  i % nDynamics);
		};
		aifStream >> timesFromFile[i] >> loaded_if[i];
	}
	aifStream.close();

	//Check if we have existing times, if not, use the times we've just read from the file
	if (AIFTimes_.size() != nDynamics)
		AIFTimes_ = timesFromFile;

	mdm_ProgramLogger::logProgramMessage(
		"IF successfully read from " + filename);

}

//
void mdm_AIF::writeIFToFile(const std::vector<double> &if_to_save, const std::string &filename)
{
  std::ofstream aifStream(filename, std::ios::out);

  if (if_to_save.size() != AIFTimes_.size())
    throw mdm_exception(__func__, boost::format(
      "Size of IF values (%1%) does not match number of times (%2%)")
      % if_to_save.size() % AIFTimes_.size());

  if (!aifStream.is_open())
    throw mdm_exception(__func__,
      boost::format("Unable to open IF file %1% for writing") % filename);

  //
  for (size_t i = 0; i < if_to_save.size(); i++)
    aifStream << AIFTimes_[i] << " " << if_to_save[i] << std::endl;

	mdm_ProgramLogger::logProgramMessage(
		"IF successfully written to " + filename);

  return;
}

//