qt-migration-legatus/qt6-after/qt6-migrated-legatus/Lts/DDS/FTS/generic_fts_impl.h

#pragma once

#include "connext.h"
#include "generic_fts_internal.h"

#include <DDS/Core/core.hpp>
#include <DDS/Generic/generic_base.h>
#include <DDS/Generic/generic_logger.h>
#include <DDS/Generic/wait_set_handler.h>
#include <queue>
#include <sstream>
#include <type_traits>
#include <typeinfo>

#ifndef tr_lts
#define tr_lts(ctx, t) (t)
#endif

namespace FTS
{
template <class C> class HasStatusFunction
{
    template <class T> static std::true_type testSignature(void (T::*)(const Core::FtReplyState&));

    template <class T> static decltype(testSignature(&T::replyState)) test(std::nullptr_t);

    template <class T> static std::false_type test(...);

  public:
    using type = decltype(test<C>(nullptr));
    static const bool value = type::value;
};

template <typename GENERIC_FTS> struct TopicNames;

template <typename TOPICTYPE_REQUEST, typename TOPICTYPE_REPLY, typename DUMMY>
class GenericFtsImpl final : public GenericFts<TOPICTYPE_REQUEST, TOPICTYPE_REPLY>, public GenericFtsInternal
{
  public:
    using RequestTopic = TOPICTYPE_REQUEST;
    using ReplyTopic = TOPICTYPE_REPLY;
    using TopicNameHelper = TopicNames<GenericFtsImpl<RequestTopic, ReplyTopic, DUMMY>>;

  private:
    using SubscriberCallback = std::function<void(const rti::core::SampleIdentity&, const RequestTopic&)>;

    template <typename T = ReplyTopic, typename std::enable_if<HasStatusFunction<T>::value, int>::type = 0>
    void sendReplyState(Core::FtReplyState replyState, const rti::core::SampleIdentity& requestSampleIdentity)
    {
        connext::WriteSample<ReplyTopic> reply;
        reply.data().replyState(replyState);

        bool bLastTelegram =
            ((replyState != Core::FtReplyState_def::InProgress) && (replyState != Core::FtReplyState_def::NotStarted));
        m_replier->send_reply(reply, requestSampleIdentity, bLastTelegram);
    }

    template <typename T = ReplyTopic, typename std::enable_if<!HasStatusFunction<T>::value, int>::type = 0>
    void sendReplyState(Core::FtReplyState, const rti::core::SampleIdentity&)
    {
    }

    template <typename T, typename std::enable_if<HasStatusFunction<T>::value, int>::type = 0>
    void setReply(T& data, Core::FtReplyState status)
    {
        data.replyState(status);
    }

    template <typename T, typename std::enable_if<!HasStatusFunction<T>::value, int>::type = 0>
    void setReply(T&, Core::FtReplyState)
    {
    }

  public:
    GenericFtsImpl(const dds::domain::DomainParticipant& domainParticipant, const dds::pub::Publisher& ddsPublisher,
                   const dds::sub::Subscriber& ddsSubscriber, DDSGeneric::WaitSetHandler* wsHandler)
        : GenericFts<RequestTopic, ReplyTopic>(), GenericFtsInternal(), m_wsHandler(wsHandler),
          m_subscriberCallback(nullptr),
          m_logger(DDS::getLogger(TopicNameHelper::functionTopicName(), DDS::DataType::FUNCTION_TOPIC_SERVER))
    {
        Q_ASSERT(domainParticipant != nullptr);
        Q_ASSERT(ddsPublisher != nullptr);
        Q_ASSERT(ddsSubscriber != nullptr);
        Q_ASSERT(wsHandler != nullptr);

        qRegisterMetaType<RequestTopic>(TopicNameHelper::requestTopicTypeName());

        connext::ReplierParams<RequestTopic, ReplyTopic> replierParams(domainParticipant);

        replierParams.request_topic_name(TopicNameHelper::requestTopicName());
        replierParams.reply_topic_name(TopicNameHelper::replyTopicName());
        replierParams.publisher(ddsPublisher);
        replierParams.subscriber(ddsSubscriber);
        replierParams.datareader_qos(readerQos(m_logger));
        replierParams.datawriter_qos(writerQos(m_logger));

        m_replier = new connext::Replier<RequestTopic, ReplyTopic>(replierParams);
        m_readCondition = new dds::sub::cond::ReadCondition(m_replier->get_request_datareader(),
                                                            dds::sub::status::SampleState::not_read());

        m_wsHandler->attachWaitSet(*m_readCondition, this);
    }

