#if defined (_MSC_VER) && !defined (_WIN64)
#pragma warning(disable:4244) // boost::number_distance::distance()
                              // converts 64 to 32 bits integers
#endif

#include <cstdlib>
#include <fstream>
#include <iostream>
#include <string>

#include <boost/function_output_iterator.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Classification.h>
#include <CGAL/Point_set_3.h>
#include <CGAL/Point_set_3/IO.h>
#include <CGAL/jet_estimate_normals.h>
#include <CGAL/Shape_detection/Region_growing.h>
#include <CGAL/Real_timer.h>

#ifdef CGAL_LINKED_WITH_TBB
typedef CGAL::Parallel_tag   Concurrency_tag;
#else
typedef CGAL::Sequential_tag Concurrency_tag;
#endif

typedef CGAL::Simple_cartesian<double> Kernel;
typedef Kernel::Point_3                Point;
typedef Kernel::Iso_cuboid_3           Iso_cuboid_3;

typedef CGAL::Point_set_3<Point> Point_set;

typedef Point_set::Point_map                   Pmap;
typedef Point_set::Vector_map                  Vmap;
typedef Point_set::Property_map<int>           Imap;
typedef Point_set::Property_map<unsigned char> UCmap;

typedef CGAL::Shape_detection::Point_set::Sphere_neighbor_query<Kernel, Point_set, Pmap>                Neighbor_query;
typedef CGAL::Shape_detection::Point_set::Least_squares_plane_fit_region<Kernel, Point_set, Pmap, Vmap> Region_type;
typedef CGAL::Shape_detection::Region_growing<Point_set, Neighbor_query, Region_type>                   Region_growing;

namespace Classification = CGAL::Classification;

typedef Classification::Label_handle   Label_handle;
typedef Classification::Feature_handle Feature_handle;
typedef Classification::Label_set      Label_set;
typedef Classification::Feature_set    Feature_set;

typedef Classification::Local_eigen_analysis                                 Local_eigen_analysis;
typedef Classification::Point_set_feature_generator<Kernel, Point_set, Pmap> Feature_generator;
typedef Classification::Cluster<Point_set, Pmap>                             Cluster;

int main (int argc, char** argv)
{
  std::string filename        = "data/b9.ply";
  std::string filename_config = "data/b9_clusters_config.gz";

  if (argc > 1)
    filename = argv[1];
  if (argc > 2)
    filename_config = argv[2];

  std::ifstream in (filename.c_str(), std::ios::binary);
  Point_set pts;

  std::cerr << "Reading input" << std::endl;
  in >> pts;

  std::cerr << "Estimating normals" << std::endl;
  CGAL::Real_timer t;
  t.start();
  pts.add_normal_map();
  CGAL::jet_estimate_normals<Concurrency_tag> (pts, 12);
  t.stop();
  std::cerr << "Done in " << t.time() << " second(s)" << std::endl;
  t.reset();

  Feature_set pointwise_features;

  std::cerr << "Generating pointwise features" << std::endl;
  t.start();
  Feature_generator generator (pts, pts.point_map(), 5); // using 5 scales

#ifdef CGAL_LINKED_WITH_TBB
  pointwise_features.begin_parallel_additions();
#endif

  generator.generate_point_based_features (pointwise_features);
  generator.generate_normal_based_features (pointwise_features, pts.normal_map());

#ifdef CGAL_LINKED_WITH_TBB
  pointwise_features.end_parallel_additions();
#endif

  t.stop();
  std::cerr << "Done in " << t.time() << " second(s)" << std::endl;

  ///////////////////////////////////////////////////////////////////
  //! [Cluster]

  std::cerr << "Detecting planes and creating clusters" << std::endl;
  t.start();

  const double search_sphere_radius  = 1.0;
  const double max_distance_to_plane = 1.0;
  const double max_accepted_angle    = 25.0;
  const std::size_t min_region_size  = 10;

  Neighbor_query neighbor_query (
    pts,
    search_sphere_radius,
    pts.point_map());
  Region_type region_type (
    pts,
    max_distance_to_plane, max_accepted_angle, min_region_size,
    pts.point_map(), pts.normal_map());
  Region_growing region_growing (
    pts, neighbor_query, region_type);

  std::vector<Cluster> clusters;
  region_growing.detect
    (boost::make_function_output_iterator
     ([&](const std::vector<std::size_t>& region) -> void {

        // Create a new cluster.
        Classification::Cluster<Point_set, Pmap> cluster (pts, pts.point_map());
        for (const std::size_t idx : region)
          cluster.insert(idx);
        clusters.push_back(cluster);
      }));

  t.stop();
  std::cerr << clusters.size() << " clusters created in "
            << t.time() << " second(s)" << std::endl;
  t.reset();

  //! [Cluster]
  ///////////////////////////////////////////////////////////////////

  std::cerr << "Computing cluster features" << std::endl;

  ///////////////////////////////////////////////////////////////////
  //! [Eigen]

  Local_eigen_analysis eigen = Local_eigen_analysis::create_from_point_clusters (clusters);

  //! [Eigen]
  ///////////////////////////////////////////////////////////////////

  t.start();

  ///////////////////////////////////////////////////////////////////
  //! [Features]

  Feature_set features;

#ifdef CGAL_LINKED_WITH_TBB
  features.begin_parallel_additions();
#endif

  // First, compute means of features.
  for (std::size_t i = 0; i < pointwise_features.size(); ++ i)
    features.add<Classification::Feature::Cluster_mean_of_feature> (clusters, pointwise_features[i]);

#ifdef CGAL_LINKED_WITH_TBB
  features.end_parallel_additions();
  features.begin_parallel_additions();
#endif

  // Then, compute variances of features (and remaining cluster features).
  for (std::size_t i = 0; i < pointwise_features.size(); ++ i)
    features.add<Classification::Feature::Cluster_variance_of_feature> (clusters,
                                                                        pointwise_features[i], // i^th feature
                                                                        features[i]);          // mean of i^th feature

  features.add<Classification::Feature::Cluster_size> (clusters);
  features.add<Classification::Feature::Cluster_vertical_extent> (clusters);

  for (std::size_t i = 0; i < 3; ++ i)
    features.add<Classification::Feature::Eigenvalue> (clusters, eigen, (unsigned int)(i));

#ifdef CGAL_LINKED_WITH_TBB
  features.end_parallel_additions();
#endif

  //! [Features]
  ///////////////////////////////////////////////////////////////////

  t.stop();

  // Add types.
  Label_set labels;
  Label_handle ground     = labels.add ("ground");
  Label_handle vegetation = labels.add ("vegetation");
  Label_handle roof       = labels.add ("roof");

  std::vector<int> label_indices(clusters.size(), -1);

  std::cerr << "Using ETHZ Random Forest Classifier" << std::endl;
  Classification::ETHZ_random_forest_classifier classifier (labels, features);

  std::cerr << "Loading configuration" << std::endl;
  std::ifstream in_config (filename_config, std::ios_base::in | std::ios_base::binary);
  classifier.load_configuration (in_config);

  std::cerr << "Classifying" << std::endl;
  t.reset();
  t.start();
  Classification::classify<Concurrency_tag> (clusters, labels, classifier, label_indices);
  t.stop();

  std::cerr << "Classification done in " << t.time() << " second(s)" << std::endl;
  return EXIT_SUCCESS;
}


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// associated header
#include "Tasks/QPContacts.h"

// includes
// std
#include <stdexcept>

// Eigen
#include <Eigen/Geometry>

// boost
#include <boost/math/constants/constants.hpp>

namespace tasks
{

namespace qp
{

/// @throw std::domain_error if points is not a valid points index.
void checkRange(int point, const std::vector<Eigen::Vector3d> & points)
{
  if(point < 0 || point >= static_cast<int>(points.size()))
  {
    std::ostringstream str;
    str << "invalid point index: must be in the range [0," << points.size() << "[";
    throw std::domain_error(str.str());
  }
}

/**
 *													FrictionCone
 */

FrictionCone::FrictionCone(const Eigen::Matrix3d & frame, int nrGen, double mu, double dir) : generators(nrGen)
{
  Eigen::Vector3d normal(frame.row(2));
  Eigen::Vector3d tan(dir * frame.row(0));
  double angle = std::atan(mu);

  Eigen::Vector3d gen = Eigen::AngleAxisd(angle, tan) * normal;
  double step = (boost::math::constants::pi<double>() * 2.) / nrGen;

  for(int i = 0; i < nrGen; ++i)
  {
    generators[i] = Eigen::AngleAxisd(dir * step * i, normal) * gen;
  }
}

/**
 *													ContactId
 */

ContactId::ContactId() : r1Index(-1), r2Index(-1), r1BodyName(""), r2BodyName(""), ambiguityId(-1) {}

ContactId::ContactId(int r1I, int r2I, const std::string & r1BName, const std::string & r2BName, int ambId)
: r1Index(r1I), r2Index(r2I), r1BodyName(r1BName), r2BodyName(r2BName), ambiguityId(ambId)
{
}

bool ContactId::operator==(const ContactId & cId) const
{
  return r1Index == cId.r1Index && r2Index == cId.r2Index && r1BodyName == cId.r1BodyName
         && r2BodyName == cId.r2BodyName && ambiguityId == cId.ambiguityId;
}

bool ContactId::operator!=(const ContactId & cId) const
{
  return !((*this) == cId);
}

bool ContactId::operator<(const ContactId & cId) const
{
  return r1Index < cId.r1Index || (r1Index == cId.r1Index && r1BodyName < cId.r1BodyName)
         || (r1Index == cId.r1Index && r1BodyName == cId.r1BodyName && r2Index < cId.r2Index)
         || (r1Index == cId.r1Index && r1BodyName == cId.r1BodyName && r2Index == cId.r2Index
             && r2BodyName < cId.r2BodyName)
         || (r1Index == cId.r1Index && r1BodyName == cId.r1BodyName && r2Index == cId.r2Index
             && r2BodyName == cId.r2BodyName && ambiguityId < cId.ambiguityId);
}

/**
 *													UnilateralContact
 */

UnilateralContact::UnilateralContact(int r1I,
                                     int r2I,
                                     const std::string & r1BName,
                                     const std::string & r2BName,
                                     std::vector<Eigen::Vector3d> r1P,
                                     const Eigen::Matrix3d & r1Frame,
                                     const sva::PTransformd & Xbb,
                                     int nrGen,
                                     double mu,
                                     const sva::PTransformd & Xbcf)
: contactId(r1I, r2I, r1BName, r2BName), r1Points(std::move(r1P)), r2Points(), r1Cone(r1Frame, nrGen, mu), r2Cone(),
  X_b1_b2(Xbb), X_b1_cf(Xbcf)
{
  construct(r1Frame, nrGen, mu);
}

UnilateralContact::UnilateralContact(int r1I,
                                     int r2I,
                                     const std::string & r1BName,
                                     const std::string & r2BName,
                                     int ambId,
                                     std::vector<Eigen::Vector3d> r1P,
                                     const Eigen::Matrix3d & r1Frame,
                                     const sva::PTransformd & Xbb,
                                     int nrGen,
                                     double mu,
                                     const sva::PTransformd & Xbcf)
: contactId(r1I, r2I, r1BName, r2BName, ambId), r1Points(std::move(r1P)), r2Points(), r1Cone(r1Frame, nrGen, mu),
  r2Cone(), X_b1_b2(Xbb), X_b1_cf(Xbcf)
{
  construct(r1Frame, nrGen, mu);
}

UnilateralContact::UnilateralContact(const ContactId & cId,
                                     std::vector<Eigen::Vector3d> r1P,
                                     const Eigen::Matrix3d & r1Frame,
                                     const sva::PTransformd & Xbb,
                                     int nrGen,
                                     double mu,
                                     const sva::PTransformd & Xbcf)
: contactId(cId), r1Points(std::move(r1P)), r2Points(), r1Cone(r1Frame, nrGen, mu), r2Cone(), X_b1_b2(Xbb),
  X_b1_cf(Xbcf)
{
  construct(r1Frame, nrGen, mu);
}

Eigen::Vector3d UnilateralContact::force(const Eigen::VectorXd & lambda,
                                         int /* point */,
                                         const FrictionCone & cone) const
{
  Eigen::Vector3d F(Eigen::Vector3d::Zero());

  for(std::size_t i = 0; i < cone.generators.size(); ++i)
  {
    F += cone.generators[i] * lambda(i);
  }

  return F;
}

Eigen::Vector3d UnilateralContact::force(const Eigen::VectorXd & lambda, const FrictionCone & cone) const
{
  Eigen::Vector3d F(Eigen::Vector3d::Zero());
  int pos = 0;

  for(int i = 0; i < int(r1Points.size()); ++i)
  {
    F += force(lambda.segment(pos, nrLambda(i)), i, cone);
    pos += nrLambda(i);
  }

  return F;
}

sva::ForceVecd UnilateralContact::force(const Eigen::VectorXd & lambda,
                                        const std::vector<Eigen::Vector3d> & p,
                                        const FrictionCone & c) const
{
  sva::ForceVecd F_b(Eigen::Vector6d::Zero());
  int pos = 0;

  for(int i = 0; i < int(p.size()); ++i)
  {
    // force at point p in frame b
    sva::ForceVecd F_p_b(Eigen::Vector3d::Zero(), force(lambda.segment(pos, nrLambda(i)), i, c));
    // F_b += xlt(r_b_p)^T F_p_b
    F_b += sva::PTransformd(p[i]).transMul(F_p_b);
    pos += nrLambda(i);
  }

  return F_b;
}

int UnilateralContact::nrLambda(int /* point */) const
{
  return static_cast<int>(r1Cone.generators.size());
}

int UnilateralContact::nrLambda() const
{
  int totalLambda = 0;
  for(int i = 0; i < int(r1Points.size()); ++i)
  {
    totalLambda += nrLambda(i);
  }
  return totalLambda;
}

Eigen::Vector3d UnilateralContact::sForce(const Eigen::VectorXd & lambda, int point, const FrictionCone & cone) const
{
  checkRange(point, r1Points);
  if(static_cast<int>(lambda.rows()) != nrLambda(point))
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << nrLambda(point) << ") gived (" << lambda.rows()
        << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, point, cone);
}

Eigen::Vector3d UnilateralContact::sForce(const Eigen::VectorXd & lambda, const FrictionCone & cone) const
{
  int totalLambda = nrLambda();

  if(static_cast<int>(lambda.rows()) != totalLambda)
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << totalLambda << ") gived (" << lambda.rows() << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, cone);
}

