// Copyright (c) 2014 GeometryFactory
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
// You can redistribute it and/or modify it under the terms of the GNU
// General Public License as published by the Free Software Foundation,
// either version 3 of the License, or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
//
// Author(s)     : Yin Xu, Andreas Fabri and Ilker O. Yaz

#ifndef CGAL_DEFORM_MESH_H
#define CGAL_DEFORM_MESH_H

#include <CGAL/config.h>
#include <CGAL/internal/Surface_modeling/Weights.h>
#include <CGAL/Default.h>
#include <CGAL/tuple.h>

#include <vector>
#include <list>
#include <utility>
#include <limits>
#include <boost/foreach.hpp>

/*
#define CGAL_DEFORM_MESH_USE_EXPERIMENTAL_SCALE // define it to activate optimal scale calculation,
// then you can define below to just scale, if not both rotate and scale will be activated
#define CGAL_DEFORM_MESH_JUST_EXPERIMENTAL_SCALE // to not to rotate but just scale
*/

// for default parameters
#if defined(CGAL_EIGEN3_ENABLED)
#include <CGAL/Eigen_solver_traits.h>  // for sparse linear system solver
#include <CGAL/Deformation_Eigen_polar_closest_rotation_traits_3.h>  // for 3x3 closest rotation computer
#endif

namespace CGAL {

/// \ingroup PkgSurfaceModeling
///@brief Deformation algorithm type
enum Deformation_algorithm_tag
{
  ORIGINAL_ARAP,  /**< use original as-rigid-as possible algorithm */
  SPOKES_AND_RIMS /**< use spokes and rims version of as-rigid-as possible algorithm */
};

/// @cond CGAL_DOCUMENT_INTERNAL
namespace internal {
template<class HalfedgeGraph, Deformation_algorithm_tag deformation_algorithm_tag>
struct Weight_calculator_selector {
  typedef Uniform_weight<HalfedgeGraph> weight_calculator;
};

template<class HalfedgeGraph>
struct Weight_calculator_selector<HalfedgeGraph, CGAL::SPOKES_AND_RIMS> {
  typedef Single_cotangent_weight<HalfedgeGraph> weight_calculator;
};

template<class HalfedgeGraph>
struct Weight_calculator_selector<HalfedgeGraph, CGAL::ORIGINAL_ARAP> {
  typedef Cotangent_weight<HalfedgeGraph> weight_calculator;
};
}//namespace internal
/// @endcond

 ///
 /// \ingroup PkgSurfaceModeling
 /// @brief Class providing the functionalities for deforming a triangulated surface mesh
 ///
 /// @tparam HG a model of HalfedgeGraph
 /// @tparam VIM a model of `ReadablePropertyMap`</a>  with Deform_mesh::vertex_descriptor as key and `unsigned int` as value type.
 ///         The default is `boost::property_map<HG, boost::%vertex_index_t>::%type`.
 /// @tparam HIM a model of `ReadablePropertyMap`</a>  with Deform_mesh::halfedge_descriptor as key and `unsigned int` as value type.
 ///         The default is `boost::property_map<HG, boost::%halfedge_index_t>::%type`.
 /// @tparam TAG tag for selecting the deformation algorithm
 /// @tparam WC a model of SurfaceModelingWeights, with `WC::Halfedge_graph` being `HG`.
 ///         If `TAG` is `ORIGINAL_ARAP`, the weights must be positive to guarantee a correct energy minimization.
 ///         The default is the cotangent weighting scheme. In case `TAG` is `ORIGINAL_ARAP`, negative weights are clamped to zero.
 /// @tparam ST a model of SparseLinearAlgebraTraitsWithFactor_d. If \ref thirdpartyEigen "Eigen" 3.2 (or greater) is available
 /// and `CGAL_EIGEN3_ENABLED` is defined, then an overload of `Eigen_solver_traits` is provided as default parameter.\n
  /// \code
 ///     CGAL::Eigen_solver_traits<
 ///         Eigen::SparseLU<
 ///            CGAL::Eigen_sparse_matrix<double>::EigenType,
 ///            Eigen::COLAMDOrdering<int> >  >
 /// \endcode
 /// @tparam CR a model of DeformationClosestRotationTraits_3. If \ref thirdpartyEigen "Eigen" 3.1 (or greater) is available and `CGAL_EIGEN3_ENABLED` is defined,
 /// `Deformation_Eigen_polar_closest_rotation_traits_3` is provided as default parameter.
 /// @tparam VPM a model of `ReadWritePropertyMap`</a>  with Deform_mesh::vertex_descriptor as key and a point as value type. The point type must be a model of `::RawPoint_3`.
 /// The default is `boost::property_map<HG, CGAL::vertex_point_t>::%type`.
template <
  class HG,
  class VIM=Default,
  class HIM=Default,
  Deformation_algorithm_tag TAG = SPOKES_AND_RIMS,
  class WC = Default,
  class ST = Default,
  class CR = Default,
  class VPM = Default
  >
class Deform_mesh
{
//Typedefs
public:

  /// \name Types
  /// @{
  // typedefed template parameters, main reason is doxygen creates autolink to typedefs but not template parameters
  ///
  typedef HG Halfedge_graph;

// Index maps
#ifndef DOXYGEN_RUNNING
  typedef typename Default::Get<
    VIM,
    typename boost::property_map<Halfedge_graph, boost::vertex_index_t>::type
  >::type Vertex_index_map;
  typedef typename Default::Get<
    HIM,
    typename boost::property_map<Halfedge_graph, boost::halfedge_index_t>::type
  >::type Hedge_index_map;
#else
  ///
  typedef VIM Vertex_index_map;
  ///
  typedef HIM Hedge_index_map;
#endif

// weight calculator
#ifndef DOXYGEN_RUNNING
  typedef typename Default::Get<
    WC,
    typename internal::Weight_calculator_selector<HG, TAG>::weight_calculator
  >::type Weight_calculator;
#else
  ///
  typedef WC Weight_calculator;
#endif

// sparse linear solver
#ifndef DOXYGEN_RUNNING
  typedef typename Default::Get<
    ST,
  #if defined(CGAL_EIGEN3_ENABLED)
      CGAL::Eigen_solver_traits<
          Eigen::SparseLU<
            CGAL::Eigen_sparse_matrix<double>::EigenType,
            Eigen::COLAMDOrdering<int> >  >
  #else
    ST // no parameter provided, and Eigen is not enabled: so don't compile!
  #endif
  >::type Sparse_linear_solver;
#else
  ///
  typedef ST Sparse_linear_solver;
#endif

// CR helper
#ifndef DOXYGEN_RUNNING
  typedef typename Default::Get<
    CR,
  #if defined(CGAL_EIGEN3_ENABLED)
    Deformation_Eigen_polar_closest_rotation_traits_3
  #else
    CR // no parameter provided, and Eigen is not enabled: so don't compile!
  #endif
  >::type Closest_rotation_traits;
#else
  ///
  typedef CR Closest_rotation_traits;
#endif

// vertex point pmap
#ifndef DOXYGEN_RUNNING
  typedef typename Default::Get<
    VPM,
    typename boost::property_map<Halfedge_graph, CGAL::vertex_point_t>::type
  >::type Vertex_point_map;
#else
  ///
  typedef VPM Vertex_point_map;
#endif

