RosBridgeClient.h

#pragma once

/*
*  Created on: Apr 16, 2018
*      Author: Poom Pianpak
*/

#include "Simple-WebSocket-Server/client_ws.hpp"

#include "rapidjson/include/rapidjson/document.h"
#include "rapidjson/include/rapidjson/writer.h"
#include "rapidjson/include/rapidjson/stringbuffer.h"

#include <functional>
#include <mutex>
#include <tuple>
#include <deque>
#include <thread>

using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
using InMessage =  std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;

template <typename T>
constexpr typename std::underlying_type<T>::type integral(T value)
{
    return static_cast<typename std::underlying_type<T>::type>(value);
}

class RosbridgeWsClient
{
    enum class EntryType{
        SubscribedTopic,
        AdvertisedTopic,
        AdvertisedService,
    };

    using EntryData = std::tuple<EntryType,
                             std::string /*service/topic name*/,
                             std::string /*client name*/,
                             std::weak_ptr<WsClient> /*client*/>;

    enum class EntryEnum{
        EntryType = 0,
        ServiceTopicName = 1,
        ClientName = 2,
        WPClient = 3
    };

  const std::string server_port_path;
  std::unordered_map<std::string /*client name*/,
                     std::shared_ptr<WsClient> /*client*/> client_map;
  std::deque<EntryData> service_topic_list;
  std::mutex mutex;
  std::shared_ptr<std::atomic_bool> hasConnection;
  std::shared_ptr<std::atomic_bool> stopped;

  std::shared_ptr<std::thread> pPoll_thread;
  std::string available_topics;
  std::string available_services;



  void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message);

public:
  RosbridgeWsClient(const std::string& server_port_path);

  ~RosbridgeWsClient();

 bool connected();

  // Adds a client to the client_map.
  void addClient(const std::string& client_name);

  std::shared_ptr<WsClient> getClient(const std::string& client_name);

  void stopClient(const std::string& client_name);

  void removeClient(const std::string& client_name);

  //!
  //! \brief Returns a string containing all advertised topics.
  //! \return Returns a string containing all advertised topics.
  //!
  //! \note This function will wait until the /rosapi/topics service is available.
  //! \note Call connected() in advance to ensure that a connection was established.
  //! \pre Connection must be established, \see \fn connected().
  //!
  std::string getAdvertisedTopics();

  //!
  //! \brief Returns a string containing all advertised services.
  //! \return Returns a string containing all advertised services.
  //!
  //! \note This function will wait until the /rosapi/services service is available.
  //! \note Call connected() in advance to ensure that a connection was established.
  //!
  std::string getAdvertisedServices();

  bool topicAvailable(const std::string &topic);

  // Gets the client from client_map and starts it. Advertising is essentially sending a message.
  // One client per topic!
  void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "");

  //!
  //! \brief Unadvertises the topice \p topic
  //! \param topic The topic to be unadvertised
  //! \param id
  //! \pre The topic \p topic must be advertised, \see topicAvailable().
  //!
  void unadvertise(const std::string& topic,
                   const std::string& id = "");

  void unadvertiseAll();

    //!
    //! \brief Publishes the message \p msg to the topic \p topic.
    //! \param topic The topic to publish the message.
    //! \param msg The message to publish.
    //! \param id
    //! \pre The topic \p topic must be advertised, \see topicAvailable().
    //!
  void publish(const std::string& topic, const rapidjson::Document& msg, const std::string& id = "");

  //!
  //! \brief Subscribes the client \p client_name to the topic \p topic.
  //! \param client_name
  //! \param topic
  //! \param callback
  //! \param id
  //! \param type
  //! \param throttle_rate
  //! \param queue_length
  //! \param fragment_size
  //! \param compression
  //! \pre The topic \p topic must be advertised, \see topicAvailable().
  //!
  void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "");

  void unsubscribe(const std::string& topic,
                   const std::string& id = "");

  void unsubscribeAll();

  void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback);

  void unadvertiseService(const std::string& service);

  void unadvertiseAllServices();

  void serviceResponse(const std::string& service, const std::string& id, bool result, const rapidjson::Document& values);

  void callService(const std::string& service, const InMessage& callback, const rapidjson::Document& args = {}, const std::string& id = "", int fragment_size = -1, const std::string& compression = "");

  //!
  //! \brief Checks if the service \p service is available.
  //! \param service Service name.
  //! \return Returns true if the service is available, false either.
  //! \note Don't call this function to frequently. Use \fn waitForService() instead.
  //!
  bool serviceAvailable(const std::string& service);

  //!
  //! \brief Waits until the service with the name \p service is available.
  //! \param service Service name.
  //! @note This function will block as long as the service is not available.
  //!
  void waitForService(const std::string& service);

  //!
  //! \brief Waits until the service with the name \p service is available.
  //! \param service Service name.
  //! \param stop Flag to stop waiting.
  //! @note This function will block as long as the service is not available or \p stop is false.
  //!
  void waitForService(const std::string& service, const std::shared_ptr<std::atomic_bool> stop);

  //!
  //! \brief Waits until the topic with the name \p topic is available.
  //! \param topic Topic name.
  //! @note This function will block as long as the topic is not available.
  //!
  void waitForTopic(const std::string& topic);
  //!
  //! \brief Waits until the topic with the name \p topic is available.
  //! \param topic Topic name.
  //! \param stop Flag to stop waiting.
  //! @note This function will block as long as the topic is not available or \p stop is false.
  //!
  void waitForTopic(const std::string& topic, const std::shared_ptr<std::atomic_bool> stop);
};