#include "pipeline.hpp"

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <CGAL/boost/graph/Face_filtered_graph.h>

#include <boost/property_map/property_map.hpp>
#include <boost/foreach.hpp>
#include <iostream>
#include <fstream>
#include <map>

typedef CGAL::Exact_predicates_exact_constructions_kernel    Kernel;
typedef Kernel::Point_3                                      Point;

typedef CGAL::Surface_mesh<Point>                            Mesh;
typedef boost::graph_traits<Mesh>::face_descriptor           face_descriptor;
typedef boost::graph_traits<Mesh>::faces_size_type           faces_size_type;

typedef Mesh::Property_map<face_descriptor, faces_size_type> FCCmap;
typedef CGAL::Face_filtered_graph<Mesh> Filtered_graph;

typedef boost::graph_traits<Mesh>::halfedge_descriptor       halfedge_descriptor;
typedef boost::graph_traits<Mesh>::edge_descriptor           edge_descriptor;

namespace PMP = CGAL::Polygon_mesh_processing;

using namespace std;

struct halfedge2edge
{
    halfedge2edge(const Mesh& m, std::vector<edge_descriptor>& edges)
    : m_mesh(m), m_edges(edges)
    {}
    void operator()(const halfedge_descriptor& h) const
    {
        m_edges.push_back(edge(h, m_mesh));
    }
    const Mesh& m_mesh;
    std::vector<edge_descriptor>& m_edges;
};

double target_edge_length = 6.0;//4.0;//0.04;               // arbitrary remeshing parameters, move to input
unsigned int nb_iter = 2;//3;

size_t make_vector_of_connected_components(
    Mesh pmesh,
    pair<int, int> pair_,
    vector<Mesh> &mesh_vec,
    vector<pair<int, int> >  &pair_vec
){
    FCCmap fccmap = pmesh.add_property_map<face_descriptor, faces_size_type>("f:CC").first;
    faces_size_type num = PMP::connected_components(pmesh,fccmap);
    vector< pair<size_t, size_t> > component_size(num);

    for(size_t i=0; i < num; i++) component_size[i] = make_pair(i,0);

    BOOST_FOREACH(face_descriptor f, faces(pmesh))  ++component_size[fccmap[f]].second;

    sort(component_size.begin(), component_size.end(), PMP::internal::MoreSecond());
    vector<size_t> cc_to_keep;

    for(size_t i=0; i<num; ++i) {
      Mesh tmp_mesh = pmesh;
      cc_to_keep.clear();
      cc_to_keep.push_back( component_size[i].first );
      PMP::keep_connected_components(tmp_mesh, cc_to_keep, fccmap);
      // use defaultfor: const NamedParameters &  	np   above.
      if(tmp_mesh.number_of_faces () > 0 ){
          mesh_vec.push_back(tmp_mesh);
          pair_vec.push_back(pair_);
      }
    }
}

int mkvec_cc(
        vector<Mesh> &patch_vec,
        vector<pair<int, int> >  &pair_vec
            )
{
    vector<Mesh> new_patch_vec;
    vector<pair<int, int> >  new_pair_vec;
    const double bound = cos(0.75 * CGAL_PI);

    int idx =0;
    BOOST_FOREACH(Mesh pmesh, patch_vec){
        cout << "\n idx = " << idx ;

        make_vector_of_connected_components( pmesh, pair_vec.at(idx), new_patch_vec, new_pair_vec );

        cout << ", new_pair_vec.size() = " << new_pair_vec.size();
        idx++;
    }

    for (int i=0; i<patch_vec.size(); ++i){
            cout << "\npair_vec[" << i << "] = (" << pair_vec[i].first << "," << pair_vec[i].second << ")";
    }

    for (int i=0; i<new_patch_vec.size(); ++i){
            cout << "\nnew_pair_vec[" << i << "] = (" << new_pair_vec[i].first << "," << new_pair_vec[i].second << ")";
    }

    patch_vec.swap(new_patch_vec);
    pair_vec.swap(new_pair_vec);

    // isotropic remeshing of each patch, to eliminate degenerate or ultra thin triangles.
    // does _not_ fix degenerate triangles in tetrahedral meshing of certain polyhedra combinations
//     int i =0;
//     BOOST_FOREACH(Mesh pmesh, patch_vec){
//         cout << "\n remeshing patch" << endl;
//         std::vector<edge_descriptor> border;
//         PMP::border_halfedges(faces(pmesh),
//                           pmesh,
//                           boost::make_function_output_iterator(halfedge2edge(pmesh, border))
//                          );
//         PMP::split_long_edges(border, target_edge_length, pmesh);
//         PMP::isotropic_remeshing(faces(pmesh),
//                              target_edge_length,
//                              pmesh,
//                              PMP::parameters::number_of_iterations(nb_iter)
//                              .protect_constraints(true)//i.e. protect border, here
//                              //can take a very long time if remesh triangles are too small
//                             );
//         patch_vec.at(i) = pmesh;
// //         std::string filename = "output_files2";
// //             filename += "/remeshed_patch" + to_string(i) + ".off";
// //             std::ofstream outfile(filename);
// //             outfile << pmesh;//patch_vec.at(i);
// //             outfile.close();
//         i++;
//
//         cout << "... finished remeshing patch" << endl;
//     }
//     cout << "\n finished all remeshing" << endl;
    return patch_vec.size();
}


/*
 * Copyright (c) 2012, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author: Tobias Kunz <tobias@gatech.edu>
 * Date: 05/2012
 *
 * Humanoid Robotics Lab      Georgia Institute of Technology
 * Director: Mike Stilman     http://www.golems.org
 *
 * Algorithm details and publications:
 * http://www.golems.org/node/1570
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include <iostream>
#include <cstdio>
#include <Eigen/Core>
#include "Trajectory.h"
#include "Path.h"

using namespace std;
using namespace Eigen;

int main() {
	list<VectorXd> waypoints;
	VectorXd waypoint(3);
	waypoint << 0.0, 0.0, 0.0;
	waypoints.push_back(waypoint);
	waypoint << 0.0, 0.2, 1.0;
	waypoints.push_back(waypoint);
	waypoint << 0.0, 3.0, 0.5;
	waypoints.push_back(waypoint);
	waypoint << 1.1, 2.0, 0.0;
	waypoints.push_back(waypoint);
	waypoint << 1.0, 0.0, 0.0;
	waypoints.push_back(waypoint);
	waypoint << 0.0, 1.0, 0.0;
	waypoints.push_back(waypoint);
	waypoint << 0.0, 0.0, 1.0;
	waypoints.push_back(waypoint);

	VectorXd maxAcceleration(3);
	maxAcceleration << 1.0, 1.0, 1.0;
	VectorXd maxVelocity(3);
	maxVelocity << 1.0, 1.0, 1.0;

	Trajectory trajectory(Path(waypoints, 0.1), maxVelocity, maxAcceleration);
	trajectory.outputPhasePlaneTrajectory();
	if(trajectory.isValid()) {
		double duration = trajectory.getDuration();
		cout << "Trajectory duration: " << duration << " s" << endl << endl;
		cout << "Time      Position                  Velocity" << endl;
		for(double t = 0.0; t < duration; t += 0.1) {
			printf("%6.4f   %7.4f %7.4f %7.4f   %7.4f %7.4f %7.4f\n", t, trajectory.getPosition(t)[0], trajectory.getPosition(t)[1], trajectory.getPosition(t)[2],
				trajectory.getVelocity(t)[0], trajectory.getVelocity(t)[1], trajectory.getVelocity(t)[2]);
		}
		printf("%6.4f   %7.4f %7.4f %7.4f   %7.4f %7.4f %7.4f\n", duration, trajectory.getPosition(duration)[0], trajectory.getPosition(duration)[1], trajectory.getPosition(duration)[2],
			trajectory.getVelocity(duration)[0], trajectory.getVelocity(duration)[1], trajectory.getVelocity(duration)[2]);
	}
	else {
		cout << "Trajectory generation failed." << endl;
	}

	string s;
	cin >> s;
}


/*
 (c) 2014 Glen Joseph Fernandes
 glenjofe at gmail dot com

 Distributed under the Boost Software
 License, Version 1.0.
 http://boost.org/LICENSE_1_0.txt
*/
#ifndef BOOST_ALIGN_DETAIL_IS_ALIGNMENT_CONSTANT_HPP
#define BOOST_ALIGN_DETAIL_IS_ALIGNMENT_CONSTANT_HPP

#include <boost/align/detail/integral_constant.hpp>
#include <cstddef>

namespace boost {
    namespace alignment {
        namespace detail {
            template<std::size_t N>
            struct is_alignment_constant
                : integral_constant<bool,
                    (N > 0) && ((N & (N - 1)) == 0)> {
            };
        }
    }
}

#endif


#ifndef MANIFOLDS_FUNCTIONS_SIMPLIFY_VARIADIC_HH
#define MANIFOLDS_FUNCTIONS_SIMPLIFY_VARIADIC_HH

#include "simplify.hh"
#include <iostream>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/back_inserter.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/sort.hpp>
#include <boost/mpl/at.hpp>

