#pragma once
#include "../camera/camera_geometry.hpp"
#include "../data/frame_base.hpp"
#include "../data/landmark_base.hpp"
#include "../data/utilities.hpp"
#include "../exception/exception.hpp"
#include "../kinematics/linear_transformer.hpp"
#include "../visualization/colors.hpp"

#include <opencv2/imgproc.hpp>

#include <iostream>
#include <map>

namespace dv::visualization {
/**
 * Visualize the current and past poses of objects as an image.
 */
class PoseVisualizer {
public:
	/**
	 * Pre-defined viewing modes; poses of the virtual camera used to visualize the 3D scene. The naming of the
	 * different view modes is based on the plane that the virtual camera sees in that mode, such that the virtual
	 * camera is placed perpendicular to that plane.
	 *
	 * The first letter in the viewing mode defines the horizontal axis of the plane, while the second letter defines
	 * the vertical axis of the plane.
	 */
	enum class ViewMode {
		CUSTOM = 0,
		VIEW_XY,
		VIEW_YZ,
		VIEW_ZX,
		VIEW_XZ,
		VIEW_YX,
		VIEW_ZY,
	};

	friend std::ostream &operator<<(std::ostream &os, const PoseVisualizer::ViewMode &var) {
		switch (var) {
			case PoseVisualizer::ViewMode::CUSTOM: {
				return os << "CUSTOM";
			}
			case PoseVisualizer::ViewMode::VIEW_XY: {
				return os << "VIEW_XY";
			}
			case PoseVisualizer::ViewMode::VIEW_YZ: {
				return os << "VIEW_YZ";
			}
			case PoseVisualizer::ViewMode::VIEW_ZX: {
				return os << "VIEW_ZX";
			}
			case PoseVisualizer::ViewMode::VIEW_XZ: {
				return os << "VIEW_XZ";
			}
			case PoseVisualizer::ViewMode::VIEW_YX: {
				return os << "VIEW_YX";
			}
			case PoseVisualizer::ViewMode::VIEW_ZY: {
				return os << "VIEW_ZY";
			}
		}

		return os;
	}

	/**
	 * Defined grid plane orientations that can be drawn into the 3D scene. The first letter defines the horizontal axis
	 * of the plane, while the second letter defines the vertical axis of the plane.
	 */
	enum class GridPlane {
		PLANE_NONE = 0,
		PLANE_XY,
		PLANE_YZ,
		PLANE_ZX,
	};

	friend std::ostream &operator<<(std::ostream &os, const PoseVisualizer::GridPlane &var) {
		switch (var) {
			case PoseVisualizer::GridPlane::PLANE_NONE: {
				return os << "PLANE_NONE";
			}
			case PoseVisualizer::GridPlane::PLANE_XY: {
				return os << "PLANE_XY";
			}
			case PoseVisualizer::GridPlane::PLANE_YZ: {
				return os << "PLANE_YZ";
			}
			case PoseVisualizer::GridPlane::PLANE_ZX: {
				return os << "PLANE_ZX";
			}
		}

		return os;
	}

