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

#ifndef SCALARSPACE_HH
#define SCALARSPACE_HH

#include <algorithm>
#include <functional>
#include <map>
#include <vector>

#include <boost/compressed_pair.hpp>
#include <boost/iterator/transform_iterator.hpp>

#include "fem/firstless.hh"
#include "fem/gridcombinatorics.hh"
#include "fem/views.hh"

namespace Kaskade
{
  // forward declarations
  template <class,class> class ContinuousLagrangeBoundaryMapper;

  namespace ScalarSpaceDetail
  {
    struct Empty {};

    /**
     * \brief A functor for extracting the first component of a boost compressed pair.
     */
    template <class Pair>
    struct CompressedFirst
    {
      typedef typename Pair::first_const_reference result_type;

      typename Pair::first_const_reference operator()(Pair const& pair) const
      {
        return pair.first();
      }
    };

    // Overload here as compressed pair's entries are accessed by method call.
    struct FirstLess
    {
      template <class Data>
      bool operator()(boost::compressed_pair<size_t,Data> const& a, boost::compressed_pair<size_t,Data> const& b)
      {
        return a.first() < b.first();
      }
    };

    /// Default policy, in general use this.
    /**
     * \tparam Implementation ...MapperImplementation
     * \tparam optional additional data stored for each shapefunction (see hierarchical and boundary spaces for examples)
     */
    template <class Implementation,class SFData=Empty>
    class MapperPolicy
    {
      typedef typename Implementation::Grid              Grid;
      typedef typename Implementation::GridView          GridView;
      typedef typename GridView::IndexSet                IndexSet;
      typedef typename Grid::template Codim<0>::Entity   Cell;
      typedef typename Implementation::ShapeFunctionSet  ShapeFunctionSet;
      typedef boost::compressed_pair<size_t,SFData>      Data;

    protected:
      static constexpr bool boundaryPolicy = false;

      explicit MapperPolicy(Implementation const& impl) : implementation(impl)
      {}

      /**
       * \brief Computes the number of dofs associated to each geometry type in the grid and
       *        initializes the startIndex indices accordingly, and resizes globIdx and sortedIdx appropriately.
       */
      template <class GlobalIndices, class SortedIndices>
      void initIndices(std::map<Dune::GeometryType,size_t>& startIndex, GlobalIndices& globIdx, SortedIndices& sortedIdx)
      {
        globIdx.resize(implementation.indexSet().size(0));
        sortedIdx.resize(implementation.indexSet().size(0));
        size_t n = 0;
        for (int codim=0; codim<=Grid::dimension; ++codim)
          for(auto const& geoType: implementation.indexSet().geomTypes(codim))
          {
            startIndex[geoType] = n;
            n += implementation.indexSet().size(geoType) * implementation.dofOnEntity(geoType);
          }
      }

      /**
       * \brief Store the global indices of the ansatz functions on cell into globIdx and a pair of global and local indices in sortedIdx.
       *
       * Sorting of sortedIdx will be performed in UniformScalarMapper::update() and thus must not be done here.
       *
       * \param startIndex holds offsets for different geometry types, useful if global indices are ordered according to geometry types, see below
       */
      template <class GlobalIndices, class SortedIndices>
      void computeIndices(Cell const& cell, std::map<Dune::GeometryType,size_t>& startIndex, GlobalIndices& globIdx, SortedIndices& sortedIdx, size_t cellIndex) const
      {
        ShapeFunctionSet const& sf = implementation.shapeFunctions(cell);
        size_t localNumberOfShapeFunctions = sf.size();
        globIdx[cellIndex].resize(localNumberOfShapeFunctions);
        sortedIdx[cellIndex].resize(localNumberOfShapeFunctions);

        // step through all shape functions.
        // TODO: pull common parts out of this loop
        for(size_t i=0; i<localNumberOfShapeFunctions; ++i)
        {
          Dune::GeometryType gt;
          int subentity, codim, indexInSubentity;
          SFData sfData;
          implementation.entityIndex(cell,sf[i],i,gt,subentity,codim,indexInSubentity,sfData);

          // compute the global index of the subentity to which the shape function is associated
          int gt_idx = subIndex(implementation.indexSet(),cell,codim,subentity);

          auto mi = startIndex.find(gt);
          assert(mi!=startIndex.end());

          globIdx[cellIndex][i] =  Data(mi->second+gt_idx*implementation.dofOnEntity(gt)+indexInSubentity,sfData);
          sortedIdx[cellIndex][i] = std::make_pair(globIdx[cellIndex][i].first(),i);
        }
      }

      template <class Container, class ShapeFunctionSet>
      void initShapeFunctionSet(Container& sfs, ShapeFunctionSet const& sf, size_t cellIndex) const
      {
        sfs[cellIndex] = &sf; // cache shape function set
      }

      Implementation implementation;
    };

    /**
     * \cond internal
     */
    inline bool onFace_3D(int codim, int id, int localFaceId)
    {
      // edges
      if(codim==2)
      {
        if(localFaceId==0) return (id>=0 && id<3);
        if(localFaceId==1) return (id==0 || id==3 || id==4);
        if(localFaceId==2) return (id==1 || id==3 || id==5);
        if(localFaceId==3) return (id==2 || id==4 || id==5);
      }
      // vertices
      if(codim==3)
      {
        if(localFaceId==0) return (id>=0 && id<3);
        if(localFaceId==1) return (id==0 || id==1 || id==3);
        if(localFaceId==2) return (id==0 || id==2 || id==3);
        if(localFaceId==3) return (id==1 || id==2 || id==3);
      }
      return false;
    }
    /**
     * \endcond
     */

    /// check if subentity is subentity of face
    /**
     * \param dim space dimension
     * \param codim codimension of subentity
     * \param id index within the subentities with codimension codim
     * \param localFaceId local index of face in cell
     */
   inline bool onFace(int dim, int codim, int id, int localFaceId)
    {
      if(codim==0) return false;
      if(codim==1) return id==localFaceId;

      if(dim==2)
      {
        if(localFaceId==0) return (id==0 || id==1);
        if(localFaceId==1) return (id==0 || id==2);
        if(localFaceId==2) return (id==2 || id==1);
      }
      if(dim==3) return onFace_3D(codim,id,localFaceId);

      return false;
    }

    static constexpr int defaultIndex = -94279;

    template <class Face>
    int getBoundaryId(Face const& face, std::vector<int> const& boundaryIds)
    {
      size_t boundarySegmentIndex = face.boundarySegmentIndex();
      if(boundarySegmentIndex < boundaryIds.size()) return boundaryIds[boundarySegmentIndex];
      return defaultIndex;
    }

    inline bool usedId(int id, std::vector<int> const& usedIds)
    {
      if (id==defaultIndex) return false;
      return std::find(usedIds.begin(), usedIds.end(), id) != usedIds.end();
    }

    template <class Face>
    inline bool considerFace(Face const& face, std::vector<int> const& boundaryIds, std::vector<int> const& usedIds)
    {
      if(boundaryIds.empty()) return face.boundary();
      return (face.boundary() && usedId(getBoundaryId(face,boundaryIds),usedIds));
    }

    ///
    struct RestrictToBoundary
    {
      //static constexpr int defaultIndex = -99999;

      RestrictToBoundary() : boundaryIds(), usedIds() {}
      RestrictToBoundary(RestrictToBoundary const& other) : boundaryIds(other.boundaryIds), usedIds(other.usedIds) {}
      RestrictToBoundary(std::vector<int> const& boundaryIds_, std::vector<int> const& usedIds_) : boundaryIds(boundaryIds_), usedIds(usedIds_)
      {}

      template <class Data, class GridView, class Cell>
      void treatBoundary(Data& data, GridView const& gridView, Cell const& cell, int codim, int subentity) const
      {
        // ignore shape functions that are associated with codim-0 entities
        if(codim > 0) {
          for(auto const& face : intersections(gridView,cell)) {
            // only consider boundary faces
            if(considerFace(face,boundaryIds,usedIds)) {
              if(onFace(GridView::dimension,codim,subentity,face.indexInInside())) {
                data.first() = true;
              }
            }
          }
        }
      }

      std::vector<int> const boundaryIds;
      std::vector<int> const usedIds;
    };

    template <class Policy>
    constexpr bool isRestriction()
    {
      return std::is_same<Policy,RestrictToBoundary>::value;
    }

    ///
    struct AllShapeFunctions
    {
      template <class Data, class GridView, class Cell> void treatBoundary(Data&, GridView const&, Cell const&, int, int) const {}
    };
  } // End of namespace ScalarSpaceDetail

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