namespace manifolds {

template <class Tuple>
void PrintTuple(const Tuple &t, int_<std::tuple_size<Tuple>::value>) {
  std::cout << '\n';
}

template <class Tuple, int i, class = typename std::enable_if<
                                  i != std::tuple_size<Tuple>::value>::type>
void PrintTuple(const Tuple &t, int_<i>) {
  std::cout << '(' << std::get<i>(t) << ") ";
  PrintTuple(t, int_<i + 1>());
}

template <class F> auto WrapIf(bool_<true>, F f, int i) {
  SIMPLIFY_INFO("(" << i << ", true) ");
  return f.GetFunctions();
}

template <class F> auto WrapIf(bool_<false>, F f, int i) {
  SIMPLIFY_INFO("(" << i << ", false) ");
  return make_my_tuple(f);
}

template <template <class...> class Variadic, class... Functions,
          std::size_t... indices>
auto FlattenVariadic(const tuple<Functions...> &v,
                     std::integer_sequence<std::size_t, indices...>) {
  SIMPLIFY_INFO("Flattening: ");
  auto f = tuple_cat(WrapIf(bool_<IsVariadic<Variadic, Functions>::value>(),
                            std::get<indices>(v), indices)...);
  SIMPLIFY_INFO("\nFlattened: ");
#ifdef PRINT_SIMPLIFIES
  PrintTuple(f, int_<0>());
#endif
  return f;
}

template <template <class...> class Variadic, std::size_t i, std::size_t j,
          class Tuple>
auto MergeV(const Tuple &t, std::false_type) {
  static const std::size_t max = i > j ? i : j;
  static const std::size_t min = i + j - max;
  auto middle = Variadic<typename std::tuple_element<i, Tuple>::type,
                         typename std::tuple_element<j, Tuple>::type>(
      std::get<i>(t), std::get<j>(t));
  return insert_element<i>(remove_element<min>(remove_element<max>(t)),
                           Simplify(middle));
}

template <template <class...> class, std::size_t i, std::size_t j, class Tuple>
auto MergeV(const Tuple &t, std::true_type) {
  return t;
}

template <template <class...> class Variadic, class Tuple>
auto SimplifyVHelper(const Tuple &t, int_<std::tuple_size<Tuple>::value - 1>) {
  return MakeVariadic<Variadic>(t);
}

template <template <class...> class Variadic, int i, class Tuple,
          class = typename std::enable_if<i + 1 !=
                                          std::tuple_size<Tuple>::value>::type>
auto SimplifyVHelper(const Tuple &t, int_<i>) {
  typedef Variadic<typename std::tuple_element<i, Tuple>::type,
                   typename std::tuple_element<i + 1, Tuple>::type> VType;
  SIMPLIFY_INFO("Checking if " << VType(std::get<i>(t), std::get<i + 1>(t))
                               << " simplifies: ");
  typedef SimplifiedType<VType> SType;
  typedef typename std::is_same<VType, SType>::type Simplified;
  auto next = MergeV<Variadic, i, i + 1>(t, Simplified());
  return SimplifyVHelper<Variadic>(next, int_<i + Simplified::value>());
}

template <std::size_t iter = 0> struct Wrapper {
  template <template <class...> class Variadic, int i, class Tuple, int j,
            class = typename std::enable_if<
                i + 1 != std::tuple_size<Tuple>::value &&j !=
                std::tuple_size<Tuple>::value>::type>
  static auto MergeAllV(const Tuple &t, int_<i>, int_<j>) {
    typedef Variadic<typename std::tuple_element<i, Tuple>::type,
                     typename std::tuple_element<j, Tuple>::type> VType;
    typedef SimplifiedType<VType> SType;
    SIMPLIFY_INFO(
        "Simplifying:\n  " << (std::get<i>(t)) << "\n  " << (std::get<j>(t))
                           << "\ninto "
                           << Simplify(MakeVariadic<Variadic>(make_my_tuple(
                                  std::get<i>(t), std::get<j>(t)))) << "\n");
    typedef typename std::is_same<SType, VType>::type Simplified;
    auto next = MergeV<Variadic, i, j>(t, Simplified());
    return Wrapper<iter + 1>::template MergeAllV<Variadic>(
        next, int_<i>(), int_<j + Simplified::value>());
  }

  template <template <class...> class Variadic, int i, class Tuple,
            class = typename std::enable_if<
                i + 1 != std::tuple_size<Tuple>::value>::type>
  static auto MergeAllV(const Tuple &t, int_<i>,
                        int_<std::tuple_size<Tuple>::value>) {
    return Wrapper<iter + 1>::template MergeAllV<Variadic>(t, int_<i + 1>(),
                                                           int_<i + 2>());
  }

  template <template <class...> class Variadic, int i, class Tuple>
  static auto MergeAllV(const Tuple &t, int_<std::tuple_size<Tuple>::value - 1>,
                        int_<i>) {
    return t;
  }
};

template <class Tuple, std::size_t... indices>
auto SimplifyIndividuals(const Tuple &t,
                         std::integer_sequence<std::size_t, indices...>) {
  return make_my_tuple(Simplify(std::get<indices>(t))...);
}

template <class... indices> struct FunctionReOrgComparator {
  template <class, class, class = void> struct apply_h;

  template <int i, int j>
  struct apply_h<int_<i>, int_<j>,
                 typename std::enable_if<i != j>::type> : bool_<(i < j)> {};

  template <int i, int j, class... Rest>
  struct apply_h<int_<i>, list<int_<j>, Rest...> > : bool_<(i < j)> {};

  template <int i, int j, class... Rest>
  struct apply_h<list<int_<i>, Rest...>, int_<j> > : bool_<(i < j)> {};

  template <int i, class... Rest1, class... Rest2>
  struct apply_h<int_<i>,
                 list<list<Rest1...>, Rest2...> > : apply_h<int_<i>,
                                                            list<Rest1...> > {};

  template <int i, class... Rest1, class... Rest2>
  struct apply_h<list<list<Rest1...>, Rest2...>,
                 int_<i> > : apply_h<list<Rest1...>, int_<i> > {};

  template <int i, class... Rest3, class... Rest1, class... Rest2>
  struct apply_h<list<int_<i>, Rest3...>,
                 list<list<Rest1...>, Rest2...> > : apply_h<int_<i>,
                                                            list<Rest1...> > {};

  template <int i, class... Rest3, class... Rest1, class... Rest2>
  struct apply_h<list<list<Rest1...>, Rest2...>,
                 list<int_<i>, Rest3...> > : apply_h<list<Rest1...>,
                                                     int_<i> > {};

  template <class... Rest1, class... Rest2, class... IRest1, class... IRest2>
  struct apply_h<list<list<IRest1...>, Rest1...>,
                 list<list<IRest2...>, Rest2...> > : apply_h<list<IRest1...>,
                                                             list<IRest2...> > {
  };

  template <class... Rest1, class... Rest2, class... IRest>
  struct apply_h<list<list<IRest...>, Rest1...>,
                 list<list<IRest...>, Rest2...> > : apply_h<list<Rest1...>,
                                                            list<Rest2...> > {};

  template <int i, int j, class... Rest1, class... Rest2>
  struct apply_h<list<int_<i>, Rest1...>, list<int_<j>, Rest2...>,
                 typename std::enable_if<i != j>::type> : bool_<(i < j)> {};

  template <class T> struct apply_h<T, T> : std::false_type {};

  template <int i, class... Rest1, class... Rest2>
  struct apply_h<list<int_<i>, Rest1...>, list<int_<i>, Rest2...>,
                 typename std::enable_if<
                     !are_same<holder<Rest1...>, holder<Rest2...> >::value>::
                     type> : apply_h<list<Rest1...>, list<Rest2...> > {};

  template <class Void>
  struct apply_h<list<>, list<>, Void> : std::false_type {};

  template <class i, class... is>
  struct apply_h<list<>, list<i, is...> > : std::true_type {};

  template <class i, class... is>
  struct apply_h<list<i, is...>, list<> > : std::false_type {};

  template <class i1, class i2>
  struct apply : apply_h<typename nth<i1::value, indices...>::type,
                         typename nth<i2::value, indices...>::type> {};
};

template <template <class...> class V, class... Functions, std::size_t... tis>
auto SortElements(V<Functions...> v,
                  std::integer_sequence<std::size_t, tis...>) {
  auto t = v.GetFunctions();
  typedef decltype(t) Tuple;
  typedef boost::mpl::range_c<std::size_t, 0, std::tuple_size<Tuple>::value>
  indices;
  typedef typename boost::mpl::copy<
      indices, boost::mpl::back_inserter<boost::mpl::vector<> > >::type
  indices_v;
  typedef typename boost::mpl::sort<
      indices_v, FunctionReOrgComparator<typename std::tuple_element<
                     tis, Tuple>::type::indices...> >::type sorted_indices;
  return V<typename nth<boost::mpl::at_c<sorted_indices, tis>::type::value,
                        Functions...>::
               type...>(make_my_tuple(
      std::get<boost::mpl::at_c<sorted_indices, tis>::type::value>(t)...));
}

template <template <class...> class V, class Tuple, bool b>
auto SimplifyTheV(const Tuple &t, int_<1>, bool_<b>) {
  return std::get<0>(t);
}

template <template <class...> class V, class Tuple>
auto SimplifyTheV(const Tuple &t, int_<2>, std::false_type) {
  return MakeVariadic<V>(t);
}

template <template <class...> class V, class Tuple>
auto SimplifyTheV(const Tuple &t, int_<2>, std::true_type) {
  return SortElements(MakeVariadic<V>(t), std::make_index_sequence<2>());
}

template <template <class...> class V, class Tuple, int i,
          class = typename std::enable_if<(i > 2)>::type>
auto SimplifyTheV(const Tuple &t, int_<i>, std::true_type) {
  auto result = Wrapper<>::template MergeAllV<V>(t, int_<0>(), int_<1>());
  static const int size = std::tuple_size<decltype(result)>::value;
  return SortElements(MakeVariadic<V>(result),
                      std::make_index_sequence<size>());
}

template <template <class...> class V, class Tuple, int i,
          class = typename std::enable_if<(i > 2)>::type>
auto SimplifyTheV(const Tuple &t, int_<i>, std::false_type) {
  return SimplifyVHelper<V>(t, int_<0>());
}

template <template <class...> class V, class Tuple, bool abelian>
auto SimplifyV(const Tuple &t, bool_<abelian> b) {
  auto t2 = FlattenVariadic<V>(
      t, std::make_index_sequence<std::tuple_size<Tuple>::value>());
  auto t3 = SimplifyIndividuals(
      t2, std::make_index_sequence<std::tuple_size<decltype(t2)>::value>());
  static const int size = std::tuple_size<decltype(t3)>::value;
  return SimplifyTheV<V>(t3, int_<size>(), b);
}

template <class TupleOut, class TupleIn, std::size_t... iouts>
auto DistributeComposition(TupleOut to, TupleIn ti,
                           std::integer_sequence<std::size_t, iouts...>) {
  return make_my_tuple(std::get<iouts>(to)(ti)...);
}

// Careful when calling this!
// Outer needs to be an instance of Variadic<...>
// and Inner needs to be an instance of FunctionMatrix<...>
template <template <class...> class Variadic,
          template <class...> class Composition, class Outer, class Inner>
auto DistributeComposition(Composition<Outer, Inner> c) {
  auto t = c.GetFunctions();
  auto it = std::get<1>(t);
  auto ot = std::get<0>(t).GetFunctions();
  auto tr = DistributeComposition(
      ot, it, std::make_index_sequence<std::tuple_size<decltype(ot)>::value>());
  return MakeVariadic<Variadic>(tr);
}
}

#endif


#pragma once
#include <atomic>
#include <mutex>

#include <Fastor/Fastor.h>
#include <boost/atomic.hpp>
#include <boost/atomic/atomic_flag.hpp>
#include <boost/preprocessor/cat.hpp>
#include <eigen3/Eigen/Core>

#include "Typedefs.hpp"

namespace FeltElements
{
struct Attributes;

namespace Solver
{
struct Params
{
	std::size_t num_steps{1};
	std::size_t num_force_increments{1};
};

struct Stats
{
	std::atomic_uint step_counter{0};
	std::atomic_uint force_increment_counter{0};
	std::atomic<Scalar> max_norm{0};
};

class Base
{
public:
	Base(Mesh & mesh, Attributes & attrs, Params params) noexcept
		: m_mesh{mesh}, m_attrs{attrs}, m_params{std::move(params)}
	{
	}
	virtual ~Base() = default;
	virtual void solve() = 0;

protected:
	class Pauser
	{
	public:
		struct Unpauser
		{
			Unpauser(Unpauser const &) = delete;
			Unpauser(Unpauser &&) = delete;
			void operator=(Unpauser const &) = delete;
			void operator=(Unpauser &&) = delete;
			~Unpauser()
			{
				pause.flag.clear(boost::memory_order_release);
				pause.flag.notify_one();
				pause.lock.unlock();
			}
			Pauser & pause;
		};

		Unpauser scoped_pause() noexcept
		{
			flag.test_and_set(boost::memory_order_acquire);
			lock.lock();
			return Unpauser{*this};
		}

		void wait_while_paused() noexcept
		{
			if (!flag.test(boost::memory_order_relaxed))
				return;
			lock.unlock();
			flag.wait(true, boost::memory_order_relaxed);
			lock.lock();
		}

	private:
		std::mutex lock{};
		boost::atomic_flag flag{};
		std::scoped_lock<std::mutex> m_running{lock};
	};

	void update_elements_stiffness_and_residual();
	Scalar find_epsilon() const;

public:
	using Unpauser = Pauser::Unpauser;
	Pauser pauser{};
	Stats stats{};

protected:
	Mesh & m_mesh;
	Attributes & m_attrs;
	Params m_params;
};

class Matrix : public Base
{
#define EIGEN_FASTOR_ALIGN BOOST_PP_CAT(Eigen::Aligned, FASTOR_MEMORY_ALIGNMENT_VALUE)
	template <int rows, int cols>
	using EigenConstTensorMap =
		Eigen::Map<Eigen::Matrix<Scalar, rows, cols, Eigen::RowMajor> const, EIGEN_FASTOR_ALIGN>;
	using VerticesMatrix = Eigen::Matrix<Scalar, Eigen::Dynamic, 3, Eigen::RowMajor>;
	using EigenFixedDOFs = Eigen::VectorXd;

public:
	using EigenMapOvmVertices = Eigen::Map<VerticesMatrix const>;
	using EigenMapTensorVertices = Eigen::Map<
		VerticesMatrix const,
		Eigen::Unaligned,
		Eigen::Stride<(FASTOR_MEMORY_ALIGNMENT_VALUE / sizeof(Scalar)), 1>>;

	using Base::Base;
	void solve() final;
};	// namespace Matrix

class Gauss : public Base
{
public:
	using Base::Base;
	void solve() final;
};

}  // namespace Solver
}  // namespace FeltElements


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_COMPARE_GREATER_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COMPARE_GREATER_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-reduction
    Function object implementing compare_greater capabilities

    return a bool that is the result of the lexicographic
    test for > on all elements of the entries

    It is probably not what you wish. Have a look to @ref is_greater

    @par Semantic:

    For every parameters @c x and @c y :

    @code
    bool r = compare_greater(x,y);
    @endcode

    is similar to:

    @code
      bool r = false;
      for(std::size_t i=0;i<cardinal_of<A0>::value;++i)
      {
        if (x[i] > y[i])  {r =  true; break;}
        if (y[i] > x[i])  {r = false; break;}
      }
    @endcode

    @see is_greater
  **/
  bool compare_greater(Value const& v0, Value const& v1);
} }
#endif

#include <boost/simd/function/scalar/compare_greater.hpp>
#include <boost/simd/function/simd/compare_greater.hpp>

#endif


#ifndef _maps_DepthImage_hpp_
#define _maps_DepthImage_hpp_

#include <vector>
#include <memory>
#include <Eigen/Geometry>
#include "Types.hpp"

namespace pcl {
  class RangeImage;
}

namespace maps {

class DepthImage {
public:
  enum Type {
    TypeDisparity,
    TypeDepth,
    TypeRange,
  };

  enum AccumulationMethod {
    AccumulationMethodClosest,
    AccumulationMethodFurthest,
    AccumulationMethodMean,
    AccumulationMethodMedian,
    AccumulationMethodClosestPercentile,
    AccumulationMethodRobustBlend,
  };

  typedef std::shared_ptr<DepthImage> Ptr;

  struct Helper;

public:
  DepthImage();
  ~DepthImage();

  void setSize(const int iWidth, const int iHeight);
  int getWidth() const;
  int getHeight() const;

  const float getInvalidValue(const Type iType) const;

  bool setData(const std::vector<float>& iData, const Type iType);
  const std::vector<float>& getData(const Type iType) const;

  void setOrthographic(const bool iVal);
  bool isOrthographic() const;

  void setPose(const Eigen::Isometry3f& iPose);
  Eigen::Isometry3f getPose() const;

  void setCalib(const Eigen::Matrix3f& iCalib);
  Eigen::Matrix3f getCalib() const;

  void setProjector(const Eigen::Projective3f& iProjector);
  Eigen::Projective3f getProjector() const;

  void setAccumulationMethod(const AccumulationMethod iMethod);

  void create(const maps::PointCloud::Ptr& iCloud);

  maps::PointCloud::Ptr getAsPointCloud() const;
  boost::shared_ptr<pcl::RangeImage> getAsRangeImage() const;
  Eigen::Vector3f project(const Eigen::Vector3f& iPoint,
                          const Type iType) const;
  Eigen::Vector3f unproject(const Eigen::Vector3f& iPoint,
                            const Type iType) const;

protected:
  std::shared_ptr<Helper> mHelper;
};

}

