//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Copyright 2004, 2005 Trustees of Indiana University
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek,
//          Doug Gregor, D. Kevin McGrath
//
// Distributed under 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)
//=======================================================================//
#ifndef BOOST_GRAPH_KING_HPP
#define BOOST_GRAPH_KING_HPP

#include <boost/config.hpp>
#include <boost/graph/detail/sparse_ordering.hpp>
#include <boost/graph/graph_utility.hpp>

/*
  King Algorithm for matrix reordering
*/

namespace boost {
  namespace detail {
    template<typename OutputIterator, typename Buffer, typename Compare,
             typename PseudoDegreeMap, typename VecMap, typename VertexIndexMap>
    class bfs_king_visitor:public default_bfs_visitor
    {
    public:
      bfs_king_visitor(OutputIterator *iter, Buffer *b, Compare compare,
                       PseudoDegreeMap deg, std::vector<int> loc, VecMap color,
                       VertexIndexMap vertices):
        permutation(iter), Qptr(b), degree(deg), comp(compare),
        Qlocation(loc), colors(color), vertex_map(vertices) { }

      template <typename Vertex, typename Graph>
      void finish_vertex(Vertex, Graph& g) {
        typename graph_traits<Graph>::out_edge_iterator ei, ei_end;
        Vertex v, w;

        typedef typename std::deque<Vertex>::reverse_iterator reverse_iterator;

        reverse_iterator rend = Qptr->rend()-index_begin;
        reverse_iterator rbegin = Qptr->rbegin();

        //heap the vertices already there
        std::make_heap(rbegin, rend, boost::bind<bool>(comp, _2, _1));

        unsigned i = 0;

        for(i = index_begin; i != Qptr->size(); ++i){
          colors[get(vertex_map, (*Qptr)[i])] = 1;
          Qlocation[get(vertex_map, (*Qptr)[i])] = i;
        }

        i = 0;

        for( ; rbegin != rend; rend--){
          percolate_down<Vertex>(i);
          w = (*Qptr)[index_begin+i];
          for (boost::tie(ei, ei_end) = out_edges(w, g); ei != ei_end; ++ei) {
            v = target(*ei, g);
            put(degree, v, get(degree, v) - 1);

            if (colors[get(vertex_map, v)] == 1) {
              percolate_up<Vertex>(get(vertex_map, v), i);
            }
          }

          colors[get(vertex_map, w)] = 0;
          i++;
        }
      }

      template <typename Vertex, typename Graph>
      void examine_vertex(Vertex u, const Graph&) {

        *(*permutation)++ = u;
        index_begin = Qptr->size();

      }
    protected:

      //this function replaces pop_heap, and tracks state information
      template <typename Vertex>
      void percolate_down(int offset){
        int heap_last = index_begin + offset;
        int heap_first = Qptr->size() - 1;

        //pop_heap functionality:
        //swap first, last
        std::swap((*Qptr)[heap_last], (*Qptr)[heap_first]);

        //swap in the location queue
        std::swap(Qlocation[heap_first], Qlocation[heap_last]);

        //set drifter, children
        int drifter = heap_first;
        int drifter_heap = Qptr->size() - drifter;

        int right_child_heap = drifter_heap * 2 + 1;
        int right_child = Qptr->size() - right_child_heap;

        int left_child_heap = drifter_heap * 2;
        int left_child = Qptr->size() - left_child_heap;

        //check that we are staying in the heap
        bool valid = (right_child < heap_last) ? false : true;

        //pick smallest child of drifter, and keep in mind there might only be left child
        int smallest_child = (valid && get(degree, (*Qptr)[left_child]) > get(degree,(*Qptr)[right_child])) ?
          right_child : left_child;

        while(valid && smallest_child < heap_last && comp((*Qptr)[drifter], (*Qptr)[smallest_child])){

          //if smallest child smaller than drifter, swap them
          std::swap((*Qptr)[smallest_child], (*Qptr)[drifter]);
          std::swap(Qlocation[drifter], Qlocation[smallest_child]);

          //update the values, run again, as necessary
          drifter = smallest_child;
          drifter_heap = Qptr->size() - drifter;

          right_child_heap = drifter_heap * 2 + 1;
          right_child = Qptr->size() - right_child_heap;

          left_child_heap = drifter_heap * 2;
          left_child = Qptr->size() - left_child_heap;

          valid = (right_child < heap_last) ? false : true;

          smallest_child = (valid && get(degree, (*Qptr)[left_child]) > get(degree,(*Qptr)[right_child])) ?
            right_child : left_child;
        }

      }

      // this is like percolate down, but we always compare against the
      // parent, as there is only a single choice
      template <typename Vertex>
      void percolate_up(int vertex, int offset){

        int child_location = Qlocation[vertex];
        int heap_child_location = Qptr->size() - child_location;
        int heap_parent_location = (int)(heap_child_location/2);
        unsigned parent_location = Qptr->size() - heap_parent_location;

        bool valid = (heap_parent_location != 0 && child_location > index_begin + offset &&
                      parent_location < Qptr->size());

        while(valid && comp((*Qptr)[child_location], (*Qptr)[parent_location])){

          //swap in the heap
          std::swap((*Qptr)[child_location], (*Qptr)[parent_location]);

          //swap in the location queue
          std::swap(Qlocation[child_location], Qlocation[parent_location]);

          child_location = parent_location;
          heap_child_location = heap_parent_location;
          heap_parent_location = (int)(heap_child_location/2);
          parent_location = Qptr->size() - heap_parent_location;
          valid = (heap_parent_location != 0 && child_location > index_begin + offset);
        }
      }

      OutputIterator *permutation;
      int index_begin;
      Buffer *Qptr;
      PseudoDegreeMap degree;
      Compare comp;
      std::vector<int> Qlocation;
      VecMap colors;
      VertexIndexMap vertex_map;
    };

  } // namespace detail

  template<class Graph, class OutputIterator, class ColorMap, class DegreeMap,
           typename VertexIndexMap>
  OutputIterator
  king_ordering(const Graph& g,
                std::deque< typename graph_traits<Graph>::vertex_descriptor >
                  vertex_queue,
                OutputIterator permutation,
                ColorMap color, DegreeMap degree,
                VertexIndexMap index_map)
  {
    typedef typename property_traits<DegreeMap>::value_type ds_type;
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef iterator_property_map<typename std::vector<ds_type>::iterator, VertexIndexMap, ds_type, ds_type&> PseudoDegreeMap;
    typedef indirect_cmp<PseudoDegreeMap, std::less<ds_type> > Compare;
    typedef typename boost::sparse::sparse_ordering_queue<Vertex> queue;
    typedef typename detail::bfs_king_visitor<OutputIterator, queue, Compare,
      PseudoDegreeMap, std::vector<int>, VertexIndexMap > Visitor;
    typedef typename graph_traits<Graph>::vertices_size_type
      vertices_size_type;
    std::vector<ds_type> pseudo_degree_vec(num_vertices(g));
    PseudoDegreeMap pseudo_degree(pseudo_degree_vec.begin(), index_map);

    typename graph_traits<Graph>::vertex_iterator ui, ui_end;
    queue Q;
    // Copy degree to pseudo_degree
    // initialize the color map
    for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui){
      put(pseudo_degree, *ui, get(degree, *ui));
      put(color, *ui, Color::white());
    }

    Compare comp(pseudo_degree);
    std::vector<int> colors(num_vertices(g));

    for(vertices_size_type i = 0; i < num_vertices(g); i++)
      colors[i] = 0;

    std::vector<int> loc(num_vertices(g));

    //create the visitor
    Visitor vis(&permutation, &Q, comp, pseudo_degree, loc, colors, index_map);

    while( !vertex_queue.empty() ) {
      Vertex s = vertex_queue.front();
      vertex_queue.pop_front();

      //call BFS with visitor
      breadth_first_visit(g, s, Q, vis, color);
    }

    return permutation;
  }

  // This is the case where only a single starting vertex is supplied.
  template <class Graph, class OutputIterator,
            class ColorMap, class DegreeMap, typename VertexIndexMap>
  OutputIterator
  king_ordering(const Graph& g,
                typename graph_traits<Graph>::vertex_descriptor s,
                OutputIterator permutation,
                ColorMap color, DegreeMap degree, VertexIndexMap index_map)
  {

    std::deque< typename graph_traits<Graph>::vertex_descriptor > vertex_queue;
    vertex_queue.push_front( s );
    return king_ordering(g, vertex_queue, permutation, color, degree,
                         index_map);
  }

  template < class Graph, class OutputIterator,
             class ColorMap, class DegreeMap, class VertexIndexMap>
  OutputIterator
  king_ordering(const Graph& G, OutputIterator permutation,
                ColorMap color, DegreeMap degree, VertexIndexMap index_map)
  {
    if (has_no_vertices(G))
      return permutation;

    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;

    std::deque<Vertex>      vertex_queue;

    // Mark everything white
    BGL_FORALL_VERTICES_T(v, G, Graph) put(color, v, Color::white());

    // Find one vertex from each connected component
    BGL_FORALL_VERTICES_T(v, G, Graph) {
      if (get(color, v) == Color::white()) {
        depth_first_visit(G, v, dfs_visitor<>(), color);
        vertex_queue.push_back(v);
      }
    }

    // Find starting nodes for all vertices
    // TBD: How to do this with a directed graph?
    for (typename std::deque<Vertex>::iterator i = vertex_queue.begin();
         i != vertex_queue.end(); ++i)
      *i = find_starting_node(G, *i, color, degree);

    return king_ordering(G, vertex_queue, permutation, color, degree,
                         index_map);
  }

  template<typename Graph, typename OutputIterator, typename VertexIndexMap>
  OutputIterator
  king_ordering(const Graph& G, OutputIterator permutation,
                VertexIndexMap index_map)
  {
    if (has_no_vertices(G))
      return permutation;

    std::vector<default_color_type> colors(num_vertices(G));
    return king_ordering(G, permutation,
                         make_iterator_property_map(&colors[0], index_map,
                                                    colors[0]),
                         make_out_degree_map(G), index_map);
  }

  template<typename Graph, typename OutputIterator>
  inline OutputIterator
  king_ordering(const Graph& G, OutputIterator permutation)
  { return king_ordering(G, permutation, get(vertex_index, G)); }

} // namespace boost

#endif // BOOST_GRAPH_KING_HPP


// These headers are deprecated.
//
// #include <boost/math/common_factor_ct.hpp>
// #include <boost/math/common_factor.hpp>
// #include <boost/math/common_factor_rt.hpp>

#include <boost/cstdfloat.hpp>
#include <boost/math/complex.hpp>
#include <boost/math/distributions.hpp>
#include <boost/math_fwd.hpp>
#include <boost/math/octonion.hpp>
#include <boost/math/quaternion.hpp>
#include <boost/math/special_functions.hpp>

// This header is used by the other drivers.
//
// #include <boost/math/tr1.hpp>