sva::ForceVecd UnilateralContact::sForce(const Eigen::VectorXd & lambda,
                                         const std::vector<Eigen::Vector3d> & r_b_pi,
                                         const FrictionCone & c_b) const
{
  int totalLambda = nrLambda();

  if(static_cast<int>(lambda.rows()) != totalLambda)
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << totalLambda << ") gived (" << lambda.rows() << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, r_b_pi, c_b);
}

int UnilateralContact::sNrLambda(int point) const
{
  checkRange(point, r1Points);
  return nrLambda(point);
}

void UnilateralContact::construct(const Eigen::MatrixXd & r1Frame, int nrGen, double mu)
{
  // compute points in b2 coordinate
  r2Points.reserve(r1Points.size());
  for(const Eigen::Vector3d & p : r1Points)
  {
    r2Points.push_back((sva::PTransformd(p) * X_b1_b2.inv()).translation());
  }

  // compute points frame in b2 coordinate
  // Eigen::Matrix3d r2Frame = (X_b1_b2*sva::PTransformd(Eigen::Matrix3d(r1Frame))).rotation();
  Eigen::Matrix3d r2Frame = (sva::PTransformd(Eigen::Matrix3d(r1Frame)) * X_b1_b2.inv()).rotation();

  // create the b2 cone
  // We take the oppostie frame because force are opposed
  r2Cone = FrictionCone(-r2Frame, nrGen, mu, -1.);
}

/**
 *													BilateralContact
 */

BilateralContact::BilateralContact(int r1I,
                                   int r2I,
                                   const std::string & r1BName,
                                   const std::string & r2BName,
                                   std::vector<Eigen::Vector3d> r1P,
                                   const std::vector<Eigen::Matrix3d> & r1Frames,
                                   const sva::PTransformd & Xbb,
                                   int nrGen,
                                   double mu,
                                   const sva::PTransformd & Xbcf)
: contactId(r1I, r2I, r1BName, r2BName), r1Points(std::move(r1P)), r2Points(), r1Cones(r1Points.size()),
  r2Cones(r1Points.size()), X_b1_b2(Xbb), X_b1_cf(Xbcf)
{
  construct(r1Frames, nrGen, mu);
}

BilateralContact::BilateralContact(int r1I,
                                   int r2I,
                                   const std::string & r1BName,
                                   const std::string & r2BName,
                                   int ambId,
                                   std::vector<Eigen::Vector3d> r1P,
                                   const std::vector<Eigen::Matrix3d> & r1Frames,
                                   const sva::PTransformd & Xbb,
                                   int nrGen,
                                   double mu,
                                   const sva::PTransformd & Xbcf)
: contactId(r1I, r2I, r1BName, r2BName, ambId), r1Points(std::move(r1P)), r2Points(), r1Cones(r1Points.size()),
  r2Cones(r1Points.size()), X_b1_b2(Xbb), X_b1_cf(Xbcf)
{
  construct(r1Frames, nrGen, mu);
}

BilateralContact::BilateralContact(const ContactId & cId,
                                   std::vector<Eigen::Vector3d> r1P,
                                   const std::vector<Eigen::Matrix3d> & r1Frames,
                                   const sva::PTransformd & Xbb,
                                   int nrGen,
                                   double mu,
                                   const sva::PTransformd & Xbcf)
: contactId(cId), r1Points(std::move(r1P)), r2Points(), r1Cones(r1Points.size()), r2Cones(r1Points.size()),
  X_b1_b2(Xbb), X_b1_cf(Xbcf)
{
  construct(r1Frames, nrGen, mu);
}

BilateralContact::BilateralContact(const UnilateralContact & c)
: contactId(c.contactId), r1Points(c.r1Points), r2Points(c.r2Points), r1Cones(c.r1Points.size(), c.r1Cone),
  r2Cones(c.r1Points.size(), c.r2Cone), X_b1_b2(c.X_b1_b2), X_b1_cf(c.X_b1_cf)
{
}

Eigen::Vector3d BilateralContact::force(const Eigen::VectorXd & lambda,
                                        int point,
                                        const std::vector<FrictionCone> & cones) const
{
  Eigen::Vector3d F(Eigen::Vector3d::Zero());

  for(std::size_t i = 0; i < cones[point].generators.size(); ++i)
  {
    F += cones[point].generators[i] * lambda(i);
  }

  return F;
}

Eigen::Vector3d BilateralContact::force(const Eigen::VectorXd & lambda, const std::vector<FrictionCone> & cones) const
{
  Eigen::Vector3d F(Eigen::Vector3d::Zero());
  int pos = 0;

  for(int i = 0; i < int(r1Points.size()); ++i)
  {
    F += force(lambda.segment(pos, nrLambda(i)), i, cones);
    pos += nrLambda(i);
  }

  return F;
}

sva::ForceVecd BilateralContact::force(const Eigen::VectorXd & lambda,
                                       const std::vector<Eigen::Vector3d> & p,
                                       const std::vector<FrictionCone> & c) const
{
  sva::ForceVecd F_b(Eigen::Vector6d::Zero());
  int pos = 0;

  for(int i = 0; i < int(p.size()); ++i)
  {
    // force at point p in frame b
    sva::ForceVecd F_p_b(Eigen::Vector3d::Zero(), force(lambda.segment(pos, nrLambda(i)), i, c));
    // F_b += xlt(r_b_p)^T F_p_b
    F_b += sva::PTransformd(p[i]).transMul(F_p_b);
    pos += nrLambda(i);
  }

  return F_b;
}

int BilateralContact::nrLambda(int point) const
{
  return static_cast<int>(r1Cones[point].generators.size());
}

int BilateralContact::nrLambda() const
{
  int totalLambda = 0;
  for(int i = 0; i < int(r1Points.size()); ++i)
  {
    totalLambda += nrLambda(i);
  }
  return totalLambda;
}

Eigen::Vector3d BilateralContact::sForce(const Eigen::VectorXd & lambda,
                                         int point,
                                         const std::vector<FrictionCone> & cones) const
{
  checkRange(point, r1Points);
  if(static_cast<int>(lambda.rows()) != nrLambda(point))
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << nrLambda(point) << ") gived (" << lambda.rows()
        << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, point, cones);
}

Eigen::Vector3d BilateralContact::sForce(const Eigen::VectorXd & lambda, const std::vector<FrictionCone> & cones) const
{
  int totalLambda = nrLambda();

  if(static_cast<int>(lambda.rows()) != totalLambda)
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << totalLambda << ") gived (" << lambda.rows() << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, cones);
}

sva::ForceVecd BilateralContact::sForce(const Eigen::VectorXd & lambda,
                                        const std::vector<Eigen::Vector3d> & r_b_pi,
                                        const std::vector<FrictionCone> & c_pi_b) const
{
  int totalLambda = nrLambda();

  if(static_cast<int>(lambda.rows()) != totalLambda)
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << totalLambda << ") gived (" << lambda.rows() << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, r_b_pi, c_pi_b);
}

int BilateralContact::sNrLambda(int point) const
{
  checkRange(point, r1Points);
  return nrLambda(point);
}

void BilateralContact::construct(const std::vector<Eigen::Matrix3d> & r1Frames, int nrGen, double mu)
{
  assert(r1Points.size() == r1Frames.size());

  r2Points.reserve(r1Points.size());
  sva::PTransformd X_b2_b1(X_b1_b2.inv());
  for(std::size_t i = 0; i < r1Points.size(); ++i)
  {
    // compute point i in b2 coordinate
    sva::PTransformd X_b1_p(r1Frames[i], r1Points[i]);
    sva::PTransformd X_b2_p = X_b1_p * X_b2_b1;
    r2Points.push_back(X_b2_p.translation());

    // construct r1 cone
    r1Cones[i] = FrictionCone(r1Frames[i], nrGen, mu);
    // create the b2 cone
    // We take the oppostie frame because force are opposed
    r2Cones[i] = FrictionCone(-X_b2_p.rotation(), nrGen, mu, -1.);
  }
}

} // namespace qp

} // namespace tasks


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

// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// Copyright (c) 2016-2018 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP

#include <boost/geometry/srs/spheroid.hpp>

#include <boost/geometry/formulas/area_formulas.hpp>
#include <boost/geometry/formulas/authalic_radius_sqr.hpp>
#include <boost/geometry/formulas/eccentricity_sqr.hpp>

#include <boost/geometry/strategies/area.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