#endif


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/mlogeps2.hpp>
#include <boost/simd/constant/eps.hpp>
// #include <boost/simd/function/log.hpp>
// #include <boost/simd/function/sqr.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>
//TODO
STF_CASE_TPL( "Check mlogeps2 behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mlogeps2;
  using boost::simd::Mlogeps2;

  STF_TYPE_IS(decltype(Mlogeps2<T>()), T);
  STF_EQUAL(Mlogeps2<T>(), T(0));
  STF_EQUAL(mlogeps2( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check mlogeps2 behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mlogeps2;
  using boost::simd::Mlogeps2;

  STF_TYPE_IS(decltype(Mlogeps2<T>()), T);
//   STF_IEEE_EQUAL(Mlogeps2<T>(), log(sqr(Eps<T>()));
//   STF_IEEE_EQUAL(mlogeps2( as(T{}) ), log(sqr(Eps<T>()));
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_MAX_INTERVAL_GAP_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_MAX_INTERVAL_GAP_HPP

#include <cstddef>
#include <queue>
#include <utility>
#include <vector>

#include <boost/core/ref.hpp>
#include <boost/range.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/type_traits/remove_reference.hpp>

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/algorithms/detail/sweep.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace max_interval_gap
{

// the class Interval must provide the following:
// * must define the type value_type
// * must define the type difference_type
// * must have the methods:
//   value_type get<Index>() const
//   difference_type length() const
// where an Index value of 0 (resp., 1) refers to the left (resp.,
// right) endpoint of the interval

template <typename Interval>
class sweep_event
{
public:
    typedef Interval interval_type;
    typedef typename Interval::value_type time_type;

    sweep_event(Interval const& interval, bool start_event = true)
        : m_interval(boost::cref(interval))
        , m_start_event(start_event)
    {}

    inline bool is_start_event() const
    {
        return m_start_event;
    }

    inline interval_type const& interval() const
    {
        return m_interval;
    }

    inline time_type time() const
    {
        return (m_start_event)
            ? interval().template get<0>()
            : interval().template get<1>();
    }

    inline bool operator<(sweep_event const& other) const
    {
        if (! math::equals(time(), other.time()))
        {
            return time() < other.time();
        }
        // a start-event is before an end-event with the same event time
        return is_start_event() && ! other.is_start_event();
    }

private:
    boost::reference_wrapper<Interval const> m_interval;
    bool m_start_event;
};

template <typename Event>
struct event_greater
{
    inline bool operator()(Event const& event1, Event const& event2) const
    {
        return event2 < event1;
    }
};

struct initialization_visitor
{
    template <typename Range, typename PriorityQueue, typename EventVisitor>
    static inline void apply(Range const& range,
                             PriorityQueue& queue,
                             EventVisitor&)
    {
        BOOST_GEOMETRY_ASSERT(queue.empty());

        // it is faster to build the queue directly from the entire
        // range, rather than insert elements one after the other
        PriorityQueue pq(boost::begin(range), boost::end(range));
        std::swap(pq, queue);
    }
};

template <typename Event>
class event_visitor
{
    typedef typename Event::time_type event_time_type;
    typedef typename Event::interval_type::difference_type difference_type;

    typedef typename boost::remove_const
        <
            typename boost::remove_reference
                <
                    event_time_type
                >::type
        >::type bare_time_type;

public:
    event_visitor()
        : m_overlap_count(0)
        , m_max_gap_left(0)
        , m_max_gap_right(0)
    {}

    template <typename PriorityQueue>
    inline void apply(Event const& event, PriorityQueue& queue)
    {
        if (event.is_start_event())
        {
            ++m_overlap_count;
            queue.push(Event(event.interval(), false));
        }
        else
        {
            --m_overlap_count;
            if (m_overlap_count == 0 && ! queue.empty())
            {
                // we may have a gap
                BOOST_GEOMETRY_ASSERT(queue.top().is_start_event());

                event_time_type next_event_time
                    = queue.top().interval().template get<0>();
                difference_type gap = next_event_time - event.time();
                if (gap > max_gap())
                {
                    m_max_gap_left = event.time();
                    m_max_gap_right = next_event_time;
                }
            }
        }
    }

    bare_time_type const& max_gap_left() const
    {
        return m_max_gap_left;
    }

    bare_time_type const& max_gap_right() const
    {
        return m_max_gap_right;
    }

    difference_type max_gap() const
    {
        return m_max_gap_right - m_max_gap_left;
    }

private:
    std::size_t m_overlap_count;
    bare_time_type m_max_gap_left, m_max_gap_right;
};

}} // namespace detail::max_interval_gap
#endif // DOXYGEN_NO_DETAIL

// Given a range of intervals I1, I2, ..., In, maximum_gap() returns
// the maximum length of an interval M that satisfies the following
// properties:
//
// 1. M.left >= min(I1, I2, ..., In)
// 2. M.right <= max(I1, I2, ..., In)
// 3. intersection(interior(M), Ik) is the empty set for all k=1, ..., n
// 4. length(M) is maximal
//
// where M.left and M.right denote the left and right extreme values
// for the interval M, and length(M) is equal to M.right - M.left.
//
// If M does not exist (or, alternatively, M is identified as the
// empty set), 0 is returned.
//
// The algorithm proceeds for performing a sweep: the left endpoints
// are inserted into a min-priority queue with the priority being the
// value of the endpoint. The sweep algorithm maintains an "overlap
// counter" that counts the number of overlaping intervals at any
// specific sweep-time value.
// There are two types of events encountered during the sweep:
// (a) a start event: the left endpoint of an interval is found.
//     In this case the overlap count is increased by one and the
//     right endpoint of the interval in inserted into the event queue
// (b) an end event: the right endpoint of an interval is found.
//     In this case the overlap count is decreased by one. If the
//     updated overlap count is 0, then we could expect to have a gap
//     in-between intervals. This gap is measured as the (absolute)
//     distance of the current interval right endpoint (being
//     processed) to the upcoming left endpoint of the next interval
//     to be processed (if such an interval exists). If the measured
//     gap is greater than the current maximum gap, it is recorded.
// The initial maximum gap is initialized to 0. This value is returned
// if no gap is found during the sweeping procedure.

template <typename RangeOfIntervals, typename T>
inline typename boost::range_value<RangeOfIntervals>::type::difference_type
maximum_gap(RangeOfIntervals const& range_of_intervals,
            T& max_gap_left, T& max_gap_right)
{
    typedef typename boost::range_value<RangeOfIntervals>::type interval_type;
    typedef detail::max_interval_gap::sweep_event<interval_type> event_type;

    // create a min-priority queue for the events
    std::priority_queue
        <
            event_type,
            std::vector<event_type>,
            detail::max_interval_gap::event_greater<event_type>
        > queue;

    // define initialization and event-process visitors
    detail::max_interval_gap::initialization_visitor init_visitor;
    detail::max_interval_gap::event_visitor<event_type> sweep_visitor;

    // perform the sweep
    geometry::sweep(range_of_intervals,
                    queue,
                    init_visitor,
                    sweep_visitor);

    max_gap_left = sweep_visitor.max_gap_left();
    max_gap_right = sweep_visitor.max_gap_right();
    return sweep_visitor.max_gap();
}

template <typename RangeOfIntervals>
inline typename boost::range_value<RangeOfIntervals>::type::difference_type
maximum_gap(RangeOfIntervals const& range_of_intervals)
{
    typedef typename boost::remove_const
        <
            typename boost::remove_reference
                <
                    typename boost::range_value
                        <
                            RangeOfIntervals
                        >::type::value_type
                >::type
        >::type value_type;

    value_type left, right;

    return maximum_gap(range_of_intervals, left, right);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_MAX_INTERVAL_GAP_HPP


// Date Last Altered: $Date: 2008-12-01 13:34:05 -0700 (Mon, 01 Dec 2008) $
// Revision Number: $Revision: 423 $
//--------------------------------------------*-C++-*------------------------------------//
/*! \file AngleTest.cc
 *  \author Greg Davidson
 *  \date May 29, 2008
 *
 *  \brief Includes all of the unit tests for the Angular Quadrature Module
 *
 *  This file runs all of the unit tests for the Field Module by including all
 *  of the test suites defined in the include files. */

#define BOOST_TEST_MODULE AngleTest
#include <boost/test/included/unit_test.hpp>

#include "Angle1D.hh"
#include "AngleQuad1D.hh"
#include "AngleVector1D.hh"

// Test the Angle1D class
#include "AngleTest.hh"

// Test the Angular Quadrature
#include "AngleQuadTest.hh"

// Test the AngleVector class
#include "AngleVectorTest.hh"


#pragma once
#include "VimCommon.h"
#pragma warning (disable:4756)

#include <iostream>
#include <queue>

#ifdef _OPENMP
#include <omp.h>
#endif // _OPENMP

#ifndef _OPENMP
int omp_get_num_procs(void) { return 1; }
int omp_get_thread_num(void) { return 0; }
#endif // _OPENMP

#define _f3_(x, y, z) (float)(x), (float)(y), (float)(z)
#define _i3_(x, y, z) (int)(x), (int)(y), (int)(z)
#define _fp_ float*

#define __v3minmax(__INOUT, __IN, __OP) {\
	__INOUT.x = __OP(__INOUT.x, __IN.x);\
	__INOUT.y = __OP(__INOUT.y, __IN.y);\
	__INOUT.z = __OP(__INOUT.z, __IN.z);\
};

#define EXB 2

using namespace vmobjects;
using namespace vmmath;
using namespace std;

template <typename T>
inline T __ReadVoxel(const vmint3& idx, const int width_slice, const int exb, T** vol_slices)
{
	return vol_slices[idx.z + exb][idx.x + exb + (idx.y + exb) * width_slice];
};

template <typename T>
inline void __WriteVoxel(T v, const vmint3& idx, const int width_slice, const int exb, T** vol_slices)
{
	vol_slices[idx.z + exb][idx.x + exb + (idx.y + exb) * width_slice] = v;
};

inline vmint3 __MultInt3(const vmint3* pi3_0, const vmint3* pi3_1)
{
	return vmint3(pi3_0->x * pi3_1->x, pi3_0->y * pi3_1->y, pi3_0->z * pi3_1->z);
}

inline bool __SafeCheck(const vmint3& idx, const vmint3& vol_size)
{
	vmint3 max_dir = idx - (vol_size - vmint3(1, 1, 1));
	vmint3 mult_dot = __MultInt3(&max_dir, &idx);
	return !(mult_dot.x > 0 || mult_dot.y > 0 || mult_dot.z > 0);
}

template <typename T>
inline T __Safe__ReadVoxel(const vmint3& idx, const vmint3& vol_size, const int width_slice, const int exb, T** vol_slices, const T bnd_v = 0)
{
	return __SafeCheck(idx, vol_size) ? __ReadVoxel<T>(idx, width_slice, exb, vol_slices) : (T)bnd_v;
};

inline float __TrilinearInterpolation(float v_0, float v_1, float v_2, float v_3, float v_4, float v_5, float v_6, float v_7,
	const vmfloat3& ratio)
{
	float v01 = v_0 * (1.f - ratio.x) + v_1 * ratio.x;
	float v23 = v_2 * (1.f - ratio.x) + v_3 * ratio.x;
	float v0123 = v01 * (1.f - ratio.y) + v23 * ratio.y;
	float v45 = v_4 * (1.f - ratio.x) + v_5 * ratio.x;
	float v67 = v_6 * (1.f - ratio.x) + v_7 * ratio.x;
	float v4567 = v45 * (1.f - ratio.y) + v67 * ratio.y;
	return v0123 * (1.f - ratio.z) + v4567 * ratio.z;
}

template <typename T>
inline float __Safe_TrilinearSample(const vmfloat3& pos_sample, const vmint3& vol_size, const int width_slice, const int exb, T** vol_slices, const T bnd_v = 0)
{
	vmfloat3 __pos_sample = pos_sample + vmfloat3(1000.f, 1000.f, 1000.f); // SAFE //
	vmint3 idx_sample = vmint3((int)__pos_sample.x, (int)__pos_sample.y, (int)__pos_sample.z);
	vmfloat3 ratio = vmfloat3((__pos_sample.x) - (float)(idx_sample.x), (__pos_sample.y) - (float)(idx_sample.y), (__pos_sample.z) - (float)(idx_sample.z));
	idx_sample -= vmint3(1000, 1000, 1000);

	float v0, v1, v2, v3, v4, v5, v6, v7;
	v0 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(0, 0, 0), vol_size, width_slice, exb, vol_slices, bnd_v);
	v1 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(1, 0, 0), vol_size, width_slice, exb, vol_slices, bnd_v);
	v2 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(0, 1, 0), vol_size, width_slice, exb, vol_slices, bnd_v);
	v3 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(1, 1, 0), vol_size, width_slice, exb, vol_slices, bnd_v);
	v4 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(0, 0, 1), vol_size, width_slice, exb, vol_slices, bnd_v);
	v5 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(1, 0, 1), vol_size, width_slice, exb, vol_slices, bnd_v);
	v6 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(0, 1, 1), vol_size, width_slice, exb, vol_slices, bnd_v);
	v7 = (float)__Safe__ReadVoxel<T>(idx_sample + vmint3(1, 1, 1), vol_size, width_slice, exb, vol_slices, bnd_v);
	return __TrilinearInterpolation(v0, v1, v2, v3, v4, v5, v6, v7, ratio);
};

template <typename T>
inline vmfloat3 __Safe_Gradient_by_Samples(const vmfloat3& pos_sample, const vmint3& vol_size, const vmfloat3& dir_x, const vmfloat3& dir_y, const vmfloat3& dir_z, const int width_slice, const int exb, T** vol_slices)
{
	float v_XL = __Safe_TrilinearSample<T>(pos_sample - dir_x, vol_size, width_slice, exb, vol_slices);
	float v_XR = __Safe_TrilinearSample<T>(pos_sample + dir_x, vol_size, width_slice, exb, vol_slices);
	float v_YL = __Safe_TrilinearSample<T>(pos_sample - dir_y, vol_size, width_slice, exb, vol_slices);
	float v_YR = __Safe_TrilinearSample<T>(pos_sample + dir_y, vol_size, width_slice, exb, vol_slices);
	float v_ZL = __Safe_TrilinearSample<T>(pos_sample - dir_z, vol_size, width_slice, exb, vol_slices);
	float v_ZR = __Safe_TrilinearSample<T>(pos_sample + dir_z, vol_size, width_slice, exb, vol_slices);
	float g_x = v_XR - v_XL;
	float g_y = v_YR - v_YL;
	float g_z = v_ZR - v_ZL;
	return vmfloat3(g_x, g_y, g_z) * 0.5f;
}

#define SGRAD(p, vinfo) __Safe_Gradient_by_Samples(p, vinfo.vol_size, \
	vinfo.vec_grad_dirs[0], vinfo.vec_grad_dirs[1], vinfo.vec_grad_dirs[2], \
		vinfo.width_slice, EXB, vinfo.vol_slices)

template <typename T>
void ___debugout(std::string str, T v)
{
	std::cout << str.c_str() << std::to_string(v).c_str() << std::endl;
}

template <typename T>
struct __VolSampleInfo
{
	//ushort** vol_slices_filtered;
	T** vol_slices;
	vmint3 vol_size;
	int width_slice;
	vmmat44f mat_ws2vs;
	vmmat44f mat_vs2ws;
	vmfloat3 vec_grad_dirs[3];
	float min_sample_dist;

	__VolSampleInfo() : vol_slices(NULL), //vol_slices_origin(NULL),
		vol_size(vmint3()), width_slice(0), mat_ws2vs(NULL), mat_vs2ws(NULL) {};

	__VolSampleInfo(T** _vol_slices, vmint3 _vol_size,
		int _width_slice, vmmat44f _mat_ws2vs, vmmat44f _mat_vs2ws,
		vmfloat3 _vec_grad_dirs[3], float _min_sample_dist) : vol_slices(_vol_slices),
		vol_size(_vol_size), width_slice(_width_slice), mat_ws2vs(_mat_ws2vs), mat_vs2ws(_mat_vs2ws), min_sample_dist(_min_sample_dist)
	{
		//vol_slices_filtered = _vol_slices;
		vec_grad_dirs[0] = _vec_grad_dirs[0];
		vec_grad_dirs[1] = _vec_grad_dirs[1];
		vec_grad_dirs[2] = _vec_grad_dirs[2];
	}
};

