/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2014 - 2020 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Wolfgang Bangerth, Texas A&M University, 2014
 *          Luca Heltai, SISSA, 2014
 *          D. Sarah Stamps, MIT, 2014
 */

// 让我们从这里需要的包含文件开始。显然，我们需要描述三角形的文件（  tria.h  ），以及允许我们创建和输出三角形的文件（  grid_generator.h  和  grid_out.h  ）。此外，我们需要声明Manifold和ChartManifold类的头文件，我们将需要这些类来描述几何体（ manifold.h ）。然后我们还需要以下头文件中的 GridTools::transform() 函数；这个函数的用途将在我们使用它时讨论。

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/manifold.h>
#include <deal.II/grid/grid_tools.h>

// 其余的包含文件与读取地形数据有关。正如介绍中所解释的，我们将从一个文件中读取它，然后使用下面头文件中第一个声明的 Functions::InterpolatedUniformGridData  类。因为数据很大，所以我们读取的文件是以gzip压缩数据的形式存储的，我们利用BOOST提供的一些功能来直接读取gzipped数据。

#include <deal.II/base/function_lib.h>

#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/device/file.hpp>

#include <fstream>
#include <iostream>
#include <memory>

// 上事的最后部分是打开一个命名空间，把所有东西都放进去，然后把dealii命名空间导入其中。

namespace Step53
{
  using namespace dealii;
// @sect3{Describing topography: AfricaTopography}

// 这个程序的第一个重要部分是描述地形 $h(\hat phi,\hat \theta)$ 作为经度和纬度的函数的类。正如在介绍中所讨论的那样，我们在这里将使我们的生活更容易一些，不以最普遍的方式来写这个类，而是只为我们在这里感兴趣的特定目的来写：插值从一个非常具体的数据文件中获得的数据，该文件包含了关于世界上一个特定地区的信息，我们知道该地区的范围。

// 该类的总体布局已经在上面讨论过了。下面是它的声明，包括我们在初始化 topography_data 成员变量时需要的三个静态成员函数。

  class AfricaTopography
  {
  public:
    AfricaTopography();

    double value(const double lon, const double lat) const;

  private:
    const Functions::InterpolatedUniformGridData<2> topography_data;

    static std::vector<double> get_data();
  };

// 让我们来看看这个类的实现。该类的有趣部分是构造函数和 value() 函数。前者初始化了 Functions::InterpolatedUniformGridData 成员变量，我们将使用这个构造函数，它要求我们传入我们要插值的二维数据集的端点（这里由区间 $[-6.983333, 11.98333]$ 给出）。 ]，使用介绍中讨论的切换端点的技巧，和 $[25, 35.983333]$ ，都是以度数给出的），数据被分割成的区间数（纬度方向379，经度方向219，总共 $380\times 220$ 个数据点），和一个包含数据的表对象。然后，数据的大小当然是 $380\times 220$ ，我们通过提供一个迭代器给下面 std::vector 函数返回的 get_data() 对象的83,600个元素中的第一个来初始化它。注意，我们在这里调用的所有成员函数都是静态的，因为(i)它们不访问类的任何成员变量，(ii)因为它们是在对象没有完全初始化的时候调用的。

  AfricaTopography::AfricaTopography()
    : topography_data({{std::make_pair(-6.983333, 11.966667),
                        std::make_pair(25, 35.95)}},
                      {{379, 219}},
                      Table<2, double>(380, 220, get_data().begin()))
  {}

  double AfricaTopography::value(const double lon, const double lat) const
  {
    return topography_data.value(
      Point<2>(-lat * 180 / numbers::PI, lon * 180 / numbers::PI));
  }

// 唯一一个更有意义的函数是 get_data() 函数。它返回一个临时向量，其中包含描述高度的所有83600个数据点，并从文件 topography.txt.gz 中读取。因为文件被gzip压缩了，所以我们不能直接通过类型为 std::ifstream, 的对象来读取它，但在BOOST库中有一些方便的方法（见http:www.boost.org），允许我们从压缩的文件中读取，而不用先在磁盘上解压缩。其结果是，基本上，只是另一个输入流，就所有的实际目的而言，看起来就像我们一直使用的那些输入流。

// 当读取数据时，我们读取三列数据，但忽略了前两列。最后一列的数据被附加到一个数组中，我们返回的数组将被复制到 topography_data 的表中，并被初始化。由于BOOST.iostreams库在输入文件不存在、不可读或不包含正确的数据行数时没有提供非常有用的异常，我们捕捉它可能产生的所有异常并创建我们自己的异常。为此，在 catch 子句中，我们让程序运行到一个 AssertThrow(false, ...) 语句中。由于条件总是假的，这总是会触发一个异常。换句话说，这相当于写了 throw ExcMessage("...") ，但它也填补了异常对象中的某些字段，这些字段以后会被打印在屏幕上，识别出发生异常的函数、文件和行。

  std::vector<double> AfricaTopography::get_data()
  {
    std::vector<double> data;

// 创建一个流，我们从gzipped数据中读取

    boost::iostreams::filtering_istream in;
    in.push(boost::iostreams::basic_gzip_decompressor<>());
    in.push(boost::iostreams::file_source("topography.txt.gz"));

    for (unsigned int line = 0; line < 83600; ++line)
      {
        try
          {
            double lat, lon, elevation;
            in >> lat >> lon >> elevation;

            data.push_back(elevation);
          }
        catch (...)
          {
            AssertThrow(false,
                        ExcMessage("Could not read all 83,600 data points "
                                   "from the file <topography.txt.gz>!"));
          }
      }

    return data;
  }
// @sect3{Describing the geometry: AfricaGeometry}

// 下面的类是本程序的主类。它的结构已经在介绍中详细描述过了，不需要再多做介绍。

  class AfricaGeometry : public ChartManifold<3, 3>
  {
  public:
    virtual Point<3> pull_back(const Point<3> &space_point) const override;

    virtual Point<3> push_forward(const Point<3> &chart_point) const override;

    virtual std::unique_ptr<Manifold<3, 3>> clone() const override;

  private:
    static const double R;
    static const double ellipticity;

    const AfricaTopography topography;

    Point<3> push_forward_wgs84(const Point<3> &phi_theta_d) const;
    Point<3> pull_back_wgs84(const Point<3> &x) const;

    Point<3> push_forward_topo(const Point<3> &phi_theta_d_hat) const;
    Point<3> pull_back_topo(const Point<3> &phi_theta_d) const;
  };

  const double AfricaGeometry::R           = 6378137;
  const double AfricaGeometry::ellipticity = 8.1819190842622e-2;

// 如果你读过介绍，实现起来也是非常简单的。特别是，回拉和前推函数都只是WGS 84和地形图映射各自函数的串联。

  Point<3> AfricaGeometry::pull_back(const Point<3> &space_point) const
  {
    return pull_back_topo(pull_back_wgs84(space_point));
  }

  Point<3> AfricaGeometry::push_forward(const Point<3> &chart_point) const
  {
    return push_forward_wgs84(push_forward_topo(chart_point));
  }

// 下一个函数是Manifold基类的接口所要求的，它允许克隆AfricaGeometry类。注意，虽然该函数返回一个  `std::unique_ptr<Manifold<3,3>>`,  我们在内部创建了一个 `unique_ptr<AfricaGeometry>`。换句话说，这个库需要一个指向基类的指针，我们通过创建一个指向派生类的指针来提供这个指针。

  std::unique_ptr<Manifold<3, 3>> AfricaGeometry::clone() const
  {
    return std::make_unique<AfricaGeometry>();
  }

// 下面的两个函数就定义了对应于地球WGS84参考形状的正向和反向变换。正向变换遵循介绍中所示的公式。反变换要复杂得多，至少不是直观的。它还存在一个问题，即它返回一个角度，在函数结束时，如果它应该从那里逃出来，我们需要将其夹回区间 $[0,2\pi]$ 。

  Point<3> AfricaGeometry::push_forward_wgs84(const Point<3> &phi_theta_d) const
  {
    const double phi   = phi_theta_d[0];
    const double theta = phi_theta_d[1];
    const double d     = phi_theta_d[2];

    const double R_bar = R / std::sqrt(1 - (ellipticity * ellipticity *
                                            std::sin(theta) * std::sin(theta)));

    return {(R_bar + d) * std::cos(phi) * std::cos(theta),
            (R_bar + d) * std::sin(phi) * std::cos(theta),
            ((1 - ellipticity * ellipticity) * R_bar + d) * std::sin(theta)};
  }

  Point<3> AfricaGeometry::pull_back_wgs84(const Point<3> &x) const
  {
    const double b   = std::sqrt(R * R * (1 - ellipticity * ellipticity));
    const double ep  = std::sqrt((R * R - b * b) / (b * b));
    const double p   = std::sqrt(x(0) * x(0) + x(1) * x(1));
    const double th  = std::atan2(R * x(2), b * p);
    const double phi = std::atan2(x(1), x(0));
    const double theta =
      std::atan2(x(2) + ep * ep * b * std::pow(std::sin(th), 3),
                 (p -
                  (ellipticity * ellipticity * R * std::pow(std::cos(th), 3))));
    const double R_bar =
      R / (std::sqrt(1 - ellipticity * ellipticity * std::sin(theta) *
                           std::sin(theta)));
    const double R_plus_d = p / std::cos(theta);

    Point<3> phi_theta_d;
    if (phi < 0)
      phi_theta_d[0] = phi + 2 * numbers::PI;
    else if (phi > 2 * numbers::PI)
      phi_theta_d[0] = phi - 2 * numbers::PI;
    else
      phi_theta_d[0] = phi;
    phi_theta_d[1] = theta;
    phi_theta_d[2] = R_plus_d - R_bar;
    return phi_theta_d;
  }

// 与此相反，地形变换完全按照介绍中的描述进行。因此，没有什么可以补充的。

  Point<3>
  AfricaGeometry::push_forward_topo(const Point<3> &phi_theta_d_hat) const
  {
    const double d_hat = phi_theta_d_hat[2];
    const double h = topography.value(phi_theta_d_hat[0], phi_theta_d_hat[1]);
    const double d = d_hat + (d_hat + 500000) / 500000 * h;
    return {phi_theta_d_hat[0], phi_theta_d_hat[1], d};
  }

  Point<3> AfricaGeometry::pull_back_topo(const Point<3> &phi_theta_d) const
  {
    const double d     = phi_theta_d[2];
    const double h     = topography.value(phi_theta_d[0], phi_theta_d[1]);
    const double d_hat = 500000 * (d - h) / (500000 + h);
    return {phi_theta_d[0], phi_theta_d[1], d_hat};
  }
// @sect3{Creating the mesh}

// 在描述了几何体的属性之后，现在是处理用于离散它的网格的时候了。为此，我们为几何体和三角形创建对象，然后继续创建一个与参考域 $1\times 2\times 1$ 相对应的 $\hat U=[26,35]\times[-10,5]\times[-500000,0]$ 矩形网格。我们选择这个数目的细分，因为它导致了单元格大致上像立方体，而不是在某个方向上被拉伸。

// 当然，我们实际上对参考域的网格划分不感兴趣。我们感兴趣的是对真实域的网格划分。因此，我们将使用 GridTools::transform() 函数，它只是根据一个给定的变换来移动三角形的每个点。它想要的变换函数是一个将参考域中的一个点作为其单一参数的函数，并返回我们想要映射到的域中的相应位置。当然，这正是我们使用的几何学的前推函数。我们用一个lambda函数来包装它，以获得转换所需的那种函数对象。