  /// The type for vertex descriptor
  typedef typename boost::graph_traits<Halfedge_graph>::vertex_descriptor vertex_descriptor;
  /// The type for halfedge descriptor
  typedef typename boost::graph_traits<Halfedge_graph>::halfedge_descriptor halfedge_descriptor;
  /// The 3D point type, model of `::RawPoint_3`
  typedef typename boost::property_traits<Vertex_point_map>::value_type Point;
  /// A constant iterator range over the vertices of the region-of-interest.
  /// It is a model of `ConstRange` with `vertex_descriptor` as iterator value type.
  typedef std::vector<vertex_descriptor> Roi_vertex_range;
/// @}

private:
  typedef Deform_mesh<HG, VIM, HIM, TAG, WC, ST, CR> Self;
  // Repeat Halfedge_graph types
  typedef typename boost::graph_traits<Halfedge_graph>::vertex_iterator     vertex_iterator;
  typedef typename boost::graph_traits<Halfedge_graph>::halfedge_iterator       halfedge_iterator;
  typedef typename boost::graph_traits<Halfedge_graph>::in_edge_iterator    in_edge_iterator;
  typedef typename boost::graph_traits<Halfedge_graph>::out_edge_iterator   out_edge_iterator;

  typedef typename Closest_rotation_traits::Matrix CR_matrix;
  typedef typename Closest_rotation_traits::Vector CR_vector;

// Data members.
  Halfedge_graph& m_halfedge_graph;                   /**< Source triangulated surface mesh for modeling */

  std::vector<Point> original;                        ///< original positions of roi (size: ros + boundary_of_ros)
  std::vector<Point> solution;                        ///< storing position of ros vertices during iterations (size: ros + boundary_of_ros)

  Vertex_index_map vertex_index_map;                  ///< storing indices of all vertices
  Hedge_index_map   hedge_index_map;                  ///< storing indices of all halfedges

  std::vector<vertex_descriptor> roi;                 ///< region of interest
  std::vector<vertex_descriptor> ros;                 ///< region of solution, including roi and hard constraints on boundary of roi

  std::vector<std::size_t> ros_id_map;                ///< (size: num vertices)
  std::vector<bool>        is_roi_map;                ///< (size: num vertices)
  std::vector<bool>        is_ctrl_map;               ///< (size: num vertices)

  std::vector<double> hedge_weight;                   ///< all halfedge weights
  std::vector<CR_matrix> rot_mtr;                     ///< rotation matrices of ros vertices (size: ros)

  Sparse_linear_solver m_solver;                      ///< linear sparse solver
  unsigned int m_iterations;                          ///< number of maximal iterations
  double m_tolerance;                                 ///< tolerance of convergence

  bool need_preprocess_factorization;                 ///< is there any need to compute L and factorize
  bool need_preprocess_region_of_solution;            ///< is there any need to compute region of solution

  bool last_preprocess_successful;                    ///< stores the result of last call to preprocess()

  Weight_calculator weight_calculator;

  Vertex_point_map vertex_point_map;

#ifdef CGAL_DEFORM_MESH_USE_EXPERIMENTAL_SR_ARAP
public:
// SR-ARAP [Zohar13]
  double m_sr_arap_alpha;
private:
#endif
#ifdef CGAL_DEFORM_MESH_USE_EXPERIMENTAL_SCALE
  std::vector<double> scales;
#endif

#ifndef CGAL_CFG_NO_CPP0X_DELETED_AND_DEFAULT_FUNCTIONS
public:
  Deform_mesh(const Self&) = delete; // no copy
#else
private:
  Deform_mesh(const Self&); // no copy
#endif


// Public methods
public:

  /// \cond SKIP_FROM_MANUAL
  //vertex_point_map set by default
  Deform_mesh(Halfedge_graph& halfedge_graph,
              Vertex_index_map vertex_index_map,
              Hedge_index_map hedge_index_map
              )
    : m_halfedge_graph(halfedge_graph), vertex_index_map(vertex_index_map), hedge_index_map(hedge_index_map),
      ros_id_map(std::vector<std::size_t>(num_vertices(halfedge_graph), (std::numeric_limits<std::size_t>::max)() )),
      is_roi_map(std::vector<bool>(num_vertices(halfedge_graph), false)),
      is_ctrl_map(std::vector<bool>(num_vertices(halfedge_graph), false)),
      m_iterations(5), m_tolerance(1e-4),
      need_preprocess_factorization(true),
      need_preprocess_region_of_solution(true),
      last_preprocess_successful(false),
      weight_calculator(Weight_calculator()),
      vertex_point_map(get(vertex_point, halfedge_graph))
  {
    init();
  }

  //vertex_point_map and hedge_index_map set by default
  Deform_mesh(Halfedge_graph& halfedge_graph,
              Vertex_index_map vertex_index_map
              )
    : m_halfedge_graph(halfedge_graph), vertex_index_map(vertex_index_map),
      hedge_index_map(get(boost::halfedge_index, halfedge_graph)),
      ros_id_map(std::vector<std::size_t>(num_vertices(halfedge_graph), (std::numeric_limits<std::size_t>::max)() )),
      is_roi_map(std::vector<bool>(num_vertices(halfedge_graph), false)),
      is_ctrl_map(std::vector<bool>(num_vertices(halfedge_graph), false)),
      m_iterations(5), m_tolerance(1e-4),
      need_preprocess_factorization(true),
      need_preprocess_region_of_solution(true),
      last_preprocess_successful(false),
      weight_calculator(Weight_calculator()),
      vertex_point_map(get(vertex_point, halfedge_graph))
  {
    init();
  }
  //vertex_point_map, hedge_index_map and vertex_index_map set by default
  Deform_mesh(Halfedge_graph& halfedge_graph
              )
    : m_halfedge_graph(halfedge_graph),
      vertex_index_map(get(boost::vertex_index, halfedge_graph)),
      hedge_index_map(get(boost::halfedge_index, halfedge_graph)),
      ros_id_map(std::vector<std::size_t>(num_vertices(halfedge_graph), (std::numeric_limits<std::size_t>::max)() )),
      is_roi_map(std::vector<bool>(num_vertices(halfedge_graph), false)),
      is_ctrl_map(std::vector<bool>(num_vertices(halfedge_graph), false)),
      m_iterations(5), m_tolerance(1e-4),
      need_preprocess_factorization(true),
      need_preprocess_region_of_solution(true),
      last_preprocess_successful(false),
      weight_calculator(Weight_calculator()),
      vertex_point_map(get(vertex_point, halfedge_graph))
  {
    init();
  }

