/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

/**
 * \file
 *
 * Matrix factorization with the Alternative Least Squares (ALS)  - parallel coordinate descent algorithm.
 * See the papers:
 * H.-F. Yu, C.-J. Hsieh, S. Si, I. S. Dhillon, Scalable Coordinate Descent Approaches to Parallel Matrix Factorization for Recommender Systems. IEEE International Conference on Data Mining(ICDM), December 2012.
 * Steffen Rendle, Zeno Gantner, Christoph Freudenthaler, and Lars Schmidt-Thieme. 2011. Fast context-aware recommendations with factorization machines. In Proceedings of the 34th international ACM SIGIR conference on Research and development in Information Retrieval (SIGIR '11). ACM, New York, NY, USA, 635-644.
 * Written by Danny Bickson, CMU
 */

#include <graphlab/util/stl_util.hpp>
#include <graphlab/util/timer.hpp>
#include <graphlab.hpp>
#include <graphlab/engine/gl3engine.hpp>
#include <Eigen/Dense>
#include "eigen_serialization.hpp"
#include <graphlab/macros_def.hpp>

typedef Eigen::VectorXd vec_type;

#define ALS_COORD_MAP_REDUCE 0
#define ALS_COORD_TRANSFORM 1
//when using negative node id range, we are not allowed to use
//0 and 1 so we add 2.
const static int SAFE_NEG_OFFSET=2;
const static int regnormal = 0;
static bool debug;
int max_iter = 10;
double maxval = 1e100;
double minval = -1e100;
std::string predictions;
bool isuser(uint node){
  return ((int)node) >= 0;
}

/**
 * \ingroup toolkit_matrix_factorization
 *
 * \brief the vertex data type which contains the latent pvec.
 *
 * Each row and each column in the matrix corresponds to a different
 * vertex in the SGD graph.  Associated with each vertex is a pvec
 * (vector) of latent parameters that represent that vertex.  The goal
 * of the SGD algorithm is to find the values for these latent
 * parameters such that the non-zero entries in the matrix can be
 * predicted by taking the dot product of the row and column pvecs.
 */
struct vertex_data {
  /**
   * \brief A shared "constant" that specifies the number of latent
   * values to use.
   */
  static size_t NLATENT;
  /** \brief The latent pvec for this vertex */
  vec_type pvec;
  vec_type prev;
  float z;
  int t; //index inside the latent feature vector

  /**
   * \brief Simple default constructor which randomizes the vertex
   *  data
   */
  vertex_data() : t(0),z(0) { if (debug) pvec = vec_type::Ones(NLATENT); else randomize(); prev = vec_type::Zero(NLATENT); }
  /** \brief Randomizes the latent pvec */
  void randomize() { pvec.resize(NLATENT); pvec.setRandom(); }
  /** \brief Save the vertex data to a binary archive */
  void save(graphlab::oarchive& arc) const {
    arc << pvec << t << prev << z;
  }
  /** \brief Load the vertex data from a binary archive */
  void load(graphlab::iarchive& arc) {
    arc >> pvec >> t >> prev >> z;
  }
}; // end of vertex data

std::size_t hash_value(vertex_data const& b) {
  return (size_t)b.pvec[0]*1000;
}

/**
 * \brief The edge data stores the entry in the matrix.
 *
 * In addition the edge data sgdo stores the most recent error estimate.
 */
struct edge_data : public graphlab::IS_POD_TYPE {
  /**
   * \brief The type of data on the edge;
   *
   * \li *Train:* the observed value is correct and used in training
   * \li *Validate:* the observed value is correct but not used in training
   * \li *Predict:* The observed value is not correct and should not be
   *        used in training.
   */
  enum data_role_type { TRAIN, VALIDATE, PREDICT  };

  /** \brief the observed value for the edge */
  float obs;

  /** \brief cached value for A_ij - prediction */
  float R_ij;

  /** \brief The train/validation/test designation of the edge */
  data_role_type role;

  /** \brief basic initialization */
  edge_data(float obs = 0, data_role_type role = PREDICT) :
    obs(obs), role(role), R_ij(0) { }

}; // end of edge data

std::size_t hash_value(edge_data const& b) {
  return boost::hash_value(b.obs);
}

/**
 * \brief The graph type is defined in terms of the vertex and edge
 * data.
 */
typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;
typedef graphlab::gl3engine<graph_type> engine_type;

bool isuser_node(const graph_type::vertex_type& vertex){
  return isuser(vertex.id());
}

/**
 * \brief The graph loader function is a line parser used for
 * distributed graph construction.
 */
inline bool graph_loader(graph_type& graph,
                         const std::string& filename,
                         const std::string& line) {
  ASSERT_FALSE(line.empty());
  // Determine the role of the data
  edge_data::data_role_type role = edge_data::TRAIN;
  if(boost::ends_with(filename,".validate")) role = edge_data::VALIDATE;
  else if(boost::ends_with(filename, ".predict")) role = edge_data::PREDICT;
  // Parse the line
  std::stringstream strm(line);
  graph_type::vertex_id_type source_id(-1), target_id(-1);
  float obs(0);
  strm >> source_id >> target_id;

  // for test files (.predict) no need to read the actual rating value.
  if(role == edge_data::TRAIN || role == edge_data::VALIDATE) {
    strm >> obs;
  }
  target_id = -(graphlab::vertex_id_type(target_id + SAFE_NEG_OFFSET));

  // Create an edge and add it to the graph
  graph.add_edge(source_id, target_id, edge_data(obs, role));
  return true; // successful load
} // end of graph_loader

double LAMBDA = 0.001;

class gather_type {
public:
  double numerator;
  double denominator;

  gather_type() {
    numerator = 0;
    denominator = 0;
  }

  gather_type(double numerator, double denominator) : numerator(numerator),
     denominator(denominator){
  }

  /** \brief Save the values to a binary archive */
  void save(graphlab::oarchive& arc) const { arc << numerator << denominator; }

  /** \brief Read the values from a binary archive */
  void load(graphlab::iarchive& arc) { arc >> numerator >> denominator; }

  /**
   * sums up values
   */
  gather_type& operator+=(const gather_type& other) {
    numerator += other.numerator;
    denominator += other.denominator;
    return *this;
  } // end of operator+=

}; // end of gather type

gather_type als_coord_map(const graph_type::vertex_type& center,
                         graph_type::edge_type& edge,
                         const graph_type::vertex_type& other) {

   if (center.data().t == 0){
     double prediction = center.data().pvec.dot(other.data().pvec);
     prediction = std::min(prediction, maxval);
     prediction = std::max(prediction, minval);
     edge.data().R_ij = edge.data().obs - prediction;
   }
   //compute numerator of equation (6) in ICDM paper above
   //             (A_ij        - w_i^T*h_j  + wit          * h_jt        )*h_jt
   gather_type ret((edge.data().R_ij
                               + center.data().pvec[center.data().t] * other.data().pvec[center.data().t])*other.data().pvec[center.data().t],
   //compute denominator of equation (6) in ICDM paper above
   //h_jt^2
     pow(other.data().pvec[center.data().t], 2));
   return ret;

}

void als_coord_transform(const graph_type::vertex_type& center,
                         graph_type::edge_type& edge,
                         const graph_type::vertex_type& other) {
   //update using equation (7) in ICDM paper
   //R_ij     -= (z             - w_it         )*h_jt
   edge.data().R_ij -= (center.data().z - center.data().prev[center.data().t])*other.data().pvec[center.data().t];
}

//sum up two numerators and denomenators
void als_coord_combine(gather_type& v1, const gather_type& v2) {
    v1 += v2;
}

//the main update function
void als_coord_function(engine_type::context_type& context,
                  graph_type::vertex_type& vertex) {

   double regularization = LAMBDA;
   for (vertex.data().t=0; vertex.data().t< (int)vertex_data::NLATENT; vertex.data().t++){
     gather_type frac =  context.map_reduce<gather_type>(ALS_COORD_MAP_REDUCE, graphlab::ALL_EDGES);
     assert(frac.denominator > 0);
     vertex.data().z = (frac.numerator/(frac.denominator+regularization));
     vertex.data().prev = vertex.data().pvec;
     //update using equation (8) in ICDM paper
     //w_it                              = z;
     vertex.data().pvec[vertex.data().t] = vertex.data().z;

     //update the cached R_ij using equation (7) in ICDM paper
     context.edge_transform(ALS_COORD_TRANSFORM, graphlab::ALL_EDGES);
   }

}

/**
 * \brief Given an edge compute the error associated with that edge
 */
double extract_l2_error(const graph_type::edge_type & edge) {
  double pred =
      edge.source().data().pvec.dot(edge.target().data().pvec);
  double rmse = (edge.data().obs - pred) * (edge.data().obs - pred);
  return rmse;
} // end of extract_l2_error

size_t vertex_data::NLATENT = 20;
/**
 * \brief The prediction saver is used by the graph.save routine to
 * output the final predictions back to the filesystem.
 */
struct prediction_saver {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  std::string save_vertex(const vertex_type& vertex) const {
    return ""; //nop
  }
  std::string save_edge(const edge_type& edge) const {
    if(edge.data().role == edge_data::PREDICT) {
      std::stringstream strm;
      double prediction =
        edge.source().data().pvec.dot(edge.target().data().pvec);
      prediction = std::min(prediction, maxval);
      prediction = std::max(prediction, minval);
      strm << edge.source().id() << '\t';
      strm << (-edge.target().id() - SAFE_NEG_OFFSET) << '\t';
      strm << prediction << '\n';
      return strm.str();
    } else return "";
  }
}; // end of prediction_saver

