PhoenixSignalManager.h

 
#pragma once
#include <vector>
#include <string>
#include <unordered_set>
#include <unordered_map>
#include <ctre/phoenix6/StatusSignal.hpp>
#include <frc2/command/CommandPtr.h>
#include <frc2/command/Commands.h>
 
namespace valor {
 
namespace util {
 
class PhoenixSignalManager {
   public:
    static PhoenixSignalManager& GetInstance();
 
    template <std::derived_from<ctre::phoenix6::BaseStatusSignal>... Signals>
    void AddSignals(ctre::phoenix6::CANBus bus, Signals&... signals) {
        wpi::array<ctre::phoenix6::BaseStatusSignal*, sizeof...(Signals)> newSignals{(&signals)...};
        auto& bucket = defaultSignals[std::string{bus.GetName()}];
        bucket.insert(bucket.begin(), newSignals.begin(), newSignals.end());
    }
 
    template <std::derived_from<ctre::phoenix6::BaseStatusSignal>... Signals>
    void AddSignals(std::string bucketName, ctre::phoenix6::CANBus bus, Signals&... signals) {
        wpi::array<ctre::phoenix6::BaseStatusSignal*, sizeof...(Signals)> newSignals{(&signals)...};
        auto& bucket = buckets[bucketName][std::string{bus.GetName()}];
        bucket.insert(bucket.begin(), newSignals.begin(), newSignals.end());
    }
 
    template <std::derived_from<ctre::phoenix6::BaseStatusSignal>... Signals>
    void AddSignals(std::string bucketName, Signals&... signals) {
        wpi::array<ctre::phoenix6::BaseStatusSignal*, sizeof...(Signals)> newSignals{(&signals)...};
        auto& bucket = buckets[bucketName];
        for (auto& sig : newSignals) {
            for (auto& [busName, signals] : defaultSignals) {
                auto it = std::find(signals.begin(), signals.end(), sig);
                if (it != signals.end()) {
                    signals.erase(it);
                    bucket[busName].push_back(sig);
                    break;
                }
            }
        }
    }
 
    void RefreshSignals(std::string bucket);
    void WaitForSignals(std::string bucket, units::second_t timeout);
 
    void RefreshSignals();
 
    void AutoRefreshSignals(bool refresh);
 
   private:
    PhoenixSignalManager();
 
    frc2::CommandPtr refreshSignalsCommand{frc2::cmd::Run([this] { RefreshSignals(); }).IgnoringDisable(true)};
    std::unordered_map<std::string, std::unordered_map<std::string, std::vector<ctre::phoenix6::BaseStatusSignal*>>> buckets;
    std::unordered_map<std::string, std::vector<ctre::phoenix6::BaseStatusSignal*>> defaultSignals;
};
 
}  // namespace util
 
}  // namespace valor