  void run()
  {
    AfricaGeometry   geometry;
    Triangulation<3> triangulation;

    {
      const Point<3> corner_points[2] = {
        Point<3>(26 * numbers::PI / 180, -10 * numbers::PI / 180, -500000),
        Point<3>(35 * numbers::PI / 180, 5 * numbers::PI / 180, 0)};
      std::vector<unsigned int> subdivisions(3);
      subdivisions[0] = 1;
      subdivisions[1] = 2;
      subdivisions[2] = 1;
      GridGenerator::subdivided_hyper_rectangle(
        triangulation, subdivisions, corner_points[0], corner_points[1], true);

      GridTools::transform(
        [&geometry](const Point<3> &chart_point) {
          return geometry.push_forward(chart_point);
        },
        triangulation);
    }

// 下一步是向三角计算说明，在细化网格时，每当需要一个新的点时，都要使用我们的几何对象。我们通过告诉三角计算对所有流形指示器为零的物体使用我们的几何体，然后继续用流形指示器为零标记所有单元及其边界面和边。这确保了三角计算在每次需要新的顶点时都会参考我们的几何对象。由于流形指标是由母体继承给子体的，这也会在几个递归细化步骤之后发生。

    triangulation.set_manifold(0, geometry);
    for (const auto &cell : triangulation.active_cell_iterators())
      cell->set_all_manifold_ids(0);

// 最后一步是在最初的 $1\times 2\times 1$ 粗略网格之外细化该网格。我们可以在全局范围内细化若干次，但由于本教程程序的目的，我们实际上只对靠近表面的情况感兴趣，所以我们只是对所有在边界上有一个指标为5的面的单元进行6次细化。在我们上面使用的 GridGenerator::subdivided_hyper_rectangle() 函数的文档中查找，发现边界指标5对应于域的顶面（这就是上面调用 GridGenerator::subdivided_hyper_rectangle() 的最后一个 true 参数的含义：通过给每个边界分配一个独特的边界指标来给边界 "着色"）。

    for (unsigned int i = 0; i < 6; ++i)
      {
        for (const auto &cell : triangulation.active_cell_iterators())
          for (const auto &face : cell->face_iterators())
            if (face->boundary_id() == 5)
              {
                cell->set_refine_flag();
                break;
              }
        triangulation.execute_coarsening_and_refinement();

        std::cout << "Refinement step " << i + 1 << ": "
                  << triangulation.n_active_cells() << " cells, "
                  << GridTools::minimal_cell_diameter(triangulation) / 1000
                  << "km minimal cell diameter" << std::endl;
      }

// 做完这一切，我们现在可以将网格输出到一个自己的文件中。

    const std::string filename = "mesh.vtu";
    std::ofstream     out(filename);
    GridOut           grid_out;
    grid_out.write_vtu(triangulation, out);
  }
} // namespace Step53

//  @sect3{The main function}

// 最后是主函数，它采用了从  step-6  开始的所有教程程序中使用的相同方案。这里没有什么可做的，只需要调用单一的  run()  函数。

