// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*!
 * @file calculatorServer.cxx
 * Server implementation for interfaces
 *
 * This file was generated by the tool fastddsgen (version: 4.1.0).
 */

#include "calculatorServer.hpp"

#include <atomic>
#include <condition_variable>
#include <map>
#include <memory>
#include <mutex>
#include <queue>
#include <stdexcept>
#include <string>
#include <thread>
#include <vector>

#include <fastdds/dds/builtin/topic/PublicationBuiltinTopicData.hpp>
#include <fastdds/dds/core/condition/Condition.hpp>
#include <fastdds/dds/core/condition/GuardCondition.hpp>
#include <fastdds/dds/core/condition/WaitSet.hpp>
#include <fastdds/dds/core/Time_t.hpp>
#include <fastdds/dds/domain/DomainParticipant.hpp>
#include <fastdds/dds/domain/qos/ReplierQos.hpp>
#include <fastdds/dds/log/Log.hpp>
#include <fastdds/dds/rpc/exceptions.hpp>
#include <fastdds/dds/rpc/interfaces.hpp>
#include <fastdds/dds/rpc/RequestInfo.hpp>
#include <fastdds/dds/rpc/Replier.hpp>
#include <fastdds/dds/rpc/Service.hpp>
#include <fastdds/dds/rpc/ServiceTypeSupport.hpp>
#include <fastdds/dds/subscriber/DataReader.hpp>

#include "calculator.hpp"
#include "calculator_details.hpp"
#include "calculatorPubSubTypes.hpp"