  // Constructor with all the parameters provided
  Deform_mesh(Halfedge_graph& halfedge_graph,
    Vertex_index_map vertex_index_map,
    Hedge_index_map hedge_index_map,
    Vertex_point_map vertex_point_map,
    Weight_calculator weight_calculator = Weight_calculator()
    )
    : m_halfedge_graph(halfedge_graph), vertex_index_map(vertex_index_map), hedge_index_map(hedge_index_map),
    ros_id_map(std::vector<std::size_t>(num_vertices(halfedge_graph), (std::numeric_limits<std::size_t>::max)() )),
    is_roi_map(std::vector<bool>(num_vertices(halfedge_graph), false)),
    is_ctrl_map(std::vector<bool>(num_vertices(halfedge_graph), false)),
    m_iterations(5), m_tolerance(1e-4),
    need_preprocess_factorization(true),
    need_preprocess_region_of_solution(true),
    last_preprocess_successful(false),
    weight_calculator(weight_calculator),
    vertex_point_map(vertex_point_map)
  {
    init();
  }
  /// \endcond
  #if DOXYGEN_RUNNING
/// \name Construction
/// @{
    /**
   * The constructor of a deformation object
   *
   * @pre the halfedge_graph consists of only triangular facets
   * @param halfedge_graph triangulated surface mesh to deform
   * @param vertex_index_map property map for associating an id to each vertex, from `0` to `num_vertices(halfedge_graph)-1`.
   * @param hedge_index_map property map for associating an id to each halfedge, from `0` to `2*num_edges(halfedge_graph)-1`.
   * @param vertex_point_map property map used to access the points associated to each vertex of the graph.
   * @param weight_calculator function object or pointer for weight calculation
   */
  Deform_mesh(Halfedge_graph& halfedge_graph,
    Vertex_index_map vertex_index_map=get(boost::vertex_index, halfedge_graph),
    Hedge_index_map hedge_index_map=get(boost::halfedge_index, halfedge_graph),
    Vertex_point_map vertex_point_map=get(vertex_point, halfedge_graph),
    Weight_calculator weight_calculator = Weight_calculator()
    );
/// @}
  #endif

private:
  void init() {
    // compute halfedge weights
    halfedge_iterator eb, ee;
    hedge_weight.reserve(2*num_edges(m_halfedge_graph));
    for(cpp11::tie(eb, ee) = halfedges(m_halfedge_graph); eb != ee; ++eb)
    {
      hedge_weight.push_back(
        this->weight_calculator(*eb, m_halfedge_graph, vertex_point_map));
    }
#ifdef CGAL_DEFORM_MESH_USE_EXPERIMENTAL_SR_ARAP
    m_sr_arap_alpha=2;
#endif
  }

public:

/// \name Preprocessing
/// @{
  /**
   * Erases all the vertices from the region-of-interest (control vertices included).
   */
  void clear_roi_vertices(){
    need_preprocess_both();
    // clear roi vertices
    roi.clear();
    //set to false all bits
    is_roi_map.assign(num_vertices(m_halfedge_graph), false);
    is_ctrl_map.assign(num_vertices(m_halfedge_graph), false);
  }

  /**
   * Erases all the vertices from the set of control vertices.
   */
  void clear_control_vertices(){
    need_preprocess_factorization=true;
    //set to false all bits
    is_ctrl_map.assign(num_vertices(m_halfedge_graph), false);
  }

  /**
   * Inserts a vertex in the set of control vertices. The vertex is also inserted in the region-of-interest if it is not already in it.
   * @param vd the vertex to be inserted
   * @return `true` if `vd` is not already a control vertex.
   */
  bool insert_control_vertex(vertex_descriptor vd)
  {
    if(is_control_vertex(vd)) { return false; }
    need_preprocess_factorization=true;

    insert_roi_vertex(vd); // also insert it as roi

    is_ctrl_map[id(vd)] = true;
    return true;
  }

  /**
   * Inserts a range of vertices in the set of control vertices. The vertices are also inserted in the region-of-interest if they are not already in it.
   * @tparam InputIterator input iterator type with `vertex_descriptor` as value type
   * @param begin first iterator of the range of vertices
   * @param end past-the-end iterator of the range of vertices
   */
  template<class InputIterator>
  void insert_control_vertices(InputIterator begin, InputIterator end)
  {
    for( ;begin != end; ++begin)
    {
      insert_control_vertex(*begin);
    }
  }

  /**
   * Erases a vertex from the set of control vertices.
   * @param vd the vertex to be erased
   * @return `true` if `vd` was a control vertex.
   */
  bool erase_control_vertex(vertex_descriptor vd)
  {
    if(!is_control_vertex(vd)) { return false; }

    need_preprocess_factorization=true;
    is_ctrl_map[id(vd)] = false;
    return true;
  }

  /**
   * Inserts a range of vertices in the region-of-interest
   * @tparam InputIterator input iterator with `vertex_descriptor` as value type
   * @param begin first iterator of the range of vertices
   * @param end past-the-end iterator of the range of vertices
   */
  template<class InputIterator>
  void insert_roi_vertices(InputIterator begin, InputIterator end)
  {
    for( ;begin != end; ++begin)
    {
      insert_roi_vertex(*begin);
    }
  }

  /**
   * Inserts a vertex in the region-of-interest
   * @param vd the vertex to be inserted
   * @return `true` if `vd` is not already in the region-of-interest.
   */
  bool insert_roi_vertex(vertex_descriptor vd)
  {
    if(is_roi_vertex(vd)) { return false; }
    need_preprocess_both();

    is_roi_map[id(vd)] = true;
    roi.push_back(vd);
    return true;
  }

  /**
   * Erases a vertex from the region-of-interest and the set of control vertices.
   * \note At the next call to `preprocess()`, any vertex that is no longer in the region-of-interest will be assigned to its original position
   * (that is the position of the vertex at the time of construction or after the last call to `overwrite_initial_geometry()`).
   * @param vd the vertex to be erased
   * @return `true` `vd` was a vertex from the region-of-interest.
   */
  bool erase_roi_vertex(vertex_descriptor vd)
  {
    if(!is_roi_vertex(vd)) { return false; }

    erase_control_vertex(vd); // also erase from being control

    typename std::vector<vertex_descriptor>::iterator it = std::find(roi.begin(), roi.end(), vd);
    if(it != roi.end())
    {
      is_roi_map[id(vd)] = false;
      roi.erase(it);

      need_preprocess_both();
      return true;
    }

    CGAL_assertion(false); // inconsistency between is_roi_map, and roi vector!
    return false;
  }

  /**
   * Preprocessing function that need to be called each time the region-of-interest or the set
   * of control vertices are changed before calling `deform()`.
   * If not already done, `deform()` first calls this function.
   * \cgalAdvancedBegin
   * Collects the vertices not in the region-of-interest that are adjacent to a vertex from the
   * region-of-interest (these vertices are internally considered as fixed control vertices).
   * Then assembles and factorizes the Laplacian matrix used in the function `deform()`.
   * \cgalAdvancedEnd
   * \note A modification of the set of control vertices or the region-of-interest invalidates the
   * preprocessing data.
   * @return `true` if successful.
   * A common reason for failure is that the system is rank deficient,
   * which happens for example when all the vertices are in the region-of-interest and no control vertices are set, or
   * if the weighting scheme used features too many zero and breaks the connectivity information.
   */
  bool preprocess()
  {
    region_of_solution();
    assemble_laplacian_and_factorize();
    return last_preprocess_successful; // which is set by assemble_laplacian_and_factorize()
  }
/// @} Preprocessing

/// \name Deformation
/// @{

  /**
   * Sets the target position of a control vertex.
   * @param vd the control vertex the target position is set
   * @param target_position the new target position
   */
  void set_target_position(vertex_descriptor vd, const Point& target_position)
  {
    region_of_solution(); // we require ros ids, so if there is any need to preprocess of region of solution -do it.

    if(!is_control_vertex(vd)) { return; }
    solution[ros_id(vd)] = target_position;
  }

  /**
   * Updates the target position of `vd` by applying     a translation of vector `t`.
   *
   * @tparam Vect is a 3D vector class, with `Vect(double x,double y, double z)` being a constructor from its %Cartesian coordinates
   *         and `double Vect::operator[](int i)` with i=0,1 or 2 returning its %Cartesian coordinates.
   *
   * @param vd a control vertex
   * @param t translation vector
   * \pre `is_control_vertex(vd)`
   */
  template<class Vect>
  void translate(vertex_descriptor vd, const Vect& t)
  {
    region_of_solution(); // we require ros ids, so if there is any need to preprocess of region of solution -do it.

    std::size_t v_id = ros_id(vd);
    solution[v_id] = add_to_point(solution[v_id], t);
  }

