// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018-2021 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>

#include "open3d/geometry/BoundingVolume.h"
#include "open3d/geometry/Geometry.h"
#include "open3d/utility/Optional.h"

#pragma once

namespace open3d {
namespace geometry {

/// \class Line3D
///
/// \brief Line3D is a class which derives from
/// Eigen::ParametrizedLine<double, 3> in order to capture the semantic
/// differences between a "line", "ray", and "line segment" for operations in
/// which the difference is important, such as intersection and distance tests.
/// The underlying Eigen object can always be retrieved with the .Line() method.
///
/// \details The taxonomy of the Line3D class and its derived classes, Ray3D
/// and Segment3D, was created in order to find a compromise between the
/// goals of providing an easy-to-use API, enforcing obvious correctness
/// when dealing with operations in which line semantics are important, and
/// maintaining reasonably high performance.
///
/// The underlying motivation is to enforce correctness when performing line
/// operations based on Eigen::ParametrizedLine<double, 3> even as subtleties
/// about how a line is represented begin to affect the outcomes of different
/// operations.  Some performance is sacrificed in the use of virtual functions
/// for a clean API in cases where the compiler cannot determine at compile time
/// which function will be called and cannot de-virtualize the call.
///
/// In such cases where performance is extremely important, avoid iterating
/// through a list of derived objects stored as Line3D and calling virtual
/// functions on them so that the compiler can hopefully remove the vtable
/// lookup, or consider a hand implementation of your problem in which you
/// carefully account for the semantics yourself.
class Line3D : protected Eigen::ParametrizedLine<double, 3> {
public:
    /// \brief Creates a line through two points.  The line origin will take the
    /// value of p0, and the line direction will be a normalized vector from
    /// p0 to p1
    static Line3D Through(const Eigen::Vector3d& p0,
                          const Eigen::Vector3d& p1) {
        return {p0, (p1 - p0).normalized()};
    }

    /// \enum LineType
    ///
    /// \brief Specifies different semantic interpretations of 3d lines
    enum class LineType {
        /// Lines extend infinitely in both directions
        Line = 0,

        /// Rays have an origin and a direction, and extend to infinity in
        /// that direction
        Ray = 1,

        /// Segments have both an origin and an endpoint and are finite in
        /// nature
        Segment = 2,
    };

    /// \brief Default user constructor
    Line3D(const Eigen::Vector3d& origin, const Eigen::Vector3d& direction);

    virtual ~Line3D() = default;

    /// \brief Gets the semantic type of the line
    LineType GetLineType() const { return line_type_; }

    /// \brief Gets the line's origin point
    const Eigen::Vector3d& Origin() const { return m_origin; }

    /// \brief Gets the line's direction vector
    const Eigen::Vector3d& Direction() const { return m_direction; }

    /// \brief Gets the length of the line, which for lines and rays will return
    /// positive infinity, but for segments will return a finite positive value.
    virtual double Length() const {
        return std::numeric_limits<double>::infinity();
    }

    /// \brief Transform the Line3D by the given matrix
    virtual void Transform(const Eigen::Transform<double, 3, Eigen::Affine>& t);

    /// \brief Returns a const reference to the underlying
    /// Eigen::ParametrizedLine object
    const Eigen::ParametrizedLine<double, 3>& Line() const { return *this; }

    /// \brief Calculates the intersection parameter between the line and a
    /// plane taking into account line semantics. Returns an empty result if
    /// there is no intersection. On a Line3D this returns the same result as
    /// .Line().intersectionParameter(plane)
    virtual utility::optional<double> IntersectionParameter(
            const Eigen::Hyperplane<double, 3>& plane) const;

    /// \brief Calculates the parameter of a point projected onto the line
    /// taking into account special semantics.
    ///
    /// \details On a Line3D this is the point directly projected onto the
    /// infinite line, and represents the closest point on the line to the test
    /// point. A negative value indicates the projection lies behind the origin,
    /// a positive value is in front of the origin. On a Ray3D this will be a
    /// positive value only, since rays do not exist in the negative direction.
    /// On a Segment3D this will be a positive value which is less than or equal
    /// to the segment length.
    double ProjectionParameter(const Eigen::Vector3d& point) const;

    /// \brief Calculates a point projected onto the line, taking into account
    /// special semantics.
    ///
    /// \details On a Line3D this is the point directly projected onto
    /// the infinite line, and represents the closest point on the line to the
    /// test point.  On a Ray3D this will either be a point on the ray's
    /// positive direction or the ray origin, whichever is closer. On a
    /// Segment3D this will be either one of the segment's endpoints or a point
    /// between them, whichever is closest to the test point.
    virtual Eigen::Vector3d Projection(const Eigen::Vector3d& point) const;

