// Copyright (c) 2015 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
// You can redistribute it and/or modify it under the terms of the GNU
// General Public License as published by the Free Software Foundation,
// either version 3 of the License, or (at your option) any later version.
//
// 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$
// SPDX-License-Identifier: GPL-3.0+
//
//
// Author(s)     : Sven Oesau, Yannick Verdie, Clément Jamin, Pierre Alliez
//

#ifndef CGAL_SHAPE_DETECTION_3_CONE_H
#define CGAL_SHAPE_DETECTION_3_CONE_H

#include <CGAL/license/Point_set_shape_detection_3.h>


#include <CGAL/Shape_detection_3/Shape_base.h>
#include <CGAL/number_utils.h>
#include <cmath>

/*!
 \file Cone.h
 */


namespace CGAL {
  namespace Shape_detection_3 {
  /*!
   \brief Cone implements Shape_base.
    The cone is represented by its apex, the axis and the opening angle.
    This representation models an open infinite single-cone.
    \tparam Traits a model of `EfficientRANSACTraits`
   \ingroup PkgPointSetShapeDetection3Shapes
   */

  template <class Traits>
  class Cone : public Shape_base<Traits> {
    using Shape_base<Traits>::update_label;

  public:
    /// \cond SKIP_IN_MANUAL
    typedef typename Traits::Point_map Point_map;
     ///< property map to access the location of an input point.
    typedef typename Traits::Normal_map Normal_map;
     ///< property map to access the unoriented normal of an input point.
    typedef typename Traits::FT FT; ///< number type.
    typedef typename Traits::Point_3 Point_3;///< point type.
    typedef typename Traits::Vector_3 Vector_3;///< vector type.
    /// \endcond


    Cone() : Shape_base<Traits>(), m_wrap(false) {}
      
    /*!
      The opening angle between the axis and the surface of the cone.
     */
    FT angle() const {
        return m_angle;
    }
    
    /*!
      The apex of the cone.
     */
    Point_3 apex() const {
        return m_apex;
    }
    
    /*!
      The axis points from the apex into the cone.
     */
    Vector_3 axis() const {
        return m_axis;
    }
    
    /*!
      Helper function to write apex, axis and angle of the cone and
      number of assigned points into a string.
     */
    /// \cond SKIP_IN_MANUAL
    std::string info() const {
        std::stringstream sstr;
        
        sstr << "Type: cone apex: (" << this->get_x(m_apex) << ", " << this->get_y(m_apex);
        sstr << ", " << this->get_z(m_apex) << ") axis: (" << this->get_x(m_axis) << ", ";
        sstr << this->get_y(m_axis) << ", " << this->get_z(m_axis) << ") angle:" << m_angle;
        sstr << " #Pts: " << this->m_indices.size();
        
        return sstr.str();
    }

    /*!
    Computes squared Euclidean distance from query point to the shape.
    */ 
    FT squared_distance(const Point_3 &p) const {
      Vector_3 toApex = this->constr_vec(m_apex, p);
      FT a = this->sqlen(toApex);

      // projection on axis
      FT b = this->scalar_pdct(toApex, m_axis);

      // distance to axis
      if (a - b * b <= 0)
        return 0;

      FT l = CGAL::sqrt(a - b * b);
      FT c = m_cos_ang * l;
      FT d = m_neg_sin_ang * b;

      // far on other side?
      return (b < 0 && c - d < 0) ? a : CGAL::abs(c + d) * CGAL::abs(c + d);
    }
    /// \endcond