  /**
   * Equivalent to calling the overload taking only one control vertex, for each vertex in the range `[begin,end[`.
   *
   * @tparam InputIterator input iterator type with `vertex_descriptor` as value type
   * @tparam Vect is a 3D vector class, with `Vect(double x,double y, double z)` being a constructor from its %Cartesian coordinates
   *         and `double Vect::operator[](int i)` with i=0,1 or 2 returning its %Cartesian coordinates.
   *
   * @param begin first iterator of the range of vertices
   * @param end past-the-end iterator of the range of vertices
   * @param t translation vector
   */
  template<class InputIterator, class Vect>
  void translate(InputIterator begin, InputIterator end, const Vect& t)
  {
    region_of_solution(); // we require ros ids, so if there is any need to preprocess of region of solution -do it.

    for(; begin != end; ++begin) {
      std::size_t v_id = ros_id(*begin);
      solution[v_id] = add_to_point(solution[v_id], t);
    }
  }

  /**
   * Updates the target position of `vd` by applying to its last target position
   * a rotation defined by the quaternion `quat`, the center of the rotation being
   * the origin translated by `to_rotation_center` .
   *
   * @tparam Quaternion is a quaternion class with `Vect operator*(Quaternion, Vect)` returning the product of a quaternion with a vector
   * @tparam Vect is a 3D vector class, with `Vect(double x,double y, double z)` being a constructor from its %Cartesian coordinates
   *         and `double Vect::operator[](int i)` with i=0,1 or 2 returning its %Cartesian coordinates.
   *
   * @param vd a control vertex
   * @param to_rotation_center the vector to translate the origin to the center of the rotation
   * @param quat quaternion of the rotation
   * \pre `is_control_vertex(vd)`
   * \pre `quad` represents a rotation
   */
  template <typename Quaternion, typename Vect>
  void rotate(vertex_descriptor vd, const Vect& to_rotation_center, const Quaternion& quat)
  {
    CGAL_precondition( is_control_vertex(vd) );
    region_of_solution(); // we require ros ids, so if there is any need to preprocess of region of solution -do it.

    std::size_t v_id = ros_id(vd);
    Vect v = quat * sub_to_vect(solution[v_id], to_rotation_center);
    solution[v_id] = Point( to_rotation_center[0]+v[0],
                            to_rotation_center[1]+v[1],
                            to_rotation_center[2]+v[2] );
  }

  /**
   * Equivalent to calling the overload taking only one control vertex, for each vertex in the range `[begin,end[`.
   *
   * @tparam InputIterator input iterator type with `vertex_descriptor` as value type
   * @tparam Quaternion is a quaternion class with `Vect operator*(Quaternion, Vect)` returning the product of a quaternion with a vector
   * @tparam Vect is a 3D vector class, with `Vect(double x,double y, double z)` being a constructor from its %Cartesian coordinates
   *         and `double Vect::operator[](int i)` with i=0,1 or 2 returning its %Cartesian coordinates.
   *
   * @param begin first iterator of the range of vertices
   * @param end past-the-end iterator of the range of vertices
   * @param to_rotation_center the vector to translate the origin to the center of the rotation
   * @param quat quaternion of the rotation
   * \pre `quad` represents a rotation
   */
  template <typename InputIterator, typename Vect, typename Quaternion>
  void rotate(InputIterator begin, InputIterator end, const Vect& to_rotation_center, const Quaternion& quat)
  {
    region_of_solution(); // we require ros ids, so if there is any need to preprocess of region of solution -do it.

    for(; begin != end; ++begin) {
      std::size_t v_id = ros_id(*begin);
      Vect v = quat * sub_to_vect(solution[v_id], to_rotation_center);
      solution[v_id] = Point( to_rotation_center[0]+v[0],
                              to_rotation_center[1]+v[1],
                              to_rotation_center[2]+v[2] );
    }
  }

  /**
    * Returns the target position of a control vertex.
    * \param vd a control vertex
    * \pre `is_control_vertex(vd)`
    */
  const Point& target_position(vertex_descriptor vd)
  {
    region_of_solution();

    CGAL_precondition( is_control_vertex(vd) );
    return solution[ ros_id(vd) ];
  }

  /**
   * Deforms the region-of-interest according to the deformation algorithm, using the target positions of each control vertex set by using `rotate()`, `translate()`, or `set_target_position()`.
   * The points associated to each vertex of the input graph that are inside the region-of-interest are updated.
   * \note Nothing happens if `preprocess()` returns `false`.
   * @see set_iterations(unsigned int iterations), set_tolerance(double tolerance), deform(unsigned int iterations, double tolerance)
   */
  void deform()
  {
    deform(m_iterations, m_tolerance);
  }

  /**
   * Same as `deform()` but the number of iterations and the tolerance are one-time parameters.
   * @param iterations number of iterations for optimization procedure
   * @param tolerance tolerance of convergence (see explanations set_tolerance(double tolerance))
   */
  void deform(unsigned int iterations, double tolerance)
  {
    preprocess();

    if(!last_preprocess_successful) {
      CGAL_warning(false);
      return;
    }
    // Note: no energy based termination occurs at first iteration
    // because comparing energy of original model (before deformation) and deformed model (deformed_1_iteration)
    // simply does not make sense, comparison is meaningful between deformed_(i)_iteration & deformed_(i+1)_iteration

    double energy_this = 0; // initial value is not important, because we skip first iteration
    double energy_last;

    // iterations
    for ( unsigned int ite = 0; ite < iterations; ++ite)
    {
      // main steps of optimization
      update_solution();
#ifndef CGAL_DEFORM_MESH_JUST_EXPERIMENTAL_SCALE
      optimal_rotations();
#endif
#ifdef CGAL_DEFORM_MESH_USE_EXPERIMENTAL_SCALE
      optimal_scales();
#endif
      // energy based termination
      if(tolerance > 0.0 && (ite + 1) < iterations) // if tolerance <= 0 then don't compute energy
      {                                             // also no need compute energy if this iteration is the last iteration
        energy_last = energy_this;
        energy_this = energy();
        CGAL_warning(energy_this >= 0);

        if(ite != 0) // skip first iteration
        {
          double energy_dif = std::abs((energy_last - energy_this) / energy_this);
          if ( energy_dif < tolerance ) { break; }
        }
      }
    }
    // copy solution to the target surface mesh
    assign_solution();
  }

  /**
   * Resets the points associated to the vertices of the region-of-interest at their
   * initial positions at time of the functor construction or after
   * the last call to `overwrite_initial_geometry()`.
   * \note if the region-of-interest or the set of control vertices have been
   * modified since the last call to `preprocess()`, it will be called prior
   * to the reset.
   */
  void reset()
  {
    if(roi.empty()) { return; } // no ROI to reset

    region_of_solution(); // since we are using original vector

    //restore the current positions to be the original positions
    BOOST_FOREACH(vertex_descriptor vd, roi_vertices())
    {
      put(vertex_point_map, vd, original[ros_id(vd)]);
      solution[ros_id(vd)]=original[ros_id(vd)];
    }

    // also set rotation matrix to identity
    std::fill(rot_mtr.begin(), rot_mtr.end(),
              Closest_rotation_traits().identity_matrix());
  }