template <typename T>
__VolSampleInfo<T> Get_volsample_info(VmVObjectVolume* pCVolume, const double sample_dist_scale)
{
	if (pCVolume == NULL) return __VolSampleInfo<T>();

	vmfloat3 vecVoxelGradDirs[3];// = { vmfloat3(1, 0, 0), vmfloat3(0, 1, 0), vmfloat3(0, 0, 1) };
	VolumeData* volArchive = pCVolume->GetVolumeData();
	float min_dist_sample = (float)min(min(volArchive->vox_pitch.x, volArchive->vox_pitch.y), volArchive->vox_pitch.z);
	fTransformVector(&vecVoxelGradDirs[0], &vmfloat3(min_dist_sample, 0, 0), &pCVolume->GetMatrixWS2OSf());
	fTransformVector(&vecVoxelGradDirs[1], &vmfloat3(0, min_dist_sample, 0), &pCVolume->GetMatrixWS2OSf());
	fTransformVector(&vecVoxelGradDirs[2], &vmfloat3(0, 0, min_dist_sample), &pCVolume->GetMatrixWS2OSf());

	vmfloat3 vecVoxelGradDirs_unit[3] = {
		vecVoxelGradDirs[0] * (float)sample_dist_scale,
		vecVoxelGradDirs[1] * (float)sample_dist_scale,
		vecVoxelGradDirs[2] * (float)sample_dist_scale };

	VolumeData *volArchiveSample = pCVolume->GetVolumeData();
	vmint3 volSize = volArchiveSample->vol_size;
	vmint3 volSizeEx = volArchiveSample->bnd_size;
	int widthSamplePitch = volSize.x + volSizeEx.x * 2;

	return __VolSampleInfo<T>((T**)volArchiveSample->vol_slices,
		volSize, widthSamplePitch, pCVolume->GetMatrixWS2OSf(), pCVolume->GetMatrixOS2WSf(), vecVoxelGradDirs_unit, min_dist_sample);
};

#include "../nanoflann.hpp"
template <typename T, typename TT>
struct PointCloud
{
	//public:
	const TT* pts;
	const size_t num_pts;
	PointCloud(const TT* _pts, const size_t _num_pts) : pts(_pts), num_pts(_num_pts) { }
	PointCloud(const vector<TT>& vtr_pts) : pts(&vtr_pts[0]), num_pts(vtr_pts.size()) { }

	// Must return the number of data points
	inline size_t kdtree_get_point_count() const { return num_pts; }

	// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
	inline T kdtree_distance(const T *p1, const size_t idx_p2, size_t) const
	{
		const T d0 = p1[0] - pts[idx_p2].x;
		const T d1 = p1[1] - pts[idx_p2].y;
		const T d2 = p1[2] - pts[idx_p2].z;
		return d0 * d0 + d1 * d1 + d2 * d2;
	}

	// Returns the dim'th component of the idx'th point in the class:
	// Since this is inlined and the "dim" argument is typically an immediate value, the
	//  "if/else's" are actually solved at compile time.
	inline T kdtree_get_pt(const size_t idx, int dim) const
	{
		if (dim == 0) return pts[idx].x;
		else if (dim == 1) return pts[idx].y;
		else return pts[idx].z;
	}

	// Optional bounding-box computation: return false to default to a standard bbox computation loop.
	//   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
	//   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
	template <class BBOX>
	bool kdtree_get_bbox(BBOX&) const { return false; }
};

typedef nanoflann::KDTreeSingleIndexAdaptor<
	nanoflann::L2_Simple_Adaptor<float, PointCloud<float, vmfloat3> >,
	PointCloud<float, vmfloat3>,
	3 // dim
> kd_tree_t;

void make_band(__VolSampleInfo<char>& band_info, const int v_low, const int v_high, const __VolSampleInfo<ushort>& vol_info_outside, const __VolSampleInfo<ushort>& vol_info_inside)
{
	const int clip_bnd = 2;
#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int z = 0; z < band_info.vol_size.z; z++)
		for (int y = 0; y < band_info.vol_size.y; y++)
			for (int x = 0; x < band_info.vol_size.x; x++)
			{
				if (x < clip_bnd || y < clip_bnd || z < clip_bnd
					|| x >= band_info.vol_size.x - clip_bnd
					|| y >= band_info.vol_size.y - clip_bnd
					|| z >= band_info.vol_size.z - clip_bnd)
					continue;
				int v_out = __ReadVoxel(vmint3(x, y, z), vol_info_outside.width_slice, EXB, vol_info_outside.vol_slices);
				int v_in = __ReadVoxel(vmint3(x, y, z), vol_info_inside.width_slice, EXB, vol_info_inside.vol_slices);
				if (v_in > v_high) __WriteVoxel((char)-1, vmint3(x, y, z), band_info.width_slice, EXB, band_info.vol_slices);
				else if (v_out < v_low) __WriteVoxel((char)1, vmint3(x, y, z), band_info.width_slice, EXB, band_info.vol_slices);
			}
}

template <typename T>
void get_isosurface_vtx(set<tuple<float, float, float>>& voxel_pts_set,
	const float isovalue, const uint sampleoffset, const __VolSampleInfo<T>& vol_info)
{
	auto __read_v = [&](vmint3& idx)
	{
		idx.x = min(idx.x, vol_info.vol_size.x - 1);
		idx.y = min(idx.y, vol_info.vol_size.y - 1);
		idx.z = min(idx.z, vol_info.vol_size.z - 1);
		return (float)__ReadVoxel(idx, vol_info.width_slice, EXB, vol_info.vol_slices);
	};

	const float offset = (float)sampleoffset;
	for (int z = 0; z <= vol_info.vol_size.z; z += sampleoffset)
	{
		for (int y = 0; y <= vol_info.vol_size.y; y += sampleoffset)
		{
			for (int x = 0; x <= vol_info.vol_size.x; x += sampleoffset)
			{
				// sample at 8 nodes of a cell
				float samplevalues[4] = {
					__read_v(vmint3(x + 0, y + 0, z + 0)),
					__read_v(vmint3(x + sampleoffset, y + 0, z + 0)),
					__read_v(vmint3(x + 0, y + sampleoffset, z + 0)),
					__read_v(vmint3(x + 0, y + 0, z + sampleoffset)),
				};

				bool isOriSmall = samplevalues[0] < isovalue;
				bool isEdgeXSmall = samplevalues[1] < isovalue;
				bool isEdgeYSmall = samplevalues[2] < isovalue;
				bool isEdgeZSmall = samplevalues[3] < isovalue;

				if (isOriSmall != isEdgeXSmall)
				{
					float ratio = (float)(isovalue - samplevalues[0]) / (float)(samplevalues[1] - samplevalues[0]);
					vmfloat3 pos = vmfloat3((float)x + ratio * offset, (float)y, (float)z);
					voxel_pts_set.insert(tuple<float, float, float>(pos.x, pos.y, pos.z));
				}
				if (isOriSmall != isEdgeYSmall)
				{
					float ratio = (float)(isovalue - samplevalues[0]) / (float)(samplevalues[2] - samplevalues[0]);
					vmfloat3 pos = vmfloat3((float)x, (float)y + ratio * offset, (float)z);
					voxel_pts_set.insert(tuple<float, float, float>(pos.x, pos.y, pos.z));
				}
				if (isOriSmall != isEdgeZSmall)
				{
					float ratio = (float)(isovalue - samplevalues[0]) / (float)(samplevalues[3] - samplevalues[0]);
					vmfloat3 pos = vmfloat3((float)x, (float)y, (float)z + ratio * offset);
					voxel_pts_set.insert(tuple<float, float, float>(pos.x, pos.y, pos.z));
				}
			} // for x
		} // for y
	} //for z
}

void simplify_points_ugrid(vector<vmfloat3>& pos_simplified_pts, vmfloat3& aabb_diff, const vector<vmfloat3>& pos_pts, const float grid_length)
{
	uint num_pts = (uint)pos_pts.size();
	vmfloat3 aabb_min(FLT_MAX), aabb_max(-FLT_MAX);
	for (uint i = 0; i < num_pts; i++)
	{
		const vmfloat3& pos_pt = pos_pts[i];
		__v3minmax(aabb_min, pos_pt, min);
		__v3minmax(aabb_max, pos_pt, max);
	}
	aabb_diff = aabb_max - aabb_min;
	vmint3 aabb_size = vmint3(aabb_diff / grid_length) + vmint3(1);
	uint* index_map = new uint[aabb_size.x * aabb_size.y * aabb_size.z];
	memset(index_map, 0, sizeof(uint) * aabb_size.x * aabb_size.y * aabb_size.z);

	uint count = 0;
	for (uint i = 0; i < num_pts; i++)
	{
		const vmfloat3& pos_pt = pos_pts[i];
		vmfloat3 pos_cell = (pos_pt - aabb_min) / grid_length;
		vmint3 idx_cell = pos_cell;
		uint addr = (uint)idx_cell.x + (uint)(idx_cell.y * aabb_size.x) + (uint)idx_cell.z * (uint)(aabb_size.x * aabb_size.y);
		uint prev_idx = index_map[addr];
		if (prev_idx == 0)
		{
			index_map[addr] = i + 1;
			count++;
		}
		else
		{
			const vmfloat3& pos_prev_pt = pos_pts[prev_idx];
			vmfloat3 pos_prev_cell = (pos_prev_pt - aabb_min) / grid_length;
			vmint3 idx_prev_cell = pos_prev_cell;

			vmfloat3 cell_pos = pos_cell - (vmfloat3)idx_cell - vmfloat3(0.5f);
			vmfloat3 prev_cell_pos = pos_prev_cell - (vmfloat3)idx_prev_cell - vmfloat3(0.5f);
			if (fLengthVectorSq(&cell_pos) < fLengthVectorSq(&prev_cell_pos))
				index_map[addr] = i + 1;
		}
	}

	pos_simplified_pts.assign(count, vmfloat3());
	uint count_idx = 0;
	for (uint i = 0; i < (uint)aabb_size.x * (uint)aabb_size.y * (uint)aabb_size.z; i++)
	{
		uint idx = index_map[i];
		if (idx > 0)
			pos_simplified_pts[count_idx++] = pos_pts[idx - 1];
	}

	VMSAFE_DELETEARRAY(index_map);
}

int OtsuThresholdValue(ullong* pullHistogramValues, int histo_size, int begin_idx, int end_idx)
{
	double total_elements = 0;
	double sum1 = 0;

	//for (int i = begin_idx; i <= end_idx; i++)
	for (int i = 0; i < histo_size; i++)
	{
		total_elements += (double)pullHistogramValues[i];
		sum1 += (double)i * (double)pullHistogramValues[i];
	}

	double sumB = 0;
	double wB = 0;
	double _maximum = 0;
	begin_idx = min(begin_idx, histo_size - 1);
	end_idx = min(end_idx, histo_size - 1);

	int otsuThreshold = 0;
	for (int i = begin_idx; i <= end_idx; i++)
	{
		wB += (double)pullHistogramValues[i];
		double wF = total_elements - wB;
		if (wB == 0 || wF == 0)
			continue;

		sumB += (double)i * (double)pullHistogramValues[i];
		double mF = (sum1 - sumB) / wF;
		double btn = wB * wF * ((sumB / wB) - mF) * ((sumB / wB) - mF);
		if (btn >= _maximum)
		{
			otsuThreshold = i;
			_maximum = btn;
		}
	}

	return otsuThreshold;
}