    /// \brief Returns the lower intersection parameter for a line with an
    /// axis aligned bounding box or empty if no intersection. Uses the slab
    /// method, see warning below.
    ///
    /// \details Calculates the lower intersection parameter of a parameterized
    /// line with an axis aligned bounding box. The intersection point can be
    /// recovered with .Line().pointAt(...). If the line does not intersect the
    /// box the optional return value will be empty. Also note that if the AABB
    /// is behind the line's origin point, the value returned will still be of
    /// the lower intersection, which is the first intersection in the direction
    /// of the line, not the intersection closer to the origin.
    ///
    /// This implementation is based off of Tavian Barnes' optimized branchless
    /// slab method. https://tavianator.com/2011/ray_box.html. It runs in
    /// roughly 5% of the time as the the naive exact method, but can degenerate
    /// in specific conditions where a line lies exactly in one of the AABB's
    /// planes.
    ///
    /// \warning A line that lies exactly in one of the AABB's planes within the
    /// double floating point precision will not intersect correctly by this
    /// method
    virtual utility::optional<double> SlabAABB(
            const AxisAlignedBoundingBox& box) const;

    /// \brief Returns the lower intersection parameter for a line with an
    /// axis aligned bounding box or empty if no intersection. This method is
    /// about 20x slower than the slab method, see details to know when to use.
    ///
    /// \details Calculates the lower intersection parameter of a parameterized
    /// line with an axis aligned bounding box. The intersection point can be
    /// recovered with .Line().pointAt(...). If the line does not intersect the
    /// box the return value will be empty. Also note that if the AABB is behind
    /// the line's origin point, the value returned will still be of the lower
    /// intersection, which is the first intersection in the direction of the
    /// line, not the intersection closer to the origin.
    ///
    /// This implementation is a naive exact method that considers intersections
    /// with all six bounding box planes. It is not optimized for speed and
    /// should only be used when a problem is conditioned such that the slab
    /// method is unacceptable. Use this when a line is likely to lie exactly
    /// in one of the AABB planes and false negatives are unacceptable.
    /// Typically this will only happen when lines are axis-aligned and both
    /// lines and bounding volumes are regularly spaced, and every intersection
    /// is important.  In such cases if performance is important, a simple
    /// custom implementation based on the problem directionality will likely
    /// outperform even the slab method.
    virtual utility::optional<double> ExactAABB(
            const AxisAlignedBoundingBox& box) const;

    /// \brief Computes the two corresponding parameters of the closest distance
    /// between two Line3D objects, including derived types Ray3D and Segment3D,
    /// respecting the semantics of the line type.
    std::pair<double, double> ClosestParameters(const Line3D& other) const;

    /// \brief Computes the two closest points between this Line3D object and
    /// the other, including of derived types Ray3D and Segment3D, respecting
    /// the semantics of the line types.
    std::pair<Eigen::Vector3d, Eigen::Vector3d> ClosestPoints(
            const Line3D& other) const;

    /// \brief Gets the closest distance between two Line3D objects, including
    /// derived types Ray3D and Segment3D, respecting the semantics of the line
    /// type.
    double DistanceTo(const Line3D& other) const;

    /// \brief Clamps/bounds a parameter value to the closest valid place where
    /// the entity exists.  On a Line3D, the value will be unchanged, on a Ray3D
    /// a negative value will be made 0, and on a Segment3D a negative value
    /// will be made 0 and a positive value greater than Length() will take
    /// the value of Length()
    virtual double ClampParameter(double parameter) const { return parameter; }

    /// \brief Verifies that a given parameter value is valid for the semantics
    /// of the line object. For lines, any parameter is valid, for rays any
    /// positive parameter is valid, and for segments any parameter between 0
    /// and the segment length is valid.
    virtual bool IsParameterValid(double parameter) const { return true; }

protected:
    /// \brief Internal constructor for inherited classes that allows the
    /// setting of the LineType
    Line3D(const Eigen::Vector3d& origin,
           const Eigen::Vector3d& direction,
           LineType type);

    /// \brief Calculates the common t_min and t_max values of the slab AABB
    /// intersection method. These values are computed identically for any
    /// semantic interpretation of the line, it's up to the derived classes
    /// to use them in conjunction with other information to determine what the
    /// intersection parameter is.
    std::pair<double, double> SlabAABBBase(
            const AxisAlignedBoundingBox& box) const;

private:
    const LineType line_type_ = LineType::Line;

