#pragma once

#include <set>

#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/geometry/proximity/plane.h"
#include "drake/geometry/shape_specification.h"
#include "drake/geometry/utilities.h"
#include "drake/math/rigid_transform.h"

namespace drake {
namespace geometry {
namespace internal {

// Forward declarations.
template <typename>
class BvhUpdater;

}  // namespace internal

// Forward declarations.
template <typename>
class AabbMaker;
class Obb;

/** Axis-aligned bounding box. The box is defined in a canonical frame B such
 that it is centered on Bo and its extents are aligned with B's axes. However,
 the box is posed in a hierarchical frame H. Because this is an _axis-aligned_
 bounding box, `R_HB = I`. Therefore the pose of the box is completely captured
 with p_HoBo_H (see center()).

 Because of this, an instance of Aabb is a frame-dependent quantity and should
 be expressed that way. For example, for a mesh measured and expressed in frame
 M, the bounding boxes on its triangles will be measured and expressed in the
 same frame.

 ```
 auto mesh_M = ...;
 Aabb bv_M = ...;  // A bounding volume for mesh_M in the same frame.
 ```
 */
class Aabb {
 public:
  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Aabb);

  /** The class used for various creation operations on this bounding volume. */
  template <typename MeshType>
  using Maker = AabbMaker<MeshType>;

  /** Constructs an axis-aligned bounding box measured and expressed in frame H.

   @param p_HoBo        The position vector from the hierarchy frame's origin to
                        the box's canonical origin, expressed in frame H. The
                        box is centered on Bo and aligned with Bx, By, and Bz.
   @param half_width    The _half_ measures of the box in each of the Bx, By,
                        and Bz directions. (Also the half measures in the Hx,
                        Hy, and Hz directions because R_HB = I.)
   @pre half_width.x(), half_width.y(), half_width.z() are not negative.
  */
  Aabb(const Vector3<double>& p_HoBo, const Vector3<double>& half_width)
      : center_(p_HoBo), half_width_(half_width) {
    DRAKE_DEMAND(half_width.x() >= 0.0);
    DRAKE_DEMAND(half_width.y() >= 0.0);
    DRAKE_DEMAND(half_width.z() >= 0.0);
  }

  /** Returns the center of the box -- equivalent to the position vector from
   the hierarchy frame's origin Ho to `this` box's origin Bo: `p_HoBo_H`. */
  const Vector3<double>& center() const { return center_; }

  /** Returns the half_width. */
  const Vector3<double>& half_width() const { return half_width_; }

  /** The point on the axis-aligned box with the smallest measures along the
   Bx-, By-, and Bz-directions. */
  Vector3<double> lower() const { return center_ - half_width_; }

  /** The point on the axis-aligned box with the largest measures along the Bx-,
   By-, and Bz-directions. */
  Vector3<double> upper() const { return center_ + half_width_; }

  // TODO(SeanCurtis-TRI): I've added this to be compatible with the Obb in
  //  terms of the generic BVH. The generic BVH sorts the vertices along a
  //  particular axis. For aabb. the axis is always one of the basis axes,
  //  therefore, it is sufficient to simply compare two floats. For Obb, we have
  //  to compare two dot products; I dot each vertex position w.r.t. one of the
  //  axes of the containing Obb. It would be better if this operation weren't
  //  built into the BVH but a function of the geometry type.
  //  The challenge in doing this is *what* I'm sorting. "CentroidPair" is
  //  defined inside the Bvh. I'll need to do some refactoring so that I don't
  //  get a circular dependency or any such thing (Or do things in a weird
  //  template-y fashion).
  /** Returns the pose X_HB of the box frame B in the hierarchy frame H. */
  math::RigidTransformd pose() const { return math::RigidTransformd{center_}; }

  /** @return Volume of the bounding box.  */
  double CalcVolume() const {
    // Double the three half widths using * 8 instead of repeating * 2 three
    // times to help the compiler out.
    return half_width_[0] * half_width_[1] * half_width_[2] * 8;
  }

  /** Reports whether the two axis-aligned bounding boxes `a_G` and `b_H`
   intersect. The poses of `a_G` and `b_H` are defined in their corresponding
   hierarchy frames G and H, respectively.

   @param a_G       The first axis-aligned box.
   @param b_H       The second axis-aligned box.
   @param X_GH      The relative pose between hierarchy frame G and hierarchy
                    frame H.
   @returns `true` if the boxes intersect.
   @pydrake_mkdoc_identifier{aabb_aabb} */
  static bool HasOverlap(const Aabb& a_G, const Aabb& b_H,
                         const math::RigidTransformd& X_GH);