/*!
\brief Geographic area calculation
\ingroup strategies
\details Geographic area calculation by trapezoidal rule plus integral
         approximation that gives the ellipsoidal correction
\tparam FormulaPolicy Formula used to calculate azimuths
\tparam SeriesOrder The order of approximation of the geodesic integral
\tparam Spheroid The spheroid model
\tparam CalculationType \tparam_calculation
\author See
- Danielsen JS, The area under the geodesic. Surv Rev 30(232): 61–66, 1989
- Charles F.F Karney, Algorithms for geodesics, 2011 https://arxiv.org/pdf/1109.4448.pdf

\qbk{
[heading See also]
\* [link geometry.reference.algorithms.area.area_2_with_strategy area (with strategy)]
\* [link geometry.reference.srs.srs_spheroid srs::spheroid]
}
*/
template
<
    typename FormulaPolicy = strategy::andoyer,
    std::size_t SeriesOrder = strategy::default_order<FormulaPolicy>::value,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
{
    // Switch between two kinds of approximation(series in eps and n v.s.series in k ^ 2 and e'^2)
    static const bool ExpandEpsN = true;
    // LongSegment Enables special handling of long segments
    static const bool LongSegment = false;

    //Select default types in case they are not set

public:
    template <typename Geometry>
    struct result_type
        : strategy::area::detail::result_type
            <
                Geometry,
                CalculationType
            >
    {};

protected :
    struct spheroid_constants
    {
        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::value,
                typename geometry::radius_type<Spheroid>::type,
                CalculationType
            >::type calc_t;

        Spheroid m_spheroid;
        calc_t const m_a2;  // squared equatorial radius
        calc_t const m_e2;  // squared eccentricity
        calc_t const m_ep2; // squared second eccentricity
        calc_t const m_ep;  // second eccentricity
        calc_t const m_c2;  // squared authalic radius

        inline spheroid_constants(Spheroid const& spheroid)
            : m_spheroid(spheroid)
            , m_a2(math::sqr(get_radius<0>(spheroid)))
            , m_e2(formula::eccentricity_sqr<calc_t>(spheroid))
            , m_ep2(m_e2 / (calc_t(1.0) - m_e2))
            , m_ep(math::sqrt(m_ep2))
            , m_c2(formula_dispatch::authalic_radius_sqr
                    <
                        calc_t, Spheroid, srs_spheroid_tag
                    >::apply(m_a2, m_e2))
        {}
    };

public:
    template <typename Geometry>
    class state
    {
        friend class geographic;

        typedef typename result_type<Geometry>::type return_type;

    public:
        inline state()
            : m_excess_sum(0)
            , m_correction_sum(0)
            , m_crosses_prime_meridian(0)
        {}

    private:
        inline return_type area(spheroid_constants const& spheroid_const) const
        {
            return_type result;

            return_type sum = spheroid_const.m_c2 * m_excess_sum
                   + spheroid_const.m_e2 * spheroid_const.m_a2 * m_correction_sum;

            // If encircles some pole
            if (m_crosses_prime_meridian % 2 == 1)
            {
                std::size_t times_crosses_prime_meridian
                        = 1 + (m_crosses_prime_meridian / 2);

                result = return_type(2.0)
                         * geometry::math::pi<return_type>()
                         * spheroid_const.m_c2
                         * return_type(times_crosses_prime_meridian)
                         - geometry::math::abs(sum);

                if (geometry::math::sign<return_type>(sum) == 1)
                {
                    result = - result;
                }

            }
            else
            {
                result = sum;
            }

            return result;
        }

        return_type m_excess_sum;
        return_type m_correction_sum;

        // Keep track if encircles some pole
        std::size_t m_crosses_prime_meridian;
    };

public :
    explicit inline geographic(Spheroid const& spheroid = Spheroid())
        : m_spheroid_constants(spheroid)
    {}

    template <typename PointOfSegment, typename Geometry>
    inline void apply(PointOfSegment const& p1,
                      PointOfSegment const& p2,
                      state<Geometry>& st) const
    {
        if (! geometry::math::equals(get<0>(p1), get<0>(p2)))
        {
            typedef geometry::formula::area_formulas
                <
                    typename result_type<Geometry>::type,
                    SeriesOrder, ExpandEpsN
                > area_formulas;

            typename area_formulas::return_type_ellipsoidal result =
                     area_formulas::template ellipsoidal<FormulaPolicy::template inverse>
                                             (p1, p2, m_spheroid_constants);

            st.m_excess_sum += result.spherical_term;
            st.m_correction_sum += result.ellipsoidal_term;

            // Keep track whenever a segment crosses the prime meridian
            if (area_formulas::crosses_prime_meridian(p1, p2))
            {
                st.m_crosses_prime_meridian++;
            }
        }
    }

    template <typename Geometry>
    inline typename result_type<Geometry>::type
        result(state<Geometry> const& st) const
    {
        return st.area(m_spheroid_constants);
    }

private:
    spheroid_constants m_spheroid_constants;

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <>
struct default_strategy<geographic_tag>
{
    typedef strategy::area::geographic<> type;
};

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP


#ifndef TENSOR_HH
#define TENSOR_HH

#include <type_traits>
#include <Eigen/Dense>
#include "../ElasticityTensor.hh"
#include "../Flattening.hh"
#include "../SymmetricMatrix.hh"
#include "../Types.hh"
#include "EnergyTraits.hh"

template<typename _Real,
         size_t t_N,
         typename _Storage_t,
         typename _ConstStorageRef_t,
         typename _Derived>
_Real
doubleContract(const ConstSymmetricMatrixBase<_Real, t_N, _Storage_t, _ConstStorageRef_t>& lhs,
               const Eigen::MatrixBase<_Derived>& rhs)
{
    // Note: Some template metaprogramming should be used to test that the
    // one scalar is convertible into the other and make the return type
    // the type that is greater for the convertible relation.
    static_assert(std::is_same<_Real, typename _Derived::Scalar>::value,
                  "Different scalar types between the operand is not supported");

    static_assert(_Derived::RowsAtCompileTime == _Derived::ColsAtCompileTime &&
                    _Derived::RowsAtCompileTime == t_N,
                  "");

    // Note: This can be optimized by using the fact that lhs is symmetric
    _Real e = 0;
    for (size_t i = 0; i < t_N; ++i) {
        for (size_t j = 0; j < t_N; ++j) {
            e += lhs(i, j) * rhs(i, j);
        }
    }

    return e;
}

template<typename _Real,
         size_t t_N,
         typename _Storage_t,
         typename _ConstStorageRef_t,
         typename _Derived>
_Real
doubleContract(const Eigen::MatrixBase<_Derived>& lhs,
               const ConstSymmetricMatrixBase<_Real, t_N, _Storage_t, _ConstStorageRef_t>& rhs)
{
    return doubleContract(rhs, lhs);
}

template<typename _Derived>
void symmetrize(Eigen::MatrixBase<_Derived>& m) {
    static_assert(_Derived::RowsAtCompileTime == _Derived::ColsAtCompileTime,
                  "Symmetrization only makes sense for square matrices");
    m = 0.5 * (m + m.transpose()).eval();
}

template<typename EigenType>
using SMVType = SymmetricMatrixValue<typename EigenType::Scalar,
                                     EigenType::RowsAtCompileTime>;

template<typename _Derived>
SMVType<_Derived>
symmetrized(const Eigen::MatrixBase<_Derived> &A) {
    static_assert(_Derived::RowsAtCompileTime == _Derived::ColsAtCompileTime,
                  "Symmetrization only makes sense for square matrices");
    return SMVType<_Derived>(0.5 * (A + A.transpose()), typename SMVType<_Derived>::skip_validation());
}

template<typename _Derived>
SMVType<_Derived>
symmetrized_x2(const Eigen::MatrixBase<_Derived> &A) {
    static_assert(_Derived::RowsAtCompileTime == _Derived::ColsAtCompileTime,
                  "Symmetrization only makes sense for square matrices");
    return SMVType<_Derived>(A + A.transpose(), typename SMVType<_Derived>::skip_validation());
}

template<typename _Derived>
bool isSymmetric(const Eigen::MatrixBase<_Derived>& matrix) {
    static_assert(_Derived::RowsAtCompileTime == _Derived::ColsAtCompileTime,
                  "Symmetry check only makes sense for square matrices");

    for (size_t col = 0; col < _Derived::ColsAtCompileTime; ++col) {
        for (size_t row = 0; row <= col; ++row) {
            if (std::abs(matrix(row, col) - matrix(col, row)) > 1e-13)
                return false;
        }
    }
    return true;
}

// Compute the scalar product of two matrices A : B
template<typename _Derived1, typename _Derived2>
typename _Derived1::Scalar doubleContract(const Eigen::MatrixBase<_Derived1>& A, const Eigen::MatrixBase<_Derived2>& B)
{
    static_assert((int(_Derived1::RowsAtCompileTime) == int(_Derived2::RowsAtCompileTime)) &&
                  (int(_Derived1::ColsAtCompileTime) == int(_Derived2::ColsAtCompileTime)), "Dimensions of A and B must match to compute A : B");
    return (A.transpose() * B).trace();
}

template<typename _Real, size_t _Dim, typename Derived>
SymmetricMatrixValue<_Real, _Dim>
doubleContract(const ElasticityTensor<_Real, _Dim>& A, const Eigen::MatrixBase<Derived>& B)
{
    SymmetricMatrixValue<_Real, _Dim> result;
    for (size_t i = 0; i < _Dim; ++i)
        for (size_t j = i; j < _Dim; ++j)
            for (size_t k = 0; k < _Dim; ++k)
                for (size_t l = 0; l < _Dim; ++l)
                    result(i, j) += A(i, j, k, l) * B(k, l);

    return result;
}

/**
 *  Puts in the given matrix zeros everywhere except in (\a row, \a col).
 */
template<typename Derived>
void
setUnitMatrix(size_t row, size_t col, Eigen::MatrixBase<Derived>& out)
{
    out.setZero();
    out(row, col) = 1.;
}

// Note: This could be factored by having a function that returnes the major index and another
// function that returnes the non-major index.
/**
 *  Matrix indices manipulation helper
 */
template<size_t _Dimension, size_t _StoragePolicy = Eigen::ColMajor>
struct Indices;

template<size_t _Dimension>
struct Indices<_Dimension, Eigen::ColMajor>
{
    static std::tuple<size_t, size_t> getNext(size_t row, size_t col)
    {
        ++row;
        if (row == _Dimension)
        {
            return std::make_tuple(0, col + 1);
        }
        return std::make_tuple(row, col);
    }

    static std::tuple<size_t, size_t> getUpperTriangleNext(size_t row, size_t col)
    {
        ++row;
        if (row > col)
        {
            return std::make_tuple(0, col + 1);
        }
        return std::make_tuple(row, col);
    }

    static bool arePastEnd(size_t /* row */, size_t col) { return col >= _Dimension; }
};

template<size_t _Dimension>
struct Indices<_Dimension, Eigen::RowMajor>
{
    static std::tuple<size_t, size_t> getNext(size_t row, size_t col)
    {
        ++col;
        if (col == _Dimension)
        {
            return std::make_tuple(row + 1, 0);
        }
        return std::make_tuple(row, col);
    }

    static std::tuple<size_t, size_t> getUpperTriangleNext(size_t row, size_t col)
    {
        ++col;
        if (col > row)
        {
            return std::make_tuple(row + 1, 0);
        }
        return std::make_tuple(row, col);
    }

    static bool arePastEnd(size_t row, size_t /* col */) { return row >= _Dimension; }
};

template<size_t _Dimension, size_t _StoragePolicy = Eigen::ColMajor>
std::tuple<size_t, size_t> getNextIndices(size_t row, size_t col) {
    return Indices<_Dimension, _StoragePolicy>::getNext(row, col);
}

template<size_t _Dimension, size_t _StoragePolicy = Eigen::ColMajor>
std::tuple<size_t, size_t> getUpperTriangleNextIndices(size_t row, size_t col) {
    return Indices<_Dimension, _StoragePolicy>::getUpperTriangleNext(row, col);
}

template<size_t _Dimension, size_t _StoragePolicy = Eigen::ColMajor>
bool arePastEndIndices(size_t row, size_t col) {
    return Indices<_Dimension, _StoragePolicy>::arePastEnd(row, col);
}

////////////////////////////////////////////////////////////////////////////////
// Support for accelerating calculations involving Jacobians of vector-valued
// shape functions
////////////////////////////////////////////////////////////////////////////////

// The vectorized shape functions are of the form
//      e_c phi_n
// and their Jacobians look like:
//      e_c \otimes grad phi_n
// where e_c is a canonical basis vector for R^D (the output space dimension)
// and grad phi_n is a vector in R^N (the input space dimension).
// This class provides a compact representation for these Jacobians which
// will also allow more efficient contraction operations.
template<int D, class GradType>
struct VectorizedShapeFunctionJacobian {
    static constexpr int N = GradType::RowsAtCompileTime;

    // Emulate part of Eigen's interface.
    // This also allows VectorizedShapeFunctionJacobian to
    // masquerade as a DxN matrix in metaprogramming type checks (e.g., isMatrixOfSize).
    static constexpr int RowsAtCompileTime = D;
    static constexpr int ColsAtCompileTime = N;
    using Scalar     = typename GradType::Scalar;
    using MatrixType = Eigen::Matrix<Scalar, D, N>;
    using ColVec     = Eigen::Matrix<Scalar, D, 1>;
    using Derived    = MatrixType;

    int c;
    GradType g;

    VectorizedShapeFunctionJacobian(int cc, Eigen::Ref<const GradType> gg)
        : c(cc), g(gg) { }

    MatrixType toMatrix() const {
        MatrixType result(MatrixType::Zero());
        result.row(c) = g.transpose();
        return result;
    }

    // Note: it doesn't seem possible to actually use this explicit cast operator
    // except by directly calling `.operator MatrixType()`--this is because Eigen's
    // converting constructor is preferred when issuing a
    // `static_cast<MatrixType()` or `MatrixType()`.
    explicit operator MatrixType() const { // Allow conversion to underlying matrix type when necessary.
        return toMatrix();
    }

    // Note: this method provides the same conversion interface as Eigen::DenseBase::matrix();
    // this allows generic code to call `.matrix()` on VSFJ or Eigen types.
    MatrixType matrix() const { return toMatrix(); }

    template<class Derived>
    friend auto operator*(const VectorizedShapeFunctionJacobian &A, const Eigen::MatrixBase<Derived> &B) {
        using ResultType = VectorizedShapeFunctionJacobian<D, Eigen::Matrix<Scalar, Derived::ColsAtCompileTime, 1>>;
        return ResultType(A.c, B.template cast<Scalar>().transpose() * A.g);
    }

    template<class Derived>
    friend auto operator*(const Eigen::MatrixBase<Derived> &A, const VectorizedShapeFunctionJacobian &B) {
        return A.col(B.c).template cast<Scalar>() * B.g.transpose();
    }

    template<typename Real2, class Enable = std::enable_if_t<std::is_arithmetic<Real2>::value>>
    friend VectorizedShapeFunctionJacobian operator*(const Real2 &s, const VectorizedShapeFunctionJacobian &B) {
        return VectorizedShapeFunctionJacobian(B.c, s * B.g);
    }

    template<typename Real2, class Enable = std::enable_if_t<std::is_arithmetic<Real2>::value>>
    friend VectorizedShapeFunctionJacobian operator*(const VectorizedShapeFunctionJacobian &B, const Real2 &s) {
        return VectorizedShapeFunctionJacobian(B.c, s * B.g);
    }

    template<class Derived>
    friend MatrixType operator+(const VectorizedShapeFunctionJacobian &A, const Eigen::MatrixBase<Derived> &B) {
        static_assert((RowsAtCompileTime == Derived::RowsAtCompileTime) &&
                      (ColsAtCompileTime == Derived::ColsAtCompileTime), "Size mismatch");
        MatrixType result(B);
        result.row(A.c) += A.g.transpose();
        return result;
    }

    template<class Derived>
    friend ColVec colCross(const VectorizedShapeFunctionJacobian &A, int j, const Eigen::MatrixBase<Derived> &v) {
        // g[j] * e_c.cross(v)
        ColVec result;
        result[ A.c         ] = 0.0;
        result[(A.c + 2) % D] =  A.g[j] * v[(A.c + 1) % D];
        result[(A.c + 1) % D] = -A.g[j] * v[(A.c + 2) % D];
        return result;
    }

    template<class Derived>
    friend MatrixType operator+(const Eigen::MatrixBase<Derived> &A, const VectorizedShapeFunctionJacobian &B) {
        return B + A;
    }
};

// A : (B.c otimes B.g)
template<class Derived, int D, class GradType>
typename Derived::Scalar doubleContract(const Eigen::MatrixBase<Derived> &A,
                      const VectorizedShapeFunctionJacobian<D, GradType> &B) {
    return A.row(B.c).dot(B.g);
}

template<class Derived, int D, class GradType>
auto doubleContract(const VectorizedShapeFunctionJacobian<D, GradType> &A,
                      const Eigen::MatrixBase<Derived> &B) { return doubleContract(B, A); }

template<class T>
struct IsVectorizedShapeFunctionJacobian { static constexpr bool value = false; };
template<int D, class GradType>
struct IsVectorizedShapeFunctionJacobian<VectorizedShapeFunctionJacobian<D, GradType>> {
    static constexpr bool value = true;
};

// Some operations that can be accelerated with VectorizedShapeFunctionJacobian types.
template<class AType, class BType, typename =
    std::enable_if_t<!IsVectorizedShapeFunctionJacobian<AType>::value ||
                     !IsVectorizedShapeFunctionJacobian<BType>::value, void>>
bool AtBKnownZero(const AType &, const BType &) { return false; }

template<int D, class GradType>
bool AtBKnownZero(const VectorizedShapeFunctionJacobian<D, GradType> &A,
                  const VectorizedShapeFunctionJacobian<D, GradType> &B) {
    return A.c != B.c;
}

template<class AType, class BType, typename =
    std::enable_if_t<!IsVectorizedShapeFunctionJacobian<AType>::value ||
                     !IsVectorizedShapeFunctionJacobian<BType>::value, void>>
auto computeAtB(const AType &A, const BType &B) {
    return A.transpose() * B;
}

template<int D, class GradType>
auto computeAtB(const VectorizedShapeFunctionJacobian<D, GradType> &A,
                const VectorizedShapeFunctionJacobian<D, GradType> &B) {
    using VSFJ = VectorizedShapeFunctionJacobian<D, GradType>;
    using Scalar = typename VSFJ::Scalar;
    using Result = Eigen::Matrix<Scalar, VSFJ::N, VSFJ::N>;

    if (A.c == B.c)
        return Result(A.g * B.g.transpose());
    return Result(Result::Zero());
}

template<class Derived1, class Derived2>
Eigen::Matrix<typename Derived1::Scalar, 3, 1>
colCross(const Eigen::MatrixBase<Derived1> &A, int j, const Eigen::MatrixBase<Derived2> &v) {
    static_assert((Derived1::RowsAtCompileTime == 3) && (Derived2::RowsAtCompileTime == 3) && (Derived2::ColsAtCompileTime == 1),
                  "Unexpected sizes for colCross");
    return A.col(j).cross(v);
}

template<int D, class GradType>
SMVType<VectorizedShapeFunctionJacobian<D, GradType>>
symmetrized(const VectorizedShapeFunctionJacobian<D, GradType> &A) {
    SMVType<VectorizedShapeFunctionJacobian<D, GradType>> result; // zero-initializes
    for (int i = 0; i < int(D); ++i)
        result(A.c, i) = ((i == A.c) ? 1.0 : 0.5) * A.g[i];
    return result;
}

// Note: C better be symmetric!
template<class Mat_>
Eigen::Matrix<typename Mat_::Scalar,
              Mat_::RowsAtCompileTime,
              Mat_::ColsAtCompileTime>
spdMatrixSqrt(const Mat_ &C) {
    constexpr static int N = Mat_::RowsAtCompileTime;
    static_assert((N == 2) || (N == 3), "Unexpected matrix size");
    using MNd = Eigen::Matrix<typename Mat_::Scalar, N, N>;
    return Eigen::SelfAdjointEigenSolver<MNd>(C).operatorSqrt();
}

// Compute the double contraction `C : e` for fourth order tensor `C` and matrix `e`.
// Assumes that C has been flattened with the same ordering as e's storage storage order!
template<class FlattenedTensorDerived, class Derived>
std::enable_if_t<(FlattenedTensorDerived::RowsAtCompileTime == FlattenedTensorDerived::ColsAtCompileTime)
                && (FlattenedTensorDerived::ColsAtCompileTime == (Derived::RowsAtCompileTime * Derived::ColsAtCompileTime)),
Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>>
applyFlattened4thOrderTensor(const Eigen::MatrixBase<FlattenedTensorDerived> &C, const Eigen::MatrixBase<Derived> &e) {
    using Scalar = typename Derived::Scalar;
    constexpr int M = Derived::RowsAtCompileTime,
                  N = Derived::ColsAtCompileTime;
    using FlatMatrix = Eigen::Matrix<Scalar, M * N, 1>;
    Eigen::Matrix<Scalar, M, N, Derived::Options> result;
    Eigen::Map<FlatMatrix>(result.data()) = C * Eigen::Map<const FlatMatrix>(e.derived().data()).eval();
    return result;
}

// Compute the double contraction `C : e` for fourth order tensor `C` and matrix `e`.
// Assumes that C has been flattened **in column major order**
template<class FlattenedTensorDerived, int D, class GradType>
std::enable_if_t<(FlattenedTensorDerived::RowsAtCompileTime == FlattenedTensorDerived::RowsAtCompileTime)
                && (FlattenedTensorDerived::ColsAtCompileTime == (D * GradType::RowsAtCompileTime)),
Eigen::Matrix<typename FlattenedTensorDerived::Scalar, D, GradType::RowsAtCompileTime>>
applyFlattened4thOrderTensor(const Eigen::MatrixBase<FlattenedTensorDerived> &C,
                             const VectorizedShapeFunctionJacobian<D, GradType> &e) {
    using Scalar = typename FlattenedTensorDerived::Scalar;
    constexpr int M = D,
                  N = GradType::RowsAtCompileTime;
    using FlatMatrix = Eigen::Matrix<Scalar, M * N, 1>;
    // "e" consists of a single nonzero row at index "e.c" with values "e.g"
    // We assume column major ordering, so the flattened version of "e" has
    // nonzero values at indices `e.c + D * i`.
    FlatMatrix flatResult = C.col(e.c) * e.g[0];
    for (int i = 1; i < N; ++i)
        flatResult += C.col(e.c + D * i) * e.g[i];
    return Eigen::Map<Eigen::Matrix<Scalar, M, N>>(flatResult.data());
}

#endif


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_DET_HPP
#define BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_DET_HPP

#include <boost/hana/equal.hpp>
#include <boost/hana/eval_if.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/functional/fix.hpp>
#include <boost/hana/functional/flip.hpp>
#include <boost/hana/functional/on.hpp>
#include <boost/hana/functional/partial.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/power.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/remove_at.hpp>
#include <boost/hana/transform.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/unpack.hpp>

#include <utility>

#include "matrix.hpp"

namespace cppcon {
    namespace hana = boost::hana;
    auto det = hana::fix([](auto det, auto&& m) -> decltype(auto) {
        auto matrix_minor = [=](auto&& m, auto i, auto j) -> decltype(auto) {
            return det(hana::unpack(
                hana::transform(
                    hana::remove_at(rows(std::forward<decltype(m)>(m)), i),
                    hana::partial(hana::flip(hana::remove_at), j)
                ),
                matrix
            ));
        };

        auto cofactor = [=](auto&& m, auto i, auto j) {
            return hana::power(hana::int_c<-1>, hana::plus(i, j)) *
                    matrix_minor(std::forward<decltype(m)>(m), i, j);
        };

        return hana::eval_if(m.size() == hana::size_c<1>,
            hana::always(m.at(hana::size_c<0>, hana::size_c<0>)),
            [=](auto _) {
                auto cofactors_1st_row = hana::unpack(_(hana::make_range)(hana::size_c<0>, m.ncolumns()),
                    hana::on(hana::make_tuple, hana::partial(cofactor, m, hana::size_c<0>))
                );
                return detail::tuple_scalar_product(hana::front(rows(m)), cofactors_1st_row);
            }
        );
    });
} // end namespace cppcon

#endif // !BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_DET_HPP


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   MetaScale SAS
//
//          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_SCALAR_DET_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_SCALAR_DET_HPP_INCLUDED

#include <nt2/linalg/functions/det.hpp>
#include <nt2/include/functions/getrf.hpp>
#include <nt2/include/functions/numel.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <nt2/include/functions/globalprod.hpp>
#include <nt2/core/container/table/table.hpp>
#include <boost/core/ignore_unused.hpp>

namespace nt2{ namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( det_, tag::cpu_
                            , (A0)
                            , ((ast_<A0, nt2::container::domain>))
                            )
  {
    typedef typename A0::value_type                 type_t;
    typedef typename boost::dispatch::meta::as_floating<type_t>
                                          ::type  result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      BOOST_ASSERT_MSG( issquare(a0)
                      , "DET: Argument must be a square matrix"
                      );

      nt2::container::table<typename A0::value_type>  lu(a0);
      nt2::container::table<nt2_la_int>               ip;

      // Factorize A as L/U
      nt2_la_int  info = nt2::getrf(boost::proto::value(lu),boost::proto::value(ip));
      boost::ignore_unused(info);

      // DET(A) is the product of LU(A) diagonal by -1 at the power of
      // the number of non-permutations done in LU(A)
      std::size_t n     = nt2::numel(ip);
      result_type sign  = One<result_type>();

      // TODO: Parallelize this somehow ?
      for(std::size_t i = 1;i <= n;++i)
        sign  *= (ip(i) != nt2_la_int(i)) ? 1 : -1;

      return nt2::globalprod(nt2::diag_of(lu))*sign;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( det_, tag::cpu_
                            , (A0)
                            , (scalar_< unspecified_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>
                                          ::type  result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return a0;
    }
  };
} }

#endif


#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/slam3d/vertex_pointxyz.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <iostream>

#include "common.h"
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <Eigen/Dense>

using namespace Sophus;
using namespace Eigen;
using namespace std;

struct Camera
{
    Camera() {}

    Camera(double* data)
    {
        Rt = g2o::SE3Quat::exp(Eigen::Map<Eigen::Matrix<double, 6, 1>>(data));

        f = data[6];
        k1 = data[7];
        k2 = data[8];
    }

    void set_to(double* data) const
    {
        Eigen::Matrix<double, 6, 1> rt = Rt.log();

        for (int i = 0; i < 6; ++i)
            data[i] = rt[i];

        data[6] = f;
        data[7] = k1;
        data[8] = k2;
    }

    g2o::SE3Quat Rt; // g2o::SE3Quat is stored as rotation first and then translation
    double f = 0.0, k1 = 0.0, k2 = 0.0;
};

class VertexCamera: public g2o::BaseVertex<6, Camera>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {

        _estimate = Camera();
        // _estimate = g2o::SE3Quat();
    }

    virtual void oplusImpl(const double *update) override {
        // _estimate = g2o::SE3Quat::exp(Eigen::Map<const Eigen::Matrix<double, 6, 1>>(update)) * _estimate;
        _estimate.Rt = g2o::SE3Quat::exp(Eigen::Map<const Eigen::Matrix<double, 6, 1>>(update)) * _estimate.Rt;

    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
};

class VertexLandmark: public g2o::BaseVertex<3, Eigen::Vector3d>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {
        _estimate = Eigen::Vector3d::Zero();
    }

    virtual void oplusImpl(const double *update) override {
        _estimate += Eigen::Map<const Eigen::Vector3d>(update);
    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
};

class EdgeReprojection: public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexLandmark, VertexCamera>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeReprojection(double f, double k1, double k2) : f_(f), k1_(k1), k2_(k2) { }

    virtual void computeError() override {
        const VertexCamera* v_cam = static_cast<VertexCamera*>(_vertices[1]);
        const VertexLandmark* v_point = static_cast<VertexLandmark*>(_vertices[0]);
        auto cam = v_cam->estimate();
        Eigen::Vector3d X = v_point->estimate();
        Eigen::Vector3d X_cam = cam.Rt * X;
        X_cam /= X_cam.z();

        // auto p2 = X_cam.x() * X_cam.x() + X_cam.y() * X_cam.y();
        // auto r = 1.0 + p2 * (k1_ + (p2 * k2_));

        Eigen::Vector2d uv = -X_cam.head<2>() *  f_ ; // minus because of the dataset projection
        _error = _measurement - uv;
    }

    virtual void linearizeOplus() override {
        const VertexCamera *v_cam = static_cast<VertexCamera*>(_vertices[1]);
        const VertexLandmark *v_landmark = static_cast<VertexLandmark*>(_vertices[0]);
        Eigen::Vector3d X = v_landmark->estimate();
        auto cam = v_cam->estimate();
        Eigen::Vector3d Xc = cam.Rt * X;

        double x = Xc.x();
        double y = Xc.y();
        double z = Xc.z();
        double z_2 = z * z;

        Eigen::Matrix<double, 2, 3> dedXc;
        dedXc << -f_ / z, 0.0, f_ * x / z_2,   // -f_ is because of the dataset projection function (not standard)
                0.0, -f_ / z, f_ * y / z_2;
        Eigen::Matrix3d dXcdR;
        dXcdR << 0.0, -z, y,
                 z, 0.0, -x,
                 -y, x, 0.0;
        Eigen::Matrix3d dXcdt = Eigen::Matrix3d::Identity();
        _jacobianOplusXj.block<2, 3>(0, 0) = dedXc * -dXcdR; // -dXcdR <=> -Xc^
        _jacobianOplusXj.block<2, 3>(0, 3) = dedXc * dXcdt;

        _jacobianOplusXi = dedXc * cam.Rt.rotation().matrix();

         _jacobianOplusXi *= -1; // take the negative jacobian because the error is defined as (measurement - projection)
         _jacobianOplusXj *= -1;

    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
    private:
        double f_, k1_, k2_;

};

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

    if (argc != 2) {
        cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;
        return 1;
    }

    BALProblem dataset(argv[1]);
    dataset.Normalize();
    // dataset.Perturb(0.1, 0.5, 0.5);
    dataset.WriteToPLYFile("initial_pc.ply");

    std::cout << "\n";
    std::cout << "nb cameras: " << dataset.num_cameras() << std::endl;
    std::cout << "nb landmarks: " << dataset.num_points() << std::endl;
    std::cout << "nb observations: " << dataset.num_observations() << std::endl;
    std::cout << "nb parameters: " << dataset.num_parameters() << std::endl;
    std::cout << "check: " << dataset.num_cameras() * 9 + dataset.num_points()*3 << std::endl;

    // pose dimension 9, landmark is 3
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;

    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
    );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    auto* cameras = dataset.mutable_cameras();
    std::vector<VertexCamera*> camera_vertices;
    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        auto *c = new VertexCamera();
        c->setId(i);
        c->setEstimate(Camera(cameras + (i*dataset.camera_block_size())));
        optimizer.addVertex(c);
        camera_vertices.push_back(c);
    }

    auto* landmarks = dataset.mutable_points();
    std::vector<VertexLandmark*> landmark_vertices;
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        auto* l = new VertexLandmark();
        l->setId(dataset.num_cameras() + i);
        l->setEstimate(Eigen::Map<Eigen::Vector3d>(landmarks + i*dataset.point_block_size()));
        l->setMarginalized(true);
        optimizer.addVertex(l);
        landmark_vertices.push_back(l);
    }

    auto* observations = dataset.observations();
    auto* cam_indices = dataset.camera_index();
    auto* landmark_indices = dataset.point_index();
    for (int i = 0; i < dataset.num_observations(); ++i)
    {
        auto c = Camera(cameras + (cam_indices[i]*dataset.camera_block_size()));
        auto* e = new EdgeReprojection(c.f, c.k1, c.k2);
        e->setVertex(1, camera_vertices[cam_indices[i]]);
        e->setVertex(0, landmark_vertices[landmark_indices[i]]);
        e->setMeasurement(Eigen::Map<const Eigen::Vector2d>(observations + i*2));
        e->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(e);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(40);

    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        camera_vertices[i]->estimate().set_to(cameras + (i * dataset.camera_block_size()));
        // auto v = camera_vertices[i]->estimate().log();
        // double* pt = cameras + (i * dataset.camera_block_size());
        // for (int j = 0; j < 6; ++j)
        // {
        //     pt[j] = v[j];
        // }
    }
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        Eigen::Vector3d X = landmark_vertices[i]->estimate();
        landmarks[i*3] = X.x();
        landmarks[i*3+1] = X.y();
        landmarks[i*3+2] = X.z();
    }

    dataset.WriteToPLYFile("after_ba_g2o.ply");

    return 0;
}