void raytraversal_gradmax(std::vector<vmfloat3>& lmax_pts,
	const std::vector<vmfloat3>& pos_pts, const std::vector<vmfloat3>& dir_pts,
	const int num_maxrc_steps,
	const float dir_scale, /*const bool use_quadinterpolation, const bool use_consistent_dir,*/
	const float min_v, const float max_v,
	const __VolSampleInfo<char>& band_info,
	const __VolSampleInfo<ushort>& vol_info)
{
	auto __gradient = [&](const vmfloat3& pos_vs)
	{
		return SGRAD(pos_vs, vol_info);
		//return -__Safe_TrilinearSample_T(pos_vs, mask_Info.vol_size, mask_Info.width_slice, 0, mask_Info.grad_slices, vmfloat3());
	};

	int num_pts = (int)pos_pts.size();
	lmax_pts.assign(num_pts, vmfloat3(0, 0, 0));
	//std::vector<__float3> tmp_lmax_pts(num_pts, __float3(FLT_MAX, FLT_MAX, FLT_MAX));
	vmfloat3 vol_size_f(_f3_(vol_info.vol_size.x, vol_info.vol_size.y, vol_info.vol_size.z));
	const float safe_bnd = 2.f;

#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int i = 0; i < num_pts; i++)
	{
		const vmfloat3& dir = dir_pts[i];
		if (fLengthVectorSq(&dir) < FLT_EPSILON)
		{
			lmax_pts[i] = pos_pts[i];
			continue;
		}

		float max_gmsq = -1;
		const vmfloat3& pos_start = pos_pts[i];
		vmfloat3 pos_lmax = pos_start;

		for (int j = 0; j < num_maxrc_steps; j++)
		{
			vmfloat3 pos_cur = pos_start + dir * dir_scale * (float)j, pos_cur_vs;
			fTransformPoint(&pos_cur_vs, &pos_cur, &vol_info.mat_ws2vs);

			if (pos_cur_vs.x < safe_bnd || pos_cur_vs.y < safe_bnd || pos_cur_vs.z < safe_bnd
				|| pos_cur_vs.x >= vol_size_f.x - safe_bnd
				|| pos_cur_vs.y >= vol_size_f.y - safe_bnd
				|| pos_cur_vs.z >= vol_size_f.z - safe_bnd)
				continue;

			if (j > 1 && band_info.vol_slices) // j > 1 is for safe zone
			{
				float mask_value = __Safe_TrilinearSample<char>(pos_cur_vs, band_info.vol_size, band_info.width_slice, EXB, band_info.vol_slices, -1);
				if (mask_value == 1.f || mask_value == -1.f)
					break;
			}

			vmfloat3 grad = SGRAD(pos_cur_vs, vol_info);
			float sample_v = __Safe_TrilinearSample<ushort>(pos_cur_vs, vol_info.vol_size, vol_info.width_slice, EXB, vol_info.vol_slices, -1);
			if (fDotVector(&dir, &grad) >= 0 || sample_v  < min_v || sample_v > max_v)
				continue;

			float gmsq = fLengthVectorSq(&grad);
			if (max_gmsq < gmsq)
			{
				max_gmsq = gmsq;
				pos_lmax = pos_cur;
			}
		}

//#define QUADRATIC
#ifdef QUADRATIC
		auto quadratic_maxsampler = [](const vmfloat3& pos_a, const vmfloat3& pos_c, const float gm_a, const float gm_b, const float gm_c)
		{
			vmfloat3 pos_max;
			float _div_ = (gm_a + gm_c) - 2.f * gm_b;
			if (_div_ == 0)
			{
				pos_max = pos_a;
			}
			else
			{
				float t = (3.f * gm_a + gm_c - 4.f * gm_b) / (4.f * _div_);
				pos_max = pos_a + min(max(t, 0.f), 1.f) * (pos_c - pos_a);
			}
			return pos_max;
		};
		if (max_gmsq > 0)
		{
			vmfloat3 pos_a = pos_lmax - dir * dir_scale, pos_a_vs;
			vmfloat3 pos_c = pos_lmax + dir * dir_scale, pos_c_vs;
			fTransformPoint(&pos_a_vs, &pos_a, &vol_info.mat_ws2vs);
			fTransformPoint(&pos_c_vs, &pos_c, &vol_info.mat_ws2vs);
			float edge_a = fLengthVector(&SGRAD(pos_a_vs, vol_info));
			float edge_c = fLengthVector(&SGRAD(pos_c_vs, vol_info));
			lmax_pts[i] = quadratic_maxsampler(pos_a, pos_c, edge_a, sqrt(max_gmsq), edge_c);
		}
		else
			lmax_pts[i] = pos_lmax;
#else
		lmax_pts[i] = pos_lmax;
#endif
	}
}

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/Cholesky>
#include <Eigen/Householder>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#define _DIM_ 3
auto __construct_covariance_matrix = [](const Eigen::VectorXd& cov) -> Eigen::MatrixXd
{
	Eigen::MatrixXd m(_DIM_, _DIM_);

	for (std::size_t i = 0; i < _DIM_; ++i)
	{
		for (std::size_t j = i; j < _DIM_; ++j)
		{
			m(i, j) = static_cast<float>(cov[(_DIM_ * i) + j - ((i * (i + 1)) / 2)]);

			if (i != j)
				m(j, i) = m(i, j);
		}
	}

	return m;
};
auto __diagonalize_selfadjoint_matrix = [](Eigen::MatrixXd& m, Eigen::MatrixXd& eigenvectors, Eigen::VectorXd& eigenvalues) -> bool
{
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver;

	//eigensolver.computeDirect(m);
	eigensolver.compute(m);

	if (eigensolver.info() != Eigen::Success)
		return false;

	eigenvalues = eigensolver.eigenvalues();
	eigenvectors = eigensolver.eigenvectors();

	return true;
};
auto __diagonalize_selfadjoint_covariance_matrix = [&]
(const Eigen::VectorXd& cov, float* eigenvalues, vmfloat3* eigenvectors)
{
	Eigen::MatrixXd m = __construct_covariance_matrix(cov);

	// Diagonalizing the matrix
	Eigen::VectorXd eigenvalues_;
	Eigen::MatrixXd eigenvectors_;
	bool res = __diagonalize_selfadjoint_matrix(m, eigenvectors_, eigenvalues_);

	if (res)
	{
		for (std::size_t i = 0; i < _DIM_; ++i)
		{
			eigenvalues[i] = static_cast<float>(eigenvalues_[i]);

			for (std::size_t j = 0; j < _DIM_; ++j)
				((float*)eigenvectors)[_DIM_*i + j] = static_cast<float>(eigenvectors_(j, i));
		}
	}

	return res;
};

float estimate_shape_variation(const vmfloat3& pos_src, const vmfloat3& nrl_src, const vmfloat3* pos_pts, const vmfloat3* nrl_pts, const kd_tree_t& kdt_index, const float kernel_radius)
{
	float sv = 1.f;
	const float r_sq = kernel_radius * kernel_radius;

	static nanoflann::SearchParams params;
	params.sorted = false;

	std::vector<std::pair<size_t, float>> ret_matches;
	const int nMatches = (int)kdt_index.radiusSearch((float*)&pos_src, r_sq, ret_matches, params);

	std::vector<int> valid_idx_pts;
	for (int k = 0; k < nMatches; k++)
	{
		int idx_neighbor = (int)ret_matches[k].first;
		vmfloat3 nrl_nb = nrl_pts[idx_neighbor];
		if (nrl_src.x * nrl_nb.x + nrl_src.y * nrl_nb.y + nrl_src.z * nrl_nb.z >= 0)
			valid_idx_pts.push_back(idx_neighbor);
	}

	const int num_valid_nb_pts = (int)valid_idx_pts.size();
	if (num_valid_nb_pts < 3)
		return 1; // treated as an outlier

	vmdouble3 pos_centroid = vmdouble3();
	double ft_num_ptns = (double)nMatches;
	for (int k = 0; k < nMatches; k++)
	{
		int idx_nb = (int)ret_matches[k].first;
		vmdouble3 pos_nb = pos_pts[idx_nb];
		pos_centroid.x += ((double)pos_nb.x / ft_num_ptns);
		pos_centroid.y += ((double)pos_nb.y / ft_num_ptns);
		pos_centroid.z += ((double)pos_nb.z / ft_num_ptns);
	}

	vmfloat3 pos_centroid_f = vmfloat3((float)pos_centroid.x, (float)pos_centroid.y, (float)pos_centroid.z);
	//Eigen::VectorXd evecs(6); evecs << 0, 0, 0, 0, 0, 0;
	Eigen::VectorXd evecs = Eigen::VectorXd::Zero(6);
	for (int k = 0; k < nMatches; k++)
	{
		int idx_nb = (int)ret_matches[k].first;
		vmfloat3 pos_nb = pos_pts[idx_nb];
		vmfloat3 diff = vmfloat3(pos_centroid_f.x - pos_nb.x, pos_centroid_f.y - pos_nb.y, pos_centroid_f.z - pos_nb.z);
		evecs(0) += diff.x * diff.x;
		evecs(1) += diff.x * diff.y;
		evecs(2) += diff.x * diff.z;
		evecs(3) += diff.y * diff.y;
		evecs(4) += diff.y * diff.z;
		evecs(5) += diff.z * diff.z;
	}

	float eigenvalues[3];
	vmfloat3 eigenvectors[3];
	if (__diagonalize_selfadjoint_covariance_matrix(evecs, eigenvalues, eigenvectors))
	{
		// eigenvalues[0] is smallest
		float sum_egv = eigenvalues[0] + eigenvalues[1] + eigenvalues[2];
		sv = eigenvalues[0] / sum_egv * 3.f;
	}

	return sv;
}

float estimate_orient_variation(const vmfloat3& pos_src, const __VolSampleInfo<ushort>& vol_info, const float kernel_radius)
{
	auto __sample_v = [&](const vmfloat3& pos_vs)
	{
		return __Safe_TrilinearSample(pos_vs, vol_info.vol_size, vol_info.width_slice, EXB, vol_info.vol_slices);
	};

	float ov = 1.f;
	float voxel_kernel = kernel_radius / vol_info.min_sample_dist;
	vmfloat3 dirs[3] = { vol_info.vec_grad_dirs[0] * 0.5f * voxel_kernel, vol_info.vec_grad_dirs[1] * 0.5f * voxel_kernel, vol_info.vec_grad_dirs[2] * 0.5f * voxel_kernel };

	vmfloat3 pos_sample_vs;
	fTransformPoint(&pos_sample_vs, &pos_src, &vol_info.mat_ws2vs);

	float v = __sample_v(pos_sample_vs);
	float v_XXR = __sample_v(pos_sample_vs + 2.f * dirs[0]);
	float v_XXL = __sample_v(pos_sample_vs - 2.f * dirs[0]);
	float v_YYR = __sample_v(pos_sample_vs + 2.f * dirs[1]);
	float v_YYL = __sample_v(pos_sample_vs - 2.f * dirs[1]);
	float v_ZZR = __sample_v(pos_sample_vs + 2.f * dirs[2]);
	float v_ZZL = __sample_v(pos_sample_vs - 2.f * dirs[2]);
	float v_XR = __sample_v(pos_sample_vs + dirs[0]);
	float v_XL = __sample_v(pos_sample_vs - dirs[0]);
	float v_YR = __sample_v(pos_sample_vs + dirs[1]);
	float v_YL = __sample_v(pos_sample_vs - dirs[1]);
	float v_ZR = __sample_v(pos_sample_vs + dirs[2]);
	float v_ZL = __sample_v(pos_sample_vs - dirs[2]);
	float v_XRYR = __sample_v(pos_sample_vs + dirs[0] + dirs[1]);
	float v_XRYL = __sample_v(pos_sample_vs + dirs[0] - dirs[1]);
	float v_XLYR = __sample_v(pos_sample_vs - dirs[0] + dirs[1]);
	float v_XLYL = __sample_v(pos_sample_vs - dirs[0] - dirs[1]);
	float v_YRZR = __sample_v(pos_sample_vs + dirs[1] + dirs[2]);
	float v_YRZL = __sample_v(pos_sample_vs + dirs[1] - dirs[2]);
	float v_YLZR = __sample_v(pos_sample_vs - dirs[1] + dirs[2]);
	float v_YLZL = __sample_v(pos_sample_vs - dirs[1] - dirs[2]);
	float v_XRZR = __sample_v(pos_sample_vs + dirs[0] + dirs[2]);
	float v_XRZL = __sample_v(pos_sample_vs + dirs[0] - dirs[2]);
	float v_XLZR = __sample_v(pos_sample_vs - dirs[0] + dirs[2]);
	float v_XLZL = __sample_v(pos_sample_vs - dirs[0] - dirs[2]);

	vmmat44f H;

	vmfloat3 g = vmfloat3(v_XR - v_XL, v_YR - v_YL, v_ZR - v_ZL);
	H[0][0] = v_XXR - 2.f * v + v_XXL; // f_xx
	H[0][1] = (v_XRYR - v_XLYR - v_XRYL + v_XLYL);	//f_xy
	H[0][2] = (v_XRZR - v_XLZR - v_XRZL + v_XLZL);	//f_xz
	H[1][0] = H[0][1];
	H[1][1] = v_YYR - 2.f * v + v_YYL;								//f_yy
	H[1][2] = (v_YRZR - v_YLZR - v_YRZL + v_YLZL);	//f_yz
	H[2][0] = H[0][2];
	H[2][1] = H[1][2];
	H[2][2] = v_ZZR - 2.f * v + v_ZZL;								//f_zz
	H[3][3] = 1;
	//
	float gm = fLengthVector(&g);
	if (gm > FLT_EPSILON)
	{
		vmfloat3 n = -g / gm;
		vmmat44f nnT, P;
		nnT[0][0] = n.x*n.x;
		nnT[0][1] = n.x*n.y;
		nnT[0][2] = n.x*n.z;
		nnT[1][0] = n.y*n.x;
		nnT[1][1] = n.y*n.y;
		nnT[1][2] = n.y*n.z;
		nnT[2][0] = n.z*n.x;
		nnT[2][1] = n.z*n.y;
		nnT[2][2] = n.z*n.z;
		nnT[3][3] = 0;
		P[0][0] = 1.0f;
		P[1][1] = 1.0f;
		P[2][2] = 1.0f;
		P[3][3] = 1.0f;
		P = P - nnT;
		/////////////////////////
		vmmat44f FF = (-P * H) / (float)gm * nnT;
		float fFlowCurv = sqrt(FF[0][0] * FF[0][0] + FF[0][1] * FF[0][1] + FF[0][2] * FF[0][2]
			+ FF[1][0] * FF[1][0] + FF[1][1] * FF[1][1] + FF[1][2] * FF[1][2]
			+ FF[2][0] * FF[2][0] + FF[2][1] * FF[2][1] + FF[2][2] * FF[2][2]);
		ov = min(fFlowCurv, 1.0f);
	}
	return ov;
}

void compute_geometry_info(vector<vmfloat3>& nrl_pts, vector<vmfloat2>& gm_gc_pts, vector<vmfloat3>& pos_pts, const float kernel_radius, const __VolSampleInfo<ushort>& vol_info)
{
	int num_pts = (int)pos_pts.size();
	nrl_pts.assign(num_pts, vmfloat3());
	gm_gc_pts.assign(num_pts, vmfloat2());

#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int i = 0; i < num_pts; i++)
	{
		vmfloat3 pos_src = pos_pts[i], pos_vs;
		fTransformPoint(&pos_vs, &pos_src, &vol_info.mat_ws2vs);
		nrl_pts[i] = -SGRAD(pos_vs, vol_info);
		float leng = fLengthVector(&nrl_pts[i]);
		nrl_pts[i] = leng > FLT_EPSILON ? nrl_pts[i] / leng : vmfloat3();
		gm_gc_pts[i].x = leng;
	}

	PointCloud<float, vmfloat3> pc_kdt(pos_pts);
	kd_tree_t kdt(3, pc_kdt, nanoflann::KDTreeSingleIndexAdaptorParams(10));
	kdt.buildIndex();

#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int i = 0; i < num_pts; i++)
	{
		vmfloat3 pos_src = pos_pts[i];
		float sv = min(estimate_shape_variation(pos_src, nrl_pts[i], &pos_pts[0], &nrl_pts[0], kdt, kernel_radius), 1.f);
		float ov = min(estimate_orient_variation(pos_src, vol_info, kernel_radius), 1.f);
		gm_gc_pts[i].y = max(sv, ov);
	}
}

