// Copyright (c) 2011 GeometryFactory
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you may redistribute it under
// the terms of the Q Public License version 1.0.
// See the file LICENSE.QPL distributed with CGAL.
//
// 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


#ifndef CGAL_DEFORM_MESH_H
#define CGAL_DEFORM_MESH_H

#include <CGAL/trace.h>
#include <CGAL/Timer.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/boost/graph/properties_Polyhedron_3.h>
#include <CGAL/boost/graph/halfedge_graph_traits_Polyhedron_3.h>
#include <CGAL/FPU_extension.h>

#include <Eigen/Eigen>
#include <Eigen/SVD>


namespace CGAL {


template <class Polyhedron, class SparseLinearAlgebraTraits_d, 
          class VertexIndexMap, class EdgeIndexMap>
class Deform_mesh
{
// Public types
public:

  // Geometric types              
  typedef typename Polyhedron::Traits         Kernel;
  typedef typename Kernel::Vector_3           Vector;
  typedef typename Kernel::Point_3            Point;

  // Repeat Polyhedron types
  typedef typename boost::graph_traits<Polyhedron>::vertex_descriptor		vertex_descriptor;
  typedef typename boost::graph_traits<Polyhedron>::vertex_iterator		  vertex_iterator;
  typedef typename boost::graph_traits<Polyhedron>::edge_descriptor		  edge_descriptor;
  typedef typename boost::graph_traits<Polyhedron>::edge_iterator		    edge_iterator;
  typedef typename boost::graph_traits<Polyhedron>::in_edge_iterator		in_edge_iterator;

  // Data members.
public:

  Polyhedron& polyhedron;                     // source mesh, can not be modified
  std::vector<vertex_descriptor> roi;
  std::vector<vertex_descriptor> hdl;         // user specified handles, storing the target positions
  std::vector<vertex_descriptor> ros;         // region of solution, including roi and hard constraints outside roi
  VertexIndexMap vertex_index_map;              // storing indices of all vertices 
  EdgeIndexMap edge_index_map;                  // storing indices of all edges
  std::vector<std::size_t> ros_id;                    // index of ros vertices
  std::vector<std::size_t> is_roi;                    // flag indicating vertex inside roi or not 
  std::vector<std::size_t> is_hdl;               
 
  unsigned int iterations;                                     // number of maximal iterations
  double tolerance;                                   // tolerance of convergence 

  std::vector<Eigen::Matrix3d>  rot_mtr;                       // rotation matrices of ros vertices
  std::vector<double> edge_weight;                    // weight of edges
  SparseLinearAlgebraTraits_d m_solver;               // linear sparse solver
  std::vector<Point>  solution;                       // storing position of all vertices during iterations
  std::vector<Point> original;
  // Public methods
public:

  // The constructor gets the polyhedron that we will model
  Deform_mesh(Polyhedron& P, const VertexIndexMap& vertex_index_map_, const EdgeIndexMap& edge_index_map_)
    :polyhedron(P), vertex_index_map(vertex_index_map_), edge_index_map(edge_index_map_)
  {

    // initialize index maps
    vertex_iterator vb, ve;
    int idx = 0;
    for(boost::tie(vb,ve) = boost::vertices(polyhedron); vb != ve; ++vb )
    {
      put(vertex_index_map, *vb, idx++);
    }

    edge_iterator eb, ee;
    idx = 0;
    for(boost::tie(eb,ee) = boost::edges(polyhedron); eb != ee; ++eb )
    {
      boost::put(edge_index_map, *eb, idx++);
    }

    // initialize solution
    for(boost::tie(vb,ve) = boost::vertices(polyhedron); vb != ve; ++vb )
    {
      solution.push_back( (*vb)->point() );
      original.push_back( (*vb)->point() );
    }

    // initialize flag vectors of roi, handle, ros 
    ros_id.resize(boost::num_vertices(polyhedron), 0);
    is_roi.resize(boost::num_vertices(polyhedron), 0);
    is_hdl.resize(boost::num_vertices(polyhedron), 0);


    // AF: What is a good number of iterations
    // YX: I will add another new threshold according to energy value.
    iterations = 5;
    tolerance = 1e-4;       

  }