#ifndef BLUB_PROCEDURAL_VOXEL_EDIT_NOISE_HPP
#define BLUB_PROCEDURAL_VOXEL_EDIT_NOISE_HPP

#include "blub/core/vector.hpp"
#include "blub/math/axisAlignedBox.hpp"
#include "blub/procedural/log/global.hpp"
#include "blub/procedural/voxel/edit/base.hpp"

#include <boost/function/function2.hpp>

#include <numeric>

namespace blub
{
namespace procedural
{
namespace voxel
{
namespace edit
{

/**
 * @brief The noise class generates a random terrain using simplex noise http://en.wikipedia.org/wiki/Simplex_noise .
 * Original code http://webstaff.itn.liu.se/~stegu/simplexnoise/SimplexNoise.java
 */
template <class configType>
class noise : public base<configType>
{
public:
    typedef boost::function<bool (vector3, real&)> t_callbackInterpolation;

    typedef configType t_config;
    typedef base<t_config> t_base;
    typedef sharedPointer<noise> pointer;
    typedef typename t_config::t_data t_voxel;

    /**
     * @brief creates an instance of the class and returns it as shared_ptr<>
     * @param desc Defines the size in which the terrain should get generated.
     * @param scale Gets used to scale the voxel-position before calculating the interpolation
     * @param seed Used before calling std::random_shuffle
     * @param callbackInterpolation Callback for the generated interpolation.
     * @return An instance of the class as shared_ptr<>
     */
    static pointer create(const blub::axisAlignedBox& desc,
                          const vector3& scale,
                          const uint32& seed = 0,
                          const t_callbackInterpolation& callbackInterpolation = [] (const vector3&, real& value) {value*=1024;return true;})
    {
        return pointer(new noise(desc, scale, seed, callbackInterpolation));
    }
    /**
     * @brief ~noise destructor
     */
    virtual ~noise()
    {
        ;
    }