  /**
   * Sets the initial positions of the vertices from the region-of-interest to the current positions. Calling this function has the same effect as creating
   * a new deformation object with the current deformed halfedge-graph, keeping the region-of-interest and the set of control vertices.
   * \note if the region-of-interest or the set of control vertices have been modified since the last call to `preprocess()`,
   * it will be called prior to the overwrite.
   *
   * \cgalAdvancedBegin
   * This function might have a non-negligible effect on the result.
   * The Laplacian matrix of the free vertices and the optimal rotations
   * are computed using the original positions of the points
   * associated to the vertices. Thus, if a deformed version of the surface mesh
   * is used as reference, the surface mesh properties the algorithm
   * tries to preserve are those of an altered version (which are already
   * degraded).
   * \cgalAdvancedEnd
   */
  void overwrite_initial_geometry()
  {
    if(roi.empty()) { return; } // no ROI to overwrite

    region_of_solution(); // the roi should be preprocessed since we are using original_position vec

    BOOST_FOREACH(vertex_descriptor vd, roi_vertices())
    {
      original[ros_id(vd)] = get(vertex_point_map, vd);
    }

    // now I need to compute weights for halfedges incident to roi vertices
    std::vector<bool> is_weight_computed(2*num_edges(m_halfedge_graph), false);
    BOOST_FOREACH(vertex_descriptor vd, roi_vertices())
    {
      in_edge_iterator e, e_end;
      for (cpp11::tie(e,e_end) = in_edges(vd, m_halfedge_graph); e != e_end; e++)
      {
        halfedge_descriptor he = halfedge(*e, m_halfedge_graph);
        std::size_t id_e = id(he);
        if(is_weight_computed[id_e]) { continue; }

        hedge_weight[id_e] = weight_calculator(he, m_halfedge_graph, vertex_point_map);
        is_weight_computed[id_e] = true;

        halfedge_descriptor e_opp = opposite(he, m_halfedge_graph);
        std::size_t id_e_opp = id(e_opp);

        hedge_weight[id_e_opp] = weight_calculator(e_opp, m_halfedge_graph, vertex_point_map);
        is_weight_computed[id_e_opp] = true;
      }
    }

    // also set rotation matrix to identity
    std::fill(rot_mtr.begin(), rot_mtr.end(), Closest_rotation_traits().identity_matrix());

    need_preprocess_both(); // now we need reprocess
  }

/// @} Deformation

/// \name Utilities
/// @{

  /**
   * Gets the default number of iterations (5) or the value passed to the function `set_iterations()`
   */
  unsigned int iterations()
  { return m_iterations; }

  /**
   * Gets the default tolerance parameter (1e-4) or the value passed to the function `set_tolerance()`
   */
  double tolerance()
  { return m_tolerance; }

  /**
   * Sets the maximum number of iterations ran by `deform()`
   */
  void set_iterations(unsigned int iterations)
  { this->m_iterations = iterations; }

   /// @brief Sets the tolerance of the convergence used in `deform()`.
   /// If `tolerance==0`, no energy based termination criteria is used (preventing to do the energy computation at each iteration step)
   ///
   /// `tolerance >` \f$|\mathrm{energy}(m_i) - \mathrm{energy}(m_{i-1})| / \mathrm{energy}(m_i)\f$ will be used as a termination criterium.
  void set_tolerance(double tolerance)
  { this->m_tolerance = tolerance; }

  /**
   * Returns the range of vertices in the region-of-interest.
   */
  Roi_vertex_range roi_vertices() const
  {
    return roi;
  }

  /**
   * Tests whether a vertex is inside the region-of-interest.
   * @param vd the query vertex
   * @return `true` if `vd` has been inserted to (and not erased from) the region-of-interest.
   */
  bool is_roi_vertex(vertex_descriptor vd) const
  { return is_roi_map[id(vd)]; }

  /**
   * Tests whether a vertex is a control vertex.
   * @param vd the query vertex
   * @return `true` if `vd` has been inserted to (and not erased from) the set of control vertices.
   */
  bool is_control_vertex(vertex_descriptor vd) const
  { return is_ctrl_map[id(vd)]; }

  /**
   * Provides access to the halfedge graph being deformed
   */
  const Halfedge_graph& halfedge_graph() const
  { return m_halfedge_graph; }

/// @} Utilities


private:

  /// Assigns id to one ring neighbor of vd, and also push them into push_vector
  void assign_ros_id_to_one_ring(vertex_descriptor vd,
                             std::size_t& next_id,
                             std::vector<vertex_descriptor>& push_vector)
  {
    in_edge_iterator e, e_end;
    for (cpp11::tie(e,e_end) = in_edges(vd, m_halfedge_graph); e != e_end; e++)
    {
      vertex_descriptor vt = source(*e, m_halfedge_graph);
      if(ros_id(vt) == (std::numeric_limits<std::size_t>::max)())  // neighboring vertex which is outside of roi and not visited previously (i.e. need an id)
      {
        ros_id(vt) = next_id++;
        push_vector.push_back(vt);
      }
    }
  }

  /// Find region of solution, including roi and hard constraints, which is the 1-ring vertices out roi
  /// Contains four parts:
  ///  - if there is any vertex which is no longer roi, set its position back to original position
  ///  - assign a ros id to vertices inside ros + ros-boundary
  ///  - reinitialize rotation matrices, if a vertex is previously ros, use its previous matrix, otherwise set zero
  ///  - reinitialize original, and solution,
  ///      + if a vertex is previously roi, then use its original position in old_origional, else use point().
  ///        In both case we are using "original position" of the vertex.
  ///      + same for solution (it is required to prevent jumping effects)
  void region_of_solution()
  {
    if(!need_preprocess_region_of_solution) { return; }
    need_preprocess_region_of_solution = false;

    std::vector<std::size_t>  old_ros_id_map = ros_id_map;
    std::vector<CR_matrix>    old_rot_mtr    = rot_mtr;
    std::vector<Point>        old_solution   = solution;
    std::vector<Point>        old_original   = original;

    // any vertices which are no longer ROI, should be assigned to their original position, so that:
    // IF a vertex is ROI (actually if its ros + boundary) previously (when previous region_of_solution() is called)
    // we have its original position in old_original
    // ELSE
    // we have its original position in vertex->point()
    // (current ros is actually old ros - we did not change it yet)
    for(typename std::vector<vertex_descriptor>::iterator it = ros.begin(); it != ros.end(); ++it)
    {
      if(!is_roi_vertex(*it)) {
        put(vertex_point_map, *it, old_original[ old_ros_id_map[id(*it)] ]);
      }
    }

    ////////////////////////////////////////////////////////////////
    // assign id to vertices inside: roi, boundary of roi (roi + boundary of roi = ros),
    //                               and boundary of ros
    // keep in mind that id order does not have to be compatible with ros order
    ros.clear(); // clear ros
    ros.insert(ros.end(), roi.begin(), roi.end());

    ros_id_map.assign(num_vertices(m_halfedge_graph), (std::numeric_limits<std::size_t>::max)()); // use max as not assigned mark

    for(std::size_t i = 0; i < roi.size(); i++)  // assign id to all roi vertices
    { ros_id(roi[i]) = i; }

    // now assign an id to vertices on boundary of roi
    std::size_t next_ros_index = roi.size();
    for(std::size_t i = 0; i < roi.size(); i++)
    { assign_ros_id_to_one_ring(roi[i], next_ros_index, ros); }

    std::vector<vertex_descriptor> outside_ros;
    // boundary of ros also must have ids because in CR calculation,
    // one-ring neighbor of ROS vertices are reached.
    for(std::size_t i = roi.size(); i < ros.size(); i++)
    { assign_ros_id_to_one_ring(ros[i], next_ros_index, outside_ros); }
    ////////////////////////////////////////////////////////////////

    // initialize the rotation matrices (size: ros)
    rot_mtr.resize(ros.size());
    for(std::size_t i = 0; i < rot_mtr.size(); i++)
    {
      std::size_t v_ros_id = ros_id(ros[i]);
      std::size_t v_id = id(ros[i]);

      // any vertex which is previously ROS has a rotation matrix
      // use that matrix to prevent jumping effects
      if(old_ros_id_map[v_id] != (std::numeric_limits<std::size_t>::max)()
          && old_ros_id_map[v_id] < old_rot_mtr.size()) {
          // && boundary of ros vertices also have ids so check whether it is ros
        rot_mtr[v_ros_id] = old_rot_mtr[ old_ros_id_map[v_id] ];
      }
      else {
        rot_mtr[v_ros_id] = Closest_rotation_traits().identity_matrix();
      }
    }

    // initialize solution and original (size: ros + boundary_of_ros)

    // for simplifying coding effort, I also put boundary of ros into solution and original
    // because boundary of ros vertices are reached in optimal_rotations() and energy()
    solution.resize(ros.size() + outside_ros.size());
    original.resize(ros.size() + outside_ros.size());

    for(std::size_t i = 0; i < ros.size(); i++)
    {
      std::size_t v_ros_id = ros_id(ros[i]);
      std::size_t v_id = id(ros[i]);

      if(is_roi_vertex(ros[i]) && old_ros_id_map[v_id] != (std::numeric_limits<std::size_t>::max)()) {
        // if it is currently roi and previously ros + boundary
        // (actually I just need to assign old's to new's if a vertex is currently and previously ROI
        // but no harm on assigning if its also previously ros + boundary because
        // those(old_original & old_solution) will be equal to original position)
        original[v_ros_id] = old_original[old_ros_id_map[v_id]];
        solution[v_ros_id] = old_solution[old_ros_id_map[v_id]];
      }
      else {
        solution[v_ros_id] = get(vertex_point_map, ros[i]);
        original[v_ros_id] = get(vertex_point_map, ros[i]);
      }
    }

    for(std::size_t i = 0; i < outside_ros.size(); ++i)
    {
      std::size_t v_ros_id = ros_id(outside_ros[i]);
      original[v_ros_id] = get(vertex_point_map, outside_ros[i]);
      solution[v_ros_id] = get(vertex_point_map, outside_ros[i]);
    }

#ifdef CGAL_DEFORM_MESH_USE_EXPERIMENTAL_SCALE
    scales.resize(ros.size());
    std::fill(scales.begin(), scales.end(), 1.0);
#endif
  }

