// Copyright (c) 2009-2010 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL: https://github.com/CGAL/cgal/blob/v6.2-beta1/Mesh_3/include/CGAL/Mesh_3/internal/Polyline.h $
// $Id: include/CGAL/Mesh_3/internal/Polyline.h 431cfb5357d $
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Stéphane Tayeb, Laurent Rineau
//
//******************************************************************************
// File Description :
//
//******************************************************************************

#ifndef CGAL_MESH_3_INTERNAL_POLYLINE_H
#define CGAL_MESH_3_INTERNAL_POLYLINE_H

#include <CGAL/license/Mesh_3.h>

#include <CGAL/enum.h>
#include <CGAL/iterator.h>
#include <CGAL/number_utils.h>

#include <iostream>
#include <optional>
#include <utility>
#include <vector>

namespace CGAL {

/// @cond DEVELOPERS
namespace Mesh_3 {
namespace internal {

template <typename Point_3, typename FT>
struct Polyline_vertex {
  Point_3 point;
  mutable std::optional<FT> length_from_start{std::nullopt};
  Polyline_vertex(const Point_3& p) : point(p) {}
  Polyline_vertex(const Point_3& p, FT length) : point(p), length_from_start(length) {}

  operator const Point_3&() const { return point; }

  friend std::ostream& operator<<(std::ostream& os, const Polyline_vertex& pv)
  {
    os << pv.point;
    return os;
  }

  friend bool operator==(const Polyline_vertex& pv1, const Polyline_vertex& pv2)
  {
    return pv1.point == pv2.point;
  }

  friend bool operator!=(const Polyline_vertex& pv1, const Polyline_vertex& pv2)
  {
    return !(pv1 == pv2);
  }

  friend bool operator==(const Polyline_vertex& pv, const Point_3& p)
  {
    return pv.point == p;
  }

  friend bool operator==(const Point_3& p, const Polyline_vertex& pv)
  {
    return pv.point == p;
  }

  friend bool operator!=(const Polyline_vertex& pv, const Point_3& p)
  {
    return !(pv == p);
  }

  friend bool operator!=(const Point_3& p, const Polyline_vertex& pv)
  {
    return !(pv == p);
  }
};

template <typename Kernel>
class Polyline
{
  typedef typename Kernel::Point_3  Point_3;
  typedef typename Kernel::Segment_3 Segment_3;
  typedef typename Kernel::FT       FT;

  typedef Polyline_vertex<Point_3, FT>   Vertex;
  typedef std::vector<Vertex>      Data;

public:
  typedef typename Data::const_iterator const_iterator;
  typedef std::pair<Point_3, const_iterator> Point_and_position;

  Polyline() {}
  ~Polyline() {}

  /// adds a point at the end of the polyline
  void add_point(const Point_3& p)
  {
    if( points_.empty() || p != end_point() ) {
      points_.push_back(p);
    }
  }

  /// returns the starting point of the polyline
  const Point_3& start_point() const
  {
    CGAL_assertion( ! points_.empty() );
    return points_.front();
  }

  /// returns the ending point of the polyline
  const Point_3& end_point() const
  {
    CGAL_assertion( ! points_.empty() );
    return points_.back();
  }

  /// returns `true` if the polyline is not degenerate
  bool is_valid() const
  {
    return points_.size() > 1;
  }

  /// returns `true` if polyline is a loop
  bool is_loop() const
  {
    return start_point() == end_point();
  }

  const_iterator next(const_iterator it, Orientation orientation) const {
    if(orientation == POSITIVE) {
      CGAL_assertion(it != (points_.end() - 1));
      if(it == (points_.end() - 2)) {
        CGAL_assertion(is_loop());
        it = points_.begin();
      } else {
        ++it;
      }
    } else {
      CGAL_assertion(orientation == NEGATIVE);
      CGAL_assertion(it != points_.begin());
      if(it == (points_.begin() + 1)) {
        if(!is_loop())
        {
          std::cout << "FEATURE POINTS" << std::endl;
          std::cout.precision(17);
          for(const auto& p : points_)
            std::cout << p << std::endl;
          CGAL_assertion(false);
        }
        CGAL_assertion(is_loop());
        it = points_.end() - 1;
      } else {
        --it;
      }
    }
    return it;
  }