int
main ()
{
  return boost::math::prime (0) == 2 && boost::math::prime (9) == 29 ? 0 : 1;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file NoiseModel.cpp
 * @date Jan 13, 2010
 * @author Richard Roberts
 * @author Frank Dellaert
 */

#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/timing.h>

#include <boost/format.hpp>
#include <boost/make_shared.hpp>

#include <cmath>
#include <iostream>
#include <limits>
#include <stdexcept>
#include <typeinfo>

using namespace std;

namespace gtsam {
namespace noiseModel {

/* ************************************************************************* */
// update A, b
// A' \define A_{S}-ar and b'\define b-ad
// Linear algebra: takes away projection on latest orthogonal
// Graph: make a new factor on the separator S
// __attribute__ ((noinline))  // uncomment to prevent inlining when profiling
template<class MATRIX>
void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
  size_t n = Ab.cols()-1;
  Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
}

/* ************************************************************************* */
// check *above the diagonal* for non-zero entries
boost::optional<Vector> checkIfDiagonal(const Matrix M) {
  size_t m = M.rows(), n = M.cols();
  // check all non-diagonal entries
  bool full = false;
  size_t i, j;
  for (i = 0; i < m; i++)
    if (!full)
      for (j = i + 1; j < n; j++)
        if (std::abs(M(i, j)) > 1e-9) {
          full = true;
          break;
        }
  if (full) {
    return boost::none;
  } else {
    Vector diagonal(n);
    for (j = 0; j < n; j++)
      diagonal(j) = M(j, j);
    return diagonal;
  }
}

/* ************************************************************************* */
Vector Base::sigmas() const {
  throw("Base::sigmas: sigmas() not implemented for this noise model");
}

/* ************************************************************************* */
double Base::squaredMahalanobisDistance(const Vector& v) const {
  // Note: for Diagonal, which does ediv_, will be correct for constraints
  Vector w = whiten(v);
  return w.dot(w);
}

/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
  size_t m = R.rows(), n = R.cols();
  if (m != n)
    throw invalid_argument("Gaussian::SqrtInformation: R not square");
  if (smart) {
    boost::optional<Vector> diagonal = checkIfDiagonal(R);
    if (diagonal)
      return Diagonal::Sigmas(diagonal->array().inverse(), true);
  }
  // NOTE(frank): only reaches here if !(smart && diagonal)
  return shared_ptr(new Gaussian(R.rows(), R));
}

/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart) {
  size_t m = information.rows(), n = information.cols();
  if (m != n)
    throw invalid_argument("Gaussian::Information: R not square");
  boost::optional<Vector> diagonal = boost::none;
  if (smart)
    diagonal = checkIfDiagonal(information);
  if (diagonal)
    return Diagonal::Precisions(*diagonal, true);
  else {
    Eigen::LLT<Matrix> llt(information);
    Matrix R = llt.matrixU();
    return shared_ptr(new Gaussian(n, R));
  }
}

/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
    bool smart) {
  size_t m = covariance.rows(), n = covariance.cols();
  if (m != n)
    throw invalid_argument("Gaussian::Covariance: covariance not square");
  boost::optional<Vector> variances = boost::none;
  if (smart)
    variances = checkIfDiagonal(covariance);
  if (variances)
    return Diagonal::Variances(*variances, true);
  else {
    // NOTE: if cov = L'*L, then the square root information R can be found by
    // QR, as L.inverse() = Q*R, with Q some rotation matrix. However, R has
    // annoying sign flips with respect the simpler Information(inv(cov)),
    // hence we choose the simpler path here:
    return Information(covariance.inverse(), false);
  }
}

/* ************************************************************************* */
void Gaussian::print(const string& name) const {
  gtsam::print(thisR(), name + "Gaussian");
}

/* ************************************************************************* */
bool Gaussian::equals(const Base& expected, double tol) const {
  const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
  if (p == nullptr) return false;
  if (typeid(*this) != typeid(*p)) return false;
  return equal_with_abs_tol(R(), p->R(), sqrt(tol));
}

/* ************************************************************************* */
Matrix Gaussian::covariance() const {
  // Uses a fast version of `covariance = information().inverse();`
  const Matrix& R = this->R();
  Matrix I = Matrix::Identity(R.rows(), R.cols());
  // Fast inverse of upper-triangular matrix R using forward-substitution
  Matrix Rinv = R.triangularView<Eigen::Upper>().solve(I);
  // (R' * R)^{-1} = R^{-1} * R^{-1}'
  return Rinv * Rinv.transpose();
}

/* ************************************************************************* */
Vector Gaussian::sigmas() const {
  return Vector(covariance().diagonal()).cwiseSqrt();
}

/* ************************************************************************* */
Vector Gaussian::whiten(const Vector& v) const {
  return thisR() * v;
}

/* ************************************************************************* */
Vector Gaussian::unwhiten(const Vector& v) const {
  return backSubstituteUpper(thisR(), v);
}

/* ************************************************************************* */
Matrix Gaussian::Whiten(const Matrix& H) const {
  return thisR() * H;
}

/* ************************************************************************* */
void Gaussian::WhitenInPlace(Matrix& H) const {
  H = thisR() * H;
}

/* ************************************************************************* */
void Gaussian::WhitenInPlace(Eigen::Block<Matrix> H) const {
  H = thisR() * H;
}

/* ************************************************************************* */
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QR(Matrix& Ab) const {

  gttic(Gaussian_noise_model_QR);

  static const bool debug = false;

  // get size(A) and maxRank
  // TODO: really no rank problems ?
   size_t m = Ab.rows(), n = Ab.cols()-1;
   size_t maxRank = min(m,n);

  // pre-whiten everything (cheaply if possible)
  WhitenInPlace(Ab);

  if(debug) gtsam::print(Ab, "Whitened Ab: ");

  // Eigen QR - much faster than older householder approach
  inplace_QR(Ab);
  Ab.triangularView<Eigen::StrictlyLower>().setZero();

  // hand-coded householder implementation
  // TODO: necessary to isolate last column?
  // householder(Ab, maxRank);

  return noiseModel::Unit::Create(maxRank);
}

void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
  for(Matrix& Aj: A) { WhitenInPlace(Aj); }
  whitenInPlace(b);
}

void Gaussian::WhitenSystem(Matrix& A, Vector& b) const {
  WhitenInPlace(A);
  whitenInPlace(b);
}

void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const {
  WhitenInPlace(A1);
  WhitenInPlace(A2);
  whitenInPlace(b);
}

void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
  WhitenInPlace(A1);
  WhitenInPlace(A2);
  WhitenInPlace(A3);
  whitenInPlace(b);
}

/* ************************************************************************* */
// Diagonal
/* ************************************************************************* */
Diagonal::Diagonal() :
    Gaussian(1) // TODO: Frank asks: really sure about this?
{
}

/* ************************************************************************* */
Diagonal::Diagonal(const Vector& sigmas)
    : Gaussian(sigmas.size()),
      sigmas_(sigmas),
      invsigmas_(sigmas.array().inverse()),
      precisions_(invsigmas_.array().square()) {
}

/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
  if (smart) {
    // check whether all the same entry
    size_t n = variances.size();
    for (size_t j = 1; j < n; j++)
      if (variances(j) != variances(0)) goto full;
    return Isotropic::Variance(n, variances(0), true);
  }
  full: return shared_ptr(new Diagonal(variances.cwiseSqrt()));
}

/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
  if (smart) {
    size_t n = sigmas.size();
    if (n==0) goto full;
    // look for zeros to make a constraint
    for (size_t j=0; j< n; ++j)
      if (sigmas(j)<1e-8)
        return Constrained::MixedSigmas(sigmas);
    // check whether all the same entry
    for (size_t j = 1; j < n; j++)
      if (sigmas(j) != sigmas(0)) goto full;
    return Isotropic::Sigma(n, sigmas(0), true);
  }
  full: return Diagonal::shared_ptr(new Diagonal(sigmas));
}

/* ************************************************************************* */
void Diagonal::print(const string& name) const {
  gtsam::print(sigmas_, name + "diagonal sigmas");
}

/* ************************************************************************* */
Vector Diagonal::whiten(const Vector& v) const {
  return v.cwiseProduct(invsigmas_);
}

/* ************************************************************************* */
Vector Diagonal::unwhiten(const Vector& v) const {
  return v.cwiseProduct(sigmas_);
}

/* ************************************************************************* */
Matrix Diagonal::Whiten(const Matrix& H) const {
  return vector_scale(invsigmas(), H);
}

/* ************************************************************************* */
void Diagonal::WhitenInPlace(Matrix& H) const {
  vector_scale_inplace(invsigmas(), H);
}

/* ************************************************************************* */
void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
  H = invsigmas().asDiagonal() * H;
}

/* ************************************************************************* */
// Constrained
/* ************************************************************************* */

namespace internal {
// switch precisions and invsigmas to finite value
// TODO: why?? And, why not just ask s==0.0 below ?
static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
  for (Vector::Index i = 0; i < sigmas.size(); ++i)
    if (!std::isfinite(1. / sigmas[i])) {
      precisions[i] = 0.0;
      invsigmas[i] = 0.0;
    }
}
}

/* ************************************************************************* */
Constrained::Constrained(const Vector& sigmas)
  : Diagonal(sigmas), mu_(Vector::Constant(sigmas.size(), 1000.0)) {
  internal::fix(sigmas, precisions_, invsigmas_);
}

/* ************************************************************************* */
Constrained::Constrained(const Vector& mu, const Vector& sigmas)
  : Diagonal(sigmas), mu_(mu) {
  internal::fix(sigmas, precisions_, invsigmas_);
}

/* ************************************************************************* */
Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu,
    const Vector& sigmas) {
  return shared_ptr(new Constrained(mu, sigmas));
}

/* ************************************************************************* */
bool Constrained::constrained(size_t i) const {
  // TODO why not just check sigmas_[i]==0.0 ?
  return !std::isfinite(1./sigmas_[i]);
}

/* ************************************************************************* */
void Constrained::print(const std::string& name) const {
  gtsam::print(sigmas_, name + "constrained sigmas");
  gtsam::print(mu_, name + "constrained mu");
}

/* ************************************************************************* */
Vector Constrained::whiten(const Vector& v) const {
  // If sigmas[i] is not 0 then divide v[i] by sigmas[i], as usually done in
  // other normal Gaussian noise model. Otherwise, sigmas[i] = 0 indicating
  // a hard constraint, we don't do anything.
  const Vector& a = v;
  const Vector& b = sigmas_;
  size_t n = a.size();
  assert (b.size()==a.size());
  Vector c(n);
  for( size_t i = 0; i < n; i++ ) {
    const double& ai = a(i), bi = b(i);
    c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
  }
  return c;
}

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/* ************************************************************************* */
double Constrained::error(const Vector& v) const {
  Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
  for (size_t i=0; i<dim_; ++i)  // add mu weights on constrained variables
    if (constrained(i)) // whiten makes constrained variables zero
      w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
  return 0.5 * w.dot(w);
}
#endif

/* ************************************************************************* */
double Constrained::squaredMahalanobisDistance(const Vector& v) const {
  Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
  for (size_t i=0; i<dim_; ++i)  // add mu weights on constrained variables
    if (constrained(i)) // whiten makes constrained variables zero
      w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
  return w.dot(w);
}

/* ************************************************************************* */
Matrix Constrained::Whiten(const Matrix& H) const {
  Matrix A = H;
  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
    if (!constrained(i)) // if constrained, leave row of A as is
      A.row(i) *= invsigmas_(i);
  return A;
}

/* ************************************************************************* */
void Constrained::WhitenInPlace(Matrix& H) const {
  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
    if (!constrained(i)) // if constrained, leave row of H as is
      H.row(i) *= invsigmas_(i);
}

/* ************************************************************************* */
void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
    if (!constrained(i)) // if constrained, leave row of H as is
      H.row(i) *= invsigmas_(i);
}

/* ************************************************************************* */
Constrained::shared_ptr Constrained::unit() const {
  Vector sigmas = Vector::Ones(dim());
  for (size_t i=0; i<dim(); ++i)
    if (constrained(i))
      sigmas(i) = 0.0;
  return MixedSigmas(mu_, sigmas);
}