  void clear_roi()
  {
    roi.clear();
    is_roi.clear();
    is_roi.resize( boost::num_vertices(polyhedron), 0 );
  }

  void insert_roi(vertex_descriptor vd)   
  {
    std::size_t idx = get(vertex_index_map, vd);
    if (!is_roi[idx])
    {
      roi.push_back(vd);
      is_roi[idx] = 1;
    }
  }

  // Re-assign handles from begin to end
  void assign_handles(vertex_iterator begin, vertex_iterator end)
  {
    hdl.clear();
    hdl.insert(hdl.end(), begin, end);

    is_hdl.clear();  
    is_hdl.resize( boost::num_vertices(polyhedron), 0 );    // mark all the vertices as handle or not
    for (int i = 0; i < hdl.size(); i++)
    {
      is_hdl[ get(vertex_index_map, hdl[i]) ] = 1;
    }
  }

  void clear_handles()
  {
    hdl.clear();
    is_hdl.clear(); 
    is_hdl.resize( boost::num_vertices(polyhedron), 0 );
  }

  void insert_handle(vertex_descriptor vd)  
  {
    std::size_t idx = get(vertex_index_map, vd);
    if (!is_hdl[idx])
    {
      hdl.push_back(vd);
      is_hdl[idx] = 1;
    }
  }

  // compute cotangent weights of all edges 
  void compute_edge_weight()
  {
    // refer the information that whether the weight of an edge has already computed
    std::vector<int> edge_weight_computed(boost::num_edges(polyhedron));  

    edge_weight.clear();
    edge_weight.resize( boost::num_edges(polyhedron), 0 ); 

    edge_iterator eb, ee;
    for(boost::tie(eb,ee) = boost::edges(polyhedron); eb != ee; ++eb )
    {
      std::size_t e_idx = boost::get(edge_index_map, *eb);
      if ( !edge_weight_computed[e_idx] )
      {
        double weight = cot_weight(*eb);
        // replace cotangent weight by mean-value coordinate
        if ( weight < 0 )
        {
          weight = mean_value(*eb);
          edge_weight[e_idx] = weight;
          edge_weight_computed[e_idx] = 1;
          // assign the weights to opposite edges
          edge_descriptor e_oppo = CGAL::opposite_edge(*eb, polyhedron);   
          std::size_t e_oppo_idx = boost::get(edge_index_map, e_oppo);
          edge_weight[e_oppo_idx] = mean_value(e_oppo);
          edge_weight_computed[e_oppo_idx] = 1;
        }
        else
        {
          edge_weight[e_idx] = weight;
          edge_weight_computed[e_idx] = 1;
          // assign the weights to opposite edges
          edge_descriptor e_oppo = CGAL::opposite_edge(*eb, polyhedron);   
          std::size_t e_oppo_idx = boost::get(edge_index_map, e_oppo);
          edge_weight[e_oppo_idx] = weight;
          edge_weight_computed[e_oppo_idx] = 1;
        }
        
      }


    }
  }

