// 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/v5.1/Mesh_3/include/CGAL/Mesh_3/Sizing_grid.h $
// $Id: Sizing_grid.h e9d41d7 2020-04-21T10:03:00+02:00 Maxime Gimeno
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Stephane Tayeb, Jane Tournois, Camille Wormser, Pierre Alliez
//
//******************************************************************************
// File Description :
//******************************************************************************

#ifndef CGAL_MESH_3_SIZING_GRID_H
#define CGAL_MESH_3_SIZING_GRID_H

#include <CGAL/license/Mesh_3.h>


#include <CGAL/Mesh_3/config.h>

#include <CGAL/basic.h>
#include <queue>

namespace CGAL {

#define CGAL_MESH_3_INFINITE_SIZE 1e30

namespace Mesh_3 {

template <class Gt>
class Sizing_grid_node
{
public:
  typedef typename Gt::Point_3 Point;
  typedef typename Gt::FT FT;

  FT m_init_size;
  FT m_size;
  FT m_distance;
  bool m_done;
  Point m_point;
  int m_indices[3];
  Sizing_grid_node* m_pRef_node;

public:
  Sizing_grid_node()
  {
    m_done = false;
    m_size = CGAL_MESH_3_INFINITE_SIZE;
    m_pRef_node = nullptr;
    m_indices[0] = m_indices[1] = m_indices[2] = 0;
  }

  ~Sizing_grid_node() {}

  const Point& point() const { return m_point; }
  Point& point() { return m_point; }

  Sizing_grid_node* ref_node() { return m_pRef_node; }
  void ref_node(Sizing_grid_node* pRef_node) { m_pRef_node = pRef_node; }

  bool& done() { return m_done; }
  const bool& done() const { return m_done; }

  FT& size() { return m_size; }
  const FT& size() const { return m_size; }

  FT& init_size() { return m_init_size; }
  const FT& init_size() const { return m_init_size; }

  const int& i() const { return m_indices[0]; }
  const int& j() const { return m_indices[1]; }
  const int& k() const { return m_indices[2]; }
  int& i() { return m_indices[0]; }
  int& j() { return m_indices[1]; }
  int& k() { return m_indices[2]; }

  void indices(const int i,
               const int j,
               const int k)
  {
    m_indices[0] = i;
    m_indices[1] = j;
    m_indices[2] = k;
  }
};

// functor for priority queue
template<class c>
struct less_candidate_size
{
bool operator()(const c& c1,
                const c& c2) const
{
  return c1.size() > c2.size();
}
};





template <class Tr>
class Sizing_grid
{
public:
  typedef typename Tr::Geom_traits Gt;
  typedef typename Gt::FT FT;

  typedef typename Tr::Weighted_point Weighted_point;
  typedef typename Tr::Bare_point Bare_point;

  typedef typename Gt::Vector_3 Vector;
  typedef Sizing_grid_node<Gt> Node;
  typedef typename std::pair<Bare_point, FT> Constraint;

private:
  // grid
  Node ***m_pppNodes;
  FT m_k;
  FT m_ds;
  FT m_dv;
  FT m_xrange[3];
  FT m_yrange[3];
  FT m_zrange[3];
  unsigned int m_nx,m_ny,m_nz;
  FT m_max_size;
  bool m_updated;
  std::list<Constraint> m_constraints;

  class Candidate_size
  {
  private:
    FT m_size;
    Node* m_pNode;
    Node* m_pRef_node;

  public:

    Candidate_size(Node* pNode,
                   Node* pRef_node,
                   const FT k)
    {
      m_pNode = pNode;
      m_pRef_node = pRef_node;

      // size = K d(node,v) + init_size(v)
      const Bare_point& p1 = pNode->point();
      const Bare_point& p2 = m_pRef_node->point();
      FT distance = (FT)std::sqrt(CGAL_NTS to_double(CGAL::squared_distance(p1,p2)));
      m_size = k * distance + m_pRef_node->init_size();
    }

    ~Candidate_size() {}

  public:
    Candidate_size(const Candidate_size& c)
    {
      m_pNode = c.node();
      m_pRef_node = c.ref_node();
      m_size = c.size();
    }

    Node* node() const { return m_pNode; }
    Node* ref_node() const { return m_pRef_node; }

    FT& size() { return m_size; }
    const FT& size() const { return m_size; }
  };