  protected:
      /// \cond SKIP_IN_MANUAL
    virtual void create_shape(const std::vector<std::size_t> &indices) {
      Point_3 p1 = this->point(indices[0]);
      Point_3 p2 = this->point(indices[1]);
      Point_3 p3 = this->point(indices[2]);

      Vector_3 n1 = this->normal(indices[0]);
      Vector_3 n2 = this->normal(indices[1]);
      Vector_3 n3 = this->normal(indices[2]);

      // first calculate intersection of three planes -> apex

      Vector_3 lineDir = this->cross_pdct(n1, n2);
      FT length = CGAL::sqrt(this->sqlen(lineDir));
      if (length == 0)
        return;

      lineDir = this->scale(lineDir, (FT)1.0 / length);

      // lineDir not normalized direction of intersection lines
      //  of two planes (p1, n1) and (p2, n2)
      // get point on line by moving point p1 onto line
      Vector_3 orthLineInPlane = this->cross_pdct(n1, lineDir);
      length = CGAL::sqrt(this->sqlen(orthLineInPlane));
      if (length == 0)
        return;

      orthLineInPlane = this->scale(orthLineInPlane, (FT)1.0 / length);

      // distance of p1 to (p2, n2)
      FT d = this->scalar_pdct(this->constr_vec(CGAL::ORIGIN, p1), n2)
        - this->scalar_pdct(this->constr_vec(CGAL::ORIGIN, p2), n2);
      // projection of orthLineInPlane onto p2
      FT l = this->scalar_pdct(orthLineInPlane, n2);
      if (l == 0)
        return;

      Point_3 pointOnLine = this->transl(
        p1, this->scale(orthLineInPlane, -d/l));

      // distance of pLineDir to (p3, n3)
      d = this->scalar_pdct(this->constr_vec(CGAL::ORIGIN, pointOnLine), n3)
        - this->scalar_pdct(this->constr_vec(CGAL::ORIGIN, p3), n3);
      l = this->scalar_pdct(lineDir, n3);
      if (l == 0)
        return;

      m_apex = this->transl(pointOnLine, this->scale(lineDir, -d/l));

      // 2. find axis
      Vector_3 v1 = this->constr_vec(m_apex, p1);
      length = CGAL::sqrt(this->sqlen(v1));
      if (length == 0)
        return;
      v1 = this->scale(v1, (FT)1.0 / length);
      Point_3 c1 = this->transl(m_apex, v1);

      Vector_3 v2 = this->constr_vec(m_apex, p2);
      length = CGAL::sqrt(this->sqlen(v2));
      if (length == 0)
        return;
      v2 = this->scale(v2, (FT)1.0 / length);
      Point_3 c2 = this->transl(m_apex, v2);

      Vector_3 v3 = this->constr_vec(m_apex, p3);
      length = CGAL::sqrt(this->sqlen(v3));
      if (length == 0)
        return;
      v3 = this->scale(v3, (FT)1.0 / length);
      Point_3 c3 = this->transl(m_apex, v3);

      m_axis = this->cross_pdct(this->constr_vec(c2, c1), this->constr_vec(c3, c1));
      m_axis = (this->scalar_pdct(orthLineInPlane, m_axis) < 0) ? 
        this->scale(m_axis, FT(-1)) : m_axis;
      length = CGAL::sqrt(this->sqlen(m_axis));
      if (length == 0)
        return;
      m_axis = this->scale(m_axis, (FT)1.0 / length);

      m_angle = acos(this->scalar_pdct(v1, m_axis)) + acos(this->scalar_pdct(v2, m_axis)) + acos(this->scalar_pdct(v3, m_axis));
      m_angle /= (FT)3.0;
      if (m_angle < 0 || m_angle > CGAL_PI / (FT)2.12)
        return;

      m_neg_sin_ang = -sin(m_angle);
      m_cos_ang = cos(m_angle);

      this->m_is_valid = true;
    }

    virtual void squared_distance(const std::vector<std::size_t> &indices,
                                  std::vector<FT> &dists) const {
      for (std::size_t i = 0;i<indices.size();i++) {
          Vector_3 to_apex = this->constr_vec(m_apex, this->point(indices[i]));

          FT a = this->sqlen(to_apex);

          // projection on axis
          FT b = this->scalar_pdct(to_apex, m_axis);

          // distance to axis
          FT l = CGAL::sqrt(a - b * b);
          FT c = m_cos_ang * l;
          FT d = m_neg_sin_ang * b;

          // far on other side?
          dists[i] = 
            (b < 0 && c - d < 0) ? a : CGAL::abs(c + d) * CGAL::abs(c + d);
        }
      }

