#include <Rcpp.h>
// Copyright (c) 2009 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL: https://github.com/CGAL/cgal/blob/v6.1/Mesh_3/include/CGAL/Mesh_3/Mesh_global_optimizer.h $
// $Id: include/CGAL/Mesh_3/Mesh_global_optimizer.h b26b07a1242 $
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Stephane Tayeb
//
//******************************************************************************
// File Description :
//******************************************************************************

#ifndef CGAL_MESH_3_MESH_GLOBAL_OPTIMIZER_H
#define CGAL_MESH_3_MESH_GLOBAL_OPTIMIZER_H

#include <CGAL/license/Mesh_3.h>

#include <CGAL/disable_warnings.h>

#include <CGAL/Mesh_3/config.h>

#include <CGAL/Real_timer.h>
#include <CGAL/Mesh_3/C3T3_helpers.h>
#include <CGAL/Mesh_3/Triangulation_helpers.h>
#include <CGAL/Origin.h>
#include <CGAL/Mesh_optimization_return_code.h>
#include <CGAL/Mesh_3/Null_global_optimizer_visitor.h>
#include <CGAL/Time_stamper.h>

#include <CGAL/iterator.h>

#include <CGAL/Mesh_3/Concurrent_mesher_config.h>

#ifdef CGAL_MESH_3_PROFILING
  #include <CGAL/Mesh_3/Profiling_tools.h>
#endif

#include <vector>
#include <list>
#include <limits>

#ifdef CGAL_LINKED_WITH_TBB
# include <atomic>
# include <mutex>
# include <tbb/parallel_for_each.h>
# include <tbb/concurrent_vector.h>
#endif

namespace CGAL {

namespace Mesh_3 {


/************************************************
// Class Mesh_global_optimizer_base
// Two versions: sequential / parallel
************************************************/

// Sequential
template <typename Tr, typename Concurrency_tag>
class Mesh_global_optimizer_base
{
protected:
  typedef typename Tr::Geom_traits                          GT;
  typedef typename GT::FT                                   FT;
  typedef typename GT::Vector_3                             Vector_3;
  typedef typename Tr::Lock_data_structure                  Lock_data_structure;

  // The sizing field info is stored inside the move vector because it is computed
  // when the move is computed. This is because the parallel version uses the threadsafe
  // version of incident_cells (which thus requires points to not be moving yet)
  struct Move
  {
    typename Tr::Vertex_handle vertex_;
    Vector_3 move_;
    FT size_;
  };
  typedef std::vector<Move>                                 Moves_vector;
  typedef unsigned int                                      Nb_frozen_points_type;

  Mesh_global_optimizer_base(const Bbox_3 &, int)
    : nb_frozen_points_(0), big_moves_size_(0) {}

  void update_big_moves(const FT& new_sq_move)
  {
    if (big_moves_.size() < big_moves_size_ )
      big_moves_.insert(new_sq_move);
    else
    {
      FT smallest = *(big_moves_.begin());
      if( new_sq_move > smallest )
      {
        big_moves_.erase(big_moves_.begin());
        big_moves_.insert(new_sq_move);
      }
    }
  }

  void clear_big_moves()
  {
    big_moves_.clear();
  }

  Lock_data_structure *get_lock_data_structure() { return 0; }
  void unlock_all_elements() {}


  // Workaround for problem with VC and /permissive
  // See: https://gist.github.com/afabri/0416bebec1c32fb4efd6632446698972
  void increment_frozen_points() const
  {
    ++nb_frozen_points_;
  }

protected:
  mutable unsigned int nb_frozen_points_;
  std::size_t big_moves_size_;
  std::multiset<FT> big_moves_;
};

#ifdef CGAL_LINKED_WITH_TBB
// Parallel
template <typename Tr>
class Mesh_global_optimizer_base<Tr, Parallel_tag>
{

protected:
  typedef typename Tr::Geom_traits                          GT;
  typedef typename GT::FT                                   FT;
  typedef typename GT::Vector_3                             Vector_3;
  typedef typename Tr::Lock_data_structure                  Lock_data_structure;

  struct Move
  {
    typename Tr::Vertex_handle vertex_;
    Vector_3 move_;
    FT size_;
  };
  typedef tbb::concurrent_vector<Move>                      Moves_vector;
  typedef std::atomic<unsigned int>                         Nb_frozen_points_type ;

  Mesh_global_optimizer_base(const Bbox_3 &bbox, int num_grid_cells_per_axis)
    : nb_frozen_points_(0), big_moves_size_(0)
    , m_lock_ds(bbox, num_grid_cells_per_axis)
  {
    big_moves_current_size_ = 0;
    big_moves_smallest_ = (std::numeric_limits<FT>::max)();
  }