int main()
{
  try
    {
      Step53::run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
}


// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

/// \file
/// Operations to calculate closest points and ray intersections
/// to triangles and meshes.

#include "mesh_intersect.h"

#include <limits>

#include <vital/logger/logger.h>

#include <Eigen/Geometry>

namespace kwiver {

namespace arrows {

namespace core {

using namespace kwiver::vital;

// ----------------------------------------------------------------------------
bool
mesh_intersect_triangle(
  const point_3d& p, const vector_3d& d, const point_3d& a, const point_3d& b,
  const point_3d& c, double& dist, double& u, double& v )
{
  vector_3d n( ( b.value() - a.value() ).cross( c.value() - a.value() ) );
  return mesh_intersect_triangle( p, d, a, b, c, n, dist, u, v );
}

// ----------------------------------------------------------------------------
bool
mesh_intersect_triangle(
  const point_3d& p, const vector_3d& d, const point_3d& a, const point_3d& b,
  const point_3d& c, const vector_3d& n, double& dist, double& u, double& v )
{
  double denom = -d.dot( n );

  if( denom <= 0 ) // back facing triangles
  {
    return false;
  }

  vector_3d ap( p.value() - a.value() );
  vector_3d t( d.cross( ap ) );
  v = ( b.value() - p.value() ).dot( t );
  if( v < 0.0 || v > denom )
  {
    return false;
  }

  u = -( c.value() - p.value() ).dot( t );
  if( u < 0.0 || u + v > denom )
  {
    return false;
  }

  dist = ap.dot( n );
  if( dist < 0.0 )
  {
    return false;
  }

  u /= denom;
  v /= denom;
  dist /= denom;

  return true;
}

// ----------------------------------------------------------------------------
bool
mesh_intersect_triangle_min_dist(
  const point_3d& p, const vector_3d& d, const point_3d& a, const point_3d& b,
  const point_3d& c, const vector_3d& n, double& dist, double& u, double& v )
{
  double denom = -d.dot( n );

  if( denom <= 0 ) // back facing triangles
  {
    return false;
  }

  vector_3d ap( p.value() - a.value() );
  double new_dist = ap.dot( n ) / denom;

  if( new_dist < 0.0 || new_dist > dist )
  {
    return false;
  }

  vector_3d t( d.cross( ap ) );
  v = ( b.value() - p.value() ).dot( t );
  if( v < 0.0 || v > denom )
  {
    return false;
  }

  u = -( c.value() - p.value() ).dot( t );
  if( u < 0.0 || u + v > denom )
  {
    return false;
  }

  dist = new_dist;
  u /= denom;
  v /= denom;

  return true;
}

// ----------------------------------------------------------------------------
unsigned char
mesh_triangle_closest_point(
  const point_3d& p, const point_3d& a, const point_3d& b, const point_3d& c,
  const vector_3d& n, double& dist, double& u, double& v )
{
  double denom = 1.0 / n.squaredNorm();

  vector_3d ap( p.value() - a.value() );
  vector_3d bp( p.value() - b.value() );
  vector_3d cp( p.value() - c.value() );

  vector_3d t( n.cross( ap ) );
  v = bp.dot( t ) * denom;
  u = -cp.dot( t ) * denom;

  vector_3d ab( b.value() - a.value() );
  vector_3d bc( c.value() - b.value() );
  vector_3d ca( a.value() - c.value() );

  double eps = std::numeric_limits< double >::epsilon();

  unsigned char state = 0;
  double uv;
  if( u <= eps )
  {
    double p_v = v - u * ab.dot( ca ) / ca.squaredNorm();

    if( p_v <= eps )
    {
      state = 1;
    }
    else if( p_v >= 1.0 )
    {
      state = 4;
    }
    else
    {
      u = 0.0;
      v = p_v;
      dist = ( ( 1 - v ) * ap + v * cp ).norm();
      return 5;
    }
  }
  if( v <= eps )
  {
    double p_u = u - v * ca.dot( ab ) / ab.squaredNorm();

    if( p_u <= eps )
    {
      state = 1;
    }
    else if( p_u >= 1.0 )
    {
      state = 2;
    }
    else
    {
      u = p_u;
      v = 0.0;
      dist = ( ( 1 - u ) * ap + u * bp ).norm();
      return 3;
    }
  }
  if( ( uv = 1.0 - u - v ) <= eps )
  {
    double s = -ca.dot( bc ) / bc.squaredNorm();
    double p_u = u + uv * s;
    double p_v = v + uv * ( 1.0 - s );
    if( p_v <= eps )
    {
      state = 2;
    }
    else if( p_u <= eps )
    {
      state = 4;
    }
    else
    {
      u = p_u;
      v = p_v;
      dist = ( u * bp + v * cp ).norm();
      return 6;
    }
  }

  switch( state )
  {
    case 1:
      u = 0.0;
      v = 0.0;
      dist = ap.norm();
      return 1;
    case 2:
      u = 1.0;
      v = 0.0;
      dist = bp.norm();
      return 2;
    case 4:
      u = 0.0;
      v = 1.0;
      dist = cp.norm();
      return 4;
    default:
      dist = std::abs( ap.dot( n ) * std::sqrt( denom ) );
      return 7;
  }

  return 0;
}

// ----------------------------------------------------------------------------
unsigned char
mesh_triangle_closest_point(
  const point_3d& p, const point_3d& a, const point_3d& b, const point_3d& c,
  double& dist, double& u, double& v )
{
  vector_3d n( ( b.value() - a.value() ).cross( c.value() - a.value() ) );
  return mesh_triangle_closest_point( p, a, b, c, n, dist, u, v );
}

// ----------------------------------------------------------------------------
vital::point_3d
mesh_triangle_closest_point(
  const point_3d& p, const point_3d& a, const point_3d& b, const point_3d& c,
  double& dist )
{
  double u, v;
  mesh_triangle_closest_point( p, a, b, c, dist, u, v );

  double t = 1 - u - v;
  return point_3d( t * a[ 0 ] + u * b[ 0 ] + v * c[ 0 ],
                   t * a[ 1 ] + u * b[ 1 ] + v * c[ 1 ],
                   t * a[ 2 ] + u * b[ 2 ] + v * c[ 2 ] );
}

// ----------------------------------------------------------------------------
int
mesh_closest_point(
  const point_3d& p, const mesh& mesh, point_3d& cp, double& u, double& v )
{
  // check for a triangular mesh
  if( mesh.faces().regularity() != 3 )
  {
    LOG_ERROR( vital::get_logger( "arrows.core.mesh_closest_point" ),
               "Closest point calculation requires triangular mesh." );
    return -1;
  }

  const mesh_vertex_array< 3 >& verts = mesh.vertices< 3 >();
  const mesh_regular_face_array< 3 >& faces =
    static_cast< const mesh_regular_face_array< 3 >& >( mesh.faces() );

  int isect = -1;
  double u1, v1;
  double shortest_dist = std::numeric_limits< double >::infinity();

  for( unsigned int i = 0; i < faces.size(); ++i )
  {
    const mesh_regular_face< 3 >& f = faces[ i ];
    vital::point_3d a( verts[ f[ 0 ] ] );
    vital::point_3d b( verts[ f[ 1 ] ] );
    vital::point_3d c( verts[ f[ 2 ] ] );
    double dist = shortest_dist;
    if( mesh_triangle_closest_point( p, a, b, c, dist, u1, v1 ) &&
        dist < shortest_dist )
    {
      u = u1;
      v = v1;
      isect = i;
      shortest_dist = dist;
    }
  }

  // Get the closest point in physical space from barycentric coordinates
  double t = 1 - u - v;
  const mesh_regular_face< 3 >& f = faces[ isect ];
  vital::point_3d a( verts[ f[ 0 ] ] );
  vital::point_3d b( verts[ f[ 1 ] ] );
  vital::point_3d c( verts[ f[ 2 ] ] );
  cp.set_value( vector_3d( t * a[ 0 ] + u * b[ 0 ] + v * c[ 0 ],
                           t * a[ 1 ] + u * b[ 1 ] + v * c[ 1 ],
                           t * a[ 2 ] + u * b[ 2 ] + v * c[ 2 ] ) );
  return isect;
}

// ----------------------------------------------------------------------------
int
mesh_intersect(
  const point_3d& p, const vector_3d& d, const mesh& mesh, double& dist,
  double& u, double& v )
{
  // check for a triangular mesh
  if( mesh.faces().regularity() != 3 )
  {
    LOG_ERROR( vital::get_logger( "arrows.core.mesh_closest_point" ),
               "Closest point calculation requires triangular mesh." );
    return -1;
  }

  // Calculate normals if needed
  if( !mesh.faces().has_normals() )
  {
    LOG_ERROR( vital::get_logger( "arrows.core.mesh_closest_point" ),
               "Closest point calculation requires faces normals." );
    return -1;
  }

  const mesh_vertex_array< 3 >& verts = mesh.vertices< 3 >();
  const mesh_regular_face_array< 3 >& faces =
    static_cast< const mesh_regular_face_array< 3 >& >( mesh.faces() );

  // Check that normal magnitude corresponds to face area
  if( ( ( verts[ faces[ 0 ][ 1 ] ] - verts[ faces[ 0 ][ 0 ] ] )
        .cross( verts[ faces[ 0 ][ 2 ] ] - verts[ faces[ 0 ][ 0 ] ] ) -
        0.5 * faces.normal( 0 ) ).norm() < 1e-14 )
  {
    LOG_ERROR( vital::get_logger(
                 "arrows.core.mesh_closest_point" ),
               "Closest point calculation requires faces normal lengths be set to face area." );
    return -1;
  }

  int isect = -1;
  dist = std::numeric_limits< double >::infinity();
  for( unsigned int i = 0; i < faces.size(); ++i )
  {
    const mesh_regular_face< 3 >& f = faces[ i ];
    vital::point_3d a( verts[ f[ 0 ] ] );
    vital::point_3d b( verts[ f[ 1 ] ] );
    vital::point_3d c( verts[ f[ 2 ] ] );
    if( mesh_intersect_triangle_min_dist( p, d, a, b, c, faces.normal( i ),
                                          dist, u, v ) )
    {
      isect = i;
    }
  }
  return isect;
}

} // namespace core

} // namespace arrows

} // namespace kwiver


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * 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 "utility/openmp.hpp"

#include "geometry/meshop.hpp"
#include "geometry/mesh-voxelizer.hpp"
#include "math/geometry_core.hpp"
#include "imgproc/scanconversion.hpp"

//#include <opencv2/opencv.hpp>

#include <boost/lexical_cast.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include <algorithm>
//#define RASTERIZE_MESH_DEBUG

namespace geometry{

MeshVoxelizer::MeshVoxelizer(const Parameters &params){
    params_ = params;
}

void MeshVoxelizer::add( geometry::Mesh & mesh ){
    meshes.push_back(&mesh);
}

void MeshVoxelizer::voxelize(){
    if(meshes.size()<=0){
        LOG( warn2 )<<"Zero meshes to voxelize. Skipping voxelization.";
        return;
    }

    //add floor to mimic closed mesh
    std::vector<geometry::Mesh> seals;
    if(params_.addSeal){
        for(auto & mesh: meshes){
            seals.push_back(sealOfMesh(*mesh));
        }
    }

    //compute united extents
    math::Extents3 extents(math::InvalidExtents{});
    for(auto & mesh: meshes){
        math::Extents3 meshExtents = math::computeExtents(mesh->vertices);
        extents = math::unite(extents, meshExtents);
    }
    for(auto & mesh: seals){
        math::Extents3 meshExtents = math::computeExtents(mesh.vertices);
        extents = math::unite(extents, meshExtents);
    }

    LOG( info2 )<<"Meshes extents: "<<extents;

    // volume size

    {
        math::Point3 extentsSize = extents.ur-extents.ll;
        math::Point3i volumeGridRes(
                    std::ceil(extentsSize(0)/params_.voxelSize)
                    ,std::ceil(extentsSize(1)/params_.voxelSize)
                    ,std::ceil(extentsSize(2)/params_.voxelSize));
        math::Point3 newExtentsSize = volumeGridRes*params_.voxelSize;
        math::Point3 extentsCenter = math::center(extents);

        extents = math::Extents3( extentsCenter-newExtentsSize/2
                                 , extentsCenter+newExtentsSize/2);

        if ( valid(params_.overrideExtents) ) {
            LOG(info2) << "Using supplied extents for voxelization.";

            extents = params_.overrideExtents;

            for (uint i(0); i < 3; ++i) {
                volumeGridRes(i)
                    = std::ceil(size(extents)(i)/params_.voxelSize);
            }
        }

        LOG(info2) << "Volumetric grid resolution: " << volumeGridRes;
        LOG(info2) << "Volume extents: " << extents;
    }

    //generate directions for 3 main axes + axes of icosahedron faces
    std::vector<math::Point3> directions;
    directions.push_back(-math::Point3(1,0,0));
    directions.push_back(-math::Point3(0,1,0));
    directions.push_back(-math::Point3(0,0,1));

    directions.push_back(-math::Point3(1,1,1));
    directions.push_back(-math::Point3(1,-1, 1));
    directions.push_back(-math::Point3(-1,1,1));
    directions.push_back(-math::Point3(-1,-1,1));
    directions.push_back(-math::Point3(1,0,1));
    directions.push_back(-math::Point3(-1,0,1));
    directions.push_back(-math::Point3(0,1,1));
    directions.push_back(-math::Point3(0,-1,1));
    directions.push_back(-math::Point3(1,1,0));
    directions.push_back(-math::Point3(1,-1,0));

    //generate projection matrices and layered zbuffers

    Projections projections;
    ProjectionResults results;
    for(const auto & dir: directions){
        Projection projection = orthoProj( dir, extents, params_.voxelSize);
        projections.push_back(projection);
    }

    LOG( info2 )<<"Rasterizing meshes";
    //rasterize mesh using generated projection matrices
    for(uint p=0; p<projections.size(); ++p){
        LayeredZBuffer buffer(projections[p].viewportSize);
        for(auto & mesh: meshes){
            rasterizeMesh(*mesh, projections[p].transformation, buffer);
        }
        for(auto & mesh: seals){
            rasterizeMesh(mesh, projections[p].transformation, buffer);
        }
        buffer.sortCells();
        results.push_back(ProjectionResult( projections[p].transformation
                                      , CompressedLayeredZBuffer(buffer)));
    }

#ifdef RASTERIZE_MESH_DEBUG
    uint i=0;
    for( auto &proj : results){
        fs::path path = fs::path(std::string("depth")
                                 +boost::lexical_cast<std::string>(i++)
                                 +std::string(".png"));
        visualizeDepthMap(proj, extents ,path);
    }
#endif

    //create new volume
    /**************
     * Ugly hack (shaveVolume)
     *
     * Voxelization has problems with edges - create volume 2*cells
     * smaller from both x and y directions
     *
     * Voxelization itself should be fixed instead
     **************************/
    {
        math::Size3i volSize( std::ceil( size(extents)(0)/params_.voxelSize )
                            , std::ceil( size(extents)(1)/params_.voxelSize )
                            , std::ceil( size(extents)(2)/params_.voxelSize ));

        math::Point3 ll( extents.ll(0)
                       , extents.ll(1)
                       , extents.ll(2));

        if (params_.shaveVolume) {
            for (uint i(0); i < 2; ++i) {
                // strip 2 from each side
                volSize(i) -= 4;
                // shift extents's ll by 2 voxel size because that is what we stripped
                ll(i) += 2*params_.voxelSize;
            }
        }

        // prepare volume for filtering and downsampling: reserve space so all
        // dimensions can be inflated to closest larger odd value
        // add few layers of cells on the ceiling of the volume as a margin
        // for subsampling.
        math::Size3i capacity( volSize(0) + !(volSize(0) % 2)
                             , volSize(1) + !(volSize(1) % 2)
                             , volSize(2) + !(volSize(2) % 2) + 4);

        LOG(info2) << "Creating volume of size " << volSize
                   << " and capacity " << capacity;

        volume_ = std::unique_ptr<Volume>( new Volume( ll
                                                     , params_.voxelSize
                                                     , volSize
                                                     , VoxelizerUnit::empty()
                                                     , capacity));
    }

    math::Size3i vSize = volume_->cSize();
    long long volMem = (long long)vSize.width * vSize.height
                    * vSize.depth * sizeof(unsigned short);
    LOG( info2 )<<"Memory consumption of volume: "
        << volMem/1024.0/1024.0/1024.0 << " GB.";

    long long mem = 0;
    for(auto res : results){
        mem += res.buffer.mem();
    }

    LOG( info2 )<<"Memory consumption of zbuffers: "
        << mem/1024.0/1024.0/1024.0 << " GB.";

    uint progress = 0;
    LOG( info2 )<<"Voxelization progress: "<<progress;
#ifdef _OPENMP
    #pragma omp parallel for schedule( dynamic, 10 )
#endif
    for(int x=0; x< vSize.width; ++x){
        for(int y=0; y< vSize.height; ++y){
            for(int z=0; z< vSize.depth; ++z){
                //voxel center position
                auto voxCenter
                        = volume_->grid2geo(math::Point3(x,y,z));
                if(isInside({voxCenter.x, voxCenter.y, voxCenter.z}, results)){
                        volume_->set( x, y, z,VoxelizerUnit::full());
                }

            }
        }
        //voxelization progress
        uint newProgress = std::ceil((float)x/vSize.width * 100);
        if(newProgress>progress){
            progress=newProgress;
            LOG( info2 )<<"Voxelization progress: "<<progress;
        }
    }

    if(params_.addSeal){
        LOG( info2 )<<"Filling volume from seal";
        fillVolumeFromSeal();
    }
}

std::shared_ptr<MeshVoxelizer::Volume> MeshVoxelizer::volume(){
    return volume_;
}

void MeshVoxelizer::reset(){
    std::vector<geometry::Mesh*>().swap(meshes);
}

MeshVoxelizer::Projection MeshVoxelizer::orthoProj(const math::Point3 &direction
                            , const math::Extents3 &extents
                            , const float &voxelSize
                            ){
    LOG( debug )<<"Projection direction: "<<direction;
    LOG( debug )<<"Mesh extents: "<<extents;

    math::Point3 normalizedDir = math::normalize(direction);

    math::Matrix4 projMat = ublas::identity_matrix<double>(4);
    //translate so the center of the extents is in the middle
    math::Point3 extentsCenter = math::center(extents);
    projMat(0,3) = -extentsCenter[0];
    projMat(1,3) = -extentsCenter[1];
    projMat(2,3) = -extentsCenter[2];

    //LOG( debug )<<"Projection matrix - after centering: "<<projMat;

    //rotate scene in specified direction
    math::Matrix4 rotation = ublas::identity_matrix<double>(4);

    math::Point3 up(0,0,-1);
    if(std::abs(ublas::inner_prod(normalizedDir,math::Point3(0,0,1)))==1){
        up = math::Point3(0,1,0);
    }
    math::Point3 right = math::normalize(math::crossProduct(up,direction));
    up = math::normalize(math::crossProduct(normalizedDir,right));

    auto c1 = ublas::row(rotation,0);
    auto c2 = ublas::row(rotation,1);
    auto c3 = ublas::row(rotation,2);

    ublas::subrange(c1,0,3) = right;
    ublas::subrange(c2,0,3) = up;
    ublas::subrange(c3,0,3) = normalizedDir;

    projMat = prod(rotation,projMat);

    //find out extents images
    math::Extents2 viewExtents(math::InvalidExtents{});
    math::Points3 extPoints;
    extPoints.push_back(math::Point3(extents.ll[0],extents.ll[1],extents.ll[2]));
    extPoints.push_back(math::Point3(extents.ll[0],extents.ll[1],extents.ur[2]));
    extPoints.push_back(math::Point3(extents.ll[0],extents.ur[1],extents.ll[2]));
    extPoints.push_back(math::Point3(extents.ll[0],extents.ur[1],extents.ur[2]));
    extPoints.push_back(math::Point3(extents.ur[0],extents.ll[1],extents.ll[2]));
    extPoints.push_back(math::Point3(extents.ur[0],extents.ll[1],extents.ur[2]));
    extPoints.push_back(math::Point3(extents.ur[0],extents.ur[1],extents.ll[2]));
    extPoints.push_back(math::Point3(extents.ur[0],extents.ur[1],extents.ur[2]));

    for(const auto &point : extPoints){
        math::Point3 pointImg = transform(projMat,point);
        viewExtents.ll[0] = std::min(pointImg[0],viewExtents.ll[0]);
        viewExtents.ll[1] = std::min(pointImg[1],viewExtents.ll[1]);
        viewExtents.ur[0] = std::max(pointImg[0],viewExtents.ur[0]);
        viewExtents.ur[1] = std::max(pointImg[1],viewExtents.ur[1]);
    }

    //scale scene so the extents fit to space < -1,1 > CC/NDC
    math::Matrix4 scaleMat = ublas::identity_matrix<double>(4);
    scaleMat(0,0) = 2./size(viewExtents).width;
    scaleMat(1,1) = 2./size(viewExtents).height;

    projMat = prod(scaleMat, projMat);

    //scale and transform scene to VC
    math::Matrix4 transformMat = ublas::identity_matrix<double>(4);
    transformMat(0,3)=1;
    transformMat(1,3)=1;

    //calculate viewport size
    LOG( debug ) << "View extents: "<< viewExtents;

    math::Size2 viewport( std::ceil(size(viewExtents).width/voxelSize)
                        , std::ceil(size(viewExtents).height/voxelSize));

    LOG( debug ) << "Viewport:" << viewport;

    scaleMat = ublas::identity_matrix<double>(4);
    scaleMat(0,0) = viewport.width/2;
    scaleMat(1,1) = viewport.height/2;

    projMat = prod(transformMat, projMat);
    projMat = prod(scaleMat, projMat);

    LOG( debug )<<"Projection matrix - final from WC to CC: "<<projMat;

    for(const auto &point : extPoints){
        math::Point3 pointImg = transform(projMat,point);
        LOG( debug ) << point <<" - "<< pointImg;
    }

    return Projection(projMat, viewport);
}

geometry::Mesh MeshVoxelizer::sealOfMesh(geometry::Mesh & mesh){

    math::Extents3 extents = math::computeExtents(mesh.vertices);

    float offset = params_.voxelSize * params_.sealFactor;

    geometry::Mesh seal;
    math::Point3 extSize = extents.ur-extents.ll;

    double cellWidth = offset;

    int cols=std::ceil(extSize(0)/cellWidth);
    int rows=std::ceil(extSize(1)/cellWidth);

    std::vector<std::vector<double>> minMap
        = std::vector<std::vector<double>>(cols);
    for(int x=0; x<cols; ++x){
        minMap[x] = std::vector<double>(rows,INFINITY);
    }

    for(const auto& vertex: mesh.vertices){
        int x = std::min((int)std::floor((vertex(0)-extents.ll(0))/cellWidth),cols-1);
        int y = std::min((int)std::floor((vertex(1)-extents.ll(1))/cellWidth),rows-1);
        minMap[x][y]=std::min(minMap[x][y],vertex(2));
    }

    for(int x=0; x< cols; ++x){
        for(int y=0; y< rows; ++y){
            if(x==0 || y==0 || x==cols-1 || y==rows-1){
                //fill unsetted values
                int searchOffset=1;
                while(!std::isfinite(minMap[x][y])){
                    for(int xoff=-searchOffset; xoff<searchOffset; ++xoff ){
                        for(int yoff=-searchOffset; yoff<searchOffset; ++yoff ){
                            if( x+xoff>0 && x+xoff<(int)cols
                               && y+yoff>0 && y+yoff<(int)rows){
                                minMap[x][y]=std::min( minMap[x][y]
                                                     , minMap[x+xoff][y+yoff]);
                            }
                        }
                    }
                    searchOffset++;
                    if(searchOffset>params_.sealFactor*2){
                        minMap[x][y] = extents.ll(2);
                    }
                }
            }
        }
    }

    for(int x=1; x< cols-1; ++x){
        for(int y=1; y< rows-1; ++y){
            minMap[x][y] = extents.ll(2);
        }
    }

    for(int y=0; y< rows; ++y){
        for(int x=0; x< cols; ++x){
            seal.vertices.push_back(
                math::Point3( x*cellWidth+extents.ll(0)
                            , y*cellWidth+extents.ll(1)
                            , minMap[x][y]-offset ));
        }
    }

    for(int x=0; x< cols-1; ++x){
        for(int y=0; y< rows-1; ++y){
            seal.addFace(x+(y*cols), x+1+((y+1)*cols), x+1+(y*cols));
            seal.addFace(x+(y*cols), x+((y+1)*cols), x+1+((y+1)*cols));
        }
    }

    return seal;
}

void MeshVoxelizer::fillVolumeFromSeal(){
    math::Size3i vSize = volume_->cSize();
    for(int x = 0;x<vSize.width; ++x){
        for(int y = 0;y<vSize.height; ++y){
            //find first full voxel from the direction of seal
            int full = -1;
            for(int z = 0;z<vSize.depth; ++z){
                if(volume_->get(x,y,z)!=VoxelizerUnit::empty()){
                        full = z;
                        break;
                }
            }
            //fill from bottom up
            for(int z = 0;z<full; ++z){
                volume_->set(x,y,z,VoxelizerUnit::full());
            }
        }
    }
}

void MeshVoxelizer::rasterizeMesh( const Mesh &mesh
                                 , const math::Matrix4 &projMat
                                 , LayeredZBuffer & lZBuffer){
    std::vector<imgproc::Scanline> scanlines;

    LOG(info2) << "rasterizing " << mesh.faces.size() << " triangles";

    // draw all faces into the zBuffer
    for (const auto &face : mesh.faces)
    {
        cv::Point3f tri[3];

        int i(0);
        for (auto it(mesh.begin(face)); it != mesh.end(face); ++it, ++i) {
            math::Point3d pt(transform(projMat, *it));
            tri[i] = {float(pt(0)), float(pt(1)), float(pt(2))};
        }

        scanlines.clear();
        imgproc::scanConvertTriangle(tri, 0, lZBuffer.size.height, scanlines);

        for (const auto& sl : scanlines)
        {
            imgproc::processScanline(sl, 0, lZBuffer.size.width,
                [&](int x, int y, float z) {
                    lZBuffer.data[x][y].push_back(z);
                } );
        }
    }

}

bool MeshVoxelizer::isInside( const math::Point3 & position
                      ,  ProjectionResults & projectionResults){
    uint inside=0;
    uint outside=0;

    uint projId = 0;
    for(auto & proj : projectionResults){
        math::Point3 projPos = math::transform(proj.transformation, position);
        projId++;
        if(params_.method==Method::PARITY_COUNT){
            uint parity=0;
            if(projPos[0]<proj.buffer.size.width
                    && projPos[1]<proj.buffer.size.height
                    && projPos[0]>0 && projPos[1]>0){
                //std::cout<<proj.buffer.data[projPos[0]][projPos[1]].size()<<std::endl;
                for(auto dit = proj.buffer.begin(projPos[0],projPos[1]);
                        dit != proj.buffer.end(projPos[0],projPos[1]); ++dit){

                    if(*dit<projPos[2]){
                        parity++;
                    }
                    else{
                        break;
                    }
                }
                if(parity%2==1){
                    inside++;
                    continue;
                }
                outside++;
            }
        }

        if(params_.method==Method::RAY_STABING){
            if( projPos[2]<*proj.buffer.begin(projPos[0],projPos[1])
               && projPos[2]>*proj.buffer.end(projPos[0],projPos[1])){
                inside++;
                continue;
            }
            outside++;
        }
    }

    if(params_.method==Method::PARITY_COUNT){
        if(inside>outside)
            return true;
    }
    if(params_.method==Method::RAY_STABING){
        if(outside==0)
            return true;
    }
    return false;
}

void MeshVoxelizer::visualizeDepthMap( const ProjectionResult &proj
                                      , const math::Extents3 & extents
                                      , const fs::path & path){
    cv::Mat depthMapImg( proj.buffer.size.height, proj.buffer.size.width
                     , CV_8UC3, cv::Scalar(0,0,0));

    float min = FLT_MAX;
    float max = -FLT_MAX;

    for(int x = 0; x < proj.buffer.size.width; ++x){
        for(int y = 0; y < proj.buffer.size.height; ++y){
            if(proj.buffer.begin(x,y) == proj.buffer.end(x,y)){
                continue;
            }
            min = std::min(*proj.buffer.begin(x,y),min);
            max = std::max(*std::prev(proj.buffer.end(x,y)),max);
        }
    }

    float range = max-min;

    LOG( debug )<<"Minimal/maximal depth: "<< min <<" / "<< max
                << " Range: "<<range;

    for(int x = 0; x < proj.buffer.size.width; ++x){
        for(int y = 0; y < proj.buffer.size.height; ++y){
            if(proj.buffer.begin(x,y) == proj.buffer.end(x,y)){
                depthMapImg.at<cv::Vec3b>(y,x)=
                        cv::Vec3b(0,0,255);
                continue;
            }
            float val  = (*proj.buffer.begin(x,y) - min) / range;
            depthMapImg.at<cv::Vec3b>(y,x)=
                    cv::Vec3b(255-val*255,255-val*255,255-val*255);
        }
    }

    math::Points3 extPoints;
    extPoints.push_back(math::Point3(extents.ll[0],extents.ll[1],extents.ll[2]));
    extPoints.push_back(math::Point3(extents.ll[0],extents.ll[1],extents.ur[2]));
    extPoints.push_back(math::Point3(extents.ll[0],extents.ur[1],extents.ll[2]));
    extPoints.push_back(math::Point3(extents.ll[0],extents.ur[1],extents.ur[2]));
    extPoints.push_back(math::Point3(extents.ur[0],extents.ll[1],extents.ll[2]));
    extPoints.push_back(math::Point3(extents.ur[0],extents.ll[1],extents.ur[2]));
    extPoints.push_back(math::Point3(extents.ur[0],extents.ur[1],extents.ll[2]));
    extPoints.push_back(math::Point3(extents.ur[0],extents.ur[1],extents.ur[2]));

    for(auto &point : extPoints){
        point = transform(proj.transformation,point);
    }

    cv::line(depthMapImg, cv::Point2f(extPoints[0][0],extPoints[0][1])
            , cv::Point2f(extPoints[1][0],extPoints[1][1]),cv::Scalar(255,0,0),2);
    cv::line(depthMapImg, cv::Point2f(extPoints[2][0],extPoints[2][1])
            , cv::Point2f(extPoints[3][0],extPoints[3][1]),cv::Scalar(255,0,0),2);

    cv::line(depthMapImg, cv::Point2f(extPoints[0][0],extPoints[0][1])
            , cv::Point2f(extPoints[2][0],extPoints[2][1]),cv::Scalar(255,0,0),2);
    cv::line(depthMapImg, cv::Point2f(extPoints[1][0],extPoints[1][1])
            , cv::Point2f(extPoints[3][0],extPoints[3][1]),cv::Scalar(255,0,0),2);

    cv::line(depthMapImg, cv::Point2f(extPoints[4][0],extPoints[4][1])
            , cv::Point2f(extPoints[5][0],extPoints[5][1]),cv::Scalar(255,0,0),2);
    cv::line(depthMapImg, cv::Point2f(extPoints[6][0],extPoints[6][1])
            , cv::Point2f(extPoints[7][0],extPoints[7][1]),cv::Scalar(255,0,0),2);

    cv::line(depthMapImg, cv::Point2f(extPoints[4][0],extPoints[4][1])
            , cv::Point2f(extPoints[6][0],extPoints[6][1]),cv::Scalar(255,0,0),2);
    cv::line(depthMapImg, cv::Point2f(extPoints[5][0],extPoints[5][1])
            , cv::Point2f(extPoints[7][0],extPoints[7][1]),cv::Scalar(255,0,0),2);

    cv::line(depthMapImg, cv::Point2f(extPoints[1][0],extPoints[1][1])
            , cv::Point2f(extPoints[5][0],extPoints[5][1]),cv::Scalar(255,0,0),2);
    cv::line(depthMapImg, cv::Point2f(extPoints[0][0],extPoints[0][1])
            , cv::Point2f(extPoints[4][0],extPoints[4][1]),cv::Scalar(255,0,0),2);

    cv::line(depthMapImg, cv::Point2f(extPoints[3][0],extPoints[3][1])
            , cv::Point2f(extPoints[7][0],extPoints[7][1]),cv::Scalar(255,0,0),2);
    cv::line(depthMapImg, cv::Point2f(extPoints[2][0],extPoints[2][1])
            , cv::Point2f(extPoints[6][0],extPoints[6][1]),cv::Scalar(255,0,0),2);

    cv::imwrite( path.string().c_str(), depthMapImg );
}

} //namespace geometry


#include <stdlib.h> /* srand, rand */
#include <iostream>
#include <fstream> // creating (.csv) files
#include <vector>
#include <cstring>
#include <string>
#include <functional>
#include <armadillo> // http://arma.sourceforge.net/docs.html
#include <thread>    /* std::this_thread::sleep_for */

#include "EvolutionaryAlgorithm.hpp"

#include "mypch.hpp"

#include "EAController.hpp"

// NOT USED??
// enum events{
//     BASE_RATE = 0,
//     MUTA_RATE,
//     PRED_RATE,
//     PART_RATE,
//     GENO_RATE,
//     EV_A_RATE,
// };

namespace EvoAlg
{
    //arma::rowvec semiFinalsAndFinals[SEMI_FINALS][TOURNMENT_RATE];
    //arma::rowvec indvFinals[TOURNMENT_RATE];
    void EvolutionaryAlgorithm::saveBestIndvParamsCSV()
    {
        std::fstream myFile;
        myFile.open("constValues.cfg", std::ios_base::app);

        myFile << attributesValues[0].name << " = " << population(bestFitIndex, 0);
        for (size_t i = 1; i < NB_PARAMETERS; i++)
            myFile << std::endl
                   << attributesValues[i].name << " = " << population(bestFitIndex, i);
        //printf("k%sk\n", parametersList[i]);
        myFile.close();
    }

    // 1st step: initialize population
    void EvolutionaryAlgorithm::initializePop(int tournmentType)
    {
        double minVal, maxVal;
        std::cout << "INITIALIZING POPULATION\n";
        // for (int i = 0; i < POPULATION_SIZE; i++) {
        //     for (int j = 0; j < NB_PARAMETERS; j++) {
        //         population[i][j] = (double) (rand() % MAX_PARAM_VALUE); // number range = [0, MAX_PARAM_VALUE[
        //     }
        // }
        population.randu(); // initialize with values between 0 and 1

        //TODO: find a better way to pass a non-variable to lambda
        // auto &_attributesValues = attributesValues;
        if (tournmentType == INITIALS) {
            int index = 0;
            population.each_col([=, &index](arma::vec& popCol) { popCol = attributesValues[index].min + popCol * (attributesValues[index].max - attributesValues[index].min); index++; } );
            // for(int j = 0; j < NB_PARAMETERS; j++)
            //     population.col(j) = attributesValues[j].min + population.col(j)*(attributesValues[j].max - attributesValues[j].min); // if a normal EA is taking place, just multiply the population by a givern value
        }
        else
        {
            int middle = TOURNMENT_RATE + (POPULATION_SIZE - TOURNMENT_RATE) / 2;
            fillInitialsWithBests(tournmentType);
            for (int j = 0; j < NB_PARAMETERS; j++)
            { // Min and max values are defined for each col, and converting the current col values to a number between this constraints.
                // this starts after the first TOURNMENT_RATE individuls, and the remaining is divided into two pars, one with biased rand values, and other without
                minVal = MIN_MULT * bestTournmentIndv[tournmentType].col(j).min();
                maxVal = MAX_MULT * bestTournmentIndv[tournmentType].col(j).max();

                for (int i = TOURNMENT_RATE; i < middle; i++)
                {
                    population(i, j) = minVal + (maxVal - minVal) * (population(i, j));
                }
            }
            for (int i = middle; i < POPULATION_SIZE; i++)
                for (int j = 0; j< NB_PARAMETERS; i++)
                    population(i,j) = attributesValues[j].min + population(i,j)*(attributesValues[j].max - attributesValues[j].min);
        }

        population.print("Population matrix initialized:");

        return;
    }

    // Function to normalize fitness (values between 0 and 1)
    void EvolutionaryAlgorithm::normalizeFitness(double *normalizedFitness)
    {
        if (normalizedFitness == NULL)
            return;

        for (int i = 0; i < POPULATION_SIZE; i++)
            normalizedFitness[i] = (fitness[i] - bestFitness) / (worstFitness - bestFitness);

        return;
    }

    // 2nd step: evaluate population (calculate fitness)
    void EvolutionaryAlgorithm::evaluatePop()
    {
        //Limit the values in each col
        // for(int j = 0; j < NB_PARAMETERS; j++){
        //     population.col(j) = arma::clamp(population.col(j),attributesValues[j].min,attributesValues[j].max);
        // }
        int index = 0;

        //TODO: find a better way to pass a non-variable to lambda
        auto &_attributesValues = attributesValues;
        population.each_col([&_attributesValues, &index](arma::vec& popCol) { popCol = arma::clamp(popCol, _attributesValues[index].min, _attributesValues[index].max); index++; } );

        std::cout << "EVALUATING POPULATION\n";

        nbNoImprovementGens++; // we begin considering there was no improvement in the generation

        std::vector<std::vector<double>> populationVec2(population.n_rows);
        for (size_t i = 0; i < population.n_rows; ++i)
            populationVec2[i] = (arma::conv_to<std::vector<double>>::from(population.row(i)));

        DE_TRACE("(EvolutionaryAlgorithm) Sending population to EAController to execute it");
        auto gameplayResults = m_EAController.RunPopulationInGame(populationVec2);
        DE_TRACE("(EvolutionaryAlgorithm) Population received from EAController");

        DE_TRACE("Calculating fitness of population after gameplay results received");
        calcFitness(gameplayResults);

        for (int i = 0; i < POPULATION_SIZE; i++)
        {
            DE_ASSERT(fitness[i] >= 0);
            if (fitness[i] < bestFitness)
            { // searching for the  max fitness from new generation
                bestFitness = fitness[i];
                bestFitIndex = i;
                nbNoImprovementGens = 0; // this generation shows improvement -> reset counter
            }
            else if (fitness[i] > worstFitness)
            {
                worstFitness = fitness[i];
                worstFitIndex = i;
            }

            // printf("fitness[%d] %lf\n", i, fitness[i]);
        }

        printf("BEST FITNESS: %lf - INDEX: %d\n", bestFitness, bestFitIndex);
        printf("WORST FITNESS: %lf - INDEX: %d\n", worstFitness, worstFitIndex);

        return;
    }

    void EvolutionaryAlgorithm::crossover(int indv, arma::rowvec parent1, arma::rowvec parent2)
    {
        // for(int j = 0; j < NB_PARAMETERS; j++){
        //     population[i][j] = (parent1[j] + parent2[j])/2.0;
        // }
        population.row(indv) = (parent1 + parent2) / 2.0;

        return;
    }

    void EvolutionaryAlgorithm::elitism()
    {
        std::cout << "ELITISM\n";
        arma::rowvec bestIndv = population.row(bestFitIndex);

        for (int i = 0; i < POPULATION_SIZE; i++)
        {
            // crossover
            crossover(i, population.row(i), bestIndv);
        }

        return;
    }

    void EvolutionaryAlgorithm::tournament()
    {
        std::cout << "TOURNAMENT\n";
        arma::mat::fixed<POPULATION_SIZE, NB_PARAMETERS> oldPopulation;
        int parentIndex[2];

        // copying last population (new one will be different)
        // for (int i = 0; i < POPULATION_SIZE; i++) {
        //     for (int j = 0; j < NB_PARAMETERS; j++) {
        //         oldPopulation[i][j] = population[i][j];
        //     }
        // }
        oldPopulation = population;

        for (int i = 0; i < POPULATION_SIZE; i++)
        {
            if (i == bestFitIndex)
                continue;

            // chossing parents for new individual
            for (int j = 0; j < 2; j++)
            {
                int indexIndA = rand() % POPULATION_SIZE; // indv 1 that will "fight" to be parent
                int indexIndB = rand() % POPULATION_SIZE; // indv 2 that will "fight" to be parent

                parentIndex[j] = (fitness[indexIndA] < fitness[indexIndB] ? indexIndA : indexIndB);
            }

            // crossover
            crossover(i, oldPopulation.row(parentIndex[0]), oldPopulation.row(parentIndex[1]));
        }

        return;
    }

    //TODO: swap priorities. Smaller fitness must be morre relevant
    void EvolutionaryAlgorithm::roulette()
    {
        std::cout << "ROULETTE\n";
        arma::mat::fixed<POPULATION_SIZE, NB_PARAMETERS> oldPopulation;
        double standardizedFitness[POPULATION_SIZE];
        int parentIndex[2], rNb;
        double probSum = 0.0, partialSum = 0.0;

        (void) standardizedFitness[0];

        // copying last population (new one will be different)
        // for (int i = 0; i < POPULATION_SIZE; i++) {
        //     for (int j = 0; j < NB_PARAMETERS; j++) {
        //         oldPopulation[i][j] = population[i][j];
        //     }
        // }
        oldPopulation = population;

        // Standardize fitness (set probabilites that add up to 100%)
        for (int i = 0; i < POPULATION_SIZE; i++)
            probSum += fitness[i];
        for (int i = 0; i < POPULATION_SIZE; i++)
            standardizedFitness[i] = fitness[i] / probSum;

        // Chosing new parents for each individual
        for (int i = 0; i < POPULATION_SIZE; i++)
        {
            if (i == bestFitIndex) // preserves best individual
                continue;

            for (int k = 0; k < 2; k++)
            {                                      // chosing 2 parents
                rNb = ((double)rand() / RAND_MAX); // rand between 0 and 1
                partialSum = 0.0;

                for (int j = 0; j < POPULATION_SIZE; j++)
                { // randomly chosing and individual according to its fitness (+fitness = +probabity)
                    partialSum += fitness[j];
                    if (partialSum >= rNb)
                    {
                        parentIndex[k] = j; // new parent at index j
                        break;
                    }
                }
            }

            // crossover
            crossover(i, oldPopulation.row(parentIndex[0]), oldPopulation.row(parentIndex[1]));
        }
    }

    void EvolutionaryAlgorithm::mutate(int indIndex)
    {
        int plusMinusSign = (rand() % 2 ? MUTATE_POSITIVE_MULT : MUTATE_NEGATIVE_MULT); // mutation will increase the param value or decrease it
        int mutateParamIndex = rand() % NB_PARAMETERS;                                  // index of parameter that will be mutated

        population(indIndex, mutateParamIndex) += population(indIndex, mutateParamIndex) * mutationRate * plusMinusSign;

        return;
    }

    // 3rd step: selection + mutation and crossover
    void EvolutionaryAlgorithm::selectionAndMutation()
    { // tournament, elitism, roulette...
        std::cout << "SELECTION\n";

        // Selection method (+ crossover)
        (this->*(selectionType[selectionMethod]))(); // void (*selectionType[])() => tournament(), elitism() or roulette()

        // Mutation

        // Mutating all params from individuals
        // arma::rowvec bestIndv = population.row(maxFitIndex);

        // arma::mat mutateMatrix(POPULATION_SIZE, NB_PARAMETERS, arma::fill::randu);
        // mutateMatrix = ((mutateMatrix * MAX_PARAM_VALUE) - MAX_PARAM_VALUE/2.0) * mutationRate;

        // population = population + mutateMatrix;

        // population.row(maxFitIndex) = bestIndv;
        // population.transform( [](double x) { return ((x < 0 || x > MAX_PARAM_VALUE) ? abs(MAX_PARAM_VALUE-abs(x)) : x); } );

        // Mutating only one parameter from each individual
        for (int i = 0; i < bestFitIndex; i++)
            mutate(i);
        // (don't mutate best one)
        for (int i = bestFitIndex + 1; i < POPULATION_SIZE; i++)
            mutate(i);

        return;
    }

    // Initialize .csv file to sabe data from the EA
    // Creates the column's headers
    // generation, paramsIndv1, fitnessIndv1 ..., paramsIndvN, fitnessIndvN, bestParams, bestFitness
    void EvolutionaryAlgorithm::createCSV(std::string pathPos)
    {
        std::ofstream csvFileWriter;

        csvFileWriter.open("historyEA-" + pathPos + ".csv");
        if (!csvFileWriter.good())
        {
            std::cout << "[!] Error occurred while trying to create historyEA" << pathPos << ".csv!\n";
            return;
        }

        csvFileWriter << "generation,";
        for (int i = 0; i < POPULATION_SIZE; i++)
            csvFileWriter << "paramsIndv" << i << ",fitnessIndv" << i << ",";
        csvFileWriter << "paramsBestIndv,fitnessBestIndv\n";

        return;
    }

    std::string EvolutionaryAlgorithm::formatParamsString(int indvIndex)
    {
        std::string paramsFormated = "[ ";
        for (int i = 0; i < NB_PARAMETERS; i++)
        {
            paramsFormated += std::to_string(population(indvIndex, i));
            paramsFormated += " ";
        }
        paramsFormated += "]";

        return paramsFormated;
    }

    // Saves information about a generation in a .csv file
    // Information: generation, paramsIndv1, fitnessIndv1 ..., paramsIndvN, fitnessIndvN, bestParams, bestFitness
    void EvolutionaryAlgorithm::saveGenerationData(int generation, const std::string& pathPos)
    {
        std::ofstream csvFileWriter;

        csvFileWriter.open("historyEA-" + pathPos + ".csv", std::ios_base::app); // append instead of overwrite
        if (!csvFileWriter.good())
        {
            std::cout << "[!] Error occurred while trying to open historyEA.csv!\n";
            return;
        }

        csvFileWriter << generation << ",";
        for (int i = 0; i < POPULATION_SIZE; i++)
        {
            csvFileWriter << formatParamsString(i) << "," << fitness[i] << ",";
        }
        csvFileWriter << formatParamsString(bestFitIndex) << "," << bestFitness << "\n";
        csvFileWriter.close();
    }

    void EvolutionaryAlgorithm::increaseMutation()
    {
        if (mutationRate < 1e-3 && mutationRate > -1e-3)
        {
            mutationRate = INITIAL_MUTATION;
        }
        else
        {
            mutationRate *= MUTATION_INCREASE_RATIO;

            if (mutationRate > MAX_MUTATION_RATE) // mutation reached its max value
                mutationRate = MAX_MUTATION_RATE;
        }

        return;
    }

    // Function that will kill the worst individual every "APPLY_PREDATION_INTERVAL" number of generations
    void EvolutionaryAlgorithm::predationOfOne()
    {
        arma::rowvec newInd(NB_PARAMETERS, arma::fill::randu); // creating totally new individual
        // for(int j = 0; j < NB_PARAMETERS; j++){
        //     newInd(j) = attributesValues[j].min + newInd(j)*(attributesValues[j].max - attributesValues[j].min);
        // }
        int index = 0;
        //TODO: find a better way to pass a non-variable to lambda
        auto &_attributesValues = attributesValues;
        newInd.for_each([&_attributesValues, &index](double &paramNewInd) { paramNewInd = _attributesValues[index].min + paramNewInd * (_attributesValues[index].max - _attributesValues[index].min); index++; } );

        population.row(worstFitIndex) = newInd;

        return;
    }

    // A sequence of selections will run, this method will indicate the selection method that will be used by the next batch of generations
    void EvolutionaryAlgorithm::partIncrease()
    {
        const static int selectionMethodCount = sizeof(partsSelectionMethods)/sizeof(selectionMethods);
        partPos++;
        if (partPos > selectionMethodCount)
            partPos = selectionMethodCount;
        selectionMethod = partsSelectionMethods[partPos];

        return;
    }

    // Function that will kill all individuals but the best to reset the population and generate new individuals (without biases)
    void EvolutionaryAlgorithm::oneRemainingPopReset()
    {
        std::cout << "APPLYING POPULATION RESET\n";

        arma::rowvec best = population.row(bestFitIndex);

        initializePop(INITIALS);
        population.row(0) = best;
        partIncrease();

        return;
    }

    // Ends the batch of generations from current EA
    void EvolutionaryAlgorithm::endEABatch()
    {
        std::cout << "END EA BATCH";

        continueBatch = false;

        return;
    }

    // Generic function that checks if certain event should happen (predation, population reset, mutation increase...)
    bool EvolutionaryAlgorithm::eventHappens(int eventType)
    {
        if (nbNoImprovementGens % eventTriggerModule[eventType] == 0)
            return true; // the event being verified should happen!
        return false;
    }

    void EvolutionaryAlgorithm::checkEvents()
    {
        if (nbNoImprovementGens == 0)
            return;

        for (int i = 4; i >= 0; i--)
        {
            if (eventHappens(i + 1))
            {
                (this->*(eventTypes[i]))(); // calls one of these functions: increaseMutation, predationOfOne, partIncrease, oneRemainingPopReset, fullPopReset
                return;
            }
        }
    }

    void EvolutionaryAlgorithm::startEventTriggerList()
    {
        for (int i = 1; i < 6; i++)
            eventTriggerModule[i] *= eventTriggerModule[i - 1];
    }

    void EvolutionaryAlgorithm::evoAlg(int createPopType, const std::string& csvStr)
    {
        initializePop(createPopType);
        nbNoImprovementGens = 0;
        createCSV(csvStr);
        continueBatch = true;
        partPos = 0; // defines selection method of current batch of generations

        generationIndex = 0;
        while (continueBatch)
        {
            std::cout << "\n==== Generation " << generationIndex << " ====\n";
            // population.print("Current population:");
            evaluatePop();
            selectionAndMutation();

            saveGenerationData(generationIndex, csvStr);

            checkEvents(); // checks if mutation should increase, predation or population reset should occur etc.
            // if fullPopReset() is called, continueEA = false

            generationIndex++;
        }

        return;
    }

    void EvolutionaryAlgorithm::fillInitialsWithBests(int tournmentType)
    {
        for (int i = 0; i < TOURNMENT_RATE; i++)
            population.row(i) = bestTournmentIndv[tournmentType].row(i);
    }

    void EvolutionaryAlgorithm::semiFinalsTournment(int semiFinalPos)
    {
        for (int i = 0; i < TOURNMENT_RATE; i++)
        {
            evoAlg(INITIALS,"SF-" + std::to_string(semiFinalPos) + "_EA-" + std::to_string(i));
            bestTournmentIndv[SEMI_FINALS].row(i) = population.row(bestFitIndex); // saves the best individual from current EA
        }
        evoAlg(SEMI_FINALS,"SF-" + std::to_string(semiFinalPos));                               // this EA will use the best individuals from each previous EA
        bestTournmentIndv[FINALS].row(semiFinalPos) = population.row(bestFitIndex); // saves the best individual from current EA
        return;
    }

    void EvolutionaryAlgorithm::finalTournment()
    {
        for (int i = 0; i < TOURNMENT_RATE; i++)
            semiFinalsTournment(i);

        evoAlg(FINALS,"FINAL"); // this EA will use the best individuals from each semifinal

        // bestIndividual = population.row(bestFitIndex); not used, but I'm too afraid to delete
        std::cout << "EVOLUTIONARY ALGORITHM FINISHED!" << std::endl;
        //eaFinished = true;
        return;
    }

    EvolutionaryAlgorithm::EvolutionaryAlgorithm(EAController &eaController) : m_EAController(eaController), eaFinished(false)
    {
        srand(time(NULL));
        startEventTriggerList();
    }

    static void *runScript(void *scriptFuncObject)
    {
        EvolutionaryAlgorithm *script = (EvolutionaryAlgorithm *)scriptFuncObject;
        script->finalTournment();
        return NULL;
    }

    void EvolutionaryAlgorithm::startAlgorithm()
    {
        DE_ASSERT(scriptThread == nullptr);
        scriptThread = new std::thread(runScript, this);
    }

    EvolutionaryAlgorithm::~EvolutionaryAlgorithm()
    {
        if (scriptThread != nullptr && scriptThread->joinable())
            scriptThread->join();
        delete scriptThread;
    }

    void EvolutionaryAlgorithm::calcFitness(const std::vector<IndividualRunResult> &gameplayResultsPop)
    {
        int pos = 0;
        for (auto &indivResults : gameplayResultsPop)
        {
            fitness[pos] = 0;
            for (auto &timeResult : indivResults)
            {
                fitness[pos] += std::abs(timeResult.TargetTime - timeResult.MeasuredTime) / timeResult.TargetTime;
            }
            pos++;
        }
        return;
    }

} // namespace EvoAlg


///////////////////////////////////////////////////////////////////////////////
// lost_df.hpp                                                               //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_LOST_DF_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_LOST_DF_HPP_ER_2010
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/range.hpp>
#include <boost/accumulators/framework/depends_on.hpp> //contains_feature_of
#include <boost/accumulators/framework/accumulator_set.hpp> //visit_if
#include <boost/statistics/detail/non_parametric/contingency_table/factor/vec_levels.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/tag.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chisq_statistic{
namespace independence_between_aux{

    template<typename A>
    struct lost_df_accumulator{

        lost_df_accumulator(const A& acc,std::size_t& df)
            :acc_(acc),df_(df){}

        template<typename Key>
        void operator()(const boost::mpl::detail::wrapper<Key>& wrapper)const{
            namespace ns = contingency_table::extract;
            this->df_ += (ns::levels<Key>( this->acc_ ).size() - 1);
        }

        private:
        const A& acc_;
        mutable std::size_t& df_;
    };

    template<typename A>
    independence_between_aux::lost_df_accumulator<A>
    make_lost_df_accumulator(const A& acc,std::size_t& df){
        return independence_between_aux::lost_df_accumulator<A>(acc,df);
    }

}// independence_between_aux

  	template<typename Keys,typename AccSet>
    std::size_t lost_degrees_of_freedom(
        const boost::mpl::detail::wrapper<
            pearson_chisq_statistic::tag::independence_between<Keys>
        >& statistic,
        const AccSet& acc
    )
    {
        typedef boost::mpl::detail::wrapper<boost::mpl::_> op_;
        std::size_t result = 0;
        boost::mpl::for_each<Keys,op_>(
            independence_between_aux::make_lost_df_accumulator(acc, result)
        );
        return result;
  	}

}// pearson_chisq_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif


#pragma once
#include "branch_tree_node.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include "leaf_tree_node.hpp"
#include "marching_cubes.hpp"
#include "object.hpp"
#include "tree_config.hpp"
#include "tree_node.hpp"
#include "vector.hpp"
#include <algorithm>
#include <array>
#include <memory>
#include <vector>

namespace dmc
{
template <class Scalar>
class tree
{
public:
	typedef Scalar scalar_type;
	typedef vector<scalar_type, 3> vector_type;
	typedef object<scalar_type> object_type;
	typedef tree_config<scalar_type> config_type;
	typedef tree_node<scalar_type> node_type;
	typedef branch_tree_node<scalar_type> branch_node_type;
	typedef leaf_tree_node<scalar_type> leaf_node_type;
	typedef vertex<scalar_type> vertex_type;

	explicit tree(const vector_type &minimum, const vector_type &maximum, const config_type &config = config_type())
			: minimum_(minimum), config_(config)
	{
		auto v = maximum - minimum;

		size_ = v.map([&](auto x) {
							 return std::max(static_cast<scalar_type>(1.0), std::ceil(x / config_.grid_width));
						 })
								.template cast<std::size_t>();

		children_.resize(size_.product());
	}

	void generate(const object_type &obj)
	{
		for (std::size_t iz = 0; iz < size_.z(); ++iz)
		{
			for (std::size_t iy = 0; iy < size_.y(); ++iy)
			{
				for (std::size_t ix = 0; ix < size_.x(); ++ix)
				{
					auto minimum = minimum_ + vector<std::size_t, 3>(ix, iy, iz).cast<scalar_type>() * config_.grid_width;
					auto maximum = minimum_ + vector<std::size_t, 3>(ix + 1, iy + 1, iz + 1).cast<scalar_type>() * config_.grid_width;
					children_[index(ix, iy, iz)] = generate_impl(obj, minimum, maximum, 0);
				}
			}
		}
	}