/* ************************************************************************* */
// Special version of QR for Constrained calls slower but smarter code
// that deals with possibly zero sigmas
// It is Gram-Schmidt orthogonalization rather than Householder

// Check whether column a triggers a constraint and corresponding variable is deterministic
// Return constraint_row with maximum element in case variable plays in multiple constraints
template <typename VECTOR>
boost::optional<size_t> check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) {
  boost::optional<size_t> constraint_row;
  // not zero, so roundoff errors will not be counted
  // TODO(frank): that's a fairly crude way of dealing with roundoff errors :-(
  double max_element = 1e-9;
  for (size_t i = 0; i < m; i++) {
    if (!std::isinf(invsigmas[i]))
      continue;
    double abs_ai = std::abs(a(i,0));
    if (abs_ai > max_element) {
      max_element = abs_ai;
      constraint_row.reset(i);
    }
  }
  return constraint_row;
}

SharedDiagonal Constrained::QR(Matrix& Ab) const {
  static const double kInfinity = std::numeric_limits<double>::infinity();

  // get size(A) and maxRank
  size_t m = Ab.rows();
  const size_t n = Ab.cols() - 1;
  const size_t maxRank = min(m, n);

  // create storage for [R d]
  typedef boost::tuple<size_t, Matrix, double> Triple;
  list<Triple> Rd;

  Matrix rd(1, n + 1);  // and for row of R
  Vector invsigmas = sigmas_.array().inverse();
  Vector weights = invsigmas.array().square();  // calculate weights once

  // We loop over all columns, because the columns that can be eliminated
  // are not necessarily contiguous. For each one, estimate the corresponding
  // scalar variable x as d-rS, with S the separator (remaining columns).
  // Then update A and b by substituting x with d-rS, zero-ing out x's column.
  for (size_t j = 0; j < n; ++j) {
    // extract the first column of A
    Eigen::Block<Matrix> a = Ab.block(0, j, m, 1);

    // Check whether we need to handle as a constraint
    boost::optional<size_t> constraint_row = check_if_constraint(a, invsigmas, m);

    if (constraint_row) {
      // Handle this as a constraint, as the i^th row has zero sigma with non-zero entry A(i,j)

      // In this case, the row in [R|d] is simply the row in [A|b]
      // NOTE(frank): we used to divide by a[i] but there is no need with a constraint
      rd = Ab.row(*constraint_row);

      // Construct solution (r, d, sigma)
      Rd.push_back(boost::make_tuple(j, rd, kInfinity));

      // exit after rank exhausted
      if (Rd.size() >= maxRank)
        break;

      // The constraint row will be zeroed out, so we can save work by swapping in the
      // last valid row and decreasing m. This will save work on subsequent down-dates, too.
      m -= 1;
      if (*constraint_row != m) {
        Ab.row(*constraint_row) = Ab.row(m);
        weights(*constraint_row) = weights(m);
        invsigmas(*constraint_row) = invsigmas(m);
      }

      // get a reduced a-column which is now shorter
      Eigen::Block<Matrix> a_reduced = Ab.block(0, j, m, 1);
      a_reduced *= (1.0/rd(0, j)); // NOTE(frank): this is the 1/a[i] = 1/rd(0,j) factor we need!

      // Rank-1 down-date of Ab, expensive, using outer product
      Ab.block(0, j + 1, m, n - j).noalias() -= a_reduced * rd.middleCols(j + 1, n - j);
    } else {
      // Treat in normal Gram-Schmidt way
      // Calculate weighted pseudo-inverse and corresponding precision

      // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)
      // For diagonal Sigma, inv(Sigma) = diag(precisions)
      double precision = 0;
      Vector pseudo(m);     // allocate storage for pseudo-inverse
      for (size_t i = 0; i < m; i++) {
        double ai = a(i, 0);
        if (std::abs(ai) > 1e-9) {  // also catches remaining sigma==0 rows
          pseudo[i] = weights[i] * ai;
          precision += pseudo[i] * ai;
        } else
          pseudo[i] = 0;
      }

      if (precision > 1e-8) {
        pseudo /= precision;

        // create solution [r d], rhs is automatically r(n)
        rd(0, j) = 1.0;  // put 1 on diagonal
        rd.block(0, j + 1, 1, n - j) = pseudo.transpose() * Ab.block(0, j + 1, m, n - j);

        // construct solution (r, d, sigma)
        Rd.push_back(boost::make_tuple(j, rd, precision));
      } else {
        // If precision is zero, no information on this column
        // This is actually not limited to constraints, could happen in Gaussian::QR
        // In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
        continue;  // but even if not, no need to update if a==zeros
      }

      // exit after rank exhausted
      if (Rd.size() >= maxRank)
        break;

      // Rank-1 down-date of Ab, expensive, using outer product
      Ab.block(0, j + 1, m, n - j).noalias() -= a * rd.middleCols(j + 1, n - j);
    }
  }

  // Create storage for precisions
  Vector precisions(Rd.size());

  // Write back result in Ab, imperative as we are
  size_t i = 0;  // start with first row
  bool mixed = false;
  Ab.setZero();  // make sure we don't look below
  for (const Triple& t: Rd) {
    const size_t& j = t.get<0>();
    const Matrix& rd = t.get<1>();
    precisions(i) = t.get<2>();
    if (std::isinf(precisions(i)))
      mixed = true;
    Ab.block(i, j, 1, n + 1 - j) = rd.block(0, j, 1, n + 1 - j);
    i += 1;
  }

  // Must include mu, as the defaults might be higher, resulting in non-convergence
  return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions);
}

/* ************************************************************************* */
// Isotropic
/* ************************************************************************* */
Isotropic::shared_ptr Isotropic::Sigma(size_t dim, double sigma, bool smart)  {
  if (smart && std::abs(sigma-1.0)<1e-9) return Unit::Create(dim);
  return shared_ptr(new Isotropic(dim, sigma));
}

/* ************************************************************************* */
Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart)  {
  if (smart && std::abs(variance-1.0)<1e-9) return Unit::Create(dim);
  return shared_ptr(new Isotropic(dim, sqrt(variance)));
}

/* ************************************************************************* */
void Isotropic::print(const string& name) const {
  cout << boost::format("isotropic dim=%1% sigma=%2%") % dim() % sigma_ << endl;
}

/* ************************************************************************* */
double Isotropic::squaredMahalanobisDistance(const Vector& v) const {
  return v.dot(v) * invsigma_ * invsigma_;
}

/* ************************************************************************* */
Vector Isotropic::whiten(const Vector& v) const {
  return v * invsigma_;
}

/* ************************************************************************* */
Vector Isotropic::unwhiten(const Vector& v) const {
  return v * sigma_;
}

/* ************************************************************************* */
Matrix Isotropic::Whiten(const Matrix& H) const {
  return invsigma_ * H;
}

/* ************************************************************************* */
void Isotropic::WhitenInPlace(Matrix& H) const {
  H *= invsigma_;
}

/* ************************************************************************* */
void Isotropic::whitenInPlace(Vector& v) const {
  v *= invsigma_;
}

/* ************************************************************************* */
void Isotropic::WhitenInPlace(Eigen::Block<Matrix> H) const {
  H *= invsigma_;
}

/* ************************************************************************* */
// Unit
/* ************************************************************************* */
void Unit::print(const std::string& name) const {
  cout << name << "unit (" << dim_ << ") " << endl;
}

/* ************************************************************************* */
// Robust
/* ************************************************************************* */

void Robust::print(const std::string& name) const {
  robust_->print(name);
  noise_->print(name);
}

bool Robust::equals(const Base& expected, double tol) const {
  const Robust* p = dynamic_cast<const Robust*> (&expected);
  if (p == nullptr) return false;
  return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
}

void Robust::WhitenSystem(Vector& b) const {
  noise_->whitenInPlace(b);
  robust_->reweight(b);
}

void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
  noise_->WhitenSystem(A,b);
  robust_->reweight(A,b);
}

void Robust::WhitenSystem(Matrix& A, Vector& b) const {
  noise_->WhitenSystem(A,b);
  robust_->reweight(A,b);
}

void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const {
  noise_->WhitenSystem(A1,A2,b);
  robust_->reweight(A1,A2,b);
}

void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
  noise_->WhitenSystem(A1,A2,A3,b);
  robust_->reweight(A1,A2,A3,b);
}

Robust::shared_ptr Robust::Create(
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
  return shared_ptr(new Robust(robust,noise));
}

/* ************************************************************************* */

}
} // gtsam


#pragma once

// system includes ------------------------------------------------------------
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>
#include "aux/exceptions.h"

namespace boltzmann {
class CollisionTensor
{
 private:
  typedef Eigen::SparseMatrix<double> sparse_matrix_t;
  typedef std::shared_ptr<sparse_matrix_t> ptr_t;
  typedef Eigen::SparseLU<sparse_matrix_t> lu_t;

 public:
  CollisionTensor(int N);
  CollisionTensor()
      : N_(0)
  {
  }

  void apply(double* out, const double* in) const;
  void apply(double* out, const double* in, double* buffer) const;

  /**
   *
   *
   * @param out
   * @param in
   * @param L        #local phys. DoFs
   * @param buffer   external buffer of length N*L
   * @param local_bd_indicator true: is at boundary, do not apply scattering,
   *                           false: inner vertex, apply scattering
   */
  void apply(double* out,
             const double* in,
             const unsigned int L,
             double* buffer,
             const std::vector<bool>& local_bd_indicator = std::vector<bool>()) const;

  void apply_adaptive(double* out, const double* in, int nmax) const;
  void add(ptr_t& slice, unsigned int j);
  const sparse_matrix_t& get(int j);

  void set_mass_matrix(sparse_matrix_t& m);
  const lu_t& get_invm() const { return lu_; }
  const sparse_matrix_t& mass_matrix() const { return mass_matrix_; }

  const std::vector<ptr_t>& slices() const { return slices_; }

  /**
   * @brief read tensor from file
   *
   * @param fname Filename
   * @param N     #DoFs
   */
  void read_hdf5(const char* fname, const int N);

