/* +------------------------------------------------------------------------+
   |                     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/poses/Lie/SE.h>
#include <mrpt/system/datetime.h>

#include <map>
#include <string>

namespace mrpt::poses
{
enum FrameLookUpStatus
{
	LKUP_GOOD = 0,
	LKUP_UNKNOWN_FRAME,
	LKUP_NO_CONNECTIVITY,
	LKUP_EXTRAPOLATION_ERROR
};

/** Virtual base class for interfaces to a [ROS
 * tf2](http://wiki.ros.org/tf2)-like
 * service capable of "publishing" and "looking-up" relative poses between two
 * "coordinate frames".
 * Use derived classes for:
 *  - wrapping real ROS tf (TO-DO in
 * [mrpt-bridge](http://wiki.ros.org/mrpt_bridge)), or
 *  - using a pure MRPT standalone TF service with mrpt::poses::FrameTransformer
 *
 * Frame IDs are strings.
 * MRPT modules use the standard ROS [REP
 * 105](http://www.ros.org/reps/rep-0105.html#coordinate-frames)
 * document regarding common names for frames:
 *  - `base_link`: "the robot"
 *  - `odom`: Origin for odometry
 *  - `map`: Origin for "the map"
 *
 * \tparam DIM Can be 2 for SE(2), 2D transformations; or 3 for SE(3), 3D
 * transformations.
 * \ingroup poses_grp
 * \sa FrameTransformer, CPose3D
 */
template <int DIM>
class FrameTransformerInterface
{
   public:
	/** This will be mapped to CPose2D (DIM=2) or CPose3D (DIM=3) */
	using pose_t = typename Lie::SE<DIM>::type;
	/** This will be mapped to mrpt::math::TPose2D (DIM=2) or
	 * mrpt::math::TPose3D (DIM=3) */
	using light_type = typename Lie::SE<DIM>::light_type;

	FrameTransformerInterface();
	virtual ~FrameTransformerInterface();

	/** Publish a time-stampped transform between two frames */
	virtual void sendTransform(
		const std::string& parent_frame, const std::string& child_frame,
		const pose_t& child_wrt_parent,
		const mrpt::system::TTimeStamp& timestamp = mrpt::system::now()) = 0;

	/** Queries the current pose of `target_frame` wrt ("as seen from")
	 * `source_frame`.
	 * It tries to return the pose at the given timepoint, unless it is
	 * INVALID_TIMESTAMP (default),
	 * which means returning the latest know transformation.
	 */
	virtual FrameLookUpStatus lookupTransform(
		const std::string& target_frame, const std::string& source_frame,
		light_type& child_wrt_parent,
		const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP,
		/** Timeout */
		const double timeout_secs = .0) = 0;

};	// End of class def.

/** See docs in FrameTransformerInterface.
 *   This class is an implementation for standalone (non ROS) applications.
 * \ingroup poses_grp
 * \sa FrameTransformerInterface
 */
template <int DIM>
class FrameTransformer : public FrameTransformerInterface<DIM>
{
   public:
	using base_t = FrameTransformerInterface<DIM>;

	FrameTransformer();
	~FrameTransformer() override;

	// See base docs
	void sendTransform(
		const std::string& parent_frame, const std::string& child_frame,
		const typename base_t::pose_t& child_wrt_parent,
		const mrpt::system::TTimeStamp& timestamp =
			mrpt::system::now()) override;
	// See base docs
	FrameLookUpStatus lookupTransform(
		const std::string& target_frame, const std::string& source_frame,
		typename base_t::light_type& child_wrt_parent,
		const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP,
		const double timeout_secs = .0) override;

	/** \overload */
	FrameLookUpStatus lookupTransform(
		const std::string& target_frame, const std::string& source_frame,
		typename base_t::pose_t& child_wrt_parent,
		const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP,
		/** Timeout */
		const double timeout_secs = .0)
	{
		typename base_t::light_type p;
		FrameLookUpStatus ret = lookupTransform(
			target_frame, source_frame, p, query_time, timeout_secs);
		child_wrt_parent = typename base_t::pose_t(p);
		return ret;
	}

   protected:
	// double m_max_extrapolation_time;  //!< for extrapolation in the past or
	// in the future [s]
	// double m_max_age_pose_cache;      //!< Max age of stored poses [s]

	struct TF_TreeEdge
	{
		// TODO: CPose{2,3}DInterpolator?
		typename base_t::pose_t pose;
		mrpt::system::TTimeStamp timestamp;

		TF_TreeEdge(
			const typename base_t::pose_t& pose_,
			const mrpt::system::TTimeStamp& timestamp_)
			: pose(pose_), timestamp(timestamp_)
		{
		}
		TF_TreeEdge() : timestamp(INVALID_TIMESTAMP) {}
	};

	// map: [parent] -> { [child] -> relPoseChildWRTParent }
	using pose_tree_t =
		std::map<std::string, std::map<std::string, TF_TreeEdge>>;
	pose_tree_t m_pose_edges_buffer;
};

}  // namespace mrpt::poses