    /**
     * @brief getAxisAlignedBoundingBox returns transformed aab of the to generate voxel
     * @param trans Transform
     * @return
     */
    blub::axisAlignedBox getAxisAlignedBoundingBox(const transform& trans) const override
    {
        return blub::axisAlignedBox(m_aab.getMinimum() + trans.position,
                                    m_aab.getMaximum() + trans.position);
    }

protected:
    static real fade(const real &t)
    {
        return t * t * t * (t * (t * 6 - 15) + 10);
    }
    static real lerp(const real& t, const real& a, const real& b)
    {
        return a + t * (b - a);
    }
    static real grad(const int32& hash, const real& x, const real& y, const real& z) {
        const int32 h = hash & 15;                      // CONVERT LO 4 BITS OF HASH CODE
        const real u = h < 8 ? x : y,                   // INTO 12 GRADIENT DIRECTIONS.
                   v = h < 4 ? y : h == 12 || h == 14 ? x : z;
        return ((h & 1) == 0 ? u : -u) + ((h & 2) == 0 ? v : -v);
    }
    int32 permutation(const int32 index) const
    {
        BASSERT(index >= 0);
        BASSERT(index < 512);
        return m_permutation[index%256];
    }

    /**
     * @brief calculateOneVoxel scales pos by scale set in constructor and calculates the noise.
     * @param pos absolute voxel-position
     * @param resultVoxel gets set if interpolation larger -127
     * @return Returns true if interpolation larger -127
     */
    bool calculateOneVoxel(const vector3& pos, t_voxel* resultVoxel) const override
    {
        real x(pos.x*m_scale.x);
        real y(pos.y*m_scale.y);
        real z(pos.z*m_scale.z);

        const int32 X = static_cast<int32>(math::floor(x)) & 255;                  // FIND UNIT CUBE THAT
        const int32 Y = static_cast<int32>(math::floor(y)) & 255;                  // CONTAINS POINT.
        const int32 Z = static_cast<int32>(math::floor(z)) & 255;
        x -= static_cast<int32>(math::floor(x));                                // FIND RELATIVE X,Y,Z
        y -= static_cast<int32>(math::floor(y));                                // OF POINT IN CUBE.
        z -= static_cast<int32>(math::floor(z));
        const real u = fade(x);                                // COMPUTE FADE CURVES
        const real v = fade(y);                                // FOR EACH OF X,Y,Z.
        const real w = fade(z);
        const int32 A =  permutation(X  )+Y;
        const int32 AA = permutation(A)+Z;
        const int32 AB = permutation(A+1)+Z;      // HASH COORDINATES OF
        const int32 B =  permutation(X+1)+Y;
        const int32 BA = permutation(B)+Z;
        const int32 BB = permutation(B+1)+Z;      // THE 8 CUBE CORNERS,

        real resultInterpolation =   lerp(w, lerp(v, lerp(u, grad(permutation(AA  ), x  , y  , z   ),  // AND ADD
                                                             grad(permutation(BA  ), x-1, y  , z   )), // BLENDED
                                                     lerp(u, grad(permutation(AB  ), x  , y-1, z   ),  // RESULTS
                                                             grad(permutation(BB  ), x-1, y-1, z   ))),// FROM  8
                                             lerp(v, lerp(u, grad(permutation(AA+1), x  , y  , z-1 ),  // CORNERS
                                                             grad(permutation(BA+1), x-1, y  , z-1 )), // OF CUBE
                                                     lerp(u, grad(permutation(AB+1), x  , y-1, z-1 ),
                                                             grad(permutation(BB+1), x-1, y-1, z-1 ))));
        if (!m_callbackInterpolation(pos, resultInterpolation))
        {
            return false;
        }
        const int8 resultCasted(static_cast<int8>(math::clamp<real>(resultInterpolation, -127., 127.)));

        resultVoxel->setInterpolation(resultCasted);

        return true;
    }