	template <class Receiver>
	void enumerate(Receiver receiver)
	{
		for (std::size_t iz = 0; iz < size_.z(); ++iz)
		{
			for (std::size_t iy = 0; iy < size_.y(); ++iy)
			{
				for (std::size_t ix = 0; ix < size_.x(); ++ix)
				{
					enumerate_impl_c(*children_[index(ix, iy, iz)], receiver);

					if (ix != size_.x() - 1)
						enumerate_impl_f_x(*children_[index(ix, iy, iz)], *children_[index(ix + 1, iy, iz)], receiver);

					if (iy != size_.y() - 1)
						enumerate_impl_f_y(*children_[index(ix, iy, iz)], *children_[index(ix, iy + 1, iz)], receiver);

					if (iz != size_.z() - 1)
						enumerate_impl_f_z(*children_[index(ix, iy, iz)], *children_[index(ix, iy, iz + 1)], receiver);

					if (ix != size_.x() - 1 && iy != size_.y() - 1)
					{
						enumerate_impl_e_xy(
								*children_[index(ix, iy, iz)],
								*children_[index(ix + 1, iy, iz)],
								*children_[index(ix, iy + 1, iz)],
								*children_[index(ix + 1, iy + 1, iz)],
								receiver);
					}

					if (iy != size_.y() - 1 && iz != size_.z() - 1)
					{
						enumerate_impl_e_yz(
								*children_[index(ix, iy, iz)],
								*children_[index(ix, iy + 1, iz)],
								*children_[index(ix, iy, iz + 1)],
								*children_[index(ix, iy + 1, iz + 1)],
								receiver);
					}

					if (ix != size_.x() - 1 && iz != size_.z() - 1)
					{
						enumerate_impl_e_xz(
								*children_[index(ix, iy, iz)],
								*children_[index(ix + 1, iy, iz)],
								*children_[index(ix, iy, iz + 1)],
								*children_[index(ix + 1, iy, iz + 1)],
								receiver);
					}

					if (ix != size_.x() - 1 && iy != size_.y() - 1 && iz != size_.z() - 1)
					{
						enumerate_impl_v(
								*children_[index(ix, iy, iz)],
								*children_[index(ix + 1, iy, iz)],
								*children_[index(ix, iy + 1, iz)],
								*children_[index(ix + 1, iy + 1, iz)],
								*children_[index(ix, iy, iz + 1)],
								*children_[index(ix + 1, iy, iz + 1)],
								*children_[index(ix, iy + 1, iz + 1)],
								*children_[index(ix + 1, iy + 1, iz + 1)],
								receiver);
					}
				}
			}
		}
	}

private:
	std::size_t index(std::size_t ix, std::size_t iy, std::size_t iz) const
	{
		return iz * size_.y() * size_.x() + iy * size_.x() + ix;
	}