  /**
   * \brief A traits class defining the type of argument that is provided by
   * UniformScalarMapper:combiner on the call to the Combiner constructor.
   */
  template <class SFData>
  struct UniformScalarMapperCombinerArgument
  {
    // nice try :DD
    // private:
    typedef typename std::vector<boost::compressed_pair<size_t,SFData> >::const_iterator Iterator;
    /**
     * \brief The sequence type that is provided to the call of the Combiner constructor.
     *
     * The type is a lightweight view type and should be kept by value, not by reference.
     */
    typedef RangeView<Iterator> type;
  };

  /**
   * \ingroup fem
   * \brief Base class for uniform scalar local to global mappers.
   *
   * It manages degrees of freedom for ansatz spaces where to each type of entity
   * of the grid the same number of global degrees of freedom is
   * associated and on each cell live the same number of shape
   * functions.
   *
   * We call a degree of freedom associated to an entity e of the grid
   * - if the support of the ansatz function is contained within the union
   *   of the cells (codim 0 entitites) which are incident to e and
   * - the ansatz function vanishes on all subentities of incident cells
   *   with a codimension not less than e.
   * This includes the usual polynomial FE spaces of arbitrary but fixed order.
   *
   * With each shape function on each cell, we associate its global
   * degree of freedom and additional data the type of is specified as
   * template parameter \a SFData.
   *
   * \tparam Implementation
   * \tparam SFData type of (optional) additional data associated to global degrees of freedom
   *
   */
  template <class Implementation, class SFData = ScalarSpaceDetail::Empty>
  class UniformScalarMapper : public ScalarSpaceDetail::MapperPolicy<Implementation,SFData>
  {
  public:
    typedef typename Implementation::Grid              Grid;
    typedef typename Grid::template Codim<0>::Entity   Cell;
    typedef typename Implementation::ShapeFunctionSet  ShapeFunctionSet;
    typedef typename Implementation::Converter         Converter;
    typedef typename Implementation::Combiner          Combiner;
    typedef typename Implementation::Scalar            Scalar;
    typedef typename Implementation::GridView          GridView;
    typedef typename GridView::IndexSet                IndexSet;
    typedef std::pair<size_t,int>                      IndexPair;

  private:
    typedef boost::compressed_pair<size_t,SFData> Data;

    typedef ScalarSpaceDetail::CompressedFirst<Data> First;

    typedef boost::transform_iterator<First,typename std::vector<Data>::const_iterator> GlobalIndexIterator;
    typedef std::vector<IndexPair>::const_iterator                                      SortedIndexIterator;

    static constexpr int dim = Grid::dimension;

  public:
    typedef RangeView<GlobalIndexIterator> GlobalIndexRange;
    typedef RangeView<SortedIndexIterator> SortedIndexRange;

    /**
     * \brief Whether the ansatz functions have global support (i.e. lead to dense matrices).
     */
    static bool const globalSupport = false;

    UniformScalarMapper(Implementation const& impl) : ScalarSpaceDetail::MapperPolicy<Implementation,SFData>(impl)
    {
      update();
    }

    /**
     * \brief Returns the maximal polynomial order of shape functions encountered in any cell.
     */
    int maxOrder() const
    {
      return order;
    }

    /**
     * \brief Returns an empty range just for initialization purposes, since RangeView is not default constructible.
     */
    GlobalIndexRange initGlobalIndexRange() const
    {
      return GlobalIndexRange(GlobalIndexIterator(globIdx[0].begin(),First()),
                              GlobalIndexIterator(globIdx[0].begin(),First()));
    }

    /**
     * \brief Returns an immutable sequence containing the global indices of the shape functions associated to this cell.
     *
     * Global indices start at 0 and are consecutive - in the range returned here, an unordered subset
     * is contained.
     */
    GlobalIndexRange globalIndices(Cell const& cell) const
    {
      return globalIndices(implementation.indexSet().index(cell));
    }

    /**
     * \brief Returns an immutable sequence containing the global indices of the shape functions associated to this cell.
     *
     * Global indices start at 0 and are consecutive - in the range returned here, an unordered subset
     * is contained.
     */
    GlobalIndexRange globalIndices(size_t n) const
    {
      return GlobalIndexRange(GlobalIndexIterator(globIdx[n].begin(),First()),
                              GlobalIndexIterator(globIdx[n].end(),  First()));
    }

    /**
     * \brief Returns an empty range just for initialization, since RangeView is not default constructible.
     */
    static SortedIndexRange initSortedIndexRange()
    {
      static std::vector<IndexPair> dummy; // empty
      return SortedIndexRange(dummy.begin(),dummy.end());
    }

    /**
     * \brief Returns an immutable sequence of (global index, local index) pairs sorted in ascending global index order.
     */
    SortedIndexRange sortedIndices(Cell const& cell) const
    {
      return sortedIndices(implementation.indexSet().index(cell));
    }

    /**
     * \brief Returns an immutable sequence of (global index, local index) pairs sorted in ascending global index order.
     */
    SortedIndexRange sortedIndices(size_t n) const
    {
      return SortedIndexRange(sortedIdx[n].begin(),sortedIdx[n].end());
    }

    /**
     * \brief Returns the number of global degrees of freedom managed.
     *
     * Note that this does not correspond directly to the number of
     * coefficients in a FE function (if the FE function has more than
     * one component).
     */
    size_t size() const { return n; }

    /**
     * \brief DEPRECATED. Use maxOrder instead. This method will be removed after 2016-12-31.
     */
    int getOrder() const { return order; }

    /**
     * \brief Returns the set of shape functions defined on this cell.
     * \param cell the codim 0 entity of the grid for wich the shape functions are to be retrieved
     * \param contained if true, the method may assume that the cell is contained in the index set of the space.
     *                  (The other case occurs during interpolation between different grids).
     */
    ShapeFunctionSet const& shapefunctions(Cell const& cell, bool contained=false) const
    {
      if (contained || implementation.indexSet().contains(cell))
        return shapefunctions(implementation.indexSet().index(cell));
      else
        return implementation.shapeFunctions(cell);
      // This is the previous implementation (as of Kaskade 7.1). I'm not sure the
      // condition of the if makes sense at all. First, cell has to be contained in
      // the index set, otherwise the index and hence globalIndices(cell) is undefined.
      // However, if the cell is contained in the index set, the globalIndices are not
      // empty (compare the construction in update()). Ws-2012-06-07.
      //
      //     if ( cell.isLeaf() && globalIndices(cell).empty() )
      //       return implementation.emptyShapeFunctionSet();
      //     else
      //       return implementation.shapeFunctions(cell);
    }

    ShapeFunctionSet& shapefunctions_non_const(Cell const& cell)
    {
      return shapefunctions_non_const(implementation.indexSet().index(cell));
    }

    /**
     * \brief Returns the set of shape functions defined on this cell.
     */
    ShapeFunctionSet const& shapefunctions(size_t n) const
    {
      return *sfs[n];
    }

    ShapeFunctionSet& shapefunctions_non_const(size_t n)
    {
      return *sfs[n];
    }

    ShapeFunctionSet const& lowerShapeFunctions(Cell const& cell) const
    {
      if (globalIndices(cell).empty())
        return implementation.emptyShapeFunctionSet();
      else
        return implementation.lowerShapeFunctions(cell);
    }

    /**
     * \brief Returns a combiner for the given leaf cell.
     * \param cell the grid cell for which the combiner is requested
     * \param index the index of the cell
     */
    Combiner combiner(Cell const& cell, size_t index) const {
      assert(implementation.indexSet().index(cell)==index);
      return Combiner(rangeView(globIdx[index].begin(),globIdx[index].end()));
    }

    /**
     * \brief (Re)computes the internal data.
     *
     * This has to be called after grid modifications and on construction.
     */
    void update()
    {
      GridView const& gridView = implementation.gridView();
      IndexSet const& indexSet = implementation.indexSet();

      // For each codimension (i.e. type of subentity) compute the
      // number of global ansatz functions as well as an accumulated
      // index into an array of all ansatz functions.
      startIndex.clear();
      // compute global degrees of freedom and store the offsets for
      // different geometry types in startIndex
      // see MapperPolicy
      // Precompute and cache all the global indices. First allocate the
      // memory needed to prevent frequent reallocation.
      this->initIndices(startIndex,globIdx,sortedIdx);

      sfs.resize(indexSet.size(0));
      order = 0;

      // Step through all cells and compute the global ansatz function
      // indices of all shape functions on that cell.
      for(auto const& element : elements(gridView))
      {
        size_t const cellIndex = indexSet.index(element);
        ShapeFunctionSet const& sf = implementation.shapeFunctions(element);
        this->initShapeFunctionSet(sfs,sf,cellIndex);                           // initShapeFunctionSet is defined in template base class MapperPolicy.
        order = std::max(order,sf.order());
        this->computeIndices(element, startIndex, globIdx, sortedIdx, cellIndex);   // computeIndices is defined in template base class MapperPolicy.

        // sort the index pairs according to the global index
        std::sort(sortedIdx[cellIndex].begin(),sortedIdx[cellIndex].end(),FirstLess());

        // make sure that any assigned shape function forms at most one ansatz function.
        // This is a functionality restriction, but seems quite reasonable as a debugging check.
        // Note that not all shape functions need not be assigned. If a shape function is mapped     //Jakob: Are negative indices for unused shape function really allowed? Are'nt they simply not present in globIdx, such that the check idx[j]<0 below is nonsense?
        // to negative global index, it takes not part at all (e.g. in hp-methods).
#ifndef NDEBUG
        std::vector<int> idx(globIdx[cellIndex].size());
        for (int j=0; j<idx.size(); ++j) idx[j] = globIdx[cellIndex][j].first();
        std::sort(idx.begin(),idx.end());
        for (int j=1; j<idx.size(); ++j) assert(idx[j]>idx[j-1] || idx[j]<0);
#endif
      }

      // compute overall number of degrees of freedom
      n = 0;
      for(size_t i=0; i<globIdx.size(); ++i)
        if(globIdx[i].size() > 0)
          n = std::max(n,std::max_element(globIdx[i].begin(),globIdx[i].end(),ScalarSpaceDetail::FirstLess())->first());
      ++n;
    }

