StateManager.hpp

Go to the documentation of this file.

#pragma once

 
#include <icarus/core/CoreTypes.hpp>
#include <icarus/signal/Registry.hpp>
 
#include <cstddef>
#include <string>
#include <unordered_set>
#include <vector>
 
namespace icarus {
 
// =============================================================================
// StateBinding (Phase 6)
// =============================================================================

template <typename Scalar> struct StateBinding {
    std::string name;                 
    std::string component_name;       
    Scalar *value_ptr = nullptr;      
    Scalar *derivative_ptr = nullptr; 
};
 
// =============================================================================
// StateManager (Phase 6: Unified Signal Model)
// =============================================================================

template <typename Scalar> class StateManager {
  public:
    StateManager() = default;

    void DiscoverStates(const SignalRegistry<Scalar> &registry) {
        bindings_.clear();
 
        // Get all state pairs from the registry
        const auto &pairs = registry.get_state_pairs();
 
        for (const auto &pair : pairs) {
            StateBinding<Scalar> binding;
            binding.name = pair.value_name;
            binding.component_name = ExtractComponentName(pair.value_name);
            binding.value_ptr = static_cast<Scalar *>(pair.value_ptr);
            binding.derivative_ptr = static_cast<Scalar *>(pair.derivative_ptr);
            bindings_.push_back(binding);
        }
 
        discovered_ = true;
    }

    void ZeroDerivatives() {
        for (auto &binding : bindings_) {
            *binding.derivative_ptr = Scalar{0};
        }
    }

    [[nodiscard]] std::size_t TotalSize() const { return bindings_.size(); }

    [[nodiscard]] JanusVector<Scalar> GetState() const {
        JanusVector<Scalar> X(static_cast<Eigen::Index>(bindings_.size()));
        for (std::size_t i = 0; i < bindings_.size(); ++i) {
            X(static_cast<Eigen::Index>(i)) = *bindings_[i].value_ptr;
        }
        return X;
    }

    void SetState(const JanusVector<Scalar> &X) {
        if (static_cast<std::size_t>(X.size()) != bindings_.size()) {
            throw std::invalid_argument("State vector size mismatch: expected " +
                                        std::to_string(bindings_.size()) + ", got " +
                                        std::to_string(X.size()));
        }
        for (std::size_t i = 0; i < bindings_.size(); ++i) {
            *bindings_[i].value_ptr = X(static_cast<Eigen::Index>(i));
        }
    }

    [[nodiscard]] JanusVector<Scalar> GetDerivatives() const {
        JanusVector<Scalar> X_dot(static_cast<Eigen::Index>(bindings_.size()));
        for (std::size_t i = 0; i < bindings_.size(); ++i) {
            X_dot(static_cast<Eigen::Index>(i)) = *bindings_[i].derivative_ptr;
        }
        return X_dot;
    }

    [[nodiscard]] JanusVector<Scalar>
    GetDerivatives(const std::unordered_set<std::string> &active_components) const {
        JanusVector<Scalar> X_dot(static_cast<Eigen::Index>(bindings_.size()));
        for (std::size_t i = 0; i < bindings_.size(); ++i) {
            bool is_active = active_components.empty() ||
                             active_components.contains(bindings_[i].component_name);
            if (is_active) {
                X_dot(static_cast<Eigen::Index>(i)) = *bindings_[i].derivative_ptr;
            } else {
                X_dot(static_cast<Eigen::Index>(i)) = Scalar{0}; // Freeze state
            }
        }
        return X_dot;
    }

    [[nodiscard]] const std::vector<StateBinding<Scalar>> &GetBindings() const { return bindings_; }

    [[nodiscard]] bool IsAllocated() const { return discovered_; }
 
  private:
    std::vector<StateBinding<Scalar>> bindings_; 
    bool discovered_ = false;                    

    [[nodiscard]] static std::string ExtractComponentName(const std::string &signal_name) {
        // Signal name format: "Entity.Component.signal" or "Entity.Component.signal.axis"
        // We need to extract "Entity.Component" for scheduler matching
        // e.g., "Rocket.FuelTank.fuel_mass" -> "Rocket.FuelTank"
        auto first_dot = signal_name.find('.');
        if (first_dot == std::string::npos) {
            return signal_name; // No dot, entire name is component
        }
        auto second_dot = signal_name.find('.', first_dot + 1);
        if (second_dot == std::string::npos) {
            return signal_name.substr(0, first_dot); // Only one dot
        }
        return signal_name.substr(0, second_dot);
    }
};
 
} // namespace icarus