  // find region of solution, including roi and hard constraints, which is the 1-ring vertices out roi
  void region_of_solution()
  {
    ros.clear();
    ros.insert(ros.end(),roi.begin(), roi.end());

    // initialize the indices of ros vertices 
    ros_id.clear();
    ros_id.resize(boost::num_vertices(polyhedron), -1);
    std::size_t ros_idx;
    for ( ros_idx = 0; ros_idx < ros.size(); ros_idx++)
    {
      ros_id[ get(vertex_index_map, ros[ros_idx]) ] = ros_idx; 
    }

    for (std::size_t i = 0;i < roi.size(); i++)
    {
      vertex_descriptor vd = roi[i];
      in_edge_iterator e, e_end;
      for (boost::tie(e,e_end) = boost::in_edges(vd, polyhedron); e != e_end; e++)
      {
        vertex_descriptor vt = boost::source(*e, polyhedron);
        std::size_t idx = get(vertex_index_map, vt);
        if ( !is_roi[idx] && ros_id[idx] == -1 )    // neighboring vertices outside roi && not visited 
        {
          ros.push_back(vt);
          ros_id[idx] = ros_idx++;
        }
      }
    }

    // initialize the rotation matrices with the same size of ROS
    rot_mtr.clear();
    rot_mtr.resize(ros.size());
    for (std::size_t i = 0; i < ros.size(); i++)
    {
      rot_mtr[i].setIdentity();
    }
    
  }

  // Before we can model we have to do some precomputation
  ///
  /// @commentheading Template parameters:
  /// @param SparseLinearAlgebraTraits_d Definite positive sparse linear solver.
  void preprocess()
  {
    CGAL_TRACE_STREAM << "Calls preprocess()\n";

    Timer task_timer; task_timer.start();


    CGAL_TRACE_STREAM << "  Creates matrix...\n";

    compute_edge_weight();
    region_of_solution();

    // Assemble linear system A*X=B
    typename SparseLinearAlgebraTraits_d::Matrix A(ros.size()); // matrix is definite positive, and not necessarily symmetric
    assemble_laplacian(A);

    CGAL_TRACE_STREAM << "  Creates " << ros.size() << "*" << ros.size() << " matrix: done (" << task_timer.time() << " s)\n";

    CGAL_TRACE_STREAM << "  Pre-factorizing linear system...\n";

    // Pre-factorizing the linear system A*X=B
    task_timer.reset();
    double D;
    if(!m_solver.pre_factor(A, D))
      return;

    CGAL_TRACE_STREAM << "  Pre-factorizing linear system: done (" << task_timer.time() << " s)\n";

  }


  // Assemble Laplacian matrix A of linear system A*X=B
  ///
  /// @commentheading Template parameters:
  /// @param SparseLinearAlgebraTraits_d definite positive sparse linear solver.
  void assemble_laplacian(typename SparseLinearAlgebraTraits_d::Matrix& A)
  {
    // initialize the Laplacian matrix
    for (int i = 0; i < ros.size(); i++)
      {
        A.set_coef(i, i, 1.0, true);     
      }

    /// assign cotangent Laplacian to ros vertices
    for(std::size_t i = 0; i < ros.size(); i++)
      {
        vertex_descriptor vi = ros[i];
        std::size_t vertex_idx_i = get(vertex_index_map, vi);
        if ( is_roi[vertex_idx_i] && !is_hdl[vertex_idx_i] )          // vertices of ( roi - hdl )
          {
            double diagonal = 0;
            in_edge_iterator e, e_end;
            for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
              {
                vertex_descriptor vj = boost::source(*e, polyhedron);
                double wij = edge_weight[ boost::get(edge_index_map, *e) ];  // cotangent Laplacian weights
                std::size_t ros_idx_j = ros_id[ get(vertex_index_map, vj) ];
                A.set_coef(i, ros_idx_j, -wij, true);	// off-diagonal coefficient
                diagonal += wij;  
              }
            // diagonal coefficient
            A.set_coef(i, i, diagonal);
          } 
      }

  }

  
  // Returns the cotangent weight of specified edge_descriptor
  double cot_weight(edge_descriptor e)
  {
     vertex_descriptor v0 = boost::target(e, polyhedron);
     vertex_descriptor v1 = boost::source(e, polyhedron);
     // Only one triangle for border edges
     if (boost::get(CGAL::edge_is_border, polyhedron, e) ||
         boost::get(CGAL::edge_is_border, polyhedron, CGAL::opposite_edge(e, polyhedron)))
     {
       
       edge_descriptor e_cw = CGAL::next_edge_cw(e, polyhedron);
       vertex_descriptor v2 = boost::source(e_cw, polyhedron);
       if (boost::get(CGAL::edge_is_border, polyhedron, e_cw) ||
           boost::get(CGAL::edge_is_border, polyhedron, CGAL::opposite_edge(e_cw, polyhedron)) )
       {
          edge_descriptor e_ccw = CGAL::next_edge_ccw(e, polyhedron);
          v2 = boost::source(e_ccw, polyhedron);
       }
      
       return ( cot_value(v0, v2, v1)/2.0 );
     }
     else
     {
        edge_descriptor e_cw = CGAL::next_edge_cw(e, polyhedron);
        vertex_descriptor v2 = boost::source(e_cw, polyhedron);     
        edge_descriptor e_ccw = CGAL::next_edge_ccw(e, polyhedron);
        vertex_descriptor v3 = boost::source(e_ccw, polyhedron);

        return ( cot_value(v0, v2, v1)/2.0 + cot_value(v0, v3, v1)/2.0 );
     }
  }

