/* +------------------------------------------------------------------------+
   |                     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/core/bits_math.h>
#include <mrpt/core/format.h>
#include <mrpt/math/TPoseOrPoint.h>
#include <mrpt/math/math_frwds.h>  // CMatrixFixed

#include <cmath>  // sqrt
#include <type_traits>

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

// Ensure 1-byte memory alignment, no additional stride bytes.
#pragma pack(push, 1)

/** Trivially copiable underlying data for TPoint3D
 * 1-byte memory packed, no padding]
 */
template <typename T>
struct TPoint3D_data
{
	/** X,Y,Z coordinates */
	T x, y, z;
};

/** Base template for TPoint3D and TPoint3Df.
 * [1-byte memory packed, no padding]
 * \ingroup geometry_grp
 */
template <typename T>
struct TPoint3D_ : public TPoseOrPoint,
				   public TPoint3D_data<T>,
				   public internal::ProvideStaticResize<TPoint3D_<T>>
{
	enum
	{
		static_size = 3
	};

	/** Default constructor. Initializes to zeros. */
	constexpr TPoint3D_() : TPoint3D_data<T>{0, 0, 0} {}
	/** Constructor from coordinates.  */
	constexpr TPoint3D_(T xx, T yy, T zz) : TPoint3D_data<T>{xx, yy, zz} {}

	/** Constructor from coordinates. */
	template <typename U>
	TPoint3D_(const TPoint3D_data<U>& p)
	{
		TPoint3D_data<T>::x = static_cast<T>(p.x);
		TPoint3D_data<T>::y = static_cast<T>(p.y);
		TPoint3D_data<T>::z = static_cast<T>(p.z);
	}

	/** Constructor from column vector. */
	template <typename U>
	TPoint3D_(const mrpt::math::CMatrixFixed<U, 3, 1>& m)
	{
		TPoint3D_data<T>::x = static_cast<T>(m[0]);
		TPoint3D_data<T>::y = static_cast<T>(m[1]);
		TPoint3D_data<T>::z = static_cast<T>(m[2]);
	}

	/** Implicit constructor from TPoint2D. Zeroes the z.
	 * \sa TPoint2D
	 */
	TPoint3D_(const TPoint2D_<T>& p);
	/**
	 * Constructor from TPose2D, losing information. Zeroes the z.
	 * \sa TPose2D
	 */
	explicit TPoint3D_(const TPose2D& p);
	/**
	 * Constructor from TPose3D, losing information.
	 * \sa TPose3D
	 */
	explicit TPoint3D_(const TPose3D& p);

	/** Return a copy of this object using type U for coordinates */
	template <typename U>
	[[nodiscard]] TPoint3D_<U> cast() const
	{
		return TPoint3D_<U>(
			static_cast<U>(this->x), static_cast<U>(this->y),
			static_cast<U>(this->z));
	}

	/** Builds from the first 3 elements of a vector-like object: [x y z]
	 *
	 * \tparam Vector It can be std::vector<double>, Eigen::VectorXd, etc.
	 */
	template <typename Vector>
	static TPoint3D FromVector(const Vector& v)
	{
		TPoint3D o;
		for (int i = 0; i < 3; i++)
			o[i] = v[i];
		return o;
	}

	/** Coordinate access using operator[]. Order: x,y,z */
	T& operator[](size_t i)
	{
		switch (i)
		{
			case 0: return TPoint3D_data<T>::x;
			case 1: return TPoint3D_data<T>::y;
			case 2: return TPoint3D_data<T>::z;
			default: throw std::out_of_range("index out of range");
		}
	}
	/** Coordinate access using operator[]. Order: x,y,z */
	constexpr T operator[](size_t i) const
	{
		switch (i)
		{
			case 0: return TPoint3D_data<T>::x;
			case 1: return TPoint3D_data<T>::y;
			case 2: return TPoint3D_data<T>::z;
			default: throw std::out_of_range("index out of range");
		}
	}
	/**
	 * Point-to-point distance.
	 */
	T distanceTo(const TPoint3D_<T>& p) const
	{
		return std::sqrt(
			square(p.x - TPoint3D_data<T>::x) +
			square(p.y - TPoint3D_data<T>::y) +
			square(p.z - TPoint3D_data<T>::z));
	}
	/**
	 * Point-to-point distance, squared.
	 */
	T sqrDistanceTo(const TPoint3D_<T>& p) const
	{
		return square(p.x - TPoint3D_data<T>::x) +
			square(p.y - TPoint3D_data<T>::y) +
			square(p.z - TPoint3D_data<T>::z);
	}
	/** Squared norm: `|v|^2 = x^2+y^2+z^2` */
	T sqrNorm() const
	{
		return square(TPoint3D_data<T>::x) + square(TPoint3D_data<T>::y) +
			square(TPoint3D_data<T>::z);
	}

	/** Point norm: `|v| = sqrt(x^2+y^2+z^2)` */
	T norm() const { return std::sqrt(sqrNorm()); }

	/** Returns this vector with unit length: v/norm(v) */
	[[nodiscard]] TPoint3D_<T> unitarize() const
	{
		const T n = norm();
		ASSERT_GT_(n, 0);
		const T f = 1 / n;
		return {
			TPoint3D_data<T>::x * f, TPoint3D_data<T>::y * f,
			TPoint3D_data<T>::z * f};
	}

	/** Scale point/vector */
	TPoint3D_<T>& operator*=(const T f)
	{
		TPoint3D_data<T>::x *= f;
		TPoint3D_data<T>::y *= f;
		TPoint3D_data<T>::z *= f;
		return *this;
	}
	/** Gets the pose as a vector of doubles.
	 * \tparam Vector It can be std::vector<double>, Eigen::VectorXd, etc.
	 */
	template <typename Vector>
	void asVector(Vector& v) const
	{
		v.resize(3);
		v[0] = TPoint3D_data<T>::x;
		v[1] = TPoint3D_data<T>::y;
		v[2] = TPoint3D_data<T>::z;
	}
	/// \overload
	template <typename Vector>
	[[nodiscard]] Vector asVector() const
	{
		Vector v;
		asVector(v);
		return v;
	}

	/**
	 * Translation.
	 */
	TPoint3D_<T>& operator+=(const TPoint3D_<T>& p)
	{
		TPoint3D_data<T>::x += p.x;
		TPoint3D_data<T>::y += p.y;
		TPoint3D_data<T>::z += p.z;
		return *this;
	}
	/**
	 * Difference between points.
	 */
	TPoint3D_<T>& operator-=(const TPoint3D_<T>& p)
	{
		TPoint3D_data<T>::x -= p.x;
		TPoint3D_data<T>::y -= p.y;
		TPoint3D_data<T>::z -= p.z;
		return *this;
	}
	/**
	 * Points addition.
	 */
	[[nodiscard]] constexpr TPoint3D_<T> operator+(const TPoint3D_<T>& p) const
	{
		return {
			TPoint3D_data<T>::x + p.x, TPoint3D_data<T>::y + p.y,
			TPoint3D_data<T>::z + p.z};
	}
	/**
	 * Points substraction.
	 */
	[[nodiscard]] constexpr TPoint3D_<T> operator-(const TPoint3D_<T>& p) const
	{
		return {
			TPoint3D_data<T>::x - p.x, TPoint3D_data<T>::y - p.y,
			TPoint3D_data<T>::z - p.z};
	}

	[[nodiscard]] constexpr TPoint3D_<T> operator*(T d) const
	{
		return {
			TPoint3D_data<T>::x * d, TPoint3D_data<T>::y * d,
			TPoint3D_data<T>::z * d};
	}

	[[nodiscard]] constexpr TPoint3D_<T> operator/(T d) const
	{
		return {
			TPoint3D_data<T>::x / d, TPoint3D_data<T>::y / d,
			TPoint3D_data<T>::z / d};
	}

	bool operator<(const TPoint3D_<T>& p) const;

	/** Returns a human-readable textual representation of the object (eg:
	 * "[0.02 1.04 -0.8]" )
	 * \sa fromString
	 */
	void asString(std::string& s) const
	{
		s = mrpt::format(
			"[%f %f %f]", TPoint3D_data<T>::x, TPoint3D_data<T>::y,
			TPoint3D_data<T>::z);
	}
	[[nodiscard]] std::string asString() const
	{
		std::string s;
		asString(s);
		return s;
	}

	/** Set the current object value from a string generated by 'asString' (eg:
	 * "[0.02 1.04 -0.8]" )
	 * \sa asString
	 * \exception std::exception On invalid format
	 */
	void fromString(const std::string& s);

	[[nodiscard]] static TPoint3D_<T> FromString(const std::string& s)
	{
		TPoint3D_<T> o;
		o.fromString(s);
		return o;
	}
};

