/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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/common.h>  // MRPT_IS_X86_AMD64
#include <mrpt/math/TPoint3D.h>
#include <mrpt/poses/poses_frwds.h>
#include <mrpt/serialization/serialization_frwds.h>
#include <mrpt/typemeta/TTypeName.h>
#include <mrpt/typemeta/static_string.h>

#include <cstdint>
#include <iosfwd>
#include <string>
#include <vector>

namespace mrpt::tfest
{
/** \addtogroup mrpt_tfest_grp
 * @{ */

// Pragma defined to ensure no structure packing, so we can use SSE2
// vectorization on parts of this struture
#if defined(MRPT_IS_X86_AMD64)
#pragma pack(push, 1)
#endif

/** A structure for holding correspondences between two sets of points or
 * points-like entities in 2D or 3D. Templatized version for double or float.
 *
 * \note Before MRPT 2.4.0, "local" and "global" points were named "other", and
 * "this", respectively, and were not packed into a TPoint structure.
 */
template <typename T>
struct TMatchingPairTempl
{
	TMatchingPairTempl() = default;

	TMatchingPairTempl(
		uint32_t _globalIdx, uint32_t _localIdx, T _global_x, T _global_y,
		T _global_z, T _local_x, T _local_y, T _local_z)
		: globalIdx(_globalIdx),
		  localIdx(_localIdx),
		  global(_global_x, _global_y, _global_z),
		  local(_local_x, _local_y, _local_z)
	{
	}

	TMatchingPairTempl(
		uint32_t _globalIdx, uint32_t _localIdx,
		const mrpt::math::TPoint3D_<T>& _global,
		const mrpt::math::TPoint3D_<T>& _local)
		: globalIdx(_globalIdx),
		  localIdx(_localIdx),
		  global(_global),
		  local(_local)
	{
	}

	uint32_t globalIdx{0};
	uint32_t localIdx{0};
	mrpt::math::TPoint3D_<T> global{0, 0, 0};
	mrpt::math::TPoint3D_<T> local{0, 0, 0};
	T errorSquareAfterTransformation{0};

	/** Print contents with format:
	 *  `[globalIdx -> localIdx] (global.{x,y,z}) -> (local.{x,y,z})`
	 */
	void print(std::ostream& o) const;

	constexpr static auto getClassName()
	{
		using namespace mrpt::typemeta;
		return literal("TMatchingPairTempl<") + TTypeName<T>::get() +
			literal(">");
	}
};

template <typename T>
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const TMatchingPairTempl<T>& obj)
{
	out << obj.globalIdx << obj.localIdx << obj.global.x << obj.global.y
		<< obj.global.z << obj.local.x << obj.local.y << obj.local.z
		<< obj.errorSquareAfterTransformation;
	return out;
}

template <typename T>
mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, TMatchingPairTempl<T>& obj)
{
	in >> obj.globalIdx >> obj.localIdx >> obj.global.x >> obj.global.y >>
		obj.global.z >> obj.local.x >> obj.local.y >> obj.local.z >>
		obj.errorSquareAfterTransformation;
	return in;
}

/** A structure for holding correspondences between two sets of points or
 * points-like entities in 2D or 3D. Using `float` to save space since large
 * point clouds are likely stored in local coordinates using `float`.
 *
 * \sa TMatchingPair_d
 */
using TMatchingPair = TMatchingPairTempl<float>;

/** A structure for holding correspondences between two sets of points or
 * points-like entities in 2D or 3D. Using `double` for maximum accuracy when
 * required.
 *
 * \sa TMatchingPair
 */
using TMatchingPair_d = TMatchingPairTempl<double>;

#if defined(MRPT_IS_X86_AMD64)
#pragma pack(pop)  // End of pack = 1
#endif

/** A list of TMatchingPair.
 */
template <typename T>
class TMatchingPairListTempl : public std::vector<TMatchingPairTempl<T>>
{
   public:
	using base_t = std::vector<TMatchingPairTempl<T>>;

	/** Checks if the given index from the "other" map appears in the list. */
	bool indexOtherMapHasCorrespondence(size_t idx) const;

	/** Saves the correspondences to a text file, with each line containing
	 * these columns:
	 *  `globalIdx localIdx gx gy gz lx ly lz errSqr`
	 *
	 * You can also use:
	 * \code
	 * for (const auto &c: corrs)
	 *  std::cout << c << "\n";
	 * \endcode
	 * for a human-friendly description of contents.
	 */
	void dumpToFile(const std::string& fileName) const;