  /// Assemble Laplacian matrix A of linear system A*X=B
  void assemble_laplacian_and_factorize()
  {
    if(TAG == SPOKES_AND_RIMS)
    {
      assemble_laplacian_and_factorize_spokes_and_rims();
    }
    else
    {
      assemble_laplacian_and_factorize_arap();
    }
  }
  /// Construct matrix that corresponds to left-hand side of eq:lap_ber in user manual
  /// Also constraints are integrated as eq:lap_energy_system in user manual
  void assemble_laplacian_and_factorize_arap()
  {
    if(!need_preprocess_factorization) { return; }
    need_preprocess_factorization = false;

    typename Sparse_linear_solver::Matrix A(ros.size());

    /// assign cotangent Laplacian to ros vertices
    for(std::size_t k = 0; k < ros.size(); k++)
    {
      vertex_descriptor vi = ros[k];
      std::size_t vi_id = ros_id(vi);
      if ( is_roi_vertex(vi) && !is_control_vertex(vi) )          // vertices of ( roi - ctrl )
      {
        double diagonal = 0;
        in_edge_iterator e, e_end;
        for (cpp11::tie(e,e_end) = in_edges(vi, m_halfedge_graph); e != e_end; e++)
        {
          halfedge_descriptor he = halfedge(*e, m_halfedge_graph);
          vertex_descriptor vj = source(he, m_halfedge_graph);
          double wij = hedge_weight[id(he)];  // edge(pi - pj)
          double wji = hedge_weight[id(opposite(he, m_halfedge_graph))]; // edge(pi - pj)
          double total_weight = wij + wji;

          A.set_coef(vi_id, ros_id(vj), -total_weight, true); // off-diagonal coefficient
          diagonal += total_weight;
        }
        // diagonal coefficient
        A.set_coef(vi_id, vi_id, diagonal, true);
      }
      else
      {
        A.set_coef(vi_id, vi_id, 1.0, true);
      }
    }

    // now factorize
    double D;
    last_preprocess_successful = m_solver.factor(A, D);
    CGAL_warning(last_preprocess_successful);
  }
  /// Construct matrix that corresponds to left-hand side of eq:lap_ber_rims in user manual
  /// Also constraints are integrated as eq:lap_energy_system in user manual
  void assemble_laplacian_and_factorize_spokes_and_rims()
  {
    if(!need_preprocess_factorization) { return; }
    need_preprocess_factorization = false;

    typename Sparse_linear_solver::Matrix A(ros.size());

    /// assign cotangent Laplacian to ros vertices
    for(std::size_t k = 0; k < ros.size(); k++)
    {
      vertex_descriptor vi = ros[k];
      std::size_t vi_id = ros_id(vi);
      if ( is_roi_vertex(vi) && !is_control_vertex(vi) ) // vertices of ( roi - ctrl ): free vertices
      {
        double diagonal = 0;
        out_edge_iterator e, e_end;
        for (cpp11::tie(e,e_end) = out_edges(vi, m_halfedge_graph); e != e_end; e++)
        {
          halfedge_descriptor he = halfedge(*e, m_halfedge_graph);
          double total_weight = 0;
          // an edge contribute to energy only if it is part of an incident triangle
          // (i.e it should not be a border edge)
          if(!is_border(he, m_halfedge_graph))
          {
            double wji = hedge_weight[id(he)]; // edge(pj - pi)
            total_weight += wji;
          }

          halfedge_descriptor opp = opposite(he, m_halfedge_graph);
          if(!is_border(opp, m_halfedge_graph))
          {
            double wij = hedge_weight[id(opp)]; // edge(pi - pj)
            total_weight += wij;
          }

          // place coefficient to matrix
          vertex_descriptor vj = target(he, m_halfedge_graph);
          A.set_coef(vi_id, ros_id(vj), -total_weight, true); // off-diagonal coefficient
          diagonal += total_weight;
        }
        // diagonal coefficient
        A.set_coef(vi_id, vi_id, diagonal, true);
      }
      else // constrained vertex
      {
        A.set_coef(vi_id, vi_id, 1.0, true);
      }
    }

    // now factorize
    double D;
    last_preprocess_successful = m_solver.factor(A, D);
    CGAL_warning(last_preprocess_successful);
  }