  // Returns the cotangent value of angle v0_v1_v2
  double cot_value(vertex_descriptor v0, vertex_descriptor v1, vertex_descriptor v2)
  {
    
    Vector vec0 = v1->point() - v2->point();
    Vector vec1 = v2->point() - v0->point();
    Vector vec2 = v0->point() - v1->point();
    double e0_square = vec0.squared_length();
    double e1_square = vec1.squared_length();
    double e2_square = vec2.squared_length();
    double e0 = std::sqrt(e0_square); 
    double e2 = std::sqrt(e2_square);
    double cos_angle = ( e0_square + e2_square - e1_square ) / 2.0 / e0 / e2;
    double sin_angle = std::sqrt(1-cos_angle*cos_angle);

    return (cos_angle/sin_angle);

  }

  // Returns the tangent value of half angle v0_v1_v2/2
  double half_tan_value(vertex_descriptor v0, vertex_descriptor v1, vertex_descriptor v2)
  {

    Vector vec0 = v1->point() - v2->point();
    Vector vec1 = v2->point() - v0->point();
    Vector vec2 = v0->point() - v1->point();
    double e0_square = vec0.squared_length();
    double e1_square = vec1.squared_length();
    double e2_square = vec2.squared_length();
    double e0 = std::sqrt(e0_square); 
    double e2 = std::sqrt(e2_square);
    double cos_angle = ( e0_square + e2_square - e1_square ) / 2.0 / e0 / e2;
    double angle = acos(cos_angle);

    return ( tan(angle/2.0) );

  }

  // Returns the mean-value coordinate of specified edge_descriptor
  double mean_value(edge_descriptor e)
  {
    vertex_descriptor v0 = boost::target(e, polyhedron);
    vertex_descriptor v1 = boost::source(e, polyhedron);
    Vector vec = v0->point() - v1->point();
    double norm = std::sqrt( vec.squared_length() );

    // Only one triangle for border edges
    if (boost::get(CGAL::edge_is_border, polyhedron, e) ||
        boost::get(CGAL::edge_is_border, polyhedron, CGAL::opposite_edge(e, polyhedron)))
    {

      edge_descriptor e_cw = CGAL::next_edge_cw(e, polyhedron);
      vertex_descriptor v2 = boost::source(e_cw, polyhedron);
      if (boost::get(CGAL::edge_is_border, polyhedron, e_cw) || 
          boost::get(CGAL::edge_is_border, polyhedron, CGAL::opposite_edge(e_cw, polyhedron)) )
      {
        edge_descriptor e_ccw = CGAL::next_edge_ccw(e, polyhedron);
        v2 = boost::source(e_ccw, polyhedron);
      }

      return ( half_tan_value(v1, v0, v2)/norm );
    }
    else
    {
      edge_descriptor e_cw = CGAL::next_edge_cw(e, polyhedron);
      vertex_descriptor v2 = boost::source(e_cw, polyhedron);     
      edge_descriptor e_ccw = CGAL::next_edge_ccw(e, polyhedron);
      vertex_descriptor v3 = boost::source(e_ccw, polyhedron);

      return ( half_tan_value(v1, v0, v2)/norm + half_tan_value(v1, v0, v3)/norm );
    }
  }