	/** Saves the correspondences as a MATLAB script which draws them. */
	void saveAsMATLABScript(const std::string& filName) const;

	/** Computes the overall square error between the 2D points in the list of
	 * correspondences, given the 2D transformation "q"
	 *    \f[ \sum\limits_i e_i  \f]
	 *  Where \f$ e_i \f$ are the elements of the square error vector as
	 * computed by squareErrorVector()
	 * \sa squareErrorVector, overallSquareErrorAndPoints
	 */
	T overallSquareError(const mrpt::poses::CPose2D& q) const;

	/// \overload for SE(3) pose transformation `q`
	T overallSquareError(const mrpt::poses::CPose3D& q) const;

	/** Computes the overall square error between the 2D points in the list of
	 * correspondences, given the 2D transformation "q", and return the
	 * transformed points as well.
	 *    \f[ \sum\limits_i e_i  \f]
	 *  Where \f$ e_i \f$ are the elements of the square error vector as
	 * computed by squareErrorVector()
	 * \sa squareErrorVector
	 */
	T overallSquareErrorAndPoints(
		const mrpt::poses::CPose2D& q, std::vector<T>& xs,
		std::vector<T>& ys) const;

	/**  Returns a vector with the square error between each pair of
	 * correspondences in the list, given the SE(2) transformation `q`
	 *    Each element \f$ e_i \f$ is the square distance between the "this"
	 * (global) point and the "other" (local) point transformed through "q":
	 *    \f[ e_i = | x_{this} -  q \oplus x_{other}  |^2 \f]
	 * \sa overallSquareError
	 */
	void squareErrorVector(
		const mrpt::poses::CPose2D& q, std::vector<T>& out_sqErrs) const;

	/// \overload for SE(3) pose transformation `q`
	void squareErrorVector(
		const mrpt::poses::CPose3D& q, std::vector<T>& out_sqErrs) const;

	/**  Returns a vector with the square error between each pair of
	 * correspondences in the list and the transformed "other" (local) points,
	 * given the 2D transformation "q"
	 *    Each element \f$ e_i \f$ is the square distance between the "this"
	 * (global) point and the "other" (local) point transformed through "q":
	 *    \f[ e_i = | x_{this} -  q \oplus x_{other}  |^2 \f]
	 * \sa overallSquareError
	 */
	void squareErrorVector(
		const mrpt::poses::CPose2D& q, std::vector<T>& out_sqErrs,
		std::vector<T>& xs, std::vector<T>& ys) const;

	/** Test whether the given pair "p" is within the pairings */
	bool contains(const TMatchingPairTempl<T>& p) const;

	/** Creates a filtered list of pairings with those ones which have a single
	 * correspondence which coincides
	 * in both directions, i.e. the best pairing of element `i` in map `this`
	 * is the best match for element `j` in map `other`,
	 * and viceversa*/
	void filterUniqueRobustPairs(
		size_t num_elements_this_map,
		TMatchingPairListTempl<T>& out_filtered_list) const;
};

template <typename T>
std::ostream& operator<<(
	std::ostream& o, const mrpt::tfest::TMatchingPairTempl<T>& pairs)
{
	pairs.print(o);
	return o;
}

/** Comparison operator, first sorts by globalIdx, if equals, by localIdx
 */
template <typename T>
bool operator<(const TMatchingPairTempl<T>& a, const TMatchingPairTempl<T>& b)
{
	if (a.globalIdx == b.globalIdx) return (a.localIdx < b.localIdx);
	else
		return (a.globalIdx < b.globalIdx);
}

/** A comparison operator  */
template <typename T>
bool operator==(const TMatchingPairTempl<T>& a, const TMatchingPairTempl<T>& b)
{
	return (a.globalIdx == b.globalIdx) && (a.localIdx == b.localIdx);
}

/** A comparison operator */
template <typename T>
bool operator==(
	const TMatchingPairListTempl<T>& a, const TMatchingPairListTempl<T>& b)
{
	if (a.size() != b.size()) return false;
	for (auto it1 = a.begin(), it2 = b.begin(); it1 != a.end(); ++it1, ++it2)
		if (!((*it1) == (*it2))) return false;
	return true;
}

/** A list of TMatchingPair (T=float).  */
using TMatchingPairList = TMatchingPairListTempl<float>;

/** A list of TMatchingPair (T=double). */
using TMatchingPairList_d = TMatchingPairListTempl<double>;

/** @} */

}  // namespace mrpt::tfest
