/* +------------------------------------------------------------------------+
   |                     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/optional_ref.h>
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/poses/CPose3DPDF.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/poses/poses_frwds.h>
#include <mrpt/system/COutputLogger.h>

namespace mrpt::slam
{
/** Used as base class for other result structures of each particular algorithm
 * in CMetricMapsAlignmentAlgorithm derived classes.
 *
 * \ingroup mrpt_slam_grp
 */
struct TMetricMapAlignmentResult
{
	TMetricMapAlignmentResult() = default;
	virtual ~TMetricMapAlignmentResult() = default;

	double executionTime = 0;
};

/** A base class for any algorithm able of maps alignment. There are two methods
 *   depending on an PDF or a single 2D Pose value is available as initial guess
 * for the methods.
 *
 * \sa CPointsMap
 * \ingroup mrpt_slam_grp
 */
class CMetricMapsAlignmentAlgorithm : public mrpt::system::COutputLogger
{
   public:
	CMetricMapsAlignmentAlgorithm()
		: mrpt::system::COutputLogger("CMetricMapsAlignmentAlgorithm")
	{
	}
	/** Dtor */
	~CMetricMapsAlignmentAlgorithm() override = default;

	/** The method for aligning a pair of metric maps, for SE(2) relative poses.
	 * The meaning of some parameters and the kind of the maps to be aligned
	 * are implementation dependant, so look at the derived classes for
	 * instructions. The target is to find a PDF for the pose displacement
	 * between maps, **thus the pose of m2 relative to m1**. This pose is
	 * returned as a PDF rather than a single value.
	 *
	 * \param m1			[IN] The first map
	 * \param m2			[IN] The second map. The pose of this map respect to
	 * m1 is to be estimated.
	 * \param grossEst		[IN] An initial gross estimation for the
	 * displacement. If a given algorithm doesn't need it, set to
	 * `CPose2D(0,0,0)` for example.
	 * \param runningTime	[OUT] A pointer to a container for obtaining the
	 * algorithm running time in seconds, or nullptr if you don't need it.
	 * \param info			[OUT] See derived classes for details, or nullptr if
	 * it
	 * isn't needed.
	 *
	 * \return A smart pointer to the output estimated pose PDF.
	 * \sa CICP
	 */
	mrpt::poses::CPosePDF::Ptr Align(
		const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
		const mrpt::poses::CPose2D& grossEst,
		mrpt::optional_ref<TMetricMapAlignmentResult> outInfo = std::nullopt);

	/** \overload of Align() with a PDF for the initial pose.
	 *
	 * \return A smart pointer to the output estimated pose PDF.
	 * \sa CICP
	 */
	virtual mrpt::poses::CPosePDF::Ptr AlignPDF(
		const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
		const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
		mrpt::optional_ref<TMetricMapAlignmentResult> outInfo =
			std::nullopt) = 0;

	/** \overload of Align() for SE(3) poses.
	 *
	 * \return A smart pointer to the output estimated pose PDF.
	 * \sa CICP
	 */
	mrpt::poses::CPose3DPDF::Ptr Align3D(
		const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
		const mrpt::poses::CPose3D& grossEst,
		mrpt::optional_ref<TMetricMapAlignmentResult> outInfo = std::nullopt);

	/** \overload of Align3D() for initial guess given as a pose PDF.
	 *
	 * \return A smart pointer to the output estimated pose PDF.
	 * \sa CICP
	 */
	virtual mrpt::poses::CPose3DPDF::Ptr Align3DPDF(
		const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
		const mrpt::poses::CPose3DPDFGaussian& initialEstimationPDF,
		mrpt::optional_ref<TMetricMapAlignmentResult> outInfo =
			std::nullopt) = 0;
};

}  // namespace mrpt::slam