    ~GenericFtsImpl()
    {
        m_wsHandler->detachWaitSet(*m_readCondition, this);
        delete m_readCondition;
        delete m_replier;
    }

    bool setSubscriberCallback(const SubscriberCallback& subscriberCallback)
    {
        if (m_subscriberCallback)
        {
            WARNING_LOG(m_logger) << "Can not set subscriber callback - already set ...";
            return false;
        }

        m_subscriberCallback = subscriberCallback;
        return true;
    }

    bool sendReply(const rti::core::SampleIdentity& requestSampleIdentity, const ReplyTopic& replyData) override final
    {
        connext::WriteSampleRef<ReplyTopic> reply;
        rti::pub::WriteParams replyParams;

        reply.set_data(const_cast<ReplyTopic*>(&replyData));
        reply.set_info(&replyParams);

        m_replier->send_reply(reply, requestSampleIdentity);
        return true;
    }

    bool sendReply(const rti::core::SampleIdentity& requestIdentity,
                   const QVector<ReplyTopic>& replyData) override final
    {
        DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - IN sendReply";

        for (int i = 0; i < replyData.size(); ++i)
        {
            if (HasStatusFunction<ReplyTopic>::value)
            {
                ReplyTopic rt = replyData.at(i);
                setReply(rt,
                         i == replyData.size() - 1 ? Core::FtReplyState::Completed : Core::FtReplyState::InProgress);
                if (!sendReply(requestIdentity, rt))
                    return false;
            }
            else
            {
                if (!sendReply(requestIdentity, replyData.at(i)))
                    return false;
            }
        }

        return true;
    }

  protected:
    void processRequest(const connext::Sample<RequestTopic>& request)
    {
        DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName()
                            << " - processRequest: " << connext::DebugInfo::toReadable(request.identity());

        qint64 age = qAbs(qint64(request.info().source_timestamp().to_secs()) - time(nullptr));

        if (age > 10)
        {
            std::ostringstream ss;
            ss << request.data();
            std::string s = ss.str();
            int slen = s.length();

            static int max_slen = 1024;
            if (slen > max_slen)
            {
                s.resize(max_slen);
                s.append("...");
            }

            DEBUG_LOG(m_logger) << "received a request for topic " << TopicNameHelper::requestTopicName()
                                << " but it was outdated - ignoring. "
                                << " local time: " << (time(nullptr) - 10)
                                << "[s], source timestamp: " << qint64(request.info().source_timestamp().to_secs())
                                << "[s],  -> age: " << age << "[s], data: " << s;
            ReplyTopic rt;
            rt.errorString(tr_lts(
                "dds", "Topic %1 was outdated and therefore ignored. Does the client time match the server time?"));
            rt.errorParams({TopicNameHelper::requestTopicName()});
            sendReply(request.identity(), rt);
            return;
        }

        sendReplyState(Core::FtReplyState::InProgress, request.identity());

        m_subscriberCallback(request.identity(), request.data());
    }

    void processRequests() override
    {
        DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - processRequests";

        while (true)
        {
            connext::Sample<RequestTopic> request;
            {
                std::lock_guard<std::mutex> l(m_requestMutex);
                if (m_requests.empty())
                {
                    DEBUG_LOG(m_logger) << "No more requests";
                    return;
                }
                DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - read one request for ";
                request = m_requests.front();
                m_requests.pop();
            }
            if (!request.info().valid())
            {
                WARNING_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - No valid data";
                continue;
            }
            processRequest(request);
        }
        DEBUG_LOG(m_logger) << TopicNameHelper::requestTopicName() << " - processRequests finished";
    }

    bool enqueueRequests() override
    {
        bool bValidDataRead = false;
        connext::Sample<RequestTopic> request;
        while (m_replier->receive_request(request))
        {
            if (!request.info().valid())
                continue;
            bValidDataRead = true;
            std::lock_guard<std::mutex> l(m_requestMutex);
            m_requests.push(request);
        }
        return bValidDataRead;
    }

  private:
    connext::Replier<RequestTopic, ReplyTopic>* m_replier;
    dds::sub::cond::ReadCondition* m_readCondition;

    DDSGeneric::WaitSetHandler* m_wsHandler;

    std::queue<connext::Sample<RequestTopic>> m_requests;
    std::mutex m_requestMutex;

    SubscriberCallback m_subscriberCallback;
};

} // namespace FTS