  // Set the number of iterations made in operator()
  void set_iterations(unsigned int ite)
  {
    iterations = ite;
  }

  // Set the tolerance of convergence made in operator()
  void set_tolerance(double tole)
  {
    tolerance = tole;
  }
  
  // The operator will be called in a real time loop from the GUI.
  // assign translation vector to all handles
  void operator()(const Vector& translation)
  {
    for (std::size_t idx = 0; idx < hdl.size(); idx++)
      {
        vertex_descriptor vd = hdl[idx];
        solution[get(vertex_index_map, vd)] = original[get(vertex_index_map,vd)] + translation;
      }
  }

  // The operator will be called in a real time loop from the GUI.
  // assign translation vector to specific handle
  void operator()(vertex_descriptor vd, const Vector& translation)
  {
    std::size_t idx = get(vertex_index_map, vd);
    solution[idx] = original[idx] + translation;
  }

#ifdef CGAL_DEFORM_ROTATION

  template <typename Quaternion, typename Vect>
  void operator()(vertex_descriptor vd, const Point& rotation_center, const Quaternion& quat, const Vect& translation)
  {
    std::size_t idx = get(vertex_index_map, vd);
    Point p = CGAL::ORIGIN + ( original[idx] - rotation_center);
    Vect v = quat * Vect(p.x(),p.y(),p.z());
    p = Point(v[0], v[1], v[2]) + ( rotation_center - CGAL::ORIGIN); 
    p = p + Vector(translation[0],translation[1],translation[2]);
   
    solution[idx] = p;
  }
#endif // CGAL_DEFORM_ROTATION


  // Local step of iterations, computing optimal rotation matrices using SVD decomposition, stable
  void optimal_rotations_svd()
  {
    Eigen::Matrix3d u, v;           // orthogonal matrices 
    Eigen::Vector3d w;              // singular values
    Eigen::Matrix3d cov;            // covariance matrix
    Eigen::JacobiSVD<Eigen::Matrix3d> svd;       // SVD solver         
    Eigen::Matrix3d r;
    int num_neg = 0;

    // only accumulate ros vertices
    for ( std::size_t i = 0; i < ros.size(); i++ )
    {
      vertex_descriptor vi = ros[i];
      // compute covariance matrix
      cov.setZero();

      in_edge_iterator e, e_end;
      for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
      {
        vertex_descriptor vj = boost::source(*e, polyhedron);
        Vector pij = original[get(vertex_index_map, vi)] - original[get(vertex_index_map, vj)];
        Vector qij = solution[get(vertex_index_map, vi)] - solution[get(vertex_index_map, vj)];
        double wij = edge_weight[boost::get(edge_index_map, *e)];
        for (int j = 0; j < 3; j++)
        {
          for (int k = 0; k < 3; k++)
          {
            cov(j, k) += wij*pij[j]*qij[k]; 
          }
        }
      }
  
      // svd decomposition
      svd.compute( cov, Eigen::ComputeFullU | Eigen::ComputeFullV );
      u = svd.matrixU(); v = svd.matrixV();

      // extract rotation matrix
      r = v*u.transpose();

      // checking negative determinant of r
      if ( r.determinant() < 0 )    // changing the sign of column corresponding to smallest singular value
      {
        num_neg++; 
        w = svd.singularValues();
        for (int j = 0; j < 3; j++)
        {
          int j0 = j;
          int j1 = (j+1)%3;
          int j2 = (j1+1)%3;
          if ( w[j0] <= w[j1] && w[j0] <= w[j2] )    // smallest singular value as j0
          {
            u(0, j0) = - u(0, j0);
            u(1, j0) = - u(1, j0);
            u(2, j0) = - u(2, j0);
            break;
          }
        }

        // re-extract rotation matrix
        r = v*u.transpose();
      }
      
      rot_mtr[i] = r;
    }

    CGAL_TRACE_STREAM << num_neg << " negative rotations\n";

  }


#ifdef CGAL_DEFORM_EXPERIMENTAL      // Experimental stuff, needs further testing