 private:
  /// basis size
  unsigned int N_;
  /// buffer
  mutable Eigen::VectorXd vtmp;
  /// tensor entries
  std::vector<ptr_t> slices_;
  /// mass matrix
  sparse_matrix_t mass_matrix_;
  /// inverse of mass matrix
  lu_t lu_;
};

// ------------------------------------------------------------
inline void
CollisionTensor::apply(double* out, const double* in) const
{
  assert(mass_matrix_.rows() > 0);
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> const_vec_t;
  const_vec_t v_in(in, N_);
  vec_t v_out(out, N_);

#pragma omp parallel for
  for (unsigned int j = 0; j < N_; ++j) {
    vtmp[j] = v_in.dot((*slices_[j]) * v_in);
  }

  // this is not parallel
  v_out = lu_.solve(vtmp);
}

// ------------------------------------------------------------
inline void
CollisionTensor::apply_adaptive(double* out, const double* in, int nmax) const
{
  assert(mass_matrix_.rows() > 0);
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> const_vec_t;
  const_vec_t v_in(in, nmax);
  vec_t v_out(out, N_);

// is this load-balanced? probably not
#pragma omp parallel for
  for (unsigned int j = 0; j < N_; ++j) {
    vtmp[j] = v_in.dot((*slices_[j]).topLeftCorner(nmax, nmax) * v_in);
  }

  // this is not parallel
  v_out = lu_.solve(vtmp);
}

// ------------------------------------------------------------
inline void
CollisionTensor::apply(double* out, const double* in, double* buffer) const
{
  assert(mass_matrix_.rows() > 0);
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> const_vec_t;
  const_vec_t v_in(in, N_);
  vec_t vbuf(buffer, N_);
  vec_t v_out(out, N_);
#pragma omp parallel for schedule(guided)
  for (unsigned int j = 0; j < N_; ++j) {
    vbuf[j] = v_in.dot((*slices_[j]) * v_in);
  }
  v_out = lu_.solve(vbuf);
}

// ------------------------------------------------------------
inline void
CollisionTensor::apply(double* out,
                       const double* in,
                       const unsigned int L,
                       double* buffer,
                       const std::vector<bool>& local_bd_indicator) const
{
  assert(mass_matrix_.rows() > 0);
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> const_vec_t;

  vec_t vbuf(buffer, N_ * L);

  if (local_bd_indicator.size() == 0) {
#pragma omp parallel
    {
#pragma omp for schedule(guided)
      for (unsigned int j = 0; j < N_; ++j) {
        for (unsigned int l = 0; l < L; ++l) {
          const_vec_t v_in(in + N_ * l, N_);
          vbuf[N_ * l + j] = v_in.dot((*slices_[j]) * v_in);
        }
      }

#pragma omp for schedule(static)
      for (unsigned int l = 0; l < L; ++l) {
        vec_t vout(out + N_ * l, N_);
        const_vec_t vbuf(buffer + N_ * l, N_);
        vout = lu_.solve(vbuf);
      }
    }
  } else if (local_bd_indicator.size() == L) {
#pragma omp parallel
    {
#pragma omp for schedule(guided)
      for (unsigned int j = 0; j < N_; ++j) {
        for (unsigned int l = 0; l < L; ++l) {
          if (local_bd_indicator[l]) continue;
          const_vec_t v_in(in + N_ * l, N_);
          vbuf[N_ * l + j] = v_in.dot((*slices_[j]) * v_in);
        }
      }

#pragma omp for schedule(static)
      for (unsigned int l = 0; l < L; ++l) {
        if (local_bd_indicator[l]) continue;
        vec_t vout(out + N_ * l, N_);
        const_vec_t vbuf(buffer + N_ * l, N_);
        vout = lu_.solve(vbuf);
      }
    }

  }
#ifdef DEBUG
  else {
    BAssertThrow(false, "something went wrong with bd_indicator");
  }
#endif
}

}  // end boltzmann


#include <string>
#include <string.h>
#include <stdlib.h>
#include <iostream>
#include <cstdio>

#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/common/time.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/dinast_grabber.h>

#define FPS_CALC(_WHAT_) \
do \
{ \
    static unsigned count = 0;\
    static double last = pcl::getTime ();\
    double now = pcl::getTime (); \
    ++count; \
    if (now - last >= 1.0) \
    { \
      std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" <<  std::endl; \
      count = 0; \
      last = now; \
    } \
}while(false)

void
savePGM (const unsigned char *image, const std::string& filename)
{
  FILE *file = fopen (filename.c_str (), "w");
  if (!file)
  {
    std::cerr << "Unable to open file '" << filename << "' for writing." << std::endl;
    return;
  }

  // PGM header
  fprintf (file, "P2\n%d %d\n255\n", IMAGE_WIDTH, IMAGE_HEIGHT);

  // Write data as ASCII
  for (int i = 0; i < IMAGE_HEIGHT; ++i)
  {
    for (int j = 0; j < IMAGE_WIDTH; ++j)
    {
      fprintf (file, "%3d ", (int)*image++);
    }
    fprintf (file, "\n");
  }

  fclose (file);
}

void
keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event, void* data)
{
  static int NUM_IMAGES = 0;
  if (event.getKeySym () == "s" && event.keyDown ())
  {
    char filename[16];
    snprintf (filename, sizeof(filename), "image%.2d.pgm", NUM_IMAGES++);
    unsigned char *image = reinterpret_cast<unsigned char*> (data);
    savePGM (image, filename);
    printf ("Wrote %s\n", filename);
  }
}

void
convertImageToCloud (const unsigned char *image, pcl::PointCloud<pcl::PointXYZI> &cloud)
{
  cloud.points.resize (IMAGE_WIDTH * IMAGE_HEIGHT);
  cloud.width = IMAGE_WIDTH;
  cloud.height = IMAGE_HEIGHT;
  cloud.is_dense = false;

  int depth_idx = 0;
  int pxl[9];

  for (int x = 0; x < cloud.width; ++x)
  {
    for (int y = 0; y < cloud.height; ++y, ++depth_idx)
    {
      float xc = (float)(x - 160);
      float yc = (float)(y - 120);
      double r1 = sqrt (xc * xc + yc * yc);
      double r2 = r1 * r1;
      double r3 = r1 * r2;
      double A = -2e-5 * r3 + 0.004 * r2 + 0.1719 * r1 + 350.03;
      double B = -2e-9 * r3 + 3e-7 * r2 - 1e-5 * r1 - 0.01;

      // Low pass filtering
      /// @todo Try a bilateral filter to avoid blurring over depth boundaries
      int measure = 0;
      if ((y > 0) && (y < (IMAGE_HEIGHT - 1)) && (x > 0) && (x < (IMAGE_WIDTH - 1)))
      {
        int ipx = x + IMAGE_WIDTH * y;
#if 1
        pxl[0] = image[ipx];
        pxl[1] = image[ipx-1];
        pxl[2] = image[ipx+1];
        pxl[3] = image[ipx - IMAGE_WIDTH];
        pxl[4] = image[ipx - IMAGE_WIDTH - 1];
        pxl[5] = image[ipx - IMAGE_WIDTH + 1];
        pxl[6] = image[ipx + IMAGE_WIDTH];
        pxl[7] = image[ipx + IMAGE_WIDTH - 1];
        pxl[8] = image[ipx + IMAGE_WIDTH + 1];

        for (int ii = 0; ii < 9; ii++)
          measure += pxl[ii];
        measure /= 9;
#else
        // No blurring
        measure = image[ipx];
#endif
      }
      if (measure > 255)
        measure = 255;  // saturation for display

      unsigned char pixel = measure;//image[depth_idx];
      if (pixel < 1)
      {
        cloud.points[depth_idx].x = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].y = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].z = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].intensity = pixel;
        continue;
      }

      if (pixel > A)
        pixel = A;

      float dy = y*0.1;
      double dist = (log((double)pixel/A)/B-dy)*(7E-07*r3 - 0.0001*r2 + 0.004*r1 + 0.9985)*1.5;
      double dist_2d = r1;

      static const double dist_max_2d = 1 / 160.0; /// @todo Why not 200?
      //static const double dist_max_2d = 1 / 200.0;
      static const double FOV = 64.0 * M_PI / 180.0; // diagonal FOV?

      double theta_colati = FOV * r1 * dist_max_2d;
      double c_theta = cos (theta_colati);
      double s_theta = sin (theta_colati);
      double c_ksai = ((double)(x - 160.)) / r1;
      double s_ksai = ((double)(y - 120.)) / r1;

      cloud.points[depth_idx].x = (dist * s_theta * c_ksai) / 500.0 + 0.5; //cartesian x
      cloud.points[depth_idx].y = (dist * s_theta * s_ksai) / 500.0 + 0.5; //cartesian y
      cloud.points[depth_idx].z = (dist * c_theta);                        //cartesian z
      /// @todo This looks weird, can it cause artifacts?
      if (cloud.points[depth_idx].z < 0.01)
#if 1
        cloud.points[depth_idx].z = 0.01;
#else
        cloud.points[depth_idx].x = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].y = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].z = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].intensity = pixel;
        continue;
#endif

      cloud.points[depth_idx].z /= 500.0;
      cloud.points[depth_idx].intensity = pixel;
    }
  }
}

/* --[ */

int
main (int argc, char** argv)
{

  pcl::DinastGrabber grabber;

  grabber.findDevice (1);

  grabber.openDevice();

  std::cerr << "Device version/revision number: " << grabber.getDeviceVersion () << std::endl;

  grabber.start ();

  pcl::visualization::ImageViewer vis_img ("Dinast Image Viewer");
  pcl::visualization::PCLVisualizer vis_cld (argc, argv, "Dinast Cloud Viewer");

  unsigned char *img1 = (unsigned char*)malloc (IMAGE_SIZE);
  unsigned char *img2 = (unsigned char*)malloc (IMAGE_SIZE);

  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);

  while (true)
  {
    if (grabber.readImage ( img1, img2) == 0)
      continue;

     convertImageToCloud (img1, *cloud);

    FPS_CALC ("grabber + visualization");
    vis_img.showMonoImage (img1, IMAGE_WIDTH, IMAGE_HEIGHT);

    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> handler (cloud, "intensity");
    if (!vis_cld.updatePointCloud (cloud, handler, "DinastCloud"))
    {
      vis_cld.addPointCloud (cloud, handler, "DinastCloud");
      vis_cld.resetCameraViewpoint ("DinastCloud");
    }

    vis_img.spinOnce ();
    vis_cld.spinOnce ();

  }

  grabber.stop ();
  grabber.closeDevice ();

}


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

#ifndef SMOOTH__FEEDBACK__COMPAT__OSQP_HPP_
#define SMOOTH__FEEDBACK__COMPAT__OSQP_HPP_

/**
 * @file
 * @brief Solve quadratiac programs with OSQP.
 */

#include <Eigen/Sparse>
#include <osqp/osqp.h>

#include "smooth/feedback/qp_solver.hpp"