    /**
     * \brief Returns a half-open range of global indices that are associated
     * to the entities with given geometry type.
     *
     * This is useful to partition stiffness matrices when using higher
     * order hierarcical ansatz functions in order to use a direct
     * solver on the low order ansatz functions as a preconditioner.
     */
    std::pair<size_t,size_t> globalIndexRange(Dune::GeometryType gt) const
    {
      assert(startIndex.find(gt)!=startIndex.end());

      size_t first = startIndex.find(gt)->second;
      size_t last = size();

      for (auto const& si: startIndex)
        if (si.second>first && si.second<last)
          last = si.second;

      return std::make_pair(first,last);
    }

  private:
    // In startIndex, for any geometry type that occurs in the indexSet
    // there is an index stored, such that the dofs associated with
    // nodes on subentities of this type start at this index. We request
    // that there is a fixed number of dofs associated with each
    // subentity type, such that equal length index blocks are assigned
    // to each subentity of a given type. The layout of dofs is thus as
    // follows (example continuous 2D simplex mesh with two triangles,
    // order 4):
    //
    // [Interior Triangle Nodes, Interior Edge Nodes, Vertex Nodes] with substructuring
    // [ [[t1a,t1b,t1c],[t2a,t2b,t2c]], [[e1a,e1b],[e2a,e2b],[e3a,e3b],[e4a,e4b],[e5a,e5b]], [v1,v2,v3,v4] ]
    //     | start index triangles        | start index edges                                 | start index vertices
    // localDofs(triangles)=3, localDofs(edges)=2, localDofs(vertices)=1
    size_t                              n;
    std::map<Dune::GeometryType,size_t> startIndex;

    // In globIdx, for each cell there are the global ansatz function indices of the
    // shape functions on that cell.
    std::vector<std::vector<Data>>      globIdx;

    // In sortedIdx, for each cell there is a vector of (global index, local index) pairs, sorted ascendingly
    // by the global index. This could be computed outside on demand, but the sorting takes time and may be
    // amortized over multiple assembly passes/matrix blocks if cached here.
    std::vector<std::vector<IndexPair> > sortedIdx;

    // In sfs, for each cell there is a pointer to the shape function set on this cell cached.
    std::vector<typename std::conditional<ScalarSpaceDetail::MapperPolicy<Implementation,SFData>::boundaryPolicy,ShapeFunctionSet,ShapeFunctionSet const>::type*> sfs;
    //std::vector<ShapeFunctionSet const*> sfs;

    int                                 order;

  protected:
    using ScalarSpaceDetail::MapperPolicy<Implementation,SFData>::implementation; // direct access to base class member variable
  };
} // end of namespace Kaskade

#endif


#include <stdlib.h>

#include <boost/program_options.hpp>
#include <chrono>
#include <limits>

#include "../space/initial_triangulation.hpp"
#include "../time/basis.hpp"
#include "../tools/util.hpp"
#include "adaptive_heat_equation.hpp"
#include "problems.hpp"

using applications::AdaptiveHeatEquation;
using datastructures::DoubleTreeView;
using space::HierarchicalBasisFn;
using Time::OrthonormalWaveletFn;
using Time::ThreePointWaveletFn;

using namespace applications;
namespace po = boost::program_options;

namespace applications {
std::istream& operator>>(std::istream& in,
                         HeatEquationOptions::SpaceInverse& inverse_type) {
  std::string token;
  in >> token;
  if (token == "DirectInverse" || token == "di")
    inverse_type = HeatEquationOptions::SpaceInverse::DirectInverse;
  else if (token == "Multigrid" || token == "mg")
    inverse_type = HeatEquationOptions::SpaceInverse::Multigrid;
  else
    in.setstate(std::ios_base::failbit);
  return in;
}
}  // namespace applications

space::InitialTriangulation InitialTriangulation(std::string domain,
                                                 size_t initial_refines) {
  if (domain == "square" || domain == "unit-square")
    return space::InitialTriangulation::UnitSquare(initial_refines);
  else if (domain == "lshape" || domain == "l-shape")
    return space::InitialTriangulation::LShape(initial_refines);
  else if (domain == "pacman")
    return space::InitialTriangulation::Pacman(initial_refines);
  else {
    std::cout << "domain not recognized :-(" << std::endl;
    exit(1);
  }
}