void clustering(std::map<int, std::vector<int>>& map_clusters, const std::vector<vmfloat3>& pos_pts, const kd_tree_t& kdt, const std::vector<vmfloat3>& nrl_pts, const float e_c)
{
	int num_pts = (int)pos_pts.size();
	if (num_pts == 0) return;

	nanoflann::SearchParams params;
	params.sorted = false;
	const float r_sq = e_c * e_c;

	auto is_side_angle = [](const vmfloat3& p, const vmfloat3& q, const float eps)
	{
		float angle = std::acos(max(min(fDotVector(&p, &q), 1.f), -1.f)); // 0 to PI
		return (angle > VM_PI / 2.f - eps) && (angle < VM_PI / 2.f + eps);
	};
	auto subs_norm = [](const vmfloat3& p, const vmfloat3& q)
	{
		vmfloat3 v = vmfloat3(p.x - q.x, p.y - q.y, p.z - q.z);
		fNormalizeVector(&v, &v);
		return v;
	};

	std::vector<int> cluster_map_pts;
	cluster_map_pts.assign(num_pts, 0);

	int count_id_cluster = 1;
	for (int i = 0; i < num_pts; i++)
	{
		if (cluster_map_pts[i] != 0) continue;

		std::vector<int> index_cluster_pts;
		std::queue<int> que_for_process;

		que_for_process.push(i);
		index_cluster_pts.push_back(i);
		cluster_map_pts[i] = count_id_cluster;

		int error_prop_count = 0;
		while (!que_for_process.empty())
		{
			if (error_prop_count++ > 50000000)
			{
				cout << "ERROR, num conf. clusters : " << map_clusters.size() << endl;
				return;
			}

			int source_idx = que_for_process.front();
			que_for_process.pop();

			const vmfloat3& pos_src = pos_pts[source_idx];
			const vmfloat3& nrl_src = nrl_pts[source_idx];

			std::vector<std::pair<size_t, float> >   ret_matches;
			const int nMatches = (int)kdt.radiusSearch((float*)&pos_src, r_sq, ret_matches, params);

			for (int j = 0; j < nMatches; j++)
			{
				int idx_nb = (int)ret_matches[j].first;
				vmfloat3 pos_nb = pos_pts[idx_nb];
				if (cluster_map_pts[idx_nb] == 0
					&& is_side_angle(nrl_src, subs_norm(pos_nb, pos_src), VM_fPI / 6.f)
					)
				{
					que_for_process.push(idx_nb);
					index_cluster_pts.push_back(idx_nb);
					cluster_map_pts[idx_nb] = count_id_cluster;
				}
			}
		}

		if (index_cluster_pts.size() > 0)
			map_clusters[count_id_cluster++] = index_cluster_pts;
	}
}

void estimating_hole(bool& is_boundary, bool& is_src_vector_set, vmfloat3& vector_o,
	double(&btw_angles_max)[4], double(&btw_angles_min)[4],
	const std::vector<std::pair<size_t, float> > ret_matches,
	const std::vector<vmfloat3>& pos_pts, const vmfloat3& pos_src, const vmfloat3& nrl_src,
	const float thres_susceptibility_sq)
{
	const int nMatches = (int)ret_matches.size();
	for (int j = 0; j < nMatches; j++)
	{
		int idx_nb = (int)ret_matches[j].first;

		const vmfloat3& pos_nb = pos_pts[idx_nb];

		// plane is defined by pos_src and nrl_src
		vmfloat3 _v = pos_nb - pos_src;
		vmfloat3 _tv = nrl_src * fDotVector(&_v, &nrl_src);
		vmfloat3 pos_nb_on_plane = pos_nb - _tv;
		vmfloat3 vec_on_plane = pos_nb_on_plane - pos_src;
		double length_sq = fLengthVectorSq(&vec_on_plane);

		if (length_sq > thres_susceptibility_sq)
		{
			vec_on_plane /= (float)std::sqrt(length_sq);

			if (!is_src_vector_set)
			{
				is_src_vector_set = true;
				vector_o = vec_on_plane;
			}
			else
			{
				double btw_angle = std::acos(max(min(fDotVector(&vector_o, &vec_on_plane), 1.f), -1.f)); // 0 to PI
				vmfloat3 cross_v;
				fCrossDotVector(&cross_v, &vec_on_plane, &vector_o);
				if (fDotVector(&nrl_src, &cross_v) < 0)
					btw_angle = 2 * VM_fPI - btw_angle;
				//ordered_angles.insert((float)btw_angle);

				if (btw_angle < VM_fPI * 0.5)
				{
					btw_angles_max[0] = max(btw_angle, btw_angles_max[0]);
					//btw_angles_min[0] = min(btw_angle, btw_angles_min[0]);//
				}
				else if (btw_angle < VM_fPI)
				{
					btw_angles_max[1] = max(btw_angle, btw_angles_max[1]);
					btw_angles_min[1] = min(btw_angle, btw_angles_min[1]);
				}
				else if (btw_angle < VM_fPI * 1.5)
				{
					btw_angles_max[2] = max(btw_angle, btw_angles_max[2]);
					btw_angles_min[2] = min(btw_angle, btw_angles_min[2]);
				}
				else
				{
					//btw_angles_max[3] = max(btw_angle, btw_angles_max[3]); //
					btw_angles_min[3] = min(btw_angle, btw_angles_min[3]);
				}

				if (btw_angles_min[1] - btw_angles_max[0] < VM_fPI / 2.
					&& btw_angles_min[2] - btw_angles_max[1] < VM_fPI / 2.
					&& btw_angles_min[3] - btw_angles_max[2] < VM_fPI / 2.)
				{
					is_boundary = false;
					break; // for (int j = 0; j < nMatches; j++)
				}
			}
		}
	}
}

void detecting_boundary(std::vector<int>& bnd_id_pts, const std::vector<vmfloat3>& pos_pts, const std::vector<vmfloat3>& nrl_pts,
	const kd_tree_t& kdt_index, const float e_b, const bool consistent_dir_neighbors/*projecting except opposite directional points of the target poin*/)
{
	float thres_susceptibility_sq = e_b * 0.01f;
	thres_susceptibility_sq *= thres_susceptibility_sq;
	int num_pts = (int)pos_pts.size();

	nanoflann::SearchParams params;
	params.sorted = false;
	const float r_sq = e_b * e_b;
	// params.eps

	bool* hole_flags = new bool[num_pts];
	ZeroMemory(hole_flags, sizeof(bool) * num_pts);

#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int i = 0; i < num_pts; i++)
	{
		const vmfloat3& pos_src = pos_pts[i];
		const vmfloat3& nrl_src = nrl_pts[i];

		std::vector<std::pair<size_t, float> >   ret_matches;
		const int nMatches = (int)kdt_index.radiusSearch((float*)&pos_src, r_sq, ret_matches, params);

		//std::set<float> ordered_angles;
		bool is_src_vector_set = false;
		vmfloat3 vector_o;

		double btw_angles_max[4] = { -100000., -100000., -100000., -100000. };
		double btw_angles_min[4] = { 100000., 100000., 100000., 100000. };
		bool is_boundary = true;
		// criterion 2 angle criterion
		std::vector<std::pair<size_t, float> >   dir_matches;
		{
			for (int j = 0; j < nMatches; j++)
			{
				vmfloat3 nrl_nb = nrl_pts[ret_matches[j].first];
				if (fDotVector((vmfloat3*)&nrl_src, (vmfloat3*)&nrl_nb) >= 0 || !consistent_dir_neighbors)
					dir_matches.push_back(ret_matches[j]);
			}
		}

		estimating_hole(is_boundary, is_src_vector_set, vector_o,
			btw_angles_max, btw_angles_min, dir_matches, pos_pts, pos_src, nrl_src, thres_susceptibility_sq);

		// store boundary result
		if (is_boundary) hole_flags[i] = true;
	}

	for (int i = 0; i < num_pts; i++)
		if (hole_flags[i]) bnd_id_pts.push_back(i);

	delete[] hole_flags;
}

#define RANGE_SURF_STEP_MAX_NUM 10
#define SURFACE_REFINEMENT_NUM 5
void relocate_to_target_densities(vmfloat3* pos_relocated_pts,
	const vmfloat3* pos_pts, const vmfloat3* nrl_pts, const float* densities_pts, const int num_pts,
	const float sample_dist, const __VolSampleInfo<ushort>& vol_info)
{
	auto __sample_v = [&](const vmfloat3& pos_vs)
	{
		return __Safe_TrilinearSample(pos_vs, vol_info.vol_size, vol_info.width_slice, EXB, vol_info.vol_slices);
	};

	auto __traverse_serach_dstv = [&__sample_v, &vol_info](vmfloat3& pos_dst, const vmfloat3& pos_start, const vmfloat3& vec_sample, const float dst_v, const bool from_lower)
	{
		for (int j = 1; j < RANGE_SURF_STEP_MAX_NUM; j++)
		{
			vmfloat3 pos_sample = pos_start + vec_sample * (float)j, pos_sample_vs;
			fTransformPoint(&pos_sample_vs, &pos_sample, &vol_info.mat_ws2vs);
			float sample_v = __sample_v(pos_sample_vs);

			if (
				(sample_v > dst_v && from_lower)
				|| (sample_v < dst_v && !from_lower)
				)
			{
				// requires boundary-fitting
				vmfloat3 pos_S = pos_sample - vec_sample;
				vmfloat3 pos_E = pos_sample;
				vmfloat3 pos_O = pos_S;
				for (uint k = 0; k < SURFACE_REFINEMENT_NUM; k++)
				{
					vmfloat3 pos_bis = (pos_S + pos_E) * 0.5f, pos_bis_vs;
					fTransformPoint(&pos_bis_vs, &pos_bis, &vol_info.mat_ws2vs);
					sample_v = __sample_v(pos_bis_vs);

					if (
						(sample_v > dst_v && from_lower)
						|| (sample_v < dst_v && !from_lower))
						pos_E = pos_bis;
					else
						pos_S = pos_bis;
				}

				pos_dst = pos_S;
				return true;
			}
		} // for (uint j = 1; j < RANGE_SURF_STEP_MAX_NUM; j++)
		return false;
	};

	//pos_relocated_pts.clear();
	//pos_relocated_pts.assign(num_pts, vmfloat3());

#pragma omp parallel for num_threads( omp_get_num_procs() )
	for (int i = 0; i < num_pts; i++)
	{
		const vmfloat3& pos_sample = pos_pts[i];
		const vmfloat3& nrl_sample = nrl_pts[i];
		vmfloat3 pos_sample_vs;
		float dst_v = densities_pts[i];
		if (dst_v == 7777777)
		{
			pos_relocated_pts[i] = pos_sample;
			continue; // magic number exception
		}
		fTransformPoint(&pos_sample_vs, &pos_sample, &vol_info.mat_ws2vs);
		float sample_v = __sample_v(pos_sample_vs);

		vmfloat3 pos_dst_0, pos_dst_1;
		bool ret_0 = __traverse_serach_dstv(pos_dst_0, pos_sample, nrl_sample * sample_dist, dst_v, sample_v < dst_v);
		bool ret_1 = __traverse_serach_dstv(pos_dst_1, pos_sample, -nrl_sample * sample_dist, dst_v, sample_v < dst_v);
		if (ret_0 && ret_1)
		{
			pos_relocated_pts[i] = fLengthVectorSq(&(pos_sample - pos_dst_0)) < fLengthVectorSq(&(pos_sample - pos_dst_1)) ? pos_dst_0 : pos_dst_1;
		}
		else if (ret_0)
		{
			pos_relocated_pts[i] = pos_dst_0;
		}
		else if (ret_1)
		{
			pos_relocated_pts[i] = pos_dst_1;
		}
		else
		{
			pos_relocated_pts[i] = pos_sample;
		}
	} // for (uint i = 0; i < numVoxels; i++)
}

//#include <opencv2/highgui.hpp>
// ui work
void register_pobj(VmVObjectPrimitive& pobj, const vector<vmfloat3>& pos_pts, const vector<vmfloat3>& nrl_pts, const vector<vmfloat3>* ptr_clr_pts)
{
	PrimitiveData vtx_data;
	vtx_data.is_ccw = true; // NA //
	vtx_data.is_stripe = false; // NA//
	vtx_data.check_redundancy = true; // NA //
	vtx_data.ptype = PrimitiveTypePOINT;
	vtx_data.idx_stride = 1;
	vtx_data.num_vtx = (uint)pos_pts.size();
	vtx_data.num_prims = vtx_data.num_vtx;

	vmfloat3* buf_vtx_pos = new vmfloat3[vtx_data.num_vtx];
	vmfloat3* buf_nrl_pos = new vmfloat3[vtx_data.num_vtx];
	vmfloat3* buf_clr_pos = new vmfloat3[vtx_data.num_vtx];
	memcpy(buf_vtx_pos, &pos_pts[0], sizeof(vmfloat3) * vtx_data.num_vtx);
	memcpy(buf_nrl_pos, &nrl_pts[0], sizeof(vmfloat3) * vtx_data.num_vtx);
	if (ptr_clr_pts)
	{
		memcpy(buf_clr_pos, &ptr_clr_pts->at(0), sizeof(vmfloat3) * vtx_data.num_vtx);
	}
	else
	{
		for (uint i = 0; i < vtx_data.num_vtx; i++)
			buf_clr_pos[i] = vmfloat3(1);
	}

	vtx_data.ReplaceOrAddVerticeDefinition("POSITION", buf_vtx_pos);
	vtx_data.ReplaceOrAddVerticeDefinition("NORMAL", buf_nrl_pos);
	vtx_data.ReplaceOrAddVerticeDefinition("TEXCOORD0", buf_clr_pos); // special case //
	vtx_data.ComputeOrthoBoundingBoxWithCurrentValues();

	pobj.RegisterPrimitiveData(vtx_data);
	pobj.UpdateKDTree();
	pobj.RegisterCustomParameter("_bool_ApplyShadingFactors", true);
	pobj.RegisterCustomParameter("_bool_CtModelerStep", true);
}

#define COLORMAP_SIZE 1024
void fill_jet_colormap(int* colorarray, int ary_size)
{
	int prev_idx = 0;

	float gap = (float)ary_size / 4.f;
	vmfloat3 redf = vmfloat3(1.f, 0, 0);
	vmfloat3 greenf = vmfloat3(0, 1.f, 0);
	vmfloat3 bluef = vmfloat3(0, 0, 1.f);

	int index_1 = (int)(gap / 2.f + 0.5f);
	int index_2 = (int)(gap / 2.f + gap + 0.5f);
	int index_3 = (int)(gap / 2.f + 2 * gap + 0.5f);
	int index_4 = (int)(gap / 2.f + 3 * gap + 0.5f);
	int index_5 = ary_size;

	auto float3_2_int = [&](vmfloat3& c)
	{
		byte r = (byte)__min(c.x * 255.f, 255.f);
		byte g = (byte)__min(c.y * 255.f, 255.f);
		byte b = (byte)__min(c.z * 255.f, 255.f);
		return (r << 16) | (g << 8) | (b);
	};

	for (int i = 0; i <= index_1; i++)
	{
		vmfloat3 clr = bluef * (0.5f + 0.5f * (float)i / (float)index_1);
		colorarray[i] = float3_2_int(clr);
	}
	for (int i = index_1; i <= index_2; i++)
	{
		float ratio = (float)(i - index_1) / (float)(index_2 - index_1);
		vmfloat3 clr = bluef + greenf * ratio;
		colorarray[i] = float3_2_int(clr);
	}
	for (int i = index_2; i <= index_3; i++)
	{
		float ratio = (float)(i - index_2) / (float)(index_3 - index_2);
		vmfloat3 clr = greenf + bluef * (1.f - ratio) + redf * ratio;
		colorarray[i] = float3_2_int(clr);
	}
	for (int i = index_3; i <= index_4; i++)
	{
		float ratio = (float)(i - index_3) / (float)(index_4 - index_3);
		vmfloat3 clr = redf + greenf * (1.f - ratio);
		colorarray[i] = float3_2_int(clr);
	}
	for (int i = index_4; i < index_5; i++)
	{
		vmfloat3 clr = redf * (1.f - 0.5f * (float)(i - index_4) / (float)(index_5 - index_4));
		colorarray[i] = float3_2_int(clr);
	}
}

