/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/exceptions.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/serialization/serialization_frwds.h>
#include <mrpt/typemeta/TTypeName.h>

#include <algorithm>  // max()
#include <optional>

namespace mrpt::math
{
/** \addtogroup  geometry_grp
 * @{ */

/** A bounding box defined by the 3D points at each minimum-maximum corner.
 */
template <typename T>
struct TBoundingBox_
{
  enum class CTOR_FLAGS
  {
    NoFlags = 0,
    AllowUnordered
  };

  TBoundingBox_() = default;

  /** Ctor from min-max corners.
   * A bounding box may have a zero volume if max==min.
   * It is ilegal for a coordinate of the `max` vector to be smaller than its
   * `min` counterpart, in which case an exception will be thrown, except if
   * the flag CTOR_FLAGS::AllowUnordered is passed.
   */
  TBoundingBox_(
      const mrpt::math::TPoint3D_<T>& Min,
      const mrpt::math::TPoint3D_<T>& Max,
      const CTOR_FLAGS f = CTOR_FLAGS::NoFlags) :
      min(Min), max(Max)
  {
    if (f != CTOR_FLAGS::AllowUnordered)
      ASSERT_(max.x >= min.x && max.y >= min.y && max.z >= min.z);
  }

  /** The corners of the bounding box */
  mrpt::math::TPoint3D_<T> min, max;

  /** Initialize with min=+Infinity, max=-Infinity. This is useful as an
   * initial value before processing a list of points to keep their
   * minimum/maximum. */
  [[nodiscard]] static TBoundingBox_<T> PlusMinusInfinity()
  {
    const T i = std::numeric_limits<T>::max();
    return {
        { i,  i,  i},
        {-i, -i, -i},
        CTOR_FLAGS::AllowUnordered
    };
  }

  /** Construct a bounding box from two points, by selecting their minimum and
   * maximum (x,y,z) coordinates, so the point coordinates do not need to be
   * already sorted by the user as one being the minimum and maximum corners.
   * \note (New in MRPT 2.5.6)
   */
  [[nodiscard]] static TBoundingBox_<T> FromUnsortedPoints(
      const mrpt::math::TPoint3D_<T>& pt1, const mrpt::math::TPoint3D_<T>& pt2)
  {
    TBoundingBox_<T> r;
    r.min.x = std::min(pt1.x, pt2.x);
    r.min.y = std::min(pt1.y, pt2.y);
    r.min.z = std::min(pt1.z, pt2.z);
    r.max.x = std::max(pt1.x, pt2.x);
    r.max.y = std::max(pt1.y, pt2.y);
    r.max.z = std::max(pt1.z, pt2.z);
    return r;
  }

  /** Returns the volume of the box */
  [[nodiscard]] T volume() const { return (max.x - min.x) * (max.y - min.y) * (max.z - min.z); }

  /** Returns the intersection of this bounding box with "b", or std::nullopt
   * if no intersection exists.
   * Note that borders are enlarged by "epsilon" before to testing for
   * intersection to handle numerical innacuracies, for example on planar
   * bounding boxes with a fixed "z".
   */
  [[nodiscard]] std::optional<TBoundingBox_<T>> intersection(
      const TBoundingBox_<T>& b, const T epsilon = static_cast<T>(1e-4)) const
  {
    if (b.min.x - epsilon > max.x || b.min.y - epsilon > max.y || b.min.z - epsilon > max.z ||
        b.max.x + epsilon < min.x || b.max.y + epsilon < min.y || b.max.z + epsilon < min.z)
      return {};

    return {TBoundingBox_<T>(
        {std::max(min.x, b.min.x), std::max(min.y, b.min.y), std::max(min.z, b.min.z)},
        {std::min(max.x, b.max.x), std::min(max.y, b.max.y), std::min(max.z, b.max.z)},
        CTOR_FLAGS::AllowUnordered)};
  }

