/* +------------------------------------------------------------------------+
   |                     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/typemeta/TTypeName.h>
#include <mrpt/typemeta/TTypeName_stl.h>

#include <any>
#include <functional>
#include <iostream>
#include <list>
#include <memory>  // shared_ptr
#include <mutex>
#include <unordered_map>

namespace mrpt::comms
{
/** Nodelet-like Pub/Sub communications (in `#include <mrpt/comms/nodelets.h>`)
 * \ingroup mrpt_comms_grp
 * @{ */
class Subscriber
{
   private:
	Subscriber(
		std::function<void(const std::any&)>&& func,
		std::function<void()>&& cleanup);

   public:
	using Ptr = std::shared_ptr<Subscriber>;
	~Subscriber();

	static Ptr create(
		std::function<void(const std::any&)>&& func,
		std::function<void()>&& cleanup);

	void pub(const std::any& a);

   private:
	std::function<void(const std::any&)> m_func;
	std::function<void()> m_cleanup;
};

class Topic : public std::enable_shared_from_this<Topic>
{
   private:
	Topic(std::function<void()>&& cleanup);

   public:
	using Ptr = std::shared_ptr<Topic>;

	~Topic();

	template <typename ARG, typename Callable>
	Subscriber::Ptr createSubscriber(Callable&& func)
	{
		std::lock_guard<std::mutex> lock(m_mutex);
		m_subs.emplace_back();
		auto it = m_subs.end();
		it--;
		auto capturedShared = shared_from_this();
		auto newNode = Subscriber::create(
			[func{std::forward<Callable>(func)}](const std::any& anyArg) {
				try
				{
					std::invoke(func, std::any_cast<const ARG&>(anyArg));
				}
				catch (std::bad_any_cast&)
				{
					std::cerr << "Subscriber has wrong type: "
							  << mrpt::typemeta::TTypeName<ARG>::get()
							  << std::endl;
				}
			},
			// cleanup function
			[=] {
				// list operations don't invalidate iterators
				// This iterator capture will be valid
				capturedShared->cleanupSubscriber(it);
			});
		*it = newNode;
		return newNode;
	}

	void publish(const std::any& any);
	void cleanupSubscriber(std::list<std::weak_ptr<Subscriber>>::iterator it);

	template <typename CLEANUP>
	static Ptr create(CLEANUP&& cleanup)
	{
		return Ptr(new Topic(std::forward<CLEANUP>(cleanup)));
	}

   private:
	std::mutex m_mutex;
	std::list<std::weak_ptr<Subscriber>> m_subs;
	std::function<void()> m_cleanup;
};

/** The central directory of existing topics for pub/sub */
class TopicDirectory : public std::enable_shared_from_this<TopicDirectory>
{
   private:
	TopicDirectory();

   public:
	using Ptr = std::shared_ptr<TopicDirectory>;

	template <typename PATH>
	Topic::Ptr getTopic(PATH&& path)
	{
		std::lock_guard<std::mutex> lock(m_mutex);
		auto it = m_mapService.find(path);
		if (it != m_mapService.end())
		{
			auto ptr = it->second.lock();
			if (ptr) return ptr;
		}

		auto capturedShared = shared_from_this();
		auto newNode =
			Topic::create([=]() { capturedShared->cleanupTopic(path); });
		m_mapService.insert({path, newNode});
		return newNode;
	}

	void cleanupTopic(const std::string& key);
	static Ptr create();

   private:
	std::mutex m_mutex;
	std::unordered_map<std::string, std::weak_ptr<Topic>> m_mapService;
};

/** @} */  // end grouping

}  // namespace mrpt::comms