  /// Local step of iterations, computing optimal rotation matrices
  void optimal_rotations()
  {
    if(TAG == SPOKES_AND_RIMS)
    {
      optimal_rotations_spokes_and_rims();
    }
    else
    {
      optimal_rotations_arap();
    }
  }
  void optimal_rotations_arap()
  {
    Closest_rotation_traits cr_traits;
    CR_matrix cov = cr_traits.zero_matrix();

    // only accumulate ros vertices
    for ( std::size_t k = 0; k < ros.size(); k++ )
    {
      vertex_descriptor vi = ros[k];
      std::size_t vi_id = ros_id(vi);
      // compute covariance matrix (user manual eq:cov_matrix)
      cov = cr_traits.zero_matrix();

      in_edge_iterator e, e_end;

#ifdef CGAL_DEFORM_MESH_USE_EXPERIMENTAL_SR_ARAP
      cpp11::tie(e,e_end) = in_edges(vi, m_halfedge_graph);
      double ne_i=std::distance(e,e_end);
#endif
      for (cpp11::tie(e,e_end) = in_edges(vi, m_halfedge_graph); e != e_end; e++)
      {
        halfedge_descriptor he=halfedge(*e, m_halfedge_graph);
        vertex_descriptor vj = source(he, m_halfedge_graph);
        std::size_t vj_id = ros_id(vj);

        const CR_vector& pij = sub_to_CR_vector(original[vi_id], original[vj_id]);
        const CR_vector& qij = sub_to_CR_vector(solution[vi_id], solution[vj_id]);
        double wij = hedge_weight[id(he)];

        cr_traits.add_scalar_t_vector_t_vector_transpose(cov, wij, pij, qij); // cov += wij * (pij * qij)
#ifdef CGAL_DEFORM_MESH_USE_EXPERIMENTAL_SR_ARAP
        // add neighbor rotation
        cov += m_sr_arap_alpha * rot_mtr[vj_id].transpose() / ne_i;
#endif

      }

      cr_traits.compute_close_rotation(cov, rot_mtr[vi_id]);
    }
  }
  void optimal_rotations_spokes_and_rims()
  {
    Closest_rotation_traits cr_traits;
    CR_matrix cov =cr_traits.zero_matrix();

    // only accumulate ros vertices
    for ( std::size_t k = 0; k < ros.size(); k++ )
    {
      vertex_descriptor vi = ros[k];
      std::size_t vi_id = ros_id(vi);
      // compute covariance matrix
      cov = cr_traits.zero_matrix();

      //iterate through all triangles
      out_edge_iterator e, e_end;
      for (cpp11::tie(e,e_end) = out_edges(vi, m_halfedge_graph); e != e_end; e++)
      {
        halfedge_descriptor he = halfedge(*e, m_halfedge_graph);
        if(is_border(he, m_halfedge_graph)) { continue; } // no facet
        // iterate edges around facet
        halfedge_descriptor hedge_around_facet = he;
        do
        {
          vertex_descriptor v1 = target(hedge_around_facet, m_halfedge_graph);
          vertex_descriptor v2 = source(hedge_around_facet, m_halfedge_graph);

          std::size_t v1_id = ros_id(v1); std::size_t v2_id = ros_id(v2);

          const CR_vector& p12 = sub_to_CR_vector(original[v1_id], original[v2_id]);
          const CR_vector& q12 = sub_to_CR_vector(solution[v1_id], solution[v2_id]);
          double w12 = hedge_weight[id(hedge_around_facet)];

          cr_traits.add_scalar_t_vector_t_vector_transpose(cov, w12, p12, q12); // cov += w12 * (p12 * q12);

        } while( (hedge_around_facet = next(hedge_around_facet, m_halfedge_graph)) != he);
      }

      cr_traits.compute_close_rotation(cov, rot_mtr[vi_id]);
    }
  }

#ifdef CGAL_DEFORM_MESH_USE_EXPERIMENTAL_SCALE
  void optimal_scales()
  {
    for ( std::size_t k = 0; k < ros.size(); k++ )
    {
      vertex_descriptor vi = ros[k];
      std::size_t vi_id = ros_id(vi);
      // compute covariance matrix (user manual eq:cov_matrix)
      double eT_eR = 0, eRT_eR = 0;

      in_edge_iterator e, e_end;
      for (cpp11::tie(e,e_end) = in_edges(vi, m_halfedge_graph); e != e_end; e++)
      {
        halfedge_descriptor he = *e;
        vertex_descriptor vj = source(he, m_halfedge_graph);
        std::size_t vj_id = ros_id(vj);

        const CR_vector& pij = sub_to_CR_vector(original[vi_id], original[vj_id]);
        const CR_vector& qij = sub_to_CR_vector(solution[vi_id], solution[vj_id]);

        double wij = hedge_weight[id(he)];

        const CR_vector& pRij = rot_mtr[vi_id] * pij;
        eRT_eR += pRij[0]*pRij[0] + pRij[1]*pRij[1] + pRij[2]*pRij[2];
        eT_eR  += qij[0]*pRij[0]  + qij[1]*pRij[1]  + qij[2]*pRij[2];
      }

      scales[vi_id] = eT_eR / eRT_eR;
    }
  }
#endif

  /// Global step of iterations, updating solution
  void update_solution()
  {
    if(TAG == SPOKES_AND_RIMS)
    {
      update_solution_spokes_and_rims();
    }
    else
    {
      update_solution_arap();
    }
  }
  /// calculate right-hand side of eq:lap_ber in user manual and solve the system
  void update_solution_arap()
  {
    typename Sparse_linear_solver::Vector X(ros.size()), Bx(ros.size());
    typename Sparse_linear_solver::Vector Y(ros.size()), By(ros.size());
    typename Sparse_linear_solver::Vector Z(ros.size()), Bz(ros.size());

    Closest_rotation_traits cr_traits;

    // assemble right columns of linear system
    for ( std::size_t k = 0; k < ros.size(); k++ )
    {
      vertex_descriptor vi = ros[k];
      std::size_t vi_id = ros_id(vi);

      if ( is_roi_vertex(vi) && !is_control_vertex(vi) )
      {// free vertices
        // sum of right-hand side of eq:lap_ber in user manual
        CR_vector xyz = cr_traits.vector(0, 0, 0);

        in_edge_iterator e, e_end;
        for (cpp11::tie(e,e_end) = in_edges(vi, m_halfedge_graph); e != e_end; e++)
        {
          halfedge_descriptor he = halfedge(*e, m_halfedge_graph);
          vertex_descriptor vj = source(he, m_halfedge_graph);
          std::size_t vj_id = ros_id(vj);

          const CR_vector& pij = sub_to_CR_vector(original[vi_id], original[vj_id]);

          double wij = hedge_weight[id(he)];
          double wji = hedge_weight[id(opposite(he, m_halfedge_graph))];
#ifndef CGAL_DEFORM_MESH_USE_EXPERIMENTAL_SCALE
          cr_traits.add__scalar_t_matrix_p_scalar_t_matrix__t_vector(xyz, wij, rot_mtr[vi_id], wji, rot_mtr[vj_id], pij);
#else
        cr_traits.add__scalar_t_matrix_p_scalar_t_matrix__t_vector(xyz, wij * scales[vi_id], rot_mtr[vi_id],
          wji * scales[vj_id], rot_mtr[vj_id], pij);
#endif
          // corresponds xyz += (wij*rot_mtr[vi_id] + wji*rot_mtr[vj_id]) * pij
        }
        Bx[vi_id] = cr_traits.vector_coordinate(xyz, 0);
        By[vi_id] = cr_traits.vector_coordinate(xyz, 1);
        Bz[vi_id] = cr_traits.vector_coordinate(xyz, 2);
      }
      else
      {// constrained vertex
        Bx[vi_id] = solution[vi_id][0]; By[vi_id] = solution[vi_id][1]; Bz[vi_id] = solution[vi_id][2];
      }
    }

    // solve "A*X = B".
    bool is_all_solved = m_solver.linear_solver(Bx, X) && m_solver.linear_solver(By, Y) && m_solver.linear_solver(Bz, Z);
    if(!is_all_solved) {
      CGAL_warning(false);
      return;
    }
    // copy to solution
    for (std::size_t i = 0; i < ros.size(); i++)
    {
      std::size_t v_id = ros_id(ros[i]);
      Point p(X[v_id], Y[v_id], Z[v_id]);
      solution[v_id] = p;
    }
  }
  /// calculate right-hand side of eq:lap_ber_rims in user manual and solve the system
  void update_solution_spokes_and_rims()
  {
    typename Sparse_linear_solver::Vector X(ros.size()), Bx(ros.size());
    typename Sparse_linear_solver::Vector Y(ros.size()), By(ros.size());
    typename Sparse_linear_solver::Vector Z(ros.size()), Bz(ros.size());

    Closest_rotation_traits cr_traits;

    // assemble right columns of linear system
    for ( std::size_t k = 0; k < ros.size(); k++ )
    {
      vertex_descriptor vi = ros[k];
      std::size_t vi_id = ros_id(vi);

      if ( is_roi_vertex(vi) && !is_control_vertex(vi) )
      {// free vertices
        // sum of right-hand side of eq:lap_ber_rims in user manual
        CR_vector xyz = cr_traits.vector(0, 0, 0);

        out_edge_iterator e, e_end;
        for (cpp11::tie(e,e_end) = out_edges(vi, m_halfedge_graph); e != e_end; e++)
        {
          halfedge_descriptor he = halfedge(*e, m_halfedge_graph);
          vertex_descriptor vj = target(he, m_halfedge_graph);
          std::size_t vj_id = ros_id(vj);

          const CR_vector& pij = sub_to_CR_vector(original[vi_id], original[vj_id]);

          if(!is_border(he, m_halfedge_graph))
          {
            vertex_descriptor vn = target(next(he, m_halfedge_graph), m_halfedge_graph); // opp vertex of e_ij
            double wji = hedge_weight[id(he)] / 3.0;  // edge(pj - pi)
            cr_traits.add_scalar_t_matrix_sum_t_vector(xyz, wji, rot_mtr[vi_id], rot_mtr[vj_id], rot_mtr[ros_id(vn)], pij);
            // corresponds  xyz += wji*(rot_mtr[vi_id] + rot_mtr[vj_id] + rot_mtr[ros_id(vn)])*pij;
          }

          halfedge_descriptor opp = opposite(he, m_halfedge_graph);
          if(!is_border(opp, m_halfedge_graph))
          {
            vertex_descriptor vm = target(next(opp, m_halfedge_graph), m_halfedge_graph); // other opp vertex of e_ij
            double wij = hedge_weight[id(opp)] / 3.0;  // edge(pi - pj)
            cr_traits.add_scalar_t_matrix_sum_t_vector(xyz, wij, rot_mtr[vi_id], rot_mtr[vj_id], rot_mtr[ros_id(vm)], pij);
            // corresponds xyz += wij * ( rot_mtr[vi_id] + rot_mtr[vj_id] + rot_mtr[ros_id(vm)] ) * pij
          }
        }
        Bx[vi_id] = cr_traits.vector_coordinate(xyz, 0);
        By[vi_id] = cr_traits.vector_coordinate(xyz, 1);
        Bz[vi_id] = cr_traits.vector_coordinate(xyz, 2);
      }
      else
      {// constrained vertices
        Bx[vi_id] = solution[vi_id][0]; By[vi_id] = solution[vi_id][1]; Bz[vi_id] = solution[vi_id][2];
      }
    }
    // solve "A*X = B".
    bool is_all_solved = m_solver.linear_solver(Bx, X) && m_solver.linear_solver(By, Y) && m_solver.linear_solver(Bz, Z);
    if(!is_all_solved) {
      CGAL_warning(false);
      return;
    }

    // copy to solution
    for (std::size_t i = 0; i < ros.size(); i++)
    {
      std::size_t v_id = ros_id(ros[i]);
      Point p(X[v_id], Y[v_id], Z[v_id]);
      solution[v_id] = p;
    }
  }

