/* +------------------------------------------------------------------------+
   |                     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/optional_ref.h>
#include <mrpt/math/TPoint3D.h>

#include <array>
#include <optional>

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

/** 3D line, represented by a base point and a director vector.
 * \sa TLine2D,TSegment3D,TPlane,TPolygon3D,TPoint3D
 */
struct TLine3D
{
 public:
  /** Fast default constructor. Initializes to all zeros. */
  TLine3D() = default;

  /** Constructor from two points, through which the line will pass.
   * \throw std::logic_error if both points are the same.
   * \sa mrpt::math::getRegressionLine()
   */
  TLine3D(const TPoint3D& p1, const TPoint3D& p2);

  /** Constructor from 3D segment  */
  explicit TLine3D(const TSegment3D& s);

  /** Constructor from 2D object. Zeroes the z. */
  explicit TLine3D(const TLine2D& l);

  /** Static constructor from a point and a director vector.
   * \note [New in MRPT 2.0.4]
   */
  static TLine3D FromPointAndDirector(const TPoint3D& basePoint, const TVector3D& directorVector);

  /** Static constructor from two points.
   * \note [New in MRPT 2.0.4]
   * \sa mrpt::math::getRegressionLine()
   */
  static TLine3D FromTwoPoints(const TPoint3D& p1, const TPoint3D& p2);

  /** Base point */
  TPoint3D pBase;
  /** Director vector */
  TVector3D director{.0, .0, .0};
  /** Check whether a point is inside the line */
  bool contains(const TPoint3D& point) const;

  /** Absolute distance between the line and a point. */
  double distance(const TPoint3D& point) const;

  /** Closest point to `p` along the line. It is computed as the intersection
   * of `this` with the plane perpendicular to `this` that passes through `p`
   * \note [New in MRPT 2.3.1]
   */
  TPoint3D closestPointTo(const TPoint3D& p) const;

  /** Minimum distance between this and another line.
   * The return will be std::nullopt if lines are parallel, or zero if they
   * intersect, a positive number otherwise.
   * \note New in MRPT 2.3.0
   */
  std::optional<double> distance(
      const TLine3D& point,
      const mrpt::optional_ref<mrpt::math::TPoint3D>& outMidPoint = std::nullopt) const;
  /**
   * Unitarize director vector.
   */
  void unitarize();
  /** Get director vector */
  void getDirectorVector(double (&vector)[3]) const
  {
    for (size_t i = 0; i < 3; i++) vector[i] = director[i];
  }
  /** Get director vector (may be NOT unitary if not set so by the user) \sa
   * getUnitaryDirectorVector(), unitarize() */
  inline const TVector3D& getDirectorVector() const { return director; }

  /**
   * Unitarize and then get director vector.
   */
  void getUnitaryDirectorVector(double (&vector)[3])
  {
    unitarize();
    getDirectorVector(vector);
  }
  /**
   * Project into 2D space, discarding the Z coordinate.
   * \throw std::logic_error if the line's director vector is orthogonal to
   * the XY plane.
   */
  void generate2DObject(TLine2D& l) const;

  /** Returns "P=[x,y,z] u=[ux,uy,uz]"
   * \note [New in MRPT 2.1.0]
   */
  std::string asString() const;
};

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

/** Text streaming function */
std::ostream& operator<<(std::ostream& o, const TLine3D& p);

/** @} */

}  // namespace mrpt::math

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