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/stringbuffer.h"
#include "rapidjson/include/rapidjson/writer.h"

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

bool is_valid_port_path(std::string server_port_path);

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> isConnected;
  std::shared_ptr<std::atomic_bool> stopped;
  std::string available_topics;
  std::string available_services;
  std::shared_ptr<std::thread> periodic_thread;

  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(const std::string &server_port_path, bool run);

  ~RosbridgeWsClient();

  bool connected();

  void run();
  bool running();
  void stop();
  void reset();

  // 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::function<bool(void)> 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::function<bool(void)> stop);
};