int main(int argc, char* argv[]) {
  std::string problem, domain;
  size_t initial_refines = 0;
  size_t max_level = 0;
  size_t max_dofs = 0;
  std::string refine;
  bool calculate_condition_PY = false;
  bool calculate_condition_PX = false;
  bool print_time_apply = false;
  bool print_centers = false;
  double solve_rtol = 1e-5;
  boost::program_options::options_description problem_optdesc(
      "Problem options");
  problem_optdesc.add_options()(
      "problem", po::value<std::string>(&problem)->default_value("singular"))(
      "domain", po::value<std::string>(&domain)->default_value("square"))(
      "initial_refines", po::value<size_t>(&initial_refines))(
      "max_level",
      po::value<size_t>(&max_level)
          ->default_value(std::numeric_limits<std::size_t>::max()))(
      "max_dofs", po::value<size_t>(&max_dofs)->default_value(
                      std::numeric_limits<std::size_t>::max()))(
      "refine", po::value<std::string>(&refine)->default_value("sparse"))(
      "print_centers", po::value<bool>(&print_centers))(
      "print_time_apply", po::value<bool>(&print_time_apply))(
      "calculate_condition_PX", po::value<bool>(&calculate_condition_PX))(
      "calculate_condition_PY", po::value<bool>(&calculate_condition_PY));

  AdaptiveHeatEquationOptions adapt_opts;
  boost::program_options::options_description adapt_optdesc(
      "AdaptiveHeatEquation options");
  adapt_optdesc.add_options()("use_cache",
                              po::value<bool>(&adapt_opts.use_cache))(
      "build_space_mats", po::value<bool>(&adapt_opts.build_space_mats))(
      "solve_rtol", po::value<double>(&solve_rtol))(
      "solve_maxit", po::value<size_t>(&adapt_opts.solve_maxit))(
      "estimate_saturation_layers",
      po::value<size_t>(&adapt_opts.estimate_saturation_layers))(
      "estimate_mean_zero", po::value<bool>(&adapt_opts.estimate_mean_zero))(
      "mark_theta", po::value<double>(&adapt_opts.mark_theta))(
      "PX_alpha", po::value<double>(&adapt_opts.PX_alpha))(
      "PX_inv",
      po::value<HeatEquationOptions::SpaceInverse>(&adapt_opts.PX_inv))(
      "PY_inv",
      po::value<HeatEquationOptions::SpaceInverse>(&adapt_opts.PY_inv))(
      "PXY_mg_build", po::value<bool>(&adapt_opts.PXY_mg_build))(
      "PX_mg_cycles", po::value<size_t>(&adapt_opts.PX_mg_cycles))(
      "PY_mg_cycles", po::value<size_t>(&adapt_opts.PY_mg_cycles));
  boost::program_options::options_description cmdline_options;
  cmdline_options.add(problem_optdesc).add(adapt_optdesc);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(),
            vm);
  po::notify(vm);
  if (refine != "sparse" && refine != "full" && refine != "local") {
    std::cout << "Refine method `" << refine << "` not recognized."
              << std::endl;
    return 1;
  }

  std::cout << "Problem options:" << std::endl;
  std::cout << "\tProblem: " << problem << std::endl;
  std::cout << "\tDomain: " << domain
            << "; initial-refines: " << initial_refines << std::endl;
  std::cout << "\tRefinement method: " << refine << std::endl;
  std::cout << std::endl;
  std::cout << adapt_opts << "\tsolve-rtol: " << solve_rtol << std::endl
            << std::endl;

  auto T = InitialTriangulation(domain, initial_refines);
  auto B = Time::Bases();
  auto vec_Xd = std::make_shared<
      DoubleTreeVector<ThreePointWaveletFn, HierarchicalBasisFn>>(
      B.three_point_tree.meta_root(), T.hierarch_basis_tree.meta_root());
  auto start_algorithm = std::chrono::steady_clock::now();

  Eigen::VectorXd solution = Eigen::VectorXd::Zero(vec_Xd->container().size());
  std::vector<typename HeatEquation::TypeXVector::DNType*> max_gradedness;
  for (int level = 1; level < max_level; level++) {
    std::pair<std::unique_ptr<LinearFormBase<Time::OrthonormalWaveletFn>>,
              std::unique_ptr<LinearFormBase<Time::ThreePointWaveletFn>>>
        problem_data;
    if (problem == "smooth")
      problem_data = SmoothProblem();
    else if (problem == "singular")
      problem_data = SingularProblem();
    else if (problem == "cylinder")
      problem_data = CylinderProblem();
    else if (problem == "moving-peak")
      problem_data = MovingPeakProblem(vec_Xd);
    else {
      std::cout << "problem not recognized :-(" << std::endl;
      return 1;
    }
    vec_Xd->FromVectorContainer(solution);

    // Refine underlying
    if (refine == "sparse")
      vec_Xd->SparseRefine(2 * level, {2, 1}, /* grow_tree */ true);
    else if (refine == "full")
      vec_Xd->UniformRefine({level, 2 * level}, /* grow_tree */ true);
    else if (refine == "local")
      vec_Xd->DeepRefine(
          [level](auto dblnode) {
            auto [psi_time, psi_space] = dblnode;
            if (2 * psi_time->level() + psi_space->level() > 2 * level)
              return false;
            return (psi_time->is_metaroot() ||
                    psi_time->Interval().first == 0) &&
                   (psi_space->is_metaroot() ||
                    psi_space->TouchesDomainBoundary());
          },
          [&](auto dblnode) {
            dblnode->node_0()->Refine();
            dblnode->node_1()->Refine();
          });

    solution = vec_Xd->ToVectorContainer();
    size_t ndof_Xd = vec_Xd->Bfs().size();  // A slight overestimate.
    if (ndof_Xd == 0) continue;
    if (ndof_Xd > max_dofs) break;
    AdaptiveHeatEquation heat_eq(vec_Xd, std::move(problem_data.first),
                                 std::move(problem_data.second), adapt_opts);
    size_t ndof_Xdd = heat_eq.vec_Xdd()->Bfs().size();
    size_t ndof_Ydd = heat_eq.vec_Ydd()->Bfs().size();
    std::cout << "\nlevel: " << level << "\n\tXDelta-size: " << ndof_Xd
              << "\n\tXDelta-Gradedness: "
              << vec_Xd->Gradedness(&max_gradedness)
              << "\n\tXDelta-time-size: " << vec_Xd->Project_0()->Bfs().size()
              << "\n\tXDelta-space-size: " << vec_Xd->Project_1()->Bfs().size()
              << "\n\tXDeltaDelta-size: " << ndof_Xdd
              << "\n\tYDeltaDelta-size: " << ndof_Ydd
              << "\n\ttotal-memory-kB: " << getmem() << std::flush;

    if (calculate_condition_PY || calculate_condition_PX) {
      auto start = std::chrono::steady_clock::now();
      std::chrono::duration<double> duration_cond =
          std::chrono::steady_clock::now() - start;

      if (calculate_condition_PY) {
        // Set the initial vector to something valid.
        heat_eq.vec_Ydd()->Reset();
        for (auto nv : heat_eq.vec_Ydd()->Bfs())
          if (!nv->node_1()->on_domain_boundary()) nv->set_random();
        auto lanczos_Y = tools::linalg::Lanczos(
            *heat_eq.heat_d_dd()->A(), *heat_eq.heat_d_dd()->P_Y(),
            heat_eq.vec_Ydd()->ToVectorContainer());
        std::cout << "\n\tlmin-PY-A: " << lanczos_Y.min()
                  << "\n\tlmax-PY-A: " << lanczos_Y.max();
      }

      if (calculate_condition_PX) {
        // Set the initial vector to something valid.
        heat_eq.vec_Xd()->Reset();
        for (auto nv : heat_eq.vec_Xd()->Bfs())
          if (!nv->node_1()->on_domain_boundary()) nv->set_random();
        auto lanczos_X = tools::linalg::Lanczos(
            *heat_eq.heat_d_dd()->S(), *heat_eq.heat_d_dd()->P_X(),
            heat_eq.vec_Xd()->ToVectorContainer());
        std::cout << "\n\tlmin-PX-S: " << lanczos_X.min()
                  << "\n\tlmax-PX-S: " << lanczos_X.max();
      }
      std::cout << "\n\tcond-time: " << duration_cond.count();
      continue;
    }

    // Solve - estimate.
    auto start = std::chrono::steady_clock::now();
    auto [cur_solution, pcg_data] =
        heat_eq.Solve(solution, heat_eq.RHS(), solve_rtol,
                      tools::linalg::StoppingCriterium::Relative);
    solution = cur_solution;
    std::chrono::duration<double> duration_solve =
        std::chrono::steady_clock::now() - start;
    std::cout << "\n\tsolve-PCG-steps: " << pcg_data.iterations
              << "\n\tsolve-time: " << duration_solve.count()
              << "\n\tsolve-memory: " << getmem() << std::flush;

    if (print_time_apply) {
      auto heat_d_dd = heat_eq.heat_d_dd();
      std::cout << "\n\tA-time-per-apply: " << heat_d_dd->A()->TimePerApply()
                << "\n\tB-time-per-apply: " << heat_d_dd->B()->TimePerApply()
                << "\n\tBT-time-per-apply: " << heat_d_dd->BT()->TimePerApply()
                << "\n\tG-time-per-apply: " << heat_d_dd->G()->TimePerApply()
                << "\n\tP_Y-time-per-apply: "
                << heat_d_dd->P_Y()->TimePerApply()
                << "\n\tP_X-time-per-apply: "
                << heat_d_dd->P_X()->TimePerApply()
                << "\n\tS-time-per-apply: " << heat_d_dd->S()->TimePerApply()
                << "\n\ttotal-time-apply: " << heat_d_dd->TotalTimeApply()
                << "\n\ttotal-time-construct: "
                << heat_d_dd->TotalTimeConstruct() << std::flush;
    }

    if (print_centers) {
      vec_Xd->FromVectorContainer(solution);
      auto print_dblnode = [](auto dblnode) {
        std::cout << "((" << dblnode->node_0()->level() << ","
                  << dblnode->node_0()->center() << "),"
                  << "(" << dblnode->node_1()->level() << ",("
                  << dblnode->node_1()->center().first << ","
                  << dblnode->node_1()->center().second
                  << ")) : " << dblnode->value() << ";";
      };

      std::cout << "\n\tcenters: ";
      for (auto dblnode : vec_Xd->Bfs()) print_dblnode(dblnode);
    }

    start = std::chrono::steady_clock::now();
    auto [residual, global_errors] = heat_eq.Estimate(solution);
    auto [residual_norm, global_error] = global_errors;
    std::chrono::duration<double> duration_estimate =
        std::chrono::steady_clock::now() - start;

    std::cout << "\n\tresidual-norm: " << residual_norm
              << "\n\testimate-time: " << duration_estimate.count()
              << "\n\testimate-memory: " << getmem() << std::flush;
    std::cout << "\n\tglobal-error: " << global_error.error
              << "\n\tYnorm-error: " << global_error.error_Yprime
              << "\n\tT0-error: " << global_error.error_t0
              << "\n\ttotal-time-algorithm: "
              << std::chrono::duration<double>(
                     std::chrono::steady_clock::now() - start_algorithm)
                     .count()
              << std::flush;

#ifdef VERBOSE
    std::cerr << std::endl << "Adaptive::Trees" << std::endl;
    std::cerr << "  T.vertex:   #bfs =  " << T.vertex_tree.Bfs().size()
              << std::endl;
    std::cerr << "  T.element:  #bfs =  " << T.elem_tree.Bfs().size()
              << std::endl;
    std::cerr << "  T.hierarch: #bfs =  " << T.hierarch_basis_tree.Bfs().size()
              << std::endl;
    std::cerr << std::endl;
    std::cerr << "  B.elem:     #bfs =  " << B.elem_tree.Bfs().size()
              << std::endl;
    std::cerr << "  B.three_pt: #bfs =  " << B.three_point_tree.Bfs().size()
              << std::endl;
    std::cerr << "  B.ortho:    #bfs =  " << B.ortho_tree.Bfs().size()
              << std::endl;
#endif
  }

  return 0;
}