    // Pre-calculated inverse values for the line's direction used to
    // accelerate the slab method
    double x_inv_;
    double y_inv_;
    double z_inv_;
};

/// \class Ray3D
///
/// \brief A ray is a semantic interpretation of Eigen::ParametrizedLine which
/// has an origin and a direction and extends infinitely only in that specific
/// direction.
class Ray3D : public Line3D {
public:
    /// \brief Creates a Ray3D through two points.  The ray origin will take the
    /// value of p0, and the direction will be a normalized vector from p0 to p1
    static Ray3D Through(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
        return {p0, (p1 - p0).normalized()};
    }

    /// \brief Default constructor, requires point and direction
    Ray3D(const Eigen::Vector3d& origin, const Eigen::Vector3d& direction);

    /// \brief Gets the length of the line, which for lines and rays will return
    /// positive infinity, but for segments will return a finite positive value.
    double Length() const override {
        return std::numeric_limits<double>::infinity();
    }

    /// \brief Calculates the intersection parameter between the line and a
    /// plane taking into account ray semantics. Returns an empty result if
    /// there is no intersection. On a Ray3D this means that intersections
    /// behind the origin are invalid, so the return value will always be
    /// positive.
    utility::optional<double> IntersectionParameter(
            const Eigen::Hyperplane<double, 3>& plane) const override;

    /// \brief Returns the lower intersection parameter for a ray with an
    /// axis aligned bounding box or empty if no intersection. Uses the slab
    /// method, see warning below.
    ///
    /// \details Calculates the lower intersection parameter of a parameterized
    /// ray with an axis aligned bounding box. The intersection point can be
    /// recovered with .Line().pointAt(...). If the ray does not intersect the
    /// box the optional return value will be empty. No intersection behind the
    /// ray origin will be counted, and if the ray originates from within the
    /// bounding box the parameter value will be 0.
    ///
    /// This implementation is based off of Tavian Barnes' optimized branchless
    /// slab method. https://tavianator.com/2011/ray_box.html. It runs in
    /// roughly 5% of the time as the the naive exact method, but can degenerate
    /// in specific conditions where a ray lies exactly in one of the AABB's
    /// planes.
    ///
    /// \warning A ray that lies exactly in one of the AABB's planes within the
    /// double floating point precision will not intersect correctly by this
    /// method
    utility::optional<double> SlabAABB(
            const AxisAlignedBoundingBox& box) const override;

    /// \brief Clamps/bounds a parameter value to the closest valid place where
    /// the entity exists.  On a Line3D, the value will be unchanged, on a Ray3D
    /// a negative value will be made 0, and on a Segment3D a negative value
    /// will be made 0 and a positive value greater than Length() will take
    /// the value of Length()
    double ClampParameter(double parameter) const override {
        return std::max(parameter, 0.);
    }

    /// \brief Verifies that a given parameter value is valid for the semantics
    /// of the line object. For lines, any parameter is valid, for rays any
    /// positive parameter is valid, and for segments any parameter between 0
    /// and the segment length is valid.
    bool IsParameterValid(double parameter) const override {
        return parameter >= 0;
    }
};

/// \class Segment3D
///
/// \brief A segment is a semantic interpretation of Eigen::ParametrizedLine
/// which has an origin and an endpoint and exists finitely between them.
///
/// \details One of the main motivations behind this class, and the Line3D
/// taxonomy in general, is the ambiguity of the Eigen documentation with
/// regards to the ParametrizedLine's direction. The documentation warns that
/// the direction vector is expected to be normalized and makes no guarantees
/// about behavior when this expectation is not met.  However, ParametrizedLine
/// does behave correctly when the direction vector is scaled. This class
/// exists as a seam to ensure the correct behavior can be produced regardless
/// of what happens in the underlying Eigen implementation without changing
/// the api surface for client code.
class Segment3D : public Line3D {
public:
    /// \brief Creates a Segment3D through two points.  The origin will take the
    /// value of p0, and the endpoint be p1. The direction will be a normalized
    /// vector from p0 to p1.
    static Segment3D Through(const Eigen::Vector3d& p0,
                             const Eigen::Vector3d& p1) {
        return {p0, p1};
    }

    /// \brief Default constructor for Segment3D takes the start and end points
    /// of the segment \p start_point \p end_point
    Segment3D(const Eigen::Vector3d& start_point,
              const Eigen::Vector3d& end_point);

