/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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/math/TPoseOrPoint.h>

#include <array>

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

/** 2D line without bounds, represented by its equation \f$Ax+By+C=0\f$.
 * \sa TLine3D,TSegment2D,TPolygon2D,TPoint2D
 */
struct TLine2D
{
   public:
	/** Constructor from two points, through which the line will pass.
	 * \throw logic_error if both points are the same
	 * \sa mrpt::math::getRegressionLine()
	 */
	TLine2D(const TPoint2D& p1, const TPoint2D& p2);

	/** Constructor from a segment */
	explicit TLine2D(const TSegment2D& s);

	/** Fast default constructor. Initializes to undefined values. */
	TLine2D() = default;

	/** Constructor from line's coefficients */
	constexpr TLine2D(double A, double B, double C) : coefs{A, B, C} {}

	/** Construction from 3D object, discarding the Z.
	 * \throw std::logic_error if the line is normal to the XY plane.
	 */
	explicit TLine2D(const TLine3D& l);

	/** Static constructor from Ax+By+C=0 coefficients.
	 * \note [New in MRPT 2.0.4]
	 */
	static TLine2D FromCoefficientsABC(double A, double B, double C);

	/** Static constructor from two points.
	 * \note [New in MRPT 2.0.4]
	 */
	static TLine2D FromTwoPoints(const TPoint2D& p1, const TPoint2D& p2);

	/** Line coefficients, stored as an array: \f$\left[A,B,C\right]\f$ */
	std::array<double, 3> coefs{{0, 0, 0}};
	/**
	 * Evaluate point in the line's equation.
	 */
	double evaluatePoint(const TPoint2D& point) const;
	/**
	 * Check whether a point is inside the line.
	 */
	bool contains(const TPoint2D& point) const;
	/**
	 * Distance from a given point.
	 */
	double distance(const TPoint2D& point) const;
	/**
	 * Distance with sign from a given point (sign indicates side).
	 */
	double signedDistance(const TPoint2D& point) const;
	/**
	 * Get line's normal vector.
	 */
	void getNormalVector(double (&vector)[2]) const;
	/**
	 * Unitarize line's normal vector.
	 */
	void unitarize();
	/**
	 * Get line's normal vector after unitarizing line.
	 */
	void getUnitaryNormalVector(double (&vector)[2])
	{
		unitarize();
		getNormalVector(vector);
	}
	/**
	 * Get line's director vector.
	 */
	void getDirectorVector(double (&vector)[2]) const;
	/**
	 * Unitarize line and then get director vector.
	 */
	void getUnitaryDirectorVector(double (&vector)[2])
	{
		unitarize();
		getDirectorVector(vector);
	}
	/**
	 * Project into 3D space, setting the z to 0.
	 */
	void generate3DObject(TLine3D& l) const;

	void getAsPose2D(TPose2D& outPose) const;
	void getAsPose2DForcingOrigin(
		const TPoint2D& origin, TPose2D& outPose) const;

	/** Returns "[A, B, C]"
	 * \note [New in MRPT 2.1.0]
	 *
	 * \note Do not inherit from mrpt::Stringifyable to avoid virtual class
	 * table and keeping the class trivially-copiable.
	 */
	std::string asString() const;
};

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

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

/** @} */

}  // namespace mrpt::math

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