/*****************************************************************************/
/*  Copyright (c) 2015, Karl Pauwels                                         */
/*  All rights reserved.                                                     */
/*                                                                           */
/*  Redistribution and use in source and binary forms, with or without       */
/*  modification, are permitted provided that the following conditions       */
/*  are met:                                                                 */
/*                                                                           */
/*  1. Redistributions of source code must retain the above copyright        */
/*  notice, this list of conditions and the following disclaimer.            */
/*                                                                           */
/*  2. Redistributions in binary form must reproduce the above copyright     */
/*  notice, this list of conditions and the following disclaimer in the      */
/*  documentation and/or other materials provided with the distribution.     */
/*                                                                           */
/*  3. Neither the name of the copyright holder nor the names of its         */
/*  contributors may be used to endorse or promote products derived from     */
/*  this software without specific prior written permission.                 */
/*                                                                           */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      */
/*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        */
/*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR    */
/*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT     */
/*  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   */
/*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         */
/*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,    */
/*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY    */
/*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE    */
/*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.     */
/*****************************************************************************/

#include <cmath>
#include <cstdio>
#include <translation_rotation_3d.h>
#undef Success
#include <Eigen/Geometry>

namespace pose {

const double TranslationRotation3D::PI_ = 3.1415926535897931;

TranslationRotation3D::TranslationRotation3D(bool valid) : valid_{ valid } {
  double tmpT[3] = { 0.0, 0.0, 0.0 };
  setT(tmpT);
  double tmpR[3] = { 0.0, 0.0, 0.0 };
  setR(tmpR);
}

TranslationRotation3D::TranslationRotation3D(const double *T_in,
                                             const double *R_in)
    : valid_{ true } {
  setT(T_in);
  setR(R_in);
}

TranslationRotation3D::TranslationRotation3D(Eigen::Vector3d T,
                                             Eigen::Vector3d R) {
  double T_in[3];
  double R_in[3];
  Eigen::Map<Eigen::Vector3d> T_tmp(T_in);
  Eigen::Map<Eigen::Vector3d> R_tmp(R_in);
  T_tmp = T;
  R_tmp = R;
  setT(T_in);
  setR(R_in);
}

template <typename Type>
TranslationRotation3D::TranslationRotation3D(const Type TR_in[6])
    : valid_{ true } {
  double T_in[3], R_in[3];
  for (int i = 0; i < 3; i++) {
    T_in[i] = static_cast<double>(TR_in[i]);
    R_in[i] = static_cast<double>(TR_in[i + 3]);
  }
  setT(T_in);
  setR(R_in);
}
template TranslationRotation3D::TranslationRotation3D<float>(
    const float TR_in[6]);
template TranslationRotation3D::TranslationRotation3D<double>(
    const double TR_in[6]);

TranslationRotation3D::TranslationRotation3D(
    const Ogre::Vector3 &ogre_translation,
    const Ogre::Quaternion &ogre_rotation)
    : valid_{ true } {
  double tmpT[3] = { ogre_translation.x, ogre_translation.y,
                     ogre_translation.z };
  double tmpR_mat[9];

  Eigen::Quaterniond q_eigen(ogre_rotation.w, ogre_rotation.x, ogre_rotation.y,
                             ogre_rotation.z);

  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_eig(tmpR_mat);
  rot_eig = q_eigen.toRotationMatrix();

  setT(tmpT);
  setR_mat(tmpR_mat);
}

bool TranslationRotation3D::operator==(const TranslationRotation3D &op) const {
  bool equal = true;
  for (int i = 0; i < 3; i++) {
    equal = (equal && (op.T_[i] == T_[i]));
    equal = (equal && (op.R_[i] == R_[i]));
  }
  equal = (equal && (op.valid_ == valid_));
  return (equal);
}

bool TranslationRotation3D::operator!=(const TranslationRotation3D &op) const {
  return (!(*this == op));
}

TranslationRotation3D &TranslationRotation3D::
operator*=(const TranslationRotation3D &rhs) {
  // Fl = Fl*Fr;
  Eigen::Map<Eigen::Vector3d> tra_left(T_);
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_left(R_mat_);
  double T_rhs[3];
  rhs.getT(T_rhs);
  Eigen::Map<Eigen::Vector3d> tra_right(T_rhs);
  double R_mat_rhs[9];
  rhs.getR_mat(R_mat_rhs);
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_right(
      R_mat_rhs);

  tra_left = rot_left * tra_right + tra_left;
  rot_left *= rot_right;
  updateR();

  // convert back to rotation matrix to increase numerical stability
  updateR_mat();

  // apply logical AND to validity
  setValid(isValid() && rhs.isValid());

  return *this;
}

double TranslationRotation3D::normT2() const {
  return (T_[0] * T_[0] + T_[1] * T_[1] + T_[2] * T_[2]);
}

double TranslationRotation3D::normR2() const {
  return (R_[0] * R_[0] + R_[1] * R_[1] + R_[2] * R_[2]);
}

TranslationRotation3D TranslationRotation3D::changeHandedness() const {

  // Eigen conversion
  // ----------------

  //  Eigen::Vector3d t(t_buf[0], t_buf[1], t_buf[2]);
  //  Eigen::Matrix3d Rl;

  //  Rl << r_buf[0], r_buf[1], r_buf[2],
  //      r_buf[3], r_buf[4], r_buf[5],
  //      r_buf[6], r_buf[7], r_buf[8];

  //  // transform translation to right-handed system (flip-y)
  //  t(1) = -t(1);

  //  // transform rotation to right-handed system
  //  // flip-y, followed by 180 degree rotation around x-axis to convert ogre
  // (z-out)
  //  // to vision (z-forward)
  //  Eigen::Matrix3d Sy = Eigen::Matrix<double, 3, 3>::Identity();
  //  Sy(1,1) = -1.0;
  //  Eigen::Matrix3d Rx_180 = Eigen::Matrix<double, 3, 3>::Identity();
  //  Rx_180(1,1) = -1.0;
  //  Rx_180(2,2) = -1.0;
  //  Eigen::Matrix3d R = Sy * Rl * Sy * Rx_180;

  // direct conversion
  // -----------------

  // flip ty
  double T_out[]{ T_[0], -T_[1], T_[2] };

  // (flip row 2 and then flip column 3)
  double R_out[]{ R_mat_[0], R_mat_[1], -R_mat_[2], -R_mat_[3], -R_mat_[4],
                  R_mat_[5], R_mat_[6], R_mat_[7],  -R_mat_[8] };

  TranslationRotation3D TR_out;
  TR_out.setT(T_out);
  TR_out.setR_mat(R_out);
  TR_out.setValid(valid_);

  return TR_out;
}

TranslationRotation3D TranslationRotation3D::rotateX180() const {
  double T_out[]{ T_[0], T_[1], T_[2] };
  double R_out[]{ R_mat_[0],  -R_mat_[1], -R_mat_[2], R_mat_[3], -R_mat_[4],
                  -R_mat_[5], R_mat_[6],  -R_mat_[7], -R_mat_[8] };

  TranslationRotation3D TR_out;
  TR_out.setT(T_out);
  TR_out.setR_mat(R_out);
  TR_out.setValid(valid_);

  return TR_out;
}

TranslationRotation3D TranslationRotation3D::inverseTransform() const {

  Eigen::Map<const Eigen::Vector3d> tra(T_);
  Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot(R_mat_);

  double rot_inv_ptr[9];
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_inv(
      rot_inv_ptr);

  rot_inv = rot.transpose();
  double tra_inv_ptr[3];
  Eigen::Map<Eigen::Vector3d> tra_inv(tra_inv_ptr);
  tra_inv = -rot_inv * tra;

  TranslationRotation3D pose_inv;
  pose_inv.setT(tra_inv_ptr);
  pose_inv.setR_mat(rot_inv_ptr);
  pose_inv.setValid(valid_);

  return (pose_inv);
}

Ogre::Vector3 TranslationRotation3D::ogreTranslation() const {
  return Ogre::Vector3(T_[0], T_[1], T_[2]);
}

void TranslationRotation3D::getQuaternion(double &x, double &y, double &z,
                                          double &w) const {
  Eigen::Matrix3d R_eigen;
  R_eigen << R_mat_[0], R_mat_[1], R_mat_[2], R_mat_[3], R_mat_[4], R_mat_[5],
      R_mat_[6], R_mat_[7], R_mat_[8];

  Eigen::Quaterniond q_eigen;
  q_eigen = R_eigen;

  x = q_eigen.x();
  y = q_eigen.y();
  z = q_eigen.z();
  w = q_eigen.w();
}

Ogre::Quaternion TranslationRotation3D::ogreRotation() const {
  double x, y, z, w;
  getQuaternion(x, y, z, w);
  return Ogre::Quaternion(w, x, y, z);
}

Eigen::Vector3d TranslationRotation3D::eigenTranslation() const {
  Eigen::Map<const Eigen::Vector3d> trans(T_);
  return trans;
}

Eigen::Matrix3d TranslationRotation3D::eigenRotation() const {
  Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_mat(
      R_mat_);
  return rot_mat;
}

Eigen::MatrixXd TranslationRotation3D::adjoint() const {
  double F_ptr[4 * 4];
  Eigen::Map<const Eigen::Matrix<double, 4, 4, Eigen::RowMajor> > F(F_ptr);
  getF(F_ptr);
  Eigen::MatrixXd adj(6, 6);
  Eigen::Vector3d T = F.topRightCorner<3, 1>();
  Eigen::Matrix3d R = F.topLeftCorner<3, 3>();
  Eigen::Matrix3d skewT;
  skewT << 0.0, -T(2), T(1), T(2), 0.0, -T(0), -T(1), T(0), 0.0;
  adj << R, skewT *R, Eigen::Matrix3d::Zero(), R;
  return adj;
}

void TranslationRotation3D::createGLModelMatrix(float *M_out) const {
  double TGL[3], RGL[3];
  getT(TGL);
  getR(RGL);

  TGL[2] = -TGL[2];
  RGL[2] = -RGL[2];

  TranslationRotation3D TR(TGL, RGL);

  double R_matGL[9];
  TR.getR_mat(R_matGL);

  M_out[0] = R_matGL[0];
  M_out[1] = R_matGL[1];
  M_out[2] = R_matGL[2];
  M_out[3] = 0.0;
  M_out[4] = R_matGL[3];
  M_out[5] = R_matGL[4];
  M_out[6] = R_matGL[5];
  M_out[7] = 0.0;
  M_out[8] = R_matGL[6];
  M_out[9] = R_matGL[7];
  M_out[10] = R_matGL[8];
  M_out[11] = 0.0;
  M_out[12] = TGL[0];
  M_out[13] = TGL[1];
  M_out[14] = TGL[2];
  M_out[15] = 1.0;
}

bool TranslationRotation3D::isFinite() const {
  bool res = true;
  for (int i = 0; i < 3; i++) {
    res = res && std::isfinite(T_[i]);
    res = res && std::isfinite(R_[i]);
  }

  return res;
}

void TranslationRotation3D::getEuler(double &Ex, double &Ey, double &Ez) const {
  //  [ 0 1 2
  //    3 4 5
  //    6 7 8 ]
  // http://nghiaho.com/?page_id=846

  double r32 = R_mat_[7];
  double r33 = R_mat_[8];
  double r31 = R_mat_[6];
  double r21 = R_mat_[3];
  double r11 = R_mat_[0];

  Ex = atan2(r32, r33);
  Ey = atan2(-r31, sqrt(r32 * r32 + r33 * r33));
  Ez = atan2(r21, r11);
}

void TranslationRotation3D::getF(double *F_out) const {
  Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor> > hom(F_out);
  hom << eigenRotation(), eigenTranslation(), 0.0, 0.0, 0.0, 1.0;
}

void TranslationRotation3D::setT(const double *T_in) {
  for (int i = 0; i < 3; i++)
    T_[i] = T_in[i];
}

void TranslationRotation3D::setR(const double *R_in) {
  for (int i = 0; i < 3; i++)
    R_[i] = R_in[i];
  updateR_mat();
}

void TranslationRotation3D::setR_mat(double *R_mat_in) {
  for (int i = 0; i < 9; i++)
    R_mat_[i] = R_mat_in[i];
  updateR();
}

void TranslationRotation3D::setF(const std::vector<double> &F_in) {
  if (F_in.size() != 16)
    throw std::runtime_error(
        "TranslationRotation3D::setF: F_in requires 16 elements");

  if ((F_in.at(12) != 0.0) || (F_in.at(13) != 0.0) || (F_in.at(14) != 0.0) ||
      (F_in.at(15) != 1.0))
    throw std::runtime_error(
        "TranslationRotation3D::setF: bottom row of F_in should be [0 0 0 1]");

  Eigen::Map<const Eigen::Matrix<double, 4, 4, Eigen::RowMajor> > F_in_eig(
      F_in.data());

  Eigen::Transform<double, 3, Eigen::Affine> F;
  F = F_in_eig;

  double tmpT[3];
  Eigen::Map<Eigen::Vector3d> tra_eig(tmpT);
  tra_eig = F.translation();

  double tmpR_mat[9];
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_eig(tmpR_mat);
  rot_eig = F.rotation();

  setT(tmpT);
  setR_mat(tmpR_mat);
  updateR_mat(); // for stability
}

void TranslationRotation3D::translateX(double Tx) { T_[0] += Tx; }

void TranslationRotation3D::translateY(double Ty) { T_[1] += Ty; }

void TranslationRotation3D::translateZ(double Tz) { T_[2] += Tz; }

void TranslationRotation3D::rotateX(double angle_deg) {
  double angle_rad = angle_deg * PI_ / 180.0;
  double c = cos(angle_rad);
  double s = sin(angle_rad);
  Eigen::Matrix3d Rx;
  Rx << 1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c;
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot(R_mat_);
  rot *= Rx;
  updateR();
  updateR_mat(); // for stability
}

void TranslationRotation3D::rotateY(double angle_deg) {
  double angle_rad = angle_deg * PI_ / 180.0;
  double c = cos(angle_rad);
  double s = sin(angle_rad);
  Eigen::Matrix3d Rx;
  Rx << c, 0.0, s, 0.0, 1.0, 0.0, -s, 0, c;
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot(R_mat_);
  rot *= Rx;
  updateR();
  updateR_mat(); // for stability
}

void TranslationRotation3D::rotateZ(double angle_deg) {
  double angle_rad = angle_deg * PI_ / 180.0;
  double c = cos(angle_rad);
  double s = sin(angle_rad);
  Eigen::Matrix3d Rx;
  Rx << c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0;
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot(R_mat_);
  rot *= Rx;
  updateR();
  updateR_mat(); // for stability
}

void TranslationRotation3D::show() const {
  printf("T =     %+2.6f %+2.6f %+2.6f\n", T_[0], T_[1], T_[2]);
  printf("R =     %+2.6f %+2.6f %+2.6f\n", R_[0], R_[1], R_[2]);
  printf("R_mat = %+2.6f %+2.6f %+2.6f\n", R_mat_[0], R_mat_[1], R_mat_[2]);
  printf("        %+2.6f %+2.6f %+2.6f\n", R_mat_[3], R_mat_[4], R_mat_[5]);
  printf("        %+2.6f %+2.6f %+2.6f\n", R_mat_[6], R_mat_[7], R_mat_[8]);
}

void TranslationRotation3D::showCompact() const {
  printf("T = %+2.6f %+2.6f %+2.6f | R = %+2.6f %+2.6f %+2.6f ", T_[0], T_[1],
         T_[2], R_[0], R_[1], R_[2]);
  if (isValid())
    printf("valid\n");
  else
    printf("invalid\n");
}

void TranslationRotation3D::updateR() {
  Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_mat(
      R_mat_);
  Eigen::Map<Eigen::Vector3d> rot_axis_angle(R_);
  Eigen::AngleAxis<double> tmp(rot_mat);
  rot_axis_angle = tmp.angle() * tmp.axis();
}

void TranslationRotation3D::updateR_mat() {

  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_mat(R_mat_);
  Eigen::Map<const Eigen::Vector3d> rot_axis_angle(R_);

  double angle = rot_axis_angle.norm();

  if (angle < 1e-15) {
    // identity matrix
    rot_mat = Eigen::Matrix<double, 3, 3>::Identity();
  } else {
    rot_mat = Eigen::AngleAxis<double>(angle, rot_axis_angle / angle)
                  .toRotationMatrix();
  }
}
}