namespace calculator_example {

//{ interface Calculator

namespace detail {

namespace fdds = eprosima::fastdds::dds;
namespace frpc = eprosima::fastdds::dds::rpc;
namespace frtps = eprosima::fastdds::rtps;

class CalculatorServerLogic
    : public frpc::RpcServer
    , public std::enable_shared_from_this<CalculatorServerLogic>
{
    using RequestType = Calculator_Request;
    using ReplyType = Calculator_Reply;

public:

    CalculatorServerLogic(
            fdds::DomainParticipant& part,
            const char* service_name,
            const fdds::ReplierQos& qos,
            size_t thread_pool_size,
            std::shared_ptr<CalculatorServer_IServerImplementation> implementation)
        : CalculatorServerLogic(
                part,
                service_name,
                qos,
                std::make_shared<ThreadPool>(*this, thread_pool_size),
                std::move(implementation))
    {
    }

    CalculatorServerLogic(
            fdds::DomainParticipant& part,
            const char* service_name,
            const fdds::ReplierQos& qos,
            std::shared_ptr<frpc::RpcServerSchedulingStrategy> scheduler,
            std::shared_ptr<CalculatorServer_IServerImplementation> implementation)
        : frpc::RpcServer()
        , participant_(part)
        , request_scheduler_(scheduler)
        , implementation_(std::move(implementation))
    {
        // Register the service type support
        auto service_type = create_Calculator_service_type_support();
        auto ret = service_type.register_service_type(&participant_, "calculator_example::Calculator");
        if (ret != fdds::RETCODE_OK)
        {
            throw std::runtime_error("Error registering service type");
        }

        // Create the service
        service_ = participant_.create_service(service_name, "calculator_example::Calculator");
        if (nullptr == service_)
        {
            throw std::runtime_error("Error creating service");
        }

        // Create the replier
        replier_ = participant_.create_service_replier(service_, qos);
        if (nullptr == replier_)
        {
            throw std::runtime_error("Error creating requester");
        }
    }

    ~CalculatorServerLogic() override
    {
        if (nullptr != replier_)
        {
            participant_.delete_service_replier(service_->get_service_name(), replier_);
        }

        if (nullptr != service_)
        {
            participant_.delete_service(service_);
        }
    }

    void run() override
    {
        finish_condition_.set_trigger_value(false);
        fdds::WaitSet waitset;
        waitset.attach_condition(finish_condition_);
        fdds::StatusCondition& status_condition = replier_->get_replier_reader()->get_statuscondition();
        status_condition.set_enabled_statuses(fdds::StatusMask::data_available());
        waitset.attach_condition(status_condition);

        while (true)
        {
            fdds::ConditionSeq active_conditions;
            waitset.wait(active_conditions, fdds::c_TimeInfinite);

            // Early exit if the finish condition is triggered
            if (finish_condition_.get_trigger_value())
            {
                break;
            }

            // Take and process the requests
            auto ctx = std::make_shared<RequestContext>();
            while (fdds::RETCODE_OK == ctx->take_from(replier_))
            {
                if (ctx->validate_request())
                {
                    process_request(ctx);
                }
                else
                {
                    ctx->send_exception(frpc::RemoteExceptionCode_t::REMOTE_EX_INVALID_ARGUMENT, replier_);
                }

                // Prepare next request context
                ctx = std::make_shared<RequestContext>();
            }
        }
    }

    void stop() override
    {
        // Notify all threads to finish
        finish_condition_.set_trigger_value(true);

        // Cancel all pending requests
        {
            std::lock_guard<std::mutex> _(mtx_);
            for (auto& it : processing_requests_)
            {
                it.second->cancel();
            }
            processing_requests_.clear();
        }

        // Wait for all threads to finish
        request_scheduler_->server_stopped(shared_from_this());
    }

    void execute_request(
            const std::shared_ptr<frpc::RpcRequest>& request) override
    {
        auto ctx = std::dynamic_pointer_cast<RequestContext>(request);
        if (ctx)
        {
            execute_request(ctx);
        }
        else
        {
            throw std::runtime_error("Invalid request context type");
        }
    }

private:

    //{ Output feed helpers

    struct IOutputFeedCancellator
    {
        virtual ~IOutputFeedCancellator() = default;
        virtual void cancel() = 0;
    };

    //} Output feed helpers

    //{ Input feed helpers

    struct IInputFeedProcessor
    {
        virtual ~IInputFeedProcessor() = default;
        virtual bool process_additional_request(
                const RequestType& request) = 0;
        virtual void cancel_input_feed() = 0;
    };

    //} Input feed helpers

    //{ operation representation_limits

    //} operation representation_limits

    //{ operation addition

    //} operation addition

    //{ operation subtraction

    //} operation subtraction

    //{ operation fibonacci_seq

    struct fibonacci_seq_result_writer :
        public frpc::RpcServerWriter<int32_t>,
        public IOutputFeedCancellator
    {
        fibonacci_seq_result_writer(
                const frpc::RequestInfo& info,
                frpc::Replier* replier)
            : info_(info)
            , replier_(replier)
        {
            info_.has_more_replies = true;
            reply_.fibonacci_seq = calculator_example::detail::Calculator_fibonacci_seq_Result{};
            reply_.fibonacci_seq->result = calculator_example::detail::Calculator_fibonacci_seq_Out{};
        }

        void write(
                const int32_t& value) override
        {
            if (cancelled_.load())
            {
                throw frpc::RpcFeedCancelledException(0);
            }
            reply_.fibonacci_seq->result->return_ = value;
            replier_->send_reply(&reply_, info_);
        }

        void write(
                int32_t&& value) override
        {
            if (cancelled_.load())
            {
                throw frpc::RpcFeedCancelledException(0);
            }
            reply_.fibonacci_seq->result->return_ = value;
            replier_->send_reply(&reply_, info_);
        }

        void cancel() override
        {
            cancelled_.store(true);
        }

    private:

        frpc::RequestInfo info_;
        frpc::Replier* replier_ = nullptr;
        ReplyType reply_{};
        std::atomic<bool> cancelled_{ false };

    };

    //} operation fibonacci_seq

    //{ operation sum_all

    struct sum_all_value_reader :
        public frpc::RpcServerReader<int32_t>,
        public IInputFeedProcessor
    {
        sum_all_value_reader() = default;

        bool process_additional_request(
                const RequestType& request) override
        {
            if (request.sum_all_value.has_value())
            {
                if (request.sum_all_value->finished_.has_value())
                {
                    std::lock_guard<std::mutex> _(mtx_);
                    if (!finished_)
                    {
                        finished_ = true;
                        status_ = request.sum_all_value->finished_.value();
                        cv_.notify_all();
                    }
                    return true;
                }
                else if (request.sum_all_value->value.has_value())
                {
                    std::lock_guard<std::mutex> _(mtx_);
                    if (!finished_)
                    {
                        queue_.push(request.sum_all_value->value.value());
                        cv_.notify_all();
                    }
                    return true;
                }
            }
            return false;
        }

        void cancel_input_feed() override
        {
            std::lock_guard<std::mutex> _(mtx_);
            finished_ = true;
            cv_.notify_all();
        }

        bool read(
                int32_t& value) override
        {
            bool ret_val = false;
            std::unique_lock<std::mutex> lock(mtx_);
            while (!try_read(value, ret_val))
            {
                cv_.wait(lock);
            }
            return ret_val;
        }

        bool read(
                int32_t& value,
                const fdds::Duration_t& timeout) override
        {
            bool ret_val = false;
            std::unique_lock<std::mutex> lock(mtx_);
            std::chrono::steady_clock::time_point end_time =
                    std::chrono::steady_clock::now() +
                    std::chrono::seconds(timeout.seconds) +
                    std::chrono::nanoseconds(timeout.nanosec);
            while (!try_read(value, ret_val))
            {
                if (cv_.wait_until(lock, end_time) == std::cv_status::timeout)
                {
                    throw frpc::RpcTimeoutException();
                }
            }
            return ret_val;
        }

    private:

        bool try_read(
                int32_t& value,
                bool& ret_val)
        {
            if (!queue_.empty())
            {
                value = queue_.front();
                queue_.pop();
                ret_val = true;
                return true;
            }

            if (finished_)
            {
                if (status_ == frpc::RPC_STATUS_CODE_OK)
                {
                    ret_val = false;
                    return true;
                }
                else
                {
                    throw frpc::RpcFeedCancelledException(status_);
                }
            }

            ret_val = false;
            return false;
        }

        std::mutex mtx_;
        std::condition_variable cv_;
        std::queue<int32_t> queue_;
        bool finished_{ false };
        frpc::RpcStatusCode status_{ frpc::RPC_STATUS_CODE_OK };

    };


    //} operation sum_all

    //{ operation accumulator

    struct accumulator_value_reader :
        public frpc::RpcServerReader<int32_t>,
        public IInputFeedProcessor
    {
        accumulator_value_reader() = default;

        bool process_additional_request(
                const RequestType& request) override
        {
            if (request.accumulator_value.has_value())
            {
                if (request.accumulator_value->finished_.has_value())
                {
                    std::lock_guard<std::mutex> _(mtx_);
                    if (!finished_)
                    {
                        finished_ = true;
                        status_ = request.accumulator_value->finished_.value();
                        cv_.notify_all();
                    }
                    return true;
                }
                else if (request.accumulator_value->value.has_value())
                {
                    std::lock_guard<std::mutex> _(mtx_);
                    if (!finished_)
                    {
                        queue_.push(request.accumulator_value->value.value());
                        cv_.notify_all();
                    }
                    return true;
                }
            }
            return false;
        }

        void cancel_input_feed() override
        {
            std::lock_guard<std::mutex> _(mtx_);
            finished_ = true;
            cv_.notify_all();
        }

        bool read(
                int32_t& value) override
        {
            bool ret_val = false;
            std::unique_lock<std::mutex> lock(mtx_);
            while (!try_read(value, ret_val))
            {
                cv_.wait(lock);
            }
            return ret_val;
        }

        bool read(
                int32_t& value,
                const fdds::Duration_t& timeout) override
        {
            bool ret_val = false;
            std::unique_lock<std::mutex> lock(mtx_);
            std::chrono::steady_clock::time_point end_time =
                    std::chrono::steady_clock::now() +
                    std::chrono::seconds(timeout.seconds) +
                    std::chrono::nanoseconds(timeout.nanosec);
            while (!try_read(value, ret_val))
            {
                if (cv_.wait_until(lock, end_time) == std::cv_status::timeout)
                {
                    throw frpc::RpcTimeoutException();
                }
            }
            return ret_val;
        }

    private:

        bool try_read(
                int32_t& value,
                bool& ret_val)
        {
            if (!queue_.empty())
            {
                value = queue_.front();
                queue_.pop();
                ret_val = true;
                return true;
            }

            if (finished_)
            {
                if (status_ == frpc::RPC_STATUS_CODE_OK)
                {
                    ret_val = false;
                    return true;
                }
                else
                {
                    throw frpc::RpcFeedCancelledException(status_);
                }
            }

            ret_val = false;
            return false;
        }

        std::mutex mtx_;
        std::condition_variable cv_;
        std::queue<int32_t> queue_;
        bool finished_{ false };
        frpc::RpcStatusCode status_{ frpc::RPC_STATUS_CODE_OK };

    };


    struct accumulator_result_writer :
        public frpc::RpcServerWriter<int32_t>,
        public IOutputFeedCancellator
    {
        accumulator_result_writer(
                const frpc::RequestInfo& info,
                frpc::Replier* replier)
            : info_(info)
            , replier_(replier)
        {
            info_.has_more_replies = true;
            reply_.accumulator = calculator_example::detail::Calculator_accumulator_Result{};
            reply_.accumulator->result = calculator_example::detail::Calculator_accumulator_Out{};
        }

        void write(
                const int32_t& value) override
        {
            if (cancelled_.load())
            {
                throw frpc::RpcFeedCancelledException(0);
            }
            reply_.accumulator->result->return_ = value;
            replier_->send_reply(&reply_, info_);
        }

        void write(
                int32_t&& value) override
        {
            if (cancelled_.load())
            {
                throw frpc::RpcFeedCancelledException(0);
            }
            reply_.accumulator->result->return_ = value;
            replier_->send_reply(&reply_, info_);
        }

        void cancel() override
        {
            cancelled_.store(true);
        }

    private:

        frpc::RequestInfo info_;
        frpc::Replier* replier_ = nullptr;
        ReplyType reply_{};
        std::atomic<bool> cancelled_{ false };

    };

    //} operation accumulator

    //{ operation filter

    struct filter_value_reader :
        public frpc::RpcServerReader<int32_t>,
        public IInputFeedProcessor
    {
        filter_value_reader() = default;

        bool process_additional_request(
                const RequestType& request) override
        {
            if (request.filter_value.has_value())
            {
                if (request.filter_value->finished_.has_value())
                {
                    std::lock_guard<std::mutex> _(mtx_);
                    if (!finished_)
                    {
                        finished_ = true;
                        status_ = request.filter_value->finished_.value();
                        cv_.notify_all();
                    }
                    return true;
                }
                else if (request.filter_value->value.has_value())
                {
                    std::lock_guard<std::mutex> _(mtx_);
                    if (!finished_)
                    {
                        queue_.push(request.filter_value->value.value());
                        cv_.notify_all();
                    }
                    return true;
                }
            }
            return false;
        }

        void cancel_input_feed() override
        {
            std::lock_guard<std::mutex> _(mtx_);
            finished_ = true;
            cv_.notify_all();
        }

        bool read(
                int32_t& value) override
        {
            bool ret_val = false;
            std::unique_lock<std::mutex> lock(mtx_);
            while (!try_read(value, ret_val))
            {
                cv_.wait(lock);
            }
            return ret_val;
        }

        bool read(
                int32_t& value,
                const fdds::Duration_t& timeout) override
        {
            bool ret_val = false;
            std::unique_lock<std::mutex> lock(mtx_);
            std::chrono::steady_clock::time_point end_time =
                    std::chrono::steady_clock::now() +
                    std::chrono::seconds(timeout.seconds) +
                    std::chrono::nanoseconds(timeout.nanosec);
            while (!try_read(value, ret_val))
            {
                if (cv_.wait_until(lock, end_time) == std::cv_status::timeout)
                {
                    throw frpc::RpcTimeoutException();
                }
            }
            return ret_val;
        }

    private:

        bool try_read(
                int32_t& value,
                bool& ret_val)
        {
            if (!queue_.empty())
            {
                value = queue_.front();
                queue_.pop();
                ret_val = true;
                return true;
            }

            if (finished_)
            {
                if (status_ == frpc::RPC_STATUS_CODE_OK)
                {
                    ret_val = false;
                    return true;
                }
                else
                {
                    throw frpc::RpcFeedCancelledException(status_);
                }
            }

            ret_val = false;
            return false;
        }

        std::mutex mtx_;
        std::condition_variable cv_;
        std::queue<int32_t> queue_;
        bool finished_{ false };
        frpc::RpcStatusCode status_{ frpc::RPC_STATUS_CODE_OK };

    };


    struct filter_result_writer :
        public frpc::RpcServerWriter<int32_t>,
        public IOutputFeedCancellator
    {
        filter_result_writer(
                const frpc::RequestInfo& info,
                frpc::Replier* replier)
            : info_(info)
            , replier_(replier)
        {
            info_.has_more_replies = true;
            reply_.filter = calculator_example::detail::Calculator_filter_Result{};
            reply_.filter->result = calculator_example::detail::Calculator_filter_Out{};
        }

        void write(
                const int32_t& value) override
        {
            if (cancelled_.load())
            {
                throw frpc::RpcFeedCancelledException(0);
            }
            reply_.filter->result->return_ = value;
            replier_->send_reply(&reply_, info_);
        }

        void write(
                int32_t&& value) override
        {
            if (cancelled_.load())
            {
                throw frpc::RpcFeedCancelledException(0);
            }
            reply_.filter->result->return_ = value;
            replier_->send_reply(&reply_, info_);
        }

        void cancel() override
        {
            cancelled_.store(true);
        }

    private:

        frpc::RequestInfo info_;
        frpc::Replier* replier_ = nullptr;
        ReplyType reply_{};
        std::atomic<bool> cancelled_{ false };

    };

    //} operation filter

    struct RequestContext : frpc::RpcRequest
    {
        RequestType request;
        frpc::RequestInfo info;
        frtps::GUID_t client_id;
        fdds::PublicationBuiltinTopicData pub_data;

        struct representation_limits_feeds_t
        {
        }
        representation_limits_feeds;

        struct addition_feeds_t
        {
        }
        addition_feeds;

        struct subtraction_feeds_t
        {
        }
        subtraction_feeds;

        struct fibonacci_seq_feeds_t
        {
            std::shared_ptr<fibonacci_seq_result_writer> result_writer;
        }
        fibonacci_seq_feeds;

        struct sum_all_feeds_t
        {
            std::shared_ptr<sum_all_value_reader> value;

        }
        sum_all_feeds;

        struct accumulator_feeds_t
        {
            std::shared_ptr<accumulator_value_reader> value;

            std::shared_ptr<accumulator_result_writer> result_writer;
        }
        accumulator_feeds;

        struct filter_feeds_t
        {
            std::shared_ptr<filter_value_reader> value;

            std::shared_ptr<filter_result_writer> result_writer;
        }
        filter_feeds;

        const frtps::GUID_t& get_client_id() const override
        {
            return client_id;
        }

        const frtps::RemoteLocatorList& get_client_locators() const override
        {
            return pub_data.remote_locators;
        }

        fdds::ReturnCode_t take_from(
                frpc::Replier* replier)
        {
            fdds::ReturnCode_t ret = replier->take_request(&request, info);
            if (ret == fdds::RETCODE_OK)
            {
                client_id = info.sample_identity.writer_guid();
                ret = replier->get_replier_reader()->get_matched_publication_data(pub_data, client_id);
            }
            return ret;
        }

        bool validate_request() const
        {
            size_t n_fields = 0;
            n_fields += request.representation_limits.has_value() ? 1 : 0;
            n_fields += request.addition.has_value() ? 1 : 0;
            n_fields += request.subtraction.has_value() ? 1 : 0;
            n_fields += request.fibonacci_seq.has_value() ? 1 : 0;
            n_fields += request.sum_all.has_value() ? 1 : 0;
            n_fields += request.sum_all_value.has_value() ? 1 : 0;
            n_fields += request.accumulator.has_value() ? 1 : 0;
            n_fields += request.accumulator_value.has_value() ? 1 : 0;
            n_fields += request.filter.has_value() ? 1 : 0;
            n_fields += request.filter_value.has_value() ? 1 : 0;
            n_fields += request.feed_cancel_.has_value() ? 1 : 0;

            return n_fields == 1U;
        }

        void process_additional_request(
                const std::shared_ptr<RequestContext>& ctx,
                frpc::Replier* replier,
                bool& should_erase)
        {
            should_erase = false;
            if (ctx->info.related_sample_identity == info.related_sample_identity)
            {
                if (ctx->request.feed_cancel_.has_value())
                {
                    if (output_feed_cancellator_)
                    {
                        output_feed_cancellator_->cancel();
                    }
                    else
                    {
                        EPROSIMA_LOG_ERROR(RPC_SERVER, "Output feed cancel request received, but no output feed is active.");
                    }

                    return;
                }
                // Pass request to input feed processors
                should_erase = true;
                for (const auto& input_feed : input_feed_processors_)
                {
                    if (input_feed->process_additional_request(ctx->request))
                    {
                        should_erase = false;
                        break;
                    }
                }

                // If no input feed processor handled the request, send an exception
                if (should_erase)
                {
                    send_exception(frpc::RemoteExceptionCode_t::REMOTE_EX_INVALID_ARGUMENT, replier);
                }
            }
            else
            {
                // This is not the expected request
                should_erase = true;
            }
        }

        bool prepare(
                frpc::Replier* replier)
        {
            if (request.representation_limits.has_value())
            {
                return prepare_representation_limits(replier);
            }

            if (request.addition.has_value())
            {
                return prepare_addition(replier);
            }

            if (request.subtraction.has_value())
            {
                return prepare_subtraction(replier);
            }

            if (request.fibonacci_seq.has_value())
            {
                return prepare_fibonacci_seq(replier);
            }

            if (request.sum_all.has_value())
            {
                return prepare_sum_all(replier);
            }

            if (request.accumulator.has_value())
            {
                return prepare_accumulator(replier);
            }

            if (request.filter.has_value())
            {
                return prepare_filter(replier);
            }


            send_exception(frpc::RemoteExceptionCode_t::REMOTE_EX_UNKNOWN_OPERATION, replier);
            return false;
        }

        void send_exception(
                frpc::RemoteExceptionCode_t ex,
                frpc::Replier* replier)
        {
            ReplyType reply{};
            reply.remoteEx = ex;
            replier->send_reply(&reply, info);
        }

        void cancel()
        {
            // Cancel output feed
            if (output_feed_cancellator_)
            {
                output_feed_cancellator_->cancel();
            }

            // Cancel input feeds
            for (const auto& input_feed : input_feed_processors_)
            {
                input_feed->cancel_input_feed();
            }
        }

    private:

        std::shared_ptr<IOutputFeedCancellator> output_feed_cancellator_;
        std::vector<std::shared_ptr<IInputFeedProcessor>> input_feed_processors_;

        bool prepare_representation_limits(
                frpc::Replier* replier)
        {
            static_cast<void>(replier);
            return true;
        }

        bool prepare_addition(
                frpc::Replier* replier)
        {
            static_cast<void>(replier);
            return true;
        }

        bool prepare_subtraction(
                frpc::Replier* replier)
        {
            static_cast<void>(replier);
            return true;
        }

        bool prepare_fibonacci_seq(
                frpc::Replier* replier)
        {
            static_cast<void>(replier);
            fibonacci_seq_feeds.result_writer = std::make_shared<fibonacci_seq_result_writer>(info, replier);
            output_feed_cancellator_ = fibonacci_seq_feeds.result_writer;
            return true;
        }

        bool prepare_sum_all(
                frpc::Replier* replier)
        {
            static_cast<void>(replier);
            sum_all_feeds.value = std::make_shared<sum_all_value_reader>();
            input_feed_processors_.push_back(sum_all_feeds.value);

            return true;
        }

        bool prepare_accumulator(
                frpc::Replier* replier)
        {
            static_cast<void>(replier);
            accumulator_feeds.value = std::make_shared<accumulator_value_reader>();
            input_feed_processors_.push_back(accumulator_feeds.value);

            accumulator_feeds.result_writer = std::make_shared<accumulator_result_writer>(info, replier);
            output_feed_cancellator_ = accumulator_feeds.result_writer;
            return true;
        }

        bool prepare_filter(
                frpc::Replier* replier)
        {
            static_cast<void>(replier);
            filter_feeds.value = std::make_shared<filter_value_reader>();
            input_feed_processors_.push_back(filter_feeds.value);

            filter_feeds.result_writer = std::make_shared<filter_result_writer>(info, replier);
            output_feed_cancellator_ = filter_feeds.result_writer;
            return true;
        }

    };

    struct ThreadPool
        : public frpc::RpcServerSchedulingStrategy
    {
        ThreadPool(
                CalculatorServerLogic& server,
                size_t num_threads)
            : server_(server)
        {
            // Create worker threads (at least one)
            if (num_threads == 0)
            {
                num_threads = 1;
            }

            auto process_requests = [this]()
                    {
                        while (!finished_)
                        {
                            std::shared_ptr<frpc::RpcRequest> req;
                            {
                                std::unique_lock<std::mutex> lock(mtx_);
                                cv_.wait(lock, [this]()
                                        {
                                            return finished_ || !requests_.empty();
                                        });
                                if (finished_)
                                {
                                    break;
                                }
                                req = requests_.front();
                                requests_.pop();
                            }

                            // Process the request
                            server_.execute_request(req);
                        }
                    };

            for (size_t i = 0; i < num_threads; ++i)
            {
                threads_.emplace_back(process_requests);
            }
        }

        void schedule_request(
                const std::shared_ptr<frpc::RpcRequest>& req,
                const std::shared_ptr<frpc::RpcServer>& server) override
        {
            static_cast<void>(server);

            std::lock_guard<std::mutex> lock(mtx_);
            if (!finished_)
            {
                requests_.push(req);
                cv_.notify_one();
            }
        }

        void server_stopped(
                const std::shared_ptr<frpc::RpcServer>& server) override
        {
            static_cast<void>(server);

            // Notify all threads in the pool to stop
            {
                std::lock_guard<std::mutex> lock(mtx_);
                finished_ = true;
                cv_.notify_all();
            }

            // Wait for all threads to finish
            for (auto& thread : threads_)
            {
                if (thread.joinable())
                {
                    thread.join();
                }
            }
            threads_.clear();
        }

        CalculatorServerLogic& server_;
        std::mutex mtx_;
        std::condition_variable cv_;
        std::queue<std::shared_ptr<frpc::RpcRequest>> requests_;
        bool finished_{ false };
        std::vector<std::thread> threads_;
    };

    void process_request(
            const std::shared_ptr<RequestContext>& ctx)
    {
        auto id = ctx->info.related_sample_identity;

        {
            std::lock_guard<std::mutex> _(mtx_);
            auto it = processing_requests_.find(id);
            if (it != processing_requests_.end())
            {
                bool should_erase = false;
                it->second->process_additional_request(ctx, replier_, should_erase);
                if (should_erase)
                {
                    processing_requests_.erase(it);
                }
                return;
            }

            if (!ctx->prepare(replier_))
            {
                return;
            }

            processing_requests_[id] = ctx;
        }

        request_scheduler_->schedule_request(ctx, shared_from_this());
    }

    void execute_request(
            const std::shared_ptr<RequestContext>& req)
    {
        try
        {
            for (;;)
            {
                if (req->request.representation_limits.has_value())
                {
                    {
                        ReplyType reply{};
                        reply.representation_limits = calculator_example::detail::Calculator_representation_limits_Result{};
                        reply.representation_limits->result = implementation_->representation_limits(
                            *req);
                        replier_->send_reply(&reply, req->info);
                    }
                    break;
                }

                if (req->request.addition.has_value())
                {
                    try
                    {
                        ReplyType reply{};
                        reply.addition = calculator_example::detail::Calculator_addition_Result{};
                        reply.addition->result = calculator_example::detail::Calculator_addition_Out{};
                        reply.addition->result->return_ = implementation_->addition(
                            *req,
                            req->request.addition->value1,
                            req->request.addition->value2);
                        replier_->send_reply(&reply, req->info);
                    }
                    catch (const calculator_example::OverflowException& ex)
                    {
                        ReplyType reply{};
                        reply.addition = calculator_example::detail::Calculator_addition_Result{};
                        reply.addition->calculator_example_OverflowException_ex = ex;
                        replier_->send_reply(&reply, req->info);
                    }
                    break;
                }

                if (req->request.subtraction.has_value())
                {
                    try
                    {
                        ReplyType reply{};
                        reply.subtraction = calculator_example::detail::Calculator_subtraction_Result{};
                        reply.subtraction->result = calculator_example::detail::Calculator_subtraction_Out{};
                        reply.subtraction->result->return_ = implementation_->subtraction(
                            *req,
                            req->request.subtraction->value1,
                            req->request.subtraction->value2);
                        replier_->send_reply(&reply, req->info);
                    }
                    catch (const calculator_example::OverflowException& ex)
                    {
                        ReplyType reply{};
                        reply.subtraction = calculator_example::detail::Calculator_subtraction_Result{};
                        reply.subtraction->calculator_example_OverflowException_ex = ex;
                        replier_->send_reply(&reply, req->info);
                    }
                    break;
                }

                if (req->request.fibonacci_seq.has_value())
                {
                    try
                    {
                        implementation_->fibonacci_seq(
                            *req,
                            req->request.fibonacci_seq->n_results,
                            *(req->fibonacci_seq_feeds.result_writer));
                        ReplyType reply{};
                        reply.fibonacci_seq = calculator_example::detail::Calculator_fibonacci_seq_Result{};
                        reply.fibonacci_seq->result = calculator_example::detail::Calculator_fibonacci_seq_Out{};
                        reply.fibonacci_seq->result->finished_ = true;
                        replier_->send_reply(&reply, req->info);
                    }
                    catch (const frpc::RpcFeedCancelledException& /*ex*/)
                    {
                        ReplyType reply{};
                        reply.fibonacci_seq = calculator_example::detail::Calculator_fibonacci_seq_Result{};
                        reply.fibonacci_seq->result = calculator_example::detail::Calculator_fibonacci_seq_Out{};
                        reply.fibonacci_seq->result->finished_ = true;
                        replier_->send_reply(&reply, req->info);
                    }
                    catch (const calculator_example::OverflowException& ex)
                    {
                        ReplyType reply{};
                        reply.fibonacci_seq = calculator_example::detail::Calculator_fibonacci_seq_Result{};
                        reply.fibonacci_seq->calculator_example_OverflowException_ex = ex;
                        replier_->send_reply(&reply, req->info);
                    }
                    break;
                }

                if (req->request.sum_all.has_value())
                {
                    try
                    {
                        ReplyType reply{};
                        reply.sum_all = calculator_example::detail::Calculator_sum_all_Result{};
                        reply.sum_all->result = calculator_example::detail::Calculator_sum_all_Out{};
                        reply.sum_all->result->return_ = implementation_->sum_all(
                            *req,
                            *(req->sum_all_feeds.value));
                        replier_->send_reply(&reply, req->info);
                    }
                    catch (const calculator_example::OverflowException& ex)
                    {
                        ReplyType reply{};
                        reply.sum_all = calculator_example::detail::Calculator_sum_all_Result{};
                        reply.sum_all->calculator_example_OverflowException_ex = ex;
                        replier_->send_reply(&reply, req->info);
                    }
                    break;
                }

                if (req->request.accumulator.has_value())
                {
                    try
                    {
                        implementation_->accumulator(
                            *req,
                            *(req->accumulator_feeds.value),
                            *(req->accumulator_feeds.result_writer));
                        ReplyType reply{};
                        reply.accumulator = calculator_example::detail::Calculator_accumulator_Result{};
                        reply.accumulator->result = calculator_example::detail::Calculator_accumulator_Out{};
                        reply.accumulator->result->finished_ = true;
                        replier_->send_reply(&reply, req->info);
                    }
                    catch (const frpc::RpcFeedCancelledException& /*ex*/)
                    {
                        ReplyType reply{};
                        reply.accumulator = calculator_example::detail::Calculator_accumulator_Result{};
                        reply.accumulator->result = calculator_example::detail::Calculator_accumulator_Out{};
                        reply.accumulator->result->finished_ = true;
                        replier_->send_reply(&reply, req->info);
                    }
                    catch (const calculator_example::OverflowException& ex)
                    {
                        ReplyType reply{};
                        reply.accumulator = calculator_example::detail::Calculator_accumulator_Result{};
                        reply.accumulator->calculator_example_OverflowException_ex = ex;
                        replier_->send_reply(&reply, req->info);
                    }
                    break;
                }

                if (req->request.filter.has_value())
                {
                    try
                    {
                        implementation_->filter(
                            *req,
                            *(req->filter_feeds.value),
                            req->request.filter->filter_kind,
                            *(req->filter_feeds.result_writer));
                        ReplyType reply{};
                        reply.filter = calculator_example::detail::Calculator_filter_Result{};
                        reply.filter->result = calculator_example::detail::Calculator_filter_Out{};
                        reply.filter->result->finished_ = true;
                        replier_->send_reply(&reply, req->info);
                    }
                    catch (const frpc::RpcFeedCancelledException& /*ex*/)
                    {
                        ReplyType reply{};
                        reply.filter = calculator_example::detail::Calculator_filter_Result{};
                        reply.filter->result = calculator_example::detail::Calculator_filter_Out{};
                        reply.filter->result->finished_ = true;
                        replier_->send_reply(&reply, req->info);
                    }
                    break;
                }

                req->send_exception(frpc::RemoteExceptionCode_t::REMOTE_EX_UNSUPPORTED, replier_);
                break;
            }
        }
        catch (const frpc::RpcRemoteException& ex)
        {
            req->send_exception(ex.code(), replier_);
        }
        catch (...)
        {
            req->send_exception(frpc::RemoteExceptionCode_t::REMOTE_EX_UNKNOWN_EXCEPTION, replier_);
        }

        {
            std::lock_guard<std::mutex> _(mtx_);
            processing_requests_.erase(req->info.related_sample_identity);
        }
    }

    fdds::DomainParticipant& participant_;
    frpc::Service* service_ = nullptr;
    frpc::Replier* replier_ = nullptr;
    fdds::GuardCondition finish_condition_;
    std::mutex mtx_;
    std::map<frtps::SampleIdentity, std::shared_ptr<RequestContext>> processing_requests_;
    std::shared_ptr<frpc::RpcServerSchedulingStrategy> request_scheduler_;
    std::shared_ptr<CalculatorServer_IServerImplementation> implementation_;

};

struct CalculatorServerProxy
    : public frpc::RpcServer
{
    CalculatorServerProxy(
            std::shared_ptr<frpc::RpcServer> impl)
        : impl_(std::move(impl))
    {
    }

    ~CalculatorServerProxy() override
    {
        if (impl_)
        {
            impl_->stop();
        }
    }

    void run() override
    {
        impl_->run();
    }

    void stop() override
    {
        impl_->stop();
    }

    void execute_request(
            const std::shared_ptr<frpc::RpcRequest>& request) override
    {
        impl_->execute_request(request);
    }

private:

   std::shared_ptr<frpc::RpcServer> impl_;
};

}  // namespace detail

std::shared_ptr<eprosima::fastdds::dds::rpc::RpcServer> create_CalculatorServer(
        eprosima::fastdds::dds::DomainParticipant& part,
        const char* service_name,
        const eprosima::fastdds::dds::ReplierQos& qos,
        size_t thread_pool_size,
        std::shared_ptr<CalculatorServer_IServerImplementation> implementation)
{
    auto ptr = std::make_shared<detail::CalculatorServerLogic>(
        part, service_name, qos, thread_pool_size, implementation);
    return std::make_shared<detail::CalculatorServerProxy>(ptr);
}

std::shared_ptr<eprosima::fastdds::dds::rpc::RpcServer> create_CalculatorServer(
        eprosima::fastdds::dds::DomainParticipant& part,
        const char* service_name,
        const eprosima::fastdds::dds::ReplierQos& qos,
        std::shared_ptr<eprosima::fastdds::dds::rpc::RpcServerSchedulingStrategy> scheduler,
        std::shared_ptr<CalculatorServer_IServerImplementation> implementation)
{
    auto ptr = std::make_shared<detail::CalculatorServerLogic>(
        part, service_name, qos, scheduler, implementation);
    return std::make_shared<detail::CalculatorServerProxy>(ptr);
}

//} interface Calculator


} // namespace calculator_example