/** Lightweight 3D point. Allows coordinate access using [] operator.
 * (1-byte memory packed, no padding).
 * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
 */
using TPoint3D = TPoint3D_<double>;
using TPoint3Df = TPoint3D_<float>;

/** Useful type alias for 3-vectors.
 * (1-byte memory packed, no padding) */
using TVector3D = TPoint3D;
using TVector3Df = TPoint3Df;

/** XYZ point (double) + Intensity(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZIu8
{
	mrpt::math::TPoint3D pt;
	uint8_t intensity{0};
	TPointXYZIu8() : pt() {}
	constexpr TPointXYZIu8(double x, double y, double z, uint8_t intensity_val)
		: pt(x, y, z), intensity(intensity_val)
	{
	}
};
/** XYZ point (double) + RGB(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZRGBu8
{
	mrpt::math::TPoint3D pt;
	uint8_t r{0}, g{0}, b{0};
	TPointXYZRGBu8() = default;
	constexpr TPointXYZRGBu8(
		double x, double y, double z, uint8_t R_val, uint8_t G_val,
		uint8_t B_val)
		: pt(x, y, z), r(R_val), g(G_val), b(B_val)
	{
	}
};
/** XYZ point (float) + Intensity(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZfIu8
{
	mrpt::math::TPoint3Df pt;
	uint8_t intensity{0};
	TPointXYZfIu8() = default;
	constexpr TPointXYZfIu8(float x, float y, float z, uint8_t intensity_val)
		: pt(x, y, z), intensity(intensity_val)
	{
	}
};
/** XYZ point (float) + RGB(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZfRGBu8
{
	mrpt::math::TPoint3Df pt;
	uint8_t r{0}, g{0}, b{0};
	TPointXYZfRGBu8() : pt() {}
	constexpr TPointXYZfRGBu8(
		float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
		: pt(x, y, z), r(R_val), g(G_val), b(B_val)
	{
	}
};

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

/** XYZ point (float) + RGBA(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZfRGBAu8
{
	mrpt::math::TPoint3Df pt;
	uint8_t r{0}, g{0}, b{0}, a{0xff};
	TPointXYZfRGBAu8() : pt() {}
	constexpr TPointXYZfRGBAu8(
		float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val,
		uint8_t A_val = 0xff)
		: pt(x, y, z), r(R_val), g(G_val), b(B_val), a(A_val)
	{
	}
};

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

/** XYZ point (float) + RGBA(float) [1-byte memory packed, no padding]
 * \sa mrpt::math::TPoint3D */
