/* --------------------------------------------------------------------------
*
* (C) Copyright …
*
* ---------------------------------------------------------------------------
*/

/*!
 * @file Transform3DToMatConverter.cpp
 * @date 20/02/2018
 * @authors Alessandro Bianco
 */

/*!
 * @addtogroup Converters
 *
 * Implementation of Transform3DToMatConverter class.
 *
 *
 * @{
 */

/* --------------------------------------------------------------------------
 *
 * Includes
 *
 * --------------------------------------------------------------------------
 */

#include "Transform3DToMatConverter.hpp"
#include <Errors/Assert.hpp>
#include <Eigen/Geometry>

namespace Converters {

using namespace PoseWrapper;

/* --------------------------------------------------------------------------
 *
 * Public Member Functions
 *
 * --------------------------------------------------------------------------
 */
const cv::Mat Transform3DToMatConverter::Convert(const PoseWrapper::Transform3DConstPtr& transform)
	{
	Eigen::Matrix4f conversion;

	Eigen::Quaternionf eigenRotation( GetWOrientation(*transform), GetXOrientation(*transform), GetYOrientation(*transform), GetZOrientation(*transform));
	Eigen::Matrix3f rotationMatrix = eigenRotation.toRotationMatrix();

	Eigen::Translation<float, 3> eigenTranslation( GetXPosition(*transform), GetYPosition(*transform), GetZPosition(*transform));

	conversion << 	rotationMatrix(0,0), rotationMatrix(0,1), rotationMatrix(0,2), eigenTranslation.x(),
			rotationMatrix(1,0), rotationMatrix(1,1), rotationMatrix(1,2), eigenTranslation.y(),
			rotationMatrix(2,0), rotationMatrix(2,1), rotationMatrix(2,2), eigenTranslation.z(),
			0, 0, 0, 1;

	cv::Mat matrixConversion(3, 4, CV_32FC1);
	for(unsigned row = 0; row < 3; row++)
		{
		for(unsigned column = 0; column<4; column++)
			{
			matrixConversion.at<float>(row, column) = conversion(row, column);
			}
		}

	return matrixConversion;
	}

const cv::Mat Transform3DToMatConverter::ConvertShared(const PoseWrapper::Transform3DSharedConstPtr& transform)
	{
	return Convert(transform.get());
	}

}

/** @} */


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_TKSOLVE_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_TKSOLVE_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <nt2/sdk/memory/container.hpp>

/*!
 * \ingroup algebra
 * \defgroup algebra_tksolve tksolve
 *
 * \par Description
 * function x = tksolve(r, b, tran)
 * ksolve     solves block triangular kronecker system.
 *            x = tksolve(r, b, tran) solves
 *                  a*x = b  if tran = '',
 *                 a'*x = b  if tran = 't',
 *            where a = kron(eye,r) + kron(transpose(r),eye).
 *            default: tran = ''.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/tksolve.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \param a the matrix or vector expression a
 *
 * \param type of tksolve required
 *
 * \par Notes
 *   Call the dedicated lapack routines available on the target.
 * \par
 *
**/

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag tksolve_ of functor tksolve
     *        in namespace nt2::tag for toolbox algebra
    **/
    struct tksolve_ :  tag::formal_ { typedef tag::formal_ parent; };
  }

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::tksolve_, tksolve, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::tksolve_, tksolve, 3)

}

#endif


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Projection_traits_xy_3.h>

#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_with_info_2.h>

#include <CGAL/boost/graph/graph_traits_Delaunay_triangulation_2.h>
#include <CGAL/boost/graph/copy_face_graph.h>

#include <CGAL/Point_set_3.h>
#include <CGAL/Point_set_3/IO.h>
#include <CGAL/compute_average_spacing.h>

#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/locate.h>

#include <CGAL/Polygon_mesh_processing/triangulate_hole.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>

#include <boost/graph/adjacency_list.hpp>
#include <CGAL/boost/graph/split_graph_into_polylines.h>

#include <CGAL/IO/WKT.h>

#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_triangulation_plus_2.h>

#include <CGAL/Polyline_simplification_2/simplify.h>
#include <CGAL/Polyline_simplification_2/Squared_distance_cost.h>

#include <CGAL/Classification.h>

#include <CGAL/Random.h>

#include <fstream>
#include <queue>

#include "include/Color_ramp.h"

///////////////////////////////////////////////////////////////////
//! [TIN DS]

using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;
using Projection_traits = CGAL::Projection_traits_xy_3<Kernel>;
using Point_2 = Kernel::Point_2;
using Point_3 = Kernel::Point_3;
using Segment_3 = Kernel::Segment_3;

// Triangulated Irregular Network
using TIN = CGAL::Delaunay_triangulation_2<Projection_traits>;

//! [TIN DS]
///////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////
//! [TIN_with_info DS]

// Triangulated Irregular Network (with info)
using Point_set = CGAL::Point_set_3<Point_3>;
using Vbi = CGAL::Triangulation_vertex_base_with_info_2 <Point_set::Index, Projection_traits>;
using Fbi = CGAL::Triangulation_face_base_with_info_2<int, Projection_traits>;
using TDS = CGAL::Triangulation_data_structure_2<Vbi, Fbi>;
using TIN_with_info = CGAL::Delaunay_triangulation_2<Projection_traits, TDS>;

//! [TIN_with_info DS]
///////////////////////////////////////////////////////////////////

namespace Classification = CGAL::Classification;

#ifdef CGAL_LINKED_WITH_TBB
using Concurrency_tag = CGAL::Parallel_tag;
#else
using Concurrency_tag = CGAL::Sequential_tag;
#endif

///////////////////////////////////////////////////////////////////
//! [Contouring functions]

bool face_has_isovalue (TIN::Face_handle fh, double isovalue)
{
  bool above = false, below = false;
  for (int i = 0; i < 3; ++ i)
  {
    // Face has isovalue if one of its vertices is above and another
    // one below
    if (fh->vertex(i)->point().z() > isovalue)
      above = true;
    if (fh->vertex(i)->point().z() < isovalue)
      below = true;
  }

  return (above && below);
}

Segment_3 isocontour_in_face (TIN::Face_handle fh, double isovalue)
{
  Point_3 source;
  Point_3 target;
  bool source_found = false;

  for (int i = 0; i < 3; ++ i)
  {
    Point_3 p0 = fh->vertex((i+1) % 3)->point();
    Point_3 p1 = fh->vertex((i+2) % 3)->point();

    // Check if the isovalue crosses segment (p0,p1)
    if ((p0.z() - isovalue) * (p1.z() - isovalue) > 0)
      continue;

    double zbottom = p0.z();
    double ztop = p1.z();
    if (zbottom > ztop)
    {
      std::swap (zbottom, ztop);
      std::swap (p0, p1);
    }

    // Compute position of segment vertex
    double ratio = (isovalue - zbottom) / (ztop - zbottom);
    Point_3 p = CGAL::barycenter (p0, (1 - ratio), p1,ratio);

    if (source_found)
      target = p;
    else
    {
      source = p;
      source_found = true;
    }
  }

  return Segment_3 (source, target);
}

//! [Contouring functions]
///////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////
//! [Contouring visitor]

template <typename Graph>
class Polylines_visitor
{
private:
  std::vector<std::vector<Point_3> >& polylines;
  Graph& graph;

public:

  Polylines_visitor (Graph& graph, std::vector<std::vector<Point_3> >& polylines)
    : polylines (polylines), graph(graph) { }

  void start_new_polyline()
  {
    polylines.push_back (std::vector<Point_3>());
  }

  void add_node (typename Graph::vertex_descriptor vd)
  {
    polylines.back().push_back (graph[vd]);
  }

  void end_polyline()
  {
    // filter small polylines
    if (polylines.back().size() < 50)
      polylines.pop_back();
  }
};

//! [Contouring visitor]
///////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////
//! [CDT]

namespace PS = CGAL::Polyline_simplification_2;
using CDT_vertex_base = PS::Vertex_base_2<Projection_traits>;
using CDT_face_base = CGAL::Constrained_triangulation_face_base_2<Projection_traits>;
using CDT_TDS = CGAL::Triangulation_data_structure_2<CDT_vertex_base, CDT_face_base>;
using CDT = CGAL::Constrained_Delaunay_triangulation_2<Projection_traits, CDT_TDS>;
using CTP = CGAL::Constrained_triangulation_plus_2<CDT>;

//! [CDT]
///////////////////////////////////////////////////////////////////