	/**
	 * Initialize a pose visualizer to visualize the current and past poses of objects as an image. This is done by
	 * creating a virtual camera in the 3D scene, and using this camera to visualize object poses/landmarks in the
	 * 3D scene.
	 *
	 * The virtual camera is assumed to have an ideal pinhole camera model.
	 *
	 * @param resolution Resolution of the virtual camera used for visualizing the 3D scene. This is used to set
	 * the intrinsics of the virtual camera using an ideal pinhole camera model and defines the resolution of the
	 * generated image.
	 * @param backgroundColor Background color.
	 * @param gridColor Color of drawn grid plane.
	 * @param gridSpacing Spacing between drawn lines for grid plane, in physical units [m].
	 * @param maxTrajectoryLength Maximum number of poses that are visualized for each object.
	 * @param landmarkLimit Maximum number of landmarks that are visualized.
	 */
	explicit PoseVisualizer(const cv::Size &resolution = cv::Size(640, 480),
		const cv::Scalar &backgroundColor = colors::darkGray, const cv::Scalar &gridColor = colors::gray,
		const float gridSpacing = 1.0f, const size_t maxTrajectoryLength = 10'000,
		const size_t landmarkLimit = 10'000) :
		PoseVisualizer(static_cast<float>(resolution.width) / 2.f, static_cast<float>(resolution.height) / 2.f,
			static_cast<float>(resolution.width) / 2.f, static_cast<float>(resolution.height) / 2.f, resolution,
			backgroundColor, gridColor, gridSpacing, maxTrajectoryLength, landmarkLimit) {
	}

	/**
	 * Initialize a pose visualizer to visualize the current and past poses of objects as an image. This is done by
	 * creating a virtual camera in the 3D scene, and using this camera to visualize object poses/landmarks in the
	 * 3D scene.
	 *
	 * The virtual camera is assumed to have an ideal pinhole camera model.
	 *
	 * @param fx Focal length X for the virtual camera, measured in pixels.
	 * @param fy Focal length Y for the virtual camera, measured in pixels.
	 * @param cx Central point coordinate X for the virtual camera, in pixels.
	 * @param cy Central point coordinate Y for the virtual camera, in pixels.
	 * @param resolution Resolution of the virtual camera. This defines the resolution of the generated image.
	 * @param backgroundColor Background color.
	 * @param gridColor Color of drawn grid plane.
	 * @param gridSpacing Spacing between drawn lines for grid plane, in physical units [m]
	 * @param maxTrajectoryLength Maximum number of poses that are visualized for each object.
	 * @param landmarkLimit Maximum number of landmarks that are visualized.
	 */
	PoseVisualizer(const float fx, const float fy, const float cx, const float cy, const cv::Size &resolution,
		const cv::Scalar &backgroundColor = colors::darkGray, const cv::Scalar &gridColor = colors::gray,
		const float gridSpacing = 1.0f, const size_t maxTrajectoryLength = 10'000,
		const size_t landmarkLimit = 10'000) :
		PoseVisualizer(dv::camera::CameraGeometry(fx, fy, cx, cy, resolution), backgroundColor, gridColor, gridSpacing,
			maxTrajectoryLength, landmarkLimit) {
	}

	/**
	 * Initialize a pose visualizer to visualize the current and past poses of objects as an image. This is done by
	 * creating a virtual camera in the 3D scene, and using this camera to visualize object poses/landmarks in the
	 * 3D scene.
	 * @param cameraGeometry CameraGeometry instance defining the intrinsics of the virtual camera used for
	 * visualizing the 3D scene.
	 * @param backgroundColor Background color.
	 * @param gridColor Color of drawn grid plane.
	 * @param gridSpacing Spacing between drawn lines for grid plane, in physical units [m]
	 * @param maxTrajectoryLength Maximum number of poses that are visualized for each object.
	 * @param landmarkLimit Maximum number of landmarks that are visualized.
	 */
	explicit PoseVisualizer(const dv::camera::CameraGeometry &cameraGeometry,
		const cv::Scalar &backgroundColor = colors::darkGray, const cv::Scalar &gridColor = colors::gray,
		const float gridSpacing = 1.0f, const size_t maxTrajectoryLength = 10'000,
		const size_t landmarkLimit = 10'000) :
		mViewingCameraGeometry(cameraGeometry),
		mBackgroundColor(backgroundColor),
		mGridColor(gridColor),
		mGridSpacing(gridSpacing),
		mTrajectoryLength(maxTrajectoryLength),
		mLandmarkLimit(landmarkLimit) {
		setCameraExtrinsicsFromViewMode();
	}

	/**
	 * Add a new object pose to the visualization.
	 * @param objectName Name of the object for which the pose is to be added.
	 * @param objectPose Added pose of the object.
	 */
	void accept(const std::string &objectName, const dv::Pose &objectPose) {
		accept(objectName, dv::data::transformFromPose<float>(objectPose));
	}

	/**
	 * Add a new object pose to the visualization.
	 * @param objectName Name of the object for which the pose is to be added.
	 * @param objectPose Added pose of the object.
	 */
	void accept(const std::string &objectName, const dv::kinematics::Transformationf &objectPose) {
		if (!mObjectTrajectories.contains(objectName)) {
			mObjectTrajectories.emplace(objectName, mTrajectoryLength);
		}

		mObjectTrajectories.at(objectName).pushTransformation(objectPose);
		const auto position = objectPose.getTranslation<cv::Point3f>();
		updateBounds(position.x, position.y, position.z);
	}

	/**
	 * Add a new object pose to the visualization.
	 * @param objectPose Added pose of the object.
	 */
	void accept(const dv::Pose &objectPose) {
		accept("C0", objectPose);
	}

	/**
	 * Add a new object pose to the visualization.
	 * @param objectPose Added pose of the object.
	 */
	void accept(const dv::kinematics::Transformationf &objectPose) {
		accept("C0", objectPose);
	}

	/**
	 * Add landmarks to the visualization.
	 * @param landmarks A packet of landmarks to be added to the list of landmarks drawn.
	 */
	void accept(const dv::LandmarksPacket &landmarks) {
		for (const auto &landmark : landmarks.elements) {
			accept(landmark);
		}
	}

	/**
	 * Add a landmark to the visualization.
	 * @param landmark A single landmark to be added to the list of landmarks drawn.
	 */
	void accept(const dv::Landmark &landmark) {
		mLandmarks[landmark.id] = landmark;
		updateBounds(landmark.pt.x(), landmark.pt.y(), landmark.pt.z());

		//  Erase landmarks which are too old
		if (mLandmarks.size() > mLandmarkLimit) {
			// Remove oldest landmark
			auto oldestLandmark
				= std::min_element(mLandmarks.begin(), mLandmarks.end(), [](const auto &a, const auto &b) {
					  return a.second.timestamp < b.second.timestamp;
				  });
			mLandmarks.erase(oldestLandmark);
		}
	}

	/**
	 * Return a visualization image of the 3D scene from the defined virtual camera perspective.
	 * @return The generated image for visualization.
	 */
	[[nodiscard]] dv::Frame generateFrame() {
		setCameraExtrinsicsFromViewMode();
		if (mEnableAutoScale) {
			updateGridSpacing();
		}

		cv::Mat image = cv::Mat(mViewingCameraGeometry.getResolution(), CV_8UC3, mBackgroundColor);

		drawGrid(image);
		drawObjectPoses(image);
		drawLandmarks(image);
		drawWorldCoordinateSystem(image);

		if (mEnableLegend) {
			drawLegend(image);
		}

		dv::Frame frame(getTimestamp(), image);
		frame.source = dv::FrameSource::VISUALIZATION;
		return frame;
	}

	/**
	 * Return the timestamp of the most recent object pose.
	 * @return Timestamp in Unix microsecond format.
	 */
	[[nodiscard]] int64_t getTimestamp() const {
		int64_t timestamp = 0;
		for (const auto &[objectName, objectPoses] : mObjectTrajectories) {
			const int64_t lastestTime = objectPoses.latestTransformation().getTimestamp();
			if (lastestTime > timestamp) {
				timestamp = lastestTime;
			}
		}

		return timestamp;
	}

	/**
	 * Clear landmark with given ID from the visualization.
	 * @param id ID of landmark to be removed.
	 */
	void clearLandmarkWithId(const int64_t id) {
		mLandmarks.erase(id);
	}

	/**
	 * Clear all landmarks from the visualization within the time interval [startTime, endTime).
	 * @param startTime The start time of the removed slice of landmarks (inclusive).
	 * @param endTime The end time of the removed slice of landmarks (exclusive).
	 */
	void clearLandmarksInInterval(const int64_t startTime, const int64_t endTime) {
		std::erase_if(mLandmarks, [startTime, endTime](const auto &landmarkPair) {
			return landmarkPair.second.timestamp >= startTime && landmarkPair.second.timestamp < endTime;
		});
	}

	/**
	 * Clear all landmarks from the visualization.
	 */
	void clearLandmarks() {
		mLandmarks.clear();
	}

	/**
	 * Clear all saved poses for the given object from the visualization.
	 * @param objectName Name of the object for which the list of poses is to be removed.
	 */
	void clearObject(const std::string &objectName) {
		mObjectTrajectories.erase(objectName);
	}

	/**
	 * Clear all saved object poses from the visualization with the time interval [startTime, endTime).
	 * @param startTime The start time of the removed slice of object poses (inclusive).
	 * @param endTime The end time of the removed slice of object poses (exclusive).
	 */
	void clearObjectPosesInInterval(const int64_t startTime, const int64_t endTime) {
		for (auto &[objectName, objectTransforms] : mObjectTrajectories) {
			dv::kinematics::LinearTransformerf newObjectTransforms(mTrajectoryLength);
			for (auto it = objectTransforms.begin(); it != objectTransforms.end(); ++it) {
				if (it->getTimestamp() >= startTime && it->getTimestamp() < endTime) {
					continue;
				}
				newObjectTransforms.pushTransformation(*it);
			}

			objectTransforms = newObjectTransforms;
		}
	}

	/**
	 * Clear all saved object poses from the visualization.
	 */
	void clearAllObjects() {
		mObjectTrajectories.clear();
	}

	/**
	 * Reset the visualizer to a default state (default camera position, resolution, etc.).
	 */
	void reset() {
		// Clear all saved data
		mObjectTrajectories.clear();
		mLandmarks.clear();

		// Redefine default viewing camera intrinsics + extrinsics
		mViewMode = ViewMode::VIEW_XY;

		// Clear bounds on current points
		mMinBoundX = std::numeric_limits<float>::max();
		mMinBoundY = std::numeric_limits<float>::max();
		mMinBoundZ = std::numeric_limits<float>::max();

		mMaxBoundX = std::numeric_limits<float>::min();
		mMaxBoundY = std::numeric_limits<float>::min();
		mMaxBoundZ = std::numeric_limits<float>::min();

		// Redefine default grid and coordinate frame sizes
		setGridPlane(GridPlane::PLANE_XY);
		mGridSpacing = 1.0f;

		mWorldCoordinateFrameSize  = 1.0f;
		mObjectCoordinateFrameSize = 0.2f;
		mBoundsSet                 = false;
	}

	/// Getters and setters

	/**
	 * Get the resolution of the virtual camera used for visualization.
	 * @return Resolution of the virtual camera.
	 */
	[[nodiscard]] cv::Size getResolution() const {
		return mViewingCameraGeometry.getResolution();
	}

	/**
	 * Set the resolution of the virtual camera used for visualization.
	 * @param resolution Resolution of the virtual camera.
	 */
	void setResolution(const cv::Size &resolution) {
		mViewingCameraGeometry = dv::camera::CameraGeometry(mViewingCameraGeometry.getDistortion(),
			mViewingCameraGeometry.getFocalLength().x, mViewingCameraGeometry.getFocalLength().y,
			mViewingCameraGeometry.getCentralPoint().x, mViewingCameraGeometry.getCentralPoint().y, resolution,
			mViewingCameraGeometry.getDistortionModel());
	}

	/**
	 * Get the intrinsics of the virtual camera used for visualization.
	 * @return CameraGeometry instance describing the intrinsics of the virtual camera.
	 */
	[[nodiscard]] dv::camera::CameraGeometry getCameraIntrinsics() const {
		return mViewingCameraGeometry;
	}

	/**
	 * Set the intrinsics of the virtual camera used for visualization.
	 * @param cameraGeometry CameraGeometry instance describing the intrinsics of the virtual camera.
	 */
	void setCameraIntrinsics(const dv::camera::CameraGeometry &cameraGeometry) {
		mViewingCameraGeometry = cameraGeometry;
	}

	/**
	 * Set the intrinsics of the virtual camera used for visualization assuming an ideal pinhole camera model.
	 * @param fx Focal length X for the virtual camera, measured in pixels.
	 * @param fy Focal length Y for the virtual camera, measured in pixels.
	 * @param cx Central point coordinate X for the virtual camera, in pixels.
	 * @param cy Central point coordinate Y for the virtual camera, in pixels.
	 * @param resolution Resolution of the virtual camera.
	 */
	void setCameraIntrinsics(
		const float fx, const float fy, const float cx, const float cy, const cv::Size &resolution) {
		mViewingCameraGeometry = dv::camera::CameraGeometry(fx, fy, cx, cy, resolution);
	}

	/**
	 * Get the position of the virtual camera used for visualization with respect to the world coordinate system.
	 * @tparam Output Type of the 3D vector used for the camera position.
	 * @return Position of the virtual camera in the world coordinate system.
	 */
	template<concepts::Coordinate3DConstructible Output = Eigen::Vector3f>
	[[nodiscard]] Output getCameraPosition() const {
		return mT_C_W.inverse().getTranslation<Output>();
	}

	/**
	 * Set the position of the virtual camera used for visualization with respect to the world coordinate system.
	 * @tparam InputType Type of the 3D vector representing the camera position.
	 * @param newPosition Position of the virtual camera in the world coordinate system.
	 */
	template<concepts::Coordinate3D InputType>
	void setCameraPosition(const InputType &newPosition) {
		if constexpr (concepts::Coordinate3DAccessors<InputType>) {
			mCameraPosition = Eigen::Vector3f(newPosition.x(), newPosition.y(), newPosition.z());
		}
		else {
			mCameraPosition = Eigen::Vector3f(newPosition.x, newPosition.y, newPosition.z);
		}
		dv::kinematics::Transformationf T_W_C(0, mCameraPosition, mT_C_W.inverse().getRotationMatrix());
		mT_C_W = T_W_C.inverse();
	}

	/**
	 * Get the orientation of the virtual camera used for visualization as XYZ Euler angles (in degrees) with respect
	 * to the world coordinate system.
	 * @tparam Output Type of the 3D vector representing the camera orientation.
	 * @return XYZ Euler angles representing the camera orientation with respect to the world coordinate system
	 * (in degrees).
	 */
	template<concepts::Coordinate3DConstructible Output = Eigen::Vector3f>
	[[nodiscard]] Output getCameraOrientation() const {
#if EIGEN_MAJOR_VERSION < 5
		Eigen::Vector3f eulerAngles = mT_C_W.inverse().getRotationMatrix().eulerAngles(0, 1, 2);
#else
		Eigen::Vector3f eulerAngles = mT_C_W.inverse().getRotationMatrix().canonicalEulerAngles(0, 1, 2);
#endif
		eulerAngles = eulerAngles * RAD_TO_DEG;
		return Output(eulerAngles.x(), eulerAngles.y(), eulerAngles.z());
	}

	/**
	 * Set the orientation of the virtual camera used for visualization as XYZ Euler angles (in degrees) with respect
	 * to the world coordinate system.
	 * @tparam InputType Type of the 3D vector representing the camera orientation
	 * @param newOrientation XYZ Euler angles representing the new camera orientation with respect to the world
	 * coordinate system (in degrees).
	 */
	template<concepts::Coordinate3D InputType>
	void setCameraOrientation(const InputType &newOrientation) {
		float rotXDeg, rotYDeg, rotZDeg;
		if constexpr (concepts::Coordinate3DAccessors<InputType>) {
			rotXDeg = newOrientation.x();
			rotYDeg = newOrientation.y();
			rotZDeg = newOrientation.z();
		}
		else {
			rotXDeg = newOrientation.x;
			rotYDeg = newOrientation.y;
			rotZDeg = newOrientation.z;
		}

		Eigen::Quaternionf quat = Eigen::AngleAxisf(rotZDeg * DEG_TO_RAD, Eigen::Vector3f::UnitZ())
								* Eigen::AngleAxisf(rotYDeg * DEG_TO_RAD, Eigen::Vector3f::UnitY())
								* Eigen::AngleAxisf(rotXDeg * DEG_TO_RAD, Eigen::Vector3f::UnitX());
		dv::kinematics::Transformationf T_W_VC(0, mT_C_W.inverse().getTranslation(), quat);
		mT_C_W = T_W_VC.inverse();
	}

	/**
	 * Get the pose of the virtual camera used for visualization with respect to the world coordinate system.
	 * @return Transformationf instance describing the pose of the virtual camera with respect to the world coordinate
	 * system.
	 */
	[[nodiscard]] dv::kinematics::Transformationf getCameraPose() {
		return mT_C_W.inverse();
	}

	/**
	 * Set the pose of the virtual camera used for visualization with respect to the world coordinate system.
	 * @param T_W_C Transformationf instance describing the pose of the virtual camera with respect to the world
	 * coordinate system.
	 */
	void setCameraPose(const dv::kinematics::Transformationf &T_W_C) {
		mCameraPosition = T_W_C.getTranslation();
		mT_C_W          = T_W_C.inverse();
	}

	/**
	 * Get the view mode (pre-defined pose) of the virtual camera used for visualization.
	 * @return The current viewing mode of the virtual camera.
	 */
	[[nodiscard]] ViewMode getViewMode() const {
		return mViewMode;
	}

	/**
	 * Set the view mode (pre-defined pose) of the virtual camera used for visualization to one of the pre-defined
	 * view modes.
	 * @param mode Viewing mode of the virtual camera.
	 */
	void setViewMode(const ViewMode mode) {
		mViewMode = mode;
		setCameraExtrinsicsFromViewMode();
	}

	/**
	 * Get the grid plane orientation with respect to the world coordinate system (from a list of pre-defined
	 * orientations).
	 * @return Orientation of the visualized grid plane.
	 */
	[[nodiscard]] GridPlane getGridPlane() const {
		return mGridPlane;
	}

	/**
	 * Set the grid plane orientation with respect to the world coordinate system (from a list of pre-defined
	 * orientations). If set to PLANE_NONE, the grid plane is removed from the visualization.
	 * @param plane Orientation of the visualized grid plane.
	 */
	void setGridPlane(const GridPlane plane) {
		mGridPlane = plane;
	}

	/**
	 * Get the spacing between drawn lines used for visualizing the grid plane, in physical units [m].
	 * @return Spacing between drawn lines for visualizing the grid plane.
	 */
	[[nodiscard]] float getGridSpacing() const {
		if (mGridPlane == GridPlane::PLANE_NONE) {
			return 0;
		}
		return mGridSpacing;
	}

	/**
	 * Set the spacing between drawn lines used for visualizing the grid plane, in physical units [m].
	 * @param gridSpacing Spacing between drawn lines for visualizing the grid plane.
	 */
	void setGridSpacing(const float gridSpacing) {
		if (mGridPlane == GridPlane::PLANE_NONE || mEnableAutoScale) {
			return;
		}

		mGridSpacing = gridSpacing;
	}

	/**
	 * Get the size of the visualized world coordinate frame, in physical units [m].
	 * @return Size of the visualized world coordinate frame.
	 */
	[[nodiscard]] float getWorldCoordinateSize() const {
		return mWorldCoordinateFrameSize;
	}

	/**
	 * Set the size of the visualized world coordinate frame, in physical units [m]. To disable the visualization of the
	 * world coordinate frame, set this value to a value <=0.
	 * @param newSize Size of the visualized world coordinate frame.
	 */
	void setWorldCoordinateSize(const float newSize) {
		mWorldCoordinateFrameSize = newSize;
	}

	/**
	 * Get the size of the visualized coordinate frame for object poses, in physical units [m].
	 * @return Size of the visualized coordinate frame for object poses.
	 */
	[[nodiscard]] float getObjectCoordinateSize() const {
		return mObjectCoordinateFrameSize;
	}

	/**
	 * Get the size of the visualized coordinate frame for object poses, in physical units [m]. To disable the
	 * visualization of the coordinate frame, set this value to a value <=0.
	 * @param newSize Size of the visualized coordinate frame for object poses.
	 */
	void setObjectCoordinateSize(const float newSize) {
		mObjectCoordinateFrameSize = newSize;
	}

	/**
	 * Get the line thickness used for drawing, in pixels.
	 * @return Drawing line thickness in pixels.
	 */
	[[nodiscard]] int32_t getLineThickness() const {
		return mLineThickness;
	}

	/**
	 * Set the line thickness used for drawing, in pixels.
	 * @param newThickness Drawing line thickness in pixels.
	 */
	void setLineThickness(const int32_t newThickness) {
		dv::runtime_assert(
			[newThickness] {
				return newThickness >= 1;
			},
			[] {
				return "line thickness must be at least 1";
			});

		mLineThickness = newThickness;
	}

	/**
	 * Get the background color.
	 * @return		Background color.
	 */
	[[nodiscard]] const cv::Scalar &getBackgroundColor() const {
		return mBackgroundColor;
	}

	/**
	 * Set new background color.
	 * @param backgroundColor 	OpenCV scalar for the background color.
	 */
	void setBackgroundColor(const cv::Scalar &backgroundColor) {
		mBackgroundColor = backgroundColor;
	}

	/**
	 * Get the grid line color.
	 * @return 		Grid line color
	 */
	[[nodiscard]] const cv::Scalar &getGridColor() const {
		return mGridColor;
	}

	/**
	 * Set new grid line color
	 * @param mGridColor 	OpenCV scalar for the grid line color.
	 */
	void setGridColor(const cv::Scalar &gridColor) {
		mGridColor = gridColor;
	}

	/**
	 * Get the maximum number of landmarks to be drawn.
	 * @return 		Maximum number of landmarks
	 */
	[[nodiscard]] size_t getLandmarkLimit() const {
		return mLandmarkLimit;
	}

	/**
	 * Set a limit for number of landmarks that are stored and drawn.
	 * @param numLandmarks 		Number of landmarks
	 */
	void setLandmarkLimit(const size_t numLandmarks) {
		mLandmarkLimit = numLandmarks;
	}

	/**
	 * Get the number of landmarks currently stored in the visualizer.
	 * @return 		Number of landmarks stored in the visualizer
	 */
	[[nodiscard]] size_t getNumberOfLandmarks() const {
		return mLandmarks.size();
	}

	/**
	 * Get the number of objects who's poses are currently stored in the visualizer.
	 * @return Number of objects who's poses are stored in the visualizer
	 */
	[[nodiscard]] size_t getNumberOfObjects() const {
		return mObjectTrajectories.size();
	}

	/**
	 * Get if auto-scaling of the visualizer is enabled; scaling of the grid and viewing camera position based on the
	 * minimum and maximum bounds from the provided data.
	 * @return True if auto-scaling is enabled.
	 */
	[[nodiscard]] bool isAutoScalingEnabled() const {
		return mEnableAutoScale && mViewMode != ViewMode::CUSTOM;
	}

	/**
	 * Enable/disable auto-scaling of the visualizer; scaling of the grid and viewing camera position based on the
	 * minimum and maximum bounds from the provided data.
	 * @param enable If true, enable grid autoscaling.
	 */
	void setAutoScaling(const bool enable) {
		mEnableAutoScale = enable;
	}

	/**
	 * Get if the visualization of the legend is enabled.
	 * @return True if legend visualization is enabled.
	 */
	[[nodiscard]] bool isLegendVisualizationEnabled() const {
		return mEnableLegend;
	}

	/**
	 * Enable the visualization of a legend showing the viewing camera position, latest timestamp of provided data, and
	 * the spacing of the drawn grid.
	 * @param enable If true, enable legend visualization.
	 */
	void setLegendVisualization(const bool enable) {
		mEnableLegend = enable;
	}

private:
	/**
	 * Update the minimum and maximum bounds of the visualizer data based on a provided 3D point.
	 * @param x X position of 3D point.
	 * @param y Y position of 3D point.
	 * @param z Z position of 3D point.
	 */
	void updateBounds(const float x, const float y, const float z) {
		mBoundsSet = true;

		if (x < mMinBoundX) {
			mMinBoundX = x;
		}

		if (x > mMaxBoundX) {
			mMaxBoundX = x;
		}

		if (y < mMinBoundY) {
			mMinBoundY = y;
		}

		if (y > mMaxBoundY) {
			mMaxBoundY = y;
		}

		if (z < mMinBoundZ) {
			mMinBoundZ = z;
		}

		if (z > mMaxBoundZ) {
			mMaxBoundZ = z;
		}
	}

	/**
	 * Compute the maximum span of the provided visualizer data along a given plane.
	 * @param grid Plane along which the maximum span of the provided visualizer data is computed.
	 * @return Maximum span of provided visualizer data along the given plane.
	 */
	[[nodiscard]] float computeSpan(const GridPlane grid) const {
		// Compute span based on bounds
		const auto computeBound = [](const float max, const float min) -> float {
			return 2 * std::max(std::abs(max), std::abs(min));
		};

		switch (grid) {
			case (GridPlane::PLANE_XY): {
				return std::max(computeBound(mMaxBoundX, mMinBoundX), computeBound(mMaxBoundY, mMinBoundY));
			}
			case (GridPlane::PLANE_ZX): {
				return std::max(computeBound(mMaxBoundX, mMinBoundX), computeBound(mMaxBoundZ, mMinBoundZ));
			}
			case (GridPlane::PLANE_YZ): {
				return std::max(computeBound(mMaxBoundZ, mMinBoundZ), computeBound(mMaxBoundY, mMinBoundY));
			}
			default: {
				return -1;
			}
		}
	}

	/**
	 * Use the camera view to set the pose the virtual camera.
	 */
	void setCameraExtrinsicsFromViewMode() {
		if (mViewMode == ViewMode::CUSTOM) {
			return;
		}

		// Determine the orientation at which the camera is placed based on the given ViewMode
		GridPlane viewPlane;
		Eigen::Vector3f orientation, planeNormal;

		switch (mViewMode) {
			case (ViewMode::VIEW_XY): {
				viewPlane   = GridPlane::PLANE_XY;
				orientation = Eigen::Vector3f(180.0f, 0.0f, 0.0f);
				planeNormal = Eigen::Vector3f::UnitZ();
				break;
			}
			case (ViewMode::VIEW_YZ): {
				viewPlane   = GridPlane::PLANE_YZ;
				orientation = Eigen::Vector3f(-90.0f, 0.0f, 90.0f);
				planeNormal = Eigen::Vector3f::UnitX();
				break;
			}
			case (ViewMode::VIEW_ZX): {
				viewPlane   = GridPlane::PLANE_ZX;
				orientation = Eigen::Vector3f(-90.0f, -90.0f, 180.0f);
				planeNormal = Eigen::Vector3f::UnitY();
				break;
			}
			case (ViewMode::VIEW_XZ): {
				viewPlane   = GridPlane::PLANE_ZX;
				orientation = Eigen::Vector3f(-90.0f, 0.0f, 0.0f);
				planeNormal = -Eigen::Vector3f::UnitY();
				break;
			}
			case (ViewMode::VIEW_YX): {
				viewPlane   = GridPlane::PLANE_XY;
				orientation = Eigen::Vector3f(0.0f, 0.0f, 90.0f);
				planeNormal = -Eigen::Vector3f::UnitZ();
				break;
			}
			case (ViewMode::VIEW_ZY): {
				viewPlane   = GridPlane::PLANE_YZ;
				orientation = Eigen::Vector3f(0.0f, -90.0f, 180.0f);
				planeNormal = -Eigen::Vector3f::UnitX();
				break;
			}
			default: {
				throw dv::exceptions::InvalidArgument<ViewMode>("Invalid view mode provided", mViewMode);
			}
		}

		// If autoscaling is enabled, the distance of the virtual camera from the view plane is set based on the bounds
		// of provided data (so that all data is in view), else the distance is set to DEFAULT_CAMERA_HEIGHT
		float distanceToPlane;
		if (mEnableAutoScale && mBoundsSet) {
			const Eigen::Vector3f maxBounds(mMaxBoundX, mMaxBoundY, mMaxBoundZ);
			const float maxPointHeightFromPlane = std::abs(maxBounds.dot(planeNormal));

			const float maxSpan = computeSpan(viewPlane);
			if (maxSpan > std::numeric_limits<float>::epsilon()) {
				const auto smallerDimension = static_cast<float>(std::min(
					mViewingCameraGeometry.getResolution().width, mViewingCameraGeometry.getResolution().height));
				const float maxFocalLength
					= std::max(mViewingCameraGeometry.getFocalLength().x, mViewingCameraGeometry.getFocalLength().y);
				const float distanceToMaxPt = maxSpan * maxFocalLength / smallerDimension;
				distanceToPlane             = distanceToMaxPt + maxPointHeightFromPlane;
			}
			else {
				distanceToPlane = DEFAULT_CAMERA_HEIGHT;
			}
		}
		else {
			distanceToPlane = DEFAULT_CAMERA_HEIGHT;
		}

		setCameraPosition(distanceToPlane * planeNormal);
		setCameraOrientation(orientation);
	}

	/**
	 * Compute a nice value for the half width of the drawn grid based on the viewing camera position in the world
	 * coordinate system. The value is chosen so that when the grid is placed relative to the viewing camera position,
	 * and the viewing camera is oriented at 90 degrees with respect to the grid, the width of the grid is at least 5
	 * times the minimum width so that edge of the drawn grid is visible in the camera view.
	 * @return Half width of the drawn grid.
	 */
	[[nodiscard]] float getGridHalfWidth() const {
		if (mGridPlane == GridPlane::PLANE_NONE) {
			return 0.0f;
		}

		const float fovX
			= std::atan(mViewingCameraGeometry.getCentralPoint().x / mViewingCameraGeometry.getFocalLength().x);
		const float fovY
			= std::atan(mViewingCameraGeometry.getCentralPoint().y / mViewingCameraGeometry.getFocalLength().y);
		const float maxFoV = std::max(fovX, fovY);
		float minFarDistFactor
			= std::tan(std::numbers::pi_v<float> / 2.0f - maxFoV); // Min factor needed to at least see one line

		Eigen::Vector3f planeNormal;
		switch (mGridPlane) {
			case (GridPlane::PLANE_XY): {
				planeNormal = Eigen::Vector3f::UnitZ();
				break;
			}
			case (GridPlane::PLANE_YZ): {
				planeNormal = Eigen::Vector3f::UnitX();
				break;
			}
			case (GridPlane::PLANE_ZX): {
				planeNormal = Eigen::Vector3f::UnitY();
				break;
			}
			default: {
				throw std::runtime_error("Invalid view mode passed to pose visualizer");
			}
		}

		// Note: minimum far distance guarantees at least one line is seen
		const float minFarDistance = std::abs(mCameraPosition.dot(planeNormal) * minFarDistFactor);

		// Add multiplicative factor to minimum far distance so that more than one line is seen
		const float additionalFactor = 5.0f;

		return additionalFactor * minFarDistance;
	}

	[[nodiscard]] bool isWithinGridBounds(const Eigen::Vector3f &pos) const {
		if (mGridPlane == GridPlane::PLANE_NONE) {
			return true;
		}

		const float gridHalfWidth = getGridHalfWidth();
		Eigen::Vector3f horDir, vertDir;
		switch (mGridPlane) {
			case (GridPlane::PLANE_XY): {
				horDir  = Eigen::Vector3f::UnitX();
				vertDir = Eigen::Vector3f::UnitY();
				break;
			}
			case (GridPlane::PLANE_ZX): {
				horDir  = Eigen::Vector3f::UnitZ();
				vertDir = Eigen::Vector3f::UnitX();
				break;
			}
			case (GridPlane::PLANE_YZ): {
				horDir  = Eigen::Vector3f::UnitY();
				vertDir = Eigen::Vector3f::UnitZ();
				break;
			}
		}

		const float horCamPos  = mCameraPosition.dot(horDir);
		const float vertCamPos = mCameraPosition.dot(vertDir);

		const float horPos  = pos.dot(horDir);
		const float vertPos = pos.dot(vertDir);

		return ((horCamPos - gridHalfWidth <= horPos) && (horCamPos + gridHalfWidth >= horPos))
			&& ((vertCamPos - gridHalfWidth <= vertPos) && (vertCamPos + gridHalfWidth >= vertPos));
	}

	/**
	 * For use in autoscaling mode, update the grid spacing based on the viewing camera position so that the number of
	 * drawn grid lines along each axis of the grid is less than #LIMIT_NUM_GRID_LINES. Note that rather than
	 * continuously varying the grid spacing, the grid spacing grows/shrinks in integer multiples of
	 * #GRID_SPACING_GROWTH_FACTOR.
	 */
	void updateGridSpacing() {
		if (mGridPlane == GridPlane::PLANE_NONE) {
			return;
		}

		const float gridHalfWidth          = getGridHalfWidth();
		const float recommendedGridSpacing = 2 * gridHalfWidth / static_cast<float>(LIMIT_NUM_GRID_LINES);
		if (recommendedGridSpacing <= std::numeric_limits<float>::epsilon()) {
			return;
		}

		if (recommendedGridSpacing < mGridSpacing) {
			const int32_t multiple = (mGridSpacing / recommendedGridSpacing) / GRID_SPACING_GROWTH_FACTOR;
			if (multiple > 0) {
				mGridSpacing /= (multiple * GRID_SPACING_GROWTH_FACTOR);
			}
		}
		else {
			const int32_t multiple = (recommendedGridSpacing / mGridSpacing) / GRID_SPACING_GROWTH_FACTOR;
			if (multiple > 0) {
				mGridSpacing *= (multiple * GRID_SPACING_GROWTH_FACTOR);
			}
		}
	}

	/**
	 * Add text to the visualizer image.
	 * @param image Visualization image to draw to.
	 * @param ptInPx 2D pixel position of added text.
	 * @param text Text to be added to the image.
	 * @param color Color of the added text.
	 */
	void addText(cv::Mat &image, const cv::Point2i ptInPx, const std::string &text, const cv::Scalar &color) const {
		if (text.empty()) {
			return;
		}
		cv::Point2i textOffset(10, 10);
		cv::putText(image, text, ptInPx + textOffset, cv::FONT_HERSHEY_COMPLEX, 0.5, color, 1);
	}

	/**
	 * Get the size of the drawn text.
	 * @param text Text to draw.
	 * @return Size of the bounding box for the drawn text on the image.
	 */
	[[nodiscard]] cv::Size getTextSize(const std::string &text) const {
		if (text.empty()) {
			return {0, 0};
		}

		return cv::getTextSize(text, cv::FONT_HERSHEY_COMPLEX, 0.5, 1, nullptr);
	}

	[[nodiscard]] cv::Point2i project(const Eigen::Vector3f &ptInCam) const {
		if (mViewingCameraGeometry.isUndistortionAvailable()) {
			auto distortedPt = mViewingCameraGeometry.distort<Eigen::Vector3f>(ptInCam);
			return mViewingCameraGeometry.project<cv::Point2i>(distortedPt);
		}
		else {
			return mViewingCameraGeometry.project<cv::Point2i>(ptInCam);
		}
	}

	/**
	 * Draw a 3D point represented in the world coordinate system onto the visualization image.
	 * @param image Visualization image to draw to.
	 * @param pt 3D point represented in the world coordinate system.
	 * @param radiusPx Radius of the drawn circle for the point, represented in pixels.
	 * @param color Color of the drawn point.
	 * @param textToAdd Optional text to additionally add next to the drawn point.
	 */
	void drawPoint(cv::Mat &image, const Eigen::Vector3f &pt, const int32_t radiusPx, const cv::Scalar &color,
		const std::string &textToAdd = "") const {
		if (!isWithinGridBounds(pt)) {
			return;
		}

		Eigen::Vector3f ptInCam = mT_C_W.transformPoint(pt);
		if (ptInCam.z() <= std::numeric_limits<float>::epsilon()) {
			return;
		}

		const cv::Point2i ptInPx = project(ptInCam);

		cv::circle(image, ptInPx, radiusPx, color, cv::FILLED);
		addText(image, ptInPx, textToAdd, color);
	}

	/**
	 * Draw a line in 3D space based on the endpoints of the line in the world coordinate system.
	 * @param image Visualization image to draw to.
	 * @param startPt 3D point given in the world coordinate system representing the start point of the line.
	 * @param endPt 3D point given in the world coordinate system representing the end point of the line.
	 * @param color Color of the drawn line.
	 * @param textToAdd Optional text to additionally add next to the drawn line.
	 */
	void drawLine(cv::Mat &image, const Eigen::Vector3f &startPt, const Eigen::Vector3f &endPt, const cv::Scalar &color,
		const std::string &textToAdd = "") const {
		// Ray-plane intersection to make sure that lines are drawn even if one of the points falls outside the image
		// plane
		const Eigen::Vector3f startPtInCam = mT_C_W.transformPoint(startPt);
		const Eigen::Vector3f endPtInCam   = mT_C_W.transformPoint(endPt);
		if (startPtInCam.z() <= std::numeric_limits<float>::epsilon()
			&& endPtInCam.z() <= std::numeric_limits<float>::epsilon()) {
			return;
		}

		// If both start and end point infront of camera, can draw normally
		if (startPtInCam.z() > std::numeric_limits<float>::epsilon()
			&& endPtInCam.z() > std::numeric_limits<float>::epsilon()) {
			const cv::Point2i startPtPx = project(startPtInCam);
			const cv::Point2i endPtPx   = project(endPtInCam);
			cv::line(image, startPtPx, endPtPx, color, mLineThickness, cv::LINE_AA);
			addText(image, endPtPx, textToAdd, color);
			return;
		}

		// Else, one point is infront and one is behind. Compute the intersection of the line with the image, and draw
		// the line segment only as that
		Eigen::Vector3f rayDirection  = startPtInCam - endPtInCam;
		rayDirection                 /= rayDirection.norm();

		// Image plane at a depth of focalLength in physical units (focalLength = startPt.z + scale * rayDir.z)
		float focalLength
			= std::min(mViewingCameraGeometry.getFocalLength().x, mViewingCameraGeometry.getFocalLength().y);
		focalLength                          *= PIXEL_PITCH;
		const float scale                     = (focalLength - startPtInCam.z()) / rayDirection.z();
		const Eigen::Vector3f intersectionPt  = startPtInCam + scale * rayDirection;

		// Draw line segment from whichever line is infront to intersection
		if (startPtInCam.z() > std::numeric_limits<float>::epsilon()) {
			const cv::Point2i startPtPx        = project(startPtInCam);
			const cv::Point2i intersectionPtPx = project(intersectionPt);
			cv::line(image, startPtPx, intersectionPtPx, color, 1, cv::LINE_AA);
			addText(image, intersectionPtPx, textToAdd, color);
			return;
		}

		if (endPtInCam.z() > std::numeric_limits<float>::epsilon()) {
			const cv::Point2i intersectionPtPx = project(intersectionPt);
			const cv::Point2i endPtPx          = project(endPtInCam);
			cv::line(image, intersectionPtPx, endPtPx, color, 1, cv::LINE_AA);
			addText(image, intersectionPtPx, textToAdd, color);
			return;
		}
	}

	/**
	 * Draw a uniform grid onto the visualization image representing the defined grid plane.
	 * @param image Visualization image to draw to.
	 */
	void drawGrid(cv::Mat &image) const {
		// Get horizontal and vertical directions
		Eigen::Vector3f vertDir, horDir;
		cv::Scalar vertDirColor, horDirColor;
		switch (mGridPlane) {
			case (GridPlane::PLANE_NONE): {
				return;
			}
			case (GridPlane::PLANE_XY): {
				horDir       = Eigen::Vector3f::UnitX();
				vertDir      = Eigen::Vector3f::UnitY();
				horDirColor  = colors::red;
				vertDirColor = colors::lime;
				break;
			}
			case (GridPlane::PLANE_ZX): {
				horDir       = Eigen::Vector3f::UnitZ();
				vertDir      = Eigen::Vector3f::UnitX();
				horDirColor  = colors::red;
				vertDirColor = colors::blue;
				break;
			}
			case (GridPlane::PLANE_YZ): {
				horDir       = Eigen::Vector3f::UnitY();
				vertDir      = Eigen::Vector3f::UnitZ();
				horDirColor  = colors::lime;
				vertDirColor = colors::blue;
				break;
			}
		}
		Eigen::Vector3f planeNormal  = vertDir.cross(horDir);
		Eigen::Vector3f origin       = mCameraPosition;
		origin                      -= planeNormal.dot(origin) * planeNormal;

		const float gridHalfWidth  = getGridHalfWidth();
		const size_t numLinesDrawn = 2 * gridHalfWidth / mGridSpacing;
		Eigen::Vector3f topLeft    = origin + vertDir * gridHalfWidth - horDir * gridHalfWidth;

		// Update vertical position to fit grid spacing step
		const int32_t vertStep = topLeft.dot(vertDir) / mGridSpacing;
		const float vertPos    = vertStep * mGridSpacing;

		// Replace top left entry for vertical position with new entry
		topLeft -= topLeft.dot(vertDir) * vertDir;
		topLeft += vertPos * vertDir;

		// Update horizontal position to fit grid spacing step
		const int32_t horStep = topLeft.dot(horDir) / mGridSpacing;
		const float horPos    = horStep * mGridSpacing;

		// Replace top left entry for horizontal position with new entry
		topLeft -= topLeft.dot(horDir) * horDir;
		topLeft += horPos * horDir;

		// Loop for horizontal lines
		for (size_t i = 0; i < numLinesDrawn; ++i) {
			const Eigen::Vector3f lineStart = topLeft - (i * mGridSpacing) * vertDir;
			const Eigen::Vector3f lineEnd   = lineStart + (horDir * (2 * gridHalfWidth));

			cv::Scalar color = mGridColor;
			if (std::abs(lineStart.dot(vertDir)) <= std::numeric_limits<float>::epsilon()) {
				color = horDirColor;
			}
			drawLine(image, lineStart, lineEnd, color);
		}

		// Loop for vertical lines
		for (size_t j = 0; j < numLinesDrawn; ++j) {
			const Eigen::Vector3f lineStart = topLeft + (j * mGridSpacing) * horDir;
			const Eigen::Vector3f lineEnd   = lineStart - (vertDir * (2 * gridHalfWidth));

			cv::Scalar color = mGridColor;
			if (std::abs(lineStart.dot(horDir)) <= std::numeric_limits<float>::epsilon()) {
				color = vertDirColor;
			}
			drawLine(image, lineStart, lineEnd, color);
		}
	}

	/**
	 * Draw a coordinate frame onto the visualization image.
	 * @param image Visualization image to draw to.
	 * @param T_W_O Transform representing the pose of the coordinate frame with respect to the world coordinate system.
	 * @param frameSize Size of the frame to be drawn [m].
	 * @param frameColor Optional color of the coordinate frame drawn. If not specified, defaults to red for x axis,
	 * lime for y axis, and blue for z axis.
	 * @param textToAdd Optional text to additionally add next to the drawn coordinate frame.
	 */
	void drawCoordinateFrame(cv::Mat &image, const dv::kinematics::Transformationf &T_W_O, const float frameSize,
		const std::optional<cv::Scalar> &frameColor = std::nullopt, const std::string &textToAdd = "") const {
		if (frameSize <= 0.f) {
			return;
		}
		Eigen::Vector3f origin(0.f, 0.f, 0.f);
		origin = T_W_O.transformPoint(origin);

		if (!isWithinGridBounds(origin)) {
			return;
		}

		if (frameColor.has_value()) {
			drawPoint(image, origin, 2 * mLineThickness, frameColor.value());
		}
		else {
			drawPoint(image, origin, 2 * mLineThickness, colors::blue);
		}

		// Define the coordinate system in the object coordinate frame
		Eigen::Vector3f xAxis(frameSize, 0.f, 0.f);
		Eigen::Vector3f yAxis(0.f, frameSize, 0.f);
		Eigen::Vector3f zAxis(0.f, 0.f, frameSize);

		// Map from object to world frame
		xAxis = T_W_O.transformPoint(xAxis);
		yAxis = T_W_O.transformPoint(yAxis);
		zAxis = T_W_O.transformPoint(zAxis);

		if (frameColor.has_value()) {
			drawLine(image, origin, xAxis, frameColor.value(), "x");
			drawLine(image, origin, yAxis, frameColor.value(), "y");
			drawLine(image, origin, zAxis, frameColor.value(), "z");
		}
		else {
			drawLine(image, origin, xAxis, colors::red, "x");
			drawLine(image, origin, yAxis, colors::lime, "y");
			drawLine(image, origin, zAxis, colors::blue, "z");
		}

		const Eigen::Vector3f ptInCam = mT_C_W.transformPoint(origin);
		if (ptInCam.z() <= std::numeric_limits<float>::epsilon()) {
			return;
		}

		const cv::Point2i ptInPx = project(ptInCam);
		if (frameColor.has_value()) {
			addText(image, ptInPx, textToAdd, frameColor.value());
		}
		else {
			addText(image, ptInPx, textToAdd, colors::blue);
		}
	}

	/**
	 * Draw the world coordinate frame onto the visualization image.
	 * @param image Visualization image to draw to.
	 */
	void drawWorldCoordinateSystem(cv::Mat &image) const {
		drawCoordinateFrame(image, dv::kinematics::Transformationf(), mWorldCoordinateFrameSize);
	}

	/**
	 * Draw the different object poses onto the visualization image.
	 * @param image Visualization image to draw to.
	 */
	void drawObjectPoses(cv::Mat &image) const {
		// Each object pose is drawn as a circle, with lines connecting them
		size_t i = 0;
		for (const auto &[objectName, objectPoses] : mObjectTrajectories) {
			const cv::Scalar color = colors::someNeonColor(i);

			Eigen::Vector3f lastObjectPosition;
			for (auto objectPose = objectPoses.cbegin(); objectPose != objectPoses.cend(); ++objectPose) {
				Eigen::Vector3f newObjectPositionW = objectPose->getTranslation();

				// Add name of object next to last pose
				if ((objectPose + 1) == objectPoses.cend()) {
					drawPoint(image, newObjectPositionW, 2 * mLineThickness, color, objectName);

					// If object coordinate frame size should be drawn, additionally draw for last pose
					drawCoordinateFrame(image, *objectPose, mObjectCoordinateFrameSize, color, objectName);
				}
				else {
					drawPoint(image, newObjectPositionW, 2 * mLineThickness, color);
				}

				// Draw line from previous pixel to current pixel (making sure line segment is at least one pixel)
				if (objectPose == objectPoses.cbegin()) {
					lastObjectPosition = newObjectPositionW;
					continue;
				}

				if (isWithinGridBounds(lastObjectPosition) && isWithinGridBounds(newObjectPositionW)) {
					drawLine(image, lastObjectPosition, newObjectPositionW, color);
					lastObjectPosition = newObjectPositionW;
				}
			}
			++i;
		}
	}

	/**
	 * Draw the defined landmarks onto the visualization image.
	 * @param image Visualization image to draw to.
	 */
	void drawLandmarks(cv::Mat &image) const {
		// Landmarks will just be drawn as circles
		for (const auto &[id, landmark] : mLandmarks) {
			const cv::Scalar color = colors::someNeonColor(id);
			drawPoint(
				image, Eigen::Vector3f(landmark.pt.x(), landmark.pt.y(), landmark.pt.z()), 2 * mLineThickness, color);
		}
	}

	/**
	 * Draw a legend onto the image specifying the viewing camera position, latest timestamp, and grid spacing
	 * @param image
	 */
	void drawLegend(cv::Mat &image) const {
		cv::Size rectangleSize;

		// Add viewing camera position
		std::stringstream posText;
		posText << fmt::format(
			"Position = [{:.2f}, {:.2f}, {:.2f}]", mCameraPosition.x(), mCameraPosition.y(), mCameraPosition.z());
		addText(image, cv::Point2i(0, 5), posText.str(), dv::visualization::colors::white);
		const cv::Size posTextSize = getTextSize(posText.str());
		if (posTextSize.width > rectangleSize.width) {
			rectangleSize.width = posTextSize.width;
		}
		rectangleSize.height += posTextSize.height;

		// Add latest timestamp
		const int64_t timestamp = getTimestamp();
		// Format the timestamp to a nice string
		auto tsToString = [](const int64_t ts) {
			const int64_t s         = ts / 1000000;
			const int64_t us        = ts % 1000000;
			const int64_t roundedMs = us / 1000;

			// Determine whether the timestamp is in UTC or counting from 0 assuming nobody records longer than 1w
			if (s > 3600 * 24 * 7) {
				return fmt::format("{}.{:06}", dv::toTimePoint(ts), us);
			}
			else {
				// Simple timestamp in seconds . milliseconds
				std::stringstream ss;
				ss << "Timestamp = " << s << "." << roundedMs << "s";
				return ss.str();
			}
		};
		std::stringstream timeText;
		timeText << tsToString(timestamp);
		addText(image, cv::Point2i(0, 25), timeText.str(), dv::visualization::colors::white);
		const cv::Size timeTextSize = getTextSize(timeText.str());
		if (timeTextSize.width > rectangleSize.width) {
			rectangleSize.width = timeTextSize.width;
		}
		rectangleSize.height += timeTextSize.height;

		// Add grid spacing
		if (mGridPlane != GridPlane::PLANE_NONE) {
			std::string gridText;

			if (mGridSpacing < 1) {
				// Millimeters (below one meter)
				gridText = fmt::format("Grid spacing: {:.3} mm", mGridSpacing * 1000.0f);
			}
			else if (mGridSpacing < 1'000) {
				// Meters (between one meter and one kilometer)
				gridText = fmt::format("Grid spacing: {:.3} m", mGridSpacing);
			}
			else {
				// Kilometers (above one kilometer, I guess we don't need to handle light-year distance drift?
				gridText = fmt::format("Grid spacing: {:.3} km", mGridSpacing / 1'000.f);
			}
			addText(image, cv::Point2i(0, 45), gridText, dv::visualization::colors::white);
			const cv::Size gridTextSize = getTextSize(gridText);
			if (gridTextSize.width > rectangleSize.width) {
				rectangleSize.width = gridTextSize.width;
			}
			rectangleSize.height += gridTextSize.height;
		}

		rectangleSize.width  += 20;
		rectangleSize.height += 30;

		// Draw white rectangle in top left
		cv::rectangle(image, cv::Point2i(0, 0), cv::Point2i(rectangleSize.width, rectangleSize.height),
			dv::visualization::colors::white);
	}

	/// Intrinsics of the virtual camera used for visualization
	dv::camera::CameraGeometry mViewingCameraGeometry;
	static constexpr float PIXEL_PITCH{9e-6f}; // Default pixel pitch used for the virtual camera

	/// Extrinsics of the virtual camera used for visualization
	dv::kinematics::Transformationf
		mT_C_W; // Transformation from world coordinate system to viewing camera coordinate system
	Eigen::Vector3f mCameraPosition;
	ViewMode mViewMode{ViewMode::VIEW_XY};              // Default options for the viewing camera pose
	static constexpr float DEFAULT_CAMERA_HEIGHT{5.0f}; // Default camera height from view mode plane

	/// Colors for drawing the visualization image
	cv::Scalar mBackgroundColor{dv::visualization::colors::darkGray};
	cv::Scalar mGridColor{dv::visualization::colors::gray};

	/// Parameters for visualized grid
	GridPlane mGridPlane{GridPlane::PLANE_XY};
	bool mEnableAutoScale{true};

	/// Parameters for scaling grid spacing
	float mGridSpacing; //[m]
	static constexpr float GRID_SPACING_GROWTH_FACTOR{2.0f};
	static constexpr size_t LIMIT_NUM_GRID_LINES{100};

	/// Bounds for range of drawn points
	bool mBoundsSet{false};
	float mMinBoundX{std::numeric_limits<float>::max()}, mMinBoundY{std::numeric_limits<float>::max()},
		mMinBoundZ{std::numeric_limits<float>::max()};
	float mMaxBoundX{std::numeric_limits<float>::min()}, mMaxBoundY{std::numeric_limits<float>::min()},
		mMaxBoundZ{std::numeric_limits<float>::min()};

	/// Visualized object poses and the maximum number of poses that are visualized
	std::unordered_map<std::string, dv::kinematics::LinearTransformerf> mObjectTrajectories;
	size_t mTrajectoryLength;

	/// Visualized landmarks and the maximum number landmarks that are visualized
	std::unordered_map<int64_t, dv::Landmark> mLandmarks;
	size_t mLandmarkLimit;

	/// Visualization parameters for size of coordinate frames and line thicknesses
	float mWorldCoordinateFrameSize{1.f};  // Size of visualized world coordinate frame [m]
	float mObjectCoordinateFrameSize{0.2}; // Size of visualized object coordinate frame [m]
	int32_t mLineThickness{1};             // Thickness of drawn lines [px]

	/// Option to enable/disable legend visualization
	bool mEnableLegend{true};

	/// Static variables for converting from degrees to radians
	static constexpr float RAD_TO_DEG = 180.0f / std::numbers::pi_v<float>;
	static constexpr float DEG_TO_RAD = 1.f / RAD_TO_DEG;
};

static_assert(concepts::FrameOutputGenerator<dv::visualization::PoseVisualizer>);
static_assert(concepts::Accepts<dv::visualization::PoseVisualizer, dv::kinematics::Transformationf>);
static_assert(concepts::Accepts<dv::visualization::PoseVisualizer, dv::LandmarksPacket>);

} // namespace dv::visualization

// fmt compatibility for enum class printing.
template<>
struct fmt::formatter<dv::visualization::PoseVisualizer::ViewMode> : fmt::ostream_formatter {};

template<>
struct fmt::formatter<dv::visualization::PoseVisualizer::GridPlane> : fmt::ostream_formatter {};