    /**
     * @brief noise Contructor - same as create()
     */
    noise(const blub::axisAlignedBox& desc, const vector3& scale, const uint32& seed, const t_callbackInterpolation &callbackInterpolation)
        : m_aab(desc)
        , m_scale(scale)
        , m_callbackInterpolation(callbackInterpolation)
        , m_permutation(256)
    {
        std::srand(seed);
        std::iota(m_permutation.begin(), m_permutation.end(), 0);
        std::random_shuffle(m_permutation.begin(), m_permutation.end());
    }

protected:
    const blub::axisAlignedBox m_aab;
    const vector3 m_scale;
    const t_callbackInterpolation m_callbackInterpolation;

    vector<uint8_t> m_permutation;

};

}
}
}
}

#endif // BLUB_PROCEDURAL_VOXEL_EDIT_NOISE_HPP


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/QR>
#include <manifold/SO3.h>

std::vector<Eigen::Matrix3f> Gs_;
float tau_R_;
Eigen::Matrix3f Sigma_t_;

struct Stats {
  uint32_t N;
  Eigen::Vector3f sum;
  Eigen::Matrix3f outer;
  void reset(double regularize) {
    N = 0;
    sum.fill(0.);
    outer = Eigen::Matrix3f::Identity()*regularize;
  }
  void add(const Eigen::Vector3f& x) {
    ++N;
    sum += x;
    outer += x*x.transpose();
  }
};

Eigen::Matrix<float,6,1> OdomJacobian(
  const Eigen::Vector3f& t, const Eigen::Vector3f& t_prev,
  const Eigen::Matrix3f& R, const Eigen::Matrix3f& R_prev) {

  Eigen::Matrix<float,6,1> J;
  J.topRows<3>() = Sigma_t_.ldlt().solve(t-t_prev);
  for (uint32_t l=0; l<3; ++l)
    J(l+3) = -tau_R_*(R_prev.transpose()*Gs_[l]*R).trace();
  return J;
}

Eigen::Matrix<float,6,6> OdomHessian(
  const Eigen::Vector3f& t, const Eigen::Vector3f& t_prev,
  const Eigen::Matrix3f& R, const Eigen::Matrix3f& R_prev) {
  Eigen::Matrix<float,6,6> H = Eigen::Matrix<float,6,6>::Zero();
  H.topLeftCorner<3,3>() = Sigma_t_.inverse();
  for (uint32_t l=0; l<3; ++l) for (uint32_t m=0; m<3; ++m) {
    Eigen::Matrix3f Glmml = (Gs_[l]*Gs_[m] + Gs_[m]*Gs_[l]);
    H(l+3,m+3) = -0.5*tau_R_*(R_prev.transpose()*Glmml*R).trace();
  }
  return H;
};

void AddObsHessian(
  const Eigen::Vector3f& x,
  const Eigen::Vector3f& y,
  const Eigen::Matrix3f& covInv,
  double w,
  Eigen::Matrix<float,6,6>& H) {

  H.topLeftCorner<3,3>() += w*covInv;

  for (uint32_t m=0; m<3; ++m) {
    H.block<3,1>(0,m+3) += w*covInv*Gs_[m]*y;
  }

  for (uint32_t l=0; l<3; ++l) for (uint32_t m=0; m<3; ++m) {
    Eigen::Matrix3f Glmml = (Gs_[l]*Gs_[m] + Gs_[m]*Gs_[l]);
    H(l+3,m+3) += w*(-y.dot(Gs_[l]*covInv*Gs_[m]*y)
//        + 2.*y.dot(covInv*Glmml*y)
        +0.5*x.dot(covInv*Glmml*y));
  }
  H.bottomLeftCorner<3,3>() = H.topRightCorner<3,3>().transpose();
}

void AddClusterHessian(
    const Stats& ss,
    const Eigen::Matrix3f& R,
    const Eigen::Matrix3f& covInv,
    const Eigen::LDLT<Eigen::Matrix3f>& covLdlt,
    const Eigen::Vector3f& a,
    Eigen::Matrix<float,6,6>& H) {

  H.topLeftCorner<3,3>() += ss.N*covInv;
  for (uint32_t m=0; m<3; ++m) {
    H.block<3,1>(0,m+3) += covLdlt.solve(Gs_[m]*R*ss.sum);
  }
  Eigen::Matrix3f Hw;
  for (uint32_t l=0; l<3; ++l) for (uint32_t m=0; m<3; ++m) {
    Eigen::Matrix3f Glmml = (Gs_[l]*Gs_[m]+Gs_[m]*Gs_[l]);
//    H(l+3,m+3) +=
    Hw(l,m) = 0.5*(R*ss.outer*R.transpose()*(covLdlt.solve(Glmml)
          -2.*Gs_[l]*covLdlt.solve(Gs_[m]))).trace()
      +0.5*a.dot(covLdlt.solve(Glmml*R*ss.sum));
  }
//  std::cout << Hw << std::endl;
  H.bottomRightCorner<3,3>() += Hw;
  H.bottomLeftCorner<3,3>() = H.topRightCorner<3,3>().transpose();
}

void AddObsJacobian( const Eigen::Vector3f& x, const Eigen::Vector3f&
    y, const Eigen::Matrix3f& covInv, float w,
    Eigen::Matrix<float,6,1>& J) {
  J.topRows<3>() += w*covInv*x;
  for (uint32_t l=0; l<3; ++l)
    J(l+3) += w*x.dot(covInv*Gs_[l]*y);
}
void AddClusterJacobian(
    const Stats& ss,
    const Eigen::Matrix3f& R,
    const Eigen::LDLT<Eigen::Matrix3f>& covLdlt,
    const Eigen::Vector3f& a,
    Eigen::Matrix<float,6,1>& J) {

  J.topRows<3>() += covLdlt.solve(R*ss.sum + ss.N*a);
  for (uint32_t l=0; l<3; ++l)
    J(l+3) += (R*ss.outer*R.transpose()*covLdlt.solve(Gs_[l])).trace()
      + a.dot(covLdlt.solve(Gs_[l]*R*ss.sum));
}

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

  Gs_ = std::vector<Eigen::Matrix3f>(3,Eigen::Matrix3f::Zero());
  // so(3) generators
  Gs_[0](1,2) = -1;
  Gs_[0](2,1) = 1;
  Gs_[1](0,2) = 1;
  Gs_[1](2,0) = -1;
  Gs_[2](0,1) = -1;
  Gs_[2](1,0) = 1;
  // Basically ignore priors
  tau_R_ = 0.00001;
  Sigma_t_ = 10000.*Eigen::Matrix3f::Identity();

  uint32_t K = 4;
  uint32_t N = 40;

  double theta0 = 0.*M_PI/180.;
  Eigen::Matrix3f Rmu;
  Rmu << 1, 0, 0,
         0, cos(theta0), sin(theta0),
         0, -sin(theta0), cos(theta0);
  Eigen::Matrix3f R = Rmu;
  Eigen::Matrix3f covInv = 0.01*Eigen::Matrix3f::Identity();

  Eigen::Vector3f t(0,0,0.);
  Eigen::Vector3f t_prev(0,0,0);

  std::vector<Eigen::Vector3f> mus;
  mus.push_back(Eigen::Vector3f(1.,0.,0.));
  mus.push_back(Eigen::Vector3f(0.,1.,0.));
  mus.push_back(Eigen::Vector3f(0.,0.,1.));
  mus.push_back(Eigen::Vector3f(0.3,0.3,0.3));

  double theta = 15.*M_PI/180.;
  double phi = 25*M_PI/180.;
  std::vector<Eigen::Vector3f> ps;
  std::vector<uint32_t> zs;
  Eigen::Vector3f t_true = Eigen::Vector3f(0.1,.1,0.1);
  Eigen::Matrix3f R_true;
  R_true << 1, 0, 0,
            0,  cos(theta), sin(theta),
            0, -sin(theta), cos(theta);
  Eigen::Matrix3f R2;
  R2     << cos(phi),  0,  sin(phi),
            0,           1., 0,
            -sin(phi), 0,  cos(phi);
  R_true *= R2;
//  R_true << sin(theta)*cos(phi), cos(theta)*cos(phi), -sin(phi),
//            sin(theta)*sin(phi), cos(theta)*sin(phi), cos(phi),
//            cos(theta), -sin(theta), 0.;

  for (uint32_t i=0; i<N/K; ++i) {
    ps.push_back(Eigen::Vector3f(1.,0,0));
    ps.back() = R_true*ps.back() + t_true;
    ps.push_back(Eigen::Vector3f(0,1.,0.));
    ps.back() = R_true*ps.back() + t_true;
    ps.push_back(Eigen::Vector3f(0,0.,1.));
    ps.back() = R_true*ps.back() + t_true;
    ps.push_back(Eigen::Vector3f(0.3,0.3,0.3));
    ps.back() = R_true*ps.back() + t_true;
    zs.push_back(0);
    zs.push_back(1);
    zs.push_back(2);
    zs.push_back(3);
  }

  std::vector<Stats> ss(K);
  for (uint32_t k=0; k<K; ++k) ss[k].reset(0.);
  for (uint32_t i=0; i<N; ++i) {
    ss[zs[i]].add(ps[i]);
  }
  for (uint32_t k=0; k<K; ++k) {
    std::cout << k <<  std::endl;
    std::cout << ss[k].N << std::endl;
    std::cout << ss[k].sum.transpose() << std::endl;
    std::cout << ss[k].outer << std::endl;
  }
  Eigen::LDLT<Eigen::Matrix3f> covLdlt(covInv.inverse());

  std::cout << "Using SO(3) formulation first order -------------------"
    << std::endl;

  R = Rmu;
  t.fill(0);
  float delta = 1.;
  float f_prev = 1e99;
  float f = -tau_R_*(Rmu.transpose()*R).trace()
    + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
  for (uint32_t i=0; i<N; ++i) {
    const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
    f += 0.5*x.dot(covInv*x);
  }
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100000; ++it) {
    Eigen::Matrix<float,6,1> J = OdomJacobian(t, t_prev, R, Rmu);
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f y = R*ps[i];
      const Eigen::Vector3f x = y+t-mus[zs[i]];
      AddObsJacobian(x,y,covInv,1.,J);
    }
    J = -delta*J;

    t = t + J.topRows<3>();
    Eigen::Vector3f Jw = J.bottomRows<3>();
    SO3f R_(R);
    R = (R_ + Jw).matrix();

    f_prev = f;
    f = -tau_R_*(Rmu.transpose()*R).trace()
      + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
      f += 0.5*x.dot(covInv*x);
    }
    if (it%100==0)
      std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9)
      break;
  }

  std::cout << " -- d angle " << acos(((R*R_true).trace()-1)*0.5)*180./M_PI
            << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << "t " << std::endl << t.transpose() << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;
  std::cout << "inverses" << std::endl;
  std::cout << R.transpose() << std::endl;
  std::cout << "t " << (-R.transpose()*t).transpose() << std::endl;
  std::cout << "true" << std::endl;
  std::cout << R_true << std::endl;
  std::cout << "t " << t_true.transpose() << std::endl;

  std::cout << "Using SO(3) formulation first order SS -------------------"
    << std::endl;

  R = Rmu;
  t.fill(0);
  delta = 1.;
  f_prev = 1e99;
  f = -tau_R_*(Rmu.transpose()*R).trace()
    + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
  for (uint32_t i=0; i<N; ++i) {
    const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
    f += 0.5*x.dot(covInv*x);
  }
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100000; ++it) {
    Eigen::Matrix<float,6,1> J = OdomJacobian(t, t_prev, R, Rmu);
    for (uint32_t k=0; k<K; ++k) {
      const Eigen::Vector3f a = t-mus[k];
      AddClusterJacobian(ss[k],R,covLdlt,a,J);
    }
    J = -delta*J;

    t = t + J.topRows<3>();
    Eigen::Vector3f Jw = J.bottomRows<3>();
    SO3f R_(R);
    R = (R_ + Jw).matrix();

    f_prev = f;
    f = -tau_R_*(Rmu.transpose()*R).trace()
      + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
      f += 0.5*x.dot(covInv*x);
    }
    if (it%100==0)
      std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9)
      break;
  }

  std::cout << " -- d angle " << acos(((R*R_true).trace()-1)*0.5)*180./M_PI
            << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << "t " << std::endl << t.transpose() << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;
  std::cout << "inverses" << std::endl;
  std::cout << R.transpose() << std::endl;
  std::cout << "t " << (-R.transpose()*t).transpose() << std::endl;
  std::cout << "true" << std::endl;
  std::cout << R_true << std::endl;
  std::cout << "t " << t_true.transpose() << std::endl;

  std::cout << "Using SO(3) formulation second order -------------------"
    << std::endl;
  R = Rmu;
  t.fill(0);
  delta = 0.5;
  f_prev = 1e99;
  f = -tau_R_*(Rmu.transpose()*R).trace()
    + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
  for (uint32_t i=0; i<N; ++i) {
    const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
    f += 0.5*x.dot(covInv*x);
  }
  std::cout << "f=" << f << std::endl;
  Eigen::Vector3f tPrev = t;
  Eigen::Matrix3f RPrev = R;
  for (uint32_t it=0; it<2000; ++it) {
    Eigen::Matrix<float,6,1> J = OdomJacobian(t, t_prev, R, Rmu);
    Eigen::Matrix<float,6,6> H = OdomHessian(t, t_prev, R, Rmu);
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f y = R*ps[i];
      const Eigen::Vector3f x = y+t-mus[zs[i]];
      AddObsJacobian(x,y,covInv,1.,J);
      AddObsHessian(x,y,covInv,1.,H);
    }
    if(it < 10) {
      std::cout << J.transpose() << std::endl;
      std::cout << H << std::endl;
      std::cout << - H.ldlt().solve(J).transpose() << std::endl;
    }