  /// Assign solution to target surface mesh
  void assign_solution()
  {
    for(std::size_t i = 0; i < ros.size(); ++i){
      std::size_t v_id = ros_id(ros[i]);
      if(is_roi_vertex(ros[i]))
      {
        put(vertex_point_map, ros[i], solution[v_id]);
      }
    }
  }

  /// Compute modeling energy
  double energy() const
  {
    if(TAG == SPOKES_AND_RIMS)
    {
      return energy_spokes_and_rims();
    }
    else
    {
      return energy_arap();
      return 0;
    }
  }
  double energy_arap() const
  {
    Closest_rotation_traits cr_traits;

    double sum_of_energy = 0;
    // only accumulate ros vertices
    for( std::size_t k = 0; k < ros.size(); k++ )
    {
      vertex_descriptor vi = ros[k];
      std::size_t vi_id = ros_id(vi);

      in_edge_iterator e, e_end;
      for (cpp11::tie(e,e_end) = in_edges(vi, m_halfedge_graph); e != e_end; e++)
      {
        halfedge_descriptor he = halfedge(*e, m_halfedge_graph);
        vertex_descriptor vj = source(he, m_halfedge_graph);
        std::size_t vj_id = ros_id(vj);

        const CR_vector& pij = sub_to_CR_vector(original[vi_id], original[vj_id]);
        const CR_vector& qij = sub_to_CR_vector(solution[vi_id], solution[vj_id]);

        double wij = hedge_weight[id(he)];

        sum_of_energy += wij * cr_traits.squared_norm_vector_scalar_vector_subs(qij, rot_mtr[vi_id], pij);
        // sum_of_energy += wij * ( qij - rot_mtr[vi_id]*pij )^2
      }
    }
    return sum_of_energy;
  }
  double energy_spokes_and_rims() const
  {
    Closest_rotation_traits cr_traits;

    double sum_of_energy = 0;
    // only accumulate ros vertices
    for( std::size_t k = 0; k < ros.size(); k++ )
    {
      vertex_descriptor vi = ros[k];
      std::size_t vi_id = ros_id(vi);
      //iterate through all triangles
      out_edge_iterator e, e_end;
      for (cpp11::tie(e,e_end) = out_edges(vi, m_halfedge_graph); e != e_end; e++)
      {
        halfedge_descriptor he = halfedge(*e, m_halfedge_graph);
        if(is_border(he, m_halfedge_graph)) { continue; } // no facet
        // iterate edges around facet
        halfedge_descriptor hedge_around_facet = he;
        do
        {
          vertex_descriptor v1 = target(hedge_around_facet, m_halfedge_graph);
          vertex_descriptor v2 = source(hedge_around_facet, m_halfedge_graph);
          std::size_t v1_id = ros_id(v1); std::size_t v2_id = ros_id(v2);

          const CR_vector& p12 = sub_to_CR_vector(original[v1_id], original[v2_id]);
          const CR_vector& q12 = sub_to_CR_vector(solution[v1_id], solution[v2_id]);
          double w12 = hedge_weight[id(hedge_around_facet)];

          sum_of_energy += w12 * cr_traits.squared_norm_vector_scalar_vector_subs(q12, rot_mtr[vi_id], p12);
          // sum_of_energy += w12 * ( q12 - rot_mtr[vi_id]*p12 )^2

        } while( (hedge_around_facet = next(hedge_around_facet, m_halfedge_graph)) != he);
      }
    }
    return sum_of_energy;
  }

  void need_preprocess_both()
  {
    need_preprocess_factorization = true;
    need_preprocess_region_of_solution = true;
  }

  /// p1 - p2, return CR_vector
  CR_vector sub_to_CR_vector(const Point& p1, const Point& p2) const
  {
    return Closest_rotation_traits().vector(p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]);
  }

  template<class Vect>
  Point add_to_point(const Point& p, const Vect& v) {
    return Point(p[0] + v[0], p[1] + v[1], p[2] + v[2]);
  }

  template<class Vect>
  Vect sub_to_vect(const Point& p, const Vect& v) {
    return Vect(p[0] - v[0], p[1] - v[1], p[2] - v[2]);
  }

  /// shorthand of get(vertex_index_map, v)
  std::size_t id(vertex_descriptor vd) const
  { return get(vertex_index_map, vd); }

  std::size_t& ros_id(vertex_descriptor vd)
  { return ros_id_map[id(vd)]; }
  std::size_t  ros_id(vertex_descriptor vd) const
  { return ros_id_map[id(vd)]; }

  /// shorthand of get(hedge_index_map, e)
  std::size_t id(halfedge_descriptor e) const
  {
    return get(hedge_index_map, e);
  }
};
} //namespace CGAL
#endif  // CGAL_DEFORM_MESH_H