  /** Returns the union of this bounding box with "b", i.e. a new bounding box
   * comprising both `this` and `b` */
  [[nodiscard]] TBoundingBox_<T> unionWith(const TBoundingBox_<T>& b) const
  {
    return {TBoundingBox_<T>(
        {std::min(min.x, b.min.x), std::min(min.y, b.min.y), std::min(min.z, b.min.z)},
        {std::max(max.x, b.max.x), std::max(max.y, b.max.y), std::max(max.z, b.max.z)})};
  }

  /** Expands the box limits to include the given point */
  void updateWithPoint(const mrpt::math::TPoint3D_<T>& p)
  {
    mrpt::keep_min(min.x, p.x);
    mrpt::keep_min(min.y, p.y);
    mrpt::keep_min(min.z, p.z);

    mrpt::keep_max(max.x, p.x);
    mrpt::keep_max(max.y, p.y);
    mrpt::keep_max(max.z, p.z);
  }

  /** Returns true if the point lies within the bounding box (including the
   * exact border)
   * \note (New in MRPT 2.3.3)
   */
  [[nodiscard]] bool containsPoint(const mrpt::math::TPoint3D_<T>& p) const
  {
    return p.x >= min.x && p.y >= min.y && p.z >= min.z && p.x <= max.x && p.y <= max.y &&
           p.z <= max.z;
  }

  /** Returns a new bounding box, transforming `this` from local coordinates
   * to global coordinates, as if `this` was given with respect to `pose`, ie:
   *
   * \code
   *  return.min = pose \oplus this->min
   *  return.max = pose \oplus this->max
   * \endcode
   * \tparam POSE_T Can be mrpt::poses::CPose3D, or mrpt::math::TPose3D
   *
   * \note If a rotation exists, the output bounding box will no longer be an
   * accurate representation of the actual 3D box.
   */
  template <typename POSE_T>
  [[nodiscard]] TBoundingBox_<T> compose(const POSE_T& pose) const
  {
    return FromUnsortedPoints(pose.composePoint(min), pose.composePoint(max));
  }

  /** Returns a new bounding box, transforming `this` from global coordinates
   * to local coordinates with respect to `pose`, ie:
   *
   * \code
   *  return.min = this->min \ominus pose
   *  return.max = this->max \ominus pose
   * \endcode
   * \tparam POSE_T Can be mrpt::poses::CPose3D, or mrpt::math::TPose3D
   *
   * \note If a rotation exists, the output bounding box will no longer be an
   * accurate representation of the actual 3D box.
   */
  template <typename POSE_T>
  [[nodiscard]] TBoundingBox_<T> inverseCompose(const POSE_T& pose) const
  {
    return FromUnsortedPoints(pose.inverseComposePoint(min), pose.inverseComposePoint(max));
  }

  /** Print bounding box as a string with format
   * "(minx,miny,minz)-(maxx,maxy,maxz)"
   *
   * \note Do not inherit from mrpt::Stringifyable to avoid virtual class
   * table and keeping the class trivially-copiable.
   */
  [[nodiscard]] std::string asString() const
  {
    std::string s = min.asString();
    s += "-";
    s += max.asString();
    return s;
  }

  [[nodiscard]] bool operator==(const TBoundingBox_<T>& o) const
  {
    return max == o.max && min == o.min;
  }

  [[nodiscard]] bool operator!=(const TBoundingBox_<T>& o) const
  {
    return max != o.max || min != o.min;
  }
};

/** A bounding box defined by the 3D points at each minimum-maximum corner.
 * \sa mrpt::math::TPoint3D, mrpt::math::TPoint3Df
 */
using TBoundingBox = TBoundingBox_<double>;
using TBoundingBoxf = TBoundingBox_<float>;

mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TBoundingBoxf& bb);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TBoundingBoxf& bb);

mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TBoundingBox& bb);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TBoundingBox& bb);

/** @} */

}  // namespace mrpt::math

namespace mrpt::typemeta
{
// Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TBoundingBox, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TBoundingBoxf, mrpt::math)
}  // namespace mrpt::typemeta