int main (int argc, char** argv)
{
  if (argc != 2)
  {
    std::cerr << "Usage: " << argv[0] << " points.ply" << std::endl;
    return EXIT_FAILURE;
  }

  ///////////////////////////////////////////////////////////////////
  //! [Init DSM]

  // Read points
  std::ifstream ifile (argv[1], std::ios_base::binary);
  CGAL::Point_set_3<Point_3> points;
  ifile >> points;
  std::cerr << points.size() << " point(s) read" << std::endl;

  // Create DSM
  TIN dsm (points.points().begin(), points.points().end());

  //! [Init DSM]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Save DSM]

  using Mesh = CGAL::Surface_mesh<Point_3>;

  Mesh dsm_mesh;
  CGAL::copy_face_graph (dsm, dsm_mesh);
  std::ofstream dsm_ofile ("dsm.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (dsm_ofile);
  CGAL::IO::write_PLY (dsm_ofile, dsm_mesh);
  dsm_ofile.close();

  //! [Save DSM]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [TIN_with_info]

  auto idx_to_point_with_info
    = [&](const Point_set::Index& idx) -> std::pair<Point_3, Point_set::Index>
      {
        return std::make_pair (points.point(idx), idx);
      };

  TIN_with_info tin_with_info
    (boost::make_transform_iterator (points.begin(), idx_to_point_with_info),
     boost::make_transform_iterator (points.end(), idx_to_point_with_info));

  //! [TIN_with_info]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Components]

  double spacing = CGAL::compute_average_spacing<Concurrency_tag>(points, 6);
  spacing *= 2;

  auto face_height
    = [&](const TIN_with_info::Face_handle fh) -> double
      {
        double out = 0.;
        for (int i = 0; i < 3; ++ i)
          out = (std::max) (out, CGAL::abs(fh->vertex(i)->point().z() - fh->vertex((i+1)%3)->point().z()));
        return out;
      };

  // Initialize faces info
  for (TIN_with_info::Face_handle fh : tin_with_info.all_face_handles())
    if (tin_with_info.is_infinite(fh) || face_height(fh) > spacing) // Filtered faces are given info() = -2
      fh->info() = -2;
    else // Pending faces are given info() = -1;
      fh->info() = -1;

  // Flooding algorithm
  std::vector<int> component_size;
  for (TIN_with_info::Face_handle fh : tin_with_info.finite_face_handles())
  {
    if (fh->info() != -1)
      continue;

    std::queue<TIN_with_info::Face_handle> todo;
    todo.push(fh);

    int size = 0;
    while (!todo.empty())
    {
      TIN_with_info::Face_handle current = todo.front();
      todo.pop();

      if (current->info() != -1)
        continue;
      current->info() = int(component_size.size());
      ++ size;

      for (int i = 0; i < 3; ++ i)
        todo.push (current->neighbor(i));
    }

    component_size.push_back (size);
  }

  std::cerr << component_size.size() << " connected component(s) found" << std::endl;

  //! [Components]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Save TIN with info]

  Mesh tin_colored_mesh;

  Mesh::Property_map<Mesh::Face_index, CGAL::IO::Color>
    color_map = tin_colored_mesh.add_property_map<Mesh::Face_index, CGAL::IO::Color>("f:color").first;

  CGAL::copy_face_graph (tin_with_info, tin_colored_mesh,
                         CGAL::parameters::face_to_face_output_iterator
                         (boost::make_function_output_iterator
                          ([&](const std::pair<TIN_with_info::Face_handle, Mesh::Face_index>& ff)
                           {
                             // Color unassigned faces gray
                             if (ff.first->info() < 0)
                               color_map[ff.second] = CGAL::IO::Color(128, 128, 128);
                             else
                             {
                               // Random color seeded by the component ID
                               CGAL::Random r (ff.first->info());
                               color_map[ff.second] = CGAL::IO::Color (r.get_int(64, 192),
                                                                   r.get_int(64, 192),
                                                                   r.get_int(64, 192));
                             }
                           })));

  std::ofstream tin_colored_ofile ("colored_tin.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (tin_colored_ofile);
  CGAL::IO::write_PLY (tin_colored_ofile, tin_colored_mesh);
  tin_colored_ofile.close();

  //! [Save TIN with info]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Filtering]

  int min_size = int(points.size() / 2);

  std::vector<TIN_with_info::Vertex_handle> to_remove;
  for (TIN_with_info::Vertex_handle vh : tin_with_info.finite_vertex_handles())
  {
    TIN_with_info::Face_circulator circ = tin_with_info.incident_faces (vh),
      start = circ;

    // Remove a vertex if it's only adjacent to components smaller than threshold
    bool keep = false;
    do
    {
      if (circ->info() >= 0 && component_size[std::size_t(circ->info())] > min_size)
      {
        keep = true;
        break;
      }
    }
    while (++ circ != start);

    if (!keep)
      to_remove.push_back (vh);
  }

  std::cerr << to_remove.size() << " vertices(s) will be removed after filtering" << std::endl;
  for (TIN_with_info::Vertex_handle vh : to_remove)
    tin_with_info.remove (vh);

  //! [Filtering]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Hole filling]

  // Copy and keep track of overly large faces
  Mesh dtm_mesh;

  std::vector<Mesh::Face_index> face_selection;
  Mesh::Property_map<Mesh::Face_index, bool> face_selection_map
   = dtm_mesh.add_property_map<Mesh::Face_index, bool>("is_selected", false).first;

  double limit = CGAL::square (5 * spacing);
  CGAL::copy_face_graph (tin_with_info, dtm_mesh,
                         CGAL::parameters::face_to_face_output_iterator
                         (boost::make_function_output_iterator
                          ([&](const std::pair<TIN_with_info::Face_handle, Mesh::Face_index>& ff)
                           {
                             double longest_edge = 0.;
                             bool border = false;
                             for (int i = 0; i < 3; ++ i)
                             {
                               longest_edge = (std::max)(longest_edge, CGAL::squared_distance
                                                         (ff.first->vertex((i+1)%3)->point(),
                                                          ff.first->vertex((i+2)%3)->point()));

                               TIN_with_info::Face_circulator circ
                                 = tin_with_info.incident_faces (ff.first->vertex(i)),
                                 start = circ;
                               do
                               {
                                 if (tin_with_info.is_infinite (circ))
                                 {
                                   border = true;
                                   break;
                                 }
                               }
                               while (++ circ != start);

                               if (border)
                                 break;
                             }

                             // Select if face is too big AND it's not
                             // on the border (to have closed holes)
                             if (!border && longest_edge > limit)
                             {
                               face_selection_map[ff.second] = true;
                               face_selection.push_back (ff.second);
                             }
                           })));

  // Save original DTM
  std::ofstream dtm_ofile ("dtm.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (dtm_ofile);
  CGAL::IO::write_PLY (dtm_ofile, dtm_mesh);
  dtm_ofile.close();

  std::cerr << face_selection.size() << " face(s) are selected for removal" << std::endl;

  // Expand face selection to keep a well formed 2-manifold mesh after removal
  CGAL::expand_face_selection_for_removal (face_selection, dtm_mesh, face_selection_map);
  face_selection.clear();
  for (Mesh::Face_index fi : faces(dtm_mesh))
    if (face_selection_map[fi])
      face_selection.push_back(fi);

  std::cerr << face_selection.size() << " face(s) are selected for removal after expansion" << std::endl;

  for (Mesh::Face_index fi : face_selection)
    CGAL::Euler::remove_face (halfedge(fi, dtm_mesh), dtm_mesh);
  dtm_mesh.collect_garbage();

  if (!dtm_mesh.is_valid())
    std::cerr << "Invalid mesh!" << std::endl;

  // Save filtered DTM
  std::ofstream dtm_holes_ofile ("dtm_with_holes.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (dtm_holes_ofile);
  CGAL::IO::write_PLY (dtm_holes_ofile, dtm_mesh);
  dtm_holes_ofile.close();

  // Get all holes
  std::vector<Mesh::Halfedge_index> holes;
  CGAL::Polygon_mesh_processing::extract_boundary_cycles (dtm_mesh, std::back_inserter (holes));

  std::cerr << holes.size() << " hole(s) identified" << std::endl;

  // Identify outer hull (hole with maximum size)
  double max_size = 0.;
  Mesh::Halfedge_index outer_hull;
  for (Mesh::Halfedge_index hi : holes)
  {
    CGAL::Bbox_3 hole_bbox;
    for (Mesh::Halfedge_index haf : CGAL::halfedges_around_face(hi, dtm_mesh))
    {
      const Point_3& p = dtm_mesh.point(target(haf, dtm_mesh));
      hole_bbox += p.bbox();
    }
    double size = CGAL::squared_distance (Point_2(hole_bbox.xmin(), hole_bbox.ymin()),
                                          Point_2(hole_bbox.xmax(), hole_bbox.ymax()));
    if (size > max_size)
    {
      max_size = size;
      outer_hull = hi;
    }
  }

  // Fill all holes except the bigest (which is the outer hull of the mesh)
  for (Mesh::Halfedge_index hi : holes)
    if (hi != outer_hull)
      CGAL::Polygon_mesh_processing::triangulate_refine_and_fair_hole
        (dtm_mesh, hi, CGAL::Emptyset_iterator(), CGAL::Emptyset_iterator());

  // Save DTM with holes filled
  std::ofstream dtm_filled_ofile ("dtm_filled.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (dtm_filled_ofile);
  CGAL::IO::write_PLY (dtm_filled_ofile, dtm_mesh);
  dtm_filled_ofile.close();

  //! [Hole filling]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Remeshing]

  CGAL::Polygon_mesh_processing::isotropic_remeshing (faces(dtm_mesh), spacing, dtm_mesh);

  std::ofstream dtm_remeshed_ofile ("dtm_remeshed.ply", std::ios_base::binary);
  CGAL::IO::set_binary_mode (dtm_remeshed_ofile);
  CGAL::IO::write_PLY (dtm_remeshed_ofile, dtm_mesh);
  dtm_remeshed_ofile.close();

  //! [Remeshing]
  ///////////////////////////////////////////////////////////////////

  TIN dtm_clean (dtm_mesh.points().begin(), dtm_mesh.points().end());

  ///////////////////////////////////////////////////////////////////
  //! [Rastering]

  CGAL::Bbox_3 bbox = CGAL::bbox_3 (points.points().begin(), points.points().end());

  // Generate raster image 1920-pixels large
  std::size_t width = 1920;
  std::size_t height = std::size_t((bbox.ymax() - bbox.ymin()) * 1920 / (bbox.xmax() - bbox.xmin()));

  std::cerr << "Rastering with resolution " << width << "x" << height << std::endl;

  // Use PPM format (Portable PixMap) for simplicity
  std::ofstream raster_ofile ("raster.ppm", std::ios_base::binary);

  // PPM header
  raster_ofile << "P6" << std::endl // magic number
               << width << " " << height << std::endl // dimensions of the image
               << 255 << std::endl; // maximum color value

  // Use rainbow color ramp output
  Color_ramp color_ramp;

  // Keeping track of location from one point to its neighbor allows
  // for fast locate in DT
  TIN::Face_handle location;

  // Query each pixel of the image
  for (std::size_t y = 0; y < height; ++ y)
    for (std::size_t x = 0; x < width; ++ x)
    {
      Point_3 query (bbox.xmin() + x * (bbox.xmax() - bbox.xmin()) / double(width),
                     bbox.ymin() + (height-y) * (bbox.ymax() - bbox.ymin()) / double(height),
                     0); // not relevant for location in 2D

      location = dtm_clean.locate (query, location);

      // Points outside the convex hull will be colored black
      std::array<unsigned char, 3> colors { 0, 0, 0 };
      if (!dtm_clean.is_infinite(location))
      {
        std::array<double, 3> barycentric_coordinates
          = CGAL::Polygon_mesh_processing::barycentric_coordinates
          (Point_2 (location->vertex(0)->point().x(), location->vertex(0)->point().y()),
           Point_2 (location->vertex(1)->point().x(), location->vertex(1)->point().y()),
           Point_2 (location->vertex(2)->point().x(), location->vertex(2)->point().y()),
           Point_2 (query.x(), query.y()),
           Kernel());

        double height_at_query
          = (barycentric_coordinates[0] * location->vertex(0)->point().z()
             + barycentric_coordinates[1] * location->vertex(1)->point().z()
             + barycentric_coordinates[2] * location->vertex(2)->point().z());

        // Color ramp generates a color depending on a value from 0 to 1
        double height_ratio = (height_at_query - bbox.zmin()) / (bbox.zmax() - bbox.zmin());
        colors = color_ramp.get(height_ratio);
      }
      raster_ofile.write ((char*)(&colors), 3);
    }

  raster_ofile.close();

  //! [Rastering]
  ///////////////////////////////////////////////////////////////////

  // Smooth heights with 5 successive Gaussian filters
  double gaussian_variance = 4 * spacing * spacing;
  for (TIN::Vertex_handle vh : dtm_clean.finite_vertex_handles())
  {
    double z = vh->point().z();
    double total_weight = 1;

    TIN::Vertex_circulator circ = dtm_clean.incident_vertices (vh),
      start = circ;

    do
    {
      if (!dtm_clean.is_infinite(circ))
      {
        double sq_dist = CGAL::squared_distance (vh->point(), circ->point());

        double weight = std::exp(- sq_dist / gaussian_variance);
        z += weight * circ->point().z();
        total_weight += weight;
      }
    }
    while (++ circ != start);

    z /= total_weight;

    vh->point() = Point_3 (vh->point().x(), vh->point().y(), z);
  }

  ///////////////////////////////////////////////////////////////////
  //! [Contouring extraction]

  std::array<double, 50> isovalues; // Contour 50 isovalues
  for (std::size_t i = 0; i < isovalues.size(); ++ i)
    isovalues[i] = bbox.zmin() + ((i+1) * (bbox.zmax() - bbox.zmin()) / (isovalues.size() - 2));

  // First find on each face if they are crossed by some isovalues and
  // extract segments in a graph
  using Segment_graph = boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS, Point_3>;
  Segment_graph graph;
  using Map_p2v = std::map<Point_3, Segment_graph::vertex_descriptor>;
  Map_p2v map_p2v;
  for (TIN::Face_handle vh : dtm_clean.finite_face_handles())
    for (double iv : isovalues)
      if (face_has_isovalue (vh, iv))
      {
        Segment_3 segment = isocontour_in_face (vh, iv);
        for (const Point_3& p : { segment.source(), segment.target() })
        {
          // Only insert end points of segments once to get a well connected graph
          Map_p2v::iterator iter;
          bool inserted;
          std::tie (iter, inserted) = map_p2v.insert (std::make_pair (p, Segment_graph::vertex_descriptor()));
          if (inserted)
          {
            iter->second = boost::add_vertex (graph);
            graph[iter->second] = p;
          }
        }
        boost::add_edge (map_p2v[segment.source()], map_p2v[segment.target()], graph);
      }

  //! [Contouring extraction]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Contouring split]

  // Split segments into polylines
  std::vector<std::vector<Point_3> > polylines;
  Polylines_visitor<Segment_graph> visitor (graph, polylines);
  CGAL::split_graph_into_polylines (graph, visitor);

  std::cerr << polylines.size() << " polylines computed, with "
            << map_p2v.size() << " vertices in total" << std::endl;

  // Output to WKT file
  std::ofstream contour_ofile ("contour.wkt");
  contour_ofile.precision(18);
  CGAL::IO::write_multi_linestring_WKT (contour_ofile, polylines);
  contour_ofile.close();

  //! [Contouring split]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Contouring simplify]

  // Construct constrained Delaunay triangulation with polylines as constraints
  CTP ctp;
  for (const std::vector<Point_3>& poly : polylines)
    ctp.insert_constraint (poly.begin(), poly.end());

  // Simplification algorithm with limit on distance
  PS::simplify (ctp, PS::Squared_distance_cost(), PS::Stop_above_cost_threshold (16 * spacing * spacing));

  polylines.clear();
  for (CTP::Constraint_id cid : ctp.constraints())
  {
    polylines.push_back (std::vector<Point_3>());
    polylines.back().reserve (ctp.vertices_in_constraint (cid).size());
    for (CTP::Vertex_handle vh : ctp.vertices_in_constraint(cid))
      polylines.back().push_back (vh->point());
  }

  std::size_t nb_vertices
    = std::accumulate (polylines.begin(), polylines.end(), 0u,
                       [](std::size_t size, const std::vector<Point_3>& poly) -> std::size_t
                       { return size + poly.size(); });

  std::cerr << nb_vertices
            << " vertices remaining after simplification ("
            << 100. * (nb_vertices / double(map_p2v.size())) << "%)" << std::endl;

  // Output to WKT file
  std::ofstream simplified_ofile ("simplified.wkt");
  simplified_ofile.precision(18);
  CGAL::IO::write_multi_linestring_WKT (simplified_ofile, polylines);
  simplified_ofile.close();

  //! [Contouring simplify]
  ///////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////
  //! [Classification]

  // Get training from input
  Point_set::Property_map<int> training_map;
  bool training_found;
  std::tie (training_map, training_found) = points.property_map<int>("training");

  if (training_found)
  {
    std::cerr << "Classifying ground/vegetation/building" << std::endl;

    // Create labels
    Classification::Label_set labels ({ "ground", "vegetation", "building" });

    // Generate features
    Classification::Feature_set features;
    Classification::Point_set_feature_generator<Kernel, Point_set, Point_set::Point_map>
      generator (points, points.point_map(), 5); // 5 scales

#ifdef CGAL_LINKED_WITH_TBB
    // If TBB is used, features can be computed in parallel
    features.begin_parallel_additions();
    generator.generate_point_based_features (features);
    features.end_parallel_additions();
#else
    generator.generate_point_based_features (features);
#endif

    // Train a random forest classifier
    Classification::ETHZ::Random_forest_classifier classifier (labels, features);
    classifier.train (points.range(training_map));

    // Classify with graphcut regularization
    Point_set::Property_map<int> label_map = points.add_property_map<int>("labels").first;
    Classification::classify_with_graphcut<Concurrency_tag>
      (points, points.point_map(), labels, classifier,
       generator.neighborhood().k_neighbor_query(12), // regularize on 12-neighbors graph
       0.5f, // graphcut weight
       12, // Subdivide to speed-up process
       label_map);

    // Evaluate
    std::cerr << "Mean IoU on training data = "
              << Classification::Evaluation(labels,
                                            points.range(training_map),
                                            points.range(label_map)).mean_intersection_over_union() << std::endl;

    // Save the classified point set
    std::ofstream classified_ofile ("classified.ply");
    CGAL::IO::set_binary_mode (classified_ofile);
    classified_ofile << points;
    classified_ofile.close();
  }

  //! [Classification]
  ///////////////////////////////////////////////////////////////////

  return EXIT_SUCCESS;
}


/*  This file is part of libDAI - http://www.libdai.org/
 *
 *  Copyright (c) 2006-2011, The libDAI authors. All rights reserved.
 *
 *  Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.
 */

#include <algorithm>
#include <cmath>
#include <boost/dynamic_bitset.hpp>
#include <dai/regiongraph.h>
#include <dai/factorgraph.h>
#include <dai/clustergraph.h>

namespace dai {

using namespace std;

void RegionGraph::construct( const FactorGraph &fg, const std::vector<VarSet> &ors, const std::vector<Region> &irs, const std::vector<std::pair<size_t,size_t> > &edges ) {
    // Copy factor graph structure
    FactorGraph::operator=( fg );

    // Copy inner regions
    _IRs = irs;

    // Construct outer regions (giving them counting number 1.0)
    _ORs.clear();
    _ORs.reserve( ors.size() );
    bforeach( const VarSet &alpha, ors )
        _ORs.push_back( FRegion(Factor(alpha, 1.0), 1.0) );

    // For each factor, find an outer region that subsumes that factor.
    // Then, multiply the outer region with that factor.
    _fac2OR.clear();
    _fac2OR.reserve( nrFactors() );
    for( size_t I = 0; I < nrFactors(); I++ ) {
        size_t alpha;
        for( alpha = 0; alpha < nrORs(); alpha++ )
            if( OR(alpha).vars() >> factor(I).vars() ) {
                _fac2OR.push_back( alpha );
                break;
            }
        DAI_ASSERT( alpha != nrORs() );
    }
    recomputeORs();

    // Create bipartite graph
    _G.construct( nrORs(), nrIRs(), edges.begin(), edges.end() );
}

void RegionGraph::constructCVM( const FactorGraph &fg, const std::vector<VarSet> &cl, size_t verbose ) {
    if( verbose )
        cerr << "constructCVM called (" << fg.nrVars() << " vars, " << fg.nrFactors() << " facs, " << cl.size() << " clusters)" << endl;

    // Retain only maximal clusters
    if( verbose )
        cerr << "  Constructing ClusterGraph" << endl;
    ClusterGraph cg( cl );
    if( verbose )
        cerr << "  Erasing non-maximal clusters" << endl;
    cg.eraseNonMaximal();

    // Create inner regions - first pass
    if( verbose )
        cerr << "  Creating inner regions (first pass)" << endl;
    set<VarSet> betas;
    for( size_t alpha = 0; alpha < cg.nrClusters(); alpha++ )
        for( size_t alpha2 = alpha; (++alpha2) != cg.nrClusters(); ) {
            VarSet intersection = cg.cluster(alpha) & cg.cluster(alpha2);
            if( intersection.size() > 0 )
                betas.insert( intersection );
        }

    // Create inner regions - subsequent passes
    if( verbose )
        cerr << "  Creating inner regions (next passes)" << endl;
    set<VarSet> new_betas;
    do {
        new_betas.clear();
        for( set<VarSet>::const_iterator gamma = betas.begin(); gamma != betas.end(); gamma++ )
            for( set<VarSet>::const_iterator gamma2 = gamma; (++gamma2) != betas.end(); ) {
                VarSet intersection = (*gamma) & (*gamma2);
                if( (intersection.size() > 0) && (betas.count(intersection) == 0) )
                    new_betas.insert( intersection );
            }
        betas.insert(new_betas.begin(), new_betas.end());
    } while( new_betas.size() );

    // Create inner regions - final phase
    if( verbose )
        cerr << "  Creating inner regions (final phase)" << endl;
    vector<Region> irs;
    irs.reserve( betas.size() );
    for( set<VarSet>::const_iterator beta = betas.begin(); beta != betas.end(); beta++ )
        irs.push_back( Region(*beta,0.0) );

    // Create edges
    if( verbose )
        cerr << "  Creating edges" << endl;
    vector<pair<size_t,size_t> > edges;
    for( size_t beta = 0; beta < irs.size(); beta++ )
        for( size_t alpha = 0; alpha < cg.nrClusters(); alpha++ )
            if( cg.cluster(alpha) >> irs[beta] )
                edges.push_back( pair<size_t,size_t>(alpha,beta) );

    // Construct region graph
    if( verbose )
        cerr << "  Constructing region graph" << endl;
    construct( fg, cg.clusters(), irs, edges );

    // Calculate counting numbers
    if( verbose )
        cerr << "  Calculating counting numbers" << endl;
    calcCVMCountingNumbers();

    if( verbose )
        cerr << "Done." << endl;
}

void RegionGraph::calcCVMCountingNumbers() {
    // Calculates counting numbers of inner regions based upon counting numbers of outer regions

    vector<vector<size_t> > ancestors(nrIRs());
    boost::dynamic_bitset<> assigned(nrIRs());
    for( size_t beta = 0; beta < nrIRs(); beta++ ) {
        IR(beta).c() = 0.0;
        for( size_t beta2 = 0; beta2 < nrIRs(); beta2++ )
            if( (beta2 != beta) && IR(beta2) >> IR(beta) )
                ancestors[beta].push_back(beta2);
    }

    bool new_counting;
    do {
        new_counting = false;
        for( size_t beta = 0; beta < nrIRs(); beta++ ) {
            if( !assigned[beta] ) {
                bool has_unassigned_ancestor = false;
                for( vector<size_t>::const_iterator beta2 = ancestors[beta].begin(); (beta2 != ancestors[beta].end()) && !has_unassigned_ancestor; beta2++ )
                    if( !assigned[*beta2] )
                        has_unassigned_ancestor = true;
                if( !has_unassigned_ancestor ) {
                    Real c = 1.0;
                    bforeach( const Neighbor &alpha, nbIR(beta) )
                        c -= OR(alpha).c();
                    for( vector<size_t>::const_iterator beta2 = ancestors[beta].begin(); beta2 != ancestors[beta].end(); beta2++ )
                        c -= IR(*beta2).c();
                    IR(beta).c() = c;
                    assigned.set(beta, true);
                    new_counting = true;
                }
            }
        }
    } while( new_counting );
}

bool RegionGraph::checkCountingNumbers() const {
    // Checks whether the counting numbers satisfy the fundamental relation

    bool all_valid = true;
    for( vector<Var>::const_iterator n = vars().begin(); n != vars().end(); n++ ) {
        Real c_n = 0.0;
        for( size_t alpha = 0; alpha < nrORs(); alpha++ )
            if( OR(alpha).vars().contains( *n ) )
                c_n += OR(alpha).c();
        for( size_t beta = 0; beta < nrIRs(); beta++ )
            if( IR(beta).contains( *n ) )
                c_n += IR(beta).c();
        if( fabs(c_n - 1.0) > 1e-15 ) {
            all_valid = false;
            cerr << "WARNING: counting numbers do not satisfy relation for " << *n << "(c_n = " << c_n << ")." << endl;
        }
    }

    return all_valid;
}

void RegionGraph::recomputeORs() {
    for( size_t alpha = 0; alpha < nrORs(); alpha++ )
        OR(alpha).fill( 1.0 );
    for( size_t I = 0; I < nrFactors(); I++ )
        if( fac2OR(I) != -1U )
            OR( fac2OR(I) ) *= factor( I );
}

void RegionGraph::recomputeORs( const VarSet &ns ) {
    for( size_t alpha = 0; alpha < nrORs(); alpha++ )
        if( OR(alpha).vars().intersects( ns ) )
            OR(alpha).fill( 1.0 );
    for( size_t I = 0; I < nrFactors(); I++ )
        if( fac2OR(I) != -1U )
            if( OR( fac2OR(I) ).vars().intersects( ns ) )
                OR( fac2OR(I) ) *= factor( I );
}

void RegionGraph::recomputeOR( size_t I ) {
    DAI_ASSERT( I < nrFactors() );
    if( fac2OR(I) != -1U ) {
        size_t alpha = fac2OR(I);
        OR(alpha).fill( 1.0 );
        for( size_t J = 0; J < nrFactors(); J++ )
            if( fac2OR(J) == alpha )
                OR(alpha) *= factor( J );
    }
}

/// Send RegionGraph to output stream
ostream & operator << (ostream & os, const RegionGraph & rg) {
    os << "digraph RegionGraph {" << endl;
    os << "node[shape=box];" << endl;
    for( size_t alpha = 0; alpha < rg.nrORs(); alpha++ )
        os << "\ta" << alpha << " [label=\"a" << alpha << ": " << rg.OR(alpha).vars() << ", c=" << rg.OR(alpha).c() << "\"];" << endl;
    os << "node[shape=ellipse];" << endl;
    for( size_t beta = 0; beta < rg.nrIRs(); beta++ )
        os << "\tb" << beta << " [label=\"b" << beta << ": " << (VarSet)rg.IR(beta) << ", c=" << rg.IR(beta).c() << "\"];" << endl;
    for( size_t alpha = 0; alpha < rg.nrORs(); alpha++ )
        bforeach( const Neighbor &beta, rg.nbOR(alpha) )
            os << "\ta" << alpha << " -> b" << beta << ";" << endl;
    os << "}" << endl;
    return os;
}

} // end of namespace dai


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

// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_GET_LEFT_TURNS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_GET_LEFT_TURNS_HPP

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/algorithms/detail/overlay/segment_identifier.hpp>
#include <boost/geometry/algorithms/detail/overlay/turn_info.hpp>
#include <boost/geometry/iterators/closing_iterator.hpp>
#include <boost/geometry/iterators/ever_circling_iterator.hpp>
#include <boost/geometry/strategies/side.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

// TODO: move this to /util/
template <typename T>
inline std::pair<T, T> ordered_pair(T const& first, T const& second)
{
    return first < second ? std::make_pair(first, second) : std::make_pair(second, first);
}

namespace left_turns
{

template <typename Point>
struct turn_angle_info
{
    segment_identifier seg_id;
    int turn_index;
    Point points[2];

    turn_angle_info(segment_identifier const& id, Point const& from, Point const& to)
        : seg_id(id)
        , turn_index(-1)
    {
        points[0] = from;
        points[1] = to;
    }
};

template <typename Point>
struct angle_info
{
    segment_identifier seg_id;
    Point point;
    bool incoming;
    bool blocked;

    inline angle_info(segment_identifier const& id, bool inc, Point const& p)
        : seg_id(id)
        , point(p)
        , incoming(inc)
        , blocked(false)
    {
    }
};

template <typename Vector>
inline int get_quadrant(Vector const& vector)
{
    // Return quadrant as layouted in the code below:
    // 3 | 0
    // -----
    // 2 | 1
    return geometry::get<1>(vector) >= 0
        ? (geometry::get<0>(vector)  < 0 ? 3 : 0)
        : (geometry::get<0>(vector)  < 0 ? 2 : 1)
        ;
}

template <typename Vector>
inline int squared_length(Vector const& vector)
{
    return geometry::get<0>(vector) * geometry::get<0>(vector)
         + geometry::get<1>(vector) * geometry::get<1>(vector)
         ;
}

template <typename Point>
struct angle_less
{
    typedef Point vector_type;
    typedef typename strategy::side::services::default_strategy
    <
        typename cs_tag<Point>::type
    >::type side_strategy_type;

    angle_less(Point const& origin)
        : m_origin(origin)
    {}

    template <typename Angle>
    inline bool operator()(Angle const& p, Angle const& q) const
    {
        // Vector origin -> p and origin -> q
        vector_type pv = p.point;
        vector_type qv = q.point;
        geometry::subtract_point(pv, m_origin);
        geometry::subtract_point(qv, m_origin);

        int const quadrant_p = get_quadrant(pv);
        int const quadrant_q = get_quadrant(qv);
        if (quadrant_p != quadrant_q)
        {
            return quadrant_p < quadrant_q;
        }
        // Same quadrant, check if p is located left of q
        int const side = side_strategy_type::apply(m_origin, q.point,
                    p.point);
        if (side != 0)
        {
            return side == 1;
        }
        // Collinear, check if one is incoming
        if (p.incoming != q.incoming)
        {
            return int(p.incoming) < int(q.incoming);
        }
        // Same quadrant/side/direction, return longest first
        int const length_p = squared_length(pv);
        int const length_q = squared_length(qv);
        if (length_p != length_q)
        {
            return squared_length(pv) > squared_length(qv);
        }
        // They are still the same. Just compare on seg_id
        return p.seg_id < q.seg_id;
    }

private:
    Point m_origin;
};

template <typename Point>
struct angle_equal_to
{
    typedef Point vector_type;
    typedef typename strategy::side::services::default_strategy
    <
        typename cs_tag<Point>::type
    >::type side_strategy_type;

    inline angle_equal_to(Point const& origin)
        : m_origin(origin)
    {}

    template <typename Angle>
    inline bool operator()(Angle const& p, Angle const& q) const
    {
        // Vector origin -> p and origin -> q
        vector_type pv = p.point;
        vector_type qv = q.point;
        geometry::subtract_point(pv, m_origin);
        geometry::subtract_point(qv, m_origin);

        if (get_quadrant(pv) != get_quadrant(qv))
        {
            return false;
        }
        // Same quadrant, check if p/q are collinear
        int const side = side_strategy_type::apply(m_origin, q.point,
                    p.point);
        return side == 0;
    }

private:
    Point m_origin;
};

struct left_turn
{
    segment_identifier from;
    segment_identifier to;
};

template <typename Point, typename AngleCollection, typename OutputCollection>
inline void get_left_turns(AngleCollection const& sorted_angles, Point const& origin,
        OutputCollection& output_collection)
{
    angle_equal_to<Point> comparator(origin);
    typedef geometry::closing_iterator<AngleCollection const> closing_iterator;
    closing_iterator it(sorted_angles);
    closing_iterator end(sorted_angles, true);

    closing_iterator previous = it++;
    for( ; it != end; previous = it++)
    {
        if (! it->blocked)
        {
            bool equal = comparator(*previous, *it);
            bool include = ! equal
                && previous->incoming
                && !it->incoming;
            if (include)
            {
                left_turn turn;
                turn.from = previous->seg_id;
                turn.to = it->seg_id;
                output_collection.push_back(turn);
            }
        }
    }
}

template <typename AngleTurnCollection, typename AngleCollection>
inline void block_turns_on_right_sides(AngleTurnCollection const& turns,
        AngleCollection& sorted)
{
    // Create a small (seg_id -> index) map for fast finding turns
    std::map<segment_identifier, int> incoming;
    std::map<segment_identifier, int> outgoing;
    int index = 0;
    for (typename boost::range_iterator<AngleCollection>::type it =
        sorted.begin(); it != sorted.end(); ++it, ++index)
    {
        if (it->incoming)
        {
            incoming[it->seg_id] = index;
        }
        else
        {
            outgoing[it->seg_id] = index;
        }
    }

    // Walk through turns and block every outgoing angle on the right side
    for (typename boost::range_iterator<AngleTurnCollection const>::type it =
        turns.begin(); it != turns.end(); ++it)
    {
        int incoming_index = incoming[it->seg_id];
        int outgoing_index = outgoing[it->seg_id];
        int index = incoming_index;
        while(index != outgoing_index)
        {
            if (!sorted[index].incoming)
            {
                sorted[index].blocked = true;
            }

            // Go back (circular)
            index--;
            if (index == -1)
            {
                index = boost::size(sorted) - 1;
            }
        }
    }
}

template <typename AngleCollection, typename Point>
inline bool has_rounding_issues(AngleCollection const& angles, Point const& origin)
{
    for (typename boost::range_iterator<AngleCollection const>::type it =
        angles.begin(); it != angles.end(); ++it)
    {
        // Vector origin -> p and origin -> q
        typedef Point vector_type;
        vector_type v = it->point;
        geometry::subtract_point(v, origin);
        return geometry::math::abs(geometry::get<0>(v)) <= 1
            || geometry::math::abs(geometry::get<1>(v)) <= 1
            ;
    }
    return false;
}

}  // namespace left_turns

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_GET_LEFT_TURNS_HPP


/*=========================================================================

  Class derived from VTK's vtkPolyDataAlgorithm by Benjamin Bercovici

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/**
  @class  SBGATSrpYorp
  @author Benjamin Bercovici
  @author Jay McMahon
@date October 2018

  @brief  Computation of Fourier decomposition of force/torques caused by SRP over a shape model

  @details Wrapper around YORPLib to allow VTK's Polydata to be utilized as shape containers.
 see Scheeres, D. J. (2007). `The dynamical evolution of uniformly rotating asteroids subject to YORP`. Icarus, 188(2), 430–450. https://doi.org/10.1016/j.icarus.2006.12.015
 for more details
@copyright MIT License, Benjamin Bercovici and Jay McMahon

*/

#ifndef SBGATSrpYorp_HEADER
#define SBGATSrpYorp_HEADER

#include <vtkFiltersCoreModule.h> // For export macro
#include <vtkPolyDataAlgorithm.h>
#include <armadillo>

class VTKFILTERSCORE_EXPORT SBGATSrpYorp : public vtkPolyDataAlgorithm{
public:

  /**
   * Constructs with initial values of zero.
   */

  static SBGATSrpYorp *New();

  vtkTypeMacro(SBGATSrpYorp,vtkPolyDataAlgorithm);
  void PrintSelf(std::ostream& os, vtkIndent indent) override;
  void PrintHeader(std::ostream& os, vtkIndent indent) override;
  void PrintTrailer(std::ostream& os, vtkIndent indent) override;

  /**
  Setter to the longitude step. default is 1 deg
  @param lambdaDel longitude step (degrees)
  */
  void set_lambdaDel(double lambdaDel);

  /**
  Setter to the declination step. default is 1 deg
  @param deltaDel declination step (degrees)
  */
  void set_deltaDel(double deltaDel);

  /**
  Setter to the maximum order of the Fourier decomposition. default is 2
  @param maxFourier maximum decomposition order
  */
  void set_maxFourier(int maxFourier);

  /**
  Setter to numrefine. default is 5
  @param numrefine
  */
  void set_numrefine(int numrefine);

  /**
  Setter to number of voxel per axis. default is 40
  @param numVox voxel pex axis
  */
  void set_numVox(int numVox);

  /**
  Setter to reflectivity
  @param rho
  */
  void set_rho(double rho);

   /**
  Setter to specular component
  @param spec
  */
  void set_spec(double spec);

  /**
  Setter to number of ray "bounces" to be accounted for. default is 3
  @param howManyBounces number of ray reflections to keep track of
  */
  void set_howManyBounces(int howManyBounces);

  /**
  Setter to directory where results are stored
  @param outputFileBaseName save directory
  */
  void set_outputFileBaseName(std::string outputFileBaseName);

protected:
  SBGATSrpYorp();
  ~SBGATSrpYorp() override;

  int RequestData(vtkInformation* request,
    vtkInformationVector** inputVector,
    vtkInformationVector* outputVector) override;

  std::string outputFileBaseName;
  int numVox;
  double rho, spec;
  double lambdaDel;
  double deltaDel;
  double maxFourier;
  int howManyBounces;
  int numrefine;

private:
  SBGATSrpYorp(const SBGATSrpYorp&) = delete;
  void operator=(const SBGATSrpYorp&) = delete;
};

#endif


#include "GltfUtil.h"
#include "SimpleMesh.h"
#include "Sphere.h"
#include "VoxelGrid.h"
#include "mathkit.h"
#include <Eigen/Core>
#include <bobyqa.h>
#include <chrono>
#include <fmt/format.h>
#include <fstream>
#include <iomanip>
#include <json.hpp>
#include <random>
#include <spherical_harmonics.h>
#include <unordered_set>
#include <vector>

#ifdef WIN32
#define NOMINMAX
#include <direct.h>
#include <windows.h>
#else
#include <unistd.h>
#endif

struct SphericalHarmonics {
    vec3 L00 {};
    vec3 L11 {};
    vec3 L10 {};
    vec3 L1_1 {};
    vec3 L21 {};
    vec3 L2_1 {};
    vec3 L2_2 {};
    vec3 L20 {};
    vec3 L22 {};
};

struct SphereSet {
    SphereSet(const SimpleMesh& mesh)
        : mesh(mesh)
    {
    }

    std::vector<Sphere> spheres;
    std::vector<SphericalHarmonics> sphereSH;

    const SimpleMesh& mesh;

    struct {
        std::unique_ptr<VoxelGrid> shell;
        std::unique_ptr<VoxelGrid> filled;
        std::unique_ptr<VoxelGrid> inside;
    } grids;
};

struct Triangle {
public:
    Triangle(vec3 v0, vec3 v1, vec3 v2)
    {
        v[0] = v0;
        v[1] = v1;
        v[2] = v2;

        vec3 crossP = cross(v1 - v0, v2 - v0);
        area = length(crossP) / 2.0f;
        normal = normalize(crossP);
    }

    vec3 v[3];
    vec3 normal;
    float area;
};

vec3 getRandomSphereLocationInsideMesh(const VoxelGrid& grid, std::default_random_engine& generator)
{
    aabb3 bounds = grid.gridBounds();
    std::uniform_real_distribution<float> uniformInGridX { bounds.min.x, bounds.max.x };
    std::uniform_real_distribution<float> uniformInGridY { bounds.min.y, bounds.max.y };
    std::uniform_real_distribution<float> uniformInGridZ { bounds.min.z, bounds.max.z };

    vec3 location;
    ivec3 gridCoord;

    do {
        location = { uniformInGridX(generator),
                     uniformInGridY(generator),
                     uniformInGridZ(generator) };
        gridCoord = grid.remapToGridSpace(location, std::round);
    } while (grid.get(gridCoord) == 0);

    return location;
}

void assignSpheresRandomlyInVolume(SphereSet& set, unsigned numSpheres)
{
    auto& grid = *set.grids.inside;
    assert(numSpheres <= grid.numFilledVoxels());

    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::default_random_engine generator { seed };

    for (size_t i = 0; i < numSpheres; ++i) {
        vec3 location = getRandomSphereLocationInsideMesh(grid, generator);
        Sphere sphere { location, 0.0 };
        set.spheres.push_back(sphere);
    }

    aabb3 bounds = grid.gridBounds();
    std::uniform_real_distribution<float> uniformInGridX { bounds.min.x, bounds.max.x };
    std::uniform_real_distribution<float> uniformInGridY { bounds.min.y, bounds.max.y };
    std::uniform_real_distribution<float> uniformInGridZ { bounds.min.z, bounds.max.z };

    while (set.spheres.size() < numSpheres) {

        float x = uniformInGridX(generator);
        float y = uniformInGridY(generator);
        float z = uniformInGridZ(generator);

        ivec3 gridCoord = grid.remapToGridSpace({ x, y, z }, std::round);
        if (grid.get(gridCoord) > 0) {
            Sphere sphere { vec3(x, y, z), 0.0f };
            set.spheres.push_back(sphere);
        }
    }
}

bool sphereTriangleIntersection(const Sphere& sphere, const vec3& v0, const vec3& v1, const vec3& v2)
{
    // From https://realtimecollisiondetection.net/blog/?p=103

    vec3 P = sphere.origin;
    float r = sphere.radius;
    float rr = r * r;

    vec3 A = v0 - P;
    vec3 B = v1 - P;
    vec3 C = v2 - P;

    vec3 V = cross(B - A, C - A);
    float d = dot(A, V);
    float e = dot(V, V);
    bool sep1 = d * d > rr * e;

    float aa = dot(A, A);
    float ab = dot(A, B);
    float ac = dot(A, C);
    float bb = dot(B, B);
    float bc = dot(B, C);
    float cc = dot(C, C);

    bool sep2 = (aa > rr) & (ab > aa) & (ac > aa);
    bool sep3 = (bb > rr) & (ab > bb) & (bc > bb);
    bool sep4 = (cc > rr) & (ac > cc) & (bc > cc);

    vec3 AB = B - A;
    vec3 BC = C - B;
    vec3 CA = A - C;

    float d1 = ab - aa;
    float d2 = bc - bb;
    float d3 = ac - cc;

    float e1 = dot(AB, AB);
    float e2 = dot(BC, BC);
    float e3 = dot(CA, CA);

    vec3 Q1 = A * e1 - d1 * AB;
    vec3 Q2 = B * e2 - d2 * BC;
    vec3 Q3 = C * e3 - d3 * CA;

    vec3 QC = C * e1 - Q1;
    vec3 QA = A * e2 - Q2;
    vec3 QB = B * e3 - Q3;

    bool sep5 = (dot(Q1, Q1) > rr * e1 * e1) && (dot(Q1, QC) > 0);
    bool sep6 = (dot(Q2, Q2) > rr * e2 * e2) && (dot(Q2, QA) > 0);
    bool sep7 = (dot(Q3, Q3) > rr * e3 * e3) && (dot(Q3, QB) > 0);

    bool separated = sep1 | sep2 | sep3 | sep4 | sep5 | sep6 | sep7;
    return !separated;
}

bool sphereInsideMeshVolume(const Sphere& sphere, const SimpleMesh& mesh)
{
    // NOTE: Implicit, but this assumes that the mesh is one solid blob with no islands!

    // NOTE: To avoid expensive(/not-implemented) checks to see if the sphere centers are inside the volume
    //  of the object, we will assume they are and through other means verify that it will be invariant. The
    //  other means are: 1) spawning points inside the volume, and 2) always making steps small enough that
    //  we wont leave the volume when optimizing.

    if (sphere.radius == 0.0) {
        return true;
    }

    volatile bool result = true;
    #pragma omp parallel for shared(result)
    for (int i = 0; i < mesh.triangleCount(); ++i) {
        if (result) {
            // TODO: Maybe don't reconstruct these every query?!
            vec3 v0, v1, v2;
            mesh.triangle(i, v0, v1, v2);

            if (sphereTriangleIntersection(sphere, v0, v1, v2)) {
                result = false;
            }
        }
    }

    return result;
}

bool sphereSetInsideMeshVolume(const SphereSet& set, const SimpleMesh& mesh)
{
    for (const Sphere& sphere : set.spheres) {
        if (!sphereInsideMeshVolume(sphere, mesh)) {
            return false;
        }
    }
    return true;
}

void expandSpheresMaximally(SphereSet& set)
{
    aabb3 gridBounds = set.grids.shell->gridBounds();
    double gridDiameter = distance(gridBounds.min, gridBounds.max);

    #pragma omp parallel for
    for (int i = 0; i < set.spheres.size(); ++i) {
        Sphere& sphere = set.spheres[i];

        Sphere minSphere { sphere.origin, 0.0 };
        Sphere maxSphere { sphere.origin, gridDiameter };

        assert(sphereInsideMeshVolume(minSphere, set.mesh));
        assert(!sphereInsideMeshVolume(maxSphere, set.mesh));

        while (abs(maxSphere.radius - minSphere.radius) > 1e-8) {

            double midpoint = (minSphere.radius + maxSphere.radius) / 2.0;
            Sphere middleSphere { sphere.origin, midpoint };

            bool midOk = sphereInsideMeshVolume(middleSphere, set.mesh);
            if (midOk) {
                minSphere = middleSphere;
            } else {
                maxSphere = middleSphere;
            }
        }

        // Make sure to be conservative with the choice to we don't break the invariant
        sphere.radius = minSphere.radius;
        assert(sphereInsideMeshVolume(sphere, set.mesh));
    }

#if 0
    std::sort(set.spheres.begin(), set.spheres.end(), [](const Sphere& lhs, const Sphere& rhs) {
        return lhs.radius > rhs.radius;
    });
    fmt::print("   grid 'radius' {}\n", gridDiameter / 2.0);
    for (auto& sphere : set.spheres) {
        fmt::print("     sphere radius {}\n", sphere.radius);
    }
#endif
}

double sphereSetIntersectionVolumeOverestimation(const SphereSet& set)
{
    // Except some special cases there are no analytical solutions for volume intersection of many spheres. See
    // https://www.researchgate.net/publication/289097700_Volume_of_intersection_of_six_spheres_A_special_case_of_practical_interest
    // for some more information. In this case we don't really care about the exact intersection, but we do want to avoid as much
    // intersection as possible, so just overcounting it works here, I think..

    double intersection = 0.0;

    size_t sphereCount = set.spheres.size();
    for (size_t i = 0; i < sphereCount; ++i) {
        const Sphere& self = set.spheres[i];
        for (size_t k = i + 1; k < sphereCount; ++k) {
            const Sphere& other = set.spheres[k];
            intersection += self.overlapWith(other);
        }
    }

    return intersection;
}

double sphereSetUnionVolume(const SphereSet& set)
{
    double volume = 0.0;

    size_t sphereCount = set.spheres.size();
    for (const Sphere& sphere : set.spheres) {
        volume += sphere.volume();
    }

    // TODO: Maybe scale down the intersection a bit, since we know it overestimates?
    double intersectionOverestimation = sphereSetIntersectionVolumeOverestimation(set);
    volume -= 0.25 * intersectionOverestimation;

    return volume;
}

struct SphereFittingData {
    SphereSet* set;
    unsigned sphereIndex;
};

REAL sphereFittingObjectiveFunction(const INTEGER n, const REAL* x, void* dataPtr)
{
    assert(n == 4);

    Sphere testSphere;
    testSphere.radius = x[3];
    testSphere.origin = { x[0], x[1], x[2] };

    SphereFittingData& data = *reinterpret_cast<SphereFittingData*>(dataPtr);
    SphereSet& set = *data.set;

    // Temporarily swap in the sphere to test
    Sphere currentSphere = set.spheres[data.sphereIndex];
    set.spheres[data.sphereIndex] = testSphere;

    if (!sphereInsideMeshVolume(testSphere, set.mesh)) {
        return 99999.99; // BOBYQA doesn't seem to like infinities, but this will do
    }

    // Since BOBYQA solves a minimization problem we want to minimize
    // the *negative* volume to get a maximal volume solution.
    double metric = -sphereSetUnionVolume(set);

    // Replace the current sphere back into the sphere set
    set.spheres[data.sphereIndex] = currentSphere;

    return metric;
}

void sphereFitting(SphereSet& set)
{
    // To avoid fitting the spheres in the same order every time.
    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::shuffle(set.spheres.begin(), set.spheres.end(), std::default_random_engine(seed));

    for (size_t sphereIdx = 0; sphereIdx < set.spheres.size(); ++sphereIdx) {

        Sphere& sphere = set.spheres[sphereIdx];
        assert(sphere.radius > 0.0);

        // Origin (3) + radius (1)
        constexpr INTEGER n = 3 + 1;

        // NPT is the number of interpolation conditions.  Its value must be in the interval
        // [N+2,(N+1)(N+2)/2].  Choices that exceed 2*N+1 are not recommended.
        // TODO: Find a good value!
#if 0
        constexpr INTEGER npt = n + 2; // (6)
#else
        constexpr INTEGER npt = (n + 1) * (n + 2) / 2; // (15)
#endif

        REAL x[n] = {
            sphere.origin.x,
            sphere.origin.y,
            sphere.origin.z,
            sphere.radius
        };

        // To inscribe a box = {min, max} in a sphere we want to find the min & max
        // points which lie along the diagonal that goes through the box center.
        //
        //  sqrt(x^2 + y^2 + y^3) = r
        //   since diagonal -> x=y=z = d
        //             sqrt(3d^2) = r
        //                   3d^2 = r^2
        //                      d = sqrt(r^2 / 3)
        //
        double d = std::sqrt(sphere.radius * sphere.radius / 3.0);
        vec3 boxDiagonal = vec3(d, d, d) * 0.99f; // (avoid overstepping!)
        aabb3 boxInSphere { sphere.origin - boxDiagonal, sphere.origin + boxDiagonal };

        REAL xLower[n] = {
            boxInSphere.min.x,
            boxInSphere.min.y,
            boxInSphere.min.z,
            1e-8
        };

        aabb3 gridBounds = set.grids.shell->gridBounds();
        double gridMaxDistance = length(gridBounds.max - gridBounds.min);

        REAL xUpper[n] = {
            boxInSphere.max.x,
            boxInSphere.max.y,
            boxInSphere.max.z,
            gridMaxDistance
        };

        // "Typically, RHOBEG should be about one tenth of the greatest expected change to a variable"
        // TODO: Find good value!
        REAL rhoBeg = (2.0 * d) / 10.0;

        // RHOEND should indicate the accuracy that is required in the final values of the variables
        // NOTE: Some models are tiny, others are huge.. So an absolute/constant accuracy is probably
        //  not very helpful here. A fraction of the sphere radius could maybe work as a scale-aware metric?
        // TODO: Find good value!
        REAL rhoEnd = gridMaxDistance * 1e-8;

        //The array W will be used for working space.  Its length must be at least
        // (NPT+5)*(NPT+N)+3*N*(N+5)/2.  Upon successful return, the first element of W
        // will be set to the function value at the solution. */
        REAL workingMemory[(npt + 5) * (npt + n) + 3 * n * (n + 5) / 2];

        INTEGER logLevel = 0;
        INTEGER maxObjFunCalls = 10'000;

        SphereFittingData data { &set, sphereIdx };

        int status = bobyqa(n, npt, sphereFittingObjectiveFunction, (void*)(&data),
                            x, xLower, xUpper, rhoBeg, rhoEnd,
                            logLevel, maxObjFunCalls, workingMemory);

        switch (status) {
        case BOBYQA_SUCCESS: {
            // algorithm converged!
            vec3 newOrigin { x[0], x[1], x[2] };
            double newRadius = x[3];

#if 0
            fmt::print("      - ({}, {}, {}) r={}\n", newOrigin.x, newOrigin.y, newOrigin.z, newRadius);

            vec3 diffOrigin = newOrigin - sphere.origin;
            double diffRadius = newRadius - sphere.radius;
            //fmt::print("      - ({}, {}, {}) r={}\n", diffOrigin.x, diffOrigin.y, diffOrigin.z, diffRadius);
            //assert(length(diffOrigin) > 1e-6 && abs(diffRadius > 1e-6));
#endif

            sphere.origin = newOrigin;
            sphere.radius = newRadius;
            return;
        }
        case BOBYQA_BAD_NPT:
            fmt::print("bobyqa error: NPT is not in the required interval\n");
            assert(false); // npt is hardcoded to a valid value!
            break;
        case BOBYQA_TOO_CLOSE:
            fmt::print("bobyqa error: insufficient space between the bounds\n");
            break;
        case BOBYQA_ROUNDING_ERRORS:
            fmt::print("bobyqa error: too much cancellation in a denominator\n");
            break;
        case BOBYQA_TOO_MANY_EVALUATIONS:
            fmt::print("bobyqa error: maximum number of function evaluations exceeded\n");
            break;
        case BOBYQA_STEP_FAILED:
            fmt::print("bobyqa error: a trust region step has failed to reduce Q\n");
            break;
        default:
            assert(false);
        }
    }
}

vec3 randomPointInSphere(const Sphere& sphere)
{
    std::uniform_real_distribution<double> randomOffset { -sphere.radius, +sphere.radius };

    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::default_random_engine generator { seed };

    vec3 offset;
    do {
        offset = { randomOffset(generator), randomOffset(generator), randomOffset(generator) };
    } while (length(offset) > sphere.radius);

    return sphere.origin + offset;
}

void sphereTeleportation(SphereSet& set)
{
    if (set.spheres.size() == 1) {
        // TODO: Maybe pick a new starting point?
        // Or let the teleportation fail like this and restart from the main loop?
        return;
    }

    std::sort(set.spheres.begin(), set.spheres.end(), [](const Sphere& lhs, const Sphere& rhs) {
        return lhs.radius < rhs.radius;
    });

    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::default_random_engine generator { seed };

    size_t numToReplace = std::ceil(set.spheres.size() / 8.0);
    for (size_t i = 0; i < numToReplace; ++i) {

        Sphere& sphere = set.spheres[i];
        sphere.origin = getRandomSphereLocationInsideMesh(*set.grids.inside, generator);

        // NOTE: A sphere expansion will happen after this is done, so we don't need to take care of that now
        sphere.radius = 0.0;
    }
}

double aabbVolume(const aabb3& aabb)
{
    glm::vec<3, double> dims = aabb.max - aabb.min;
    return dims.x * dims.y * dims.z;
}

std::optional<vec3> triangleRayIntersection(vec3 v0, vec3 v1, vec3 v2, vec3 rayOrigin, vec3 rayDirection, float& outT)
{
    // From GraphicsCodex

    const vec3& e1 = v1 - v0;
    const vec3& e2 = v2 - v0;

    vec3 N = normalize(cross(e1, e2));

    vec3 q = cross(rayDirection, e2);
    float a = dot(e1, q);

    // Close to the limit of precision?
    if (fabsf(a) <= 1e-4f) {
        return {};
    }

    const vec3& s = (rayOrigin - v0) / a;
    const vec3& r = cross(s, e1);

    // Barycentric coordinates
    float b[3];
    b[0] = dot(s, q);
    b[1] = dot(r, rayDirection);
    b[2] = 1.0f - b[0] - b[1];

    // Intersected inside triangle?
    float t = dot(e2, r);
    if ((b[0] >= 0) && (b[1] >= 0) && (b[2] >= 0) && t >= 0) {

        vec3 hitPoint = rayOrigin + t * rayDirection;

        // TODO: This worked on a previous project... What's up with the order though?
        vec3 barycentric { b[2], b[0], b[1] };

        outT = t;
        return barycentric;
    }

    return {};
}

vec3 raytraceMesh(const SimpleMesh& mesh, vec3 o, vec3 d)
{
    float closestT = INFINITE;
    int64_t closestTriangle = -1;
    vec3 closestBarycentric {};

    for (size_t ti = 0; ti < mesh.triangleCount(); ++ti) {

        vec3 v0, v1, v2;
        mesh.triangle(ti, v0, v1, v2);

        float t;
        std::optional<vec3> bary = triangleRayIntersection(v0, v1, v2, o, d, t);

        if (bary.has_value() && t < closestT) {
            closestBarycentric = bary.value();
            closestTriangle = ti;
            closestT = t;
        }
    }

    if (closestTriangle == -1) {
        return vec3(0, 0, 0);
    }

    vec2 uv0, uv1, uv2;
    mesh.triangleTexcoords(closestTriangle, uv0, uv1, uv2);

    vec3 b = closestBarycentric;
    vec2 uv = b[0] * uv0 + b[1] * uv1 + b[2] * uv2;

    assert(mesh.hasTexture());
    vec3 color = mesh.texture().sample(uv);

    return color;
}

SphericalHarmonics generateSphericalHarmonics(vec3 sampleCenter, size_t numSamples, const SimpleMesh& mesh)
{

    auto rgbFunction = [&](double phi, double theta) -> vec3 {
        Eigen::Vector3d v = sh::ToVector(phi, theta);
        vec3 direction = vec3(v.x(), v.y(), v.z());
        vec3 Lsrgb = raytraceMesh(mesh, sampleCenter, direction);
        return pow(Lsrgb, vec3(2.2f));
    };

    std::vector<double> r = *sh::ProjectFunction(
        2, [&](double phi, double theta) -> double {
            vec3 color = rgbFunction(phi, theta);
            return color.r;
        },
        numSamples);
    std::vector<double> g = *sh::ProjectFunction(
        2, [&](double phi, double theta) -> double {
            vec3 color = rgbFunction(phi, theta);
            return color.g;
        },
        numSamples);
    std::vector<double> b = *sh::ProjectFunction(
        2, [&](double phi, double theta) -> double {
            vec3 color = rgbFunction(phi, theta);
            return color.b;
        },
        numSamples);

    assert(r.size() == 9);
    assert(g.size() == 9);
    assert(b.size() == 9);

    SphericalHarmonics sh;

    sh.L00 = vec3(r[0], g[0], b[0]);

    sh.L1_1 = vec3(r[1], g[1], b[1]);
    sh.L10 = vec3(r[2], g[2], b[2]);
    sh.L11 = vec3(r[3], g[3], b[3]);

    sh.L2_2 = vec3(r[4], g[4], b[4]);
    sh.L2_1 = vec3(r[5], g[5], b[5]);
    sh.L20 = vec3(r[6], g[6], b[6]);
    sh.L21 = vec3(r[7], g[7], b[7]);
    sh.L22 = vec3(r[8], g[8], b[8]);

    return sh;
}

std::vector<SphericalHarmonics> generateSphericalHarmonicsForSphereColors(const SphereSet& set, size_t numSamples)
{
    std::vector<SphericalHarmonics> SHs;

    for (const Sphere& sphere : set.spheres) {
        SphericalHarmonics sh = generateSphericalHarmonics(sphere.origin, numSamples, set.mesh);
        SHs.push_back(sh);
    }

    return SHs;
}

void generateJsonOutput(const SphereSet& sphereSet, const std::string& outPath)
{
    using namespace nlohmann;

    json j;
    j["proxy"] = "sphere-set";
    j["spheres"] = {};

    for (size_t i = 0; i < sphereSet.spheres.size(); ++i) {

        const Sphere& sphere = sphereSet.spheres[i];
        const SphericalHarmonics& sh = sphereSet.sphereSH[i];

        json jsonSphere = {
            { "center", { sphere.origin.x, sphere.origin.y, sphere.origin.z } },
            { "radius", sphere.radius },
            { "sh",
              { { "L00", { sh.L00.x, sh.L00.y, sh.L00.z } },
                { "L1_1", { sh.L1_1.x, sh.L1_1.y, sh.L1_1.z } },
                { "L10", { sh.L10.x, sh.L10.y, sh.L10.z } },
                { "L11", { sh.L11.x, sh.L11.y, sh.L11.z } },
                { "L2_2", { sh.L2_2.x, sh.L2_2.y, sh.L2_2.z } },
                { "L2_1", { sh.L2_1.x, sh.L2_1.y, sh.L2_1.z } },
                { "L20", { sh.L20.x, sh.L20.y, sh.L20.z } },
                { "L21", { sh.L21.x, sh.L21.y, sh.L21.z } },
                { "L22", { sh.L22.x, sh.L22.y, sh.L22.z } } } }
        };
        j["spheres"].push_back(jsonSphere);
    }

    std::ofstream outstream(outPath);
    outstream << std::setw(4) << j << std::endl;
}

void generateOutput(const SphereSet& sphereSet, bool doPrint = true)
{
    size_t sphereCount = sphereSet.spheres.size();

    std::string result = "const vec4 spheres[] = vec4[](\n";
    for (size_t i = 0; i < sphereCount; ++i) {
        const Sphere& sphere = sphereSet.spheres[i];
        std::string line = fmt::format("\tvec4({}, {}, {}, {}){}\n",
                                       sphere.origin.x, sphere.origin.y, sphere.origin.z,
                                       sphere.radius, (i + 1 == sphereCount) ? "" : ",");
        result += line;
    }
    result += ");\n";

    if (doPrint) {
        fmt::print(result);
    }

#ifdef WIN32
    OpenClipboard(0);
    EmptyClipboard();
    HGLOBAL hg = GlobalAlloc(GMEM_MOVEABLE, result.size() + 1);
    if (!hg) {
        CloseClipboard();
        return;
    }
    memcpy(GlobalLock(hg), result.c_str(), result.size());
    GlobalUnlock(hg);
    SetClipboardData(CF_TEXT, hg);
    CloseClipboard();
    GlobalFree(hg);
#endif
}

void setApplicationWorkingDirectory(char* executableName, const std::string& workingDir)
{
#ifdef WIN32
    char fullPathBuf[_MAX_PATH] = {};
    assert(_fullpath(fullPathBuf, executableName, sizeof(fullPathBuf)));
    std::string fullPath { fullPathBuf };

    size_t startOfWorkingDirName = fullPath.find(workingDir);
    std::string newWorkingDir = fullPath.substr(0, startOfWorkingDirName + workingDir.length() + 1);
    assert(_chdir(newWorkingDir.c_str()) == 0);
#else
    char fullPathBuf[PATH_MAX] = {};
    assert(realpath(executableName, fullPathBuf));
    std::string fullPath { fullPathBuf };

    size_t startOfWorkingDirName = fullPath.find(workingDir);
    std::string newWorkingDir = fullPath.substr(0, startOfWorkingDirName + workingDir.length() + 1);
    assert(chdir(newWorkingDir.c_str()) == 0);
#endif
}

int main(int argc, char** argv)
{
    setApplicationWorkingDirectory(argv[0], "ProxyGen");

    std::string path = "assets/Barrel/barrel.gltf";
    std::string outPath = "assets/Barrel/barrel_spheres.json";

    constexpr unsigned numSpheres = 2;
    const size_t gridDimensions = 126;

    constexpr size_t numShSamples = 4096;

    fmt::print("=> loading model '{}'\n", path);
    auto [basePath, model] = GltfUtil::loadModel(path);
    std::vector<SimpleMesh> simpleMeshes {};
    GltfUtil::bakeDownModelToSimpleMeshes(model, basePath, simpleMeshes);
    if (simpleMeshes.size() > 1) {
        fmt::print("==> model has more than one meshes; only the first will be used!\n");
    }

    const SimpleMesh& mesh = simpleMeshes[0];
    SphereSet set { mesh };

    fmt::print("=> creating voxel grids\n");

    aabb3 meshBounds { vec3(+INFINITY), vec3(-INFINITY) };
    mesh.extendAABB(meshBounds.min, meshBounds.max);

    set.grids.shell = std::make_unique<VoxelGrid>(glm::ivec3(gridDimensions), meshBounds);
    set.grids.shell->insertMesh(mesh, true);
    set.grids.shell->quantizeColors(10);

    set.grids.filled = std::make_unique<VoxelGrid>(*set.grids.shell);
    set.grids.filled->fillVolumes();

    set.grids.inside = std::make_unique<VoxelGrid>(*set.grids.filled);
    set.grids.inside->subtractGrid(*set.grids.shell);

    fmt::print("=> assigning initial spheres\n");
    assignSpheresRandomlyInVolume(set, numSpheres);

    fmt::print("=> optimizing begin\n");

    bool didJustTeleport = false;
    double previousVolume = -std::numeric_limits<double>::infinity();

    double bestVolume = std::numeric_limits<double>::min();
    std::vector<Sphere> bestSolution {};

    constexpr int maxConsecutiveReverts = 3;
    int numConsecutiveReverts = 0;

    constexpr int maxIterations = 10'000;
    int iteration = 0;

    for (; iteration < maxIterations; ++iteration) {

        fmt::print("==> expanding spheres\n");
        expandSpheresMaximally(set);
        double newVolume = sphereSetUnionVolume(set);
        fmt::print("   volume: {}\n", newVolume);

        if (newVolume > bestVolume) {
            bestVolume = newVolume;
            bestSolution = set.spheres;
        }

        if (newVolume > previousVolume) {

            fmt::print("===> sphere fitting\n");
            sphereFitting(set);

            didJustTeleport = false;
            numConsecutiveReverts = 0;

        } else {

            if (didJustTeleport) {
                fmt::print("==> teleportation failed to increase volume, ");
                if (numConsecutiveReverts < maxConsecutiveReverts) {
                    fmt::print("reverting to known good solution\n");
                    set.spheres = bestSolution;
                    numConsecutiveReverts += 1;
                    //continue; (no, we have to teleport from this new stage first!)
                } else {
                    fmt::print("and max reverts reached, so aborting\n");
                    break;
                }
            }

            fmt::print("===> sphere teleportation\n");
            sphereTeleportation(set);
            didJustTeleport = true;
        }

        previousVolume = newVolume;

        if (iteration % 10 == 0) {
            double itDone = 100.0 * double(iteration) / double(maxIterations);
            fmt::print("==> {} iterations done ({:.1f}%)\n", iteration, itDone);
            generateOutput(set, false);
        }
    }

    if (iteration == maxIterations) {
        fmt::print("==> max iterations reached\n");
    }

    if (numConsecutiveReverts == maxConsecutiveReverts) {
        fmt::print("==> max consectutive reverts reached\n");
    }

    fmt::print("=> optimizing done\n");

    fmt::print("=> generating sphere output with volume {} (AABB volume {})\n", bestVolume, aabbVolume(meshBounds));
    set.spheres = bestSolution;

    fmt::print("=> generating SH color representations for spheres ({} samples)\n", numShSamples);
    std::vector<SphericalHarmonics> SHs = generateSphericalHarmonicsForSphereColors(set, numShSamples);
    set.sphereSH = SHs;

    generateOutput(set);
    generateJsonOutput(set, outPath);
}


/*
 * optimize_tnc.cpp
 *
 *  Created on: Feb 9, 2010
 *      Author: smitty
 */

#include "optimize_state_reconstructor_periods_nlopt.h"
#include <iostream>
#include <stdio.h>
#include <nlopt.hpp>
#include <math.h>
#include <vector>

#include "state_reconstructor.h"
#include "rate_model.h"

#include <armadillo>
using namespace arma;

StateReconstructor * nloptsr_periods;
vector<RateModel> * nloptrm_periods;
vector<mat> * nloptfree_variables_periods;

//double nlopt_sr(int n, const double *x, void *state) {
//double nlopt_sr(unsigned n, const double *x, double *grad, void *my_func_data);
double nlopt_sr_periods(unsigned n, const double *x, double *grad, void *my_func_data) {
    for (unsigned int k=0; k < nloptfree_variables_periods->size(); k++) {
        for (unsigned int i=0; i < (*nloptfree_variables_periods)[k].n_rows; i++) {
            for (unsigned int j=0; j < (*nloptfree_variables_periods)[k].n_cols; j++) {
                if (i != j) {
                    (*nloptrm_periods)[k].set_Q_cell(i,j,x[int(nloptfree_variables_periods->at(k)(i,j))]);
                    if ((*nloptrm_periods)[k].get_Q()(i,j) < 0 || (*nloptrm_periods)[k].get_Q()(i,j) >= 1000) {
                        return 1000000000000;
                    }
                }
            }
        }
    }
    double like;
    for (unsigned int i=0; i < nloptrm_periods->size(); i++) {
        nloptrm_periods->at(i).set_Q_diag();
    }
    like = nloptsr_periods->eval_likelihood();
    for (unsigned int i=0; i < nloptrm_periods->size(); i++) {
        if (nloptrm_periods->at(i).neg_p == true) {
            like = 10000000000000;
            break;
        }
    }
//    cout << like << endl;
    if (like < 0 || like == std::numeric_limits<double>::infinity()) {
        like = 10000000000000;
    }
    return like;
}

void optimize_sr_periods_nlopt(vector<RateModel> * _rm,StateReconstructor * _sr, vector<mat> * _free_mask, int _nfree) {
    nloptsr_periods = _sr;
    nloptrm_periods = _rm;
    nloptfree_variables_periods = _free_mask;

    nlopt::opt opt(nlopt::LN_NELDERMEAD, _nfree);
    //nlopt::opt opt(nlopt::LN_BOBYQA, _nfree);
    //nlopt::opt opt(nlopt::LN_PRAXIS, _nfree);
    //nlopt::opt opt(nlopt::LN_SBPLX, _nfree);
    //nlopt::opt opt(nlopt::LN_COBYLA, _nfree);
    //nlopt::opt opt(nlopt::LN_NEWUOA, _nfree);

    opt.set_lower_bounds(0.0000);
    opt.set_upper_bounds(100000);
    opt.set_min_objective(nlopt_sr_periods, NULL);
    opt.set_xtol_rel(0.001);
    opt.set_maxeval(10000);

    vector<double> x(_nfree,0);
    for (unsigned int k=0; k < _rm->size(); k++) {
        for (unsigned int i=0; i < _rm->at(k).get_Q().n_rows; i++) {
            for (unsigned int j=0; j < _rm->at(k).get_Q().n_cols; j++) {
                if (i != j) {
                    x[int((*_free_mask)[k](i, j))] = _rm->at(k).get_Q()(i, j);
                    //cout << x[int((*_free_mask)[k](i,j))] << " ";
                }
            }
            cout << endl;
        }
    }
    //double minf;
    vector<double> result = opt.optimize(x);
    for (unsigned int k=0; k < _rm->size(); k++) {
        for (unsigned int i=0; i < _rm->at(k).get_Q().n_rows; i++) {
            for (unsigned int j=0; j < _rm->at(k).get_Q().n_cols; j++) {
                if (i != j) {
                    (*_free_mask)[k](i, j) = result[int((*_free_mask)[k](i, j))];
                    //cout << x[int((*(*_free_mask)[k])(i,j))] << " ";
                }
            }
        //cout << endl;
        }
    }
    //if ((&minf) < 0) {
    //   printf("nlopt failed!\n");
    //}
    //else {
    //    printf("found minimum at %0.10g\n", x[0], x[1], minf);
    //}
}


/**
 * \copyright
 * Copyright (c) 2012-2019, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/LICENSE.txt
 */

#include <limits>

#include <gtest/gtest.h>

#include <Eigen/Eigen>

#include "BaseLib/ConfigTree.h"

#include "MaterialLib/FractureModels/LinearElasticIsotropic.h"
#include "MaterialLib/FractureModels/MohrCoulomb.h"

#include "ParameterLib/ConstantParameter.h"

using namespace MaterialLib::Fracture;

static const double eps_sigma = 1e6*1e-5;
static const double eps_C = 1e10*1e-5;

TEST(MaterialLib_Fracture, LinearElasticIsotropic)
{
    ParameterLib::ConstantParameter<double> const kn("", 1e11);
    ParameterLib::ConstantParameter<double> const ks("", 1e9);
    LinearElasticIsotropic<2>::MaterialProperties const mp{kn, ks};

    double const aperture0 = 1e-5;
    double const penalty_aperture_cutoff = aperture0;
    bool const tension_cutoff = true;
    LinearElasticIsotropic<2> fractureModel{penalty_aperture_cutoff,
                                            tension_cutoff, mp};
    std::unique_ptr<FractureModelBase<2>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector2d const w_prev = Eigen::Vector2d::Zero();
    Eigen::Vector2d const sigma0 = Eigen::Vector2d::Zero();
    Eigen::Vector2d const sigma_prev = sigma0;
    Eigen::Vector2d w(-1e-5, -1e-5);

    // Result vectors, not initialized.
    Eigen::Vector2d sigma;
    Eigen::Matrix2d C;

    ParameterLib::SpatialPosition x;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(-1e4, sigma[0], eps_sigma);
    ASSERT_NEAR(-1e6, sigma[1], eps_sigma);
    ASSERT_NEAR(1e9, C(0,0), eps_C);
    ASSERT_NEAR(0, C(0,1), eps_C);
    ASSERT_NEAR(0, C(1,0), eps_C);
    ASSERT_NEAR(1e11, C(1,1), eps_C);

    w << -1e-5, 1e-5;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(0, sigma[0], eps_sigma);
    ASSERT_NEAR(0, sigma[1], eps_sigma);
    ASSERT_NEAR(0, C(0,0), eps_C);
    ASSERT_NEAR(0, C(0,1), eps_C);
    ASSERT_NEAR(0, C(1,0), eps_C);
    ASSERT_NEAR(0, C(1,1), eps_C);
}

TEST(MaterialLib_Fracture, MohrCoulomb2D_elastic)
{
    ParameterLib::ConstantParameter<double> const kn("", 1e11);
    ParameterLib::ConstantParameter<double> const ks("", 1e9);
    ParameterLib::ConstantParameter<double> const phi("", 15);
    ParameterLib::ConstantParameter<double> const psi("", 5);
    ParameterLib::ConstantParameter<double> const c("", 3e16);
    MohrCoulomb<2>::MaterialProperties const mp{kn, ks, phi, psi, c};

    double const aperture0 = 1;
    double const penalty_aperture_cutoff = aperture0;
    bool const tension_cutoff = true;
    MohrCoulomb<2> fractureModel{penalty_aperture_cutoff, tension_cutoff, mp};
    std::unique_ptr<FractureModelBase<2>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector2d const w_prev = Eigen::Vector2d::Zero();
    Eigen::Vector2d const sigma0 = Eigen::Vector2d::Zero();
    Eigen::Vector2d const sigma_prev = sigma0;
    Eigen::Vector2d w(-1e-5, -1e-5);

    // Result vectors, not initialized.
    Eigen::Vector2d sigma;
    Eigen::Matrix2d C;

    ParameterLib::SpatialPosition x;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(-1e4, sigma[0], eps_sigma);
    ASSERT_NEAR(-1e6, sigma[1], eps_sigma);
    ASSERT_NEAR(1e9, C(0, 0), eps_C);
    ASSERT_NEAR(0, C(0, 1), eps_C);
    ASSERT_NEAR(0, C(1, 0), eps_C);
    ASSERT_NEAR(1e11, C(1, 1), eps_C);

    w << -1e-5, 1e-5;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(0, sigma[0], eps_sigma);
    ASSERT_NEAR(0, sigma[1], eps_sigma);
    ASSERT_NEAR(0, C(0, 0), eps_C);
    ASSERT_NEAR(0, C(0, 1), eps_C);
    ASSERT_NEAR(0, C(1, 0), eps_C);
    ASSERT_NEAR(0, C(1, 1), eps_C);
}

TEST(MaterialLib_Fracture, MohrCoulomb2D_negative_t)
{
    ParameterLib::ConstantParameter<double> const kn("", 50e9);
    ParameterLib::ConstantParameter<double> const ks("", 20e9);
    ParameterLib::ConstantParameter<double> const phi("", 15);
    ParameterLib::ConstantParameter<double> const psi("", 5);
    ParameterLib::ConstantParameter<double> const c("", 3e6);
    MohrCoulomb<2>::MaterialProperties const mp{kn, ks, phi, psi, c};

    double const aperture0 = 1e-5;
    double const penalty_aperture_cutoff = aperture0;
    bool const tension_cutoff = true;
    MohrCoulomb<2> fractureModel{penalty_aperture_cutoff, tension_cutoff, mp};
    std::unique_ptr<FractureModelBase<2>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector2d const w_prev = Eigen::Vector2d::Zero();
    Eigen::Vector2d const sigma0(-3.46e6, -2e6);
    Eigen::Vector2d const sigma_prev = sigma0;
    Eigen::Vector2d const w(-1.08e-5, -0.25e-5);

    // Result vectors, not initialized.
    Eigen::Vector2d sigma;
    Eigen::Matrix2d C;

    ParameterLib::SpatialPosition x;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(-3.50360e6, sigma[0], eps_sigma);
    ASSERT_NEAR(-2.16271e6, sigma[1], eps_sigma);
    ASSERT_NEAR(1.10723e+09, C(0,0), eps_C);
    ASSERT_NEAR(1.26558e+10, C(0,1), eps_C);
    ASSERT_NEAR(4.13226e+09, C(1,0), eps_C);
    ASSERT_NEAR(4.72319e+10, C(1,1), eps_C);
    ASSERT_NEAR(1.06608E+5, state->getShearYieldFunctionValue(), eps_sigma);
}

TEST(MaterialLib_Fracture, MohrCoulomb2D_positive_t)
{
    ParameterLib::ConstantParameter<double> const kn("", 50e9);
    ParameterLib::ConstantParameter<double> const ks("", 20e9);
    ParameterLib::ConstantParameter<double> const phi("", 15);
    ParameterLib::ConstantParameter<double> const psi("", 5);
    ParameterLib::ConstantParameter<double> const c("", 3e6);
    MohrCoulomb<2>::MaterialProperties const mp{kn, ks, phi, psi, c};

    double const aperture0 = 1e-5;
    double const penalty_aperture_cutoff = aperture0;
    bool const tension_cutoff = true;
    MohrCoulomb<2> fractureModel{penalty_aperture_cutoff, tension_cutoff, mp};
    std::unique_ptr<FractureModelBase<2>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector2d const w_prev = Eigen::Vector2d::Zero();
    Eigen::Vector2d const sigma0(0, -2e6);
    Eigen::Vector2d const sigma_prev = sigma0;
    Eigen::Vector2d const w(20.08e-5, -0.25e-5);

    // Result vectors, not initialized.
    Eigen::Vector2d sigma;
    Eigen::Matrix2d C;

    ParameterLib::SpatialPosition x;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(253972.0, sigma[0], eps_sigma);
    ASSERT_NEAR(-2.94784e+06, sigma[1], eps_sigma);
    ASSERT_NEAR(1.10723e+09, C(0,0), eps_C);
    ASSERT_NEAR(-1.26558e+10, C(0,1), eps_C);
    ASSERT_NEAR(-4.13226e+09, C(1,0), eps_C);
    ASSERT_NEAR(4.72319e+10, C(1,1), eps_C);
    ASSERT_NEAR(446608.0, state->getShearYieldFunctionValue(), eps_sigma);
}

TEST(MaterialLib_Fracture, MohrCoulomb3D_negative_t1)
{
    ParameterLib::ConstantParameter<double> const kn("", 50e9);
    ParameterLib::ConstantParameter<double> const ks("", 20e9);
    ParameterLib::ConstantParameter<double> const phi("", 15);
    ParameterLib::ConstantParameter<double> const psi("", 5);
    ParameterLib::ConstantParameter<double> const c("", 3e6);
    MohrCoulomb<3>::MaterialProperties const mp{kn, ks, phi, psi, c};

    double const aperture0 = 1e-5;
    double const penalty_aperture_cutoff = aperture0;
    bool const tension_cutoff = true;
    MohrCoulomb<3> fractureModel{penalty_aperture_cutoff, tension_cutoff, mp};
    std::unique_ptr<FractureModelBase<3>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector3d const w_prev = Eigen::Vector3d::Zero();
    Eigen::Vector3d const sigma0(-3.46e6, 0, -2e6);
    Eigen::Vector3d const sigma_prev = sigma0;
    Eigen::Vector3d const w(-1.08e-5, 0, -0.25e-5);

    // Result vectors, not initialized.
    Eigen::Vector3d sigma;
    Eigen::Matrix3d C;

    ParameterLib::SpatialPosition x;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(-3.50360e6, sigma[0], eps_sigma);
    ASSERT_NEAR(0.0, sigma[1], eps_sigma);
    ASSERT_NEAR(-2.16271e6, sigma[2], eps_sigma);
    ASSERT_NEAR(1.10723e+09, C(0,0), eps_C);
    ASSERT_NEAR(0.0, C(0,1), eps_C);
    ASSERT_NEAR(1.26558e+10, C(0,2), eps_C);
    ASSERT_NEAR(0.0, C(1,0), eps_C);
    ASSERT_NEAR(20.e9, C(1,1), eps_C);
    ASSERT_NEAR(0.0, C(1,2), eps_C);
    ASSERT_NEAR(4.13226e+09, C(2,0), eps_C);
    ASSERT_NEAR(0.0, C(2,1), eps_C);
    ASSERT_NEAR(4.72319e+10, C(2,2), eps_C);
    ASSERT_NEAR(1.06608E+5, state->getShearYieldFunctionValue(), eps_sigma);
}

TEST(MaterialLib_Fracture, MohrCoulomb3D_positive_t1)
{
    ParameterLib::ConstantParameter<double> const kn("", 50e9);
    ParameterLib::ConstantParameter<double> const ks("", 20e9);
    ParameterLib::ConstantParameter<double> const phi("", 15);
    ParameterLib::ConstantParameter<double> const psi("", 5);
    ParameterLib::ConstantParameter<double> const c("", 3e6);
    MohrCoulomb<3>::MaterialProperties const mp{kn, ks, phi, psi, c};

    double const aperture0 = 1e-5;
    double const penalty_aperture_cutoff = aperture0;
    bool const tension_cutoff = true;
    MohrCoulomb<3> fractureModel{penalty_aperture_cutoff, tension_cutoff, mp};
    std::unique_ptr<FractureModelBase<3>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector3d const w_prev = Eigen::Vector3d::Zero();
    Eigen::Vector3d const sigma0(0, 0, -2e6);
    Eigen::Vector3d const sigma_prev = sigma0;
    Eigen::Vector3d const w(20.08e-5, 0, -0.25e-5);

    // Result vectors, not initialized.
    Eigen::Vector3d sigma;
    Eigen::Matrix3d C;

    ParameterLib::SpatialPosition x;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(253972.0, sigma[0], eps_sigma);
    ASSERT_NEAR(0.0, sigma[1], eps_sigma);
    ASSERT_NEAR(-2.94784e+06, sigma[2], eps_sigma);
    ASSERT_NEAR(1.10723e+09, C(0,0), eps_C);
    ASSERT_NEAR(0.0, C(0,1), eps_C);
    ASSERT_NEAR(-1.26558e+10, C(0,2), eps_C);
    ASSERT_NEAR(0.0, C(1,0), eps_C);
    ASSERT_NEAR(20.e9, C(1,1), eps_C);
    ASSERT_NEAR(0.0, C(1,2), eps_C);
    ASSERT_NEAR(-4.13226e+09, C(2,0), eps_C);
    ASSERT_NEAR(0.0, C(2,1), eps_C);
    ASSERT_NEAR(4.72319e+10, C(2,2), eps_C);
    ASSERT_NEAR(446608.0, state->getShearYieldFunctionValue(), eps_sigma);
}

TEST(MaterialLib_Fracture, MohrCoulomb3D_positive_t2)
{
    ParameterLib::ConstantParameter<double> const kn("", 50e9);
    ParameterLib::ConstantParameter<double> const ks("", 20e9);
    ParameterLib::ConstantParameter<double> const phi("", 15);
    ParameterLib::ConstantParameter<double> const psi("", 5);
    ParameterLib::ConstantParameter<double> const c("", 3e6);
    MohrCoulomb<3>::MaterialProperties const mp{kn, ks, phi, psi, c};

    double const aperture0 = 1e-5;
    double const penalty_aperture_cutoff = aperture0;
    bool const tension_cutoff = true;
    MohrCoulomb<3> fractureModel{penalty_aperture_cutoff, tension_cutoff, mp};
    std::unique_ptr<FractureModelBase<3>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector3d const w_prev = Eigen::Vector3d::Zero();
    Eigen::Vector3d const sigma0(0, 0, -2e6);
    Eigen::Vector3d const sigma_prev = sigma0;
    Eigen::Vector3d const w(0, 20.08e-5, -0.25e-5);

    // Result vectors, not initialized.
    Eigen::Vector3d sigma;
    Eigen::Matrix3d C;

    ParameterLib::SpatialPosition x;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(0.0, sigma[0], eps_sigma);
    ASSERT_NEAR(253972.0, sigma[1], eps_sigma);
    ASSERT_NEAR(-2.94784e+06, sigma[2], eps_sigma);
    ASSERT_NEAR(20.e9, C(0,0), eps_C);
    ASSERT_NEAR(0.0, C(0,1), eps_C);
    ASSERT_NEAR(0.0, C(0,2), eps_C);
    ASSERT_NEAR(0.0, C(1,0), eps_C);
    ASSERT_NEAR(1.10723e+09, C(1,1), eps_C);
    ASSERT_NEAR(-1.26558e+10, C(1,2), eps_C);
    ASSERT_NEAR(0.0, C(2,0), eps_C);
    ASSERT_NEAR(-4.13226e+09, C(2,1), eps_C);
    ASSERT_NEAR(4.72319e+10, C(2,2), eps_C);
    ASSERT_NEAR(446608.0, state->getShearYieldFunctionValue(), eps_sigma);
}

TEST(MaterialLib_Fracture, MohrCoulomb3D_negative_t1t2)
{
    ParameterLib::ConstantParameter<double> const kn("", 50e9);
    ParameterLib::ConstantParameter<double> const ks("", 20e9);
    ParameterLib::ConstantParameter<double> const phi("", 15);
    ParameterLib::ConstantParameter<double> const psi("", 5);
    ParameterLib::ConstantParameter<double> const c("", 3e6);
    MohrCoulomb<3>::MaterialProperties const mp{kn, ks, phi, psi, c};

    double const aperture0 = 1e-5;
    double const penalty_aperture_cutoff = aperture0;
    bool const tension_cutoff = true;
    MohrCoulomb<3> fractureModel{penalty_aperture_cutoff, tension_cutoff, mp};
    std::unique_ptr<FractureModelBase<3>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector3d const w_prev = Eigen::Vector3d::Zero();
    Eigen::Vector3d const sigma0(-3.46e6 / std::sqrt(2), -3.46e6 / std::sqrt(2),
                                 -2e6);
    Eigen::Vector3d const sigma_prev = sigma0;
    Eigen::Vector3d const w(-1.08e-5/std::sqrt(2), -1.08e-5/std::sqrt(2), -0.25e-5);

    // Result vectors, not initialized.
    Eigen::Vector3d sigma;
    Eigen::Matrix3d C;

    ParameterLib::SpatialPosition x;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(-3.50360e6/std::sqrt(2), sigma[0], eps_sigma);
    ASSERT_NEAR(-3.50360e6/std::sqrt(2), sigma[1], eps_sigma);
    ASSERT_NEAR(-2.16271e6, sigma[2], eps_sigma);
    ASSERT_NEAR(1.05536e+10, C(0,0), eps_C);
    ASSERT_NEAR(-9.44638e+09, C(0,1), eps_C);
    ASSERT_NEAR(8.94897e+09, C(0,2), eps_C);
    ASSERT_NEAR(-9.44638e+09, C(1,0), eps_C);
    ASSERT_NEAR(1.05536e+10, C(1,1), eps_C);
    ASSERT_NEAR(8.94897e+09, C(1,2), eps_C);
    ASSERT_NEAR(2.92195e+09, C(2,0), eps_C);
    ASSERT_NEAR(2.92195e+09, C(2,1), eps_C);
    ASSERT_NEAR(4.72319e+10, C(2,2), eps_C);
    ASSERT_NEAR(1.06608E+5, state->getShearYieldFunctionValue(), eps_sigma);
}

TEST(MaterialLib_Fracture, MohrCoulomb3D_negative_t1_positive_t2)
{
    ParameterLib::ConstantParameter<double> const kn("", 50e9);
    ParameterLib::ConstantParameter<double> const ks("", 20e9);
    ParameterLib::ConstantParameter<double> const phi("", 15);
    ParameterLib::ConstantParameter<double> const psi("", 5);
    ParameterLib::ConstantParameter<double> const c("", 3e6);
    MohrCoulomb<3>::MaterialProperties const mp{kn, ks, phi, psi, c};

    double const aperture0 = 1e-5;
    double const penalty_aperture_cutoff = aperture0;
    bool const tension_cutoff = true;
    MohrCoulomb<3> fractureModel{penalty_aperture_cutoff, tension_cutoff, mp};
    std::unique_ptr<FractureModelBase<3>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector3d const w_prev = Eigen::Vector3d::Zero();
    Eigen::Vector3d const sigma0(-3.46e6 / std::sqrt(2), 3.46e6 / std::sqrt(2),
                                 -2e6);
    Eigen::Vector3d const sigma_prev = sigma0;
    Eigen::Vector3d const w(-1.08e-5/std::sqrt(2), 1.08e-5/std::sqrt(2), -0.25e-5);

    // Result vectors, not initialized.
    Eigen::Vector3d sigma;
    Eigen::Matrix3d C;

    ParameterLib::SpatialPosition x;
    fractureModel.computeConstitutiveRelation(0, x, aperture0, sigma0, w_prev,
                                              w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(-3.50360e6/std::sqrt(2), sigma[0], eps_sigma);
    ASSERT_NEAR(3.50360e6/std::sqrt(2), sigma[1], eps_sigma);
    ASSERT_NEAR(-2.16271e6, sigma[2], eps_sigma);
    ASSERT_NEAR(1.05536e+10, C(0,0), eps_C);
    ASSERT_NEAR(9.44638e+09, C(0,1), eps_C);
    ASSERT_NEAR(8.94897e+09, C(0,2), eps_C);
    ASSERT_NEAR(9.44638e+09, C(1,0), eps_C);
    ASSERT_NEAR(1.05536e+10, C(1,1), eps_C);
    ASSERT_NEAR(-8.94897e+09, C(1,2), eps_C);
    ASSERT_NEAR(2.92195e+09, C(2,0), eps_C);
    ASSERT_NEAR(-2.92195e+09, C(2,1), eps_C);
    ASSERT_NEAR(4.72319e+10, C(2,2), eps_C);
    ASSERT_NEAR(1.06608E+5, state->getShearYieldFunctionValue(), eps_sigma);
}