	std::unique_ptr<node_type> generate_impl(const object_type &obj, const vector_type &minimum, const vector_type &maximum, std::size_t depth) const
	{
		std::array<vector_type, 8> points =
				{{
						{minimum.x(), minimum.y(), minimum.z()},
						{maximum.x(), minimum.y(), minimum.z()},
						{minimum.x(), maximum.y(), minimum.z()},
						{maximum.x(), maximum.y(), minimum.z()},
						{minimum.x(), minimum.y(), maximum.z()},
						{maximum.x(), minimum.y(), maximum.z()},
						{minimum.x(), maximum.y(), maximum.z()},
						{maximum.x(), maximum.y(), maximum.z()},
				}};

		std::array<scalar_type, 8> values;

		std::transform(points.begin(), points.end(), values.begin(),
									 [&](const auto &p) {
										 return obj.value(p);
									 });

		std::array<vector_type, 8> grads;

		std::transform(points.begin(), points.end(), grads.begin(),
									 [&](const auto &p) {
										 return obj.grad(p);
									 });

		Eigen::Matrix<scalar_type, 11, 4> a;

		for (int i = 0; i < 8; ++i)
		{
			a(i, 0) = grads[i].x();
			a(i, 1) = grads[i].y();
			a(i, 2) = grads[i].z();
			a(i, 3) = static_cast<scalar_type>(-1.0);
		}

		a(8, 0) = config_.nominal_weight;
		a(8, 1) = 0.0;
		a(8, 2) = 0.0;
		a(8, 3) = 0.0;
		a(9, 0) = 0.0;
		a(9, 1) = config_.nominal_weight;
		a(9, 2) = 0.0;
		a(9, 3) = 0.0;
		a(10, 0) = 0.0;
		a(10, 1) = 0.0;
		a(10, 2) = config_.nominal_weight;
		a(10, 3) = 0.0;

		Eigen::Matrix<scalar_type, 11, 1> b;

		auto medium = (minimum + maximum) * static_cast<scalar_type>(0.5);

		for (int i = 0; i < 8; ++i)
			b(i) = dot_product(grads[i], points[i] - medium) - values[i];

		b(8) = 0.0;
		b(9) = 0.0;
		b(10) = 0.0;

		Eigen::Matrix<scalar_type, 4, 1> x = a.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);