  bool is_curve_segment_covered(CGAL::Orientation orientation,
                                const Point_3& c1, const Point_3& c2,
                                const FT sq_r1, const FT sq_r2,
                                const const_iterator cc1_it,
                                const const_iterator cc2_it) const
  {
    CGAL_assertion(orientation != CGAL::ZERO);
    typename Kernel::Has_on_bounded_side_3 cover_pred =
      Kernel().has_on_bounded_side_3_object();

    typedef typename Kernel::Sphere_3 Sphere_3;
    const Sphere_3 s1(c1, sq_r1);
    const Sphere_3 s2(c2, sq_r2);

    const_iterator c1_it = cc1_it;
    const_iterator c2_it = cc2_it;

    if(orientation == CGAL::NEGATIVE) {
      ++c1_it;
      ++c2_it;
      CGAL_assertion(c1_it != points_.end());
      CGAL_assertion(c2_it != points_.end());
    }

    if(c1_it == c2_it) return cover_pred(s1, s2, c1, c2);
    const_iterator next_it = this->next(c1_it, orientation);

    if(!cover_pred(s1, s2, c1, *next_it)) return false;

    for(const_iterator it = next_it; it != c2_it; /* in body */) {
      next_it = this->next(it, orientation);
      if(!cover_pred(s1, s2, *it, *next_it)) return false;
      it = next_it;
    } // end loop ]c1_it, c2_it[

    return cover_pred(s1, s2, *c2_it, c2);
  }

  bool is_curve_segment_covered(CGAL::Orientation orientation,
                                const Point_3& c1, const Point_3& c2,
                                const FT sq_r1,    const FT sq_r2) const
  {
    return is_curve_segment_covered(orientation,
                                    c1, c2, sq_r1, sq_r2, locate(c1), locate(c2));
  }

  FT curve_segment_length(const Point_3& p, const Point_3 q,
                          CGAL::Orientation orientation) const
  {
    CGAL_assertion(orientation != CGAL::ZERO);
    const_iterator p_it = locate(p);
    const_iterator q_it = locate(q);
    return curve_segment_length(p, q, orientation, p_it, q_it);
  }

  FT curve_segment_length(const Point_3& p, const Point_3 q,
                          CGAL::Orientation orientation,
                          const_iterator p_it,
                          const_iterator q_it) const
  {
    CGAL_assertion(orientation != CGAL::ZERO);
#if CGAL_MESH_3_PROTECTION_DEBUG
    CGAL_assertion(p_it == locate(p));
    CGAL_assertion(q_it == locate(q));
#endif

    if(p_it == q_it) {
      const CGAL::Comparison_result cmp = compare_distance(*p_it,p,q);
      if( (cmp != LARGER  && orientation == POSITIVE) ||
          (cmp != SMALLER && orientation == NEGATIVE) )
      {
        // If the orientation of `p` and `q` on the segment is compatible
        // with `orientation`, then return the distance between the two
        // points.
        return distance(p, q);
      }
    }

    if(orientation == CGAL::NEGATIVE) {
      ++p_it;
      ++q_it;
      CGAL_assertion(p_it != points_.end());
      CGAL_assertion(q_it != points_.end());
    }

    const_iterator next_it = this->next(p_it, orientation);
    FT result = distance(p, *next_it);
    result += distance(next_it, q_it, orientation);
    result += distance(*q_it, q);
    return result;
  }


  /// returns the angle at the first point.
  /// \pre The polyline must be a loop.
  Angle angle_at_first_point() const {
    CGAL_precondition(is_loop());
    const Point_3& first = points_.front();
    const Point_3& next_p = points_[1];
    const Point_3& prev = points_[points_.size() - 2];
    return angle(prev, first, next_p);
  }

  /// returns the length of the polyline
  FT length() const
  {
    if(length_ < 0.)
    {
      FT result{0};
      const_iterator it = points_.begin();
      it->length_from_start = result;
      const_iterator previous = it++;

      for(const_iterator end = points_.end(); it != end; ++it, ++previous) {
        result += distance(*previous, *it);
        it->length_from_start = result;
      }
      length_ = result;
    }
    return length_;
  }