//    std::cout << H << std::endl;
//    std::cout << "J " << J.transpose() << std::endl;
    J = - delta*H.ldlt().solve(J);
//    std::cout << "dx " << J.transpose() << std::endl;

    RPrev = R;
    tPrev = t;

    t = t + J.topRows<3>();
    Eigen::Vector3f Jw = J.bottomRows<3>();
    SO3f R_(R);
    R = (R_ + Jw).matrix();

    f_prev = f;
    f = -tau_R_*(Rmu.transpose()*R).trace()
      + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
      f += 0.5*x.dot(covInv*x);
    }
    if (it%100==0)
      std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9) {
      std::cout << "@" << it << ": f=" << f << " f_prev=" << f_prev << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
      break;
    }
  }
  if (f_prev < f) {
    R = RPrev;
    t = tPrev;
  }
  std::cout << " -- d angle " << acos(((R*R_true).trace()-1)*0.5)*180./M_PI
            << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << "t " << t.transpose() << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;

  std::cout << "inverses" << std::endl;
  std::cout << R.transpose() << std::endl;
  std::cout << "t " << (-R.transpose()*t).transpose() << std::endl;
  std::cout << "true" << std::endl;
  std::cout << R_true << std::endl;
  std::cout << "t " << t_true.transpose() << std::endl;

  std::cout << "Using SO(3) formulation second order using SS -------------------"
    << std::endl;
  R = Rmu;
  t.fill(0);
  delta = 0.5;
  f_prev = 1e99;
  f = -tau_R_*(Rmu.transpose()*R).trace()
    + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
  for (uint32_t i=0; i<N; ++i) {
    const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
    f += 0.5*x.dot(covInv*x);
  }
  std::cout << "f=" << f << std::endl;
  tPrev = t;
  RPrev = R;
  std::cout << " -- d angle " << acos(((R*R_true).trace()-1)*0.5)*180./M_PI
    << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
  for (uint32_t it=0; it<2000; ++it) {
    Eigen::Matrix<float,6,1> J = OdomJacobian(t, t_prev, R, Rmu);
    Eigen::Matrix<float,6,6> H = OdomHessian(t, t_prev, R, Rmu);
    for (uint32_t k=0; k<K; ++k) {
      const Eigen::Vector3f a = t-mus[k];
      AddClusterJacobian(ss[k],R,covLdlt,a,J);
      AddClusterHessian(ss[k],R,covInv,covLdlt,a,H);
    }
    if(it < 10) {
      std::cout << J.transpose() << std::endl;
      std::cout << H << std::endl;
      std::cout << - H.ldlt().solve(J).transpose() << std::endl;
    }
//    std::cout << H << std::endl;
//    std::cout << "J " << J.transpose() << std::endl;
    J = - delta*H.ldlt().solve(J);
//    std::cout << "dx " << J.transpose() << std::endl;

    RPrev = R;
    tPrev = t;

    t = t + J.topRows<3>();
    Eigen::Vector3f Jw = J.bottomRows<3>();
    SO3f R_(R);
    R = (R_ + Jw).matrix();

    f_prev = f;
    f = -tau_R_*(Rmu.transpose()*R).trace()
      + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
      f += 0.5*x.dot(covInv*x);
    }
    if (it%1==0) {
      std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
      std::cout << " -- d angle " << acos(std::min(1.,((R*R_true).trace()-1)*0.5))*180./M_PI
        << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
    }
    if ((f_prev - f)/fabs(f) < 1e-9) {
      std::cout << "@" << it << ": f=" << f << " f_prev=" << f_prev << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
      break;
    }
  }
  if (f_prev < f) {
    R = RPrev;
    t = tPrev;
  }
  std::cout << " -- d angle " << acos(((R*R_true).trace()-1)*0.5)*180./M_PI
            << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << "t " << t.transpose() << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;

  std::cout << "inverses" << std::endl;
  std::cout << R.transpose() << std::endl;
  std::cout << "t " << (-R.transpose()*t).transpose() << std::endl;
  std::cout << "true" << std::endl;
  std::cout << R_true << std::endl;
  std::cout << "t " << t_true.transpose() << std::endl;

}


#include <Eigen/Core>

#include <igl/readOBJ.h>
#include <igl/point_mesh_squared_distance.h>

extern "C" {

Eigen::MatrixXd V1, V2;
Eigen::MatrixXi F1, F2;
void LoadObj(const char* filename, int mesh_id) {
    if (mesh_id == 1)
        igl::readOBJ(filename, V1, F1);
    else
        igl::readOBJ(filename, V2, F2);
}

double OneWayChamferDistance(const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& F1,
    const Eigen::MatrixXd& V2,
    const Eigen::MatrixXi& F2,
    int resample_points) {
    Eigen::VectorXd sqrD;
    Eigen::VectorXi I;
    Eigen::MatrixXd C;
    if (resample_points) {

        std::vector<double> areas(F1.rows() + 1, 0);
        double total_area = 0;
        for (int i = 0; i < F1.rows(); ++i) {
            const Eigen::Vector3d& v1 = V1.row(F1(i, 0));
            const Eigen::Vector3d& v2 = V1.row(F1(i, 1));
            const Eigen::Vector3d& v3 = V1.row(F1(i, 2));
            double area = ((v2 - v1).cross(v3 - v1)).norm();
            areas[i + 1] = area;
            total_area += area;
        }
        for (int i = 1; i < areas.size(); ++i)
            areas[i] += areas[i - 1];

        Eigen::MatrixXd NV(resample_points, 3);
        for (int i = 0; i < resample_points; ++i) {
            double r = rand() / (double)RAND_MAX * areas.back();
            auto lower = std::lower_bound(areas.begin(), areas.end(), r) - 1;
            int tri_idx = lower - areas.begin();
            if (tri_idx < 0) {
                tri_idx = 0;
            }
            double u = rand() / (double)RAND_MAX;
            double v = rand() / (double)RAND_MAX;
            if (u + v > 1) {
                u = 1 - u;
                v = 1 - v;
            }
            auto v1 = V1.row(F1(tri_idx, 0));
            auto v2 = V1.row(F1(tri_idx, 1));
            auto v3 = V1.row(F1(tri_idx, 2));

            NV.row(i) = u * (v2 - v1) + v * (v3 - v1) + v1;
        }
        igl::point_mesh_squared_distance(NV,V2,F2,sqrD,I,C);
    } else {
        igl::point_mesh_squared_distance(V1,V2,F2,sqrD,I,C);
    }
    double distance = 0;
    for (int i = 0; i < sqrD.size(); ++i) {
        distance += sqrt(sqrD[i]);
    }
    double ans = distance / sqrD.size();
    return ans;
}

double OneWayChamfer(int src_id, int resample_points = 0) {
    if (src_id == 1) {
        return OneWayChamferDistance(V1, F1, V2, F2, resample_points);
    }
    return OneWayChamferDistance(V2, F2, V1, F1, resample_points);
}

double TwoWayChamfer(int resample_points = 0) {
    double distance1 = OneWayChamferDistance(V1, F1, V2, F2, resample_points);
    double distance2 = OneWayChamferDistance(V2, F2, V1, F1, resample_points);

    return distance1 + distance2;
}

void SetMesh(double* vertices, int* faces, int num_V, int num_F, int id) {
    auto &V = (id == 1) ? V1 : V2;
    auto &F = (id == 1) ? F1 : F2;

    V = Eigen::MatrixXd(num_V, 3);
    for (int i = 0; i < num_V; ++i) {
        V.row(i) = Eigen::Vector3d(vertices[i * 3],vertices[i * 3 + 1],vertices[i * 3 + 2]);
    }

    F = Eigen::MatrixXi(num_F, 3);
    for (int i = 0; i < num_F; ++i) {
        F.row(i) = Eigen::Vector3i(faces[i * 3], faces[i * 3 + 1], faces[i * 3 + 2]);
    }
}

double AreaRatio(const char* f1, const char* f2) {
    LoadObj(f1, 2);
    LoadObj(f2, 1);
    {
        Eigen::MatrixXd V2_buf(V2.rows() * 2, 3);
        Eigen::MatrixXi F2_buf(F2.rows() * 2, 3);
        for (int i = 0; i < V2.rows(); ++i) {
            V2_buf.row(i) = V2.row(i);
            V2_buf.row(i + V2.rows()) = V2.row(i);
            V2_buf(i + V2.rows(), 0) = -V2_buf(i + V2.rows(), 0);
        }
        for (int i = 0; i < F2.rows(); ++i) {
            F2_buf.row(i) = F2.row(i);
            F2_buf.row(i + F2.rows()) = F2.row(i);
            for (int j = 0; j < 3; ++j)
                F2_buf(i + F2.rows(), j) += V2.rows();
        }
        F2 = F2_buf;
        V2 = V2_buf;
    }
    {
        Eigen::VectorXd sqrD;
        Eigen::VectorXi I;
        Eigen::MatrixXd C;
        std::vector<double> areas(F1.rows() + 1, 0);
        double total_area = 0;
        for (int i = 0; i < F1.rows(); ++i) {
            const Eigen::Vector3d& v1 = V1.row(F1(i, 0));
            const Eigen::Vector3d& v2 = V1.row(F1(i, 1));
            const Eigen::Vector3d& v3 = V1.row(F1(i, 2));
            double area = ((v2 - v1).cross(v3 - v1)).norm();
            areas[i + 1] = area;
            total_area += area;
        }
        for (int i = 1; i < areas.size(); ++i)
            areas[i] += areas[i - 1];

        int resample_points = 100000;
        Eigen::MatrixXd NV(resample_points, 3);
        for (int i = 0; i < resample_points; ++i) {
            double r = rand() / (double)RAND_MAX * areas.back();
            auto lower = std::lower_bound(areas.begin(), areas.end(), r) - 1;
            int tri_idx = lower - areas.begin();
            if (tri_idx < 0) {
                tri_idx = 0;
            }
            double u = rand() / (double)RAND_MAX;
            double v = rand() / (double)RAND_MAX;
            if (u + v > 1) {
                u = 1 - u;
                v = 1 - v;
            }
            auto v1 = V1.row(F1(tri_idx, 0));
            auto v2 = V1.row(F1(tri_idx, 1));
            auto v3 = V1.row(F1(tri_idx, 2));

            NV.row(i) = u * (v2 - v1) + v * (v3 - v1) + v1;
        }

        igl::point_mesh_squared_distance(NV,V2,F2,sqrD,I,C);
        int inliers = 0;
        for (int i = 0; i < sqrD.size(); ++i) {
            double dis = sqrt(sqrD[i]);
            if (dis < 0.03)
                inliers += 1;
        }
        return (double)inliers / sqrD.size();
    }
}

};