		auto center = vector_type(x(0), x(1), x(2)) + medium;
		auto offset = obj.value(center);

		auto error = scalar_type();

		for (int i = 0; i < 8; ++i)
			error += squared(offset - values[i] - dot_product(grads[i], center - points[i]));

		if (depth >= config_.maximum_depth || error < squared(config_.tolerance))
		{
			return std::make_unique<leaf_node_type>(vertex_type(center, offset));
		}
		else
		{
			std::array<std::unique_ptr<node_type>, 8> nodes =
					{{
							generate_impl(obj, {minimum.x(), minimum.y(), minimum.z()}, {medium.x(), medium.y(), medium.z()}, depth + 1),
							generate_impl(obj, {medium.x(), minimum.y(), minimum.z()}, {maximum.x(), medium.y(), medium.z()}, depth + 1),
							generate_impl(obj, {minimum.x(), medium.y(), minimum.z()}, {medium.x(), maximum.y(), medium.z()}, depth + 1),
							generate_impl(obj, {medium.x(), medium.y(), minimum.z()}, {maximum.x(), maximum.y(), medium.z()}, depth + 1),
							generate_impl(obj, {minimum.x(), minimum.y(), medium.z()}, {medium.x(), medium.y(), maximum.z()}, depth + 1),
							generate_impl(obj, {medium.x(), minimum.y(), medium.z()}, {maximum.x(), medium.y(), maximum.z()}, depth + 1),
							generate_impl(obj, {minimum.x(), medium.y(), medium.z()}, {medium.x(), maximum.y(), maximum.z()}, depth + 1),
							generate_impl(obj, {medium.x(), medium.y(), medium.z()}, {maximum.x(), maximum.y(), maximum.z()}, depth + 1),
					}};

			return std::make_unique<branch_node_type>(std::move(nodes));
		}
	}