struct linear_model_saver_U {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
  */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() > 0){
      std::string ret = boost::lexical_cast<std::string>(vertex.id()) + " ";
      for (uint i=0; i< vertex_data::NLATENT; i++)
        ret += boost::lexical_cast<std::string>(vertex.data().pvec[i]) + " ";
        ret += "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};

struct linear_model_saver_V {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
  */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() == 0){
      std::string ret = boost::lexical_cast<std::string>(-vertex.id()-SAFE_NEG_OFFSET) + ") ";
      for (uint i=0; i< vertex_data::NLATENT; i++)
        ret += boost::lexical_cast<std::string>(vertex.data().pvec[i]) + " ";
        ret += "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};

int main(int argc, char** argv) {
  global_logger().set_log_level(LOG_INFO);
  global_logger().set_log_to_console(true);

  // Parse command line options -----------------------------------------------
  const std::string description =
      "Compute the ALS factorization of a matrix.";
  graphlab::command_line_options clopts(description);
  std::string input_dir;
  std::string exec_type = "synchronous";
  clopts.attach_option("matrix", input_dir,
                       "The directory containing the matrix file");
  clopts.add_positional("matrix");
  clopts.attach_option("D", vertex_data::NLATENT,
                       "Number of latent parameters to use.");
  clopts.attach_option("maxval", maxval, "max allowed value");
  clopts.attach_option("minval", minval, "min allowed value");
  clopts.attach_option("predictions", predictions,
                       "The prefix (folder and filename) to save predictions.");
  clopts.attach_option("lambda", LAMBDA,
                       "regularization weight");
  clopts.attach_option("max_iter", max_iter,
                       "number of iterations");
  if(!clopts.parse(argc, argv) || input_dir == "") {
    std::cout << "Error in parsing command line arguments." << std::endl;
    clopts.print_description();
    return EXIT_FAILURE;
  }
  ///! Initialize control plain using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;

  dc.cout() << "Loading graph." << std::endl;
  graphlab::timer timer;
  graph_type graph(dc, clopts);
  graph.load(input_dir, graph_loader);
  dc.cout() << "Loading graph. Finished in "
            << timer.current_time() << std::endl;

  dc.cout() << "Finalizing graph." << std::endl;
  timer.start();
  graph.finalize();
  dc.cout() << "Finalizing graph. Finished in "
            << timer.current_time() << std::endl;

  if (!graph.num_edges() || !graph.num_vertices())
     logstream(LOG_FATAL)<< "Failed to load graph. Check your input path: " << input_dir << std::endl;

  dc.cout()
      << "========== Graph statistics on proc " << dc.procid()
      << " ==============="
      << "\n Num vertices: " << graph.num_vertices()
      << "\n Num edges: " << graph.num_edges()
      << "\n Num replica: " << graph.num_replicas()
      << "\n Replica to vertex ratio: "
      << float(graph.num_replicas())/graph.num_vertices()
      << "\n --------------------------------------------"
      << "\n Num local own vertices: " << graph.num_local_own_vertices()
      << "\n Num local vertices: " << graph.num_local_vertices()
      << "\n Replica to own ratio: "
      << (float)graph.num_local_vertices()/graph.num_local_own_vertices()
      << "\n Num local edges: " << graph.num_local_edges()
      //<< "\n Begin edge id: " << graph.global_eid(0)
        << "\n Edge balance ratio: "
        << float(graph.num_local_edges())/graph.num_edges()
        << std::endl;

  dc.cout() << "Creating engine" << std::endl;

  engine_type engine(dc, graph, clopts);
  engine.register_map_reduce(ALS_COORD_MAP_REDUCE, als_coord_map, als_coord_combine);
  engine.register_edge_transform(ALS_COORD_TRANSFORM, als_coord_transform);
  for (int i=0; i< max_iter; i++){
     engine.parfor_all_local_vertices(als_coord_function);
     engine.wait();
     double rmse = graph.map_reduce_edges<double>(extract_l2_error);
     dc.cout() << "RMSE = " << sqrt(rmse / graph.num_edges()) << std::endl;
  }

  const double runtime = timer.current_time();
  dc.cout() << "----------------------------------------------------------"
            << std::endl
            << "Final Runtime (seconds):   " << runtime;

  // Compute the final training error -----------------------------------------
  dc.cout() << "Final error: " << std::endl;
  // Make predictions ---------------------------------------------------------
  if(!predictions.empty()) {
    std::cout << "Saving predictions" << std::endl;
    const bool gzip_output = false;
    const bool save_vertices = false;
    const bool save_edges = true;
    const size_t threads_per_machine = 2;

    //save the predictions
    graph.save(predictions, prediction_saver(),
               gzip_output, save_vertices,
               save_edges, threads_per_machine);
    //save the linear model
    graph.save(predictions + ".U", linear_model_saver_U(),
		gzip_output, save_edges, save_vertices, threads_per_machine);
    graph.save(predictions + ".V", linear_model_saver_V(),
		gzip_output, save_edges, save_vertices, threads_per_machine);

  }

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // end of main


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

#include <boost/hana/ext/boost/mpl/integral_c.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/eval_if.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/not.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/logical.hpp>

#include <boost/mpl/bool.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
namespace hana = boost::hana;
namespace mpl = boost::mpl;

int main() {
    // eval_if
    {
        auto t = hana::test::ct_eq<3>{};
        auto e = hana::test::ct_eq<4>{};

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::eval_if(mpl::true_{}, hana::always(t), hana::always(e)),
            t
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::eval_if(mpl::false_{}, hana::always(t), hana::always(e)),
            e
        ));
    }

    // not_
    {
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::not_(mpl::true_{}),
            mpl::false_{}
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::not_(mpl::false_{}),
            mpl::true_{}
        ));
    }

    // laws
    hana::test::TestLogical<hana::ext::boost::mpl::integral_c_tag<int>>{
        hana::make_tuple(
            mpl::int_<-2>{}, mpl::integral_c<int, 0>{}, mpl::integral_c<int, 3>{}
        )
    };

    hana::test::TestLogical<hana::ext::boost::mpl::integral_c_tag<bool>>{
        hana::make_tuple(
            mpl::true_{}, mpl::false_{},
            mpl::integral_c<bool, true>{}, mpl::integral_c<bool, false>{}
        )
    };
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, logical_gte) {
  using stan::math::logical_gte;
  EXPECT_TRUE(logical_gte(1, 0));
  EXPECT_TRUE(logical_gte(2.0, 1.0));
  EXPECT_TRUE(logical_gte(2.0, 1));
  EXPECT_TRUE(logical_gte(0, -1));
  EXPECT_TRUE(logical_gte(1, 1));
  EXPECT_TRUE(logical_gte(5.7, 5.7));

  EXPECT_FALSE(logical_gte(-9.0, 5.7));
  EXPECT_FALSE(logical_gte(-2, -1));
}

TEST(MathFunctions, logical_gte_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_FALSE(stan::math::logical_gte(1.0, nan));
  EXPECT_FALSE(stan::math::logical_gte(nan, 2.0));
  EXPECT_FALSE(stan::math::logical_gte(nan, nan));
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Col;
using arma::span;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenColVecElemIndRangeGenDouble: public Expected {
    public:
      ExpectedInPlaceGenColVecElemIndRangeGenDouble() {
        cout << "Compute ExpectedInPlaceGenColVecElemIndRangeGenDouble(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters( {
            InputClass::GenColVec,
            InputClass::ElemIndRange,
            InputClass::GenDouble
          });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _genColVec = *static_cast<Col<double>*>(value.second);
                break;
              case 1:
                _fileSuffix += "," + value.first;
                _elemIndRange = *static_cast<span*>(value.second);
                break;
              case 2:
                _fileSuffix += "," + value.first;
                _genDouble = *static_cast<double*>(value.second);
                break;
            }
            ++n;
          }

          cout << "Using input: " << _fileSuffix << endl;

          _copyOfGenColVec = _genColVec;
          _copyOfElemIndRange = _elemIndRange;
          _copyOfGenDouble = _genDouble;

          expectedColVecRowsPlus();

          _genColVec = _copyOfGenColVec;
          _elemIndRange = _copyOfElemIndRange;
          _genDouble = _copyOfGenDouble;
          expectedColVecRowsMinus();

          _genColVec = _copyOfGenColVec;
          _elemIndRange = _copyOfElemIndRange;
          _genDouble = _copyOfGenDouble;
          expectedColVecRowsTimes();

          _genColVec = _copyOfGenColVec;
          _elemIndRange = _copyOfElemIndRange;
          _genDouble = _copyOfGenDouble;
          expectedColVecRowsDivide();

          _genColVec = _copyOfGenColVec;
          _elemIndRange = _copyOfElemIndRange;
          _genDouble = _copyOfGenDouble;
          expectedColVecSubvecPlus();

          _genColVec = _copyOfGenColVec;
          _elemIndRange = _copyOfElemIndRange;
          _genDouble = _copyOfGenDouble;
          expectedColVecSubvecMinus();

          _genColVec = _copyOfGenColVec;
          _elemIndRange = _copyOfElemIndRange;
          _genDouble = _copyOfGenDouble;
          expectedColVecSubvecTimes();

          _genColVec = _copyOfGenColVec;
          _elemIndRange = _copyOfElemIndRange;
          _genDouble = _copyOfGenDouble;
          expectedColVecSubvecDivide();
        }

        cout << "done." << endl;
      }

    protected:
      Col<double> _genColVec;
      Col<double> _copyOfGenColVec;

      span _elemIndRange;
      span _copyOfElemIndRange;

      double _genDouble;
      double _copyOfGenDouble;

      void expectedColVecRowsPlus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVec.in_range(_elemIndRange)) {
          return;
        }
        cout << "- Compute expectedColVecRowsPlus() ... ";

        _genColVec.rows(_elemIndRange.a, _elemIndRange.b) += _genDouble;

        save<double>("Col.rowsPlus", _genColVec);
        cout << "done." << endl;
      }

      void expectedColVecRowsMinus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVec.in_range(_elemIndRange)) {
          return;
        }
        cout << "- Compute expectedColVecRowsMinus() ... ";

        _genColVec.rows(_elemIndRange.a, _elemIndRange.b) -= _genDouble;

        save<double>("Col.rowsMinus", _genColVec);
        cout << "done." << endl;
      }

      void expectedColVecRowsTimes() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVec.in_range(_elemIndRange)) {
          return;
        }

        cout << "- Compute expectedColVecRowsTimes() ... ";

        _genColVec.rows(_elemIndRange.a, _elemIndRange.b) *= _genDouble;

        save<double>("Col.rowsTimes", _genColVec);
        cout << "done." << endl;
      }

      void expectedColVecRowsDivide() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVec.in_range(_elemIndRange)) {
          return;
        }

        cout << "- Compute expectedColVecRowsDivide() ... ";

        _genColVec.rows(_elemIndRange.a, _elemIndRange.b) /= _genDouble;

        save<double>("Col.rowsDivide", _genColVec);
        cout << "done." << endl;
      }

      void expectedColVecSubvecPlus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVec.in_range(_elemIndRange)) {
          return;
        }

        cout << "- Compute expectedColVecSubvecPlus() ... ";

        _genColVec.subvec(_elemIndRange.a, _elemIndRange.b) += _genDouble;

        save<double>("Col.subvecPlus", _genColVec);
        cout << "done." << endl;
      }

      void expectedColVecSubvecMinus() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVec.in_range(_elemIndRange)) {
          return;
        }

        cout << "- Compute expectedColVecSubvecMinus() ... ";

        _genColVec.subvec(_elemIndRange.a, _elemIndRange.b) -= _genDouble;

        save<double>("Col.subvecMinus", _genColVec);
        cout << "done." << endl;
      }

      void expectedColVecSubvecTimes() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVec.in_range(_elemIndRange)) {
          return;
        }

        cout << "- Compute expectedColVecSubvecTimes() ... ";

        _genColVec.subvec(_elemIndRange.a, _elemIndRange.b) *= _genDouble;

        save<double>("Col.subvecTimes", _genColVec);
        cout << "done." << endl;
      }

      void expectedColVecSubvecDivide() {
        if(_elemIndRange.whole) {
          return;
        }

        if (!_genColVec.in_range(_elemIndRange)) {
          return;
        }

        cout << "- Compute expectedColVecSubvecDivide() ... ";

        _genColVec.subvec(_elemIndRange.a, _elemIndRange.b) /= _genDouble;

        save<double>("Col.subvecDivide", _genColVec);
        cout << "done." << endl;
      }

  };
}