// // [[Rcpp::depends(cgal4h)]]
// #include <Rcpp.h>
//
// #include <CGAL/Cartesian.h>
// #include <CGAL/MP_Float.h>
// #include <CGAL/Quotient.h>
// #include <CGAL/Arr_linear_traits_2.h>
// #include <CGAL/Arr_segment_traits_2.h>
// #include <CGAL/Arrangement_2.h>
// // typedef CGAL::Quotient<CGAL::MP_Float> Number_type;
// // typedef CGAL::Cartesian<Number_type> Kernel;
// // // typedef CGAL::Arr_segment_traits_2<Kernel> Traits_2;
// // typedef CGAL::Arr_linear_traits_2<Kernel> Traits_2;
// // typedef Traits_2::Point_2 Point_2;
// // typedef Traits_2::X_monotone_curve_2 Segment_2;
// // typedef CGAL::Arrangement_2<Traits_2> Arrangement_2;
// // #include <CGAL/Cartesian.h>
// #include <CGAL/Exact_rational.h>
// #include <CGAL/Arr_segment_traits_2.h>
// #include <CGAL/Arr_extended_dcel.h>
// #include <CGAL/Arrangement_2.h>
// #include <CGAL/graph_traits_dual_arrangement_2.h>
// #include <CGAL/Arr_face_index_map.h>
// #include <climits>
// #include <boost/graph/breadth_first_search.hpp>
// #include <boost/graph/visitors.hpp>
//
// // A property map that reads/writes the information to/from the extended
// // face.
// template <typename Arrangement, class Type> class Extended_face_property_map {
// public:
//   typedef typename Arrangement::Face_handle       Face_handle;
//   // Boost property type definitions.
//   typedef boost::read_write_property_map_tag      category;
//   typedef Type                                    value_type;
//   typedef value_type&                             reference;
//   typedef Face_handle                             key_type;
//   // The get function is required by the property map concept.
//   friend reference get(const Extended_face_property_map& /* map */,
//                        key_type key)
//   { return key->data(); }
//   // The put function is required by the property map concept.
//   friend void put(Extended_face_property_map /* map */,
//                   key_type key, value_type val)
//   { key->set_data(val); }
// };
//
//
//
// typedef CGAL::Cartesian<CGAL::Exact_rational>                Kernel;
// typedef CGAL::Arr_segment_traits_2<Kernel>                   Traits_2;
// typedef CGAL::Arr_face_extended_dcel<Traits_2, unsigned int> Dcel;
// typedef CGAL::Arrangement_2<Traits_2, Dcel>                  Ex_arrangement;
// typedef CGAL::Dual<Ex_arrangement>                           Dual_arrangement;
// typedef CGAL::Arr_face_index_map<Ex_arrangement>             Face_index_map;
// typedef Extended_face_property_map<Ex_arrangement,unsigned int>
//                                                              Face_property_map;
// typedef Kernel::Point_2                                      Point_2;
// typedef Kernel::Segment_2                                    Segment_2;
//
//
//
// // void print_ccb (Arrangement_2::Ccb_halfedge_const_circulator circ) {
// //   Arrangement_2::Ccb_halfedge_const_circulator curr = circ;
// //   Rcpp::Rcout << "(" << curr->source()->point() << ")";
// //   do {
// //     Arrangement_2::Halfedge_const_handle he = curr->handle();
// //     Rcpp::Rcout << " [" << he->curve() << "] "
// //               << "(" << he->target()->point() << ")";
// //   } while (++curr != circ);
// //   Rcpp::Rcout << std::endl;
// // }
//
//
// // void print_face (Arrangement_2::Face_const_handle f) {
// //   // Print the outer boundary.
// //   if (f->is_unbounded())
// //     Rcpp::Rcout << "Unbounded face. " << std::endl;
// //   // else {
// //   //   Rcpp::Rcout << "Outer boundary: ";
// //   //   print_ccb (f->outer_ccb());
// //   // }
// //   // Print the boundary of each of the holes.
// //   // Arrangement_2::Hole_const_iterator hi;
// //   int index = 1;
// //   // for (hi = f->holes_begin(); hi != f->holes_end(); ++hi, ++index) {
// //   //   Rcpp::Rcout << " Hole #" << index << ": ";
// //   //   print_ccb (*hi);
// //   // }
// //   // Print the isolated vertices.
// //   Arrangement_2::Isolated_vertex_const_iterator iv;
// //   for (iv = f->isolated_vertices_begin(), index = 1;
// //        iv != f->isolated_vertices_end(); ++iv, ++index){
// //     Rcpp::Rcout << " Isolated vertex #" << index << ": "
// //               << "(" << iv->point() << ")" << std::endl;
// //   }
// // }
//
//
//
// // [[Rcpp::export]]
// Rcpp::NumericVector construct_arrangement(Rcpp::NumericVector x) {
// // 	Arrangement_2 arr;
// //   Segment_2 cv[3];
// //   Point_2 p1 (0, 0), p2 (0, 4), p3 (4, 0);
// //   cv[0] = Segment_2 (p1, p2);
// //   cv[1] = Segment_2 (p2, p3);
// //   cv[2] = Segment_2 (p3, p1);
// //   CGAL::insert (arr, &cv[0], &cv[3]);
// //   // ArrangementDcel::Face_const_iterator fit;
// //   Rcpp::Rcout << arr.number_of_faces() << " faces:" << std::endl;
// //   for (auto fit = arr.faces_begin(); fit != arr.faces_end(); ++fit)
// //     print_face (fit);
// //   return x * 2;
// // Construct an arrangement of seven intersecting line segments.
//   Point_2 p1(1, 1), p2(1, 4), p3(2, 2), p4(3, 7), p5(4, 4), p6(7, 1), p7(9, 3);
//   Ex_arrangement  arr;
//   insert(arr, Segment_2(p1, p6));
//   insert(arr, Segment_2(p1, p4));  insert(arr, Segment_2(p2, p6));
//   insert(arr, Segment_2(p3, p7));  insert(arr, Segment_2(p3, p5));
//   insert(arr, Segment_2(p6, p7));  insert(arr, Segment_2(p4, p7));
//   // Create a mapping of the arrangement faces to indices.
//   Face_index_map  index_map(arr);
//   // Perform breadth-first search from the unbounded face, using the event
//   // visitor to associate each arrangement face with its discover time.
//   unsigned int    time = 0;
//   boost::breadth_first_search(Dual_arrangement(arr), arr.unbounded_face(),
//                               boost::vertex_index_map(index_map).visitor
//                               (boost::make_bfs_visitor
//                                (stamp_times(Face_property_map(), time,
//                                             boost::on_discover_vertex()))));
//   // Print the discover time of each arrangement face.
//   Ex_arrangement::Face_iterator  fit;
//   for (fit = arr.faces_begin(); fit != arr.faces_end(); ++fit) {
//     std::cout << "Discover time " << fit->data() << " for ";
//     if (fit != arr.unbounded_face()) {
//       std::cout << "face ";
//       // print_ccb<Ex_arrangement>(fit->outer_ccb());
//     }
//     else std::cout << "the unbounded face." << std::endl;
//   }
// 	//arr.unbounded_faces_begin()
// }
//
//
// /*** R
// construct_arrangement(42)
// */


#include <boost/multiprecision/gmp.hpp>
#include <iostream>
#include <stdio.h>
#include <string.h>

void findy(mpf_t y, long unsigned int nn, int precision)
{
	mpz_t n,n1,y1,y2;
	mpz_inits(n,n1,y1,y2,NULL);

	mpz_set_ui( n,  nn);
	mpz_set_ui(n1,nn+1);

	mpz_pow_ui(y1,n1,nn+1);
	mpz_pow_ui(y2,n ,nn+1);

	mpf_t y3,y4;
	mpf_init2(y3,precision);
	mpf_init2(y4,precision);
	mpf_set_z(y3,y1);
	mpf_set_z(y4,y2);

	mpf_div(y,y3,y4);

	mpz_clear(n);
	mpz_clear(n1);
	mpz_clear(y1);
	mpz_clear(y2);
	mpf_clear(y3);
	mpf_clear(y4);
}

void findx(mpf_t x, long unsigned int nn, int precision)
{
	mpz_t n,n1,x1,x2;
	mpz_inits(n,n1,x1,x2,NULL);

	mpz_set_ui( n,  nn);
	mpz_set_ui(n1,nn+1);

	mpz_pow_ui(x1,n1,nn);
	mpz_pow_ui(x2,n ,nn);

	/*
	std::cout << "X1 = ";
	mpz_out_str(stdout, 10, x1);
	std::cout << std::endl;

	std::cout << "X2 = ";
	mpz_out_str(stdout, 10, x2);
	std::cout << std::endl;
	*/
	mpf_t x3,x4;
	mpf_init2(x3,precision);
	mpf_init2(x4,precision);
	mpf_set_z(x3,x1);
	mpf_set_z(x4,x2);

	mpf_div(x,x3,x4);

	mpz_clear(n);
	mpz_clear(n1);
	mpz_clear(x1);
	mpz_clear(x2);
	mpf_clear(x3);
	mpf_clear(x4);
}

int getsizef(mpf_t x)
{
	mp_exp_t exponent;
	char* c = mpf_get_str(NULL,&exponent,10,0,x);

	return strlen(c);
}

int main()
{
	using namespace boost::multiprecision;

	int precision = 262144;
	char* c;

	mpf_t x,y;
	mpf_init2(x,precision);
	mpf_init2(y,precision);
	mp_exp_t exponent;

	for(long unsigned int nn = 1; nn<=5000; nn++)
	{
		findx(x,nn,precision);
		findy(y,nn,precision);

		//std::cout << "X = ";
		//mpf_out_str(stdout, 10, 10, x);
		//std::cout << std::endl;
		//int sz = getsizef(x);
		c = mpf_get_str(NULL,&exponent,10,0,x);
		int sz = strlen(c);
		if(sz < 78911)//315651)	//<precision/3.33
		{
			std::cout << "N = " << nn << "\t" << "# of digits of X = " << sz << " : ";// << std::endl;
			if(sz > 7) for(int i=0; i<10; i++) std::cout << c[sz-10+i];
			std::cout << "\t";

			bool ok = true;
			for (int i=sz-8; i<sz; i++)
				for (int j= i+1; j<sz; j++)
					if(c[i] == c[j]) ok = false;

			if(ok) std::cout << "OK!!!!!";
			else std::cout << "NO";
			std::cout << std::endl;
		}

		c = mpf_get_str(NULL,&exponent,10,0,y);
		sz = strlen(c);
		if(sz < 78911)
		{
			std::cout << "N = " << nn << "\t" << "# of digits of Y = " << sz << " : ";// << std::endl;
			if(sz > 7) for(int i=0; i<10; i++) std::cout << c[sz-10+i];
			std::cout << "\t";

			bool ok = true;
			for (int i=sz-8; i<sz; i++)
				for (int j= i+1; j<sz; j++)
					if(c[i] == c[j]) ok = false;

			if(ok) std::cout << "OK!!!!!";
			else std::cout << "NO";
			std::cout << std::endl;
		}

	}

	mpf_clear(x);

	return 0;
}