    virtual void cos_to_normal(const std::vector<std::size_t> &indices, 
                               std::vector<FT> &angles) const {
      for (std::size_t i = 0;i<indices.size();i++) {
          // construct vector orthogonal to axis in direction of the point
        Vector_3 a = this->constr_vec(m_apex, this->point(indices[i]));

          Vector_3 b = this->cross_pdct(m_axis, 
                                         this->cross_pdct(m_axis, a));
          b = (this->scalar_pdct(a, b) < 0) ? this->scale(b, FT(-1)) : b;
          FT length = CGAL::sqrt(this->sqlen(b));

          if (length == 0) {
            angles[i] = (FT)1.0;
            continue;
          }

          b = this->scale(b, (FT)1.0 / length);
          b = this->sum_vectors(
            this->scale(b, m_cos_ang), 
            this->scale(m_axis, m_neg_sin_ang));

          angles[i] = CGAL::abs(this->scalar_pdct(this->normal(indices[i]), b));
        }
      }

    virtual FT cos_to_normal(const Point_3 &p, const Vector_3 &n) const {
      // construct vector orthogonal to axis in direction of the point
      Vector_3 a = this->constr_vec(m_apex, p);
      Vector_3 b = this->cross_pdct(m_axis, this->cross_pdct(m_axis, a));
      b = (this->scalar_pdct(a, b) < 0) ? this->scale(b, FT(-1)) : b;
      FT length = CGAL::sqrt(this->sqlen(b));
      if (length == 0) {
        return (FT)1.0;
      }

      b = this->scale(b, (FT)1.0 / length);
      b = this->sum_vectors(
        this->scale(b, m_cos_ang), 
        this->scale(m_axis, m_neg_sin_ang));

      return CGAL::abs(this->scalar_pdct(n, b));
    }
      
    virtual std::size_t minimum_sample_size() const {
          return 3;
      }

    virtual void post_wrap(const std::vector<unsigned int> &bitmap,
      const std::size_t &u_extent,
      const std::size_t &v_extent,
      std::vector<unsigned int> &labels) const {
      if (!m_wrap)
        return;
      // handle top index separately
      unsigned int nw = bitmap[u_extent - 1];
      unsigned int l = bitmap[0];

      // Special case v_extent is just 1
      if (v_extent == 1) {
        if (nw && nw != l) {
          l = (std::min<unsigned int>)(nw, l);
          update_label(labels, (std::max<unsigned int>)(nw, l), l);
        }

        return;
      }

      unsigned int w = bitmap[2 * u_extent - 1];
      unsigned int sw;

      if (l) {
        if (nw && nw != l) {
          l = (std::min<unsigned int>)(nw, l);
          update_label(labels, (std::max<unsigned int>)(nw, l), l);
        }
        else if (w && w != l) {
          l = (std::min<unsigned int>)(w, l);
          update_label(labels, (std::max<unsigned int>)(w, l), l);
        }
      }

      // handle mid indices
      for (std::size_t y = 1;y<v_extent - 1;y++) {
        l = bitmap[y * u_extent];
        if (!l)
          continue;

        nw = bitmap[y * u_extent - 1];
        w = bitmap[(y + 1) * u_extent - 1];
        sw = bitmap[(y + 2) * u_extent - 1];

        if (nw && nw != l) {
          l = (std::min<unsigned int>)(nw, l);
          update_label(labels, (std::max<unsigned int>)(nw, l), l);
        }
        if (w && w != l) {
          l = (std::min<unsigned int>)(w, l);
          update_label(labels, (std::max<unsigned int>)(w, l), l);
        }
        else if (sw && sw != l) {
          l = (std::min<unsigned int>)(sw, l);
          update_label(labels, (std::max<unsigned int>)(sw, l), l);
        }
      }

      // handle last index
      l = bitmap[(v_extent - 1) * u_extent];
      if (!l)
        return;

      nw = bitmap[(v_extent - 1) * u_extent - 1];
      w = bitmap[u_extent * v_extent - 1];

      if (nw && nw != l) {
        l = (std::min<unsigned int>)(nw, l);
        update_label(labels, (std::max<unsigned int>)(nw, l), l);
      }
      else if (w && w != l) {
        l = (std::min<unsigned int>)(w, l);
        update_label(labels, (std::max<unsigned int>)(w, l), l);
      }
    }