  typedef typename std::priority_queue<Candidate_size,
    std::vector<Candidate_size>,
    less_candidate_size<Candidate_size> > PQueue;


public:
  Sizing_grid(const Tr& tr)
  {
    m_k = 1.0;
    m_ds = m_dv = 0;
    m_pppNodes = nullptr;
    m_nx = m_ny = m_nz = 0;
    m_xrange[0] = m_xrange[1] = m_xrange[2] = 0;
    m_yrange[0] = m_yrange[1] = m_yrange[2] = 0;
    m_zrange[0] = m_zrange[1] = m_zrange[2] = 0;
    m_updated = false;

    // Build grid
    unsigned int nb_nodes = tr.number_of_vertices()*27;

    Bbox_3 tr_bbox;
    for ( typename Tr::Finite_cells_iterator cit = tr.finite_cells_begin() ;
         cit != tr.finite_cells_end() ;
         ++cit )
    {
      tr_bbox = tr_bbox + tr.tetrahedron(cit).bbox();
    }

    init(tr_bbox.xmin(), tr_bbox.xmax(),
         tr_bbox.ymin(), tr_bbox.ymax(),
         tr_bbox.zmin(), tr_bbox.zmax(),
         nb_nodes);
  }

  ~Sizing_grid()
  {
    cleanup();
  }

  void fill(const std::map<Weighted_point,FT>& value_map)
  {
    for ( typename std::map<Weighted_point,FT>::const_iterator it = value_map.begin() ;
         it != value_map.end() ;
         ++it )
    {
      add_constraint(it->first, it->second);
    }

    update();
  }

  FT operator()(const Point& query) const
  {
    return size_trilinear(query);
  }

private:

  FT& k() { return m_k; }
  const FT& k() const { return m_k; }

  void cleanup()
  {
    m_constraints.clear();
    unsigned int i,j;
    for(i=0; i<m_nx; i++)
    {
      for(j=0; j<m_ny; j++)
        delete [] m_pppNodes[i][j];
      delete [] m_pppNodes[i];
    }
    delete [] m_pppNodes;
    m_pppNodes = nullptr;
    m_nx = m_ny = m_nz = 0;
    m_updated = false;
  }

  bool alloc(const unsigned int nx,
             const unsigned int ny,
             const unsigned int nz)
  {
    // cleanup
    cleanup();

    // alloc
    m_pppNodes = new Node**[nx];
    if(m_pppNodes == nullptr)
    {
      cleanup();
      return false;
    }
    unsigned int i,j;
    for(i=0; i<nx; i++)
    {
      m_pppNodes[i] = new Node*[ny];
      if(m_pppNodes[i] == nullptr)
      {
        cleanup();
        return false;
      }
      for(j=0; j<ny; j++)
      {
        m_pppNodes[i][j] = new Node[nz];
        if(m_pppNodes[i][j] == nullptr)
        {
          cleanup();
          return false;
        }
      }
    }
    m_nx = nx;
    m_ny = ny;
    m_nz = nz;
    return true;
  }



  // trilinear interpolation of size
  // https://en.wikipedia.org/wiki/Trilinear_interpolation
  FT size_trilinear(const Point& query) const
  {
    FT px = query.x();
    if ( px < m_xrange[0] )
      px = m_xrange[0];
    if ( px >= m_xrange[1] )
      px = m_xrange[1]-m_ds;

    FT py = query.y();
    if ( py < m_yrange[0] )
      py = m_yrange[0];
    if ( py >= m_yrange[1] )
      py = m_yrange[1]-m_ds;

    FT pz = query.z();
    if ( pz < m_zrange[0] )
      pz = m_zrange[0];
    if ( pz >= m_zrange[1] )
      pz = m_zrange[1]-m_ds;

    CGAL_assertion(px >= m_xrange[0] && px < m_xrange[1] &&
                   py >= m_yrange[0] && py < m_yrange[1] &&
                   pz >= m_zrange[0] && pz < m_zrange[1]);

    FT x = ((px-m_xrange[0])/m_xrange[2]*(FT)m_nx);
    FT y = ((py-m_yrange[0])/m_yrange[2]*(FT)m_ny);
    FT z = ((pz-m_zrange[0])/m_zrange[2]*(FT)m_nz);
    int xl = (int)floor(x);
    int yl = (int)floor(y);
    int zl = (int)floor(z);
    int xh = (int)ceil(x);
    int yh = (int)ceil(y);
    int zh = (int)ceil(z);
    FT xd = x - xl;
    FT yd = y - yl;
    FT zd = z - zl;
    CGAL_assertion(xd >= 0.0 && xd <= 1.0);
    CGAL_assertion(yd >= 0.0 && yd <= 1.0);
    CGAL_assertion(zd >= 0.0 && zd <= 1.0);
    FT i1 = node(xl,yl,zl)->size() * (1.0 - zd) + node(xl,yl,zh)->size() * zd;
    FT i2 = node(xl,yh,zl)->size() * (1.0 - zd) + node(xl,yh,zh)->size() * zd;
    FT j1 = node(xh,yl,zl)->size() * (1.0 - zd) + node(xh,yl,zh)->size() * zd;
    FT j2 = node(xh,yh,zl)->size() * (1.0 - zd) + node(xh,yh,zh)->size() * zd;
    FT w1 = i1 * (1.0 - yd) + i2 * yd;
    FT w2 = j1 * (1.0 - yd) + j2 * yd;
    return w1 * (1.0 - xd) + w2 * xd;
  }

