multi_output_manager.hpp

Go to the documentation of this file.

#pragma once
 
#include "commrat/module/module_config.hpp"
#include "commrat/module/helpers/address_helpers.hpp"
#include "commrat/platform/threading.hpp"
#include <vector>
#include <mutex>
#include <algorithm>
#include <iostream>
#include <typeinfo>
 
namespace commrat {
 
struct SubscriberInfo {
    uint32_t base_addr{0};    
    uint8_t input_index{0};   
    
    bool operator==(const SubscriberInfo& other) const {
        return base_addr == other.base_addr && input_index == other.input_index;
    }
};
 
template<typename Derived, typename UserRegistry, typename OutputTypesTuple>
class MultiOutputManager {
protected:
    // CRTP: Get reference to derived Module class
    Derived& derived() { return static_cast<Derived&>(*this); }
    const Derived& derived() const { return static_cast<const Derived&>(*this); }
    
    // ========================================================================
    // Multi-Output Subscriber Management
    // ========================================================================
    
    std::vector<std::vector<SubscriberInfo>> output_subscribers_;
    mutable Mutex output_subscribers_mutex_;  // Protects output_subscribers_
    
    void initialize_output_subscribers() {
        constexpr std::size_t num_outputs = std::tuple_size_v<OutputTypesTuple>;
        output_subscribers_.resize(num_outputs);
    }
    
    void add_subscriber_to_output(uint32_t subscriber_base_addr, uint8_t input_index = 0, std::size_t output_idx = 0) {
        constexpr std::size_t num_outputs = std::tuple_size_v<OutputTypesTuple>;
        
        if (output_idx >= num_outputs) {
            std::cerr << "[" << derived().config_.name << "] ERROR: Output index " << output_idx 
                      << " out of range (num_outputs=" << num_outputs << ")\n";
            return;
        }
        
        Lock lock(output_subscribers_mutex_);
        auto& subs = output_subscribers_[output_idx];
        
        SubscriberInfo sub_info{subscriber_base_addr, input_index};
        
        // Check if already subscribed
        if (std::find(subs.begin(), subs.end(), sub_info) == subs.end()) {
            subs.push_back(sub_info);
            std::cout << "[" << derived().config_.name << "] Added subscriber " << subscriber_base_addr 
                      << " (input_idx=" << static_cast<int>(input_index) << ") to output[" << output_idx 
                      << "] (total: " << subs.size() << ")\n";
        }
    }
 
public:
    std::vector<SubscriberInfo> get_output_subscribers(std::size_t output_idx) const {
        Lock lock(output_subscribers_mutex_);
        if (output_idx < output_subscribers_.size()) {
            return output_subscribers_[output_idx];
        }
        return {};
    }
    
    void remove_subscriber(uint32_t subscriber_base_addr) {
        Lock lock(output_subscribers_mutex_);
        for (auto& output_subs : output_subscribers_) {
            auto it = std::remove_if(output_subs.begin(), output_subs.end(),
                [subscriber_base_addr](const SubscriberInfo& sub) {
                    return sub.base_addr == subscriber_base_addr;
                });
            output_subs.erase(it, output_subs.end());
        }
    }
 
private:
    template<std::size_t... Is>
    std::size_t find_output_index_by_type_id_impl(uint16_t type_id_low, std::index_sequence<Is...>) const {
        constexpr std::size_t num_outputs = std::tuple_size_v<OutputTypesTuple>;
        std::size_t result = num_outputs;  // Invalid index by default
        
        // Check each output type's message ID using fold expression
        ((check_output_type_match<Is>(type_id_low, result)) || ...);
        
        return result;
    }
    
    template<std::size_t Index>
    bool check_output_type_match(uint16_t type_id_low, std::size_t& result) const {
        using OutputType = std::tuple_element_t<Index, OutputTypesTuple>;
        constexpr uint32_t output_msg_id = UserRegistry::template get_message_id<OutputType>();
        constexpr uint16_t output_type_id_low = static_cast<uint16_t>(output_msg_id & 0xFFFF);
        
        if (output_type_id_low == type_id_low) {
            result = Index;
            return true;  // Found match, stop searching
        }
        return false;  // Continue searching
    }
    
protected:
    std::size_t find_output_index_by_type_id(uint16_t type_id_low) const {
        constexpr std::size_t num_outputs = std::tuple_size_v<OutputTypesTuple>;
        return find_output_index_by_type_id_impl(type_id_low, std::make_index_sequence<num_outputs>{});
    }
    
    // ========================================================================
    // Multi-Output Work Threads
    // ========================================================================
    
    std::vector<std::thread> output_work_threads_;
    
    template<std::size_t... Is>
    void spawn_all_output_work_threads(std::index_sequence<Is...>) {
        // Reserve space for all threads
        output_work_threads_.reserve(sizeof...(Is));
        
        // Spawn one thread per output type using fold expression
        (void)std::initializer_list<int>{
            (spawn_output_work_thread<Is>(), 0)...
        };
    }
    
    template<std::size_t Index>
    void spawn_output_work_thread() {
        output_work_threads_.emplace_back([this]() {
            derived().template output_work_loop<Index>();
        });
    }
    
