#ifndef INCLUDED_STDDEFX
#include "stddefx.h"
#define INCLUDED_STDDEFX
#endif

#ifndef INCLUDED_CALC
#include "calc.h"
#define INCLUDED_CALC
#endif

// Library headers.
#include <utility>
#ifndef INCLUDED_BOOST_NONCOPYABLE
#include <boost/noncopyable.hpp>
#define INCLUDED_BOOST_NONCOPYABLE
#endif
#ifndef INCLUDED_BOOST_MATH_SPECIAL_FUNCTIONS_ROUND
#include <boost/math/special_functions/round.hpp>
#define INCLUDED_BOOST_MATH_SPECIAL_FUNCTIONS_ROUND
#endif
#ifndef INCLUDED_BOOST_MATH_TR1
#include <boost/math/tr1.hpp>
#define INCLUDED_BOOST_MATH_TR1
#endif

// PCRaster library headers.

#ifndef INCLUDED_MISC
#include "misc.h"
#define INCLUDED_MISC
#endif

#ifndef INCLUDED_COM_MATH
#include "com_math.h"
#define INCLUDED_COM_MATH
#endif
#ifndef INCLUDED_COM_INTERVALTYPES
#include "com_intervaltypes.h"
#define INCLUDED_COM_INTERVALTYPES
#endif

#ifndef INCLUDED_GEO_CELLLOCVISITOR
#include "geo_celllocvisitor.h"
#define INCLUDED_GEO_CELLLOCVISITOR
#endif

#ifndef INCLUDED_GEO_SCANCONVERSION
#include "geo_scanconversion.h"
#define INCLUDED_GEO_SCANCONVERSION
#endif

#ifndef INCLUDED_FIELDAPI_INTERFACE
#include "fieldapi_interface.h"
#define INCLUDED_FIELDAPI_INTERFACE
#endif

#ifndef INCLUDED_FIELDAPI_SCALARDOMAINCHECK
#include "fieldapi_scalardomaincheck.h"
#define INCLUDED_FIELDAPI_SCALARDOMAINCHECK
#endif

// Module headers.

/*!
  \file
  This file contains the implementation of the ExtentOfView class.
*/

template<class T>
class RememberPoints: public boost::noncopyable
{

private:

  const fieldapi::ReadOnly<INT4>& d_classes;
  INT4 d_class;
  std::vector<std::pair<T, T> > d_points;

public:

  RememberPoints(const fieldapi::ReadOnly<INT4>& classes)
    : d_classes(classes)
  {
  }

  RememberPoints(const fieldapi::ReadOnly<INT4>& classes, INT4 currentClass)
    : d_classes(classes), d_class(currentClass)
  {
  }

  void setClass(INT4 currentClass) {
    d_class = currentClass;
  }

  bool operator()(T x, T y) {
    static INT4 value;
    if(d_classes.get(value, y, x) && value == d_class) {
      d_points.push_back(std::make_pair(y, x));
      return true;
    }

    return false;
  }

  std::pair<T, T>& operator[](size_t i) {
    return d_points[i];
  }

  void clear() {
    d_points.clear();
  }

  size_t size() {
    return d_points.size();
  }

  // Distance in cells.
  double distance() {
    double distance = 0.0;

    if(size()) {
      const std::pair<T, T>& begin = d_points.front();
      const std::pair<T, T>& end = d_points.back();
      double dx = static_cast<double>(end.first - begin.first);
      double dy = static_cast<double>(end.second - begin.second);
      // Add one for current cell.
      distance = boost::math::tr1::hypot(dx,dy) + 1.0;
    }

    return distance;
  }

};

extern "C" int ExtentOfView(
  MAP_REAL8 *m_result,                  // scalar, average extent of view
  const MAP_INT4* m_classes,            // nominal, classes
  const MAP_REAL8* m_nrDirections)      // scalar, number of directions
{
  ReadWriteReal8_ref(result, m_result);

  std::vector<const fieldapi::Common*> inputs;
  ReadOnlyInt4_ref(classes, m_classes);
  inputs.push_back(&classes);
  ReadOnlyReal8_ref(nrDirectionsInterface, m_nrDirections);
  inputs.push_back(&nrDirectionsInterface);

  PRECOND(classes.spatial());
  PRECOND(!nrDirectionsInterface.spatial());
  size_t nrDirections = static_cast<size_t>(nrDirectionsInterface.value(0,0));

  std::vector<fieldapi::ScalarDomainCheck> nsDomains;
  nsDomains.push_back(fieldapi::ScalarDomainCheck(nrDirectionsInterface,
        "Number of directions", com::GreaterThan<double>(0)));
  int nsCheck = fieldapi::checkScalarDomains(nsDomains,geo::CellLoc(0,0));
  if(nsCheck != -1) {
    return RetError(1, nsDomains[nsCheck].msg().c_str());
  }

  size_t nrRows = classes.nrRows();
  size_t nrCols = classes.nrCols();

  // Result is missing value if any of the inputs is.
  for(geo::CellLocVisitor visitor(classes); visitor.valid(); ++visitor) {

    if(fieldapi::nonMV(inputs, *visitor)) {
       result.put(0.0, *visitor);
    }
    else {
      result.putMV(*visitor);
    }
  }

  // Determine all direction angles.
  // Determine max distance in cells in the raster.
  double maxExtent = MAX(nrRows,nrCols);
  int offsetX = com::ceil<int, double>(boost::math::tr1::hypot(maxExtent,maxExtent));
  int offsetY;
  double subAngle = 360.0 / nrDirections;
  double angle;
  typedef std::pair<int, int> Offset;
  typedef std::vector<std::pair<double, Offset> > Offsets;
  Offsets offsets;

  for(size_t direction = 0; direction < nrDirections; ++direction) {

    angle = direction * subAngle;
    PRECOND(angle >= 0 && angle < 360);

    if(angle == 90.0) {
      offsetX = 0;
      offsetY = 100;
    }
    else if(angle == 270.0) {
      offsetX = 0;
      offsetY = -100;
    }
    else {
      if(angle < 90 || angle > 270) {
        offsetX = 100;
      }
      else {
        offsetX = -100;
      }

      angle *= M_PI / 180;
      offsetY = boost::math::iround(std::tan(angle) * offsetX);
    }
    offsets.push_back(std::make_pair(angle,
         std::make_pair(offsetX, offsetY)));
  }

  int x, y;
  double sum;
  RememberPoints<int> points(classes);

  // Loop over each cell.
  for(geo::CellLocVisitor visitor(classes); visitor.valid(); ++visitor) {
    x = (*visitor).col();
    y = (*visitor).row();
    sum = 0.0;

    // Determine class.
    if(result.isMV(*visitor)) {
      continue;
    }

    points.setClass(classes[*visitor]);

    // Loop over each direction.
    for(Offsets::const_iterator it = offsets.begin(); it != offsets.end();
         ++it) {

      points.clear();
      POSTCOND(!points.size());

      // Determine number of cells with same class.
      PRECOND(!classes.isMV(*visitor));
      geo::midpointLine(x, y, x + (*it).second.first, y + (*it).second.second,
         points);
      POSTCOND(points.size());

      sum += points.distance();
    }

    // Write sum to cell.
    // Check whether --unitcell or --unittrue is set.
    // unittrue: area is computed in true area represented by cells (default)
    // unitcell: area is computed in number of cells
    result.put(Side() * sum, *visitor);
  }

  return 0;
}


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