  /// returns the signed geodesic distance between `p` and `q`.
  FT signed_geodesic_distance(const Point_3& p, const Point_3& q) const
  {
    // Locate p & q on polyline
    const_iterator pit = locate(p);
    const_iterator qit = locate(q,false);

    return signed_geodesic_distance(p, q, pit, qit);
  }

  FT signed_geodesic_distance(const Point_3& p, const Point_3& q,
                              const_iterator pit, const_iterator qit) const
  {
#if CGAL_MESH_3_PROTECTION_DEBUG
    CGAL_assertion(pit == locate(p));
    CGAL_assertion(qit == locate(q, false));
#endif
    // If p and q are in the same segment of the polyline
    if ( pit == qit )
    {
      FT result = distance(p,q);

      // Find the closest point to *pit
      if ( compare_distance(*pit,p,q) != CGAL::LARGER )
      { return result; }
      else
      { return -result; }
    }
    if(is_loop())
    {
      FT positive_distance, negative_distance;
      if(pit <= qit)
      {
        positive_distance = curve_segment_length(p, q, CGAL::POSITIVE, pit, qit);
        negative_distance = length() - positive_distance;
      }
      else
      {
        negative_distance = curve_segment_length(q, p, CGAL::POSITIVE, qit, pit);
        positive_distance = length() - negative_distance;
      }
      return (positive_distance < negative_distance) ? positive_distance : (-negative_distance);
    }
    else
    {
      return (pit <= qit)
        ?     curve_segment_length(p, q, CGAL::POSITIVE, pit, qit)
        : ( - curve_segment_length(p, q, CGAL::NEGATIVE, pit, qit) );
    }
  }

  Point_3 point(const_iterator it) const
  {
    if(it != points_.end())
      return *it;
    else
      return end_point();
  }

  const_iterator previous_segment_source(const_iterator it) const
  {
    CGAL_assertion(it != points_.end());
    if(it == first_segment_source())
    {
      CGAL_assertion(is_loop());
      return last_segment_source();
    }
    else
      return std::prev(it);
  }

  const_iterator next_segment_source(const_iterator it) const
  {
    CGAL_assertion(it != points_.end());
    CGAL_assertion_code(if(!is_loop()))
      CGAL_assertion(it != points_.end() - 1);

    if(it == last_segment_source())
    {
      if(is_loop())
        return first_segment_source();
      else
        return points_.end();
    }
    else
      return std::next(it);
  }

  /// returns a point at geodesic distance `distance` from p along the
  /// polyline. The polyline is oriented from starting point to end point.
  /// The distance could be negative.
  Point_3 point_at(const Point_3& start_pt, FT distance) const
  {
    return point_at(start_pt, distance, locate(start_pt)).first;
  }