    void join_output_work_threads() {
        std::cout << "[" << derived().config_.name << "] Joining " << output_work_threads_.size() 
                  << " output work threads...\n";
        for (auto& thread : output_work_threads_) {
            if (thread.joinable()) {
                thread.join();
            }
        }
        output_work_threads_.clear();
    }
    
    template<std::size_t Index>
    void output_work_loop() {
        using OutputType = std::tuple_element_t<Index, OutputTypesTuple>;
        // For multi-output config, use indexed access
        uint8_t sys_id, inst_id;
        if (derived().config_.has_multi_output_config()) {
            sys_id = derived().config_.system_id(Index);
            inst_id = derived().config_.instance_id(Index);
        } else {
            sys_id = derived().config_.system_id();
            inst_id = derived().config_.instance_id();
        }
        uint32_t work_mailbox_addr = commrat::get_mailbox_address<OutputType, OutputTypesTuple, UserRegistry>(
            sys_id, inst_id, static_cast<uint8_t>(MailboxType::WORK));
        
        std::cout << "[" << derived().config_.name << "] output_work_loop[" << Index << "] started for "
                  << typeid(OutputType).name() << ", listening on WORK mailbox " 
                  << work_mailbox_addr << "\n" << std::flush;
        
        auto& work_mbx = derived().template get_work_mailbox<Index>();
        
        while (derived().running_) {
            std::cout << "[" << derived().config_.name << "] output_work_loop[" << Index << "]: waiting for message...\n" << std::flush;
            auto visitor = [this](auto&& tims_msg) {
                auto& msg = tims_msg.payload;
                using MsgType = std::decay_t<decltype(msg)>;
                
                if constexpr (std::is_same_v<MsgType, SubscribeRequestType>) {
                    std::cout << "[" << derived().config_.name << "] output_work_loop[" << Index 
                              << "] Handling SubscribeRequest\n";
                    derived().handle_subscribe_request(msg, Index);
                } else if constexpr (std::is_same_v<MsgType, SubscribeReplyType>) {
                    std::cout << "[" << derived().config_.name << "] output_work_loop[" << Index 
                              << "] Handling SubscribeReply\n";
                    derived().handle_subscribe_reply(msg);
                } else if constexpr (std::is_same_v<MsgType, UnsubscribeRequestType>) {
                    std::cout << "[" << derived().config_.name << "] output_work_loop[" << Index 
                              << "] Handling UnsubscribeRequest\n";
                    derived().handle_unsubscribe_request(msg);
                }
            };
            
            work_mbx.receive_any(visitor);
        }
        
        std::cout << "[" << derived().config_.name << "] output_work_loop[" << Index << "] ended\n";
    }
    
    // ========================================================================
    // Multi-Output Mailbox Set Lifecycle
    // ========================================================================
    
    template<std::size_t... Is>
    void start_all_mailbox_sets(std::index_sequence<Is...>) {
        (void)std::initializer_list<int>{
            (start_mailbox_set<Is>(), 0)...
        };
    }
    
    template<std::size_t Index>
    void start_mailbox_set() {
        auto& cmd = derived().template get_cmd_mailbox<Index>();
        auto cmd_result = cmd.start();
        if (!cmd_result) {
            throw std::runtime_error("[Module] Failed to start CMD mailbox for output " + std::to_string(Index));
        }
        
        auto& work = derived().template get_work_mailbox<Index>();
        auto work_result = work.start();
        if (!work_result) {
            throw std::runtime_error("[Module] Failed to start WORK mailbox for output " + std::to_string(Index));
        }
        
        auto& publish = derived().template get_publish_mailbox_public<Index>();
        auto publish_result = publish.start();
        if (!publish_result) {
            throw std::runtime_error("[Module] Failed to start PUBLISH mailbox for output " + std::to_string(Index));
        }
    }
    
    template<std::size_t... Is>
    void stop_all_mailbox_sets(std::index_sequence<Is...>) {
        (void)std::initializer_list<int>{
            (stop_mailbox_set<Is>(), 0)...
        };
    }
    
    template<std::size_t Index>
    void stop_mailbox_set() {
        derived().template get_cmd_mailbox<Index>().stop();
        derived().template get_work_mailbox<Index>().stop();
        derived().template get_publish_mailbox_public<Index>().stop();
    }
    
    // ========================================================================
    // Multi-Output Mailbox Accessors
    // ========================================================================
    
    template<std::size_t Index>
    auto& get_mailbox_set() {
        return std::get<Index>(derived().mailbox_infrastructure_);
    }
    
    template<std::size_t Index>
    auto& get_cmd_mailbox() {
        return *std::get<Index>(derived().mailbox_infrastructure_).cmd;
    }
    
    template<std::size_t Index>
    auto& get_work_mailbox() {
        return *std::get<Index>(derived().mailbox_infrastructure_).work;
    }
    
    template<std::size_t Index>
    auto& get_publish_mailbox() {
        return *std::get<Index>(derived().mailbox_infrastructure_).publish;
    }
    
public:
    template<std::size_t Index>
    auto& get_publish_mailbox_public() {
        return *std::get<Index>(derived().mailbox_infrastructure_).publish;
    }
};
 
}  // namespace commrat