  Node *node(const Point& query) const
  {
    const FT x = query.x();
    const FT y = query.y();
    const FT z = query.z();
    if(x >= m_xrange[0] && x < m_xrange[1] &&
       y >= m_yrange[0] && y < m_yrange[1] &&
       z >= m_zrange[0] && z < m_zrange[1])
    {
      int i = (int)((x-m_xrange[0])/m_xrange[2]*(FT)m_nx);
      int j = (int)((y-m_yrange[0])/m_yrange[2]*(FT)m_ny);
      int k = (int)((z-m_zrange[0])/m_zrange[2]*(FT)m_nz);
      return node(i,j,k);
    }
    return nullptr;
  }

  Node* node(const int i,
             const int j,
             const int k) const
  {
    if(m_pppNodes == nullptr)
      return nullptr;

    if(i < 0 ||
       j < 0 ||
       k < 0 ||
       i >= (int)m_nx ||
       j >= (int)m_ny ||
       k >= (int)m_nz)
      return nullptr;

    return &m_pppNodes[i][j][k];
  }

  Node* neighbor(Node* n,
                 unsigned int index)
  {
    if(n == nullptr)
      return nullptr;

    switch(index)
    {
      case 0:
        return node(n->i()-1,n->j(),n->k());
      case 1:
        return node(n->i()+1,n->j(),n->k());
      case 2:
        return node(n->i(),n->j()-1,n->k());
      case 3:
        return node(n->i(),n->j()+1,n->k());
      case 4:
        return node(n->i(),n->j(),n->k()-1);
      case 5:
        return node(n->i(),n->j(),n->k()+1);
      default:
        return nullptr;
    }
  }

  bool init(const FT xmin,
            const FT xmax,
            const FT ymin,
            const FT ymax,
            const FT zmin,
            const FT zmax,
            const unsigned int nb_samples,
            const FT ratio = 1.3)
  {
    reset();
    if(!init_range_and_alloc(xmin,xmax,ymin,ymax,zmin,zmax,nb_samples,ratio))
      return false;
    init_positions_and_indices();
    return true;
  }

  static FT len(const Vector &v)
  {
    return (FT)std::sqrt(CGAL_NTS to_double(v*v));
  }
  static FT sqlen(const Vector &v)
  {
    return v*v;
  }

  void flood(PQueue& priority_queue)
  {
    m_max_size = 0.0;
    // flood using priority queue
    while(!priority_queue.empty())
    {
      // pop candidate out of the queue
      Candidate_size candidate = priority_queue.top();
      priority_queue.pop();

      Node* pNode = candidate.node();
      if(pNode->done() == true)
        continue;

      pNode->ref_node(candidate.ref_node());
      pNode->size() = candidate.size();
      pNode->done() = true;
      m_max_size = (std::max)(m_max_size,pNode->size());

      // explore neighbors
      for(unsigned int index_neighbor = 0;
          index_neighbor < 6;
          index_neighbor++)
      {
        // TODO: change size of seeds
        Node *pNeighbor = neighbor(pNode,index_neighbor);
        if(pNeighbor != nullptr &&
           pNeighbor->done() == false)
          priority_queue.push(Candidate_size(pNeighbor,pNode->ref_node(),m_k));
      }
    }
  }