void fill_cool_colormap(int* colorarray, int ary_size)
{
	int prev_idx = 0;

	float gap = (float)ary_size / 4.f;
	vmfloat3 pinkf = vmfloat3(1.f, 0, 1.f);
	vmfloat3 skyf = vmfloat3(0, 1.f, 1.f);

	auto float3_2_int = [&](vmfloat3& c)
	{
		byte r = (byte)__min(c.x * 255.f, 255.f);
		byte g = (byte)__min(c.y * 255.f, 255.f);
		byte b = (byte)__min(c.z * 255.f, 255.f);
		return (r << 16) | (g << 8) | (b);
	};

	for (int i = 0; i < ary_size; i++)
	{
		float ratio = (float)(i) / (float)(ary_size);
		vmfloat3 clr = skyf * (1 - ratio) + pinkf * ratio;
		colorarray[i] = float3_2_int(clr);
	}
}

inline void convert_int_to_float3(const int _color, vmfloat3& rgb)
{
	rgb.r = ((_color >> 16) & 0xFF) / 255.f;
	rgb.g = ((_color >> 8) & 0xFF) / 255.f;
	rgb.b = ((_color >> 0) & 0xFF) / 255.f;
}

//auto is_inside_box = [](vmfloat3& p, vmfloat3& bp, float bs)
//{
//	vmfloat3 pos_min = bp - vmfloat3(bs);
//	vmfloat3 pos_max = bp + vmfloat3(bs);
//	return p.x >= pos_min.x && p.x <= pos_max.x
//		&& p.y >= pos_min.y && p.y <= pos_max.y
//		&& p.z >= pos_min.z && p.z <= pos_max.z;
//};
//auto cout_f3 = [](string prefix, vmfloat3& p)
//{
//	cout << prefix << p.x << ", " << p.y << ", " << p.z << endl;
//};


#include<iostream>
//#define EIGEN_USE_MKL_ALL
#include"basis.hpp"
#include"operators.hpp"
#include"diag.h"
#include"tpoperators.hpp"
#include"files.hpp"
#include"timeev.hpp"
#include"ETH.hpp"
#include<iomanip>
#include <boost/program_options.hpp>

using namespace boost::program_options;
int main(int argc, char *argv[])
{
  using namespace Eigen;
using namespace std;
using namespace Many_Body;
  using Fermi_HubbardBasis= TensorProduct<ElectronBasis, ElectronBasis>;
   using HolsteinBasis= TensorProduct<ElectronBasis, PhononBasis>;
  using Mat= Operators::Mat;
   using boost::program_options::value;
   int M{};
  int L{};
  int i{};
  int j{};
    double T{};
      double tot{};
        double dt{};
  double t0{};
  double omega{};
  double gamma{};
  double cutoff{};
  bool PB{};

  std::string sM{};
  std::string sL{};
  std::string si{};
  std::string sj{};
  std::string star{};
  std::string sT{};
    std::string stot{};
      std::string sdt{};
  std::string st0{};
  std::string somega{};
  std::string sgamma{};
  std::string starget{};
  std::string scutoff{};
  std::string sPB{};
  std::string filename="SPexT";
  try
  {
    boost::program_options::options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help screen")
      ("L", value(&L)->default_value(4), "L")
      ("i", value(&i)->default_value(0), "i")
      ("j", value(&j)->default_value(1), "j")
      ("T", value(&T)->default_value(0.1), "T")
      ("M,m", value(&M)->default_value(2), "M")
      ("tot", value(&tot)->default_value(1.), "tot")
      ("dt", value(&dt)->default_value(0.1), "dt")
      ("t0", value(&t0)->default_value(1.), "t0")
      ("gam", value(&gamma)->default_value(1.), "gamma")
      ("omg", value(&omega)->default_value(1.), "omega")
      ("pb", value(&PB)->default_value(0), "pb");
  boost::program_options::variables_map vm;
  boost::program_options::store(parse_command_line(argc, argv, desc), vm);
  boost::program_options::notify(vm);

  if (vm.count("help"))
      {std::cout << desc << '\n'; return 0;}
    else{
      if (vm.count("L"))
      {      std::cout << "L: " << vm["L"].as<int>() << '\n';
      	sL="L"+std::to_string(vm["L"].as<int>());
	filename+=sL;
      }
         if (vm.count("M"))
      {      std::cout << "M: " << vm["M"].as<int>() << '\n';
      	sM="M"+std::to_string(vm["M"].as<int>());
	filename+=sM;
      }
	   if (vm.count("i"))
      {      std::cout << "i: " << i << '\n';
      	si="i"+std::to_string(i);
	filename+=si;
      }
	   if (vm.count("j"))
      {      std::cout << "j: " << j << '\n';
      	sj="j"+std::to_string(j);
	filename+=sj;
      }
      	 if (vm.count("t0"))
      {      std::cout << "t0: " << vm["t0"].as<double>() << '\n';
      	st0="t0"+std::to_string(vm["t0"].as<double>()).substr(0, 3);
      	filename+=st0;
      }
      	 	 if (vm.count("omg"))
      {      std::cout << "omega: " << vm["omg"].as<double>() << '\n';
      	somega="omg"+std::to_string(vm["omg"].as<double>()).substr(0, 3);
      		filename+=somega;
      }

      		 if (vm.count("gam"))
      {      std::cout << "gamma: " << vm["gam"].as<double>() << '\n';
      	sgamma="gam"+std::to_string(vm["gam"].as<double>()).substr(0, 3);
      		filename+=sgamma;
      }
		      		 if (vm.count("tot"))
      {      std::cout << "tot: " << vm["tot"].as<double>() << '\n';
      	stot="tot"+std::to_string(vm["tot"].as<double>()).substr(0, 3);
      		filename+=stot;
      }
				 if (vm.count("dt"))
      {      std::cout << "dt: " << vm["dt"].as<double>() << '\n';
      	sdt="dt"+std::to_string(vm["dt"].as<double>()).substr(0, 3);
      		filename+=sdt;
      }

      		 if (vm.count("T"))
      {      std::cout << "T: " << T << '\n';
      	sT="T"+std::to_string(vm["T"].as<double>()).substr(0, 3);
      	filename+=sT;
      }
		 if (vm.count("pb"))
      {      std::cout << "PB: " << vm["pb"].as<bool>() << '\n';
      	sPB="PB"+std::to_string(vm["pb"].as<bool>());
      		filename+=sPB;
      }
      }
    }
  catch (const boost::program_options::error &ex)
  {
    std::cerr << ex.what() << '\n';
    return 0;
  }
  filename+=".bin";
  double mean=0.5*L*M*omega;

  // declaring the tensor products with 0 e, 1, e, 2 e
    ElectronBasis e0( L, 0);
  ElectronBasis e1( L, 1);
  ElectronBasis e2( L, 2);
  std::cout<< e2<<std::endl;
  PhononBasis ph(L, M);
HolsteinBasis TP0(e0, ph);
      HolsteinBasis TP1(e1, ph);
        HolsteinBasis TP2(e2, ph);

   		  std::cout<< TP1.dim << std::endl;
		  //std::cout<< ph << std::endl;
		  //    std::cout<< e1 << std::endl;
      Eigen::VectorXd eigenVals1(TP1.dim);
      Eigen::VectorXd eigenVals2(TP2.dim);
      Eigen::VectorXd eigenVals0(TP0.dim);
      Mat H0=Operators::HolsteinHam(TP0, t0, omega, gamma, PB);
      Mat H1=Operators::HolsteinHam(TP1, t0, omega, gamma, PB);
      Mat H2=Operators::HolsteinHam(TP2, t0, omega, gamma, PB);
    Eigen::MatrixXd HH0=Eigen::MatrixXd(H0);
    Eigen::MatrixXd HH1=Eigen::MatrixXd(H1);
    Eigen::MatrixXd HH2=Eigen::MatrixXd(H2);
    //         std::cout<< HH<<std::endl;
    //  Eigen::MatrixXd N=Eigen::MatrixXd(Eph);
    Many_Body::diagMat(HH0, eigenVals0);
	  Many_Body::diagMat(HH1, eigenVals1);
       Many_Body::diagMat(HH2, eigenVals2);
       //
       auto C2=Operators::CdagOperator(TP1, TP2, j,  PB);
       auto C1=Operators::CdagOperator(TP1, TP0,i,  PB);
       std::cout<<"MIN E0 "<<std::setprecision(15)<< eigenVals0(0)<< std::endl;
     std::cout<<"MIN E1 "<<std::setprecision(15)<< eigenVals1(0)<< std::endl;
     std::cout<<"MIN E2 "<<std::setprecision(15)<< eigenVals2(0)<< std::endl;
     std::cout<< " dim T0 "<< TP0.dim<<std::endl;
          std::cout<< " dim T1 "<< TP1.dim<<std::endl;
	       std::cout<< " dim T2 "<< TP2.dim<<std::endl;

     // std::cout<< Eigen::MatrixXd(Cdag) << std::endl;
     // std::cout<< std::endl;
     // std::cout<<TP0<<std::endl;
     //      std::cout<<TP1<<std::endl;
	       std::cout<< "mat "<< std::endl;
	       std::cout<< Eigen::MatrixXd(C1)*Eigen::MatrixXd(C1).transpose()<< std::endl;
      std::cout<< "endl "<<std::endl;
      std::cout<< Eigen::MatrixXd(C2) << std::endl;
     // std::cout<< "\n";
     //  std::cout<< Eigen::MatrixXd(Operators::CdagOperator(TP1, TP0,1,  PB)) << std::endl;
       int n=0;
       Eigen::MatrixXcd evExpbeta=Eigen::MatrixXcd::Zero(eigenVals1.rows(), eigenVals1.rows());
       double Z{0};
       for(int k=0; k<eigenVals1.rows(); k++)
	 {
	   double ex=std::exp(-(eigenVals1(k)-eigenVals1(0))/T);
	   evExpbeta(k, k)=std::complex<double>{std::exp(-(eigenVals1(k)-eigenVals1(0))/T), 0};
	Z+=ex;
	//	sum+=ex*en;
	 }

       //=TimeEv::EigenvalExponent(eigenVals1, -Many_Body::im*1./T);

     std::vector<double> time;
     std::vector<std::complex<double>> cdagc;
     std::vector<std::complex<double>> ccdag;
     auto H1T=HH1.transpose();
     auto H2T=HH2.transpose();
       auto H0T=HH0.transpose();
  //       while(n*dt<tot)
  //       	{

  //    	    Eigen::MatrixXcd evExp1m=TimeEv::EigenvalExponent(eigenVals1, n*dt);
  //      Eigen::MatrixXcd evExp1p=TimeEv::EigenvalExponent(eigenVals1, -n*dt);
  //       Eigen::MatrixXcd evExp2m=TimeEv::EigenvalExponent(eigenVals2, n*dt);
  //        Eigen::MatrixXcd evExp0p=TimeEv::EigenvalExponent(eigenVals0, -n*dt);

  // 	 auto CDAGC=(evExpbeta*H1T*C1*HH0*evExp0p*H0T*C1.adjoint()*HH1*evExp1m).trace()/Z;

  // 	 //.trace()/Z;
  // 		     //*evExp1m).trace()/Z;
  // 	 	 auto CCDAG= ((evExpbeta*evExp1p*H1T*C2.adjoint()*HH2*evExp2m*H2T*C2*HH1)).trace()/Z;
  // 	 // std::cout<< "shape2 "<< CCDAG.rows() << " x "  << CCDAG.cols() << " tr "<< CCDAG.trace()<<std::endl;
  // 		 std::cout<<n*dt << "\t"<< CCDAG<< " and  "<<CDAGC<< "diff "<< CCDAG+CDAGC<< std::endl;
  // 		      //	std::cout<<n*dt << "\t"<<  " and  "<<CCDAG<< "  "<< "  sum  " <<Z<<std::endl;
  //      				    time.push_back(n*dt);
  //    				    ccdag.push_back(CCDAG);
  //    			    cdagc.push_back(CDAGC);
  // n++;

  // 	 	}
  // 	std::cout<< ccdag.size() << "  "<< cdagc.size()<< std::endl;
  //     bin_write("time"+filename, time);
  //     bin_write("CCDAG"+filename, ccdag);
  //     bin_write("CDAGC"+filename, cdagc);

    // 			  int pb=PB;
    // 		  	  std::string Hs="H";
    // 			  std::string Ts="T";
    // 			  std::string phds="PHD";

    // 		      bin_write("E"+filename, Evec);
    // 		      bin_write("Nph"+filename, Ovec);
    // 		      bin_write("EK"+filename, Ovec2);
    // 		      bin_write("nX"+filename, Ovec3);
    // 		      bin_write("temp"+filename, Tr);
  return 0;
}