  /// returns a point at geodesic distance `distance` from `start_pt` along the
  /// polyline. The polyline is oriented from starting point to end point.
  /// The distance could be negative.
  Point_and_position point_at(const Point_3& start_pt,
                              FT distance,
                              const_iterator start_it) const
  {
#if CGAL_MESH_3_PROTECTION_DEBUG
    CGAL_assertion(start_it == locate(start_pt));
#endif

    // simplify the problem by moving start_pt to start_it_pt
    const Point_3& start_it_pt = *start_it;
    const_iterator start_it_pt_it = (start_it != points_.begin())
                                  ? std::prev(start_it)
                                  : start_it;
#if CGAL_MESH_3_PROTECTION_DEBUG
    CGAL_assertion(locate(start_it_pt) == start_it_pt_it);
#endif

    distance += curve_segment_length(start_it_pt, start_pt, CGAL::POSITIVE,
                                     start_it_pt_it, start_it);

    while(CGAL::abs(distance) > length())
    {
      if(distance > FT(0) && distance > length())
        distance -= length();
      else if(distance < FT(0) && distance < -length())
        distance += length();
    }

    // initialize iterators and walk forward or backward to find the segment
    // containing the point at distance from start_it_pt
    const CGAL::Sign sgn = CGAL::sign(distance);
    const FT ft_sgn = (sgn == CGAL::POSITIVE) ? FT(1) : ((sgn == CGAL::NEGATIVE) ? FT(-1) : FT(0));

    const_iterator previous = start_it;
    const_iterator pit = (sgn == CGAL::POSITIVE)
                       ? next_segment_source(previous)
                       : previous_segment_source(previous);

    FT segment_length = ft_sgn * this->distance(point(previous), point(pit));
    bool signed_changed = false;

    while(!signed_changed && CGAL::abs(distance) > CGAL::abs(segment_length))
    {
      const FT new_distance = distance - segment_length;

      signed_changed = (distance * new_distance < FT(0));

      if(!signed_changed)
        distance = new_distance; // positive or negative, distance goes closer to 0

      // increment iterators
      previous = pit;
      pit = (sgn == CGAL::POSITIVE)
          ? next_segment_source(pit)
          : previous_segment_source(pit);

      if(pit == points_.end())
      {
        break; // reached end point
      }

      // update segment length
      segment_length = ft_sgn * this->distance(point(previous), point(pit));
    };

    // return point at distance from current segment source
    using Vector_3 = typename Kernel::Vector_3;
    auto vector = Kernel().construct_vector_3_object();
    auto scale = Kernel().construct_scaled_vector_3_object();
    auto translate = Kernel().construct_translated_point_3_object();

    Vector_3 v = (sgn == CGAL::POSITIVE)
               ? vector(point(previous), point(pit))
               : vector(point(pit), point(previous));

    const Vector_3 move = scale(v, distance * 1. / CGAL::approximate_sqrt(v.squared_length()));
    const Point_3 result = translate(*previous, move);

    const_iterator result_iterator = (sgn == CGAL::POSITIVE) ? previous : pit;
    if(result_iterator->point == result && result_iterator != points_.begin())
    {
      --result_iterator;
    }
#if CGAL_MESH_3_PROTECTION_DEBUG
    CGAL_assertion(result_iterator == locate_point(result));
#endif

    return {result, result_iterator};
  }

  const_iterator locate_corner(const Point_3& p) const
  {
    const_iterator res = points_.end();
    if(p == start_point())
      res = points_.begin();
    else if(p == end_point())
      res = last_segment_source();

#if CGAL_MESH_3_PROTECTION_DEBUG
    CGAL_assertion(res == locate(p));
#endif
    CGAL_assertion(res != points_.end());
    return res;
  }

  const_iterator locate_point(const Point_3& p) const
  {
    return locate(p);
  }

  bool are_ordered_along(const Point_3& p, const Point_3& q,
                         const_iterator pit, const_iterator qit) const
  {
    CGAL_precondition(!is_loop());

    // Locate p & q on polyline
#if CGAL_MESH_3_PROTECTION_DEBUG
    CGAL_assertion(pit == locate(p));
    CGAL_assertion(qit == locate(q));//, true));
#endif

    // Points are not located on the same segment
    if ( pit != qit ) { return (pit <= qit); }

    // pit == qit, then we have to sort p&q along (pit,pit+1)
    return ( compare_distance(*pit,p,q) != CGAL::LARGER );
  }

  bool are_ordered_along(const Point_3& p, const Point_3& q) const
  {
    return are_ordered_along(p, q, locate(p), locate(q, true));
  }

  const_iterator first_segment_source() const
  {
    CGAL_precondition(is_valid());
    return points_.begin();
  }