/*
* Copyright 2019, Giorgio Zoppi
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*         http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
*        limitations under the License
*/
#include <string>
#include <vector>
#include <cstdlib>
#include <boost/date_time.hpp>
#include <amcl/mpin_BN254.h>
#include <bsd/stdlib.h>
#include <amcl/randapi.h>
#include <amcl/pbc_support.h>
#include <amcl/amcl.h>
#include <utils.h>
#include <secret_proxy.h>

namespace milagro
{
  namespace secure_store
  {

    using namespace BN254;
    using namespace boost::local_time;

      secret_proxy::secret_proxy ()
    {
      init_state ();
    }
    secret_proxy::secret_proxy (const std::string & path)
    {
      init_state ();

    }
    secret_proxy::~secret_proxy ()
    {
      KILL_CSPRNG (&_secure_random);
    }
    void secret_proxy::init_state ()
    {
      char raw[100];
      // FIXME this works only on linux and bsd
      arc4random_buf (raw, sizeof raw);
      amcl::octet row_octet =
      {
      0, sizeof (raw), raw};
      CREATE_CSPRNG (&_secure_random, &row_octet);
    }
    /*
     * Generate the D-TA master secret -
     * @param type  type of the store, optional parameter.
     */
    amcl::octet secret_proxy::generate_master_secret (store_type type)
    {
      char secret_chars[PGS_BN254];
      amcl::octet secret =
      {
      0, sizeof (secret_chars), secret_chars};
      _master_startTime = boost::posix_time::second_clock::local_time ();
      MPIN_RANDOM_GENERATE (&_secure_random, &secret);
      return secret;
    }
    /*
     *  Generate the client secret for the MPIN Protocol.
     *  One between the user_id or the hash_pin_id shall be present.
     *  If juse the user_id is present create the hash from the user_id.
     *  @param user_id optional value for the user identifier
     *  @param hash_pin_id optional value for the mpin
     *  @returns An octect to be used as a secret key
     */
    amcl::octet secret_proxy::generate_client_secret (const std::optional <
						      std::string > &user_id,
						      const std::optional <
						      std::string >
						      &hash_pin_id,
						      store_type type)
    {
      char client_id[256];
      char idhex[256];
      char token[2 * PFS_BN254 + 1];
      char hcid[PFS_BN254];
      amcl::octet HCID =
      {
      0, sizeof (hcid), hcid};
      amcl::octet CLIENT_ID =
      {
      0, sizeof (client_id), client_id};
      // compute when we shall expire.
      auto local = local_sec_clock::local_time (time_zone_ptr ());
      local += boost::gregorian::days (DAYS_CLIENT_SECRET_MAX);
      _client_expireTime = local.utc_time ();
      if (!user_id.has_value () && (!hash_pin_id.value ().size () == 64))
	{
	  throw std::invalid_argument ("Client identifier too long");
	}
      milagro::utils::make_hash_id (hash_pin_id, user_id, HCID);
      // we have here the hash id anyway.
      amcl::octet TOKEN =
      {
      0, sizeof (token), token};
      int currentSize = _master_secret.size ();
      amcl::octet S =
      {
      currentSize, currentSize,
	  const_cast < char *>(_master_secret.c_str ())};
      MPIN_GET_CLIENT_SECRET (&S, &HCID, &TOKEN);
      return TOKEN;
    }
    /*
     *  Get the time permits
     * @param mash_
     */
    std::vector < std::string >
      secret_proxy::get_time_permits (const
				      std::optional < std::string >
				      &mhash_pin_id, int count)
    {
      char permit[2 * PFS_BN254 + 1];
      int day = amcl::today ();
      std::vector < std::string > tmp;
      amcl::octet HCID;
      amcl::octet PERMIT =
      {
      0, sizeof (permit), permit};
      int currentSize = _master_secret.size ();
      amcl::octet S =
      {
      currentSize,
	  currentSize, const_cast < char *>(_master_secret.c_str ())};
      milagro::utils::make_hash_id (mhash_pin_id, std::nullopt, HCID);
      for (int i = 0; i < count; ++count)
	{
	  MPIN_GET_CLIENT_PERMIT (HASH_TYPE_BN254, day, &S, &HCID, &PERMIT);
	  // This encoding makes Time permit look random
	  if (MPIN_ENCODING (&_secure_random, &PERMIT) != 0)
	    {
	      throw std::runtime_error ("Encoding permit is not possible");
	    }
	  std::string str = milagro::utils::octet_to_string (PERMIT);
	  tmp.push_back (str);
	  std::memset (&PERMIT, 0, sizeof (amcl::octet));
	}
      return tmp;
    }
    /*
     *
     */
    boost::posix_time::ptime secret_proxy::client_key_expire ()const
    {
      return _client_expireTime;
    }
    /*
     *
     */
    boost::posix_time::ptime secret_proxy::server_key_start () const
    {
      return _master_startTime;
    }
    /*
     *
     */
    std::optional < amcl::octet > secret_proxy::search_key (int appId)
    {
      auto value = _key_store.find (appId);
      if (value != _key_store.end ())
	{
	  return std::make_optional < amcl::octet > (value->second);
	}
      return std::nullopt;

    }

  }
}