  /** Reports whether axis-aligned bounding box `aabb_G` intersects the given
   oriented bounding box `obb_H`. The poses of `aabb_G` and `obb_H` are defined
   in their corresponding hierarchy frames G and H, respectively.

   @param aabb_G   The axis-aligned box.
   @param obb_H     The oriented box.
   @param X_GH      The relative pose between the aabb hierarchy frame G and the
                    obb hierarchy frame H.
   @returns `true` if the boxes intersect.
   @pydrake_mkdoc_identifier{aabb_obb} */
  static bool HasOverlap(const Aabb& aabb_G, const Obb& obb_H,
                         const math::RigidTransformd& X_GH);

  /** Checks whether bounding volume `bv` intersects the given plane. The
   bounding volume is centered on its canonical frame B, and B is posed in the
   corresponding hierarchy frame H. The plane is defined in frame P.

   The box and plane intersect if _any_ point within the bounding volume has
   zero height (see CalcHeight()).

   @param bv_H      The bounding box to test.
   @param plane_P   The plane to test against the `bv`. The plane is expressed
                    in frame P, therefore, to evaluate the height of a point
                    with respect to it, that point must be measured and
                    expressed in P.
   @param X_PH      The relative pose between the hierarchy frame H and the
                    plane frame P.
   @returns `true` if the plane intersects the box.
   @pydrake_mkdoc_identifier{aabb_plane} */
  static bool HasOverlap(const Aabb& bv_H, const Plane<double>& plane_P,
                         const math::RigidTransformd& X_PH);

  /** Checks whether bounding volume `bv` intersects the given half space. The
   bounding volume is centered on its canonical frame B, and B is posed in the
   corresponding hierarchy frame H. The half space is defined in its
   canonical frame C (such that the boundary plane of the half space is
   perpendicular to Cz and Co lies on the boundary plane).

   The box and halfspace intersect if _any_ point within the bounding volume has
   a height less than or equal to zero.

   @param bv_H      The bounding box to test.
   @param hs_C      The half space to test against the `bv`. The half space is
                    expressed in Frame C, therefore, to evaluate the signed
                    distance of a point with respect to it, that point must be
                    measured and expressed in C.
   @param X_CH      The relative pose between the hierarchy halfspace canonical
                    frame C and the box frame B.
   @returns `true` if the half space intersects the box.
   @pydrake_mkdoc_identifier{aabb_halfspace} */
  static bool HasOverlap(const Aabb& bv_H, const HalfSpace& hs_C,
                         const math::RigidTransformd& X_CH);

  /** Compares the values of the two Aabb instances for exact equality down to
   the last bit. Assumes that the quantities are measured and expressed in
   the same frame. */
  bool Equal(const Aabb& other) const {
    if (this == &other) return true;
    return center_ == other.center_ && half_width_ == other.half_width_;
  }

 private:
  // Allow the BvhUpdater access to set_bounds().
  template <typename>
  friend class internal::BvhUpdater;

  // Provides access to the BvhUpdater to refit the Aabb. Sets the extents of
  // the bounding box based on a box spanned by the given `lower` and `upper`
  // corners.
  // @pre lower(i) ≤ upper(i), ∀ i ∈ {0, 1, 2}.
  void set_bounds(const Vector3<double>& lower, const Vector3<double>& upper) {
    center_ = (lower + upper) / 2;
    half_width_ = (upper - lower) / 2;
  }

  // Center point of the box.
  Vector3<double> center_;
  // Half width extents along each axes.
  Vector3<double> half_width_;
};

/** %AabbMaker implements the logic to fit an Aabb to a collection of points.
 The points are the position of a subset of vertices in a mesh. The Aabb will
 be measured and expressed in the same frame as the mesh.

 This serves as the interface to Bvh, allowing the Bvh to fit volumes to
 geometry without knowing the details of the bounding volume types.

 @tparam MeshType is either TriangleSurfaceMesh<T> or VolumeMesh<T>, where T is
         double or AutoDiffXd.  */
template <class MeshType>
class AabbMaker {
 public:
  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(AabbMaker);

  /** Constructs the maker with the reference mesh and the subset of vertices to
   fit (indicated by corresponding index).

   @param mesh_M     The mesh frame M.
   @param vertices   The subset of vertices to fit.
   @pre `vertices` is not empty, and each of its entry is in the
        range [0, mesh_M.num_vertices()).  */
  AabbMaker(const MeshType& mesh_M, const std::set<int>& vertices)
      : mesh_M_(mesh_M), vertices_(vertices) {
    DRAKE_DEMAND(vertices_.size() > 0);
  }

  /** Computes the bounding volume of the vertices specified in the constructor.
   @retval aabb_M    The axis-aligned bounding box posed in mesh frame M.  */
  Aabb Compute() const;

 private:
  const MeshType& mesh_M_;
  const std::set<int>& vertices_;
};

}  // namespace geometry
}  // namespace drake