  double norm_1(const Eigen::Matrix3d& X)
  {
    double sum = 0;
    for ( int i = 0; i < 3; i++ )
    {
      for ( int j = 0; j < 3; j++ )
      {
        sum += abs(X(i,j));
      }
    }
    return sum;
  }

  double norm_inf(const Eigen::Matrix3d& X)
  {
    double max_abs = abs(X(0,0));
    for ( int i = 0; i < 3; i++ )
    {
      for ( int j = 0; j < 3; j++ )
      {
        double new_abs = abs(X(i,j));
        if ( new_abs > max_abs )
        {
          max_abs = new_abs;
        }
      }
    }
    return max_abs;
  }

  // polar decomposition using Newton's method, with warm start, stable but slow
  // not used, need to be investigated later
  void polar_newton(const Eigen::Matrix3d& A, Eigen::Matrix3d &U, double tole)
  {
    Eigen::Matrix3d X = A;
    Eigen::Matrix3d Y;
    double alpha, beta, gamma;
    do 
    {
      Y = X.inverse();
      alpha = sqrt( norm_1(X) * norm_inf(X) );
      beta = sqrt( norm_1(Y) * norm_inf(Y) );
      gamma = sqrt(beta/alpha);
      X = 0.5*( gamma*X + Y.transpose()/gamma );

    } while ( abs(gamma-1) > tole );

    U = X;
  }
  