#ifndef BOOST_GEOMETRY_PROJECTIONS_LCCA_HPP
#define BOOST_GEOMETRY_PROJECTIONS_LCCA_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_mlfn.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct lcca {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace lcca
    {

            static const int MAX_ITER = 10;
            static const double DEL_TOL = 1e-12;

            template <typename T>
            struct par_lcca
            {
                T    en[EN_SIZE];
                T    r0, l, M0;
                T    C;
            };

            template <typename T> /* func to compute dr */
            inline T fS(T const& S, T const& C)
            {
                return(S * ( 1. + S * S * C));
            }

            template <typename T> /* deriv of fs */
            inline T fSp(T const& S, T const& C)
            {
                return(1. + 3.* S * S * C);
            }

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_lcca_ellipsoid : public base_t_fi<base_lcca_ellipsoid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_lcca<CalculationType> m_proj_parm;

                inline base_lcca_ellipsoid(const Parameters& par)
                    : base_t_fi<base_lcca_ellipsoid<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    CalculationType S, r, dr;

                    S = pj_mlfn(lp_lat, sin(lp_lat), cos(lp_lat), this->m_proj_parm.en) - this->m_proj_parm.M0;
                    dr = fS(S, this->m_proj_parm.C);
                    r = this->m_proj_parm.r0 - dr;
                    xy_x = this->m_par.k0 * (r * sin( lp_lon *= this->m_proj_parm.l ) );
                    xy_y = this->m_par.k0 * (this->m_proj_parm.r0 - r * cos(lp_lon) );
                }

                // INVERSE(e_inverse)  ellipsoid & spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    CalculationType theta, dr, S, dif;
                    int i;

                    xy_x /= this->m_par.k0;
                    xy_y /= this->m_par.k0;
                    theta = atan2(xy_x , this->m_proj_parm.r0 - xy_y);
                    dr = xy_y - xy_x * tan(0.5 * theta);
                    lp_lon = theta / this->m_proj_parm.l;
                    S = dr;
                    for (i = MAX_ITER; i ; --i) {
                        S -= (dif = (fS(S, this->m_proj_parm.C) - dr) / fSp(S, this->m_proj_parm.C));
                        if (fabs(dif) < DEL_TOL) break;
                    }
                    if (!i)
                        BOOST_THROW_EXCEPTION( projection_exception(-20) );
                    lp_lat = pj_inv_mlfn(S + this->m_proj_parm.M0, this->m_par.es, this->m_proj_parm.en);
                }

                static inline std::string get_name()
                {
                    return "lcca_ellipsoid";
                }

            };

            // Lambert Conformal Conic Alternative
            template <typename Parameters, typename T>
            inline void setup_lcca(Parameters& par, par_lcca<T>& proj_parm)
            {
                T s2p0, N0, R0, tan0, tan20;

                if (!pj_enfn(par.es, proj_parm.en))
                    BOOST_THROW_EXCEPTION( projection_exception(0) );
                if (!pj_param(par.params, "tlat_0").i)
                    BOOST_THROW_EXCEPTION( projection_exception(50) );
                if (par.phi0 == 0.)
                    BOOST_THROW_EXCEPTION( projection_exception(51) );
                proj_parm.l = sin(par.phi0);
                proj_parm.M0 = pj_mlfn(par.phi0, proj_parm.l, cos(par.phi0), proj_parm.en);
                s2p0 = proj_parm.l * proj_parm.l;
                R0 = 1. / (1. - par.es * s2p0);
                N0 = sqrt(R0);
                R0 *= par.one_es * N0;
                tan0 = tan(par.phi0);
                tan20 = tan0 * tan0;
                proj_parm.r0 = N0 / tan0;
                proj_parm.C = 1. / (6. * R0 * N0);
                boost::ignore_unused(tan20);
            }

    }} // namespace detail::lcca
    #endif // doxygen

    /*!
        \brief Lambert Conformal Conic Alternative projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Conic
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin
        \par Example
        \image html ex_lcca.gif
    */
    template <typename CalculationType, typename Parameters>
    struct lcca_ellipsoid : public detail::lcca::base_lcca_ellipsoid<CalculationType, Parameters>
    {
        inline lcca_ellipsoid(const Parameters& par) : detail::lcca::base_lcca_ellipsoid<CalculationType, Parameters>(par)
        {
            detail::lcca::setup_lcca(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::lcca, lcca_ellipsoid, lcca_ellipsoid)

        // Factory entry(s)
        template <typename CalculationType, typename Parameters>
        class lcca_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<lcca_ellipsoid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void lcca_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("lcca", new lcca_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_LCCA_HPP


//==============================================================================
//         Copyright 2014 - Jean-Thierry Lapresté
//         Copyright 2003 - 2013   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_LU_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_FACTORIZATIONS_LU_HPP_INCLUDED

#include <nt2/linalg/functions/lu.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/function.hpp>
#include <nt2/include/functions/getrf.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/numel.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/triu.hpp>
#include <nt2/include/functions/tri1l.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/core/container/colon/colon.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <nt2/core/utility/assign_swap.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/error/warning.hpp>
#include <nt2/linalg/options.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/core/ignore_unused.hpp>
#include <algorithm>

///Utilitary macro
/// INTERNAL ONLY undefined at end of file
#define CHECK_LAPACK_LU_SUCCESS(info)                        \
  {                                                          \
    nt2_la_int info_ = info;                                 \
    boost::ignore_unused(info_);                   \
    NT2_WARNING(info_ <= 0                                   \
               , "LU factorization has been completed, "     \
                "but input matrix is exactly singular. "     \
                "Division by zero will occur if it is used " \
                "to solve a system of equations."            \
               );                                            \
  }                                                          \
  /**/

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

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

  //============================================================================
  //lu Scalar
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( lu_, 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;
    }
  };

  //============================================================================
  //lu
  //============================================================================
  /// INTERNAL ONLY
  BOOST_DISPATCH_IMPLEMENT  ( lu_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_ < A0, nt2::tag::lu_
                                      , 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 nt2::memory::container<tag::table_, type_t, nt2::_2D>        o_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      eval(a0, a1, N0(), N1());
    }
  private:
    /// INTERNAL ONLY - y =  lu(a)
    /// y is the direct lapack return from ?getrf and permutation is lost
    /// only call for which raw_ is the default
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const& // # inputs
              , boost::mpl::long_<1> const& // # outputs
              ) const
    {
      //1_1 means 1 input 1 output
      eval1_1(a0, a1
             , nt2::policy<ext::raw_>());
    }

    /// INTERNAL ONLY - y =  lu(a, raw_)
    /// y is the direct lapack return from ?getrf, and permutation is lost
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const& // # inputs
              , boost::mpl::long_<1> const& // # outputs
              ) const
    {
      // 1_2 means 1 input 2 outputs
      eval1_2(a0, a1
             , nt2::policy<ext::raw_>());
    }

    /// INTERNAL ONLY - [y, ls] =  lu(a, raw_)
    /// y is the direct lapack return from ?getrf
    /// permutation ls is in lapack swap lines indices form
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const& // # inputs
              , boost::mpl::long_<2> const& // # outputs
              ) const
    {
      // 2_2 means 2 input 2 outputs
      eval1_2(a0, a1
             , nt2::policy<ext::raw_>());
    }

    /// INTERNAL ONLY - [pl, u] =  lu(a)
    /// pl is a permuted lower triangular matrix
    //  u  is an upper triangular matrix
    /// pl*u ==  a
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const& // # inputs
              , boost::mpl::long_<2> const& // # outputs
              ) const
    {
      // 1_2 means 1 input 2 outputs
      eval1_2(a0, a1
             , nt2::policy<ext::matrix_>());
    }

    /// INTERNAL ONLY - [l, u, p] =  lu(a)
    /// pl is a permuted lower triangular matrix
    //  u  is an upper triangular matrix
    /// pl*u ==  a
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const& // # inputs
              , boost::mpl::long_<3> const& // # outputs
              ) const
    {
      // 2_3 means 2 input 3 outputs
      eval2_3(a0, a1
             , nt2::policy<ext::matrix_>());
    }

    /// INTERNAL ONLY - [l, u, p] =  lu(a, vector_/matrix_)
    /// pl is a permuted lower triangular matrix
    //  u  is an upper triangular matrix
    /// pl*u ==  a
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const& // # inputs
              , boost::mpl::long_<3> const& // # outputs
              ) const
    {
      // 2_3 means 2 input 3 outputs
      eval2_3(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0)));
    }

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

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 1i 1o raw_
    BOOST_FORCEINLINE
    void eval1_1 ( A0& a0, A1& a1
                   , const nt2::policy<ext::raw_>&
                   ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic
                           , lu, boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      nt2::container::table<nt2_la_int> ls(of_size(dim(lu), 1));
      CHECK_LAPACK_LU_SUCCESS((nt2::getrf( boost::proto::value(lu)
                                         , boost::proto::value(ls))));
      assign_swap(boost::proto::child_c<0>(a1), lu);
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 1i 2o raw_
    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                   , const nt2::policy<ext::raw_>&
                   ) const
    {
      typedef nt2::memory::container<tag::table_, nt2_la_int, nt2::_2D> i_semantic;
      NT2_AS_TERMINAL_INOUT(o_semantic
                           , lu, boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT(i_semantic, ls
                         , boost::proto::child_c<1>(a1));

      ls.resize(of_size(dim(lu), 1));
      CHECK_LAPACK_LU_SUCCESS(nt2::getrf( boost::proto::value(lu)
                                        , boost::proto::value(ls)));
      assign_swap(boost::proto::child_c<0>(a1), lu);
      assign_swap(boost::proto::child_c<1>(a1), ls);
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 1i 2o T other from raw_
    template < class T >
    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                   , const T &
                   ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, lu
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<1>(a1));
      std::size_t d  = dim(lu);
      nt2::container::table<nt2_la_int> ls(of_size(d, 1));
      CHECK_LAPACK_LU_SUCCESS(nt2::getrf( boost::proto::value(lu)
                                        , boost::proto::value(ls)));
      nt2::container::table<nt2_la_int> ip;
      construct_ip(ls, ip, height(lu));
      boost::proto::child_c<0>(a1) = nt2::tri1l(lu(nt2::_, nt2::_(1, d) ) )(ip, nt2::_);
      boost::proto::child_c<1>(a1) = nt2::triu(lu( nt2::_(1, d), nt2::_) );
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 3o
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                   , const nt2::policy<ext::vector_>&
                   ) const
    {
      typedef typename boost::proto::result_of::child_c<A1&,2>::value_type     child;
      typedef typename child::value_type                                     itype_t;
      NT2_AS_TERMINAL_INOUT(o_semantic, lu
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<1>(a1));
      std::size_t d  = dim(lu);
      nt2::container::table<nt2_la_int> ls(of_size(d, 1));
      CHECK_LAPACK_LU_SUCCESS(nt2::getrf( boost::proto::value(lu)
                                        , boost::proto::value(ls)));
      nt2::container::table<itype_t> ip;
      construct_ip(ls, ip, height(lu));
      boost::proto::child_c<0>(a1) = nt2::tri1l(lu(nt2::_, nt2::_(1, d) ) );
      boost::proto::child_c<1>(a1) = nt2::triu(lu( nt2::_(1, d), nt2::_) );
      inverse(ip);
      assign_swap(boost::proto::child_c<2>(a1), ip);
    }

    ///////////////////////////////////////////////////////////////////////////////
    /// INTERNAL ONLY: 2i 3o
    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, lu
                           , boost::proto::child_c<0>(a0), work);
      std::size_t d  = dim(lu);
      nt2::container::table<nt2_la_int> ls(of_size(d, 1)), ip;
      CHECK_LAPACK_LU_SUCCESS(nt2::getrf( boost::proto::value(lu)
                                        , boost::proto::value(ls)));
      construct_ip(ls, ip, height(lu));
      boost::proto::child_c<1>(a1) = nt2::triu(lu( nt2::_(1, d), nt2::_) );
      boost::proto::child_c<0>(a1) = nt2::tri1l(lu(nt2::_, nt2::_(1, d) ) );
      boost::proto::child_c<2>(a1) = eye(height(lu), nt2::meta::as_<rtype_t>())(nt2::_, ip);
    }

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

    /// INTERNAL ONLY
    /// construct permutation vector from lapack swap vector
    template < class T1, class T2>
    BOOST_FORCEINLINE
    void construct_ip(T1& ls, T2& ips, size_t h) const
    {
      std::size_t d = nt2::numel(ls);
      ips = nt2::_(nt2_la_int(1), nt2_la_int(h));
      for(std::size_t i = d; i >= 1; --i)
      {
        std::swap(ips(i), ips(ls(i)));
      }
    }

    template < class T1>
    BOOST_FORCEINLINE
    void inverse(T1& ip) const
    {
      T1 inv(of_size(1, numel(ip)));
      inv(ip) =  nt2::_(nt2_la_int(1), nt2_la_int(numel(ip)));
      assign_swap(ip, inv);
    }
  };

} }

#undef CHECK_LAPACK_LU_SUCCESS

#endif