// Example program for returning a value from slots to signal invocation.
//
// Copyright Douglas Gregor 2001-2004.
// Copyright Frank Mori Hess 2009.
//
// 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)
// For more information, see http://www.boost.org

#include <iostream>
#include <boost/signals2/signal.hpp>

//[ signal_return_value_slot_defs_code_snippet
float product(float x, float y) { return x * y; }
float quotient(float x, float y) { return x / y; }
float sum(float x, float y) { return x + y; }
float difference(float x, float y) { return x - y; }
//]

int main()
{
  boost::signals2::signal<float (float x, float y)> sig;

//[ signal_return_value_main_code_snippet
  sig.connect(&product);
  sig.connect(&quotient);
  sig.connect(&sum);
  sig.connect(&difference);

  // The default combiner returns a boost::optional containing the return
  // value of the last slot in the slot list, in this case the
  // difference function.
  std::cout << *sig(5, 3) << std::endl;
//]
}


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/make.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P>
bg::model::box<P> create_box()
{
    P p1;
    P p2;
    bg::assign_values(p1, 1, 2, 5);
    bg::assign_values(p2, 3, 4, 6);
    return bg::model::box<P>(p1, p2);
}

template <typename B, typename T>
void check_box(B& to_check,
               T min_x, T min_y, T min_z,
               T max_x, T max_y, T max_z)
{
    BOOST_CHECK_EQUAL(bg::get<0>(to_check.min_corner()), min_x);
    BOOST_CHECK_EQUAL(bg::get<1>(to_check.min_corner()), min_y);
    BOOST_CHECK_EQUAL(bg::get<2>(to_check.min_corner()), min_z);
    BOOST_CHECK_EQUAL(bg::get<0>(to_check.max_corner()), max_x);
    BOOST_CHECK_EQUAL(bg::get<1>(to_check.max_corner()), max_y);
    BOOST_CHECK_EQUAL(bg::get<2>(to_check.max_corner()), max_z);
}

template <typename P>
void test_construction()
{
    typedef typename bg::coordinate_type<P>::type T;

    bg::model::box<P> b1 = bg::make_zero<bg::model::box<P> >();
    check_box(b1, T(),T(),T(),T(),T(),T());

    bg::model::box<P> b2(create_box<P>());
    check_box(b2, 1,2,5,3,4,6);

    bg::model::box<P> b3 = bg::make_inverse<bg::model::box<P> >();
    check_box(b3, boost::numeric::bounds<T>::highest(),
                  boost::numeric::bounds<T>::highest(),
                  boost::numeric::bounds<T>::highest(),
                  boost::numeric::bounds<T>::lowest(),
                  boost::numeric::bounds<T>::lowest(),
                  boost::numeric::bounds<T>::lowest());
}

template <typename P>
void test_assignment()
{
    bg::model::box<P> b(create_box<P>());
    bg::set<0>(b.min_corner(), 10);
    bg::set<1>(b.min_corner(), 20);
    bg::set<2>(b.min_corner(), 30);
    bg::set<0>(b.max_corner(), 40);
    bg::set<1>(b.max_corner(), 50);
    bg::set<2>(b.max_corner(), 60);
    check_box(b, 10,20,30,40,50,60);
}

template <typename P>
void test_all()
{
    test_construction<P>();
    test_assignment<P>();
}

int test_main(int, char* [])
{
    test_all<int[3]>();
    test_all<float[3]>();
    test_all<double[3]>();
    test_all<test::test_point>();
    test_all<bg::model::point<int, 3, bg::cs::cartesian> >();
    test_all<bg::model::point<float, 3, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 3, bg::cs::cartesian> >();

    return 0;
}


/**
 * \file fea/tetrahedral_mesh.cpp
 * This file is part of SANM, a symbolic asymptotic numerical solver.
 */

#include "fea/tetrahedral_mesh.h"
#include "fea/mesh_template.h"

#include <Eigen/Dense>

#include <cerrno>
#include <cstdio>
#include <cstring>
#include <fstream>

using namespace fea;

INST_MESH_IO_TRANS(3, TetrahedralMesh);

namespace {
void copy_mat3_cols(fp_t* dst, const Vec3& c0, const Vec3& c1, const Vec3& c2) {
    const Vec3* cs[] = {&c0, &c1, &c2};
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            dst[i * 3 + j] = (*cs[j])[i];
        }
    }
}
}  // anonymous namespace

const CoordMat3D& TetrahedralMesh::vertex_norms() const {
    if (m_vertex_norms.valid()) {
        return m_vertex_norms.val();
    }
    size_t nr_tet = this->nr_tet();
    m_vertex_norms.init();
    CoordMat3D& vertex_norms = m_vertex_norms.val();
    vertex_norms.resize(3, nr_tet * 4);
    m_tet_volumes.resize(nr_tet);
    auto ds_ptr = m_shape_matrix.set_shape({nr_tet, 3, 3}).woptr();

    for (size_t i = 0; i < nr_tet; ++i) {
        Vec3 x0 = m_vertices.col(m_tet(0, i)), x1 = m_vertices.col(m_tet(1, i)),
             x2 = m_vertices.col(m_tet(2, i)), x3 = m_vertices.col(m_tet(3, i)),
             v1 = x1 - x0, v2 = x2 - x0, v3 = x3 - x0;
        copy_mat3_cols(ds_ptr + i * 9, v1, v2, v3);

        fp_t det = v1.dot(v2.cross(v3));

        m_tet_volumes[i] = std::fabs(det) / 6;

        // - volume * D^{-T}, equal to -cofactor*sign(det)
        // normal for a vertex is also the area-weighted outward normal of the
        // opposite face

        Vec3 t1 = v2.cross(v3), t2 = v3.cross(v1), t3 = v1.cross(v2);
        if (det > 0) {
            t1 = -t1;
            t2 = -t2;
            t3 = -t3;
        }
        vertex_norms.col(4 * i + 0) = -(t1 + t2 + t3);
        vertex_norms.col(4 * i + 1) = t1;
        vertex_norms.col(4 * i + 2) = t2;
        vertex_norms.col(4 * i + 3) = t3;
    }
    vertex_norms *= 1.0 / 6;
    return vertex_norms;
}

const std::vector<fp_t>& TetrahedralMesh::tet_volumes() const {
    if (m_tet_volumes.empty()) {
        vertex_norms();
    }
    return m_tet_volumes;
}

const sanm::TensorND& TetrahedralMesh::shape_matrix() const {
    if (m_shape_matrix.empty()) {
        vertex_norms();
    }
    return m_shape_matrix;
}

const MeshVertexReverseList& TetrahedralMesh::vertex_reverse_list() const {
    if (!m_vertex_reverse_list.valid()) {
        m_vertex_reverse_list.init(
                MeshVertexReverseList::from_mesh<4>(nr_vertices(), m_tet));
    }
    return m_vertex_reverse_list.val();
}

TetrahedralMeshPtr TetrahedralMesh::make_cuboid(size_t nr_vtx_x,
                                                size_t nr_vtx_y,
                                                size_t nr_vtx_z, fp_t size) {
    sanm_assert(nr_vtx_x >= 2 && nr_vtx_y >= 2 && nr_vtx_z >= 2 && size > 0);
    // code adopted from the CompFabAssignment
    const int vertex_num = nr_vtx_x * nr_vtx_y * nr_vtx_z;
    const int element_num =
            5 * (nr_vtx_x - 1) * (nr_vtx_y - 1) * (nr_vtx_z - 1);
    auto ret = std::make_shared<TetrahedralMesh>();
    CoordMat3D& vertex = ret->m_vertices;
    TetIndexMat& element = ret->m_tet;
    vertex.resize(3, vertex_num);
    element.resize(4, element_num);
    int id = 0;
    for (size_t i = 0; i < nr_vtx_x; i++) {
        for (size_t j = 0; j < nr_vtx_y; j++) {
            for (size_t k = 0; k < nr_vtx_z; k++) {
                vertex(0, id) = i * size;
                vertex(1, id) = j * size;
                vertex(2, id) = k * size;
                if (i == 0 || i == nr_vtx_x - 1 || j == 0 ||
                    j == nr_vtx_y - 1 || k == 0 || k == nr_vtx_z - 1) {
                    ret->m_surface_vtx.insert(id);
                }
                id++;
            }
        }
    }
    auto get_cuboid_id = [nr_vtx_y, nr_vtx_z](size_t x, size_t y,
                                              size_t z) -> int {
        return (x * nr_vtx_y + y) * nr_vtx_z + z;
    };
    id = 0;
    for (size_t i = 0; i < nr_vtx_x - 1; i++) {
        for (size_t j = 0; j < nr_vtx_y - 1; j++) {
            for (size_t k = 0; k < nr_vtx_z - 1; k++) {
                int hex_id[] = {get_cuboid_id(i, j, k),
                                get_cuboid_id(i + 1, j, k),
                                get_cuboid_id(i + 1, j + 1, k),
                                get_cuboid_id(i, j + 1, k),
                                get_cuboid_id(i, j, k + 1),
                                get_cuboid_id(i + 1, j, k + 1),
                                get_cuboid_id(i + 1, j + 1, k + 1),
                                get_cuboid_id(i, j + 1, k + 1)};
                auto add_face = [&](int a, int b, int c) {
                    auto& t = ret->m_surfaces.emplace_back();
                    t[0] = hex_id[a];
                    t[1] = hex_id[b];
                    t[2] = hex_id[c];
                };

                if (i == 0) {
                    add_face(3, 0, 7);
                    add_face(7, 0, 4);
                }
                if (i == nr_vtx_x - 2) {
                    add_face(1, 2, 6);
                    add_face(6, 5, 1);
                }
                if (j == 0) {
                    add_face(0, 1, 5);
                    add_face(0, 5, 4);
                }
                if (j == nr_vtx_y - 2) {
                    add_face(7, 6, 3);
                    add_face(6, 2, 3);
                }
                if (k == 0) {
                    add_face(1, 3, 2);
                    add_face(0, 3, 1);
                }
                if (k == nr_vtx_z - 2) {
                    add_face(4, 5, 7);
                    add_face(7, 5, 6);
                }

                // 0, 2, 1, 5
                element(0, id) = hex_id[0];
                element(1, id) = hex_id[2];
                element(2, id) = hex_id[1];
                element(3, id) = hex_id[5];
                id++;
                // 0, 4, 7, 5
                element(0, id) = hex_id[0];
                element(1, id) = hex_id[4];
                element(2, id) = hex_id[7];
                element(3, id) = hex_id[5];
                id++;
                // 0, 2, 5, 7
                element(0, id) = hex_id[0];
                element(1, id) = hex_id[2];
                element(2, id) = hex_id[5];
                element(3, id) = hex_id[7];
                id++;
                // 2, 6, 5, 7
                element(0, id) = hex_id[2];
                element(1, id) = hex_id[6];
                element(2, id) = hex_id[5];
                element(3, id) = hex_id[7];
                id++;
                // 0, 7, 3, 2
                element(0, id) = hex_id[0];
                element(1, id) = hex_id[7];
                element(2, id) = hex_id[3];
                element(3, id) = hex_id[2];
                id++;
            }
        }
    }
    sanm_assert(id == element_num);
    return ret;
}