namespace smooth::feedback {

/**
 * @brief Solve a QuadraticProgram with the OSQP solver
 *
 * @param pbm QuadraticProgram to solve
 * @param prm solver paramters
 * @param warmstart initial point to start iterating from
 *
 * @return solution
 *
 * @note This is a convenience interface that performs copies and memory allocation in each
 * call. For more fine-grained control use the low-level OSQP interface (https://osqp.org/).
 */
template<typename Problem>
QPSolution<-1, -1, double> solve_qp_osqp(
  const Problem & pbm,
  const QPSolverParams & prm,
  std::optional<std::reference_wrapper<const QPSolution<-1, -1, double>>> warmstart = {})
{
  // Covert to sparse matrices with OSQP indexing
  Eigen::SparseMatrix<double, Eigen::ColMajor, c_int> P;
  Eigen::SparseMatrix<double, Eigen::ColMajor, c_int> A;
  if constexpr (std::is_base_of_v<Eigen::SparseMatrixBase<decltype(pbm.A)>, decltype(pbm.A)>) {
    A = pbm.A;
    P = pbm.P.template triangularView<Eigen::Upper>();
  } else {
    A                   = pbm.A.sparseView();
    Eigen::MatrixXd Pup = pbm.P.template triangularView<Eigen::Upper>();
    P                   = Pup.sparseView();
    A.prune(1e-6);
    P.prune(1e-6);
  }
  A.makeCompressed();
  P.makeCompressed();

  OSQPSettings * settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
  osqp_set_default_settings(settings);

  settings->verbose            = prm.verbose;
  settings->sigma              = prm.sigma;
  settings->alpha              = prm.alpha;
  settings->rho                = prm.rho;
  settings->eps_abs            = prm.eps_abs;
  settings->eps_rel            = prm.eps_rel;
  settings->eps_prim_inf       = prm.eps_primal_inf;
  settings->eps_dual_inf       = prm.eps_dual_inf;
  settings->scaling            = prm.scaling;
  settings->check_termination  = prm.stop_check_iter;
  settings->polish             = prm.polish;
  settings->polish_refine_iter = prm.polish_iter;
  settings->delta              = prm.delta;

  settings->adaptive_rho       = false;
  settings->linsys_solver      = QDLDL_SOLVER;
  settings->scaled_termination = false;

  if (prm.max_iter) {
    settings->max_iter = prm.max_iter.value();
  } else {
    settings->max_iter = std::numeric_limits<c_int>::max();
  }
  if (prm.max_time) {
    settings->time_limit =
      duration_cast<std::chrono::duration<double>>(prm.max_time.value()).count();
  } else {
    settings->time_limit = 0;
  }

  OSQPData * data = (OSQPData *)c_malloc(sizeof(OSQPData));
  data->n         = A.cols();
  data->m         = A.rows();
  data->A         = csc_matrix(
    A.rows(), A.cols(), A.nonZeros(), A.valuePtr(), A.innerIndexPtr(), A.outerIndexPtr());
  data->q = const_cast<double *>(pbm.q.data());
  data->P = csc_matrix(
    P.rows(), P.cols(), P.nonZeros(), P.valuePtr(), P.innerIndexPtr(), P.outerIndexPtr());
  data->l = const_cast<double *>(pbm.l.data());
  data->u = const_cast<double *>(pbm.u.data());

  OSQPWorkspace * work;

  QPSolution<-1, -1, double> ret;
  ret.code = QPSolutionStatus::Unknown;

  c_int error = osqp_setup(&work, data, settings);

  if (warmstart) {
    osqp_warm_start(
      work, warmstart.value().get().primal.data(), warmstart.value().get().dual.data());
    settings->warm_start = 1;
  } else {
    settings->warm_start = 0;
  }

  if (!error) { error &= osqp_solve(work); }

  if (!error) {
    switch (work->info->status_val) {
    case OSQP_SOLVED: {
      ret.code = QPSolutionStatus::Optimal;
      break;
    }
    case OSQP_PRIMAL_INFEASIBLE: {
      ret.code = QPSolutionStatus::PrimalInfeasible;
      break;
    }
    case OSQP_DUAL_INFEASIBLE: {
      ret.code = QPSolutionStatus::DualInfeasible;
      break;
    }
    case OSQP_MAX_ITER_REACHED: {
      ret.code = QPSolutionStatus::MaxIterations;
      break;
    }
    case OSQP_TIME_LIMIT_REACHED: {
      ret.code = QPSolutionStatus::MaxTime;
      break;
    }
    default: {
      break;
    }
    }

    ret.iter      = work->info->iter;
    ret.primal    = Eigen::Map<const Eigen::Matrix<double, -1, 1>>(work->solution->x, data->n);
    ret.dual      = Eigen::Map<const Eigen::Matrix<double, -1, 1>>(work->solution->y, data->m);
    ret.objective = work->info->obj_val;
  }

  osqp_cleanup(work);

  c_free(data->A);
  c_free(data->P);
  c_free(data);
  c_free(settings);

  return ret;
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__COMPAT__OSQP_HPP_


#include "igl/readOBJ.h"
#include "igl/writeOBJ.h"
// #include "igl/decimate.h"

#include <Eigen/Core>

#include "pythonlike.h"

#include <cstdlib> // exit()
#include <iostream>
#include <cassert>
#include <cstdio> // printf()

#include <igl/seam_edges.h>
#include "decimate.h"
#include "quadric_error_metric.h"
#include <igl/writeDMAT.h>

// An anonymous namespace. This hides these symbols from other modules.
namespace {

void usage( const char* argv0 )
{
    std::cerr << "Usage: " << argv0 << " <path/to/input.obj> num-vertices     <target_number_of_vertices>  [--strict] [<strictness>]" << std::endl;
    std::cerr << "Usage: " << argv0 << " <path/to/input.obj> percent-vertices <target_percent_of_vertices> [--strict] [<strictness>]" << std::endl;
    exit(-1);
}

int count_seam_edge_num(const EdgeMap& seam_vertex_edges)
{
	int count = 0;
	for(auto & v : seam_vertex_edges) {
		count += v.second.size();
	}
	return count / 2;
}

enum SeamAwareDegree
{
	NoUVShapePreserving,
	UVShapePreserving,
	Seamless
};

/*
Decimates a triangle mesh down to a target number of vertices,
preserving the UV parameterization.
TODO Q: Should we do a version that does not preserve the UV parameterization exactly,
        but instead returns a sequence of TC/FTC that can be used to transform a UV
        point between the parameterizations of the decimated and undecimated mesh?
Input parameters:
    V: The 3D positions of the input mesh (3 columns)
    TC: The 2D texture coordinates of the input mesh (2 columns)
    F: Indices into `V` for the three vertices of each triangle.
    FTC: Indices into `TC` for the three vertices of each triangle.
Output parameters:
    Vout: The 3D positions of the decimated mesh (3 columns),
          where #vertices is as close as possible to `target_num_vertices`)
    TCout: The texture coordinates of the decimated mesh (2 columns)
    Fout: Indices into `Vout` for the three vertices of each triangle.
    FTCout: Indices into `TCout` for the three vertices of each triangle.
Returns:
    True if the routine succeeded, false if an error occurred.
Notes:
    The output mesh will a vertex count as close as possible to `target_num_vertices`.
    The decimated mesh should never have fewer vertices than `target_num_vertices`.
*/
template <typename DerivedV, typename DerivedF, typename DerivedT>
bool decimate_down_to(
    const Eigen::PlainObjectBase<DerivedV>& V,
    const Eigen::PlainObjectBase<DerivedF>& F,
    const Eigen::PlainObjectBase<DerivedT>& TC,
    const Eigen::PlainObjectBase<DerivedF>& FT,
    int target_num_vertices,
    Eigen::MatrixXd& V_out,
    Eigen::MatrixXi& F_out,
    Eigen::MatrixXd& TC_out,
    Eigen::MatrixXi& FT_out,
    int seam_aware_degree
    )
{
#define DEBUG_DECIMATE_DOWN_TO
    assert( target_num_vertices > 0 );
    assert( target_num_vertices < V.rows() );

    /// 3D triangle mesh with UVs.
    // 3D
    assert( V.cols() == 3 );
    // triangle mesh
    assert( F.cols() == 3 );
    // UVs
    assert( TC.cols() == 2 );
    assert( FT.cols() == 3 );
    assert( FT.cols() == F.cols() );

    // Print information about seams.
    Eigen::MatrixXi seams, boundaries, foldovers;
    igl::seam_edges( V, TC, F, FT, seams, boundaries, foldovers );
#ifdef DEBUG_DECIMATE_DOWN_TO
    std::cout << "seams: " << seams.rows() << "\n";
    std::cout << seams << std::endl;
    std::cout << "boundaries: " << boundaries.rows() << "\n";
    std::cout << boundaries << std::endl;
    std::cout << "foldovers: " << foldovers.rows() << "\n";
    std::cout << foldovers << std::endl;
#endif

    // Collect all vertex indices involved in seams.
    std::unordered_set< int > seam_vertex_indices;
    // Also collect the edges in terms of position vertex indices themselves.
    EdgeMap seam_vertex_edges;
    {
		for( int i = 0; i < seams.rows(); ++i ) {
		    const int v1 = F( seams( i, 0 ),   seams( i, 1 ) );
		    const int v2 = F( seams( i, 0 ), ( seams( i, 1 ) + 1 ) % 3 );
			seam_vertex_indices.insert( v1 );
			seam_vertex_indices.insert( v2 );
			insert_edge( seam_vertex_edges, v1, v2 );
			// The vertices on both sides should match:
			assert( seam_vertex_indices.count( F( seams( i, 2 ),   seams( i, 3 ) ) ) );
			assert( seam_vertex_indices.count( F( seams( i, 2 ), ( seams( i, 3 ) + 1 ) % 3 ) ) );
		}
		for( int i = 0; i < boundaries.rows(); ++i ) {
		    const int v1 = F( boundaries( i, 0 ),   boundaries( i, 1 ) );
		    const int v2 = F( boundaries( i, 0 ), ( boundaries( i, 1 ) + 1 ) % 3 );
			seam_vertex_indices.insert( v1 );
			seam_vertex_indices.insert( v2 );
			insert_edge( seam_vertex_edges, v1, v2 );
		}
		for( int i = 0; i < foldovers.rows(); ++i ) {
		    const int v1 = F( foldovers( i, 0 ),   foldovers( i, 1 ) );
		    const int v2 = F( foldovers( i, 0 ), ( foldovers( i, 1 ) + 1 ) % 3 );
			seam_vertex_indices.insert( v1 );
			seam_vertex_indices.insert( v2 );
			insert_edge( seam_vertex_edges, v1, v2 );
			// The vertices on both sides should match:
			assert( seam_vertex_indices.count( F( foldovers( i, 2 ),   foldovers( i, 3 ) ) ) );
			assert( seam_vertex_indices.count( F( foldovers( i, 2 ), ( foldovers( i, 3 ) + 1 ) % 3 ) ) );
		}

	    std::cout << "# seam vertices: " << seam_vertex_indices.size() << std::endl;
		std::cout << "# seam edges: " << count_seam_edge_num(seam_vertex_edges) << std::endl;
    }

    // Compute the per-vertex quadric error metric.
    std::vector< Eigen::MatrixXd > Q;
    bool success = false;
    Eigen::VectorXi J;

	MapV5d hash_Q;
	half_edge_qslim_5d(V,F,TC,FT,hash_Q);
	std::cout << "computing initial metrics finished\n" << std::endl;
	success = decimate_halfedge_5d(
		V, F,
		TC, FT,
		seam_vertex_edges,
		hash_Q,
		target_num_vertices,
		seam_aware_degree,
		V_out, F_out,
		TC_out, FT_out
		);
	std::cout << "#seams after decimation: " << count_seam_edge_num(seam_vertex_edges) << std::endl;
    std::cout << "#interior foldeover: " << interior_foldovers.size() << std::endl;
    std::cout << "#exterior foldeover: " << exterior_foldovers.size() << std::endl;
    return success;
}
}

int main( int argc, char* argv[] ) {
    std::vector<std::string> args( argv + 1, argv + argc );
    std::string strictness;
    int seam_aware_degree = int( SeamAwareDegree::Seamless );
    const bool found_strictness = pythonlike::get_optional_parameter( args, "--strict", strictness );
	if ( found_strictness ) {
		seam_aware_degree = atoi(strictness.c_str());
	}

    if( args.size() != 3 && args.size() != 4 )	usage( argv[0] );
    std::string input_path, command, command_parameter;
    pythonlike::unpack( args.begin(), input_path, command, command_parameter );
    args.erase( args.begin(), args.begin() + 3 );

    // Does the input path exist?
    Eigen::MatrixXd V, TC, CN;
    Eigen::MatrixXi F, FT, FN;
    if( !igl::readOBJ( input_path, V, TC, CN, F, FT, FN ) ) {
        std::cerr << "ERROR: Could not read OBJ: " << input_path << std::endl;
        usage( argv[0] );
    }

    std::cout << "Loaded a mesh with " << V.rows() << " vertices and " << F.rows() << " faces: " << input_path << std::endl;

    // Get the target number of vertices.
    int target_num_vertices = 0;
    if( command == "num-vertices" ) {
        // strto<> returns 0 upon failure, which is fine, since that is invalid input for us.
        target_num_vertices = pythonlike::strto< int >( command_parameter );
    }
    else if( command == "percent-vertices" ) {
        const double percent = pythonlike::strto< double >( command_parameter );
        target_num_vertices = lround( ( percent * V.rows() )/100. );
        std::cout << command_parameter << "% of " << std::to_string( V.rows() ) << " input vertices is " << std::to_string( target_num_vertices ) << " output vertices." << std::endl;
        // Ugh, printf() requires me to specify the types of integers versus longs.
        // printf( "%.2f%% of %d input vertices is %d output vertices.", percent, V.rows(), target_num_vertices );
    }
    else {
        std::cerr << "ERROR: Unknown command: " << command << std::endl;
        usage( argv[0] );
    }

    // Check that the target number of vertices is positive and fewer than the input number of vertices.
    if( target_num_vertices <= 0 ) {
        std::cerr << "ERROR: Target number of vertices must be a positive integer: " << argv[4] << std::endl;
        usage( argv[0] );
    }
    if( target_num_vertices >= V.rows() ) {
    	std::string output_path = pythonlike::os_path_splitext( input_path ).first + "-decimated_to_" + std::to_string( V.rows() ) + "_vertices.obj";
        if( !igl::writeOBJ( output_path, V, F, CN, FN, TC, FT ) ) {
			std::cerr << "ERROR: Could not write OBJ: " << output_path << std::endl;
			usage( argv[0] );
		}
   		std::cout << "Wrote: " << output_path << std::endl;
        std::cerr << "ERROR: Target number of vertices must be smaller than the input number of vertices: " << argv[4] << std::endl;
        return 0;
    }

    // Make the default output path.
    std::string output_path = pythonlike::os_path_splitext( input_path ).first + "-decimated_to_" + std::to_string( target_num_vertices ) + "_vertices.obj";
    if( !args.empty() ) {
        output_path = args.front();
        args.erase( args.begin() );
    }

    // We should have consumed all arguments.
    if( !args.empty() ) usage( argv[0] );

    // Decimate!
    Eigen::MatrixXd V_out, TC_out, CN_out;
    Eigen::MatrixXi F_out, FT_out, FN_out;
    const bool success = decimate_down_to( V, F, TC, FT, target_num_vertices, V_out, F_out, TC_out, FT_out, seam_aware_degree );
    if( !success ) {
        std::cerr << "WARNING: decimate_down_to() returned false (target number of vertices may have been unachievable)." << std::endl;
    }

    if( !igl::writeOBJ( output_path, V_out, F_out, CN_out, FN_out, TC_out, FT_out ) ) {
        std::cerr << "ERROR: Could not write OBJ: " << output_path << std::endl;
        usage( argv[0] );
    }
    std::cout << "Wrote: " << output_path << std::endl;

    return 0;
}


/*! \file orbitaldensityrand.cpp
    \brief OrbitalDensityRandクラスの実装

    Copyright © 2019-2021 @dc1394 All Rights Reserved.
    (but this is originally adapted by サンマヤ for TDXHydrogenScene.cpp from http://sammaya.garyoutensei.com/math_phys/phys_sym/monte_hydrogen.html )
    https://tsujimotter.hatenablog.com/entry/metropolis-hastings-algorithm も参考にさせて頂きました。ありがとうございます。
    This software is released under the BSD 2-Clause License.
*/

#include "orbitaldensityrand.h"
#include "utility/utility.h"
#include <boost/assert.hpp>                                     // for boost::assert
#include <boost/math/constants/constants.hpp>                   // for boost::math::constants::pi
#include <boost/math/special_functions/spherical_harmonic.hpp>  // for boost::math::spherical_harmonic
#include <boost/range/algorithm.hpp>                            // for boost::fill

namespace orbitaldensityrand {
    // #region コンストラクタ

	OrbitalDensityRand::OrbitalDensityRand(std::shared_ptr<getdata::GetData> const & pgd)
        :   Complete([this] { return complete_.load(); }, nullptr),
            Dt([this] { return dt_; }, [this](auto dt) { return dt_ = dt; }),
            Elapsed_time([this] { return count_ * dt_; }, nullptr),
            Pth([this] { return std::cref(pth_); }, nullptr),
		    Redraw(nullptr, [this](auto redraw) { return redraw_ = redraw; }),
            Rmax([this] { return rmax_; }, nullptr),
            Thread_end(nullptr, [this](auto thread_end) {
			    thread_end_.store(thread_end);
			    return thread_end; }),
            Vertex([this] { return std::cref(vertex_); }, nullptr),
		    Vertexsize([this]{ return vertexsize_.load(); }, [this](std::vector<SimpleVertex>::size_type size) {
				vertexsize_.store(size);
				return size; }),
            pgd_(pgd),
            q0_({ 1.0, 1.0, 0.0 }),
            q_(q0_),
		    rmax_(GetRmax(pgd)),
		    vertex_(pgd_->Rho_wf_type == getdata::GetData::Rho_Wf_type::RHO ? RHO_VERTEXSIZE_INIT_VALUE : WF_VERTEXSIZE_INIT_VALUE),
            vertexsize_(pgd_->Rho_wf_type == getdata::GetData::Rho_Wf_type::RHO ? RHO_VERTEXSIZE_INIT_VALUE : WF_VERTEXSIZE_INIT_VALUE)
    {
    }

    // #endregion コンストラクタ

    // #region privateメンバ関数

    void OrbitalDensityRand::operator()(std::int32_t m, Normal_Nelson_type nornel)
    {
        if (redraw_) {
            if (vertex_.size() != vertexsize_) {
                vertex_.resize(vertexsize_);
            }

            pth_.reset(new std::thread([this, m, nornel] { ClearFillSimpleVertex(m, nornel); }), [this](std::thread * pth)
            {
                if (pth->joinable()) {
                    thread_end_.store(true);
                    pth->join();
                }

                utility::Safe_Delete<std::thread> sd;
                sd(pth);
            });
            redraw_ = false;
        }
    }

	void OrbitalDensityRand::ClearFillSimpleVertex(std::int32_t m, Normal_Nelson_type nornel)
	{
		complete_.store(false);

		SimpleVertex sv{};
		sv.Color = { 0.0f, 0.0f, 0.0f, 0.0f };
		sv.Pos = { 0.0f, 0.0f, 0.0f };
		boost::fill(vertex_, sv);

        switch (nornel)
        {
        case Normal_Nelson_type::NORMAL:
            {
                auto const threads = static_cast<std::int32_t>(std::thread::hardware_concurrency());
                auto const num = static_cast<std::int32_t>(vertexsize_.load() / threads);

                auto thvec = std::vector<std::thread>(threads);
                for (auto i = 0; i < threads - 1; i++) {
                    thvec[i] = std::thread([i, m, num, this]() { FillSimpleVertex(m, num * i, num * (i + 1)); });
                }
                thvec[threads - 1] = std::thread([m, num, threads, this]() { FillSimpleVertex(m, num * (threads - 1), static_cast<std::int32_t>(vertexsize_.load())); });

                for (auto && th : thvec) {
                    th.join();
                }
            }
            break;

        case Normal_Nelson_type::NELSON:
            FillSimpleVertex(m);
            break;

        default:
            BOOST_ASSERT(!"nornelが異常!");
            break;
        }

		complete_.store(true);
	}

    void OrbitalDensityRand::FillSimpleVertex(std::int32_t m)
    {
        using namespace boost::math;
        using namespace constants;

        auto const actual_dt = dt_ * ATTOSECTOAU;

        count_ = 0U;
        do {
            if (thread_end_) {
                return;
            }

            auto const r = std::hypot(q_[0], q_[1], q_[2]);

            if (r < pgd_->R_meshmin() || r > pgd_->R_meshmax()) {
                Resetq();
                continue;
            }

            double phi = 0.0;
            if (std::fabs(q_[1]) < THRESHOLD) {
                if (q_[0] < 0.0) {
                    phi = boost::math::constants::pi<double>();
                }
            }
            else if (q_[0] * q_[0] + q_[1] * q_[1] > 0.0) {
                auto const sign = q_[1] > 0 ? 1 : -1;
                phi = sign * std::acos(q_[0] / std::sqrt(q_[0] * q_[0] + q_[1] * q_[1]));
            }
            auto const theta = std::acos(q_[2] / r);

            auto const ylm = Spherical_harmonic(pgd_->L, m, theta, phi);
            auto const dylmdtheta = Numerical_diff(
                theta,
                myfunctional::make_functional([this, m, phi](double th) { return Spherical_harmonic(pgd_->L, m, th, phi); }));

            if (std::fabs(ylm) < THRESHOLD && std::fabs(dylmdtheta) < THRESHOLD) {
                Resetq();
                continue;
            }

            auto const dylmdphi = Numerical_diff(
                phi,
                myfunctional::make_functional([this, m, theta](double ph) { return Spherical_harmonic(pgd_->L, m, theta, ph); }));

            if (std::fabs(ylm) < THRESHOLD && std::fabs(dylmdphi) < THRESHOLD) {
                Resetq();
                continue;
            }

            auto f_x = std::sin(theta) * std::cos(phi) * pgd_->dphidr(r) / (*pgd_)(r);
            f_x += std::cos(theta) * std::cos(phi) / r * dylmdtheta / ylm;
            f_x -= std::sin(phi) / (r * std::sin(theta)) * dylmdphi / ylm;

            auto f_y = std::sin(theta) * std::sin(phi) * pgd_->dphidr(r) / (*pgd_)(r);
            f_y += std::cos(theta) * std::sin(phi) / r * dylmdtheta / ylm;
            f_y += std::cos(phi) / (r * std::sin(theta)) * dylmdphi / ylm;

            auto f_z = std::cos(theta) * pgd_->dphidr(r) / (*pgd_)(r);
            f_z -= std::sin(theta) / r * dylmdtheta / ylm;

            q_[0] += f_x * actual_dt + mr_.normal_distribution_rand() * std::sqrt(actual_dt);
            q_[1] += f_y * actual_dt + mr_.normal_distribution_rand() * std::sqrt(actual_dt);
            q_[2] += f_z * actual_dt + mr_.normal_distribution_rand() * std::sqrt(actual_dt);

            vertex_[count_].Pos.x = static_cast<float>(q_[0]);
            vertex_[count_].Pos.y = static_cast<float>(q_[1]);
            vertex_[count_].Pos.z = static_cast<float>(q_[2]);

            vertex_[count_].Color.x = 0.8f;
            vertex_[count_].Color.y = 0.0f;
            vertex_[count_].Color.z = 0.8f;
            vertex_[count_].Color.w = 1.0f;

            count_++;
        } while (count_ < vertexsize_.load());
    }

	void OrbitalDensityRand::FillSimpleVertex(std::int32_t m, std::int32_t starti, std::int32_t endi)
	{
		if (thread_end_) {
			return;
		}

		auto sign = 1;
		auto x = 0.0;
        auto y = 0.0;
        auto z = 0.0;

        myrandom::MyRandSfmt mr;

        auto nextflag = false;
        auto count_ = 0;

        do {
            if (thread_end_) {
                return;
            }

            switch (pgd_->Rho_wf_type) {
            case getdata::GetData::Rho_Wf_type::RHO:
            {
                auto rho = [this, m, nextflag](auto x, auto y, auto z) mutable
                {
                    auto const r = std::sqrt(x * x + y * y + z * z);
                    if (r < pgd_->R_meshmin()) {
                        nextflag = true;
                        return 0.0;
                    }

                    double phi = 0.0;
                    if (std::fabs(y) < EPS) {
                        if (x < 0.0) {
                            phi = boost::math::constants::pi<double>();
                        }
                    }
                    else if (x * x + y * y > 0.0) {
                        auto const sign = y > 0 ? 1 : -1;
                        phi = sign * std::acos(x / std::sqrt(x * x + y * y));
                    }

                    auto const ylm = Spherical_harmonic(pgd_->L, m, std::acos(z / r), phi);

                    return (*pgd_)(r) * ylm * ylm;
                };

                auto const maxr = pgd_->R2rhomaxr();

                auto const x_star = mr.normal_distribution_rand(x, maxr);
                auto const y_star = mr.normal_distribution_rand(y, maxr);
                auto const z_star = mr.normal_distribution_rand(z, maxr);

                auto const rho_t = rho(x, y, z);    // xの電子密度
                if (nextflag) {
                    x = x_star;
                    y = y_star;
                    z = z_star;
                    nextflag = false;
                    continue;
                }

                auto const rho_star = rho(x_star, y_star, z_star);  // x* の電子密度

                // 採択率を計算  α = p(x*) / p(x_t)
                auto const alpha = (rho_star * rho_star) / (rho_t * rho_t);

                // 採択率により決定
                auto const ar = mr.myrand();    // 0 <= ar <= 1 の一様乱数 ar を生成
                if (ar <= alpha) {              // 採択
                    x = x_star;
                    y = y_star;
                    z = z_star;
                    if (rho_star >= 0.0) {
                        sign = 1;
                    }
                    else {
                        sign = -1;
                    }
                }
            }
            break;

            case getdata::GetData::Rho_Wf_type::WF:
            {
                auto phi = [this, m, nextflag](auto x, auto y, auto z) mutable
                {
                    auto const r = std::sqrt(x * x + y * y + z * z);
                    if (r < pgd_->R_meshmin()) {
                        nextflag = true;
                        return 0.0;
                    }

                    double phi = 0.0;
                    if (std::fabs(y) < EPS) {
                        if (x < 0.0) {
                            phi = boost::math::constants::pi<double>();
                        }
                    }
                    else if (x * x + y * y > 0.0) {
                        auto const sign = y > 0 ? 1 : -1;
                        phi = sign * std::acos(x / std::sqrt(x * x + y * y));
                    }

                    return (*pgd_)(r) * Spherical_harmonic(pgd_->L, m, std::acos(z / r), phi);
                };

                auto const maxr = pgd_->R2rhomaxr();

                auto const x_star = mr.normal_distribution_rand(x, maxr);
                auto const y_star = mr.normal_distribution_rand(y, maxr);
                auto const z_star = mr.normal_distribution_rand(z, maxr);

                auto const phi_t = phi(x, y, z);    // xの波動関数
                if (nextflag) {
                    x = x_star;
                    y = y_star;
                    z = z_star;
                    nextflag = false;
                    continue;
                }

                auto const phi_star = phi(x_star, y_star, z_star);  // x* の波動関数

                // 採択率を計算  α = p(x*) / p(x_t)
                auto const alpha = (phi_star * phi_star) / (phi_t * phi_t);

                //採択率により決定
                auto const ar = mr.myrand();    // 0 <= ar <= 1 の一様乱数 ar を生成
                if (ar <= alpha) {              // 採択
                    x = x_star;
                    y = y_star;
                    z = z_star;
                    if (phi_star >= 0.0) {
                        sign = 1;
                    }
                    else {
                        sign = -1;
                    }
                }
                else {                          // 採択しない
                    continue;
                }
            }
            break;

            default:
                BOOST_ASSERT(!"何かがおかしい!");
                break;
            }

            vertex_[starti + count_].Pos.x = static_cast<float>(x);
            vertex_[starti + count_].Pos.y = static_cast<float>(y);
            vertex_[starti + count_].Pos.z = static_cast<float>(z);

            vertex_[starti + count_].Color.x = sign > 0 ? 0.8f : 0.0f;
            vertex_[starti + count_].Color.y = sign < 0 ? 0.8f : 0.0f;
            vertex_[starti + count_].Color.z = 0.8f;
            vertex_[starti + count_].Color.w = 1.0f;
            count_++;
		} while (count_ < (endi - starti));
	}

    // #endregion privateメンバ関数

    // #region フリー関数

	double GetRmax(std::shared_ptr<getdata::GetData> const & pgd)
	{
		auto const n = static_cast<double>(pgd->N);
		return (2.3622 * n + 3.3340) * n + 1.3228;
	}

    double Spherical_harmonic(std::int32_t l, std::int32_t m, double theta, double phi)
    {
        if (!m) {
            return boost::math::spherical_harmonic_r(l, m, theta, phi);
        }
        else if (m > 0) {
            auto const sign = (m & 1 ? -1 : 1);
            return (sign * boost::math::spherical_harmonic_r(l, m, theta, phi) + boost::math::spherical_harmonic_r(l, -m, theta, phi)) / std::sqrt(2.0);
        }
        else {
            m = -m;
            auto const sign = (m & 1 ? -1 : 1);
            return (sign * boost::math::spherical_harmonic_i(l, m, theta, phi) - boost::math::spherical_harmonic_i(l, -m, theta, phi)) / std::sqrt(2.0);
        }
    }

    // #endregion フリー関数
}


#ifndef EDMONDS_OPTIMUM_BRANCHING_IMPL_HPP
#define EDMONDS_OPTIMUM_BRANCHING_IMPL_HPP

#include <vector>
#include <algorithm>
#include <list>
#include <boost/property_map/property_map.hpp>
#include <boost/foreach.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/concept_check.hpp>
#include <boost/pending/disjoint_sets.hpp>

// namespace detail
//
// The namespace encapsulates classes and/or functions that are
// required for the implementation of edmonds's optimum branching
// algorithm which should not be visible to the user. This way the
// global namespace remains unpolluted.
namespace detail {
    using namespace boost;

    // class OptimumBranching
    //
    // I encapsulate all the details of edmonds's algorithm inside a
    // class. This makes the code easier to read (and easier to write)
    // since the number of template declarations is reduced
    // considerably. Besides, all the utility functions used to
    // implement one algorithm conceptually do belong together.
    //
    // Note that any concept checks are performed in the function that
    // uses this class, so there is no need to repeat them here.
    template <bool TOptimumIsMaximum,
              bool TAttemptToSpan,
              bool TGraphIsDense,
              class TEdgeListGraph,
              class TVertexIndexMap,
              class TWeightMap,
              class TInputIterator,
              class TOutputIterator>
    class OptimumBranching {
    public:
        typedef TEdgeListGraph                                        Graph;
        typedef typename graph_traits<Graph>::edge_descriptor         Edge;
        typedef typename graph_traits<Graph>::vertex_descriptor       Vertex;
        typedef typename graph_traits<Graph>::edge_iterator           EdgeIter;
        typedef typename property_traits<TWeightMap>::value_type      weight_t;
        typedef typename property_traits<TVertexIndexMap>::value_type vertex_idx_t;

        // struct EdgeNode
        //
        // One unique EdgeNode object is created for each edge of the
        // input graph. Any containers then store pointers to these
        // objects. The edges of the graph F (which is described in
        // the document describing the implementation) are stored in
        // each EdgeNode object via the parent and children members.
        // For efficiency reasons a boolean member 'removed_from_F' is
        // also present in each EdgeNode object. If this member is
        // set, then the edge (which is also a vertex in F) was
        // removed during the expansion phase of the algorithm.
        struct EdgeNode {
            Edge                        edge;
            vertex_idx_t                source;
            vertex_idx_t                target;
            weight_t                    weight;
            EdgeNode                   *parent;
            std::vector<EdgeNode *>     children;
            bool                        removed_from_F;

            EdgeNode(const Edge &e,
                     const vertex_idx_t &s,
                     const vertex_idx_t &t,
                     const weight_t w)
                : edge(e), source(s), target(t), weight(w), parent(0),
                  removed_from_F(false)
            {
                ;
            }

            // operator<
            //
            // This is only used temporarily when sorting the
            // EdgeNodes by their sources. Once a radix-sort algorithm
            // has been implemented, this is no longer needed.
            bool        operator<(const EdgeNode &en) const
            {
                return source < en.source;
            }
        };

        // The data members of the OptimumBranching class. These
        // include both the input and the variables needed internally for the implemenationa.
        const TEdgeListGraph   &g;
        const TVertexIndexMap  &index;
        const TWeightMap       &weight;
        TInputIterator          roots_begin;
        TInputIterator          roots_end;
        TOutputIterator         out;

        // The constructor
        OptimumBranching(const TEdgeListGraph &g,
                         const TVertexIndexMap &index,
                         const TWeightMap &weight,
                         TInputIterator roots_begin,
                         TInputIterator roots_end,
                         TOutputIterator out)
            : g(g), index(index), weight(weight),
              roots_begin(roots_begin), roots_end(roots_end), out(out)
        {
            ;
        }

        // remove_from_F()
        //
        // It removes the EdgeNode en and all its ancestors from the
        // graph F (by resetting the parent and children members and
        // setting the flag removed_from_F). Any newly created roots
        // of F are inserted into F_roots. Note that the root of F
        // that is removed as a consequence is not actually removed
        // from F_roots. It is simply marked as removed via the
        // 'removed_from_F' flag.
        void remove_from_F(EdgeNode *en, std::vector<EdgeNode *> &F_roots)
        {
            // Note that en is inserted into F_roots as well. But
            // since it is marked as removed_from_F it will not cause
            // any trouble. This is more efficient than making sure
            // that only the siblings of en are inserted into F_roots.
            for ( ; en != 0; en = en->parent)
            {
                en->removed_from_F = true;
                BOOST_FOREACH (EdgeNode *child, en->children)
                {
                    F_roots.push_back(child);
                    child->parent = 0;
                }

                // free the memory used in en->children.
                std::vector<EdgeNode *>().swap(en->children);
            }
        }

        // sort_edges()
        //
        // sorts a vector of EdgeNode pointers with EdgeNode.source as
        // key using the radix-sort algorithm. Also, if there are
        // several EdgeNode pointers with the same source, the
        // function only keeps the one with optimum weight.
        void sort_edges(std::vector<EdgeNode *> &edge_vec)
        {
            const int byte_len = 8;
            const int num_buckets = 1u << byte_len;
            const unsigned digits = (sizeof (vertex_idx_t)) * std::numeric_limits<unsigned char>::digits;
            const unsigned mask = (1u << byte_len) - 1;

            std::vector< std::list<EdgeNode *> > buckets(num_buckets);

            for (unsigned i = 0; byte_len * i <= digits; ++i)
            {
                BOOST_FOREACH (EdgeNode *en, edge_vec)
                {
                    buckets[(en->source >> byte_len * i) & mask].push_back(en);
                }

                edge_vec.clear();
                BOOST_FOREACH (std::list<EdgeNode *> &bucket, buckets)
                {
                    BOOST_FOREACH (EdgeNode *en, bucket)
                    {
                        if (!edge_vec.empty() && edge_vec.back()->source == en->source)
                        {
                            bool en_is_better = TOptimumIsMaximum ?
                                en->weight > edge_vec.back()->weight :
                                en->weight < edge_vec.back()->weight;
                            if (en_is_better)
                            {
                                edge_vec.back() = en;
                            }
                        }
                        else
                        {
                            edge_vec.push_back(en);
                        }
                    }
                }

                buckets.clear();
                buckets.resize(num_buckets);
            }
        }

        // operator()
        //
        // This is the main function implementing Tarjan's
        // implementation of Edmonds's algorithm.
        void operator()()
        {
            std::vector<EdgeNode> all_edges;
            vertex_idx_t max_vertex_idx;

            // Create EdgeNodes for all the edges and find the maximum vertex
            // index. Note that we skip self-loops.
            max_vertex_idx = 0;
            BOOST_FOREACH (const Edge &e, edges(g))
            {
                if (source(e, g) == target(e, g))
                    continue;

                all_edges.push_back(EdgeNode (e, source(e, g), target(e, g), get(weight, e)));
                max_vertex_idx = std::max(max_vertex_idx, index[target(e, g)]);
            }

            // insert into in_edges[v] all edges entering v.

            //!! TODO !! If sparse graphs, I have to change the
            //representation of in_edges to a special kind of priority
            //queue that are able to be merged in log n time.

            std::vector< std::vector<EdgeNode *> > in_edges(max_vertex_idx + 1);
            std::vector<weight_t> edge_weight_change(max_vertex_idx + 1);
            BOOST_FOREACH (EdgeNode &en, all_edges)
            {
                in_edges[en.target].push_back(&en);
            }
            BOOST_FOREACH (std::vector<EdgeNode *> &edges, in_edges)
            {
                sort_edges(edges);
            }

            // Save the specified roots in a random access fashion.
            std::vector<bool> is_specified_root(max_vertex_idx + 1);
            std::vector<vertex_idx_t> final_roots;
            for ( ; roots_begin != roots_end; ++roots_begin)
            {
                is_specified_root[index[*roots_begin]] = true;
                final_roots.push_back(index[*roots_begin]);
            }

            // Initialize S, W, roots, cycles, lambda, enter, F, and min
            std::vector< std::vector<EdgeNode *> > cycle(max_vertex_idx + 1);
            std::vector<EdgeNode *> lambda(max_vertex_idx + 1);
            std::vector<vertex_idx_t> roots;
            disjoint_sets_with_storage<> S(2*(max_vertex_idx +1));
            disjoint_sets_with_storage<> W(2*(max_vertex_idx +1));
            std::vector<vertex_idx_t> min(max_vertex_idx + 1);
            std::vector<EdgeNode *> enter(max_vertex_idx + 1);
            std::vector<EdgeNode *> F;
            for (vertex_idx_t v = 0; v <= max_vertex_idx; ++v)
            {
                S.make_set(v);
                W.make_set(v);
                min[v] = v;
                if (!is_specified_root[v])
                    roots.push_back(v);
            }

            // Keep adding critical edges and contracting cycles while
            // doing a whole bunch of book-keeping.
            while (!roots.empty())
            {
                // Find an S-component with an entering edge
                vertex_idx_t cur_root = roots.back(); roots.pop_back();
                if (in_edges[cur_root].empty())
                {
                    final_roots.push_back(min[cur_root]);
                    continue;
                }

                // Find an optimum-weight edge entering cur_root

                //!! TODO !! We have to do this differently for sparse graphs.
                EdgeNode *critical_edge = in_edges[cur_root].front();
                BOOST_FOREACH (EdgeNode *en, in_edges[cur_root])
                {
                    bool en_is_better = TOptimumIsMaximum ?
                        en->weight > critical_edge->weight :
                        en->weight < critical_edge->weight;
                    if (en_is_better)
                    {
                        critical_edge = en;
                    }
                }

                // Do not add critical_edge if it worsens the total
                // weight and we are not attempting to span.
                if (!TAttemptToSpan)
                {
                    bool improves = TOptimumIsMaximum ?
                        critical_edge->weight > weight_t(0) :
                        critical_edge->weight < weight_t(0);
                    if (!improves)
                    {
                        final_roots.push_back(min[cur_root]);
                        continue;
                    }
                }

                // Insert critical_edge into "F" and let any edges in
                // cycle[cur_root] be its children.
                F.push_back(critical_edge);
                BOOST_FOREACH (EdgeNode *en, cycle[cur_root])
                {
                    en->parent = critical_edge;
                    critical_edge->children.push_back(en);
                }

                // If critical_edge is a leaf in "F", then add a
                // pointer to it.
                if (cycle[cur_root].empty())
                {
                    lambda[cur_root] = critical_edge;
                }

                // If adding critical_edge didn't create a cycle
                if (W.find_set(critical_edge->source) !=
                    W.find_set(critical_edge->target))
                {
                    enter[cur_root] = critical_edge;
                    W.union_set(critical_edge->source, critical_edge->target);
                }
                else // If adding critical_edge did create a cycle
                {
                    // Find the edges of the cycle, the
                    // representatives of the strong components in the
                    // cycle, and the least costly edge of the cycle.
                    std::vector<EdgeNode *> cycle_edges;
                    std::vector<vertex_idx_t> cycle_repr;
                    EdgeNode *least_costly_edge = critical_edge;
                    enter[cur_root] = 0;

                    cycle_edges.push_back(critical_edge);
                    cycle_repr.push_back(S.find_set(critical_edge->target));
                    for (vertex_idx_t v = S.find_set(critical_edge->source);
                         enter[v] != 0; v = S.find_set(enter[v]->source))
                    {
                        cycle_edges.push_back(enter[v]);
                        cycle_repr.push_back(v);
                        bool is_less_costly = TOptimumIsMaximum ?
                            enter[v]->weight < least_costly_edge->weight :
                            enter[v]->weight > least_costly_edge->weight;
                        if (is_less_costly)
                        {
                            least_costly_edge = enter[v];
                        }
                    }
                    // change the weight of the edges entering
                    // vertices of the cycle.
                    //!! TODO !! Change this for sparse graphs
                    BOOST_FOREACH (EdgeNode *en, cycle_edges)
                    {
                        edge_weight_change[S.find_set(en->target)] =
                            least_costly_edge->weight - en->weight;
                    }

                    // Save the vertex that would be root if the newly
                    // created strong component would be a root.
                    vertex_idx_t cycle_root =
                        min[S.find_set(least_costly_edge->target)];

                    // Union all components of the cycle into one component.
                    vertex_idx_t new_repr = cycle_repr.front();
                    BOOST_FOREACH (vertex_idx_t v, cycle_repr)
                    {
                        S.link(v, new_repr);
                        new_repr = S.find_set(new_repr);
                    }
                    min[new_repr] = cycle_root;
                    roots.push_back(new_repr);
                    cycle[new_repr].swap(cycle_edges);

                    //!! TODO !! Needs to be changed for sparse graphs.
                    BOOST_FOREACH (vertex_idx_t v, cycle_repr)
                    {
                        BOOST_FOREACH (EdgeNode *en, in_edges[v])
                        {
                            en->weight += edge_weight_change[v];
                        }
                    }

                    // Merge all in_edges of the cycle into one list.
                    //!! TODO !! needs to be changed for sparse graphs.
                    std::vector<EdgeNode *> new_in_edges;
                    for (unsigned i = 1; i < cycle_repr.size(); ++i)
                    {
                        typedef typename std::vector<EdgeNode *>::iterator Iter;
                        Iter i1 = in_edges[cycle_repr[i]].begin();
                        Iter e1 = in_edges[cycle_repr[i]].end();
                        Iter i2 = in_edges[cycle_repr[i-1]].begin();
                        Iter e2 = in_edges[cycle_repr[i-1]].end();

                        ///*
                        while (i1 != e1 || i2 != e2)
                        {
                            while (i1 != e1 && S.find_set((*i1)->source) == new_repr)
                            {
                                ++i1;
                            }
                            while (i2 != e2 && S.find_set((*i2)->source) == new_repr)
                            {
                                ++i2;
                            }

                            if (i1 == e1 && i2 == e2)
                                break;

                            if (i1 == e1)
                            {
                                new_in_edges.push_back(*i2);
                                ++i2;
                            }
                            else if (i2 == e2)
                            {
                                new_in_edges.push_back(*i1);
                                ++i1;
                            }
                            else if (((*i1)->source) < ((*i2)->source))
                            {
                                new_in_edges.push_back(*i1);
                                ++i1;
                            }
                            else if ((*i1)->source > (*i2)->source)
                            {
                                new_in_edges.push_back(*i2);
                                ++i2;
                            }
                            else // if the sources are equal
                            {
                                bool i1_is_better = TOptimumIsMaximum ?
                                    (*i1)->weight > (*i2)->weight :
                                    (*i1)->weight < (*i2)->weight;
                                if (i1_is_better)
                                {
                                    new_in_edges.push_back(*i1);
                                }
                                else
                                {
                                    new_in_edges.push_back(*i2);
                                }
                                ++i1;
                                ++i2;
                            }
                        }
                        in_edges[cycle_repr[i]].swap(new_in_edges);
                        new_in_edges.clear();
                    }
                    in_edges[new_repr].swap(in_edges[cycle_repr.back()]);
                    edge_weight_change[new_repr] = weight_t(0);
                    //*/
                }
            } // while (!roots.empty())

            // Extract the optimum branching

            // Find all roots of F.
            std::vector<EdgeNode *> F_roots;
            BOOST_FOREACH (EdgeNode *en, F)
            {
                if (en->parent == 0)
                {
                    F_roots.push_back(en);
                }
            }

            // Remove edges entering the root nodes.
            BOOST_FOREACH (vertex_idx_t v, final_roots)
            {
                if (lambda[v] != 0)
                {
                    remove_from_F(lambda[v], F_roots);
                }
            }

            while (!F_roots.empty())
            {
                EdgeNode *en = F_roots.back(); F_roots.pop_back();
                if (en->removed_from_F)
                    continue;

                *out = en->edge;
                ++out;
                remove_from_F(lambda[en->target], F_roots);
            }
        }

    };
}

template <bool TOptimumIsMaximum,
          bool TAttemptToSpan,
          bool TGraphIsDense,
          class TEdgeListGraph,
          class TVertexIndexMap,
          class TWeightMap,
          class TInputIterator,
          class TOutputIterator>
void
edmonds_optimum_branching(TEdgeListGraph &g,
                          TVertexIndexMap index,
                          TWeightMap weight,
                          TInputIterator roots_begin,
                          TInputIterator roots_end,
                          TOutputIterator out)
{
    using namespace boost;

    typedef typename graph_traits<TEdgeListGraph>::edge_descriptor    Edge;
    typedef typename graph_traits<TEdgeListGraph>::vertex_descriptor  Vertex;
    typedef typename graph_traits<TEdgeListGraph>::edge_iterator      EdgeIter;
    typedef typename property_traits<TWeightMap>::value_type          weight_t;

    function_requires< EdgeListGraphConcept<TEdgeListGraph> >();
    function_requires< ReadablePropertyMapConcept<TWeightMap, Edge> >();
    function_requires< ReadablePropertyMapConcept<TVertexIndexMap, Vertex> >();
    function_requires< InputIteratorConcept<TInputIterator> >();
    function_requires< OutputIteratorConcept<TOutputIterator, Edge> >();
    //!! Add the following requirements:
    //
    // property_traits<TVertexIndexMap>::value_type is a built-in
    // integral type, or perhaps require that it can be used to index
    // into arrays.
    //
    // property_traits<TWeightMap>::value_type is a numeric type that
    // handles the operations +, -, and <.
    //
    // TInputIterator's value type is Vertex
    // TOutputIterator's value type is Edge

    ::detail::OptimumBranching<TOptimumIsMaximum, TAttemptToSpan,
        TGraphIsDense, TEdgeListGraph, TVertexIndexMap, TWeightMap,
        TInputIterator, TOutputIterator>
          optimum_branching(g, index, weight, roots_begin, roots_end, out);
    optimum_branching();
}

#endif // not EDMONDS_OPTIMUM_BRANCHING_IMPL_HPP