#include <Rcpp.h>
// Copyright (c) 2005  INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL: https://github.com/CGAL/cgal/blob/v6.0.1/Interpolation/include/CGAL/natural_neighbor_coordinates_3.h $
// $Id: include/CGAL/natural_neighbor_coordinates_3.h 50cfbde3b84 $
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Raphaelle Chaine

#ifndef CGAL_NATURAL_NEIGHBORS_3_H
#define CGAL_NATURAL_NEIGHBORS_3_H

#include <CGAL/license/Interpolation.h>

#include <CGAL/tags.h>
#include <CGAL/iterator.h>
#include <CGAL/utility.h>
#include <CGAL/assertions.h>
#include <CGAL/number_utils.h>

#include <algorithm>
#include <iostream> //TO DO : to remove
#include <map>
#include <set>
#include <utility>
#include <vector>

namespace CGAL {

// ====================== Geometric Traits utilities =========================================
// === Declarations

template <class Gt>
typename Gt::FT
signed_area(const typename Gt::Point_3& p, const typename Gt::Point_3& q,
            const typename Gt::Point_3& r, const typename Gt::Point_3& point_of_view,
            const Gt& gt = Gt());

// ====================== Delaunay Triangulation utilities ==========================
// === Declarations

template < class DT>
typename DT::Geom_traits::Point_3
construct_circumcenter(const typename DT::Facet& f,
                       const typename DT::Geom_traits::Point_3& Q,
                       const typename DT::Geom_traits& gt = typename DT::Geom_traits());

// ====================== Natural Neighbors Queries ==========================
// === Definitions

// Given a 3D point Q and a 3D Delaunay triangulation dt,
// the next two functions calculate the natural neighbors and coordinates of Q with regard of dt
//
// OutputIterator has value type
//        std::pair<Dt::Vertex_handle, Dt::Geom_traits::FT>
// Result :
// - An OutputIterator providing natural neighbors P_i of Q with unnormalized coordinates a_i associated to them
// - The normalizing coefficient (sum over i of the a_i)
// - A boolean specifying whether the calculation has succeeded or not

template <class Dt, class OutputIterator>
Triple< OutputIterator,  // iterator with value type std::pair<Dt::Vertex_handle, Dt::Geom_traits::FT>
        typename Dt::Geom_traits::FT,  // Should provide 0 and 1
        bool >
laplace_natural_neighbor_coordinates_3(const Dt& dt,
                                       const typename Dt::Geom_traits::Point_3& Q,
                                       OutputIterator nn_out,
                                       typename Dt::Geom_traits::FT&  norm_coeff,
                                       const typename Dt::Cell_handle start = CGAL_TYPENAME_DEFAULT_ARG Dt::Cell_handle())
{
  typedef typename Dt::Geom_traits Gt;
  typedef typename Gt::Point_3 Point;
  typedef typename Dt::Cell_handle Cell_handle;
  typedef typename Dt::Vertex_handle Vertex_handle;
  typedef typename Dt::Facet Facet;
  typedef typename Dt::Locate_type Locate_type;
  typedef typename Gt::FT Coord_type;

  CGAL_precondition (dt.dimension() == 3);

  Locate_type lt;
  int li, lj;
  Cell_handle c = dt.locate( Q, lt, li, lj, start);

  if ( lt == Dt::VERTEX )
  {
    *nn_out++= std::make_pair(c->vertex(li), Coord_type(1));
    return make_triple(nn_out, norm_coeff = Coord_type(1),true);
  }
  else if (dt.is_infinite(c))
  {
    //point outside the convex-hull
    return make_triple(nn_out, Coord_type(1), false);
  }

  std::set<Cell_handle> cells;
  // To replace the forbidden access to the "in conflict" flag :
  // std::find operations on this set
  std::vector<Facet> bound_facets;
  bound_facets.reserve(32);

  // Find the cells in conflict with Q
  dt.find_conflicts(Q, c,
                    std::back_inserter(bound_facets),
                    std::inserter(cells, cells.begin()));

  std::map<Vertex_handle,Coord_type> coordinate;
  typename std::map<Vertex_handle,Coord_type>::iterator coor_it;

  typename std::vector<Facet>::iterator bound_it;
  for (bound_it = bound_facets.begin(); bound_it != bound_facets.end(); ++bound_it)
  {
    //for each facet on the boundary
    Facet f1 = *bound_it;
    Cell_handle cc1 = f1.first;
    if (dt.is_infinite(cc1))
      return make_triple(nn_out, norm_coeff=Coord_type(1), false);//point outside the convex-hull

    CGAL_assertion_code(Cell_handle cc2 = cc1->neighbor(f1.second);)
    CGAL_assertion(std::find(cells.begin(),cells.end(),cc1) != cells.end());//TODO : Delete
    CGAL_assertion(std::find(cells.begin(),cells.end(),cc2) == cells.end());//TODO : Delete

    Point C_1 = construct_circumcenter<Dt>(f1, Q, dt.geom_traits());
    for(int j=1; j<4; j++)
    {
      //for each vertex P of the boundary facet
      Vertex_handle vP = cc1->vertex((f1.second+j)&3);
      Vertex_handle vR = cc1->vertex(dt.next_around_edge(f1.second,(f1.second+j)&3));

      // turn around the oriented edge vR vP
      Cell_handle cc3 = cc1;
      int num_next = dt.next_around_edge((f1.second+j)&3,f1.second);

      Cell_handle next = cc3->neighbor(num_next);
      while (std::find(cells.begin(),cells.end(),next) != cells.end())
      {
        CGAL_assertion( next != cc1 );
        cc3 = next;
        num_next = dt.next_around_edge(cc3->index(vR),cc3->index(vP));
        next = cc3->neighbor(num_next);
      }

      Point C_3 = construct_circumcenter<Dt>(Facet(cc3,num_next), Q, dt.geom_traits());
      Point midPQ = midpoint(vP->point(),Q);
      Coord_type coor_add = signed_area<Gt>(C_3,C_1,midPQ, vP->point(), dt.geom_traits());
      ((coor_it = coordinate.find(vP)) == coordinate.end())?
            coordinate[vP] = coor_add : coor_it->second += coor_add; // Replace by a function call
    }
  } //end : for each facet on the boundary

  norm_coeff = 0;
  for (coor_it=coordinate.begin(); coor_it!=coordinate.end(); ++coor_it)
  {
    Coord_type co = coor_it->second /
      (CGAL_NTS sqrt(dt.geom_traits().compute_squared_distance_3_object()(
                                        coor_it->first->point(),Q)));
    *nn_out++ = std::make_pair(coor_it->first,co);
    norm_coeff += co;
  }
  return make_triple(nn_out, norm_coeff, true);
}

template <class Dt, class OutputIterator>
Triple< OutputIterator,  // iterator with value type std::pair<Dt::Vertex_handle, Dt::Geom_traits::FT>
        typename Dt::Geom_traits::FT,  // Should provide 0 and 1
        bool >
sibson_natural_neighbor_coordinates_3(const Dt& dt,
                                      const typename Dt::Geom_traits::Point_3& Q,
                                      OutputIterator nn_out,
                                      typename Dt::Geom_traits::FT&  norm_coeff,
                                      const typename Dt::Cell_handle start = CGAL_TYPENAME_DEFAULT_ARG Dt::Cell_handle())
{
  typedef typename Dt::Geom_traits Gt;
  typedef typename Gt::Point_3 Point;
  typedef typename Dt::Cell_handle Cell_handle;
  typedef typename Dt::Vertex_handle Vertex_handle;
  typedef typename Dt::Facet Facet;
  typedef typename Dt::Locate_type Locate_type;
  typedef typename Gt::FT Coord_type;

  CGAL_precondition (dt.dimension()== 3);

  Locate_type lt;
  int li, lj;
  Cell_handle c = dt.locate( Q, lt, li, lj, start);

  if ( lt == Dt::VERTEX )
  {
    *nn_out++ = std::make_pair(c->vertex(li),Coord_type(1));
    return make_triple(nn_out,norm_coeff=Coord_type(1),true);
  }
  else if (dt.is_infinite(c))
  {
    //point outside the convex-hull
    return make_triple(nn_out, Coord_type(1), false);
  }

  std::set<Cell_handle> cells;
  typename std::set<Cell_handle>::iterator cit;
  // To replace the forbidden access to the "in conflict" flag :
  // std::find operations on this set

  // Find the cells in conflict with Q
  dt.find_conflicts(Q, c,
                    Emptyset_iterator(),
                    std::inserter(cells,cells.begin()));

  std::map<Vertex_handle,Coord_type> coordinate;
  typename std::map<Vertex_handle,Coord_type>::iterator coor_it;

  for (cit = cells.begin(); cit != cells.end(); ++cit)
  {
    // for each cell cc1 in conflict
    Cell_handle cc1 = *cit;
    CGAL_assertion(std::find(cells.begin(),cells.end(),cc1)!=cells.end());//TODO : Delete

    if (dt.is_infinite(cc1))
      return make_triple(nn_out,norm_coeff=Coord_type(1), false);//point outside the convex-hull

    typename Dt::Geom_traits::Compute_volume_3 vol =
        dt.geom_traits().compute_volume_3_object();

    Point C1 = dt.dual(cc1);
    for(int i=0; i<4; i++)
    {
      //for each neighboring cell cc2 of cc1
      Cell_handle cc2 = cc1->neighbor(i);
      if(std::find(cells.begin(),cells.end(),cc2) == cells.end())
      {
        // cc2 outside the conflict cavity
        Point C_1 = construct_circumcenter<Dt>(Facet(cc1,i), Q, dt.geom_traits());
        for(int j=1; j<4; j++)
        {
          //for each vertex P of the boundary facet
          Vertex_handle vP = cc1->vertex((i+j)&3);//&3 in place of %4
          Vertex_handle vR = cc1->vertex(dt.next_around_edge(i,(i+j)&3));

          // turn around the oriented edge vR vP
          Cell_handle cc3 = cc1;
          int num_next = dt.next_around_edge((i+j)&3,i);
          Cell_handle next = cc3->neighbor(num_next);

          while (std::find(cells.begin(),cells.end(),next) != cells.end())
          { //next is in conflict
            CGAL_assertion( next != cc1 );
            cc3 = next;
            num_next = dt.next_around_edge(cc3->index(vR),cc3->index(vP));
            next = cc3->neighbor(num_next);
          }
          if (dt.is_infinite(cc3))
          {
            //point outside the convex-hull
            return make_triple(nn_out,norm_coeff = Coord_type(1), false);
          }

          Point C3 = dt.dual(cc3);
          Point C_3 = construct_circumcenter<Dt>(Facet(cc3,num_next), Q, dt.geom_traits());
          Point midPQ = midpoint(vP->point(),Q);
          Point midPR = midpoint(vP->point(),vR->point());
          Coord_type coor_add = vol(C_1,C1,midPR,midPQ);
          coor_add -= vol(C_1,C_3,midPR,midPQ);
          coor_add += vol(C3,C_3,midPR,midPQ);
          ((coor_it = coordinate.find(vP)) == coordinate.end())?
                coordinate[vP] = coor_add : coor_it->second += coor_add;// Replace by a function call
        }
      }
      else // cc2 in the conflict cavity
      {
        CGAL_assertion(std::find(cells.begin(),cells.end(),cc2)!=cells.end());//TODO : Delete
        if (dt.is_infinite(cc2))
        {
          //point outside the convex-hull
          return make_triple(nn_out,norm_coeff = Coord_type(1), false);
        }

        Point C2 = dt.dual(cc2);
        for(int j=1;j<4;j++)
        {
          //for each vertex P of the internal facet
          Vertex_handle vP=cc1->vertex((i+j)&3);
          Vertex_handle vR=cc1->vertex(dt.next_around_edge(i,(i+j)&3));
          Point midPQ = midpoint(vP->point(),Q);
          Point midPR = midpoint(vP->point(),vR->point());
          Coord_type coor_add = vol(C2,C1,midPR,midPQ);
          ((coor_it=coordinate.find(vP))==coordinate.end())?
                coordinate[vP]=coor_add : coor_it->second+=coor_add;// Replace by a function call
        }
      }
    }
  }

  norm_coeff=0;
  for (coor_it=coordinate.begin(); coor_it!=coordinate.end(); ++coor_it)
  {
    *nn_out++ = std::make_pair(coor_it->first,coor_it->second);
    norm_coeff += coor_it->second;
  }
  return make_triple(nn_out,norm_coeff,true);
}

template <typename Dt, typename InputIterator>
bool is_correct_natural_neighborhood(const Dt& /*dt*/,
                                     const typename Dt::Geom_traits::Point_3&  Q,
                                     InputIterator it_begin, InputIterator it_end,
                                     const typename Dt::Geom_traits::FT&  norm_coeff)
{
  typedef typename Dt::Geom_traits Gt;
  typedef typename Gt::FT Coord_type;
  Coord_type sum_x(0);
  Coord_type sum_y(0);
  Coord_type sum_z(0);
  InputIterator it;
  for(it = it_begin ; it != it_end ; ++it)
  {
    sum_x += it->second*(it->first->point().x());
    sum_y += it->second*(it->first->point().y());
    sum_z += it->second*(it->first->point().z());
  }
  //!!!! to be replaced by a linear combination of points as soon
  // as it is available in the kernel.
  Rcpp::Rcout << sum_x/norm_coeff << " "
            << sum_y/norm_coeff << " "
            << sum_z/norm_coeff << std::endl;
  return ((sum_x == norm_coeff*Q.x()) && (sum_y == norm_coeff*Q.y())
          && (sum_z == norm_coeff*Q.z()));
}

// ====================== Geometric Traits utilities =========================================
// === Definitions

template <class Gt>
typename Gt::FT
signed_area(const typename Gt::Point_3& p, const typename Gt::Point_3& q,
            const typename Gt::Point_3& r,
            const typename Gt::Point_3& point_of_view,
            const Gt& gt /* = Gt() */)
{
  // signed area of the triangle p q r
  return gt.compute_area_3_object()(p,q,r)
      * (gt.orientation_3_object()(p, q, r, point_of_view) == COUNTERCLOCKWISE?+1:-1);
}

// ====================== Delaunay Triangulation utilities ==========================
// === Definitions

template < class DT>
typename DT::Geom_traits::Point_3
construct_circumcenter(const typename DT::Facet& f,
                       const typename DT::Geom_traits::Point_3& Q,
                       const typename DT::Geom_traits& gt /* = typename DT::Geom_traits() */ )
{
  CGAL_precondition(//&3 in place of %4
                    !gt.coplanar_3_object()(
                                            f.first->vertex((f.second+1)&3)->point(),
                                            f.first->vertex((f.second+2)&3)->point(),
                                            f.first->vertex((f.second+3)&3)->point(),
                                            Q));
  // else the facet is not on the envelope of the conflict cavity associated to P
  return gt.construct_circumcenter_3_object()(
             f.first->vertex((f.second+1)&3)->point(),
             f.first->vertex((f.second+2)&3)->point(),
             f.first->vertex((f.second+3)&3)->point(),
             Q);
}

} //namespace CGAL

#endif // CGAL_NATURAL_NEIGHBORS_3_H