  // polar decomposition using Eigen, 5 times faster than SVD
  template<typename Mat>
  void polar_eigen(const Mat& A, Mat& R, bool& SVD)
  {
    typedef typename Mat::Scalar Scalar;
    typedef Eigen::Matrix<typename Mat::Scalar,3,1> Vec;

    const Scalar th = std::sqrt(Eigen::NumTraits<Scalar>::dummy_precision());

    Eigen::SelfAdjointEigenSolver<Mat> eig;
    feclearexcept(FE_UNDERFLOW);
    eig.computeDirect(A.transpose()*A);
    if(fetestexcept(FE_UNDERFLOW) || eig.eigenvalues()(0)/eig.eigenvalues()(2)<th)
    {
      // The computation of the eigenvalues might have diverged.
      // Fallback to an accurate SVD based decomposiiton method.
      Eigen::JacobiSVD<Mat> svd;
      svd.compute(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
      const Mat& u = svd.matrixU(); const Mat& v = svd.matrixV();
      R = u*v.transpose();
      SVD = true;
      return;
    }

    Vec S = eig.eigenvalues().cwiseSqrt();
    R = A  * eig.eigenvectors() * S.asDiagonal().inverse()
      * eig.eigenvectors().transpose();
    SVD = false;

    if(std::abs(R.squaredNorm()-3.) > th)
    {
      // The computation of the eigenvalues might have diverged.
      // Fallback to an accurate SVD based decomposiiton method.
      Eigen::JacobiSVD<Mat> svd;
      svd.compute(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
      const Mat& u = svd.matrixU(); const Mat& v = svd.matrixV();
      R = u*v.transpose();
      SVD = true;
      return;
    }
  }

  // Local step of iterations, computing optimal rotation matrices using Polar decomposition
  void optimal_rotations_polar()
  {
    Eigen::Matrix3d u, v;           // orthogonal matrices 
    Eigen::Vector3d w;              // singular values
    Eigen::Matrix3d cov;            // covariance matrix
    Eigen::Matrix3d r;
    Eigen::JacobiSVD<Eigen::Matrix3d> svd;      // SVD solver, for non-positive covariance matrices
    int num_svd = 0;
    bool SVD = false;

    // only accumulate ros vertices
    for ( std::size_t i = 0; i < ros.size(); i++ )
    {
      vertex_descriptor vi = ros[i];
      // compute covariance matrix
      cov.setZero();

      in_edge_iterator e, e_end;
      for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
      {
        vertex_descriptor vj = boost::source(*e, polyhedron);
        Vector pij = original[get(vertex_index_map, vi)] - original[get(vertex_index_map, vj)];
        Vector qij = solution[get(vertex_index_map, vi)] - solution[get(vertex_index_map, vj)];
        double wij = edge_weight[boost::get(edge_index_map, *e)];
        for (int j = 0; j < 3; j++)
        {
          for (int k = 0; k < 3; k++)
          {
            cov(j, k) += wij*pij[j]*qij[k]; 
          }
        }
      }

      // svd decomposition
      if (cov.determinant() > 0)
      {
        polar_eigen<Eigen::Matrix3d> (cov, r, SVD);
        //polar_newton(cov, r, 1e-4);   
        if(SVD)
          num_svd++;
        r.transposeInPlace();     // the optimal rotation matrix should be transpose of decomposition result
      }
      else
      {
        svd.compute( cov, Eigen::ComputeFullU | Eigen::ComputeFullV );
        u = svd.matrixU(); v = svd.matrixV(); w = svd.singularValues();
        r = v*u.transpose();
        num_svd++;
      }
      
      // checking negative determinant of covariance matrix
      if ( r.determinant() < 0 )    // back to SVD method
      {
        if (cov.determinant() > 0)
        {
          svd.compute( cov, Eigen::ComputeFullU | Eigen::ComputeFullV );
          u = svd.matrixU(); v = svd.matrixV(); w = svd.singularValues();
          num_svd++;
        }
        for (int j = 0; j < 3; j++)
        {
          int j0 = j;
          int j1 = (j+1)%3;
          int j2 = (j1+1)%3;
          if ( w[j0] <= w[j1] && w[j0] <= w[j2] )    // smallest singular value as j0
          {
            u(0, j0) = - u(0, j0);
            u(1, j0) = - u(1, j0);
            u(2, j0) = - u(2, j0);
            break;
          }
        }

        // re-extract rotation matrix
        r = v*u.transpose();
      }

      rot_mtr[i] = r;
    }

    double svd_percent = (double)(num_svd)/ros.size();
    CGAL_TRACE_STREAM << svd_percent*100 << "% percentage SVD decompositions;";
    CGAL_TRACE_STREAM << num_svd << " SVD decompositions\n";

  }

#endif


  // Global step of iterations, updating solution
  void update_solution()
  {
    typename SparseLinearAlgebraTraits_d::Vector X(ros.size()), Bx(ros.size());
    typename SparseLinearAlgebraTraits_d::Vector Y(ros.size()), By(ros.size());
    typename SparseLinearAlgebraTraits_d::Vector Z(ros.size()), Bz(ros.size());

    // assemble right columns of linear system
    for ( std::size_t i = 0; i < ros.size(); i++ )
    {
      vertex_descriptor vi = ros[i];
      std::size_t vertex_idx_i = get(vertex_index_map, vi);
      if ( !is_roi[vertex_idx_i] || is_hdl[vertex_idx_i] )   // hard constraints or handle vertices
      {
        Bx[i] = solution[vertex_idx_i].x(); By[i] = solution[vertex_idx_i].y(); Bz[i] = solution[vertex_idx_i].z();
      }
      else  // ( roi - handle ) vertices
      {
        Bx[i] = 0; By[i] = 0; Bz[i] = 0;
        in_edge_iterator e, e_end;
        Point& pi = original[get(vertex_index_map, vi)];
        for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
        {
          vertex_descriptor vj = boost::source(*e, polyhedron);
          std::size_t ros_idx_j = ros_id[ get(vertex_index_map, vj) ]; 
          Vector pij =  pi - original[get(vertex_index_map, vj)];
          double wij = edge_weight[boost::get(edge_index_map, *e)];
          Vector rot_p(0, 0, 0);                  // vector ( r_i + r_j )*p_ij
          for (int j = 0; j < 3; j++)
          {
            double x = ( rot_mtr[i](0, j) + rot_mtr[ros_idx_j](0, j) ) * pij[j];
            double y = ( rot_mtr[i](1, j) + rot_mtr[ros_idx_j](1, j) ) * pij[j];
            double z = ( rot_mtr[i](2, j) + rot_mtr[ros_idx_j](2, j) ) * pij[j];
            
            rot_p = rot_p + Vector(x, y, z);
          }
          
          Vector vec = wij*rot_p/2.0;
          Bx[i] += vec.x(); By[i] += vec.y(); Bz[i] += vec.z(); 
        }
      }
    }

    // solve "A*X = B".
    m_solver.linear_solver(Bx, X); m_solver.linear_solver(By, Y); m_solver.linear_solver(Bz, Z);

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

  }

  // Compute modeling energy
  double energy()
  {
    double sum_of_energy = 0;
    // only accumulate ros vertices
    for( std::size_t i = 0; i < ros.size(); i++ )
    {
      vertex_descriptor vi = ros[i];
      in_edge_iterator e, e_end;
      for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
      {
        vertex_descriptor vj = boost::source(*e, polyhedron);
        Vector pij = original[get(vertex_index_map, vi)] - original[get(vertex_index_map, vj)];
        double wij = edge_weight[boost::get(edge_index_map, *e)];
        Vector rot_p(0, 0, 0);                 // vector rot_i*p_ij
        for (int j = 0; j < 3; j++)
        {
          double x = rot_mtr[i](0, j) * pij[j];
          double y = rot_mtr[i](1, j) * pij[j];
          double z = rot_mtr[i](2, j) * pij[j];
          Vector v(x, y, z);
          rot_p = rot_p + v;
        }
        Vector qij = solution[get(vertex_index_map, vi)] - solution[get(vertex_index_map, vj)];
        sum_of_energy += wij*(qij - rot_p).squared_length();
      }
    }
    return sum_of_energy;
  }

  // Deformation on roi vertices
  void deform()
  {
    double energy_this = 0;
    double energy_last;
    // iterations
    CGAL_TRACE_STREAM << "iteration started...\n";
    for ( unsigned int ite = 0; ite < iterations; ite ++)
    {
      update_solution();

#ifdef CGAL_DEFORM_EXPERIMENTAL
      optimal_rotations_polar();    // polar decomposition for optimal rotations, faster than SVD but unstable 
#else
      optimal_rotations_svd();
#endif
      energy_last = energy_this;
      energy_this = energy();
      CGAL_TRACE_STREAM << ite << " iterations: energy = " << energy_this << "\n";
      if ( abs((energy_last-energy_this)/energy_this) < tolerance )
      {
        break;
      }
    }

    CGAL_TRACE_STREAM << "iteration end!\n";

    // copy solution to target mesh
    assign_solution();
  }

  // Assign solution to target mesh
  void assign_solution()
  {
    for(std::size_t i = 0; i < roi.size(); ++i){
      roi[i]->point() = solution[get(vertex_index_map, roi[i])];
    }
  }

  // Undo: reset P to be the copied polyhedron
  void undo()
  {
    std::size_t i = 0;
    vertex_iterator vb, ve;
    for(boost::tie(vb,ve) = boost::vertices(polyhedron); vb != ve; ++vb )
      {
        (*vb)->point() = original[i++];
      }
  }
};


} //namespace CGAL

#endif  // CGAL_DEFORM_MESH_H