  void init_pqueue(PQueue& priority_queue)
  {
    // insert sizing constraints and init priority queue
    typename std::list<Constraint>::iterator it;
    for(it = m_constraints.begin();
        it != m_constraints.end();
        it++)
    {
      const Point& p = (*it).first;
      const FT init_size = (*it).second;

      // get node at position p
      Node *pNode = node(p);
      if(pNode != nullptr && pNode->done() == false) // specific
      {
        pNode->point() = p;
        pNode->init_size() = init_size;
        pNode->done() = true;
        pNode->size() = init_size;
        pNode->ref_node(pNode);

        // insert all valid neighbors to the priority queue
        for(unsigned int index_neighbor = 0;
            index_neighbor < 6;
            index_neighbor++)
        {
          Node *pNeighbor = neighbor(pNode,index_neighbor);
          if(pNeighbor != nullptr &&
             pNeighbor->done() == false)
          {
            Candidate_size candidate(pNeighbor,pNode->ref_node(),m_k);
            priority_queue.push(candidate);
          }
        }
      }
    }
  }

  bool init_range_and_alloc(const FT xmin,
                            const FT xmax,
                            const FT ymin,
                            const FT ymax,
                            const FT zmin,
                            const FT zmax,
                            const unsigned int nb_samples,
                            const FT ratio = 1.1)
  {
    m_xrange[2] = ratio*(xmax - xmin);
    m_yrange[2] = ratio*(ymax - ymin);
    m_zrange[2] = ratio*(zmax - zmin);

    FT xmid = 0.5*(xmin+xmax);
    FT ymid = 0.5*(ymin+ymax);
    FT zmid = 0.5*(zmin+zmax);

    m_xrange[0] = xmid - 0.5*m_xrange[2];
    m_yrange[0] = ymid - 0.5*m_yrange[2];
    m_zrange[0] = zmid - 0.5*m_zrange[2];

    m_xrange[1] = xmid + 0.5*m_xrange[2];
    m_yrange[1] = ymid + 0.5*m_yrange[2];
    m_zrange[1] = zmid + 0.5*m_zrange[2];

    if(m_xrange[2] == 0.0 ||
       m_yrange[2] == 0.0 ||
       m_zrange[2] == 0.0)
      return false;

    // deduce nx, ny, nz
    FT volume = m_xrange[2] * m_yrange[2] * m_zrange[2];
    m_dv = volume / (FT)nb_samples;
    m_ds = std::pow(m_dv,1.0/3.0);

    unsigned nx = (unsigned)(m_xrange[2] / m_ds);
    unsigned ny = (unsigned)(m_yrange[2] / m_ds);
    unsigned nz = (unsigned)(m_zrange[2] / m_ds);

    // alloc (and set m_nx.. variables)
    if(!alloc(nx,ny,nz))
      return false;
    return true;
  }

  void init_positions_and_indices()
  {
    // init positions and tags
    FT x = m_xrange[0] + m_ds/2;
    for(int i=0;i<(signed)m_nx;i++)
    {
      FT y = m_yrange[0] + m_ds/2;
      for(int j=0;j<(signed)m_ny;j++)
      {
        FT z = m_zrange[0] + m_ds/2;
        for(int k=0;k<(signed)m_nz;k++)
        {
          Node* pNode = &m_pppNodes[i][j][k];
          pNode->indices(i,j,k);
          pNode->point() = Point(x,y,z);
          z += m_ds;
        }
        y += m_ds;
      }
      x += m_ds;
    }
  }

  void tag_done(const bool done)
  {
    for(unsigned int i=0;i<m_nx;i++)
      for(unsigned int j=0;j<m_ny;j++)
        for(unsigned int k=0;k<m_nz;k++)
          m_pppNodes[i][j][k].done() = done;
  }

  // total reset
  void reset(FT k)
  {
    m_k = k;
    cleanup();
  }

  // reset where m_k is kept
  void reset()
  {
    cleanup();
  }

  void set_k(const FT k)
  {
    if(k != m_k)
    {
      m_k = k;
      update();
    }
  }

  void add_constraint(const Point& point,
                      const FT& size)
  {
    Constraint constraint(point,size);
    m_constraints.push_back(constraint);
  }

  // update sizing grid
  void update()
  {
    tag_done(false);
    PQueue priority_queue;
    init_pqueue(priority_queue);
    flood(priority_queue);
    m_updated = true;
  }
};

}// end namespace Mesh_3


} //namespace CGAL

#endif // CGAL_MESH_3_SIZING_GRID_H