  void update_big_moves(const FT& new_sq_move)
  {
    if (++big_moves_current_size_ <= big_moves_size_ )
    {
      std::lock_guard<std::mutex> lock(m_big_moves_mutex);
      typename std::multiset<FT>::const_iterator it = big_moves_.insert(new_sq_move);

      // New smallest move of all big moves?
      if (it == big_moves_.begin())
        big_moves_smallest_ = new_sq_move;
    }
    else
    {
      --big_moves_current_size_;

      if( new_sq_move > big_moves_smallest_ )
      {
        std::lock_guard<std::mutex> lock(m_big_moves_mutex);
        // Test it again since it may have been modified by another
        // thread in the meantime
        if( new_sq_move > big_moves_smallest_ )
        {
          big_moves_.erase(big_moves_.begin());
          typename std::multiset<FT>::const_iterator it = big_moves_.insert(new_sq_move);

          // New smallest move of all big moves?
          if (it == big_moves_.begin())
            big_moves_smallest_ = new_sq_move;
        }
      }
    }
  }

  void clear_big_moves()
  {
    big_moves_current_size_ = 0;
    big_moves_smallest_ = (std::numeric_limits<FT>::max)();
    big_moves_.clear();
  }

  Lock_data_structure *get_lock_data_structure()
  {
    return &m_lock_ds;
  }

  void unlock_all_elements()
  {
    m_lock_ds.unlock_all_points_locked_by_this_thread();
  }

  void increment_frozen_points() const
  {
    ++nb_frozen_points_;
  }
public:

protected:
  mutable std::atomic<unsigned int> nb_frozen_points_;
  std::atomic<std::size_t>  big_moves_current_size_;
  std::atomic<FT>           big_moves_smallest_;
  std::size_t               big_moves_size_;
  std::multiset<FT>         big_moves_;
  std::mutex                m_big_moves_mutex;