struct TPointXYZRGBAf
{
	mrpt::math::TPoint3Df pt;
	float R{0}, G{0}, B{0}, A{0};
	TPointXYZRGBAf() = default;

	constexpr TPointXYZRGBAf(
		float x, float y, float z, float R_val, float G_val, float B_val,
		float A_val)
		: pt(x, y, z), R(R_val), G(G_val), B(B_val), A(A_val)
	{
	}
};
#pragma pack(pop)

/** Unary minus operator for 3D points/vectors. */
template <typename T>
constexpr TPoint3D_<T> operator-(const TPoint3D_<T>& p1)
{
	return {-p1.x, -p1.y, -p1.z};
}

/** scalar times vector operator. */
template <
	typename T, typename Scalar,
	std::enable_if_t<std::is_convertible_v<Scalar, T>>* = nullptr>
constexpr TPoint3D_<T> operator*(const Scalar scalar, const TPoint3D_<T>& p)
{
	return {scalar * p.x, scalar * p.y, scalar * p.z};
}

/** Exact comparison between 3D points */
template <typename T>
constexpr bool operator==(const TPoint3D_<T>& p1, const TPoint3D_<T>& p2)
{
	return (p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z);	//-V550
}
/** Exact comparison between 3D points */
template <typename T>
constexpr bool operator!=(const TPoint3D_<T>& p1, const TPoint3D_<T>& p2)
{
	return (p1.x != p2.x) || (p1.y != p2.y) || (p1.z != p2.z);	//-V550
}

/** @} */

}  // namespace mrpt::math

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