TetrahedralMeshPtr TetrahedralMesh::from_tetgen_files(
        const std::string& filebase) {
    std::ifstream fin_ele{filebase + ".ele"}, fin_node{filebase + ".node"},
            fin_face{filebase + ".face"};
    sanm_assert(fin_ele.good() && fin_node.good() && fin_face.good(),
                "failed to open input files: %s.{ele,node,face}",
                filebase.c_str());
    auto ret = std::make_shared<TetrahedralMesh>();

    // see https://wias-berlin.de/software/tetgen/fformats.node.html
    size_t nr_vtx, dim, nr_attr, bound_mark;
    fin_node >> nr_vtx >> dim >> nr_attr >> bound_mark;
    sanm_assert(dim == 3 && !nr_attr && !bound_mark);

    CoordMat3D& vtx = ret->m_vertices;
    vtx.resize(3, nr_vtx);
    for (size_t i = 0; i < nr_vtx; ++i) {
        size_t idx;
        fin_node >> idx >> vtx(0, i) >> vtx(1, i) >> vtx(2, i);
        sanm_assert(idx == i, "failed to read vertex %zu: got idx %zu", i, idx);
    }
    sanm_assert(fin_node.good());

    // see https://wias-berlin.de/software/tetgen/fformats.ele.html
    size_t nr_tet, node_per_tet;
    fin_ele >> nr_tet >> node_per_tet >> nr_attr;
    sanm_assert(node_per_tet == 4 && !nr_attr);

    TetIndexMat& tet = ret->m_tet;
    tet.resize(4, nr_tet);
    for (size_t i = 0; i < nr_tet; ++i) {
        size_t idx;
        fin_ele >> idx >> tet(0, i) >> tet(1, i) >> tet(2, i) >> tet(3, i);
        sanm_assert(idx == i, "failed to read tetrahedron %zu: got idx %zu", i,
                    idx);
    }

    size_t nr_face, boundary_marker;
    fin_face >> nr_face >> boundary_marker;
    for (size_t i = 0; i < nr_face; ++i) {
        size_t idx;
        int a, b, c;
        fin_face >> idx >> a >> b >> c;
        sanm_assert(idx == i);
        ret->m_surface_vtx.insert(a);
        ret->m_surface_vtx.insert(b);
        ret->m_surface_vtx.insert(c);
        if (boundary_marker) {
            fin_face >> b;
        }
        // do not read into m_surfaces since tetgen may invert the surface
    }

    return ret;
}

void TetrahedralMesh::write_to_file(FILE* fout,
                                    const VertexSet* filter_set) const {
    if (!filter_set) {
        if (!m_surfaces.empty()) {
            write_to_file(fout, m_vertices, m_surfaces);
            return;
        }

        if (!m_surface_vtx.empty()) {
            filter_set = &m_surface_vtx;
        }
    }
    write_to_file(fout, m_vertices, m_tet, filter_set);
}

void TetrahedralMesh::write_to_surface_vtx_file(FILE* fout) const {
    sanm_assert(!m_surface_vtx.empty());
    int vmin = std::numeric_limits<int>::max(), vmax = 0;
    for (int i : m_surface_vtx) {
        vmin = std::min(vmin, i);
        vmax = std::max(vmax, i);
    }
    sanm_assert(vmin == 0, "min surface vtx num is not zero: %d", vmin);
    sanm_assert(vmax == static_cast<int>(m_surface_vtx.size()) - 1,
                "max surface vtx num is %d, size is %zu", vmax,
                m_surface_vtx.size());

    for (int i = vmin; i <= vmax; ++i) {
        const Vec3& v = m_vertices.col(i);
        fprintf(fout, "%g %g %g\n", v[0], v[1], v[2]);
    }
}

void TetrahedralMesh::write_to_file(FILE* fout, const CoordMat3D& V,
                                    const TetIndexMat& F,
                                    const VertexSet* filter_set) {
    sanm_assert(fout);
    std::unordered_map<int, int> vtx_id_map;
    if (filter_set) {
        sanm_assert(!filter_set->empty());
    }
    auto write_facet = [fout, filter_set, &vtx_id_map](int v0, int v1, int v2) {
        if (filter_set) {
            if (!filter_set->count(v0) || !filter_set->count(v1) ||
                !filter_set->count(v2)) {
                return;
            }
            v0 = vtx_id_map.at(v0);
            v1 = vtx_id_map.at(v1);
            v2 = vtx_id_map.at(v2);
        }
        fprintf(fout, "f %d %d %d\n", v0 + 1, v1 + 1, v2 + 1);
    };
    for (int i = 0; i < V.cols(); ++i) {
        if (!filter_set || filter_set->count(i)) {
            if (filter_set) {
                int id = vtx_id_map.size();
                vtx_id_map[i] = id;
            }
            Vec3 vi = V.col(i);
            fprintf(fout, "v %g %g %g\n", vi.x(), vi.y(), vi.z());
        }
    }

    for (Eigen::Index i = 0; i < F.cols(); ++i) {
        int i0 = F(0, i), i1 = F(1, i), i2 = F(2, i), i3 = F(3, i);
        Vec3 v0 = V.col(i0), v1 = V.col(i1), v2 = V.col(i2), v3 = V.col(i3);

        if ((v1 - v0).dot((v2 - v0).cross(v3 - v0)) > 0) {
            std::swap(i1, i2);
        }

        write_facet(i0, i1, i2);
        write_facet(i1, i3, i2);
        write_facet(i1, i0, i3);
        write_facet(i0, i2, i3);
    }
}

void TetrahedralMesh::write_to_file(FILE* fout, const CoordMat3D& V,
                                    const FaceList& F) {
    sanm_assert(fout && !F.empty());
    std::unordered_map<int, int> vtx_id_map;
    std::vector<int> vtx_ids;
    vtx_ids.reserve(F.size() * 2);
    vtx_id_map.reserve(F.size() * 2);

    for (auto&& f : F) {
        for (int v : f) {
            int id = vtx_id_map.size();
            if (vtx_id_map.insert({v, id}).second) {
                vtx_ids.push_back(v);
            }
        }
    }

    for (int i : vtx_ids) {
        Vec3 vi = V.col(i);
        fprintf(fout, "v %g %g %g\n", vi.x(), vi.y(), vi.z());
    }

    for (auto&& f : F) {
        int v0 = vtx_id_map.at(f[0]), v1 = vtx_id_map.at(f[1]),
            v2 = vtx_id_map.at(f[2]);
        fprintf(fout, "f %d %d %d\n", v0 + 1, v1 + 1, v2 + 1);
    }
}

void TetrahedralMesh::replace_with_mask(const CoordMask3D& mask,
                                        const sanm::TensorND& value) {
    fea::replace_with_mask(m_vertices, mask, value.ptr(),
                           value.shape().total_nr_elems());
    clear_cache();
}

void TetrahedralMesh::apply_vtx_delta(const CoordMat3D& delta) {
    sanm_assert(delta.cols() == m_vertices.cols());
    m_vertices += delta;
    clear_cache();
}

void TetrahedralMesh::replace_vtx(const CoordMat3D& vtx) {
    sanm_assert(vtx.cols() == m_vertices.cols());
    m_vertices = vtx;
    clear_cache();
}

void TetrahedralMesh::clear_cache() {
    m_vertex_norms.reset();
    m_tet_volumes.clear();
    m_shape_matrix.clear();
}

