multi_input_infrastructure.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "commrat/mailbox/historical_mailbox.hpp"
#include "commrat/module/module_config.hpp"
#include "commrat/module/helpers/address_helpers.hpp"
#include <iostream>
#include <tuple>
#include <optional>
#include <thread>
#include <vector>
 
namespace commrat {
 
template<typename ModuleType, typename UserRegistry, typename InputTypesTuple, std::size_t InputCount>
class MultiInputInfrastructure {
protected:
    // Helper: Create HistoricalMailbox type for each input type
    template<typename T>
    using HistoricalMailboxFor = HistoricalMailbox<UserRegistry, 100>; // TODO: Make history size configurable
    
    // Generate tuple of HistoricalMailbox types from InputTypesTuple
    template<typename Tuple>
    struct MakeHistoricalMailboxTuple;
    
    template<typename... Ts>
    struct MakeHistoricalMailboxTuple<std::tuple<Ts...>> {
        using type = std::tuple<HistoricalMailboxFor<Ts>...>;
    };
    
    using HistoricalMailboxTuple = typename MakeHistoricalMailboxTuple<InputTypesTuple>::type;
    std::optional<HistoricalMailboxTuple> input_mailboxes_;
    
    std::vector<std::thread> secondary_input_threads_;
    
    void initialize_multi_input_mailboxes() {
        create_input_mailboxes_impl(std::make_index_sequence<InputCount>{});
    }
    
    void start_input_mailboxes() {
        start_input_mailboxes_impl(std::make_index_sequence<InputCount>{});
    }
    
    template<std::size_t PrimaryIdx>
    void start_secondary_input_threads() {
        start_secondary_threads_impl<PrimaryIdx>(std::make_index_sequence<InputCount>{});
    }
    
    void join_secondary_input_threads() {
        for (auto& thread : secondary_input_threads_) {
            if (thread.joinable()) {
                thread.join();
            }
        }
    }
    
private:
    template<std::size_t Index>
    auto create_historical_mailbox_for_input() {
        auto& module = static_cast<ModuleType&>(*this);
        using InputType = std::tuple_element_t<Index, InputTypesTuple>;
        
        // RACK-style addressing: [type][sys][inst][mbx_idx]
        // Base address contains module identity (type/system/instance)
        using OutputData = typename ModuleType::OutputData;
        using OutputTypesTuple = typename ModuleType::OutputTypesTuple;
        uint32_t base_addr = commrat::calculate_base_address<OutputData, OutputTypesTuple, UserRegistry>(
            module.config_.system_id(), module.config_.instance_id());
        
        // DATA mailboxes start after all CMD mailboxes
        // Get num_outputs from ModuleType
        constexpr uint8_t num_outputs = std::tuple_size_v<OutputTypesTuple>;
        uint8_t data_mbx_index = get_data_mbx_base(num_outputs) + static_cast<uint8_t>(Index);
        
        // Full mailbox address = base | mailbox_index
        uint32_t data_mailbox_id = base_addr | data_mbx_index;
        
        // Compile-time size for THIS specific input type (memory efficient!)
        constexpr size_t input_message_size = get_data_mailbox_size<InputType>();
        
        std::cout << "[" << module.config_.name << "] Creating DATA mailbox[" << Index 
                  << "] at 0x" << std::hex << data_mailbox_id << std::dec
                  << " (base=0x" << std::hex << base_addr << std::dec 
                  << ", index=" << static_cast<int>(data_mbx_index)
                  << ", size=" << input_message_size << " bytes)\n";
        
        MailboxConfig mbx_config{
            .mailbox_id = data_mailbox_id,
            .message_slots = module.config_.data_message_slots.value(),  // Extract from DefaultVal
            .max_message_size = input_message_size,              // Per-input-type size!
            .send_priority = static_cast<uint8_t>(module.config_.priority),
            .realtime = module.config_.realtime,
            .mailbox_name = module.config_.name + "_data_" + std::to_string(Index)
        };
        
        return HistoricalMailboxFor<InputType>(
            mbx_config,
            module.config_.sync_tolerance()
        );
    }
    
    template<std::size_t... Is>
    void create_input_mailboxes_impl(std::index_sequence<Is...>) {
        input_mailboxes_ = std::make_tuple(
            create_historical_mailbox_for_input<Is>()...
        );
    }
    
    template<std::size_t... Is>
    void start_input_mailboxes_impl(std::index_sequence<Is...>) {
        auto& module = static_cast<ModuleType&>(*this);
        
        if (input_mailboxes_) {
            // Check each mailbox start result
            ([&]() {
                auto result = std::get<Is>(*input_mailboxes_).start();
                if (!result) {
                    std::cerr << "[" << module.config_.name << "] ERROR: Failed to start input mailbox " 
                              << Is << " - error " << static_cast<int>(result.get_error()) << "\n";
                }
            }(), ...);
        }
    }
    
    template<std::size_t PrimaryIdx, std::size_t... Is>
    void start_secondary_threads_impl(std::index_sequence<Is...>) {
        auto& module = static_cast<ModuleType&>(*this);
        
        // Start thread for each input except primary
        ((Is != PrimaryIdx ? 
          (secondary_input_threads_.emplace_back(&ModuleType::template secondary_input_receive_loop<Is>, &module), true) : 
          true), ...);
    }
    
protected:
    template<std::size_t InputIdx>
    void secondary_input_receive_loop() {
        auto& module = static_cast<ModuleType&>(*this);
        using InputType = std::tuple_element_t<InputIdx, InputTypesTuple>;
        auto& mailbox = std::get<InputIdx>(*input_mailboxes_);
        
        std::cout << "[" << module.config_.name << "] secondary_input_receive_loop[" << InputIdx << "] started\n";
        
        int receive_count = 0;
        while (module.running_) {
            // Blocking receive - stores in historical buffer automatically
            auto result = mailbox.template receive<InputType>();
            if (!result.has_value()) {
                std::cout << "[" << module.config_.name << "] secondary_input_receive_loop[" << InputIdx 
                          << "] receive failed after " << receive_count << " messages\n";
                break;
            }
            receive_count++;
            if (receive_count <= 3) {
                std::cout << "[" << module.config_.name << "] secondary_input_receive_loop[" << InputIdx 
                          << "] received message #" << receive_count 
                          << ", timestamp=" << result.value().header.timestamp << "\n";
            }
        }
        
        std::cout << "[" << module.config_.name << "] secondary_input_receive_loop[" << InputIdx 
                  << "] ended (total: " << receive_count << " messages)\n";
    }
};
 
} // namespace commrat