#include <nt2/include/functions/qr.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/include/functions/colon.hpp>
#include <nt2/include/functions/expand.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/geqp3.hpp>
#include <nt2/include/functions/geqrf.hpp>
#include <nt2/include/functions/gqr.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/mqr.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/triu.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/linalg/details/utility/lapack_verify.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <nt2/core/utility/assign_swap.hpp>
#include <nt2/core/container/colon/colon.hpp>
#include <nt2/linalg/options.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  //QR Scalar
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( qr_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0) const
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( qr_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&) const
    {
      return a0;
    }
  };

  //============================================================================
  //[Q,R] = QR(A)
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( qr_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::qr_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::value_type     child0;
    typedef typename child0::value_type                                      type_t;
    typedef typename meta::as_real<type_t>::type                            rtype_t;
    typedef typename meta::as_integer<rtype_t>::type                        itype_t;
    typedef nt2::memory::container<tag::table_, type_t, nt2::_2D>        o_semantic;
    typedef nt2::memory::container<tag::table_, nt2_la_int, nt2::_2D>    i_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      eval(a0, a1, N0(), N1());
    }

  private:
    /// INTERNAL ONLY - x = qr(a)
    /// return a matrix x such that triu(x) is the upper triangular factor r.
    /// this is the direct return of LAPACK ?geqp3, however tau is lost (and so q)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const& // 1 input
              , boost::mpl::long_<1> const& // 1 output
              ) const
    {
      eval1_1(a0, a1, nt2::policy<ext::raw_>());
    }

    /// INTERNAL ONLY - x = qr(a, econ_/0)
    /// return a matrix x such that triu(x) is the upper triangular factor r.
    /// this is the direct return of LAPACK ?geqp3, however tau is lost (and so q)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const& // 2 input
              , boost::mpl::long_<1> const& // 1 output
              ) const
    {
      eval2_1(a0, a1, boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    /// INTERNAL ONLY - [q, r] = qr(a)
    /// where a is m-by-n, produces an m-by-n upper triangular matrix r
    ///  and an m-by-m unitary matrix q so that a = q*r.
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&  // 1 input
              , boost::mpl::long_<2> const&  // 2 output
              ) const
    {
      eval2_2(a0, a1, nt2::policy<ext::matrix_>());
    }

    /// INTERNAL ONLY - [q, r, e] = qr(a)
    /// produces unitary q, upper triangular r and a
    /// permutation row vector e so that a(e, _)= q*r.
    /// the column permutation e is chosen so that abs(diag_of(r)) is decreasing.
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&  // 1 input
              , boost::mpl::long_<3> const&  // 3 output
              ) const
    {
      eval2_3(a0, a1, nt2::policy<ext::matrix_>());
    }

    /// INTERNAL ONLY - [x, y] = qr(a, raw_/matrix_/vector_/econ_/0 )
    /// with raw_
    ///  [x, tau] = qr(a, raw_)
    /// returns a matrix x such that triu(x) is the upper triangular factor r.
    /// The elements below the diagonal, together with the array tau, represent the
    /// unitary matrix Q as a product of min(M,N) elementary reflectors.
    /// tau contains The scalar factors of the elementary reflectors.
    /// This is the direct return of LAPACK ?geqp3
    ///
    /// with matrix_/vector is the same as  [q, r] = qr(a)
    /// [q, r] = qr(a, econ_) or  [q, r] = qr(a, 0)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      //1_2 means  1 input 2 outputs
      eval2_2(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    /// INTERNAL ONLY - [q, r, e] = qr(a, vector_/matrix_)
    /// with vector_
    /// produces unitary q, upper triangular r and a
    /// permutation row vector e so that a(e, _)= q*r.
    /// the column permutation e is chosen so that abs(diag_of(r)) is decreasing.
    /// with matrix_
    /// produces unitary q, upper triangular r and a
    /// permutation matrix e so that a*e = q*r.
    /// the column permutation e is chosen so that abs(diag_of(r)) is decreasing.
    /// Same as   [q, r, e] = qr(a)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const& // 2 input
              , boost::mpl::long_<3> const& // 2 output
              ) const
    {
      eval2_3(a0, a1, boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    ///////////////////////////////////////////////////////////////////////////////
    // evali_j bunch
    ///////////////////////////////////////////////////////////////////////////////

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 1i 1o: x  = qr(a)
    BOOST_FORCEINLINE
    void eval1_1 ( A0& a0, A1& a1
                   , const nt2::policy<ext::raw_>&
                   ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, r
                           , boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1));
      o_semantic tau(of_size(height(r), 1));
      NT2_LAPACK_VERIFY(nt2::geqrf( boost::proto::value(r)
                                  , tau));
      assign_swap(boost::proto::child_c<0>(a1), r);
    }

    /// INTERNAL ONLY: 2i 1o raw_: x  = qr(a, raw_)
    BOOST_FORCEINLINE
    void eval2_1 ( A0& a0, A1& a1
                   , const nt2::policy<ext::raw_>&
                   ) const
    {
      eval1_1(a0, a1, nt2::policy<ext::raw_>());
    }

    /// INTERNAL ONLY: 2i 1o upper_: x  = qr(a, upper_)
    BOOST_FORCEINLINE
    void eval2_1 ( A0& a0, A1& a1
                   , const nt2::policy<ext::upper_>&
                   ) const
    {
       nt2::container::table<type_t> work;
       NT2_AS_TERMINAL_INOUT(o_semantic
                            , r, boost::proto::child_c<0>(a0), work);
       o_semantic tau(of_size(height(r), 1));
       NT2_LAPACK_VERIFY(nt2::geqrf( boost::proto::value(r)
                                   ,tau));
       boost::proto::child_c<0>(a1) = triu(r);
    }

    /// INTERNAL ONLY: 2i 1o econ_: x  = qr(a, econ_)
    BOOST_FORCEINLINE
    void eval2_1 ( A0& a0, A1& a1
                   , const nt2::policy<ext::econ_>&
                   ) const
    {
      eval1_1(a0, a1, nt2::policy<ext::raw_>());
    }

    /// INTERNAL ONLY: 2i 1o 0: x  = qr(a, 0)
    BOOST_FORCEINLINE
    void eval2_1 ( A0& a0, A1& a1
                   , const int&
                   ) const
    {
      eval1_1(a0, a1, nt2::policy<ext::raw_>());
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 2o raw_: tie(x, tau) = qr(a, raw_)
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                   , const nt2::policy<ext::raw_>&
                   ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, x
                           , boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT(o_semantic, tau
                         , boost::proto::child_c<1>(a1));
      tau.resize(of_size(dim(x), 1));
      NT2_LAPACK_VERIFY(nt2::geqrf( boost::proto::value(x)
                                  , boost::proto::value(tau)));
      assign_swap(boost::proto::child_c<0>(a1), x);
      assign_swap(boost::proto::child_c<1>(a1), tau);
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 3o raw_: tie(x, tau, ip) = qr(a, nt2::raw_)
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                   , const nt2::policy<ext::raw_>&
                   ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(o_semantic, x
                           , boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT(o_semantic, tau
                         , boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT(i_semantic,  ip
                         , boost::proto::child_c<2>(a1));
      tau.resize(of_size(dim(x), 1));
      ip = nt2::zeros(nt2::width(x),1,nt2::meta::as_<nt2_la_int>());
      NT2_LAPACK_VERIFY(nt2::geqp3( boost::proto::value(x)
                                  , boost::proto::value(ip)
                                  , boost::proto::value(tau)));
      assign_swap(boost::proto::child_c<0>(a1), x);
      assign_swap(boost::proto::child_c<1>(a1), tau);
      assign_swap(boost::proto::child_c<2>(a1), ip);
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 2o matrix_/econ_: tie(q, r) = qr(a, matrix_) or
    /// tie(q, r) = qr(a, econ_), only extracting phases differ
    template < class T >
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , const T&
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(o_semantic
                           , r, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (o_semantic
                           , x, boost::proto::child_c<0>(a1));
      nt2::container::table<type_t>     tau;
      tie(x, tau) = qr(r, nt2::raw_);
      extract_qr(x, tau, r, T()); // extract raw or econ
      assign_swap(boost::proto::child_c<0>(a1), x);
      assign_swap(boost::proto::child_c<1>(a1), r);
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 2o vector_: tie(q, r) = qr(a, vector_)
    /// this is identical to  tie(q, r) = qr(a, matrix_)  ot  tie(q, r) = qr(a)
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                   , const nt2::policy<ext::vector_>&
                   ) const
    {
      eval2_2(a0, a1,  nt2::policy<ext::matrix_>()); //with 2 outputs vector_ is useless
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 2o vector_: tie(q, r) = qr(a, 0)
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                   , const int &
                   ) const
    {
      eval2_2(a0, a1, nt2::policy<ext::econ_>()); // 0 and econ_ are equivalent
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 3o vector_: tie(q, r, ip) = qr(a, econ_)
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                   , const nt2::policy<ext::econ_>&
                   ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(o_semantic, r
                           , boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (o_semantic, x
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (i_semantic, ip
                           , boost::proto::child_c<2>(a1));
      nt2::container::table<type_t> tau;
      tie(x, tau, ip) = qr(r, nt2::raw_);

      extract_qr(x, tau, r, nt2::policy<ext::econ_>());
      assign_swap(boost::proto::child_c<0>(a1), x);
      assign_swap(boost::proto::child_c<1>(a1), r);
      assign_swap(boost::proto::child_c<2>(a1), ip);
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 3o vector_: tie(q, r, ip) = qr(a, 0)
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                   , const int &
                   ) const
    {
      eval2_3(a0, a1,  nt2::policy<ext::econ_>()); // 0 and econ_ are equivalent
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 3o vector_: tie(q, r, ip) = qr(a, vector_)
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                   , const nt2::policy<ext::vector_>&
                   ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(o_semantic, r
                           , boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (o_semantic, x
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (i_semantic, ip
                           , boost::proto::child_c<2>(a1));
      nt2::container::table<type_t>     tau;
      tie(x, tau, ip) = qr(r, nt2::raw_);

      extract_qr(x, tau, r, nt2::policy<ext::raw_>());
      assign_swap(boost::proto::child_c<0>(a1), x);
      assign_swap(boost::proto::child_c<1>(a1), r);
      assign_swap(boost::proto::child_c<2>(a1), ip);
    }

     ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 3o matrix_: tie(q, r, p) = qr(a, matrix_)
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                   , const nt2::policy<ext::matrix_>&
                   ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(o_semantic, r
                           , boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (o_semantic, x
                           , boost::proto::child_c<0>(a1));
      nt2::container::table<type_t>     tau;
      nt2::container::table<nt2_la_int>  ip;
      tie(x, tau, ip) = qr(r, nt2::raw_);

      extract_qr(x, tau, r, nt2::policy<ext::raw_>());
      assign_swap(boost::proto::child_c<0>(a1), x);
      assign_swap(boost::proto::child_c<1>(a1), r);
      boost::proto::child_c<2>(a1) = eye(numel(ip), nt2::meta::as_<type_t>())(nt2::_, ip);
    }

    //////////////////////////////////////////////////////////////////////////////////////////
    /// extraction helpers
    //////////////////////////////////////////////////////////////////////////////////////////

    /// INTERNAL ONLY - Helper for Q/R extraction
    template<typename XQ, typename TAU,typename R, typename CHOICE >
    BOOST_FORCEINLINE
    void extract_qr(XQ& xq, TAU& tau, R& r, CHOICE const &) const
    {
      nt2_la_int  m  = nt2::height(xq);
      nt2_la_int  n  = nt2::width(xq);
      typedef typename XQ::value_type xqtype_t;
      r = nt2::triu(xq);

      if (m>n)
      {
        nt2::container::table<xqtype_t> complete_q = nt2::eye(m,m, nt2::meta::as_<xqtype_t>());
        nt2::mqr( boost::proto::value(xq)
                , boost::proto::value(tau)
                , boost::proto::value(complete_q) );
        xq = complete_q;
      }
      else
      {
        if(m < n)
        {
          /// TODO: Remove when aliasing works
          nt2::container::table<xqtype_t> local(xq);
          xq = nt2::expand(local,nt2::of_size(m,m));
        }
        nt2::gqr( boost::proto::value(xq)
                , boost::proto::value(tau));
      }
    }

    /// INTERNAL ONLY - Helper for Q/R extraction economy mode
    template<typename XQ, typename TAU,typename R >
    BOOST_FORCEINLINE
    void extract_qr(XQ& xq, TAU& tau, R& r, nt2::policy<ext::econ_> const &) const
    {
      nt2_la_int  m  = nt2::height(xq);
      nt2_la_int  n  = nt2::width(xq);
      typedef typename XQ::value_type xqtype_t;

      // economy mode
      r = nt2::triu(xq(_(1,nt2::min(n, m)), _) );

      if(m < n)
      {
        /// TODO: Remove when aliasing works
        nt2::container::table<xqtype_t> local(xq);
        xq = nt2::expand(local,nt2::of_size(m,m));
      }

      nt2::gqr( boost::proto::value(xq)
              , boost::proto::value(tau));
    }

    ////////////////////////////////////////////////////////////////////////////
    // some utilitaries
    ////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY - Size of L/U
    template<typename W>
    BOOST_FORCEINLINE std::size_t dim(W const& work) const
    {
      return std::min(nt2::height(work),nt2::width(work));
    }
  };
} }

#endif


#define OFDIS_INTERNAL
#include <Eigen/Core>
#include "litiv/3rdparty/ofdis/refine_variational.hpp"

inline void local_image_delete(image_t *src) {
    image_delete(src);
}

inline void local_image_delete(color_image_t *src) {
    color_image_delete(src);
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
ofdis::VarRefClass<eInput,eOutput>::VarRefClass(const float * im_ao_in, const float * im_ao_dx_in, const float * im_ao_dy_in,
                                                const float * im_bo_in, const float * im_bo_dx_in, const float * im_bo_dy_in,
                                                const camparam* cpt_in,const camparam* cpo_in,const optparam* op_in, float *flowout) :
        cpt(cpt_in), cpo(cpo_in), op(op_in) {
    // initialize parameters
    tvparams.alpha = op->tv_alpha;
    tvparams.beta = 0.0f;  // for matching term, not needed for us
    tvparams.gamma = op->tv_gamma;
    tvparams.delta = op->tv_delta;
    tvparams.n_inner_iteration = op->tv_innerit * (cpt->curr_lv+1);
    tvparams.n_solver_iteration = op->tv_solverit;//5;
    tvparams.sor_omega = op->tv_sor;
    tvparams.tmp_quarter_alpha = 0.25f*tvparams.alpha;
    tvparams.tmp_half_gamma_over3 = tvparams.gamma*0.5f/3.0f;
    tvparams.tmp_half_delta_over3 = tvparams.delta*0.5f/3.0f;
    tvparams.tmp_half_beta = tvparams.beta*0.5f;
    float deriv_filter[3] = {0.0f, -8.0f/12.0f, 1.0f/12.0f};
    deriv = convolution_new(2, deriv_filter, 0);
    float deriv_filter_flow[2] = {0.0f, -0.5f};
    deriv_flow = convolution_new(1, deriv_filter_flow, 0);
    // copy flow initialization into FV structs
    static int noparam = (eOutput==ofdis::FlowOutput_OpticalFlow)?2:1; // only horizontal displacements for stereo depth
    std::vector<image_t*> flow_sep(noparam);
    for(int i = 0; i < noparam; ++i )
        flow_sep[i] = image_new(cpt->width,cpt->height);
    for(int iy = 0; iy < cpt->height; ++iy) {
        for(int ix = 0; ix<cpt->width; ++ix) {
            int i = iy*cpt->width+ix;
            int is = iy*flow_sep[0]->stride+ix;
            for(int j = 0; j<noparam; ++j)
                flow_sep[j]->c1[is] = flowout[i*noparam+j];
        }
    }
    // copy image data into FV structs
    InputImageType* im_ao, *im_bo;
    im_ao = (InputImageType*)((eInput==ofdis::FlowInput_RGB)?(void*)color_image_new(cpt->width,cpt->height):(void*)image_new(cpt->width,cpt->height));
    im_bo = (InputImageType*)((eInput==ofdis::FlowInput_RGB)?(void*)color_image_new(cpt->width,cpt->height):(void*)image_new(cpt->width,cpt->height));
    copyimage(im_ao_in, im_ao);
    copyimage(im_bo_in, im_bo);
    // call solver
    if(eOutput==ofdis::FlowOutput_OpticalFlow)
        RefLevelOF(flow_sep[0], flow_sep[1], im_ao, im_bo);
    else
        RefLevelDE(flow_sep[0], im_ao, im_bo);
    // copy flow result back
    for(int iy = 0; iy < cpt->height; ++iy) {
        for(int ix = 0; ix<cpt->width; ++ix) {
            int i = iy*cpt->width+ix;
            int is = iy*flow_sep[0]->stride+ix;
            for(int j = 0; j<noparam; ++j)
                flowout[i*noparam+j] = flow_sep[j]->c1[is];
        }
    }
    // free FV structs
    for(int i = 0; i < noparam; ++i)
        image_delete(flow_sep[i]);
    convolution_delete(deriv);
    convolution_delete(deriv_flow);
    local_image_delete(im_ao);
    local_image_delete(im_bo);
}

inline void local_image_copy_pixel(image_t *img_t, int i, const float* img_st) {
    img_t->c1[i] = (*img_st);
}

inline void local_image_copy_pixel(color_image_t *img_t, int i, const float*& img_st) {
    img_t->c1[i] = (*img_st);
    ++img_st; img_t->c2[i] =  (*img_st);
    ++img_st; img_t->c3[i] =  (*img_st);
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::VarRefClass<eInput,eOutput>::copyimage(const float* img, InputImageType* img_t) {
    // remove image padding, start at first valid pixel
    const float* img_st = img+((eInput==ofdis::FlowInput_RGB)?3:1)*(cpt->tmp_w+1)*(cpt->imgpadding);
    for(int yi = 0; yi<cpt->height; ++yi) {
        for(int xi = 0; xi<cpt->width; ++xi,++img_st) {
            local_image_copy_pixel(img_t,yi*img_t->stride+xi,img_st);
        }
        img_st += ((eInput==ofdis::FlowInput_RGB)?3:1)*2*cpt->imgpadding;
    }
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::VarRefClass<eInput,eOutput>::RefLevelOF(image_t *wx, image_t *wy, const InputImageType* im1, const InputImageType* im2) {
    int i_inner_iteration;
    int width  = wx->width;
    int height = wx->height;
    int stride = wx->stride;
    image_t *du = image_new(width,height), *dv = image_new(width,height), // the flow increment
            *mask = image_new(width,height), // mask containing 0 if a point goes outside image boundary, 1 otherwise
            *smooth_horiz = image_new(width,height), *smooth_vert = image_new(width,height), // horiz: (i,j) contains the diffusivity coeff. from (i,j) to (i+1,j)
            *uu = image_new(width,height), *vv = image_new(width,height), // flow plus flow increment
            *a11 = image_new(width,height), *a12 = image_new(width,height), *a22 = image_new(width,height), // system matrix A of Ax=b for each pixel
            *b1 = image_new(width,height), *b2 = image_new(width,height); // system matrix b of Ax=b for each pixel
    const auto lImageCreator = [&](){return (InputImageType*)((eInput==ofdis::FlowInput_RGB)?(void*)color_image_new(width,height):(void*)image_new(width,height));};
    InputImageType *w_im2 = lImageCreator(), // warped second image
                   *Ix = lImageCreator(), *Iy = lImageCreator(), *Iz = lImageCreator(), // first order derivatives
                   *Ixx = lImageCreator(), *Ixy = lImageCreator(), *Iyy = lImageCreator(), *Ixz = lImageCreator(), *Iyz = lImageCreator(); // second order derivatives
    // warp second image
    fdf::image_warp<eInput>(w_im2, mask, im2, wx, wy);
    // compute derivatives
    fdf::get_derivatives<eInput>(im1, w_im2, deriv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz);
    // erase du and dv
    image_erase(du);
    image_erase(dv);
    // initialize uu and vv
    memcpy(uu->c1,wx->c1,wx->stride*wx->height*sizeof(float));
    memcpy(vv->c1,wy->c1,wy->stride*wy->height*sizeof(float));
    // inner fixed point iterations
    for(i_inner_iteration=0; i_inner_iteration<tvparams.n_inner_iteration; i_inner_iteration++) {
        //  compute robust function and system
        fdf::compute_smoothness(smooth_horiz, smooth_vert, uu, vv, deriv_flow, tvparams.tmp_quarter_alpha );
        //compute_data_and_match(a11, a12, a22, b1, b2, mask, wx, wy, du, dv, uu, vv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, desc_weight, desc_flow_x, desc_flow_y, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
        fdf::compute_data<eInput>(a11, a12, a22, b1, b2, mask, wx, wy, du, dv, uu, vv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
        fdf::sub_laplacian(b1, wx, smooth_horiz, smooth_vert);
        fdf::sub_laplacian(b2, wy, smooth_horiz, smooth_vert);
        // solve system
    #ifdef WITH_OPENMP
        sor_coupled_slow_but_readable(du, dv, a11, a12, a22, b1, b2, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega); // slower but parallelized
    #else
        sor_coupled(du, dv, a11, a12, a22, b1, b2, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega);
    #endif
        // update flow plus flow increment
        int i;
        v4sf *uup = (v4sf*) uu->c1, *vvp = (v4sf*) vv->c1, *wxp = (v4sf*) wx->c1, *wyp = (v4sf*) wy->c1, *dup = (v4sf*) du->c1, *dvp = (v4sf*) dv->c1;
        for(i=0 ; i<height*stride/4 ; i++) {
            (*uup) = (*wxp) + (*dup);
            (*vvp) = (*wyp) + (*dvp);
            uup+=1; vvp+=1; wxp+=1; wyp+=1;dup+=1;dvp+=1;
        }
    }
    // add flow increment to current flow
    memcpy(wx->c1,uu->c1,uu->stride*uu->height*sizeof(float));
    memcpy(wy->c1,vv->c1,vv->stride*vv->height*sizeof(float));
    // free memory
    image_delete(du);
    image_delete(dv);
    image_delete(mask);
    image_delete(smooth_horiz);
    image_delete(smooth_vert);
    image_delete(uu);
    image_delete(vv);
    image_delete(a11);
    image_delete(a12);
    image_delete(a22);
    image_delete(b1);
    image_delete(b2);
    local_image_delete(w_im2);
    local_image_delete(Ix);
    local_image_delete(Iy);
    local_image_delete(Iz);
    local_image_delete(Ixx);
    local_image_delete(Ixy);
    local_image_delete(Iyy);
    local_image_delete(Ixz);
    local_image_delete(Iyz);
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::VarRefClass<eInput,eOutput>::RefLevelDE(image_t *wx, const InputImageType* im1, const InputImageType* im2) {
    int i_inner_iteration;
    int width  = wx->width;
    int height = wx->height;
    int stride = wx->stride;
    image_t *du = image_new(width,height), *wy_dummy = image_new(width,height), // the flow increment
            *mask = image_new(width,height), // mask containing 0 if a point goes outside image boundary, 1 otherwise
            *smooth_horiz = image_new(width,height), *smooth_vert = image_new(width,height), // horiz: (i,j) contains the diffusivity coeff. from (i,j) to (i+1,j)
            *uu = image_new(width,height), // flow plus flow increment
            *a11 = image_new(width,height), // system matrix A of Ax=b for each pixel
            *b1 = image_new(width,height); // system matrix b of Ax=b for each pixel
    image_erase(wy_dummy);
    const auto lImageCreator = [&](){return (InputImageType*)((eInput==ofdis::FlowInput_RGB)?(void*)color_image_new(width,height):(void*)image_new(width,height));};
    InputImageType *w_im2 = lImageCreator(), // warped second image
                   *Ix = lImageCreator(), *Iy = lImageCreator(), *Iz = lImageCreator(), // first order derivatives
                   *Ixx = lImageCreator(), *Ixy = lImageCreator(), *Iyy = lImageCreator(), *Ixz = lImageCreator(), *Iyz = lImageCreator(); // second order derivatives
    // warp second image
    fdf::image_warp<eInput>(w_im2, mask, im2, wx, wy_dummy);
    // compute derivatives
    fdf::get_derivatives<eInput>(im1, w_im2, deriv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz);
    // erase du and dv
    image_erase(du);
    // initialize uu and vv
    memcpy(uu->c1,wx->c1,wx->stride*wx->height*sizeof(float));
    // inner fixed point iterations
    for(i_inner_iteration=0; i_inner_iteration<tvparams.n_inner_iteration; i_inner_iteration++) {
        //  compute robust function and system
        fdf::compute_smoothness(smooth_horiz, smooth_vert, uu, wy_dummy, deriv_flow, tvparams.tmp_quarter_alpha );
        fdf::compute_data_DE<eInput>(a11, b1, mask, wx, du, uu, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
        fdf::sub_laplacian(b1, wx, smooth_horiz, smooth_vert);
        // solve system
        sor_coupled_slow_but_readable_DE(du, a11, b1, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega);
        // update flow plus flow increment
        int i;
        v4sf *uup = (v4sf*) uu->c1, *wxp = (v4sf*) wx->c1, *dup = (v4sf*) du->c1;
        if(cpt->camlr==0) { // check if right or left camera, needed to truncate values above/below zero
            for(i=0; i<height*stride/4; i++) {
                (*uup) = __builtin_ia32_minps(   (*wxp) + (*dup)   ,  op->zero);
                uup+=1; wxp+=1; dup+=1;
            }
        }
        else {
            for(i=0 ; i<height*stride/4; i++) {
                (*uup) = __builtin_ia32_maxps(   (*wxp) + (*dup)   ,  op->zero);
                uup+=1; wxp+=1; dup+=1;
            }
        }
    }
    // add flow increment to current flow
    memcpy(wx->c1,uu->c1,uu->stride*uu->height*sizeof(float));
    // free memory
    image_delete(du);
    image_delete(wy_dummy);
    image_delete(mask);
    image_delete(smooth_horiz);
    image_delete(smooth_vert);
    image_delete(uu);
    image_delete(a11);
    image_delete(b1);
    local_image_delete(w_im2);
    local_image_delete(Ix);
    local_image_delete(Iy);
    local_image_delete(Iz);
    local_image_delete(Ixx);
    local_image_delete(Ixy);
    local_image_delete(Iyy);
    local_image_delete(Ixz);
    local_image_delete(Iyz);
}

template class ofdis::VarRefClass<ofdis::FlowInput_Grayscale,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::VarRefClass<ofdis::FlowInput_Gradient,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::VarRefClass<ofdis::FlowInput_RGB,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::VarRefClass<ofdis::FlowInput_Grayscale,ofdis::FlowOutput_StereoDepth>;
template class ofdis::VarRefClass<ofdis::FlowInput_Gradient,ofdis::FlowOutput_StereoDepth>;
template class ofdis::VarRefClass<ofdis::FlowInput_RGB,ofdis::FlowOutput_StereoDepth>;


/*!
 * @file     mppi_new.cpp
 * @author   Giuseppe Rizzi
 * @date     01.07.2020
 * @version  1.0
 * @brief    description
 */

#include <algorithm>

#include <Eigen/Core>
#include <chrono>
#include <iomanip>
#include <iostream>
#include <map>
#include <random>
#include <vector>

#include "mppi/core/config.h"
#include "mppi/core/cost.h"
#include "mppi/core/dynamics.h"
#include "mppi/core/rollout.h"
#include "mppi/core/solver.h"

#include "mppi/utils/logging.h"
#include "mppi/utils/savgol_filter.h"

#ifdef SIGNAL_LOGGER
#include <signal_logger/signal_logger.hpp>
#endif

namespace mppi {

Solver::Solver(dynamics_ptr dynamics, cost_ptr cost, policy_ptr policy,
               const Config& config)
    : dynamics_(std::move(dynamics)),
      cost_(std::move(cost)),
      policy_(std::move(policy)),
      config_(config) {
  init_data();
  init_threading();

  if (config_.display_update_freq) {
    start_time_ = std::chrono::high_resolution_clock::now();
  }

#ifdef SIGNAL_LOGGER
  signal_logger::add(min_cost_, "solver/rollouts/min_cost");
  signal_logger::add(max_cost_, "solver/rollouts/max_cost");
  signal_logger::add(rollouts_cost_, "solver/rollouts/costs");
  signal_logger::add(delay_steps_, "solver/delay_steps");
  signal_logger::add(rate_, "solver/rate");
  signal_logger::logger->updateLogger();
#endif
}

void Solver::init_data() {
  // TODO(giuseppe) this should be automatically computed when config is parsed
  steps_ = static_cast<int>(std::ceil(config_.horizon / config_.step_size));
  nx_ = dynamics_->get_state_dimension();
  nu_ = dynamics_->get_input_dimension();

  opt_roll_ = Rollout(steps_, nu_, nx_);
  opt_roll_cache_ = Rollout(steps_, nu_, nx_);
  nominal_.setZero(steps_, nu_);

  weights_.resize(config_.rollouts, 1.0 / config_.rollouts);
  rollouts_.resize(config_.rollouts, Rollout(steps_, nu_, nx_));
  cached_rollouts_ = std::ceil(config_.caching_factor * config_.rollouts);

  delay_steps_ = 0;
  step_count_ = 0;
  observation_set_ = false;
  reference_set_ = false;
}

void Solver::init_filter() {}

void Solver::init_threading() {
  if (config_.threads > 1) {
    std::cout << "Using multithreading. Number of threads: " << config_.threads
              << std::endl;
    pool_ = std::make_unique<ThreadPool>(config_.threads);
    futures_.resize(config_.threads);

    for (size_t i = 0; i < config_.threads; i++) {
      dynamics_v_.push_back(dynamics_->create());
      cost_v_.push_back(cost_->create());
    }
  }
}

void Solver::update_policy() {
  if (!observation_set_) {
    log_warning_throttle(1.0,
                         "Observation has never been set. Dropping update");
  } else if (!reference_set_) {
    log_warning_throttle(1.0, "Reference has never been set. Dropping update");
  } else {
    auto start = std::chrono::steady_clock::now();
    update_delay();
    copy_observation();

    for (size_t i = 0; i < config_.substeps; i++) {
      prepare_rollouts();
      update_reference();
      sample_trajectories();
      optimize();
      filter_input();

      stage_cost_ =
          cost_->get_stage_cost(x0_internal_, opt_roll_.uu[0], t0_internal_);
    }
    swap_policies();
    auto end = std::chrono::steady_clock::now();
    rate_ = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9;
  }
}

void Solver::time_it() {
  auto stop = std::chrono::high_resolution_clock::now();
  auto duration =
      std::chrono::duration_cast<std::chrono::microseconds>(stop - start_time_)
          .count();
  auto timing_string =
      "Time since optimization start: " + std::to_string(duration) + "[μs], " +
      std::to_string(duration / 1000000.0) + "[s], " +
      std::to_string(1 / (duration / 1000000.0)) + "[Hz]";
  log_info(timing_string);
  start_time_ = std::chrono::high_resolution_clock::now();
}

void Solver::set_observation(const observation_t& x, const double t) {
  {
    std::unique_lock<std::shared_mutex> lock(state_mutex_);
    x0_ = x;
    reset_time_ = t;
  }

  // initialization of rollouts data
  if (first_step_) {
    copy_observation();
    initialize_rollouts();
    first_step_ = false;
  }

  observation_set_ = true;
}

void Solver::update_delay() {
  delay_steps_ = std::ceil((reset_time_ - t0_internal_) / config_.step_size);
  policy_->update_delay(delay_steps_);
}

void Solver::copy_observation() {
  std::shared_lock<std::shared_mutex> lock(state_mutex_);
  x0_internal_ = x0_;
  t0_internal_ = reset_time_;
}

void Solver::initialize_rollouts() {
  std::shared_lock<std::shared_mutex> lock_state(state_mutex_);
  opt_roll_.clear();
  std::fill(opt_roll_.uu.begin(), opt_roll_.uu.end(),
            dynamics_->get_zero_input(x0_internal_));

  std::shared_lock<std::shared_mutex> lock(rollout_cache_mutex_);
  std::fill(opt_roll_cache_.xx.begin(), opt_roll_cache_.xx.end(), x0_internal_);
  std::fill(opt_roll_cache_.uu.begin(), opt_roll_cache_.uu.end(),
            dynamics_->get_zero_input(x0_internal_));
  for (int i = 0; i < steps_; i++) {
    opt_roll_cache_.tt[i] = t0_internal_ + config_.step_size * i;
  }
}

void Solver::prepare_rollouts() {
  // cleanup
  rollouts_cost_.clear();
  for (auto& roll : rollouts_) {
    roll.clear_cost();
    roll.valid = true;
  }
}

void Solver::set_reference_trajectory(mppi::reference_trajectory_t& ref) {
  if (ref.rr.size() != ref.tt.size()) {
    std::stringstream error;
    error << "The reference trajectory state and time dimensions do not match: "
          << ref.rr.size() << " != " << ref.tt.size();
    throw std::runtime_error(error.str());
  }
  std::unique_lock<std::shared_mutex> lock(reference_mutex_);
  rr_tt_ref_ = ref;
  reference_set_ = true;
}

void Solver::update_reference() {
  std::shared_lock<std::shared_mutex> lock(reference_mutex_);
  cost_->set_reference_trajectory(rr_tt_ref_);

  if (config_.threads > 1) {
    for (auto& cost : cost_v_) cost->set_reference_trajectory(rr_tt_ref_);
  }

}

void Solver::sample_noise(input_t& noise) { sampler_->get_sample(noise); }

void Solver::sample_trajectories_batch(dynamics_ptr& dynamics, cost_ptr& cost,
                                       const size_t start_idx,
                                       const size_t end_idx) {
  observation_t x;
  for (size_t k = start_idx; k < end_idx; k++) {
    dynamics->reset(x0_internal_, t0_internal_);
    x = x0_internal_;
    double ts;
    for (int t = 0; t < steps_; t++) {
      ts = t0_internal_ + t * config_.step_size;
      rollouts_[k].tt[t] = ts;
      rollouts_[k].uu[t] = policy_->sample(ts, k);

      // compute input-state stage cost
      double cost_temp;
      cost_temp = std::pow(config_.discount_factor, t) *
                  cost->get_stage_cost(x, rollouts_[k].uu[t], ts);

      if (std::isnan(cost_temp)) {
        std::stringstream ss;
        ss << "Something went wrong ... dynamics diverged?\n" << std::endl;
        ss << "Rollout#" << k << std::endl;
        ss << "Time  where diverged: " << t << std::endl;
        ss << "State where diverged: " << x.transpose() << std::endl;
        ss << "Previous state: " << rollouts_[k].xx[t - 1].transpose()
           << std::endl;
        ss << "Input where diverged: " << rollouts_[k].uu[t].transpose();
        ss << "Previous input: " << rollouts_[k].uu[t - 1].transpose()
           << std::endl;
        rollouts_[k].valid = false;
        rollouts_[k].total_cost = std::numeric_limits<double>::infinity();
        log_warning(ss.str());
        break;
      }

      // store data
      rollouts_[k].xx[t] = x;
      rollouts_[k].cc(t) = cost_temp;
      rollouts_[k].total_cost += cost_temp;

      // integrate dynamics    auto start = std::chrono::steady_clock::now();
      x = dynamics->step(rollouts_[k].uu[t], config_.step_size);
    }
  }
}

void Solver::sample_trajectories() {
  policy_->shift(t0_internal_);
  policy_->update_samples(weights_, cached_rollouts_);

  if (config_.threads == 1) {
    sample_trajectories_batch(dynamics_, cost_, 0, config_.rollouts);
  } else {
    for (size_t i = 0; i < config_.threads; i++) {
      futures_[i] = pool_->enqueue(
          std::bind(&Solver::sample_trajectories_batch, this,
                    std::placeholders::_1, std::placeholders::_2,
                    std::placeholders::_3, std::placeholders::_4),
          dynamics_v_[i], cost_v_[i],
          (size_t)i * config_.rollouts / config_.threads,
          (size_t)(i + 1) * config_.rollouts / config_.threads);
    }

    for (size_t i = 0; i < config_.threads; i++) futures_[i].get();
  }
}

void Solver::compute_weights() {
  // keep all non diverged rollouts
  double min_cost = std::numeric_limits<double>::max();
  double max_cost = -min_cost;

  for (size_t k = 0; k < config_.rollouts; k++) {
    if (rollouts_[k].valid) {
      const double& cost = rollouts_[k].total_cost;
      min_cost = (cost < min_cost) ? cost : min_cost;
      max_cost = (cost > max_cost) ? cost : max_cost;
    }
  }

  min_cost_ = min_cost;   //*std::min_element(rollouts_cost_.begin(), rollouts_cost_.end());
  max_cost_ = max_cost;   //*std::max_element(rollouts_cost_.begin(), rollouts_cost_.end());

  double sum = 0.0;
  for (size_t k = 0; k < config_.rollouts; k++) {
    double modified_cost = config_.h * (rollouts_[k].total_cost - min_cost_) /
                           (max_cost_ - min_cost_);

    weights_[k] = rollouts_[k].valid ? std::exp(-modified_cost) : 0.0;
    sum += weights_[k];
  }
  std::transform(weights_.begin(), weights_.end(), weights_.begin(),
                 [&sum](double v) -> double { return v / sum; });
}

void Solver::optimize() {
  // get new rollouts weights
  compute_weights();

  // update policy according to new weights
  policy_->update(weights_, config_.alpha);

  // retrieve the nominal policy for each time step
  for (int t = 0; t < steps_; t++) {
    opt_roll_.tt[t] = t0_internal_ + t * config_.step_size;
    opt_roll_.uu[t] = policy_->nominal(t0_internal_ + t * config_.step_size);
  }

}

void Solver::filter_input() {
  // only if filter is available reset to the initial opt time
  if (filter_) {
    filter_->reset(x0_internal_, t0_internal_);
  }

  // reset the dynamics such that we rollout the dynamics again
  // with the input we filter step after step (sequentially)
  dynamics_->reset(x0_internal_, t0_internal_);
  opt_roll_.xx[0] = x0_internal_;

  // sequential filtering otherwise just nominal rollout
  for (int t = 0; t < steps_ - 1; t++) {
    if (filter_) {
      filter_->apply(opt_roll_.xx[t], opt_roll_.uu[t], opt_roll_.tt[t]);
      nominal_.row(t) = opt_roll_.uu[t].transpose();
    }
    opt_roll_.xx[t + 1] = dynamics_->step(opt_roll_.uu[t], config_.step_size);
  }

  // filter the last input in the sequence (if filter is available)
  if (filter_) {
    filter_->apply(opt_roll_.xx.back(), opt_roll_.uu.back(),
                   opt_roll_.tt.back());
    nominal_.bottomRows(1) = opt_roll_.uu.back().transpose();
    policy_->set_nominal(nominal_);
  }
}

void Solver::get_input(const observation_t& x, input_t& u, const double t) {
  static double coeff;
  static size_t idx;
  {
    std::shared_lock<std::shared_mutex> lock(rollout_cache_mutex_);
    if (t < opt_roll_cache_.tt.front()) {
      std::stringstream warning;
      warning << "Queried time " << t << " smaller than first available time "
              << opt_roll_cache_.tt.front();
      log_warning_throttle(1.0, warning.str());
      u = dynamics_->get_zero_input(x);
    }

    auto lower = std::lower_bound(opt_roll_cache_.tt.begin(),
                                  opt_roll_cache_.tt.end(), t);
    if (lower == opt_roll_cache_.tt.end()) {
      std::stringstream warning;
      warning << "Queried time " << t << " larger than last available time "
              << opt_roll_cache_.tt.back();
      log_warning_throttle(1.0, warning.str());
      u = opt_roll_cache_.uu.back();
      return;
    }

    idx = std::distance(opt_roll_cache_.tt.begin(), lower);
    // first index (time)
    if (idx == 0) {
      u = opt_roll_cache_.uu.front();
    }
    // last index (time larget then last step)
    else if (idx > opt_roll_cache_.steps_) {
      u = opt_roll_cache_.uu.back();
    }
    // interpolate
    else {
      coeff = (t - *(lower - 1)) / (*lower - *(lower - 1));
      u = (1 - coeff) * opt_roll_cache_.uu[idx - 1] +
          coeff * opt_roll_cache_.uu[idx];
    }
  }
}

void Solver::get_diagonal_variance(Eigen::VectorXd& var) const {
  var = sampler_->sigma().diagonal();
}

void Solver::get_input_state(const observation_t& x, observation_t& x_nom,
                             input_t& u_nom, const double t) {
  {
    std::shared_lock<std::shared_mutex> lock(rollout_cache_mutex_);
    if (t < opt_roll_cache_.tt.front()) {
      std::stringstream warning;
      warning << "Queried time " << t << " smaller than first available time "
              << opt_roll_cache_.tt.front();
      log_warning_throttle(1.0, warning.str());
      x_nom = opt_roll_cache_.xx.front();
      u_nom = dynamics_->get_zero_input(x);
      return;
    }

    auto lower = std::lower_bound(opt_roll_cache_.tt.begin(),
                                  opt_roll_cache_.tt.end(), t);
    if (lower == opt_roll_cache_.tt.end()) {
      std::stringstream warning;
      warning << "Queried time " << t << " larger than last available time "
              << opt_roll_cache_.tt.back();
      log_warning_throttle(1.0, warning.str());
      x_nom = opt_roll_cache_.xx.back();
      u_nom = dynamics_->get_zero_input(x);
      return;
    }

    size_t idx = std::distance(opt_roll_cache_.tt.begin(), lower);

    // first
    if (idx == 0) {
      x_nom = opt_roll_cache_.xx.front();
      u_nom = opt_roll_cache_.uu.front();
    }
    // last
    else if (idx > opt_roll_cache_.steps_) {
      x_nom = opt_roll_cache_.xx.back();
      u_nom = opt_roll_cache_.uu.back();
    }
    // interpolate
    else {
      double coeff = (t - *(lower - 1)) / (*lower - *(lower - 1));
      u_nom = (1 - coeff) * opt_roll_cache_.uu[idx - 1] +
              coeff * opt_roll_cache_.uu[idx];
      x_nom = opt_roll_cache_.xx[idx];  // TODO offer a way to also do
                                        // interpolation of the state
    }
  }
}

bool Solver::get_optimal_rollout(observation_array_t& xx, input_array_t& uu) {
  std::shared_lock<std::shared_mutex> lock(rollout_cache_mutex_);
  auto lower = std::lower_bound(opt_roll_cache_.tt.begin(),
                                opt_roll_cache_.tt.end(), reset_time_);
  if (lower == opt_roll_cache_.tt.end()) return false;
  size_t offset = std::distance(opt_roll_cache_.tt.begin(), lower);

  // fill with portion of vector starting from current time
  xx = observation_array_t(opt_roll_cache_.xx.begin() + offset,
                           opt_roll_cache_.xx.end());
  uu = input_array_t(opt_roll_cache_.uu.begin() + offset,
                     opt_roll_cache_.uu.end());
  return true;
}

void Solver::get_optimal_rollout(Rollout& r) {
  std::shared_lock<std::shared_mutex> lock(rollout_cache_mutex_);

  // fill with portion of vector starting from current reset time
  r.tt = time_array_t(opt_roll_cache_.tt.begin(),
                      opt_roll_cache_.tt.end());

  r.xx = observation_array_t(opt_roll_cache_.xx.begin(),
                             opt_roll_cache_.xx.end());

  r.uu = input_array_t(opt_roll_cache_.uu.begin(),
                       opt_roll_cache_.uu.end());
}

void Solver::swap_policies() {
  std::unique_lock<std::shared_mutex> lock(rollout_cache_mutex_);
  opt_roll_cache_ = opt_roll_;
}

template <typename T>
void Solver::shift_back(std::vector<T>& v_out, const T& fill,
                        const int offset) {
  std::rotate(v_out.begin(), v_out.begin() + offset, v_out.end());
  std::fill(v_out.end() - offset, v_out.end(), fill);
}

// explicit instantiation
template void Solver::shift_back<Eigen::VectorXd>(
    std::vector<Eigen::VectorXd>& v_out, const Eigen::VectorXd& fill,
    const int offset);

void Solver::print_cost_histogram() const {
  constexpr int nbins = 10;
  double delta = (max_cost_ - min_cost_) / nbins;

  std::cout << "Rollouts cost histogram " << std::endl;
  std::cout << "Max: " << max_cost_ << ", Min: " << min_cost_
            << ", delta: " << delta << std::endl;
  std::map<int, int> hist{};
  for (const auto& cost : rollouts_cost_) {
    ++hist[std::round((cost - min_cost_) / delta)];
  }
  for (auto p : hist) {
    std::cout << std::setw(2) << p.first << ' ' << std::string(p.second, '*')
              << '\n';
  }
  std::cout << std::endl << std::endl;
}

}  // namespace mppi


#include <vector>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <list>
#include <string>
#include <math.h>
#include <fstream>
#include <float.h>
#include <boost/iostreams/stream.hpp>
#include "KNN_OpenMP_Code.h"
#include "Metrics.h"
#include <omp.h>
#include <boost/iostreams/device/mapped_file.hpp>
#include <boost/iostreams/stream.hpp>

using boost::iostreams::mapped_file_source;
using boost::iostreams::stream;
using namespace std;

/**
 * Read the output of linux command execution
 * @param  cmd  is the linux command to be executed
 * @return the output from the execution of the linux command
 */
std::string exec(const char* cmd) {
	std::array<char, 128> buffer;
	std::string result;
	std::unique_ptr<FILE, decltype(&pclose)> pipe(popen(cmd, "r"), pclose);
	if (!pipe) {
		throw std::runtime_error("popen() failed!");
	}
	while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) {
		result += buffer.data();
	}
	return result;
}

/**
 * Compute K-NN following the algorithm for shared-memory K-NN
 * @param filePath The full path to the input file containig the dataset.
 * @param N Size of Dataset without the header (i.e.(#Rows in dataset)-1).
 * @param Dim Dimension of Dataset (#Columns)
 * @param K the desired number of Nearest Neighbours to be computed
 * @param sampleRate The rate at which we do sampling
 * @param convThreshold Convergance Threshold
 * @param logFile The errors and informational messages are outputted to the log file
 * @param distanceMetric is the metric to compute the distance between the points in high-D space, by deafult should be euclidean
 * @param distanceV1 is the first optional variable needed for computing distance in some metrics
 * @param distanceV2 is the second optional variable needed for computing distance in some metrics
 * @param filePathOptionalArray The full path to optional array for the distance metric computation
 * @return B_Index indices of K-NN for each data point
 * @return B_Dist corresponding distance for K-NN indices stored in B_Index
 */
//void computeKNNs(string filePath, const int N, const int Dim, const int K, float sampleRate, const int convThreshold,int** B_Index,double** B_Dist, ofstream& logFile){
void computeKNNs(string filePath, const int N, const int Dim, const int K, float sampleRate, const int convThreshold,int** B_Index,double** B_Dist, ofstream& logFile, string distanceMetric, float distanceV1, float distanceV2, string filePathOptionalArray){

	logFile<<"------------Starting K-NN Solution------------"<<endl;
	cout<<"------------Starting K-NN Solution------------"<<endl;
	/**
	 * A 2D Array containing the entire input dataset (read from filePath).
	 */
	double** dataPoints = new double*[N];
	for (int i = 0; i < N; ++i) { dataPoints[i] = new double[Dim]; }
	/**
	 * corresponding flag for K-NN indices stored in B_Index
	 */
	short** B_IsNew = new short*[N];
	for (int i = 0; i < N; ++i) { B_IsNew[i] = new short[K]; }
	/**
	 * Data structure for new[v]
	 */
	vector<int> *New_Index = new std::vector<int>[N];
	/**
	 * Data structure for REVERSE(new[v]) or new'
	 */
	vector<int> *Reverse_New_Index = new vector<int>[N];
	/**
	 * Data Structure for SAMPLE(new'[v],pk)
	 */
	vector<int> *Sampled_Reverse_New_Index = new vector<int>[N];
	/**
	 * Data Structure for new[v] U SAMPLE(new'[v],pk)
	 */
	vector<int> *New_Final_List = new vector<int>[N];
	/**
	 * An approximation of zero in computing distances. Two points with the distance
	 * smaller than epsilon are considered as one point.
	 */
	double epsilon = 1e-10;
	short* allEntriesFilled = new short[N];
	/**
	 * At first, let's Read Dataset from Input File Using Memory Mapping
	 */
	mapped_file_source mmap(filePath);
    stream<mapped_file_source> is(mmap, std::ios::binary);
	if (is.fail())
	{
		logFile << "error in Opening Input File" << endl;
		cout << "error in Opening Input File" << endl;
		return ;
	}
	/**
	 * Remove the header info
	 */
	string dummyLine;
	getline(is, dummyLine);
	/**
	 * Reading the Entire Dataset
	 */
	for (int i = 0; i < N; ++i) {
		string temp, temp2;
		getline(is, temp);
		for (int j = 0; j < Dim; ++j) {
			temp2 = temp.substr(0, temp.find(","));
			dataPoints[i][j] = atof(temp2.c_str());
			temp.erase(0, temp.find(",") + 1);
		}
	}
	mmap.close();
	/**
	 * define a seed for random generator. Using a constant value produces
	 * the same set of random numbers and is good for debugging. Alternatively,
	 * we can select the seed number randomly as srand(time(NULL))
	 */
	srand(17);
	/**
	 * Initialization of Arrays B_IsNew and B_Dist
	 */
	for (int i = 0; i < N; ++i) {
		allEntriesFilled[i]=0;
		for (int j = 0; j < K; ++j) {
			B_IsNew[i][j] = 1;
			B_Dist[i][j] = -1.0;
		}
	}
	/**
	 * Random Initialization of B_Index
	 */
	int randomIndex, iter;
	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < K; ++j) {
			iter = 1;
			while (iter) {
				randomIndex = rand() % N;
				if (randomIndex != i) {
					B_Index[i][j] = randomIndex;
					iter = 0;
				}
			}
		}
	}
	/**
	 * Update list of K-NN indices for u1 (B_Index) if u2 is closer
	 * <p>
	 * This method correspondd to UPDATENN(B[u1],<u2,l,true>) in the paper
	 * </p>
	 * @param  Dist  represents B_Dist
	 * @param  Index represents B_Index
	 * @param  IsNew represents B_IsNew
	 * @param  u1    the indice of point that we want to potentially update its K-NN with the point u2
	 * @param  u2    the indice of potential K-NN fpr point u1
	 * @param  distance the spatial distance between u1 and u2
	 * @param  flag updates B_IsNew
	 * @return 1 if B_Index[u1][.] is updated, 0 otherwise
	 */
	auto UpdateNN = [&](int u1, int u2, double distance, int flag = 1) {

		if(allEntriesFilled[u1]==0){
			for (int j = 0; j < K; j++) {
				if (B_Dist[u1][j] < 0) {

				    for (int jj = 0; jj < j; jj++) {if (B_Index[u1][jj] == u2) return 0;}

					B_Dist[u1][j] = distance;
					B_Index[u1][j] = u2;
					B_IsNew[u1][j] = flag;
					if (j==K-1) allEntriesFilled[u1]=1;
					return 1;}
			}
		}

		else{
			for (int j = 0; j < K; j++) {
				if (B_Index[u1][j] == u2) return 0;
			}

			double max = DBL_MIN;
			int index = -1;
			for (int j = 0; j < K; j++) {
				if (B_Dist[u1][j] > max) {
					max = B_Dist[u1][j];
					index = j;
				}
			}
			if (index == -1) { logFile << "Error"; }
			if (distance < max) {
				B_Dist[u1][index] = distance;
				B_Index[u1][index] = u2;
				B_IsNew[u1][index] = flag;
				return 1;
			}
			else { return 0; }
		}
		return 0;
	};
	/**
	 * Main Loop of the Algorithm
	 */
	bool iterate = true;
	while (iterate) {
		/**
		 * Create "New" for each Datapoint
		 */
		for (int i = 0; i < N; ++i) {
			for (int j = 0; j < K; ++j) {
				if (float(rand() % 100) / 100 < sampleRate) {
					if (B_IsNew[i][j] == 1) {
						New_Index[i].push_back(B_Index[i][j]);
						B_IsNew[i][j] = 0;
					}
				}
			}
		}
		/**
		 * Create "New'"(or REVERSE("New")) for each Datapoint
		 */
		for (int i = 0; i < N; ++i) {
			for (size_t j = 0; j < New_Index[i].size(); ++j) {
				Reverse_New_Index[New_Index[i][j]].push_back(i);
			}
		}
		/**
		 * Random Sampling from "New'"
		 */
		for (int i = 0; i < N; ++i) {
			for (size_t j = 0; j < Reverse_New_Index[i].size(); ++j) {
				if (float(rand() % 100) / 100 < sampleRate) {
					Sampled_Reverse_New_Index[i].push_back(Reverse_New_Index[i][j]);
				}
			}
		}
		/**
		 * "New"= "New" U SAMPLE("New'", pK)
		 */
		for (int i = 0; i < N; ++i) {
			for (size_t j = 0; j < New_Index[i].size(); ++j) {
				New_Final_List[i].push_back(New_Index[i][j]);
			}
			for (size_t j = 0; j < Sampled_Reverse_New_Index[i].size(); ++j) {
				New_Final_List[i].push_back(Sampled_Reverse_New_Index[i][j]);
			}
		}
		/**
		 * c=c+UPDATENN(B[u1],<u2,l,true>)
		 */
		int c_criteria = 0;
		int abort=0;

		for (int i = 0; i < N; ++i) {
			if (abort != 0) break;

            #pragma omp parallel for schedule(dynamic)
			for (int it = 0; it < New_Final_List[i].size(); ++it) {
				int par1= New_Final_List[i][it];

				for (int it2 = it+1; it2 < New_Final_List[i].size(); ++it2) {
					int par2= New_Final_List[i][it2];
					if (par1 != par2 && abort ==0) {
						/**
						 * computes spatial distance between two points based on the chosen Metric
						 * @param distanceMetric the metric to compute the distance between the points in high-D space
						 * @param dataPoints represents input dataPoints read from filePath
						 * @param *it and *it2 indices of the desired points in input dataset
						 * @param Dim is #columns (or features) in input dataset
						 * @param distanceV1 is the first optional variable needed for computing distance in some metrics
						 * @param distanceV2 is the second optional variable needed for computing distance in some metrics
						 * @param filePathOptionalArray The full path to optional array for the distance metric computation
						 * @param logFile The errors and informational messages are outputted to the log file
						 * @return spatial distance between points two points
						 */
						double dista = computeDistance (distanceMetric, dataPoints, par1, par2, Dim, distanceV1, distanceV2, filePathOptionalArray, logFile);

						if (dista < epsilon) {
							logFile << "Found Duplicate Data for Points "<< par1 << " and " << par2 <<endl;;
							cout << "Found Duplicate Data for Points "<< par1 << " and " << par2 <<endl;
							abort=1;
							iterate = false;
						}
						#pragma omp critical
					    {
						    c_criteria += UpdateNN(par1, par2, dista, 1);
						    c_criteria += UpdateNN(par2, par1, dista, 1);
						}
					}
				}
			}
		}
		logFile << "c_criteria = " << c_criteria << " With Threshold Convergence of " << convThreshold << endl;
		cout << "c_criteria = " << c_criteria << " With Threshold Convergence of " << convThreshold << endl;
		if (c_criteria < convThreshold) { iterate = false; }
		/**
		 * Clear the contents of the used data structures
		 */
		for (int i = 0; i < N; ++i) {
			New_Index[i].clear();
			Reverse_New_Index[i].clear();
			Sampled_Reverse_New_Index[i].clear();
			New_Final_List[i].clear();
		}
	}

	delete[] dataPoints;
	logFile<<"------------Ending of K-NN Solution------------"<<endl;
	cout<<"------------Ending of K-NN Solution------------"<<endl;
	return;
}


/**
 * $Id$
 *
 * Copyright (C)
 * 2014 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#ifndef NDHIST_AXES_CONSTANT_BIN_WIDTH_AXIS_HPP_INCLUDED
#define NDHIST_AXES_CONSTANT_BIN_WIDTH_AXIS_HPP_INCLUDED 1

#include <cmath>
#include <string>
#include <sstream>

#include <boost/shared_ptr.hpp>

#include <boost/numpy/iterators/flat_iterator.hpp>

#include <ndhist/axis.hpp>
#include <ndhist/error.hpp>

namespace bn = boost::numpy;

namespace ndhist {
namespace axes {

/**
 * @class ConstantBinWidthAxis
 *     This class provides an axis with underlaying constant bin widths. To the
 *     user the bin widths could still vary. But as long as there is a value
 *     transformation available that transforms the bin edge values, which
 *     contuct the non-constant bin widths, into a linear scale with constant
 *     bin widths, this class can be used as axis with good performance.
 *
 *     The ValueTransform class must provide two static methods:
 *
 *         AxisValueType transform(AxisValueType const value)
 *
 *     and
 *
 *         AxisValueType back_transform(AxisValueType const value)
 *
 *     for transforming an edge value into linear scale and back.
 */
template <typename AxisValueType, typename ValueTransform>
class ConstantBinWidthAxis
  : public Axis
{
  public:
    typedef AxisValueType
            axis_value_type;

    typedef ValueTransform
            value_transform_type;

    typedef bn::iterators::single_value<axis_value_type>
            axis_value_type_traits;

    typedef Axis
            base;

    typedef ConstantBinWidthAxis<axis_value_type, value_transform_type>
            type;

  protected:
    /// The number of bins, including the possible under- and overflow bins.
    intptr_t n_bins_;

    /// The constant width of the bins. Remember, that the widths of the
    /// possible under- and overflow bins could be different from this value.
    axis_value_type bin_width_;

    /// The lower edge of the first bin with constant bin width, i.e. excluding
    /// the underflow bin.
    axis_value_type min_;

    /// The lower edge of the underflow bin.
    axis_value_type underflow_edge_;

    /// The upper edge of the overflow bin.
    axis_value_type overflow_edge_;

    /// The axis value type traits object, needed as temporary storage for
    /// dereferencing value pointers.
    axis_value_type_traits mutable avtt_;

  public:
    ConstantBinWidthAxis(
        bn::ndarray const & edges
      , std::string const & label
      , std::string const & name
      , bool has_underflow_bin
      , bool has_overflow_bin
      , bool is_extendable
      , intptr_t extension_max_fcap
      , intptr_t extension_max_bcap
    )
      : Axis(
            edges.get_size()-1
          , edges.get_dtype()
          , label
          , name
          , has_underflow_bin
          , has_overflow_bin
          , is_extendable
          , extension_max_fcap
          , extension_max_bcap
        )
    {
        // Set up the axis's function pointers.
        create_fct_                     = &type::create;
        get_bin_index_fct_              = &type::get_bin_index;
        get_binedges_ndarray_fct_       = &type::get_binedges_ndarray;
        get_lower_binedges_ndarray_fct_ = &base::get_lower_binedges_ndarray<type>;
        get_upper_binedges_ndarray_fct_ = &base::get_upper_binedges_ndarray<type>;
        get_bincenters_ndarray_fct_     = &base::get_bincenters_ndarray<type>;
        get_binwidths_ndarray_fct_      = &base::get_binwidths_ndarray<type>;
        get_n_bins_fct_                 = &type::get_n_bins;
        request_extension_fct_          = &type::request_extension;
        extend_fct_                     = &type::extend;
        create_axis_slice_fct_          = &base::create_axis_slice<type>;
        deepcopy_fct_                   = &type::deepcopy;

        //std::cout << "edges.shape(0): "<< edges.shape(0)<<std::endl;
        n_bins_ = edges.shape(0) - 1;
        //std::cout << "n_bins_: "<< n_bins_<<std::endl;
        if(n_bins_ <= 0)
        {
            std::stringstream ss;
            ss << "The edges array does not contain enough edges! It must "
               << "contain at least two edges! Remember "
               << "that the edges of the out-of-range bins, need to be "
               << "specified as well, when the axis is not extendable and is "
               << "supposed to have such bins.";
            throw ValueError(ss.str());
        }

        bn::iterators::flat_iterator< axis_value_type_traits > edges_iter(edges);

        // Set and skip the underflow edge.
        if(has_underflow_bin_)
        {
            underflow_edge_ = value_transform_type::transform(*edges_iter);
            ++edges_iter;
        }

        min_ = value_transform_type::transform(*edges_iter);
        ++edges_iter;
        if(   edges_iter.is_end()
           || (has_overflow_bin_ && edges_iter.get_iter_index() == n_bins_)
          )
        {
            // Only one bin (ie. the underflow or overflow bin) was given. So
            // we set the bin width to one.
            // Note: The bin_width_ property is used only for internal
            //       calculation. The bin width seen by the user is still
            //       calculated through the actual bin edges.
            bin_width_ = axis_value_type(1);
        }
        else
        {
            axis_value_type const value = value_transform_type::transform(*edges_iter);
            bin_width_ = value - min_;
        }

        // Set the overflow edge.
        if(has_overflow_bin_)
        {
            edges_iter.advance(edges_iter.distance_to(edges_iter.end()) - 1);
            overflow_edge_ = value_transform_type::transform(*edges_iter);
        }
    }

    /**
     * Copy constructor.
     */
    ConstantBinWidthAxis(ConstantBinWidthAxis const & other)
      : Axis(other)
      , n_bins_((*static_cast<type const *>(&other.get_axis_base())).n_bins_)
      , bin_width_((*static_cast<type const *>(&other.get_axis_base())).bin_width_)
      , min_((*static_cast<type const *>(&other.get_axis_base())).min_)
      , underflow_edge_((*static_cast<type const *>(&other.get_axis_base())).underflow_edge_)
      , overflow_edge_((*static_cast<type const *>(&other.get_axis_base())).overflow_edge_)
    {
        //std::cout << "ConstantBinWidthAxis:: Copy constructor." << std::endl<<std::flush;
    }

    static
    boost::shared_ptr<Axis>
    create(
        boost::numpy::ndarray const & edges
      , std::string const & label
      , std::string const & name
      , bool has_underflow_bin
      , bool has_overflow_bin
      , bool is_extendable
      , intptr_t extension_max_fcap
      , intptr_t extension_max_bcap
    )
    {
        return boost::shared_ptr<Axis>(new type(
            edges
          , label
          , name
          , has_underflow_bin
          , has_overflow_bin
          , is_extendable
          , extension_max_fcap
          , extension_max_bcap
        ));
    }

    static
    intptr_t
    get_n_bins(Axis const & axisbase)
    {
        type const & axis = *static_cast<type const *>(&axisbase);
        //std::cout << "constant_bin_width_axis: get_n_bins, axis_ptr: "<< &axis<< ", axis.n_bins_="<<axis.n_bins_ <<std::endl;
        return axis.n_bins_;
    }

    static
    bn::ndarray
    get_binedges_ndarray(Axis const & axisbase)
    {
        type const & axis = *static_cast<type const *>(&axisbase);

        intptr_t shape[1];
        shape[0] = axis.n_bins_ + 1;
        bn::ndarray edges_arr = bn::empty(1, shape, bn::dtype::get_builtin<axis_value_type>());
        bn::iterators::flat_iterator< axis_value_type_traits > iter(edges_arr);
        if(axis.has_underflow_bin_)
        {
            // Set the underflow edge.
            iter.set_value(value_transform_type::back_transform(axis.underflow_edge_));
            ++iter;
        }
        intptr_t idx = 0;
        while(! iter.is_end())
        {
            axis_value_type const value = axis.min_ + idx*axis.bin_width_;
            iter.set_value(value_transform_type::back_transform(value));

            ++idx;
            ++iter;
        }
        if(axis.has_overflow_bin_)
        {
            // Set the overflow edge.
            iter.advance(-1);
            iter.set_value(value_transform_type::back_transform(axis.overflow_edge_));
        }

        return edges_arr;
    }

    static
    intptr_t
    get_bin_index(Axis const & axisbase, char * value_ptr, axis::out_of_range_t & oor_flag)
    {
        type const & axis = *static_cast<type const *>(&axisbase);

        typename axis_value_type_traits::value_cref_type value_cref = axis_value_type_traits::dereference(axis.avtt_, value_ptr);
        axis_value_type const value = value_transform_type::transform(value_cref);
        //std::cout << "Got value = "<<value<<std::endl;

        if(axis.has_underflow_bin_)
        {
            if(value < axis.underflow_edge_)
            {
                // The value falls even left to the underflow bin.
                oor_flag = axis::OOR_UNDERFLOW;
                return -1;
            }
            if(value < axis.min_)
            {
                // The value falls into the underflow bin.
                //std::cout << "Got value in underflow bin"<< std::endl;
                oor_flag = axis::OOR_NONE;
                return 0;
            }
        }
        else
        {
            if(value < axis.min_)
            {
                //std::cout << "underflow: " << value << ", min = "<< data.min_ << std::endl;
                oor_flag = axis::OOR_UNDERFLOW;
                return -1;
            }
        }

        // The value is >= min_.
        intptr_t const idx = (value - axis.min_)/axis.bin_width_;

        if(axis.has_overflow_bin_)
        {
            if(idx < axis.n_bins_-2)
            {
                // The value falls into the axis range (excluding the overflow
                // bin).
                //std::cout << "Got value in normal bin"<< std::endl;
                oor_flag = axis::OOR_NONE;
                return idx + axis.has_underflow_bin_;
            }
            if(value < axis.overflow_edge_)
            {
                // The value falls into the overflow bin.
                oor_flag = axis::OOR_NONE;
                return axis.n_bins_-1;
            }

            // The value falls even right to the overflow bin.
            oor_flag = axis::OOR_OVERFLOW;
            return -1;
        }
        else
        {
            if(idx >= axis.n_bins_)
            {
                //std::cout << "overflow: " << value << ", idx = "<< idx << std::endl;
                oor_flag = axis::OOR_OVERFLOW;
                return -1;
            }
            //std::cout << "value " << value << " at " << idx << std::endl;
            oor_flag = axis::OOR_NONE;
            return idx + axis.has_underflow_bin_;
        }
    }

    // Determines the number of extra bins needed to the left (negative number
    // returned) or to the right (positive number returned) of the current axis
    // range.
    static
    intptr_t
    request_extension(Axis const & axisbase, char * value_ptr, axis::out_of_range_t const oor_flag)
    {
        type const & axis = *static_cast< type const *>(&axisbase);

        axis_value_type_traits avtt;
        typename axis_value_type_traits::value_ref_type value_cref = axis_value_type_traits::dereference(avtt, value_ptr);
        axis_value_type const value = value_transform_type::transform(value_cref);

        if(oor_flag == axis::OOR_UNDERFLOW)
        {
            intptr_t const n_extra_bins = std::ceil((std::abs(value - axis.min_) / axis.bin_width_));
            //std::cout << "request_autoscale (underflow): " << n_extra_bins << " extra bins." << std::endl<< std::flush;
            return -n_extra_bins;
        }
        else if(oor_flag == axis::OOR_OVERFLOW)
        {
            intptr_t const n_extra_bins = intptr_t((value - axis.min_)/axis.bin_width_) - (axis.n_bins_-1);
            //std::cout << "request_autoscale (overflow): " << n_extra_bins << " extra bins." << std::endl<< std::flush;
            return n_extra_bins;
        }

        return 0;
    }

    static
    void
    extend(Axis & axisbase, intptr_t f_n_extra_bins, intptr_t b_n_extra_bins)
    {
        type & axis = *static_cast< type *>(&axisbase);

        if(f_n_extra_bins > 0)
        {
            axis.n_bins_ += f_n_extra_bins;
            axis.min_    -= f_n_extra_bins * axis.bin_width_;
        }
        if(b_n_extra_bins > 0)
        {
            axis.n_bins_ += b_n_extra_bins;
        }
    }

    static
    boost::shared_ptr<Axis>
    deepcopy(Axis const & axisbase)
    {
        type const & axis = *static_cast<type const *>(&axisbase);
        return boost::shared_ptr<Axis>(new type(axis));
    }
};

}//namespace axes
}//namespace ndhist

#endif // NDHIST_AXES_CONSTANT_BIN_WIDTH_AXIS_HPP_INCLUDED


/*
	Authors: Darya Filippova, Geet Duggal, Rob Patro
	dfilippo | geet | robp @cs.cmu.edu
        See LICENSE.txt included with this distribution.
*/

#include <iostream>
#include <vector>
#include <fstream>
#include <string>

#include <boost/range/irange.hpp>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <cmath>
#include "ArmatusUtil.hpp"
#include "ArmatusParams.hpp"
#include "IntervalScheduling.hpp"
#include "ArmatusParams.hpp"
#include "ArmatusDAG.hpp"

MatrixProperties parseGZipMatrix(string path, int resolution, string chrom) {
	MatrixProperties prop;

    prop.matrix = std::make_shared<SparseMatrix>();

	ifstream file(path, ios_base::in | ios_base::binary);
    if (!file.good()) {
        std::cerr << "Couldn't read file " << path << std::endl;
        std::exit(1);
    }
    assert(file.good());
    boost::iostreams::filtering_streambuf<boost::iostreams::input> in;
    in.push(boost::iostreams::gzip_decompressor());
    in.push(file);

    string line;
    std::istream incoming(&in);
    bool firstLine = true;
    size_t i = 0;
    double tot = 0.0;
    size_t nedge = 0;
    while ( getline(incoming, line) ) {
    	vector<string> parts;
        boost::trim(line);
		boost::split(parts, line, boost::is_any_of("\t"));

        if (firstLine) {
            //if (parts.size() - 3 < 100) {
            if (parts.size() < 100) {
                cerr << "[INFO] Matrix is smaller than the recommended minimum size of 101." << endl;
                // exit(1);
            }
            prop.matrix->resize(parts.size(), parts.size(), false);
            prop.chrom = chrom;
            prop.resolution = resolution;
            cerr << prop.chrom << " at resolution " << prop.resolution << "bp" << endl;
            firstLine = false;
        }

        if (parts.size() != prop.matrix->size2()) {
            std::cerr << "Error: row " << i << " has " << parts.size()
                      << " entries, but I was expecting " << prop.matrix->size2() << std::endl;
            std::exit(3);
        }

        if (i >= prop.matrix->size2()) {
            std::cerr << "Error: I was expecting " << prop.matrix->size2()
                      << " rows, but there are more than that in the matrix file." << std::endl;
            std::exit(3);
        }

        for (size_t j : boost::irange(i, parts.size())) {
            double e  = stod(parts[j]);
            if (e > 0.0) {
                size_t row = j;
                prop.matrix->insert_element(i, row, e);
                tot += e;
                nedge++;
            }
        }

    	++i;
        if ( i % 1000 == 0 ) { std::cerr << "line " << i << "\n"; }
        if (incoming.eof()) break;
    }

    // check that we read # of rows = to the number of columns in the matrix
    if (prop.matrix->size2() != i) {
        std::cerr << "Error: it doesn't look like your matrix file had enough rows." << std::endl;
        std::cerr << "Error: expecting " << prop.matrix->size2() << " but saw " << i << std::endl;
        std::exit(1);
    }
    return prop;
}

MatrixProperties parseRaoMatrix(string path, int resolution, string chrom, bool noNormalization) {
	MatrixProperties prop;
    int n;
    int M = 0;
    auto rawCountPath = path + ".RAWobserved";

    {
	    ifstream file(rawCountPath);
        if (!file.good()) {
            std::cerr << "Couldn't read file: " << rawCountPath << std::endl;
            std::exit(1);
        }
        assert(file.good());

        string line;
        int v, w;
        double count;
        int maxv = 0;
        while ( file >> v >> w >> count )  {
            if (v > maxv) maxv = v;
            if (w > maxv) maxv = w;
            M += 1;
        }
        n = maxv/resolution+1;
        prop.chrom = chrom;
        prop.resolution = resolution;
        cerr << "Building matrix for chromosome " << prop.chrom << " at resolution " << prop.resolution << "bp with " << n << " rows." << endl;
    }

    vector<double> KRnormalization;
    auto KRPath = path + ".KRnorm";
    if (!noNormalization)
    {
        cerr << "Reading KR normalization counts" << endl;
	    ifstream file(KRPath);
        string line;
        while( getline(file, line) ) {
            KRnormalization.push_back(stod(line));
        }
    }
    cout << "Initializing matrix to zero elements" << endl;
    prop.matrix = std::make_shared<SparseMatrix>(n,n,M);
    for (int i = 0; i < n; i++) {
        for (int j =0; j < n; j++) {
            prop.matrix->insert_element(i,j, 0.0);
        }
    }
    {
	    ifstream file(rawCountPath);

        string line;
        int v, w;
        double count;
        int m = 0;
        while ( file >> v >> w >> count )  {
            size_t i = v/resolution;
            size_t j = w/resolution;
            if (noNormalization) {
                prop.matrix->insert_element(i, j, log(count));
            }
            else {
                double ki = KRnormalization[i];
                double kj = KRnormalization[j];
                if (!std::isnan(ki) and !std::isnan(kj) and ki > 0 and kj > 0) {
                    prop.matrix->insert_element(i, j, log(count/(ki*kj)));
                    // cout << i << "\t" << j << "\t" << count << "\t" << count/(ki*kj) << endl;
                }
                else {
                    // Maybe write NaN or inf issue to log/warning file
                }
            }
            m++;
            if ( m % 100000 == 0 ) { std::cerr << "" << float(m)/M*100 << "%\n"; }
        }
    }

    return prop;
}

MatrixProperties parseSparseMatrix(string filepath, int resolution, string chrom) {
  MatrixProperties prop;
  int n;
  int M=0;

  {
    ifstream file(filepath);
    if (!file.good()) {
      std::cerr << "Couldn't read file: " << filepath << std::endl;
      std::exit(1);
    }
    assert(file.good());

    string line;
    int v, w;
    double count;
    int maxv = 0;
    while ( file >> v >> w >> count ) {
      if (v > maxv) maxv = v;
      if (w > maxv) maxv = w;
      M += 1;
    }
    n = maxv/resolution+1;
    prop.chrom = chrom;
    prop.resolution = resolution;
    cerr << "Building matrix for chromosome " << prop.chrom << " at resolution " << prop.resolution << "bp with " << n << " rows." << endl;
  }

  cout << "Initializing matrix to zero elements" << endl;
  prop.matrix = std::make_shared<SparseMatrix>(n,n,M);
  for (int i=0; i < n; i++) {
    for (int j=0; j < n; j++) {
      prop.matrix->insert_element(i,j,0.0);
    }
  }

  {
    ifstream file(filepath);

    string line;
    int v,w;
    double count;
    int m= 0;
    while ( file >> v >> w >> count ) {
      size_t i = v/resolution;
      size_t j = w/resolution;
      prop.matrix->insert_element(i,j, log(count));
      m++;
      if ( m % 100000 == 0 ) { std::cerr << "" << float(m)/M*100 << "%\n";}
    }
  }
  return prop;
}

// domain size
double d(size_t const & i, size_t const & j) {return j - i + 1;}

Domain::Domain(size_t s, size_t e) : start(s), end(e) { }

DomainSet consensusDomains(WeightedDomainEnsemble& dEnsemble) {
    using PersistenceMap = map<Domain, double>;
    PersistenceMap pmap;

    for (auto dSetIdx : boost::irange(size_t{0}, dEnsemble.domainSets.size())) {
        auto& dSet = dEnsemble.domainSets[dSetIdx];
        auto weight = dEnsemble.weights[dSetIdx];
        for (auto& domain : dSet) {
            if ( pmap.find(domain) == pmap.end() ) pmap[domain] = 0;
            pmap[domain] += weight;
        }
    }

    Intervals ivals;

    for (auto domainPersistence : pmap) {
        auto domain = domainPersistence.first;
        auto persistence = domainPersistence.second;
        ivals.push_back(WeightedInterval(domain.start, domain.end, persistence));
    }

    IntervalScheduler scheduler(ivals);
    scheduler.computeSchedule();

    DomainSet dSet;
    for (auto ival : scheduler.extractIntervals()) {
        dSet.push_back(Domain(ival.start, ival.end));
    }

    sort(dSet.begin(), dSet.end());

    return dSet;
}

WeightedDomainEnsemble multiscaleDomains(std::shared_ptr<SparseMatrix> A,
    float gammaMax, double stepSize, int k, int minMeanSamples, bool justThisGamma) {

    WeightedDomainEnsemble dEnsemble;
    double eps = 1e-5;
    double gamma =0.0;
    if (justThisGamma)  {
        gamma = gammaMax;
    }

    for (; gamma <= gammaMax+eps; gamma+=stepSize) {

        cerr << "gamma=" << gamma << endl;

        ArmatusParams params(A, gamma, k, minMeanSamples); // k parameter is not used for anything in Params
        ArmatusDAG G(params); // but is used in the DAG
        G.build();
        G.computeTopK();

        auto domainEnsemble = G.extractTopK();
        auto& domains = domainEnsemble.domainSets;
        auto& weights = domainEnsemble.weights;
        dEnsemble.domainSets.insert(dEnsemble.domainSets.end(), domains.begin(), domains.end());
        dEnsemble.weights.insert(dEnsemble.weights.end(), weights.begin(), weights.end());
        for (int i = 0; i<k; i++) {
            dEnsemble.resolutions.push_back(gamma);
            dEnsemble.optidx.push_back(i);
        }
    }

    return dEnsemble;
}

void outputDomains(DomainSet dSet, string fname, MatrixProperties matProp) {
    ofstream file;
    file.open(fname);
    int res = matProp.resolution;
    for (auto d : dSet) {
        file << matProp.chrom << "\t" << (d.start)*res << "\t" << (d.end+1)*res-1 << endl;
    }
    file.close();
}

void sanityCheck(WeightedDomainEnsemble e) {
    for (auto dset : e.domainSets) {
    }
}


//
// Created by kerin on 2019-02-28.
//

#include "eigen_utils.hpp"
#include "mpi_utils.hpp"

#include "tools/eigen3.3/Dense"
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filter/gzip.hpp>

#include <iostream>
#include <cmath>
#include <map>
#include <vector>
#include <string>
#include <set>

namespace boost_io = boost::iostreams;

Eigen::MatrixXd EigenUtils::subset_matrix(const Eigen::MatrixXd &orig,
                                          const std::vector<int> &valid_points) {
	long n_cols = orig.cols(), n_rows = valid_points.size();
	Eigen::MatrixXd subset(n_rows, n_cols);

	for(int kk = 0; kk < n_rows; kk++) {
		for(int jj = 0; jj < n_cols; jj++) {
			subset(kk, jj) = orig(valid_points[kk], jj);
		}
	}
	return subset;
}

template <typename EigenMat>
void EigenUtils::write_matrix(boost_io::filtering_ostream& outf,
                              EigenMat& M,
                              std::vector<std::string >& col_names,
                              std::vector<std::string>& row_names){
	if(!col_names.empty()) {
		if (row_names.size() > 0) assert(col_names.size() == M.cols() + 1);
		if (row_names.size() == 0) assert(col_names.size() == M.cols());
		for (int jj = 0; jj < col_names.size(); jj++) {
			outf << col_names[jj];
			if (jj < col_names.size() - 1) {
				outf << " ";
			}
		}
		outf << std::endl;
	}

	if(!row_names.empty()) {
		assert(row_names.size() == M.rows());
		for (long ii = 0; ii < M.rows(); ii++) {
			outf << row_names[ii] << " ";
			for (long jj = 0; jj < M.cols(); jj++) {
				outf << M(ii, jj);
				if (jj < M.cols() - 1) {
					outf << " ";
				}
			}
			outf << std::endl;
		}
	} else {
		outf << M;
	}
}

template <typename EigenMat>
void EigenUtils::read_matrix( const std::string& filename,
                              EigenMat& M){
	std::vector<std::string> placeholder;
	EigenUtils::read_matrix(filename, M, placeholder);
}

template <typename EigenMat>
void EigenUtils::read_matrix(const std::string &filename,
                             EigenMat &M,
                             std::vector <std::string> &col_names){
	std::map<long, bool> incomplete_row;
	EigenUtils::read_matrix(filename, M, col_names, incomplete_row);
}

template <typename EigenMat>
void EigenUtils::read_matrix(const std::string &filename,
                             EigenMat &M,
                             std::vector <std::string> &col_names,
                             std::map<long, bool> &incomplete_row) {
	/* Read txt file into martix. Files can be gzipped. */

	boost_io::filtering_istream fg;
	std::string gz_str = ".gz";
	if (filename.find(gz_str) != std::string::npos) {
		fg.push(boost_io::gzip_decompressor());
	}
	fg.push(boost_io::file_source(filename));
	if (!fg) {
		throw std::runtime_error(filename+" could not be opened.");
	}

	// Read file twice to acertain number of lines
	std::string line;
	int n_rows = 0;
	getline(fg, line);
	while (getline(fg, line)) {
		n_rows++;
	}
	fg.reset();
	if (filename.find(gz_str) != std::string::npos) {
		fg.push(boost_io::gzip_decompressor());
	}
	fg.push(boost_io::file_source(filename));

	// Reading column names
	if (!getline(fg, line)) {
		throw std::runtime_error(filename+" contains zero lines.");
	}
	std::stringstream ss;
	std::string s1;
	int n_cols = 0;
	ss.clear();
	ss.str(line);
	while (ss >> s1) {
		++n_cols;
		col_names.push_back(s1);
	}
	std::cout << "Reading matrix of size " << n_rows << " x " << n_cols << " from " << filename << std::endl;

	// Write remainder of file to Eigen matrix M
	incomplete_row.clear();
	M.resize(n_rows, n_cols);
	int i = 0;
	double tmp_d;
	while (getline(fg, line)) {
		if (i >= n_rows) {
			throw std::runtime_error("ERROR: could not convert txt file (too many lines).");
		}
		ss.clear();
		ss.str(line);
		for (int k = 0; k < n_cols; k++) {
			std::string sss;
			ss >> sss;
			if (sss == "NA" || sss == "NAN" || sss == "NaN" || sss == "nan") {
				tmp_d = 0;
				incomplete_row[i] = true;
			} else {
				try {
					tmp_d = stod(sss);
				} catch (const std::invalid_argument &exc) {
					std::cout << sss << " on line " << i << std::endl;
					throw;
				}
			}
			M(i, k) = tmp_d;
		}
		i++;
	}
}

void EigenUtils::read_matrix_and_skip_cols(const std::string &filename,
                                           const int& n_skip_cols,
                                           Eigen::MatrixXd &M,
                                           std::vector <std::string> &col_names) {
	/* Assumptions:
	   - dimensions unknown
	   - assume no missing values
	 */

	boost_io::filtering_istream fg;
	std::string gz_str = ".gz";
	if (filename.find(gz_str) != std::string::npos) {
		fg.push(boost_io::gzip_decompressor());
	}
	fg.push(boost_io::file_source(filename));
	if (!fg) {
		throw std::runtime_error(filename+" could not be opened.");
	}

	// Read file twice to acertain number of lines
	std::string line;
	int n_rows = 0;
	getline(fg, line);
	while (getline(fg, line)) {
		n_rows++;
	}
	fg.reset();
	if (filename.find(gz_str) != std::string::npos) {
		fg.push(boost_io::gzip_decompressor());
	}
	fg.push(boost_io::file_source(filename));

	// Reading column names
	if (!getline(fg, line)) {
		throw std::runtime_error(filename+" contains zero lines.");
	}
	std::stringstream ss;
	std::string s1;
	int n_cols = 0;
	ss.clear();
	ss.str(line);
	while (ss >> s1) {
		++n_cols;
		col_names.push_back(s1);
	}
	std::cout << " Reading matrix of size " << n_rows << " x " << n_cols << " from " << filename << std::endl;
	assert(n_skip_cols < n_cols);

	// Write remainder of file to Eigen matrix M
	M.resize(n_rows, n_cols - n_skip_cols);
	int i = 0;
	double tmp_d;
	while (getline(fg, line)) {
		if (i >= n_rows) {
			throw std::runtime_error("ERROR: could not convert txt file (too many lines).");
		}
		ss.clear();
		ss.str(line);
		for (int k = 0; k < n_cols; k++) {
			std::string s;
			ss >> s;
			if (k >= n_skip_cols) {
				try {
					tmp_d = stod(s);
				} catch (const std::invalid_argument &exc) {
					std::cout << s << " on line " << i << std::endl;
					throw;
				}
				M(i, k - n_skip_cols) = tmp_d;
			}
		}
		i++;
	}
	if (i < n_rows) {
		throw std::runtime_error("ERROR: could not convert txt file (too few lines).");
	}
}

Eigen::MatrixXf EigenUtils::solve(const Eigen::MatrixXf &A, const Eigen::MatrixXf &b) {
	Eigen::MatrixXf x = A.colPivHouseholderQr().solve(b);
	double check = fabs((double)((A * x - b).norm()/b.norm()));
	if (check > 1e-6) {
		// std::string ms = "ERROR: could not solve covariate scatter matrix (Check = " +
		// std::to_string(check) + ").";
		// throw std::runtime_error(ms);
		std::cout << "WARNING: Error in solving covariate scatter matrix is: " << check << std::endl;
	}
	return x;
}

Eigen::MatrixXd EigenUtils::solve(const Eigen::MatrixXd &A, const Eigen::MatrixXd &b) {
	Eigen::MatrixXd x = A.colPivHouseholderQr().solve(b);
	double check = fabs((double)((A * x - b).norm()/b.norm()));
	if (check > 1e-8) {
		std::string ms = "ERROR: could not solve covariate scatter matrix (Check = " +
		                 std::to_string(check) + ").";
		throw std::runtime_error(ms);
	}
	return x;
}

template <typename EigenMat>
void EigenUtils::scale_matrix_and_remove_constant_cols(EigenMat &M,
                                                       long &n_cols,
                                                       std::vector<std::string> &col_names){
	// Scale eigen matrix passed by reference.
	// Removes columns with zero variance + updates col_names.
	// Only call on matrixes which have been reduced to complete cases,
	// as no check for incomplete rows.
	double Nlocal = M.rows();
	double Nglobal = mpiUtils::mpiReduce_inplace(&Nlocal);

	std::vector<std::size_t> keep;
	std::vector<std::string> keep_names;
	std::vector<std::string> reject_names;
	for (std::size_t k = 0; k < n_cols; k++) {
		double sum_mik2 = M.col(k).array().square().sum();
		sum_mik2 = mpiUtils::mpiReduce_inplace(&sum_mik2);
//		double sigma = 0.0;
//		double count = 0;
//		for (int i = 0; i < n_rows; i++) {
//			double val = M(i, k);
//			sigma += val * val;
//			count += 1;
//		}
		double sigma = std::sqrt(sum_mik2 / (Nglobal - 1));

//		sigma = sqrt(sigma/(count - 1));
		if (sigma > 1e-12) {
			M.col(k).array() /= sigma;
//			for (int i = 0; i < n_rows; i++) {
//				M(i, k) /= sigma;
//			}
			keep.push_back(k);
			keep_names.push_back(col_names[k]);
		} else {
			reject_names.push_back(col_names[k]);
		}
	}

	if (keep.size() != n_cols) {
		std::cout << " Removing " << (n_cols - keep.size())  << " column(s) with zero variance:" << std::endl;
		for(auto name : reject_names) {
			std::cout << name << std::endl;
		}
		// subset cols
		for (std::size_t i = 0; i < keep.size(); i++) {
			M.col(i) = M.col(keep[i]);
		}
		M.conservativeResize(M.rows(), keep.size());

		n_cols = keep.size();
		col_names = keep_names;
	}

	if (n_cols == 0) {
		throw std::runtime_error("ERROR: No columns left with nonzero variance after scale_matrix()");
	}
}

template <typename EigenMat>
void EigenUtils::center_matrix(EigenMat& M){
	// Center eigen matrix passed by reference.
	// Only call on matrixes which have been reduced to complete cases,
	// as no check for incomplete rows.
	long n_cols = M.cols();
	long n_rows = M.rows();
	double Nlocal = M.rows();
	double Nglobal = mpiUtils::mpiReduce_inplace(&Nlocal);

	for (int k = 0; k < n_cols; k++) {
		double sum_mik = 0.0;
		for (int i = 0; i < n_rows; i++) {
			sum_mik += M(i, k);
		}

		sum_mik = mpiUtils::mpiReduce_inplace(&sum_mik);
		double mu = sum_mik / Nglobal;
		for (int i = 0; i < n_rows; i++) {
			M(i, k) -= mu;
		}
	}
}

Eigen::MatrixXd EigenUtils::project_out_covars(Eigen::Ref<Eigen::MatrixXd> rhs, const Eigen::Ref<const Eigen::MatrixXd> &C,
                                               const Eigen::Ref<const Eigen::MatrixXd> &CtC_inv) {
	assert(CtC_inv.cols() == C.cols());
	assert(CtC_inv.rows() == C.cols());
	assert(C.rows() == rhs.rows());
	Eigen::MatrixXd CtRHS = C.transpose() * rhs;
	CtRHS = mpiUtils::mpiReduce_inplace(CtRHS);
	Eigen::MatrixXd beta = CtC_inv * CtRHS;
	Eigen::MatrixXd yhat = C * beta;
	Eigen::MatrixXd res = rhs - yhat;
	return res;
}

// Explicit instantiation
// https://stackoverflow.com/questions/2152002/how-do-i-force-a-particular-instance-of-a-c-template-to-instantiate

template void EigenUtils::read_matrix(const std::string&,
                                      Eigen::MatrixXf&, std::vector<std::string>&, std::map<long, bool>&);
template void EigenUtils::read_matrix(const std::string&,
                                      Eigen::MatrixXd&, std::vector<std::string>&, std::map<long, bool>&);
template void EigenUtils::read_matrix(const std::string&,
                                      Eigen::MatrixXf&, std::vector<std::string>&);
template void EigenUtils::read_matrix(const std::string&,
                                      Eigen::MatrixXd&, std::vector<std::string>&);
template void EigenUtils::read_matrix(const std::string&, Eigen::MatrixXf&);
template void EigenUtils::read_matrix(const std::string&, Eigen::MatrixXd&);
template void EigenUtils::write_matrix(boost_io::filtering_ostream&,
                                       Eigen::VectorXd&,
                                       std::vector<std::string>&,
                                       std::vector<std::string>&);
template void EigenUtils::write_matrix(boost_io::filtering_ostream&,
                                       Eigen::MatrixXd&,
                                       std::vector<std::string>&,
                                       std::vector<std::string>&);
template void EigenUtils::center_matrix(Eigen::MatrixXd&);
template void EigenUtils::center_matrix(Eigen::MatrixXf&);
template void EigenUtils::center_matrix(Eigen::VectorXd&);
template void EigenUtils::center_matrix(Eigen::VectorXf&);
template void EigenUtils::scale_matrix_and_remove_constant_cols(Eigen::MatrixXf&,
                                                                long&, std::vector<std::string>&);
template void EigenUtils::scale_matrix_and_remove_constant_cols(Eigen::MatrixXd&,
                                                                long&, std::vector<std::string>&);
template void EigenUtils::scale_matrix_and_remove_constant_cols(Eigen::VectorXd&,
                                                                long&, std::vector<std::string>&);