  const_iterator last_segment_source() const
  {
    CGAL_precondition(is_valid());
    return (points_.end() - 2);
  }

private:
  /// returns an iterator on the starting point of the segment of the
  /// polyline which contains p
  /// if end_point_first is true, then --end is returned instead of begin
  /// if p is the starting point of a loop.
  const_iterator locate(const Point_3& p, bool end_point_first=false) const
  {
    CGAL_precondition(is_valid());

    // First look if p is one of the points of the polyline
    const_iterator result = std::find(points_.begin(), points_.end(), p);
    if ( result != points_.end() )
    {
      if ( result != points_.begin() )
      { return --result; }
      else
      {
        // Treat loops
        if ( end_point_first && p == end_point() )
        { return last_segment_source(); }
        else
        { return result; }
      }
    }

    CGAL_assertion(result == points_.end());

    // Get result by projecting p on the polyline
    const_iterator it = points_.begin();
    const_iterator previous = it;
    Segment_3 nearest_segment;
    const_iterator nearest_vertex = it;
    result = nearest_vertex;
    bool nearest_is_a_segment = false;

    while ( ++it != points_.end() )
    {
      Segment_3 seg (*previous, *it);

      if(nearest_is_a_segment)
      {
        if(compare_distance(p, *it, nearest_segment) == CGAL::SMALLER)
        {
          nearest_vertex = it;
          nearest_is_a_segment = false;
          result = it;
          if (possibly(angle(*previous, *it, p) == CGAL::ACUTE) &&
              compare_distance(p, seg, *nearest_vertex) == CGAL::SMALLER)
          {
            nearest_segment = seg;
            nearest_is_a_segment = true;
            result = previous;
          }
        }
        else if(compare_distance(p, seg, nearest_segment) == CGAL::SMALLER)
        {
          nearest_segment = seg;
          result = previous;
        }
      }
      else {
        if(compare_distance(p, *it, *nearest_vertex) == CGAL::SMALLER)
        {
          nearest_vertex = it;
          result = it;
        }
        if ((nearest_vertex != it ||
             possibly(angle(*previous, *it, p) == CGAL::ACUTE)) &&
            compare_distance(p, seg, *nearest_vertex) == CGAL::SMALLER)
        {
          nearest_segment = seg;
          nearest_is_a_segment = true;
          result = previous;
        }
      }
      previous = it;
    } // end the while loop on the vertices of the polyline

    if(result == points_.begin()) {
      return (end_point_first && !nearest_is_a_segment) ? last_segment_source() : points_.begin();
    } else {
      return result;
    }
  }


  FT distance(const Point_3& p, const Point_3& q) const
  {
    typename Kernel::Compute_squared_distance_3 sq_distance =
      Kernel().compute_squared_distance_3_object();
    return CGAL::sqrt(sq_distance(p, q));
  }

  FT distance(const_iterator p_it, const_iterator q_it, CGAL::Orientation orientation) const
  {
    if(p_it == q_it)
      return FT(0);

    FT result;
    if(p_it->length_from_start && q_it->length_from_start)
    {
      if(orientation == CGAL::POSITIVE)
      {
        FT xp = *(p_it->length_from_start);
        FT xq = (is_loop() && q_it == points_.begin())
              ? length()
              : *(q_it->length_from_start);
        result = xq - xp;
      }
      else
      {
        FT xq = *(q_it->length_from_start);
        FT xp = (is_loop() && p_it == points_.begin())
              ? length()
              : *(p_it->length_from_start);
        result = xp - xq;
      }
      if(result < 0 && is_loop())
        result += length();
    }
    else
    {
      result = FT(0);
      for(const_iterator it = p_it; it != q_it; /* in body */) {
        auto next_it = this->next(it, orientation);
        result += distance(*it, *next_it);
        it = next_it;
      } // end loop ]p_it, q_it[
    }

    // CGAL_assertion(result > 0); // disabled assertion: a polyline can have zero-length segments
    return result;
  }

  Angle angle(const Point_3& p,
              const Point_3& angle_vertex_point,
              const Point_3& q) const
  {
    typename Kernel::Angle_3 compute_angle =  Kernel().angle_3_object();
    return compute_angle(p,angle_vertex_point,q);
  }

  template <typename T1, typename T2>
  CGAL::Sign compare_distance(const Point_3& p,
                              const T1& obj1,
                              const T2& obj2) const
  {
    typename Kernel::Compare_distance_3 compare_distance =
      Kernel().compare_distance_3_object();
    return compare_distance(p,obj1,obj2);
  }

public:
  Data points_;

private:
  mutable FT length_ = -1.;

}; // end class Polyline


} // end namespace internal
} // end namespace Mesh_3
} // end namespace CGAL

#endif // CGAL_MESH_3_INTERNAL_POLYLINE_H