	template <class Receiver>
	void enumerate_impl_c(const node_type &n, Receiver receiver)
	{
		if (auto b = dynamic_cast<const branch_node_type *>(&n))
		{
			enumerate_impl_c(*b->children()[0], receiver);
			enumerate_impl_c(*b->children()[1], receiver);
			enumerate_impl_c(*b->children()[2], receiver);
			enumerate_impl_c(*b->children()[3], receiver);
			enumerate_impl_c(*b->children()[4], receiver);
			enumerate_impl_c(*b->children()[5], receiver);
			enumerate_impl_c(*b->children()[6], receiver);
			enumerate_impl_c(*b->children()[7], receiver);

			enumerate_impl_f_x(*b->children()[0], *b->children()[1], receiver);
			enumerate_impl_f_x(*b->children()[2], *b->children()[3], receiver);
			enumerate_impl_f_x(*b->children()[4], *b->children()[5], receiver);
			enumerate_impl_f_x(*b->children()[6], *b->children()[7], receiver);

			enumerate_impl_f_y(*b->children()[0], *b->children()[2], receiver);
			enumerate_impl_f_y(*b->children()[1], *b->children()[3], receiver);
			enumerate_impl_f_y(*b->children()[4], *b->children()[6], receiver);
			enumerate_impl_f_y(*b->children()[5], *b->children()[7], receiver);

			enumerate_impl_f_z(*b->children()[0], *b->children()[4], receiver);
			enumerate_impl_f_z(*b->children()[1], *b->children()[5], receiver);
			enumerate_impl_f_z(*b->children()[2], *b->children()[6], receiver);
			enumerate_impl_f_z(*b->children()[3], *b->children()[7], receiver);

			enumerate_impl_e_xy(*b->children()[0], *b->children()[1], *b->children()[2], *b->children()[3], receiver);
			enumerate_impl_e_xy(*b->children()[4], *b->children()[5], *b->children()[6], *b->children()[7], receiver);

			enumerate_impl_e_yz(*b->children()[0], *b->children()[2], *b->children()[4], *b->children()[6], receiver);
			enumerate_impl_e_yz(*b->children()[1], *b->children()[3], *b->children()[5], *b->children()[7], receiver);

			enumerate_impl_e_xz(*b->children()[0], *b->children()[1], *b->children()[4], *b->children()[5], receiver);
			enumerate_impl_e_xz(*b->children()[2], *b->children()[3], *b->children()[6], *b->children()[7], receiver);

			enumerate_impl_v(
					*b->children()[0],
					*b->children()[1],
					*b->children()[2],
					*b->children()[3],
					*b->children()[4],
					*b->children()[5],
					*b->children()[6],
					*b->children()[7],
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_f_x(const node_type &n1, const node_type &n2, Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);

		if (b1 || b2)
		{
			enumerate_impl_f_x(b1 ? *b1->children()[1] : n1, b2 ? *b2->children()[0] : n2, receiver);
			enumerate_impl_f_x(b1 ? *b1->children()[3] : n1, b2 ? *b2->children()[2] : n2, receiver);
			enumerate_impl_f_x(b1 ? *b1->children()[5] : n1, b2 ? *b2->children()[4] : n2, receiver);
			enumerate_impl_f_x(b1 ? *b1->children()[7] : n1, b2 ? *b2->children()[6] : n2, receiver);

			enumerate_impl_e_xy(
					b1 ? *b1->children()[1] : n1,
					b2 ? *b2->children()[0] : n2,
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[2] : n2,
					receiver);

			enumerate_impl_e_xy(
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[4] : n2,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					receiver);

			enumerate_impl_e_xz(
					b1 ? *b1->children()[1] : n1,
					b2 ? *b2->children()[0] : n2,
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[4] : n2,
					receiver);

			enumerate_impl_e_xz(
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[2] : n2,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[1] : n1,
					b2 ? *b2->children()[0] : n2,
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[2] : n2,
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[4] : n2,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_f_y(const node_type &n1, const node_type &n2, Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);

		if (b1 || b2)
		{
			enumerate_impl_f_y(b1 ? *b1->children()[2] : n1, b2 ? *b2->children()[0] : n2, receiver);
			enumerate_impl_f_y(b1 ? *b1->children()[3] : n1, b2 ? *b2->children()[1] : n2, receiver);
			enumerate_impl_f_y(b1 ? *b1->children()[6] : n1, b2 ? *b2->children()[4] : n2, receiver);
			enumerate_impl_f_y(b1 ? *b1->children()[7] : n1, b2 ? *b2->children()[5] : n2, receiver);

			enumerate_impl_e_xy(
					b1 ? *b1->children()[2] : n1,
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[0] : n2,
					b2 ? *b2->children()[1] : n2,
					receiver);

			enumerate_impl_e_xy(
					b1 ? *b1->children()[6] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[4] : n2,
					b2 ? *b2->children()[5] : n2,
					receiver);

			enumerate_impl_e_yz(
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[1] : n2,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[5] : n2,
					receiver);

			enumerate_impl_e_yz(
					b1 ? *b1->children()[2] : n1,
					b2 ? *b2->children()[0] : n2,
					b1 ? *b1->children()[6] : n1,
					b2 ? *b2->children()[4] : n2,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[2] : n1,
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[0] : n2,
					b2 ? *b2->children()[1] : n2,
					b1 ? *b1->children()[6] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[4] : n2,
					b2 ? *b2->children()[5] : n2,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_f_z(const node_type &n1, const node_type &n2, Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);

		if (b1 || b2)
		{
			enumerate_impl_f_z(b1 ? *b1->children()[4] : n1, b2 ? *b2->children()[0] : n2, receiver);
			enumerate_impl_f_z(b1 ? *b1->children()[5] : n1, b2 ? *b2->children()[1] : n2, receiver);
			enumerate_impl_f_z(b1 ? *b1->children()[6] : n1, b2 ? *b2->children()[2] : n2, receiver);
			enumerate_impl_f_z(b1 ? *b1->children()[7] : n1, b2 ? *b2->children()[3] : n2, receiver);

			enumerate_impl_e_xz(
					b1 ? *b1->children()[4] : n1,
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[0] : n2,
					b2 ? *b2->children()[1] : n2,
					receiver);

			enumerate_impl_e_xz(
					b1 ? *b1->children()[6] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[2] : n2,
					b2 ? *b2->children()[3] : n2,
					receiver);

			enumerate_impl_e_yz(
					b1 ? *b1->children()[4] : n1,
					b1 ? *b1->children()[6] : n1,
					b2 ? *b2->children()[0] : n2,
					b2 ? *b2->children()[2] : n2,
					receiver);

			enumerate_impl_e_yz(
					b1 ? *b1->children()[5] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[1] : n2,
					b2 ? *b2->children()[3] : n2,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[4] : n1,
					b1 ? *b1->children()[5] : n1,
					b1 ? *b1->children()[6] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[0] : n2,
					b2 ? *b2->children()[1] : n2,
					b2 ? *b2->children()[2] : n2,
					b2 ? *b2->children()[3] : n2,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_e_xy(
			const node_type &n1,
			const node_type &n2,
			const node_type &n3,
			const node_type &n4,
			Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);
		auto b3 = dynamic_cast<const branch_node_type *>(&n3);
		auto b4 = dynamic_cast<const branch_node_type *>(&n4);

		if (b1 || b2 || b3 || b4)
		{
			enumerate_impl_e_xy(
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[2] : n2,
					b3 ? *b3->children()[1] : n3,
					b4 ? *b4->children()[0] : n4,
					receiver);

			enumerate_impl_e_xy(
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					b3 ? *b3->children()[5] : n3,
					b4 ? *b4->children()[4] : n4,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[3] : n1,
					b2 ? *b2->children()[2] : n2,
					b3 ? *b3->children()[1] : n3,
					b4 ? *b4->children()[0] : n4,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					b3 ? *b3->children()[5] : n3,
					b4 ? *b4->children()[4] : n4,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_e_yz(
			const node_type &n1,
			const node_type &n2,
			const node_type &n3,
			const node_type &n4,
			Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);
		auto b3 = dynamic_cast<const branch_node_type *>(&n3);
		auto b4 = dynamic_cast<const branch_node_type *>(&n4);

		if (b1 || b2 || b3 || b4)
		{
			enumerate_impl_e_yz(
					b1 ? *b1->children()[6] : n1,
					b2 ? *b2->children()[4] : n2,
					b3 ? *b3->children()[2] : n3,
					b4 ? *b4->children()[0] : n4,
					receiver);

			enumerate_impl_e_yz(
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[5] : n2,
					b3 ? *b3->children()[3] : n3,
					b4 ? *b4->children()[1] : n4,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[6] : n1,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[4] : n2,
					b2 ? *b2->children()[5] : n2,
					b3 ? *b3->children()[2] : n3,
					b3 ? *b3->children()[3] : n3,
					b4 ? *b4->children()[0] : n4,
					b4 ? *b4->children()[1] : n4,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_e_xz(
			const node_type &n1,
			const node_type &n2,
			const node_type &n3,
			const node_type &n4,
			Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);
		auto b3 = dynamic_cast<const branch_node_type *>(&n3);
		auto b4 = dynamic_cast<const branch_node_type *>(&n4);

		if (b1 || b2 || b3 || b4)
		{
			enumerate_impl_e_xz(
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[4] : n2,
					b3 ? *b3->children()[1] : n3,
					b4 ? *b4->children()[0] : n4,
					receiver);

			enumerate_impl_e_xz(
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					b3 ? *b3->children()[3] : n3,
					b4 ? *b4->children()[2] : n4,
					receiver);

			enumerate_impl_v(
					b1 ? *b1->children()[5] : n1,
					b2 ? *b2->children()[4] : n2,
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					b3 ? *b3->children()[1] : n3,
					b4 ? *b4->children()[0] : n4,
					b3 ? *b3->children()[3] : n3,
					b4 ? *b4->children()[2] : n4,
					receiver);
		}
	}

	template <class Receiver>
	void enumerate_impl_v(
			const node_type &n1,
			const node_type &n2,
			const node_type &n3,
			const node_type &n4,
			const node_type &n5,
			const node_type &n6,
			const node_type &n7,
			const node_type &n8,
			Receiver receiver)
	{
		auto b1 = dynamic_cast<const branch_node_type *>(&n1);
		auto b2 = dynamic_cast<const branch_node_type *>(&n2);
		auto b3 = dynamic_cast<const branch_node_type *>(&n3);
		auto b4 = dynamic_cast<const branch_node_type *>(&n4);
		auto b5 = dynamic_cast<const branch_node_type *>(&n5);
		auto b6 = dynamic_cast<const branch_node_type *>(&n6);
		auto b7 = dynamic_cast<const branch_node_type *>(&n7);
		auto b8 = dynamic_cast<const branch_node_type *>(&n8);

		if (b1 || b2 || b3 || b4 || b5 || b6 || b7 || b8)
		{
			enumerate_impl_v(
					b1 ? *b1->children()[7] : n1,
					b2 ? *b2->children()[6] : n2,
					b3 ? *b3->children()[5] : n3,
					b4 ? *b4->children()[4] : n4,
					b5 ? *b5->children()[3] : n5,
					b6 ? *b6->children()[2] : n6,
					b7 ? *b7->children()[1] : n7,
					b8 ? *b8->children()[0] : n8,
					receiver);
		}
		else
		{
			auto l1 = static_cast<const leaf_node_type *>(&n1);
			auto l2 = static_cast<const leaf_node_type *>(&n2);
			auto l3 = static_cast<const leaf_node_type *>(&n3);
			auto l4 = static_cast<const leaf_node_type *>(&n4);
			auto l5 = static_cast<const leaf_node_type *>(&n5);
			auto l6 = static_cast<const leaf_node_type *>(&n6);
			auto l7 = static_cast<const leaf_node_type *>(&n7);
			auto l8 = static_cast<const leaf_node_type *>(&n8);

			std::array<const vertex_type *, 8> vertices =
					{{
							&l1->vertex(),
							&l2->vertex(),
							&l3->vertex(),
							&l4->vertex(),
							&l5->vertex(),
							&l6->vertex(),
							&l7->vertex(),
							&l8->vertex(),
					}};

			marching_cubes<scalar_type>(vertices, receiver);
		}
	}

	vector_type minimum_;
	config_type config_;

	std::size_t grid_size_;
	scalar_type grid_width_;

	vector<std::size_t, 3> size_;
	std::vector<std::unique_ptr<node_type>> children_;
};
} // namespace dmc