    virtual void parameters(const std::vector<std::size_t> &indices,
      std::vector<std::pair<FT, FT> > &parameterSpace,
      FT &cluster_epsilon,
      FT min[2],
      FT max[2]) const {

      // Create basis d1, d2
      Vector_3 d1 = this->constr_vec(
        ORIGIN, this->constr_pt(FT(0), FT(0), FT(1)));
      Vector_3 d2 = this->cross_pdct(m_axis, d1);
      FT l = this->sqlen(d2);
      if (l < (FT)0.0001) {
        d1 = this->constr_vec(ORIGIN, this->constr_pt(FT(1), FT(0), FT(0)));
        d2 = this->cross_pdct(m_axis, d1);
        l = this->sqlen(d2);
      }
      d2 = this->scale(d2, FT(1) / CGAL::sqrt(l));

      d1 = this->cross_pdct(m_axis, d2);
      l = CGAL::sqrt(this->sqlen(d1));
      if (l == 0)
        return;

      d1 = this->scale(d1, (FT)1.0 / l);

      if (m_angle > CGAL_M_PI_4) {
        // Projection onto a disk preserving distance to apex

        m_wrap = false;

        // First index separately to initialize min/max
        Vector_3 d = this->constr_vec(m_apex, this->point(indices[0]));
        FT l = this->scalar_pdct(d, m_axis) / m_cos_ang;
        FT u = this->scalar_pdct(d, d1);
        FT v = this->scalar_pdct(d, d2);
        FT l2 = CGAL::sqrt(u * u + v * v);
        u = u * l/l2;
        v = v * l/l2;
        min[0] = max[0] = u;
        min[1] = max[1] = v;
        parameterSpace[0] = std::pair<FT, FT>(u, v);

        for (std::size_t i = 1;i<indices.size();i++) {
          d = this->constr_vec(m_apex, this->point(indices[i]));
          l = this->scalar_pdct(d, m_axis) / m_cos_ang;
          u = this->scalar_pdct(d, d1);
          v = this->scalar_pdct(d, d2);
          l2 = CGAL::sqrt(u * u + v * v);
          u = u * l/l2;
          v = v * l/l2;

          min[0] = (std::min<FT>)(min[0], u);
          max[0] = (std::max<FT>)(max[0], u);

          min[1] = (std::min<FT>)(min[1], v);
          max[1] = (std::max<FT>)(max[1], v);

          parameterSpace[i] = std::pair<FT, FT>(u, v);
        }
      }
      else {
        // Map onto triangle.
        // u coordinate is arclength
        // v coordinate is distance to apex

        Vector_3 d = this->constr_vec(m_apex, this->point(indices[0]));
        FT v = this->scalar_pdct(d, m_axis) / m_cos_ang;
        FT phi = atan2(this->scalar_pdct(d, d2), this->scalar_pdct(d, d1));
        FT u = FT(phi + CGAL_PI);
        FT avg_v = v;

        min[0] = max[0] = u;
        min[1] = max[1] = v;
        parameterSpace[0] = std::pair<FT, FT>(u, v);
        for (std::size_t i = 1;i<indices.size();i++) {
          d = this->constr_vec(m_apex, this->point(indices[i]));
          v = this->scalar_pdct(d, m_axis) / m_cos_ang;
          phi = atan2(this->scalar_pdct(d, d2), this->scalar_pdct(d, d1));
          u = FT(phi + CGAL_PI);

          min[0] = (std::min<FT>)(min[0], u);
          max[0] = (std::max<FT>)(max[0], u);

          min[1] = (std::min<FT>)(min[1], v);
          max[1] = (std::max<FT>)(max[1], v);

          avg_v += v;

          parameterSpace[i] = std::pair<FT, FT>(u, v);
        }

        // Scale u parameter by average circumference to arc length
        avg_v /= indices.size();
        const FT scale = -m_neg_sin_ang * avg_v;

        m_wrap = (min[0] + 2 * CGAL_PI - max[0]) * scale < cluster_epsilon;

        for (std::size_t i = 0;i<parameterSpace.size();i++) {
          std::pair<FT, FT> p = parameterSpace[i];
          parameterSpace[i] = std::pair<FT, FT>(p.first * scale, p.second);
        }

        min[0] *= scale;
        max[0] *= scale;
      }
    }

    virtual bool supports_connected_component() const {
      return true;
    }

  private:
    FT m_angle;
    Point_3 m_apex;
    Vector_3 m_axis;
    FT m_neg_sin_ang, m_cos_ang;
    mutable bool m_wrap;
      /// \endcond
  };
}
}
#endif