#include "RigidBodyIK.h"
#include "RigidBodyManipulator.h"
#include "../constraint/RigidBodyConstraint.h"
#include "../IKoptions.h"
#include <iostream>
#include <cstdlib>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;
int main()
{
  RigidBodyManipulator model("examples/Atlas/urdf/atlas_minimal_contact.urdf");

  Vector2d tspan;
  tspan<<0,1;
  VectorXd q0 = VectorXd::Zero(model.num_positions);
  // The state frame of cpp model does not match with the state frame of MATLAB model, since the dofname_to_dofnum is different in cpp and MATLAB
  q0(3) = 0.8;
  Vector3d com_lb = Vector3d::Zero();
  Vector3d com_ub = Vector3d::Zero();
  com_lb(2) = 0.9;
  com_ub(2) = 1.0;
  WorldCoMConstraint* com_kc = new WorldCoMConstraint(&model,com_lb,com_ub,tspan);
  int num_constraints = 1;
  RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
  constraint_array[0] = com_kc;
  IKoptions ikoptions(&model);
  VectorXd q_sol(model.num_positions);
  int info;
  vector<string> infeasible_constraint;
  inverseKin(&model,q0,q0,num_constraints,constraint_array,q_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    return 1;
  }
  KinematicsCache<double> cache = model.doKinematics(q_sol, 0);
  Vector3d com = model.centerOfMass(cache);
  printf("%5.2f\n%5.2f\n%5.2f\n",com(0),com(1),com(2));
  /*MATFile *presultmat;
  presultmat = matOpen("q_sol.mat","w");
  mxArray* pqsol = mxCreateDoubleMatrix(model.num_dof,1,mxREAL);
  memcpy(mxGetPrSafe(pqsol),q_sol.data(),sizeof(double)model.num_dof);
  matPutVariable(presultmat,"q_sol",pqsol);
  matClose(presultmat);*/
  delete com_kc;
  delete[] constraint_array;
  return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef ALIGNED_EIGEN_TYPES_HPP
#define ALIGNED_EIGEN_TYPES_HPP

#include <Eigen/Geometry>

namespace Eigen {

/**
 * \defgroup Aligned4Vector3_Module Aligned vector3 module
 *
 * \code
 * #include <unsupported/Eigen/Aligned4Vector3>
 * \endcode
 */
//@{

/** \class Aligned4Vector3
 *
 * \brief A vectorization friendly 3D vector
 *
 * This class represents a 3D vector internally using a 4D vector
 * such that vectorization can be seamlessly enabled. Of course,
 * the same result can be achieved by directly using a 4D vector.
 * This class makes this process simpler.
 *
 */
// TODO specialize Cwise
template <typename _Scalar>
class Aligned4Vector3;

namespace internal {
template <typename _Scalar>
struct traits<Aligned4Vector3<_Scalar> > : traits<Matrix<_Scalar, 3, 1, 0, 3, 1> > {};
}  // namespace internal

template <typename _Scalar>
class Aligned4Vector3 : public MatrixBase<Aligned4Vector3<_Scalar> > {
  typedef Matrix<_Scalar, 4, 1> CoeffType;
  CoeffType m_coeffs;

 public:
  typedef MatrixBase<Aligned4Vector3<_Scalar> > Base;
  EIGEN_DENSE_PUBLIC_INTERFACE(Aligned4Vector3)
  using Base::operator*;

  inline Index rows() const { return 3; }
  inline Index cols() const { return 1; }

  Scalar* data() { return m_coeffs.data(); }
  const Scalar* data() const { return m_coeffs.data(); }
  Index innerStride() const { return 1; }
  Index outerStride() const { return m_coeffs.outerStride(); }

  inline const Scalar& coeff(Index row, Index col) const { return m_coeffs.coeff(row, col); }

  inline Scalar& coeffRef(Index row, Index col) { return m_coeffs.coeffRef(row, col); }

  inline const Scalar& coeff(Index index) const { return m_coeffs.coeff(index); }

  inline Scalar& coeffRef(Index index) { return m_coeffs.coeffRef(index); }

  inline Aligned4Vector3(const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, Scalar(1)) {}

  inline Aligned4Vector3() : m_coeffs(Scalar(0), Scalar(0), Scalar(0), Scalar(1)) {}

  inline Aligned4Vector3(const Aligned4Vector3& other) : Base(), m_coeffs(other.m_coeffs) {}

  template <typename XprType, int Size = XprType::SizeAtCompileTime>
  struct generic_assign_selector {};

  template <typename XprType>
  struct generic_assign_selector<XprType, 4> {
    inline static void run(Aligned4Vector3& dest, const XprType& src) {
      dest.m_coeffs = src;
      dest.m_coeffs.w() = Scalar(1);
    }
  };

  template <typename XprType>
  struct generic_assign_selector<XprType, 3> {
    inline static void run(Aligned4Vector3& dest, const XprType& src) {
      dest.m_coeffs.template head<3>() = src;
      dest.m_coeffs.w() = Scalar(1);
    }
  };

  template <typename Derived>
  inline Aligned4Vector3(const MatrixBase<Derived>& other) {
    generic_assign_selector<Derived>::run(*this, other.derived());
  }

  inline Aligned4Vector3& operator=(const Aligned4Vector3& other) {
    m_coeffs = other.m_coeffs;
    return *this;
  }

  template <typename Derived>
  inline Aligned4Vector3& operator=(const MatrixBase<Derived>& other) {
    generic_assign_selector<Derived>::run(*this, other.derived());
    return *this;
  }

  inline Aligned4Vector3 operator+(const Aligned4Vector3& other) const {
    return Aligned4Vector3(m_coeffs + other.m_coeffs);
  }

  inline Aligned4Vector3& operator+=(const Aligned4Vector3& other) {
    m_coeffs += other.m_coeffs;
    return *this;
  }

  inline Aligned4Vector3 operator-(const Aligned4Vector3& other) const {
    return Aligned4Vector3(m_coeffs - other.m_coeffs);
  }

  inline Aligned4Vector3 operator-=(const Aligned4Vector3& other) {
    m_coeffs -= other.m_coeffs;
    return *this;
  }

  inline Aligned4Vector3 operator*(const Scalar& s) const { return Aligned4Vector3(m_coeffs * s); }

  inline friend Aligned4Vector3 operator*(const Scalar& s, const Aligned4Vector3& vec) {
    return Aligned4Vector3(s * vec.m_coeffs);
  }

  inline Aligned4Vector3& operator*=(const Scalar& s) {
    m_coeffs *= s;
    return *this;
  }

  inline Aligned4Vector3 operator/(const Scalar& s) const { return Aligned4Vector3(m_coeffs / s); }

  inline Aligned4Vector3& operator/=(const Scalar& s) {
    m_coeffs /= s;
    return *this;
  }

  inline Scalar dot(const Aligned4Vector3& other) const {
    eigen_assert(m_coeffs.w() == Scalar(1));
    eigen_assert(other.m_coeffs.w() == Scalar(1));
    return m_coeffs.dot(other.m_coeffs) - Scalar(1);
  }

  inline void normalize() { m_coeffs /= norm(); }

  inline Aligned4Vector3 normalized() const { return Aligned4Vector3(m_coeffs / norm()); }

  inline Scalar sum() const {
    eigen_assert(m_coeffs.w() == Scalar(1));
    return m_coeffs.sum() - Scalar(1);
  }

  inline Scalar squaredNorm() const {
    eigen_assert(m_coeffs.w() == Scalar(1));
    return m_coeffs.squaredNorm() - Scalar(1);
  }

  inline Scalar norm() const {
    using std::sqrt;
    return sqrt(squaredNorm());
  }

  inline Aligned4Vector3 cross(const Aligned4Vector3& other) const {
    return Aligned4Vector3(m_coeffs.cross3(other.m_coeffs));
  }

  template <typename Derived>
  inline bool isApprox(const MatrixBase<Derived>& other, RealScalar eps = NumTraits<Scalar>::dummy_precision()) const {
    return m_coeffs.template head<3>().isApprox(other, eps);
  }

  CoeffType& coeffs() { return m_coeffs; }
  const CoeffType& coeffs() const { return m_coeffs; }
};

namespace internal {

template <typename _Scalar>
struct eval<Aligned4Vector3<_Scalar>, Dense> {
  typedef const Aligned4Vector3<_Scalar>& type;
};

template <typename Scalar>
struct evaluator<Aligned4Vector3<Scalar> > : evaluator<Matrix<Scalar, 4, 1> > {
  typedef Aligned4Vector3<Scalar> XprType;
  typedef evaluator<Matrix<Scalar, 4, 1> > Base;

  evaluator(const XprType& m) : Base(m.coeffs()) {}
};

}  // namespace internal

//@}

}  // namespace Eigen

#endif  // ALIGNED_EIGEN_TYPES


#include <boost/test/unit_test.hpp>

#include <boost/optional/optional_io.hpp>
#include "physics.h"

BOOST_AUTO_TEST_SUITE(Physics)

BOOST_AUTO_TEST_CASE(testRectangleCollisionsFromTop)
{
  OutsideRectangle r(sf::Vector2f(0, 0), sf::Vector2f(5, 3));

  // Hit straight down
  CollisionDataOpt c1 = r.testHit(Disc(sf::Vector2f(2, 6), 1),
                                  sf::Vector2f(0, -1),
                                  2);

  BOOST_CHECK_EQUAL(CollisionData(sf::Vector2f(2, 4),
                                  sf::Vector2f(0, 1),
                                  0),
                    c1);

  // Hit down from angle
  CollisionDataOpt c2 = r.testHit(Disc(sf::Vector2f(4, 5), 1),
                                  sf::Vector2f(-1, -1),
                                  4);

  BOOST_CHECK_EQUAL(CollisionData(sf::Vector2f(3, 4),
                                  sf::Vector2f(-1, 1),
                                  3),
                    c2);
}

BOOST_AUTO_TEST_CASE(testRectangleCollisionsFromBottom)
{
  OutsideRectangle r(sf::Vector2f(0, 0), sf::Vector2f(5, 3));

  // Hit straight up
  CollisionDataOpt c1 = r.testHit(Disc(sf::Vector2f(2, -2), 1),
                                  sf::Vector2f(0, 1),
                                  3);

  BOOST_CHECK_EQUAL(CollisionData(sf::Vector2f(2, -1),
                                  sf::Vector2f(0, -1),
                                  2),
                    c1);

  // Hit up at an angle
  CollisionDataOpt c2 = r.testHit(Disc(sf::Vector2f(2, -3), 1),
                                  sf::Vector2f(1, 2),
                                  1);

  BOOST_CHECK_EQUAL(CollisionData(sf::Vector2f(3, -1),
                                  sf::Vector2f(1, -2),
                                  0),
                    c2);

  // Hit up at an angle
}

BOOST_AUTO_TEST_CASE(testRectangleCollisionsCorners)
{
  OutsideRectangle r(sf::Vector2f(0, 0), sf::Vector2f(5, 3));

  // Hit from bottom left toward top right
  CollisionDataOpt c1 = r.testHit(Disc(sf::Vector2f(-2, -2), 1),
                                  sf::Vector2f(1, 1),
                                  3);

  BOOST_CHECK_EQUAL(CollisionData(sf::Vector2f(-0.707, -0.707),
                                  sf::Vector2f(-1, -1),
                                  1.707),
                    c1);

  // Hit from top left toward bottom right
  CollisionDataOpt c2 = r.testHit(Disc(sf::Vector2f(-2, 5), 1),
                                  sf::Vector2f(1, -1),
                                  3);

  BOOST_CHECK_EQUAL(CollisionData(sf::Vector2f(-0.707, 3.707),
                                  sf::Vector2f(-1, 1),
                                  1.707),
                    c2);

  // Hit from bottom right toward top left
  CollisionDataOpt c3 = r.testHit(Disc(sf::Vector2f(7, -2), 1),
                                  sf::Vector2f(-1, 1),
                                  3);

  BOOST_CHECK_EQUAL(CollisionData(sf::Vector2f(5.707, -0.707),
                                  sf::Vector2f(1, -1),
                                  1.707),
                    c3);

  // Hit from top right toward bottom left
  CollisionDataOpt c4 = r.testHit(Disc(sf::Vector2f(7, 5), 1),
                                  sf::Vector2f(-1, -1),
                                  3);

  BOOST_CHECK_EQUAL(CollisionData(sf::Vector2f(5.707, 3.707),
                                  sf::Vector2f(1, 1),
                                  1.707),
                    c4);

}

BOOST_AUTO_TEST_CASE(testRectangleCollisionsComplexCorners)
{
  OutsideRectangle r(sf::Vector2f(0, 0), sf::Vector2f(5, 3));

  // We're going stright left, and are located slightly lower than the rectangle
  CollisionDataOpt c1 = r.testHit(Disc(sf::Vector2f(7, -0.5), 1),
                                  sf::Vector2f(-1, 0),
                                  3);

  BOOST_CHECK_EQUAL(CollisionData(sf::Vector2f(5.866, -0.5),
                                  sf::Vector2f(0.5, -0.866025),
                                  1.866),
                    c1);
}

BOOST_AUTO_TEST_CASE(testRectangleVsRectangleCollisionsHit)
{
  OutsideRectangle r(sf::Vector2f(0, 0), sf::Vector2f(5, 3));

  // With self
  BOOST_CHECK(r.testHit(r));

  // Fully inside
  BOOST_CHECK(r.testHit(OutsideRectangle(sf::Vector2f(1, 1), sf::Vector2f(4, 2))));

  // Fully outside
  BOOST_CHECK(r.testHit(OutsideRectangle(sf::Vector2f(-1, -1), sf::Vector2f(6, 4))));

  // Lower but larger
  BOOST_CHECK(r.testHit(OutsideRectangle(sf::Vector2f(-1, 1), sf::Vector2f(6, 2))));

  // Higher but narrower
  BOOST_CHECK(r.testHit(OutsideRectangle(sf::Vector2f(1, -1), sf::Vector2f(4, 4))));
}

BOOST_AUTO_TEST_CASE(testRectangleVsRectangleCollisions)
{
  OutsideRectangle r(sf::Vector2f(0, 0), sf::Vector2f(5, 3));

  // Higher
  BOOST_CHECK(!r.testHit(OutsideRectangle(sf::Vector2f(2, 6), sf::Vector2f(3, 8))));

  // Lower
  BOOST_CHECK(!r.testHit(OutsideRectangle(sf::Vector2f(2, -3), sf::Vector2f(3, -1))));

  // Left
  BOOST_CHECK(!r.testHit(OutsideRectangle(sf::Vector2f(-3, 0), sf::Vector2f(-1, 3))));

  // Right
  BOOST_CHECK(!r.testHit(OutsideRectangle(sf::Vector2f(7, 0), sf::Vector2f(9, 3))));
}

BOOST_AUTO_TEST_CASE(testRectangleVsRectangleBoundaries)
{
  OutsideRectangle r(sf::Vector2f(0, 0), sf::Vector2f(5, 3));

  // Just higher
  BOOST_CHECK(r.testHit(OutsideRectangle(sf::Vector2f(2, 3), sf::Vector2f(3, 8))));

  // Just lower
  BOOST_CHECK(r.testHit(OutsideRectangle(sf::Vector2f(2, -3), sf::Vector2f(3, 0))));

  // Just left
  BOOST_CHECK(r.testHit(OutsideRectangle(sf::Vector2f(-3, 0), sf::Vector2f(0, 3))));

  // Just right
  BOOST_CHECK(r.testHit(OutsideRectangle(sf::Vector2f(5, 0), sf::Vector2f(9, 3))));
}

BOOST_AUTO_TEST_SUITE_END()