  /// Lock data structure
  Lock_data_structure m_lock_ds;
};
#endif // CGAL_LINKED_WITH_TBB




/************************************************
// Class Mesh_global_optimizer
************************************************/

template <typename C3T3,
          typename MeshDomain,
          typename MoveFunction,
          typename Visitor_ = Null_global_optimizer_visitor<C3T3> >
class Mesh_global_optimizer
: public Mesh_global_optimizer_base<typename C3T3::Triangulation, typename C3T3::Concurrency_tag>
{
  // Types
  typedef typename C3T3::Concurrency_tag Concurrency_tag;

  typedef Mesh_global_optimizer<C3T3, MeshDomain, MoveFunction, Visitor_> Self;
  typedef Mesh_global_optimizer_base<
    typename C3T3::Triangulation, typename C3T3::Concurrency_tag>         Base;

  using Base::get_lock_data_structure;
  using Base::big_moves_;
  using Base::big_moves_size_;
  using Base::nb_frozen_points_;
  using Base::increment_frozen_points;

  typedef typename C3T3::Triangulation  Tr;
  typedef typename Tr::Geom_traits      GT;

  typedef typename Tr::Bare_point       Bare_point;
  typedef typename Tr::Weighted_point   Weighted_point;
  typedef typename Tr::Cell_handle      Cell_handle;
  typedef typename Tr::Vertex_handle    Vertex_handle;
  typedef typename Tr::Edge             Edge;
  typedef typename Tr::Vertex           Vertex;

  typedef typename GT::FT               FT;
  typedef typename GT::Vector_3         Vector_3;

  typedef typename std::vector<Cell_handle>                      Cell_vector;
  typedef typename std::vector<Vertex_handle>                    Vertex_vector;
  typedef Hash_handles_with_or_without_timestamps                Hash_fct;
  typedef typename boost::unordered_set<Vertex_handle, Hash_fct> Vertex_set;
  typedef typename Base::Moves_vector                            Moves_vector;

#ifdef CGAL_INTRUSIVE_LIST
  typedef Intrusive_list<Cell_handle>   Outdated_cell_set;
#else
  typedef std::set<Cell_handle>         Outdated_cell_set;
#endif //CGAL_INTRUSIVE_LIST

#ifdef CGAL_INTRUSIVE_LIST
  typedef Intrusive_list<Vertex_handle>  Moving_vertices_set;
#else
  typedef Vertex_set Moving_vertices_set;
#endif

  typedef typename MoveFunction::Sizing_field Sizing_field;

  typedef class C3T3_helpers<C3T3,MeshDomain> C3T3_helpers;

  // Visitor class
  // Should define:
  //  - after_compute_moves()
  //  - after_move_points()
  //  - after_rebuild_restricted_delaunay()
  //  - end_of_iteration(int iteration_number)
  typedef Visitor_ Visitor;

public:
  /**
   * Constructor
   */
  Mesh_global_optimizer(C3T3& c3t3,
                        const MeshDomain& domain,
                        const FT& freeze_ratio,
                        const bool do_freeze,
                        const FT& convergence_ratio,
                        const MoveFunction move_function = MoveFunction());

  /**
   * Launch optimization process
   *
   * @param nb_interations maximum number of iterations
   */
  Mesh_optimization_return_code operator()(int nb_iterations,
                                           Visitor v = Visitor());

  /**
  * collects all vertices of the triangulation in moving_vertices
  * (even the frozen ones)
  */
  void collect_all_vertices(Moving_vertices_set& moving_vertices);

  /// Time accessors
  void set_time_limit(double time) { time_limit_ = time; }
  double time_limit() const { return time_limit_; }

private:
  /**
   * Returns moves for vertices of set `moving_vertices`.
   */
  Moves_vector compute_moves(Moving_vertices_set& moving_vertices);

  /**
   * Returns the move for vertex `v`.
   * \warning This function should be called only on moving vertices
   *          even for frozen vertices, it could return a non-zero vector
   */
  Vector_3 compute_move(const Vertex_handle& v);

  /**
   * Updates mesh using moves of `moves` vector. Updates moving_vertices with
   * the new set of moving vertices after the move.
   */
  void update_mesh(const Moves_vector& moves,
                   Moving_vertices_set& moving_vertices,
                   Visitor& visitor);

  /**
   * Fill sizing field using sizes (avg circumradius) contained in tr_
   */
  void fill_sizing_field();

  /**
   * Returns `true` if convergence is reached
   */
  bool check_convergence() const;

  /**
   * Returns the average circumradius length of cells incident to `v`.
   */
  FT average_circumradius_length(const Vertex_handle& v) const;

  /**
   * Returns the minimum cicumradius length of cells incident to `v`.
   */
  FT min_circumradius_sq_length(const Vertex_handle& v, const Cell_vector& incident_cells) const;

  /**
   * Returns the squared circumradius length of cell `cell`.
   */
  FT sq_circumradius_length(const Cell_handle& cell,
                            const Vertex_handle& v) const;

  /**
   * Returns `true` if time_limit is reached
   */
  bool is_time_limit_reached() const
  {
    return ( (time_limit() > 0) && (running_time_.time() > time_limit()) );
  }

private:

#ifdef CGAL_LINKED_WITH_TBB
  // Functor for compute_moves function
  template <typename MGO, typename Sizing_field_, typename Moves_vector_>
  class Compute_move
  {
    typedef tbb::concurrent_vector<Vertex_handle> Vertex_conc_vector;

    MGO                  & m_mgo;
    const Sizing_field_  & m_sizing_field;
    Moves_vector_        & m_moves;
    bool                   m_do_freeze;
    Vertex_conc_vector   & m_vertices_not_moving_any_more;
    const GT             & m_gt;

  public:
    // Constructor
    Compute_move(MGO &mgo,
                 const Sizing_field_ &sizing_field,
                 Moves_vector_ &moves,
                 bool do_freeze,
                 Vertex_conc_vector &vertices_not_moving_any_more,
                 const GT &gt)
    : m_mgo(mgo),
      m_sizing_field(sizing_field),
      m_moves(moves),
      m_do_freeze(do_freeze),
      m_vertices_not_moving_any_more(vertices_not_moving_any_more),
      m_gt(gt)
    {}

    // Constructor
    Compute_move(const Compute_move &cm)
    : m_mgo(cm.m_mgo),
      m_sizing_field(cm.m_sizing_field),
      m_moves(cm.m_moves),
      m_do_freeze(cm.m_do_freeze),
      m_vertices_not_moving_any_more(cm.m_vertices_not_moving_any_more),
      m_gt(cm.m_gt)
    {}

    // operator()
    void operator()(const Vertex_handle& oldv) const
    {
      typename GT::Construct_point_3 cp = m_gt.construct_point_3_object();
      typename GT::Construct_translated_point_3 translate = m_gt.construct_translated_point_3_object();
      typedef typename Moves_vector_::value_type Move;

      Vector_3 move = m_mgo.compute_move(oldv);
      if ( CGAL::NULL_VECTOR != move )
      {
        FT size = 0.;

        if(MGO::Sizing_field::is_vertex_update_needed)
        {
          const Weighted_point& position = m_mgo.tr_.point(oldv);
          Bare_point new_position = translate(cp(position), move);
          size = m_sizing_field(new_position, oldv);
        }

        // typedef Triangulation_helpers<typename C3T3::Triangulation> Th;
        //if( !Th().inside_protecting_balls(tr_, oldv, new_position))
        //note : this is not happening for Lloyd and ODT so it's commented
        //       maybe for a new global optimizer it should be de-commented

        m_moves.push_back(Move{oldv, move, size});
      }
      else // CGAL::NULL_VECTOR == move
      {
        if(m_do_freeze)
        {
          m_vertices_not_moving_any_more.push_back(oldv);
        }
      }

      if ( m_mgo.is_time_limit_reached() )
        m_mgo.cancel_group_execution();
    }
  };

  // Functor for fill_sizing_field function
  template <typename MGO, typename Local_list_>
  class Compute_sizing_field_value
  {
    MGO                  & m_mgo;
    const GT             & m_gt;
    Local_list_          & m_local_lists;

  public:
    // Constructor
    Compute_sizing_field_value(MGO &mgo,
                               const GT &gt,
                               Local_list_ &local_lists)
    : m_mgo(mgo),
      m_gt(gt),
      m_local_lists(local_lists)
    {}

    // Constructor
    Compute_sizing_field_value(const Compute_sizing_field_value &csfv)
    : m_mgo(csfv.m_mgo),
      m_gt(csfv.m_gt),
      m_local_lists(csfv.m_local_lists)
    {}

    // operator()
    void operator()(Vertex& v) const
    {
      typename GT::Construct_point_3 cp = m_gt.construct_point_3_object();

      Vertex_handle vh
        = Tr::Triangulation_data_structure::Vertex_range::s_iterator_to(v);
      const Weighted_point& position = m_mgo.tr_.point(vh);
      m_local_lists.local().push_back(
          std::make_pair(cp(position), m_mgo.average_circumradius_length(vh)));
    }
  };

  // Functor for update_mesh function
  template <typename MGO, typename Helper, typename Tr_, typename Moves_vector_,
            typename Moving_vertices_set_, typename Outdated_cell_set_>
  class Move_vertex
  {
    MGO                                  & m_mgo;
    const Helper                         & m_helper;
    const Moves_vector_                  & m_moves;
    Moving_vertices_set_                 & m_moving_vertices;
    Outdated_cell_set_                   & m_outdated_cells;

    typedef typename Tr_::Bare_point    Bare_point;
    typedef typename Tr_::Vertex_handle Vertex_handle;

  public:
    // Constructor
    Move_vertex(MGO &mgo, const Helper &helper, const Moves_vector_ &moves,
                Moving_vertices_set_ &moving_vertices,
                Outdated_cell_set_ &outdated_cells)
    : m_mgo(mgo), m_helper(helper), m_moves(moves),
      m_moving_vertices(moving_vertices), m_outdated_cells(outdated_cells)
    {}

    // Constructor
    Move_vertex(const Move_vertex &mv)
    : m_mgo(mv.m_mgo), m_helper(mv.m_helper), m_moves(mv.m_moves),
      m_moving_vertices(mv.m_moving_vertices),
      m_outdated_cells(mv.m_outdated_cells)
    {}

    // operator()
    void operator()( const tbb::blocked_range<size_t>& r ) const
    {
      for( size_t i = r.begin() ; i != r.end() ; ++i)
      {
        const Vertex_handle& v = m_moves[i].vertex_;
        const Vector_3& move = m_moves[i].move_;

        // Get size at new position
        if ( MGO::Sizing_field::is_vertex_update_needed )
        {
          const FT size = m_moves[i].size_;

          // Move point
          bool could_lock_zone;
          Vertex_handle new_v = m_helper.move_point(
            v, move, m_outdated_cells, m_moving_vertices, &could_lock_zone);
          while (could_lock_zone == false)
          {
            new_v = m_helper.move_point(
              v, move, m_outdated_cells, m_moving_vertices, &could_lock_zone);
          }

          // Restore size in meshing_info data
          new_v->set_meshing_info(size);
        }
        else // Move point
        {
          bool could_lock_zone;
          do {
            m_helper.move_point(
              v, move, m_outdated_cells, m_moving_vertices, &could_lock_zone);
          } while (!could_lock_zone);
        }

        m_mgo.unlock_all_elements();

        // Stop if time_limit_ is reached, here we can't return without rebuilding
        // restricted delaunay
        if ( m_mgo.is_time_limit_reached() )
        {
          m_mgo.cancel_group_execution();
          break;
        }
      }
    }
  };

  void cancel_group_execution() {
    tbb_task_group_context.cancel_group_execution();
  }
#endif // CGAL_LINKED_WITH_TBB

  // -----------------------------------
  // Private data
  // -----------------------------------
  C3T3& c3t3_;
  Tr& tr_;
  const MeshDomain& domain_;
  FT sq_freeze_ratio_;
  FT convergence_ratio_;
  C3T3_helpers helper_;
  MoveFunction move_function_;
  Sizing_field sizing_field_;
  double time_limit_;
  CGAL::Real_timer running_time_;

  bool do_freeze_;

#ifdef CGAL_MESH_3_OPTIMIZER_VERBOSE
  mutable FT sum_moves_;
#endif
#ifdef CGAL_LINKED_WITH_TBB
  tbb::task_group_context tbb_task_group_context;
#endif
};


template <typename C3T3, typename Md, typename Mf, typename V_>
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
Mesh_global_optimizer(C3T3& c3t3,
                      const Md& domain,
                      const FT& freeze_ratio,
                      const bool do_freeze,
                      const FT& convergence_ratio,
                      const Mf move_function)
: Base(c3t3.bbox(),
       Concurrent_mesher_config::get().locking_grid_num_cells_per_axis)
, c3t3_(c3t3)
, tr_(c3t3_.triangulation())
, domain_(domain)
, sq_freeze_ratio_(freeze_ratio*freeze_ratio)
, convergence_ratio_(convergence_ratio)
, helper_(c3t3_,domain_, get_lock_data_structure())
, move_function_(move_function)
, sizing_field_(c3t3.triangulation())
, time_limit_(-1)
, running_time_()
, do_freeze_(do_freeze)
#ifdef CGAL_MESH_3_OPTIMIZER_VERBOSE
, sum_moves_(0)
#endif // CGAL_MESH_3_OPTIMIZER_VERBOSE
{
  // If we're multi-thread
  tr_.set_lock_data_structure(get_lock_data_structure());

#ifdef CGAL_MESH_3_OPTIMIZER_VERBOSE
  Rcpp::Rcerr << "Fill sizing field...";
  CGAL::Real_timer timer;
  timer.start();
#endif

  fill_sizing_field();

#ifdef CGAL_MESH_3_OPTIMIZER_VERBOSE
  Rcpp::Rcerr << "done (" << timer.time() << "s)\n";
#endif
}


template <typename C3T3, typename Md, typename Mf, typename V_>
Mesh_optimization_return_code
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
operator()(int nb_iterations, Visitor visitor)
{
  running_time_.reset();
  running_time_.start();

#ifdef CGAL_MESH_3_PROFILING
  WallClockTimer t;
#endif

  // Fill set containing moving vertices
  // first, take them all
#ifdef  CGAL_CONSTRUCT_INTRUSIVE_LIST_RANGE_CONSTRUCTOR
  typedef CGAL::Prevent_deref<typename Tr::Finite_vertices_iterator> It;
  Moving_vertices_set moving_vertices(It(tr_.finite_vertices_begin()),
                                      It(tr_.finite_vertices_end()));
#else
  Moving_vertices_set moving_vertices;
  collect_all_vertices(moving_vertices);
#endif

  std::size_t initial_vertices_nb = moving_vertices.size();
#ifdef CGAL_MESH_3_OPTIMIZER_VERBOSE
  double step_begin = running_time_.time();
  Rcpp::Rcerr << "Running " << Mf::name() << "-smoothing ("
    << initial_vertices_nb << " vertices)" << std::endl;
#endif //CGAL_MESH_3_OPTIMIZER_VERBOSE

  // Initialize big moves (stores the largest moves)
  this->clear_big_moves();
  big_moves_size_ =
    (std::max)(std::size_t(1), std::size_t(moving_vertices.size()/500));

  std::size_t nb_vertices_moved = (std::numeric_limits<size_t>::max)();
  bool convergence_stop = false;

  // Iterate
  int i = -1;
  while ( ++i < nb_iterations && ! is_time_limit_reached() )
  {
    if(!do_freeze_)
      nb_frozen_points_ = 0;
    else
      nb_vertices_moved = moving_vertices.size();

    // Compute move for each vertex
    Moves_vector moves = compute_moves(moving_vertices);
    visitor.after_compute_moves();

    // Pb with Freeze : sometimes a few vertices continue moving indefinitely
    // if the nb of moving vertices is < 1% of total nb AND does not decrease
    if(do_freeze_
      && double(nb_vertices_moved) < 0.005 * double(initial_vertices_nb)
      && nb_vertices_moved == moving_vertices.size())
    {
      // we should stop because we are probably entering an infinite instable loop
      convergence_stop = true;
      break;
    }

    // Stop if time_limit is reached
    if ( is_time_limit_reached() )
      break;

    // Update mesh with those moves
    update_mesh(moves, moving_vertices, visitor);
    visitor.end_of_iteration(i);

#ifdef CGAL_MESH_3_OPTIMIZER_VERBOSE
    std::size_t moving_vertices_size = moving_vertices.size();
    Rcpp::Rcerr << boost::format("\r             \r"
      "end iter.%1%, %2% / %3%, last step:%4$.2fs, step avg:%5$.2fs, avg big moves:%6$.3f ")
    % (i+1)
    % moving_vertices_size
    % initial_vertices_nb
    % (running_time_.time() - step_begin)
    % (running_time_.time() / (i+1))
    % sum_moves_;
    step_begin = running_time_.time();
#endif

    if (do_freeze_ && nb_frozen_points_ == initial_vertices_nb )
      break;

    if(check_convergence())
      break;
  }
#ifdef CGAL_MESH_3_OPTIMIZER_VERBOSE
  Rcpp::Rcerr << std::endl;
#endif

  running_time_.stop();

#ifdef CGAL_MESH_3_PROFILING
  double optim_time = t.elapsed();
# ifdef CGAL_MESH_3_EXPORT_PERFORMANCE_DATA
  CGAL_MESH_3_SET_PERFORMANCE_DATA(std::string(Mf::name()) + "_optim_time", optim_time);
# endif
#endif

#ifdef CGAL_MESH_3_OPTIMIZER_VERBOSE
  if ( do_freeze_ && nb_frozen_points_ == initial_vertices_nb )
    Rcpp::Rcerr << "All vertices frozen" << std::endl;
  else if ( do_freeze_ && convergence_stop )
    Rcpp::Rcerr << "Can't improve anymore" << std::endl;
  else if ( is_time_limit_reached() )
    Rcpp::Rcerr << "Time limit reached" << std::endl;
  else if ( check_convergence() )
    Rcpp::Rcerr << "Convergence reached" << std::endl;
  else if( i >= nb_iterations )
    Rcpp::Rcerr << "Max iteration number reached" << std::endl;

  Rcpp::Rcerr << "Total optimization time: " << running_time_.time()
            << "s" << std::endl << std::endl;
#endif

#ifdef CGAL_MESH_3_PROFILING
  Rcpp::Rcerr << std::endl << "Total optimization 'wall-clock' time: "
            << optim_time << "s" << std::endl;
#endif

  if ( do_freeze_ && nb_frozen_points_ == initial_vertices_nb )
    return ALL_VERTICES_FROZEN;

  else if ( do_freeze_ && convergence_stop )
    return CANT_IMPROVE_ANYMORE;

  else if ( is_time_limit_reached() )
    return TIME_LIMIT_REACHED;

  else if ( check_convergence() )
    return CONVERGENCE_REACHED;

  return MAX_ITERATION_NUMBER_REACHED;
}


template <typename C3T3, typename Md, typename Mf, typename V_>
void
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
collect_all_vertices(Moving_vertices_set& moving_vertices)
{
  typename Tr::Finite_vertices_iterator vit = tr_.finite_vertices_begin();
  for(; vit != tr_.finite_vertices_end(); ++vit )
    moving_vertices.insert(vit);
}

template <typename C3T3, typename Md, typename Mf, typename V_>
typename Mesh_global_optimizer<C3T3,Md,Mf,V_>::Moves_vector
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
compute_moves(Moving_vertices_set& moving_vertices)
{
  // Store new position of points which have to move
  Moves_vector moves;
  moves.reserve(moving_vertices.size());

  // reset worst_move list
  this->clear_big_moves();

#ifdef CGAL_MESH_3_PROFILING
  Rcpp::Rcerr << "Computing moves...";
  WallClockTimer t;
#endif

#ifdef CGAL_LINKED_WITH_TBB
  // Parallel
  if (std::is_convertible<Concurrency_tag, Parallel_tag>::value)
  {
    tbb::concurrent_vector<Vertex_handle> vertices_not_moving_any_more;

    // Get move for each moving vertex
    tbb::parallel_for_each(
          moving_vertices.begin(), moving_vertices.end(),
          Compute_move<Self, Sizing_field, Moves_vector>(
            *this, sizing_field_, moves, do_freeze_, vertices_not_moving_any_more,
            tr_.geom_traits())
          );

    typename tbb::concurrent_vector<Vertex_handle>::const_iterator it
        = vertices_not_moving_any_more.begin();
    typename tbb::concurrent_vector<Vertex_handle>::const_iterator it_end
        = vertices_not_moving_any_more.end();
    for ( ; it != it_end ; ++it)
    {
      moving_vertices.erase(*it);
    }
  }
  // Sequential
  else
#endif // CGAL_LINKED_WITH_TBB
  {
    typename GT::Construct_point_3 cp = tr_.geom_traits().construct_point_3_object();
    typename GT::Construct_translated_point_3 translate = tr_.geom_traits().construct_translated_point_3_object();

    // Get move for each moving vertex
    typename Moving_vertices_set::iterator vit = moving_vertices.begin();
    for ( ; vit != moving_vertices.end() ; )
    {
      Vertex_handle oldv = *vit;
      ++vit;
      Vector_3 move = compute_move(oldv);

      if ( CGAL::NULL_VECTOR != move )
      {
        FT size = 0.;
        if(Sizing_field::is_vertex_update_needed)
        {
          const Weighted_point& position = tr_.point(oldv);
          Bare_point new_position = translate(cp(position), move);
          size = sizing_field_(new_position, oldv);
        }

        moves.push_back({oldv, move, size});
      }
      else // CGAL::NULL_VECTOR == move
      {
        if(do_freeze_)
        {
          // TODO: if non-intrusive, we can optimize since we have the iterator.
          // Don't forget to do "vit = mv.erase(vit)" instead ++vit.
          moving_vertices.erase(oldv);
        }
      }

      // Stop if time_limit_ is reached
      if ( is_time_limit_reached() )
        break;
    }
  }

#ifdef CGAL_MESH_3_PROFILING
  Rcpp::Rcerr << "done in " << t.elapsed() << " seconds." << std::endl;
#endif

  return moves;
}


template <typename C3T3, typename Md, typename Mf, typename V_>
typename Mesh_global_optimizer<C3T3,Md,Mf,V_>::Vector_3
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
compute_move(const Vertex_handle& v)
{
  typename GT::Construct_point_3 cp = tr_.geom_traits().construct_point_3_object();
  typename GT::Compute_squared_length_3 sq_length = tr_.geom_traits().compute_squared_length_3_object();
  typename GT::Construct_translated_point_3 translate = tr_.geom_traits().construct_translated_point_3_object();
  typename GT::Construct_vector_3 vector = tr_.geom_traits().construct_vector_3_object();

  Cell_vector incident_cells;
  incident_cells.reserve(64);
#ifdef CGAL_LINKED_WITH_TBB
  // Parallel
  if (std::is_convertible<Concurrency_tag, Parallel_tag>::value)
  {
    tr_.incident_cells_threadsafe(v, std::back_inserter(incident_cells));
  }
  else
#endif //CGAL_LINKED_WITH_TBB
  {
    tr_.incident_cells(v, std::back_inserter(incident_cells));
  }

  // Get move from move function
  Vector_3 move = move_function_(v, incident_cells, c3t3_, sizing_field_);

  FT local_sq_size = min_circumradius_sq_length(v, incident_cells);
  if ( FT(0) == local_sq_size )
    return CGAL::NULL_VECTOR;

  // Project surface vertex
  if ( c3t3_.in_dimension(v) == 2 )
  {
    const Weighted_point& position = tr_.point(v);
    Bare_point new_position = translate(cp(position), move);
    Bare_point projected_new_position = helper_.project_on_surface(v, new_position);
    move = vector(cp(position), projected_new_position);
  }

  FT local_move_sq_ratio = sq_length(move) / local_sq_size;

  // Move point only if the displacement is big enough w.r.t. the local size
  if ( local_move_sq_ratio < sq_freeze_ratio_ )
  {
    increment_frozen_points();
    return CGAL::NULL_VECTOR;
  }

  // Update big moves
  this->update_big_moves(local_move_sq_ratio);

  return move;
}


template <typename C3T3, typename Md, typename Mf, typename V_>
void
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
update_mesh(const Moves_vector& moves,
            Moving_vertices_set& moving_vertices,
            Visitor& visitor)
{
  // Cells which have to be updated
  Outdated_cell_set outdated_cells;

#ifdef CGAL_MESH_3_PROFILING
  Rcpp::Rcerr << "Moving vertices...";
  WallClockTimer t;
#endif

#ifdef CGAL_LINKED_WITH_TBB
  // Parallel
  if (std::is_convertible<Concurrency_tag, Parallel_tag>::value)
  {
    // Apply moves in triangulation
    tbb::parallel_for(tbb::blocked_range<size_t>(0, moves.size()),
      Move_vertex<
        Self, C3T3_helpers, Tr, Moves_vector,
        Moving_vertices_set, Outdated_cell_set>(
          *this, helper_, moves, moving_vertices, outdated_cells)
      , tbb_task_group_context
    );
  }
  // Sequential
  else
#endif // CGAL_LINKED_WITH_TBB
  {
    // Apply moves in triangulation
    for ( typename Moves_vector::const_iterator it = moves.begin() ;
         it != moves.end() ;
         ++it )
    {
      const Vertex_handle& v = it->vertex_;
      const Vector_3& move = it->move_;
      // Get size at new position
      if ( Sizing_field::is_vertex_update_needed )
      {
        const FT size = it->size_;

#ifdef CGAL_MESH_3_OPTIMIZER_VERY_VERBOSE
        Rcpp::Rcerr << "Moving #" << it - moves.begin()
                  << " addr: " << &*v
                  << " pt: " << tr_.point(v)
                  << " move: " << move << std::endl;
#endif
        // Move point
        Vertex_handle new_v = helper_.move_point(v, move, outdated_cells, moving_vertices);

        // Restore size in meshing_info data
        new_v->set_meshing_info(size);
      }
      else // Move point
      {
        helper_.move_point(v, move, outdated_cells, moving_vertices);
      }

      // Stop if time_limit_ is reached, here we can't return without rebuilding
      // restricted delaunay
      if ( is_time_limit_reached() )
        break;
    }
  }

  visitor.after_move_points();

#ifdef CGAL_MESH_3_PROFILING
  Rcpp::Rcerr << "done in " << t.elapsed() << " seconds." << std::endl;
#endif


#ifdef CGAL_MESH_3_PROFILING
  Rcpp::Rcerr << "Updating C3T3 (rebuilding restricted Delaunay)...";
  t.reset();
#endif

  // Update c3t3
#ifdef CGAL_INTRUSIVE_LIST
  // AF:  rebuild_restricted_delaunay does more cell insertion/removal
  //      which clashes with our inplace list
  //      That's why we hand it in, and call clear() when we no longer need it
  helper_.rebuild_restricted_delaunay(outdated_cells,
                                      moving_vertices);
#else
  helper_.rebuild_restricted_delaunay(outdated_cells.begin(),
                                      outdated_cells.end(),
                                      moving_vertices);
#endif

  visitor.after_rebuild_restricted_delaunay();

#ifdef CGAL_MESH_3_PROFILING
  Rcpp::Rcerr << "Updating C3T3 done in " << t.elapsed() << " seconds." << std::endl;
#endif
}


template <typename C3T3, typename Md, typename Mf, typename V_>
void
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
fill_sizing_field()
{
  std::map<Bare_point,FT> value_map;

#ifdef CGAL_LINKED_WITH_TBB
  // Parallel
  if (std::is_convertible<Concurrency_tag, Parallel_tag>::value)
  {
    typedef tbb::enumerable_thread_specific<
      std::vector< std::pair<Bare_point, FT> > > Local_list;
    Local_list local_lists;

    tbb::parallel_for_each(
      tr_.finite_vertices_begin(), tr_.finite_vertices_end(),
      Compute_sizing_field_value<Self, Local_list>(*this, tr_.geom_traits(), local_lists)
    );

    for(typename Local_list::iterator it_list = local_lists.begin() ;
          it_list != local_lists.end() ;
          ++it_list )
    {
      value_map.insert(it_list->begin(), it_list->end());
    }
  }
  else
#endif //CGAL_LINKED_WITH_TBB
  {
    typename GT::Construct_point_3 cp = tr_.geom_traits().construct_point_3_object();

    // Fill map with local size
    for(typename Tr::Finite_vertices_iterator vit = tr_.finite_vertices_begin();
        vit != tr_.finite_vertices_end();
        ++vit)
    {
      const Weighted_point& position = tr_.point(vit);
      value_map.insert(std::make_pair(cp(position), average_circumradius_length(vit)));
    }
  }

  // Fill the sizing field
  sizing_field_.fill(value_map);
}


template <typename C3T3, typename Md, typename Mf, typename V_>
bool
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
check_convergence() const
{
  FT sum(0);
  for( typename std::multiset<FT>::const_iterator
       it = big_moves_.begin(), end = big_moves_.end() ; it != end ; ++it )
  {
    sum += CGAL::sqrt(*it);
  }

  // Even if set is not full, divide by the max size so that if only 1 point moves,
  // then it goes to 0.
  FT average_move = sum / FT(big_moves_size_);

#ifdef CGAL_MESH_3_OPTIMIZER_VERBOSE
  sum_moves_ = average_move;
#endif

  return ( average_move < convergence_ratio_ );
}


template <typename C3T3, typename Md, typename Mf, typename V_>
typename Mesh_global_optimizer<C3T3,Md,Mf,V_>::FT
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
average_circumradius_length(const Vertex_handle& v) const
{
  Cell_vector incident_cells;
  incident_cells.reserve(64);
#ifdef CGAL_LINKED_WITH_TBB
  // Parallel
  if (std::is_convertible<Concurrency_tag, Parallel_tag>::value)
  {
    tr_.incident_cells_threadsafe(v, std::back_inserter(incident_cells));
  }
  else
#endif //CGAL_LINKED_WITH_TBB
  {
    tr_.incident_cells(v, std::back_inserter(incident_cells));
  }

  FT sum_len (0);
  unsigned int nb = 0;

  for ( typename Cell_vector::iterator cit = incident_cells.begin() ;
       cit != incident_cells.end() ;
       ++cit)
  {
    if ( c3t3_.is_in_complex(*cit) )
    {
      sum_len += CGAL::sqrt(sq_circumradius_length(*cit,v));
      ++nb;
    }
  }

  // nb == 0 could happen if there is an isolated point.
  if ( 0 != nb )
  {
    return sum_len/nb;
  }
  else
  {
    // Use outside cells to compute size of point
    for ( typename Cell_vector::iterator cit = incident_cells.begin() ;
         cit != incident_cells.end() ;
         ++cit)
    {
      if ( !tr_.is_infinite(*cit) )
      {
        sum_len += CGAL::sqrt(sq_circumradius_length(*cit,v));
        ++nb;
      }
    }

    CGAL_assertion(nb!=0);
    CGAL_assertion(sum_len!=0);
    return sum_len/nb;
  }
}


template <typename C3T3, typename Md, typename Mf, typename V_>
typename Mesh_global_optimizer<C3T3,Md,Mf,V_>::FT
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
min_circumradius_sq_length(const Vertex_handle& v, const Cell_vector& incident_cells) const
{
  // Get first cell sq_circumradius_length
  typename Cell_vector::const_iterator cit = incident_cells.begin();
  while ( incident_cells.end() != cit && !c3t3_.is_in_complex(*cit) ) { ++cit; }

  // if vertex is isolated ...
  if ( incident_cells.end() == cit )
    return FT(0);

  // Initialize min
  FT min_sq_len = sq_circumradius_length(*cit++,v);

  // Find the minimum value
  for ( ; cit != incident_cells.end() ; ++cit )
  {
    if ( !c3t3_.is_in_complex(*cit) )
      continue;

    min_sq_len = (std::min)(min_sq_len,sq_circumradius_length(*cit,v));
  }

  return min_sq_len;
}


template <typename C3T3, typename Md, typename Mf, typename V_>
typename Mesh_global_optimizer<C3T3,Md,Mf,V_>::FT
Mesh_global_optimizer<C3T3,Md,Mf,V_>::
sq_circumradius_length(const Cell_handle& cell, const Vertex_handle& v) const
{
  typename GT::Construct_point_3 cp = tr_.geom_traits().construct_point_3_object();
  typename GT::Compute_squared_distance_3 sq_distance = tr_.geom_traits().compute_squared_distance_3_object();

  const Bare_point circumcenter = tr_.dual(cell);
  const Weighted_point& position = tr_.point(cell, cell->index(v));

  return ( sq_distance(cp(position), circumcenter) );
}

} // end namespace Mesh_3

} //namespace CGAL

#include <CGAL/enable_warnings.h>

#endif // CGAL_MESH_3_MESH_GLOBAL_OPTIMIZER_H