    /// \brief Takes a std::pair of points, the first to be used as the start
    /// point/origin and the second to be the end point
    explicit Segment3D(const std::pair<Eigen::Vector3d, Eigen::Vector3d>& pair);

    /// \brief Get the scalar length of the segment as the distance between the
    /// start point (origin) and the end point.
    double Length() const override { return length_; }

    /// \brief Calculates the midpoint of the segment
    Eigen::Vector3d MidPoint() const { return 0.5 * (origin() + end_point_); }

    /// \brief Get the end point of the segment
    const Eigen::Vector3d& EndPoint() const { return end_point_; }

    /// \brief Transform the segment by the given matrix
    void Transform(
            const Eigen::Transform<double, 3, Eigen::Affine>& t) override;

    /// \brief Get an axis-aligned bounding box representing the enclosed volume
    /// of the line segment.
    AxisAlignedBoundingBox GetBoundingBox() const;

    /// \brief Calculates the intersection parameter between the line and a
    /// plane taking into account segment semantics. Returns an empty result if
    /// there is no intersection. On a Segment3D this means that intersections
    /// behind the origin and beyond the endpoint are invalid.
    utility::optional<double> IntersectionParameter(
            const Eigen::Hyperplane<double, 3>& plane) const override;

    /// \brief Returns the lower intersection parameter for a segment with an
    /// axis aligned bounding box or empty if no intersection. Uses the slab
    /// method, see warning below.
    ///
    /// \details Calculates the lower intersection parameter of a parameterized
    /// segment with an axis aligned bounding box. The intersection point can be
    /// recovered with .Line().pointAt(...). If the segment does not
    /// intersect the box the optional return value will be empty. No
    /// intersection behind the segment origin will be counted, and if the
    /// segment originates from within the bounding box the parameter value will
    /// be 0. No intersection beyond the endpoint of the segment will be
    /// considered either.
    ///
    /// This implementation is based off of Tavian Barnes' optimized branchless
    /// slab method. https://tavianator.com/2011/segment_box.html. It runs in
    /// roughly 5% of the time as the the naive exact method, but can degenerate
    /// in specific conditions where a segment lies exactly in one of the AABB's
    /// planes.
    ///
    /// \warning A segment that lies exactly in one of the AABB's planes within
    /// the double floating point precision will not intersect correctly by this
    /// method
    utility::optional<double> SlabAABB(
            const AxisAlignedBoundingBox& box) const override;

    /// \brief Returns the lower intersection parameter for a segment with an
    /// axis aligned bounding box or empty if no intersection. This method is
    /// about 20x slower than the slab method, see details to know when to use.
    ///
    /// \details Calculates the lower intersection parameter of a parameterized
    /// segment with an axis aligned bounding box. The intersection point can be
    /// recovered with .Line().pointAt(...). If the segment does not
    /// intersect the box the return value will be empty.
    ///
    /// This implementation is a naive exact method that considers intersections
    /// with all six bounding box planes. It is not optimized for speed and
    /// should only be used when a problem is conditioned such that the slab
    /// method is unacceptable. Use this when a segment is likely to lie exactly
    /// in one of the AABB planes and false negatives are unacceptable.
    /// Typically this will only happen when segments are axis-aligned and both
    /// segments and bounding volumes are regularly spaced, and every
    /// intersection is important.  In such cases if performance is important, a
    /// simple custom implementation based on the problem directionality will
    /// likely outperform even the slab method.
    utility::optional<double> ExactAABB(
            const AxisAlignedBoundingBox& box) const override;

    /// \brief Clamps/bounds a parameter value to the closest valid place where
    /// the entity exists.  On a Line3D, the value will be unchanged, on a Ray3D
    /// a negative value will be made 0, and on a Segment3D a negative value
    /// will be made 0 and a positive value greater than Length() will take
    /// the value of Length()
    double ClampParameter(double parameter) const override {
        return std::max(std::min(parameter, length_), 0.);
    }

    /// \brief Verifies that a given parameter value is valid for the semantics
    /// of the line object. For lines, any parameter is valid, for rays any
    /// positive parameter is valid, and for segments any parameter between 0
    /// and the segment length is valid.
    bool IsParameterValid(double parameter) const override {
        return parameter >= 0 && parameter <= length_;
    }

private:
    Eigen::Vector3d end_point_;
    double length_;
};

}  // namespace geometry
}  // namespace open3d