void TetrahedralMesh::resize_inplace(fp_t scale) {
    m_vertices *= scale;
    clear_cache();
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This mex file will create a MATLAB function called example_mex_class.  If you call it
    with no arguments it will output the MATLAB .m code to create a MATLAB wrapper class.
    Paste that code into a .m file.  Then you will be able to work with this C++ class
    directly in MATLAB.
*/

#include <iostream>
#include <dlib/matrix.h>

using namespace std;
using namespace dlib;

class example_class
{
public:

    // The class must have a default constructor.  It's also the only kind of constructor
    // you can call from MATLAB.
    example_class()
    {
        xx.set_size(3,2);
        xx = 1;
    }

    // The rest of the member functions that you want to bind have to return void and
    // generally have the same syntax limitations as regular mex funcitons.
    void do_stuff(const matrix_colmajor& x)
    {
        cout << "in do_stuff" << endl;
        cout << x << endl;
        xx = x;
    }

    void do_other_stuff(int x)
    {
        cout << "in do_other_stuff" << endl;
        cout << "x: " << x << endl;
    }

    void print_state()
    {
        cout << xx << endl;
    }

    // saveobj() and load_obj() are special functions. If you provide these then you will
    // be able to save() and load() your objects using MATLAB's built in object
    // serialization.
    void saveobj(matrix_colmajor& state)
    {
        // save this object's state to state.
        state = xx;
    }
    void load_obj(const matrix_colmajor& state)
    {
        xx = state;
    }

private:
    matrix_colmajor xx;
};

// Just tell the mex wrapper the name of your class and list the methods you want to bind.
#define MEX_CLASS_NAME example_class
#define MEX_CLASS_METHODS do_stuff, do_other_stuff, print_state, saveobj, load_obj

#include "mex_wrapper.cpp"


#define CGAL_CHECK_EXPENSIVE

#include <CGAL/Simple_cartesian.h>

#include <CGAL/Surface_mesh.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/boost/graph/Seam_mesh.h>

#include <CGAL/Surface_mesh_parameterization/Error_code.h>
#include <CGAL/surface_mesh_parameterization.h>

#include <CGAL/Polygon_mesh_processing/measure.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>

#include <boost/functional/hash.hpp>

#include <iostream>
#include <fstream>

namespace SMP = CGAL::Surface_mesh_parameterization;
namespace PMP = CGAL::Polygon_mesh_processing;

typedef CGAL::Simple_cartesian<double>            Kernel;
typedef Kernel::Point_2                           Point_2;
typedef Kernel::Point_3                           Point_3;

#define MVC_POLYHEDRON_MESH
#define ARAP_POLYHEDRON_MESH
#define BARY_SURF_MESH
#define ARAP_SURF_MESH
#define DCM_PM_SEAM_MESH
#define DAC_SM_SEAM_MESH
#define ORBIFOLD_SM_MESH
#define ITERATIVE_SURF_MESH

// POLYHEDRON_MESH
typedef CGAL::Polyhedron_3<Kernel>                                PMesh;

typedef boost::graph_traits<PMesh>::vertex_descriptor             PM_vertex_descriptor;
typedef boost::graph_traits<PMesh>::halfedge_descriptor           PM_halfedge_descriptor;

typedef CGAL::Unique_hash_map<PM_halfedge_descriptor, Point_2>    PM_UV_hmap;
typedef boost::associative_property_map<PM_UV_hmap>               PM_UV_pmap;

// SURF_MESH
typedef CGAL::Surface_mesh<Point_3>                               SMesh;

typedef boost::graph_traits<SMesh>::vertex_descriptor             SM_vertex_descriptor;
typedef boost::graph_traits<SMesh>::halfedge_descriptor           SM_halfedge_descriptor;

typedef SMesh::Property_map<SM_halfedge_descriptor, Point_2>      SM_UV_pmap;

// PM_SEAM_MESH
typedef boost::graph_traits<PMesh>::edge_descriptor               PM_edge_descriptor;

typedef CGAL::Unique_hash_map<PM_edge_descriptor, bool>           PM_seam_edge_hmap;
typedef boost::associative_property_map<PM_seam_edge_hmap>        PM_seam_edge_pmap;
typedef CGAL::Unique_hash_map<PM_vertex_descriptor, bool>         PM_seam_vertex_hmap;
typedef boost::associative_property_map<PM_seam_vertex_hmap>      PM_seam_vertex_pmap;

typedef CGAL::Seam_mesh<PMesh, PM_seam_edge_pmap, PM_seam_vertex_pmap>
                                                                  PM_Seam_mesh;

typedef boost::graph_traits<PM_Seam_mesh>::vertex_descriptor      PM_SE_vertex_descriptor;
typedef boost::graph_traits<PM_Seam_mesh>::halfedge_descriptor    PM_SE_halfedge_descriptor;

// SM_SEAM_MESH
typedef boost::graph_traits<SMesh>::edge_descriptor               SM_edge_descriptor;

typedef SMesh::Property_map<SM_edge_descriptor, bool>             SM_seam_edge_pmap;
typedef SMesh::Property_map<SM_vertex_descriptor, bool>           SM_seam_vertex_pmap;

typedef CGAL::Seam_mesh<SMesh, SM_seam_edge_pmap, SM_seam_vertex_pmap>
                                                                  SM_Seam_mesh;

typedef boost::graph_traits<SM_Seam_mesh>::vertex_descriptor      SM_SE_vertex_descriptor;
typedef boost::graph_traits<SM_Seam_mesh>::halfedge_descriptor    SM_SE_halfedge_descriptor;

int main(int, char**)
{
  std::cout.precision(17);
  CGAL::IO::set_pretty_mode(std::cout);

  // ***************************************************************************
  // Default case
  // ***************************************************************************

#ifdef MVC_POLYHEDRON_MESH
  {
    std::cout << " ----------- MVC POLYHEDRON -----------" << std::endl;

    std::ifstream in(CGAL::data_file_path("meshes/mushroom.off"));
    PMesh pm;
    in >> pm;
    if(!in || num_vertices(pm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    PM_halfedge_descriptor hd = PMP::longest_border(pm).first;

    CGAL::Unique_hash_map<PM_vertex_descriptor, Point_2,
                          boost::hash<PM_vertex_descriptor> > uvhm;
    boost::associative_property_map<
      CGAL::Unique_hash_map<PM_vertex_descriptor, Point_2,
                            boost::hash<PM_vertex_descriptor> > > uvpm(uvhm);

    // Go to default (MVC)
    SMP::Error_code status = SMP::parameterize(pm, hd, uvpm);

    if(status != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with MVC (POLY)!" << std::endl;
    }
  }
#endif // MVC_POLYHEDRON_MESH

  // ***************************************************************************
  // ARAP WITH POLYHEDRON_MESH
  // ***************************************************************************

#ifdef ARAP_POLYHEDRON_MESH
  {
    std::cout << " ----------- ARAP POLYHEDRON -----------" << std::endl;

    std::ifstream in(CGAL::data_file_path("meshes/three_peaks.off"));
    PMesh pm;
    in >> pm;
    if(!in || num_vertices(pm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    PM_halfedge_descriptor hd = PMP::longest_border(pm).first;

    // UV map
    CGAL::Unique_hash_map<PM_vertex_descriptor, Point_2,
                          boost::hash<PM_vertex_descriptor> > uvhm;
    boost::associative_property_map<
      CGAL::Unique_hash_map<PM_vertex_descriptor, Point_2,
                            boost::hash<PM_vertex_descriptor> > > uvpm(uvhm);

    // Indices map
    typedef CGAL::dynamic_vertex_property_t<int>                                 Vertex_int_tag;
    typedef typename boost::property_map<PMesh, Vertex_int_tag>::type            Vertex_int_map;
    Vertex_int_map vipm = get(Vertex_int_tag(), pm);
    CGAL::Surface_mesh_parameterization::internal::fill_index_map_of_cc(hd, pm, vipm);

    // Vertex parameterized map
    typedef CGAL::dynamic_vertex_property_t<bool>                                Vertex_bool_tag;
    typedef typename boost::property_map<PMesh, Vertex_bool_tag>::type           Vertex_bool_map;
    Vertex_bool_map vpm = get(Vertex_bool_tag(), pm);

    // Parameterizer
    SMP::ARAP_parameterizer_3<PMesh> parameterizer;
    SMP::Error_code status = parameterizer.parameterize(pm, hd, uvpm, vipm, vpm);
    SMP::Error_code status_bis = SMP::parameterize(pm, parameterizer, hd, uvpm);
    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with ARAP (POLY)!" << std::endl;
    }
  }
#endif // ARAP_POLYHEDRON_MESH

  // ***************************************************************************
  // Barycentric mapping
  // ***************************************************************************

#ifdef BARY_SURF_MESH
  {
    std::cout << " ----------- BARY SURFACE MESH ----------- " << std::endl;

    std::ifstream in("data/oni.off");
    SMesh sm;
    in >> sm;
    if(!in || num_vertices(sm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    SM_halfedge_descriptor hd = PMP::longest_border(sm).first;
    assert(hd != SM_halfedge_descriptor());

    // UV map
    typedef SMesh::Property_map<SM_vertex_descriptor, Point_2>  UV_pmap;
    UV_pmap uvpm = sm.add_property_map<SM_vertex_descriptor, Point_2>("h:uv").first;

    // Indices map
    typedef boost::unordered_map<SM_vertex_descriptor, int> Indices;
    Indices indices;
    PMP::connected_component(face(opposite(hd, sm), sm), sm,
                             boost::make_function_output_iterator(
                               SMP::internal::Index_map_filler<SMesh, Indices>(sm, indices)));
    boost::associative_property_map<Indices> vipm(indices);

    // Vertex parameterized map
    boost::unordered_set<SM_vertex_descriptor> vs;
    SMP::internal::Bool_property_map<boost::unordered_set<SM_vertex_descriptor> > vpm(vs);

    // Parameterizer
    SMP::Barycentric_mapping_parameterizer_3<SMesh> parameterizer;

    SMP::Error_code status = parameterizer.parameterize(sm, hd, uvpm, vipm, vpm);
    SMP::Error_code status_bis = SMP::parameterize(sm, parameterizer, hd, uvpm);

    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with Barycentric (SM)!" << std::endl;
    }
  }
#endif // BARY_SURF_MESH

  // ***************************************************************************
  // ARAP WITH SURF_MESH
  // ***************************************************************************

#ifdef ARAP_SURF_MESH
  {
    std::cout << " ----------- ARAP SURFACE MESH -----------" << std::endl;

    std::ifstream in(CGAL::data_file_path("meshes/nefertiti.off"));
    SMesh sm;
    in >> sm;
    if(!in || num_vertices(sm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    // halfedge on the longest border
    SM_halfedge_descriptor hd = PMP::longest_border(sm).first;

    CGAL::Unique_hash_map<SM_vertex_descriptor, Point_2,
                          boost::hash<SM_vertex_descriptor> > uvhm;
    boost::associative_property_map<
      CGAL::Unique_hash_map<SM_vertex_descriptor,
                            Point_2,
                            boost::hash<SM_vertex_descriptor> > > uv_pm(uvhm);

    // Indices map
    typedef boost::unordered_map<SM_vertex_descriptor, int> Indices;
    Indices indices;
    PMP::connected_component(face(opposite(hd, sm), sm), sm,
                             boost::make_function_output_iterator(
                               SMP::internal::Index_map_filler<SMesh, Indices>(sm, indices)));
    boost::associative_property_map<Indices> vipm(indices);

    // Parameterized bool pmap
    boost::unordered_set<SM_vertex_descriptor> vs;
    SMP::internal::Bool_property_map< boost::unordered_set<SM_vertex_descriptor> > vpm(vs);

    // Parameterizer
    SMP::ARAP_parameterizer_3<SMesh> parameterizer;

    SMP::Error_code status = parameterizer.parameterize(sm, hd, uv_pm, vipm, vpm);
    SMP::Error_code status_bis = SMP::parameterize(sm, parameterizer, hd, uv_pm);

    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with ARAP (SM)!" << std::endl;
    }
  }
#endif // ARAP_SURF_MESH

#ifdef DCM_PM_SEAM_MESH
  {
    std::cout << " ----------- DCM POLYHEDRON SEAM MESH -----------" << std::endl;

    std::ifstream in(CGAL::data_file_path("meshes/fandisk.off"));
    PMesh pm;
    in >> pm;
    if(!in || num_vertices(pm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }
    const char* selection = "data/fandisk.dcm.selection.txt";

    PM_seam_edge_hmap seam_edge_hm(false);
    PM_seam_edge_pmap seam_edge_pm(seam_edge_hm);
    PM_seam_vertex_hmap seam_vertex_hm(false);
    PM_seam_vertex_pmap seam_vertex_pm(seam_vertex_hm);

    PM_Seam_mesh mesh(pm, seam_edge_pm, seam_vertex_pm);
    PM_halfedge_descriptor pmhd = mesh.add_seams(selection);
    if(pmhd == PM_halfedge_descriptor() ) {
      std::cerr << "Warning: No seams in input" << std::endl;
    }

    // The 2D points of the uv parametrisation will be written into this map
    // Note that this is a halfedge property map, and that the uv
    // is only stored for the canonical halfedges representing a vertex
    PM_UV_hmap uv_hm;
    PM_UV_pmap uv_pm(uv_hm);

    // a halfedge on the (possibly virtual) border
    PM_SE_halfedge_descriptor hd = PMP::longest_border(mesh).first;

    // Indices
    typedef boost::unordered_map<PM_SE_vertex_descriptor, int> Indices;
    Indices indices;
    PMP::connected_component(face(opposite(hd, mesh), mesh), mesh,
                             boost::make_function_output_iterator(
                               SMP::internal::Index_map_filler<PM_Seam_mesh, Indices>(mesh, indices)));
    boost::associative_property_map<Indices> vipm(indices);

    // Parameterized
    boost::unordered_set<PM_SE_vertex_descriptor> vs;
    SMP::internal::Bool_property_map<boost::unordered_set<PM_SE_vertex_descriptor> > vpm(vs);

    SMP::Discrete_conformal_map_parameterizer_3<PM_Seam_mesh> parameterizer;

    SMP::Error_code status = parameterizer.parameterize(mesh, hd, uv_pm, vipm, vpm);
    SMP::Error_code status_bis = SMP::parameterize(mesh, parameterizer, hd, uv_pm);

    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with DCM (SEAM POLY)!" << std::endl;
    }
  }
#endif // DCM_PM_SEAM_MESH

  // ***************************************************************************
  // DAC WITH SEAM_MESH (SM)
  // ***************************************************************************

#ifdef DAC_SM_SEAM_MESH
  {
    std::cout << " ----------- DAC SURFACE MESH SEAM MESH -----------" << std::endl;

    std::ifstream in(CGAL::data_file_path("meshes/bear.off"));
    SMesh sm;
    in >> sm;
    if(!in || num_vertices(sm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    const char* selection = "data/bear.dac.selection.txt";

    SM_seam_edge_pmap seam_edge_pm =
        sm.add_property_map<SM_edge_descriptor,bool>("e:on_seam", false).first;
    SM_seam_vertex_pmap seam_vertex_pm =
        sm.add_property_map<SM_vertex_descriptor,bool>("v:on_seam", false).first;

    SM_Seam_mesh mesh(sm, seam_edge_pm, seam_vertex_pm);
    SM_halfedge_descriptor smhd = mesh.add_seams(selection);
    if(smhd == SM_halfedge_descriptor() ) {
      std::cerr << "Warning: No seams in input" << std::endl;
    }

    // The 2D points of the uv parametrisation will be written into this map
    // Note that this is a halfedge property map, and that the uv
    // is only stored for the canonical halfedges representing a vertex
    SM_UV_pmap uv_pm = sm.add_property_map<SM_halfedge_descriptor,
                                           Point_2>("h:uv").first;

    // a halfedge on the (possibly virtual) border
    SM_SE_halfedge_descriptor hd = PMP::longest_border(mesh).first;

    // Indices
    typedef boost::unordered_map<SM_SE_vertex_descriptor, int> Indices;
    Indices indices;
    PMP::connected_component(face(opposite(hd, mesh), mesh), mesh,
                             boost::make_function_output_iterator(
                               SMP::internal::Index_map_filler<SM_Seam_mesh, Indices>(mesh, indices)));
    boost::associative_property_map<Indices> vipm(indices);

    // Parameterized
    boost::unordered_set<SM_SE_vertex_descriptor> vs;
    SMP::internal::Bool_property_map<boost::unordered_set<SM_SE_vertex_descriptor> > vpm(vs);

    SMP::Discrete_authalic_parameterizer_3<SM_Seam_mesh> parameterizer;

    SMP::Error_code status = parameterizer.parameterize(mesh, hd, uv_pm, vipm, vpm);
    SMP::Error_code status_bis = SMP::parameterize(mesh, parameterizer, hd, uv_pm);

    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with DAC (SEAM SM)!" << std::endl;
    }
  }
#endif // DAC_SM_SEAM_MESH

#ifdef ORBIFOLD_SM_MESH
  {
    std::cout << " ----------- ORBIFOLD SURFACE MESH -----------" << std::endl;

    SMesh sm; // underlying mesh of the seam mesh

    std::ifstream in(CGAL::data_file_path("meshes/fandisk.off"));
    in >> sm;
    if(!in || num_vertices(sm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    const char* cone_filename = "data/fandisk.orbifold.selection.txt";

    // Read the cones and find the corresponding vertex_descriptor in the underlying mesh 'sm'
    std::vector<SM_vertex_descriptor> cone_sm_vds;
    SMP::read_cones<SMesh>(sm, cone_filename, std::back_inserter(cone_sm_vds));

    // Two property maps to store the seam edges and vertices
    SM_seam_edge_pmap seam_edge_pm = sm.add_property_map<SM_edge_descriptor, bool>("e:on_seam", false).first;
    SM_seam_vertex_pmap seam_vertex_pm = sm.add_property_map<SM_vertex_descriptor, bool>("v:on_seam",false).first;

    // The seam mesh
    SM_Seam_mesh mesh(sm, seam_edge_pm, seam_vertex_pm);

    // Use the path provided between cones to create a seam mesh
    SM_halfedge_descriptor smhd = mesh.add_seams(cone_filename);
    if(smhd == SM_halfedge_descriptor() ) {
      std::list<SM_edge_descriptor> seam_edges;
      SMP::compute_shortest_paths_between_cones(sm, cone_sm_vds.begin(), cone_sm_vds.end(), seam_edges);

      // Add the seams to the seam mesh
      for(SM_edge_descriptor e : seam_edges) {
        mesh.add_seam(source(e, sm), target(e, sm));
      }
    }

    // Index map of the seam mesh (assuming a single connected component so far)
    typedef boost::unordered_map<SM_SE_vertex_descriptor, int> Indices;
    Indices indices;
    boost::associative_property_map<Indices> vimap(indices);
    int counter = 0;
    for(SM_SE_vertex_descriptor vd : vertices(mesh)) {
      put(vimap, vd, counter++);
    }

    // Mark the cones in the seam mesh
    boost::unordered_map<SM_SE_vertex_descriptor, SMP::Cone_type> cmap;
    SMP::locate_cones(mesh, cone_sm_vds.begin(), cone_sm_vds.end(), cmap);

    // The 2D points of the uv parametrisation will be written into this map
    // Note that this is a halfedge property map, and that uv values
    // are only stored for the canonical halfedges representing a vertex
    SM_UV_pmap uvmap = sm.add_property_map<SM_halfedge_descriptor, Point_2>("h:uv").first;

    // Parameterizer
    typedef SMP::Orbifold_Tutte_parameterizer_3<SM_Seam_mesh>         Parameterizer;
    Parameterizer parameterizer(SMP::Parallelogram, SMP::Cotangent);

    // a halfedge on the (possibly virtual) border
    // only used in output (will also be used to handle multiple connected components in the future)
    SM_SE_halfedge_descriptor hd = PMP::longest_border(mesh).first;

    SMP::Error_code status = parameterizer.parameterize(mesh, hd, cmap, uvmap, vimap);

    if(status != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with Orbifold (SEAM SM)!" << std::endl;
    }
  }
#endif // ORBIFOLD_SM_MESH

  // ***************************************************************************
  // ITERATIVE AUTHALIC WITH SURFACE_MESH
  // ***************************************************************************

#ifdef ITERATIVE_SURF_MESH
  {
    std::cout << " ----------- ITERATIVE AUTHALIC SURFACE MESH ----------- " << std::endl;

    std::ifstream in("data/oni.off");
    SMesh sm;
    in >> sm;
    if(!in || num_vertices(sm) == 0) {
      std::cerr << "Problem loading the input data" << std::endl;
      return EXIT_FAILURE;
    }

    SM_halfedge_descriptor hd = PMP::longest_border(sm).first;
    assert(hd != SM_halfedge_descriptor());

    // UV map
    typedef SMesh::Property_map<SM_vertex_descriptor, Point_2>  UV_pmap;
    UV_pmap uvpm = sm.add_property_map<SM_vertex_descriptor, Point_2>("h:uv").first;

    // Indices map
    typedef boost::unordered_map<SM_vertex_descriptor, int> Indices;
    Indices indices;
    PMP::connected_component(face(opposite(hd, sm), sm), sm,
                             boost::make_function_output_iterator(
                               SMP::internal::Index_map_filler<SMesh, Indices>(sm, indices)));
    boost::associative_property_map<Indices> vipm(indices);

    // Vertex parameterized map
    boost::unordered_set<SM_vertex_descriptor> vs;
    SMP::internal::Bool_property_map<boost::unordered_set<SM_vertex_descriptor> > vpm(vs);

    // Parameterizer
    SMP::Iterative_authalic_parameterizer_3<SMesh> parameterizer;

    double error = 0;
    unsigned int iterations = 15;
    SMP::Error_code status = parameterizer.parameterize(sm, hd, uvpm, vipm, vpm, iterations, error);
    SMP::Error_code status_bis = parameterizer.parameterize(sm, uvpm, 10);

    if(status != SMP::OK || status_bis != SMP::OK) {
      std::cout << "Encountered a problem: " << status << std::endl;
      return EXIT_FAILURE;
    }
    else {
      std::cout << "Parameterized with Barycentric (SM)!" << std::endl;
    }
  }
#endif // DAC_SM_SEAM_MESH

  std::cout << "Done!" << std::endl;

  return EXIT_SUCCESS;
}


// Boost.Geometry

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

// This file was modified by Oracle on 2014-2017.
// Modifications copyright (c) 2014-2017 Oracle and/or its affiliates.

// 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_VINCENTY_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP

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

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Distance calculation formulae on latlong coordinates, after Vincenty, 1975
\ingroup distance
\tparam Spheroid The reference spheroid model
\tparam CalculationType \tparam_calculation
\author See
    - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
    - http://www.icsm.gov.au/gda/gdav2.3.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template
<
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class vincenty
    : public strategy::distance::geographic
        <
            strategy::vincenty, Spheroid, CalculationType
        >
{
    typedef strategy::distance::geographic
        <
            strategy::vincenty, Spheroid, CalculationType
        > base_type;

public:
    inline vincenty()
        : base_type()
    {}

    explicit inline vincenty(Spheroid const& spheroid)
        : base_type(spheroid)
    {}
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Spheroid, typename CalculationType>
struct tag<vincenty<Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct return_type<vincenty<Spheroid, CalculationType>, P1, P2>
    : vincenty<Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template <typename Spheroid, typename CalculationType>
struct comparable_type<vincenty<Spheroid, CalculationType> >
{
    typedef vincenty<Spheroid, CalculationType> type;
};

template <typename Spheroid, typename CalculationType>
struct get_comparable<vincenty<Spheroid, CalculationType> >
{
    static inline vincenty<Spheroid, CalculationType> apply(vincenty<Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct result_from_distance<vincenty<Spheroid, CalculationType>, P1, P2 >
{
    template <typename T>
    static inline typename return_type<vincenty<Spheroid, CalculationType>, P1, P2>::type
        apply(vincenty<Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

// We might add a vincenty-like strategy also for point-segment distance, but to calculate the projected point is not trivial

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP


// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_
#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_

#include <list>
#include <unordered_map>
#include <vector>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <autoware_perception_msgs/msg/dynamic_object_with_feature_array.hpp>
class DataAssociation
{
private:
  double getDistance(
    const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1);
  geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud);
  Eigen::MatrixXi can_assign_matrix_;
  Eigen::MatrixXd max_dist_matrix_;
  Eigen::MatrixXd max_area_matrix_;
  Eigen::MatrixXd min_area_matrix_;
  const double score_threshold_;

public:
  DataAssociation();
  bool assign(
    const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
    std::unordered_map<int, int> & reverse_assignment);
  Eigen::MatrixXd calcScoreMatrix(
    const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray & object0,
    const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray & object1);
  virtual ~DataAssociation() {}
};

#endif  // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_


#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>

#include "DataStructures.hpp"
#include "TypesIterator.hpp"
#include <Eigen/Dense>

using Benchmark::BenchConfig;
using Benchmark::benchIter;
using Benchmark::RandomizedSvdConfig;
using Eigen::Index;
using Eigen::MatrixXcd;
using Eigen::MatrixXcf;
using Eigen::MatrixXd;
using Eigen::MatrixXf;

int main() {
  const unsigned int numRuns{10U};
  const unsigned int maxNumIter{4U};
  const auto sizes{std::vector<Index>{200, 500, 1000, 2000}};

  std::stringstream resultsStream;
  resultsStream << "algo,"
                << "numtype,"
                << "num_cols,"
                << "num_rows,"
                << "rank,"
                << "prng_seed,"
                << "rsvd_rank,"
                << "oversampling,"
                << "num_subspace_iter,"
                << "rsvd_prng_seed,"
                << "error,"
                << "runtime" << std::endl;

  for (unsigned int runIdx{0U}; runIdx < numRuns; ++runIdx) {
    std::cout << "Run " << runIdx + 1 << " out of " << numRuns << std::endl;
    for (unsigned int numIter{0U}; numIter < (maxNumIter + 1); ++numIter) {
      for (const Index size : sizes) {
        const BenchConfig benchConf{
            size,         // numRows
            size / 2,     // numCols
            size / 4,     // rank
            777 + runIdx, // prngSeed
        };
        benchConf.display();

        const RandomizedSvdConfig rsvdConf{
            size / 4,     // rank
            10,           // oversampling
            numIter,      // numIter
            777 + runIdx, // prngSeed
        };
        rsvdConf.display();

        benchIter<MatrixXf, MatrixXd, MatrixXcf, MatrixXcd>(benchConf, rsvdConf, resultsStream);
      }
      std::cout << std::endl;
    }
    std::cout << std::endl;
  }

  std::ofstream out("results.csv");
  out << resultsStream.str();
  out.close();

  return 0;
}


#include "cube.h"
#include "sphere.h"
#include "write_obj.h"
#include "QuadViewer.h"
#include "set_texture_from_png.h"
#include <igl/readOBJ.h>
#include <Eigen/Core>
#include <vector>
#include <iostream>
#include <memory>
#include <limits>
#include <functional>

int main(int argc, char * argv[])
{
  {
    // Create mesh of a cube
    Eigen::MatrixXd V,UV,NV;
    Eigen::MatrixXi F,UF,NF;
    cube(V,F,UV,UF,NV,NF);
    write_obj("cube.obj",V,F,UV,UF,NV,NF);
    igl::readOBJ("cube.obj",V,UV,NV,F,UF,NF);
    {
      QuadViewer v;
      v.set_mesh(V,F,UV,UF,NV,NF);
      set_texture_from_png("../data/rubiks-cube.png", v.viewer.data());
      v.launch();
    }
  }

  {
    // Create mesh of a sphere
    Eigen::MatrixXd V,UV,NV;
    Eigen::MatrixXi F,UF,NF;
    sphere(60,40,V,F,UV,UF,NV,NF);
    write_obj("sphere.obj",V,F,UV,UF,NV,NF);
    igl::readOBJ("sphere.obj",V,UV,NV,F,UF,NF);
    {
      QuadViewer v;
      v.set_mesh(V,F,UV,UF,NV,NF);
      set_texture_from_png("../data/earth-square.png", v.viewer.data());
      v.launch();
    }
  }

}


// Copyright (C) 2015  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "cluster.h"
#include <dlib/console_progress_indicator.h>
#include <dlib/image_io.h>
#include <dlib/data_io.h>
#include <dlib/image_transforms.h>
#include <dlib/misc_api.h>
#include <dlib/dir_nav.h>
#include <dlib/clustering.h>
#include <dlib/svm.h>
#include <dlib/statistics.h>

// ----------------------------------------------------------------------------------------

using namespace std;
using namespace dlib;

//  ----------------------------------------------------------------------------

struct assignment
{
    unsigned long c;
    double dist;
    unsigned long idx;

    bool operator<(const assignment& item) const
    { return dist < item.dist; }
};

std::vector<assignment> angular_cluster (
    std::vector<matrix<double,0,1> > feats,
    const unsigned long num_clusters
)
{
    DLIB_CASSERT(feats.size() != 0, "The dataset can't be empty");
    for (unsigned long i = 0; i < feats.size(); ++i)
    {
        DLIB_CASSERT(feats[i].size() == feats[0].size(), "All feature vectors must have the same length.");
    }

    // find the centroid of feats
    const matrix<double,0,1> m = mean(mat(feats));

    // Now center feats and then project onto the unit sphere.  The reason for projecting
    // onto the unit sphere is so pick_initial_centers() works in a sensible way.
    for (auto& f : feats)
    {
        f = normalize(f-m);
    }

    // now do angular clustering of the points
    std::vector<matrix<double,0,1> > centers;
    pick_initial_centers(num_clusters, centers, feats, linear_kernel<matrix<double,0,1> >(), 0.05);
    find_clusters_using_angular_kmeans(feats, centers);

    // and then report the resulting assignments
    std::vector<assignment> assignments;
    for (unsigned long i = 0; i < feats.size(); ++i)
    {
        assignment temp;
        temp.c = nearest_center(centers, feats[i]);
        temp.dist = length(feats[i] - centers[temp.c]);
        temp.idx = i;
        assignments.push_back(temp);
    }
    return assignments;
}
std::vector<assignment> chinese_cluster (
    std::vector<matrix<double,0,1> > feats,
    unsigned long &num_clusters
    )
{
    DLIB_CASSERT(feats.size() != 0, "The dataset can't be empty");
    for (unsigned long i = 0; i < feats.size(); ++i)
    {
        DLIB_CASSERT(feats[i].size() == feats[0].size(), "All feature vectors must have the same length.");
    }

    // Try to find a good value to select if we should add a vertex in the graph.  First we
    // normalize the features.
    const matrix<double,0,1> m = mean(mat(feats));

    for (auto& f : feats)
    {
        f = normalize(f-m);
    }

    // Then we find the average distance between them, that average will be a good threshold to
    // decide if pairs are connected.
    running_stats<double> rs;
    for (size_t i = 0; i < feats.size(); ++i) {
        for (size_t j = i; j < feats.size(); ++j) {
            rs.add(length(feats[i] - feats[j]));
        }
    }

    // add vertices for chinese whispers to find clusters
    std::vector<sample_pair> edges;
    for (size_t i = 0; i < feats.size(); ++i) {
        for (size_t j = i; j < feats.size(); ++j) {
            if (length(feats[i] - feats[j]) < rs.mean()) {
                edges.push_back(sample_pair(i, j, length(feats[i] - feats[j])));
            }
        }
    }

    std::vector<unsigned long> labels;
    num_clusters = chinese_whispers(edges, labels);

    std::vector<assignment> assignments;
    for (unsigned long i = 0; i < feats.size(); ++i)
    {
        assignment temp;
        temp.c = labels[i];
        temp.dist = length(feats[i]);
        temp.idx = i;
        assignments.push_back(temp);
    }
    return assignments;
}

// ----------------------------------------------------------------------------------------

bool compare_first (
    const std::pair<double,image_dataset_metadata::image>& a,
    const std::pair<double,image_dataset_metadata::image>& b
)
{
    return a.first < b.first;
}

// ----------------------------------------------------------------------------------------

double mean_aspect_ratio (
    const image_dataset_metadata::dataset& data
)
{
    double sum = 0;
    double cnt = 0;
    for (unsigned long i = 0; i < data.images.size(); ++i)
    {
        for (unsigned long j = 0; j < data.images[i].boxes.size(); ++j)
        {
            rectangle rect = data.images[i].boxes[j].rect;
            if (rect.area() == 0 || data.images[i].boxes[j].ignore)
                continue;
            sum += rect.width()/(double)rect.height();
            ++cnt;
        }
    }

    if (cnt != 0)
        return sum/cnt;
    else
        return 0;
}

// ----------------------------------------------------------------------------------------

bool has_non_ignored_boxes (const image_dataset_metadata::image& img)
{
    for (auto&& b : img.boxes)
    {
        if (!b.ignore)
            return true;
    }
    return false;
}

// ----------------------------------------------------------------------------------------

int cluster_dataset(
    const dlib::command_line_parser& parser
)
{
    // make sure the user entered an argument to this program
    if (parser.number_of_arguments() != 1)
    {
        cerr << "The --cluster option requires you to give one XML file on the command line." << endl;
        return EXIT_FAILURE;
    }

    unsigned long num_clusters = get_option(parser, "cluster", 0);
    const unsigned long chip_size = get_option(parser, "size", 8000);

    image_dataset_metadata::dataset data;

    image_dataset_metadata::load_image_dataset_metadata(data, parser[0]);
    set_current_dir(get_parent_directory(file(parser[0])));

    const double aspect_ratio = mean_aspect_ratio(data);

    dlib::array<array2d<rgb_pixel> > images;
    std::vector<matrix<double,0,1> > feats;
    console_progress_indicator pbar(data.images.size());
    // extract all the object chips and HOG features.
    cout << "Loading image data..." << endl;
    for (unsigned long i = 0; i < data.images.size(); ++i)
    {
        pbar.print_status(i);
        if (!has_non_ignored_boxes(data.images[i]))
            continue;

        array2d<rgb_pixel> img, chip;
        load_image(img, data.images[i].filename);

        for (unsigned long j = 0; j < data.images[i].boxes.size(); ++j)
        {
            if (data.images[i].boxes[j].ignore || data.images[i].boxes[j].rect.area() < 10)
                continue;
            drectangle rect = data.images[i].boxes[j].rect;
            rect = set_aspect_ratio(rect, aspect_ratio);
            extract_image_chip(img, chip_details(rect, chip_size), chip);
            feats.push_back(extract_fhog_features(chip));
            images.push_back(chip);
        }
    }

    if (feats.size() == 0)
    {
        cerr << "No non-ignored object boxes found in the XML dataset.  You can't cluster an empty dataset." << endl;
        return EXIT_FAILURE;
    }

    cout << "\nClustering objects..." << endl;
    std::vector<assignment> assignments;
    if (num_clusters) {
        assignments = angular_cluster(feats, num_clusters);
    } else {
        assignments = chinese_cluster(feats, num_clusters);
    }

    // Now output each cluster to disk as an XML file.
    for (unsigned long c = 0; c < num_clusters; ++c)
    {
        // We are going to accumulate all the image metadata for cluster c.  We put it
        // into idata so we can sort the images such that images with central chips
        // come before less central chips.  The idea being to get the good chips to
        // show up first in the listing, making it easy to manually remove bad ones if
        // that is desired.
        std::vector<std::pair<double,image_dataset_metadata::image> > idata(data.images.size());
        unsigned long idx = 0;
        for (unsigned long i = 0; i < data.images.size(); ++i)
        {
            idata[i].first = std::numeric_limits<double>::infinity();
            idata[i].second.filename = data.images[i].filename;
            idata[i].second.width = data.images[i].width;
            idata[i].second.height = data.images[i].height;
            if (!has_non_ignored_boxes(data.images[i]))
                continue;

            for (unsigned long j = 0; j < data.images[i].boxes.size(); ++j)
            {
                idata[i].second.boxes.push_back(data.images[i].boxes[j]);

                if (data.images[i].boxes[j].ignore || data.images[i].boxes[j].rect.area() < 10)
                    continue;

                // If this box goes into cluster c then update the score for the whole
                // image based on this boxes' score.  Otherwise, mark the box as
                // ignored.
                if (assignments[idx].c == c)
                    idata[i].first = std::min(idata[i].first, assignments[idx].dist);
                else
                    idata[i].second.boxes.back().ignore = true;

                ++idx;
            }
        }

        // now save idata to an xml file.
        std::sort(idata.begin(), idata.end(), compare_first);
        image_dataset_metadata::dataset cdata;
        cdata.comment = data.comment + "\n\n This file contains objects which were clustered into group " +
                     cast_to_string(c+1) + " of " + cast_to_string(num_clusters) + " groups with a chip size of " +
                     cast_to_string(chip_size) + " by imglab.";
        cdata.name = data.name;
        for (unsigned long i = 0; i < idata.size(); ++i)
        {
            // if this image has non-ignored boxes in it then include it in the output.
            if (idata[i].first != std::numeric_limits<double>::infinity())
                cdata.images.push_back(idata[i].second);
        }

        string outfile = "cluster_"+pad_int_with_zeros(c+1, 3) + ".xml";
        cout << "Saving " << outfile << endl;
        save_image_dataset_metadata(cdata, outfile);
    }

    // Now output each cluster to disk as a big tiled jpeg file.  Sort everything so, just
    // like in the xml file above, the best objects come first in the tiling.
    std::sort(assignments.begin(), assignments.end());
    for (unsigned long c = 0; c < num_clusters; ++c)
    {
        dlib::array<array2d<rgb_pixel> > temp;
        for (unsigned long i = 0; i < assignments.size(); ++i)
        {
            if (assignments[i].c == c)
                temp.push_back(images[assignments[i].idx]);
        }

        if (parser.option("webp"))
        {
            string outfile = "cluster_"+pad_int_with_zeros(c+1, 3) + ".webp";
            cout << "Saving " << outfile << endl;
            const float webp_quality = std::stof(parser.option("webp").argument());
            save_webp(tile_images(temp), outfile, webp_quality);
        }
        else
        {
            string outfile = "cluster_"+pad_int_with_zeros(c+1, 3) + ".jpg";
            cout << "Saving " << outfile << endl;
            save_jpeg(tile_images(temp), outfile);
        }
    }

    return EXIT_SUCCESS;
}

// ----------------------------------------------------------------------------------------


/* The Image Registration Toolkit (IRTK)
 *
 * Copyright 2008-2015 Imperial College London
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

// irtk includes
#include <irtkImage.h>
#include <irtkNoise.h>

// recipe includes
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

template <class VoxelType> irtkRicianNoise<VoxelType>::irtkRicianNoise(double Amplitude) : irtkNoise<VoxelType>(Amplitude)
{
}

template <class VoxelType> double irtkRicianNoise<VoxelType>::Run(int x, int y, int z, int t)
{
  boost::mt19937 rng;
  rng.seed(this->_Init);

  boost::normal_distribution<> nd(0.0, 1.0);
  boost::variate_generator<boost::mt19937&,
			   boost::normal_distribution<> > var_nor(rng, nd);

#ifdef OLD_RICIAN
  // For backward consistency:
  if (this->_input->Get(x, y, z) <= 3*this->_Amplitude) {
    // Rayleigh distributed noise in background and air
    double s1 = var_nor();
    double s2 = var_nor();
    double M  = sqrt(s1*s1+s2*s2);
    return double(this->_input->Get(x, y, z, y)) + this->_Amplitude * M;
  } else {
    // Gaussian distributed noise in foreground
    return double(this->_input->Get(x, y, z, y)) + this->_Amplitude * var_nor();
  }

#else
  // Add Rician noise by treating the magnitude image as the real part (r) of a new complex variable (r,i),
  // adding Gaussian noise to the separate parts of this complex variable, before taking the magnitude again.
  // Thanks to Ged Ridgway <gerard.ridgway@ucl.ac.uk> for this contribution.
  double r = double(this->_input->Get(x, y, z, t)) + this->_Amplitude * var_nor();
  double i = this->_Amplitude * var_nor();
  return sqrt(r*r + i*i);

#endif
}

template class irtkRicianNoise<irtkBytePixel>;
template class irtkRicianNoise<irtkGreyPixel>;
template class irtkRicianNoise<irtkRealPixel>;


#pragma once

#include <Eigen/Core>

namespace ipc {

inline double world_bbox_diagonal_length(const Eigen::MatrixXd& V)
{
    return (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm();
}

} // namespace ipc


// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>
#include <OpenTissue/core/spline/spline.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

// Boost Test declaration and Checking macros
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/floating_point_comparison.hpp>

typedef OpenTissue::spline::MathTypes<double, size_t>                        math_types;
typedef math_types::vector_type                                                    vector_type;
typedef std::vector<double>                                                        knot_container;
typedef std::vector<vector_type>                                                   point_container;
typedef OpenTissue::spline::NUBSpline<knot_container, point_container>             NUBSpline;

knot_container init_knots(int const & M)
{
  knot_container U;
  for(int i=0;i<M;++i)
    U.push_back( i*1.0 );
  return U;
}

point_container init_controls(int const & N)
{
  point_container P;
  for (int i=0;i<N;++i)
  {
    vector_type p(2);
    p(0) = i;
    p(1) = N-i;
    P.push_back(p);
  }
  return P;
}

BOOST_AUTO_TEST_SUITE(opentissue_core_spline_nubs);

BOOST_AUTO_TEST_CASE(test_construction)
{
  //  No Data Case
  {
    NUBSpline nubspline;

    knot_container & U = nubspline.get_knot_container();
    point_container & P = nubspline.get_control_container();
    BOOST_CHECK( U.size() == 0);
    BOOST_CHECK( P.size() == 0);

    knot_container const & UU = nubspline.get_knot_container();
    point_container const & PP = nubspline.get_control_container();
    BOOST_CHECK( UU.size() == 0);
    BOOST_CHECK( PP.size() == 0);

    BOOST_CHECK_EQUAL( nubspline.get_order() , 0u);
    BOOST_CHECK_EQUAL( nubspline.get_dimension(), 0u);
  }
  // The Only Legal Data case
  {
    knot_container U = init_knots(10);
    point_container P = init_controls(7);
    NUBSpline nubspline;
    BOOST_CHECK_NO_THROW( nubspline = NUBSpline(3,U,P) );

    BOOST_CHECK_EQUAL( nubspline.get_dimension(), 2u);
    BOOST_CHECK_EQUAL( nubspline.get_order(), 3u);

    knot_container & UU = nubspline.get_knot_container();
    point_container & PP = nubspline.get_control_container();
    BOOST_CHECK( UU.size() == 10);
    BOOST_CHECK( PP.size() == 7);

    knot_container const & UUU = nubspline.get_knot_container();
    point_container const & PPP = nubspline.get_control_container();
    BOOST_CHECK( UUU.size() == 10);
    BOOST_CHECK( PPP.size() == 7);
  }
  // Wrong order
  {
    knot_container U = init_knots(10);
    point_container P = init_controls(7);
    NUBSpline nubspline;
    BOOST_CHECK_THROW( nubspline = NUBSpline(2,U,P), std::invalid_argument );
  }
  // Illegal order
  {
    knot_container U = init_knots(10);
    point_container P = init_controls(7);
    NUBSpline nubspline;
    BOOST_CHECK_THROW( nubspline = NUBSpline(0,U,P), std::invalid_argument );
  }
  // Forgot knots
  {
    knot_container U;
    point_container P = init_controls(7);
    NUBSpline nubspline;

    BOOST_CHECK_THROW( nubspline = NUBSpline(3,U,P), std::invalid_argument );
  }
  // Forgot controls
  {
    knot_container U = init_knots(10);
    point_container P;
    NUBSpline nubspline;

    BOOST_CHECK_THROW( nubspline = NUBSpline(3,U,P), std::invalid_argument );
  }
  // Wrong number of controls
  {
    knot_container U = init_knots(10);
    point_container P = init_controls(6);
    NUBSpline nubspline;
    BOOST_CHECK_THROW( nubspline = NUBSpline(3,U,P), std::invalid_argument );
  }
  // Wrong number of knots
  {
    knot_container U = init_knots(9);
    point_container P = init_controls(7);
    NUBSpline nubspline;
    BOOST_CHECK_THROW( nubspline = NUBSpline(3,U,P), std::invalid_argument );
  }
}

BOOST_AUTO_TEST_SUITE_END();