PhoenixSignalManager.h

#pragma once
#include <string>
#include <unordered_map>
 
#include <ctre/phoenix6/StatusSignalCollection.hpp>
#include <frc2/command/CommandPtr.h>
#include <frc2/command/Commands.h>
 
namespace valor {
 
namespace util {
 
class PhoenixSignalManager {
   public:
    class StatusSignalCollection : public ctre::phoenix6::StatusSignalCollection {
       public:
        constexpr std::vector<ctre::phoenix6::BaseStatusSignal*>& GetSignals() { return _signals; }
    };
 
    static PhoenixSignalManager& GetInstance();
 
    template <std::derived_from<ctre::phoenix6::BaseStatusSignal>... Signals>
    inline void AddSignals(ctre::phoenix6::CANBus bus, Signals&... signals) {
        defaultSignals[std::string{bus.GetName()}].AddSignals(signals...);
    }
 
    template <std::derived_from<ctre::phoenix6::BaseStatusSignal>... Signals>
    inline void AddSignals(std::string bucketName, ctre::phoenix6::CANBus bus, Signals&... signals) {
        buckets[bucketName][std::string{bus.GetName()}].AddSignals(signals...);
    }
 
    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.GetSignals().begin(), signals.GetSignals().end(), sig);
                if (it != signals.GetSignals().end()) {
                    signals.GetSignals().erase(it);
                    bucket[busName].AddSignals(*sig);
                    break;
                }
            }
        }
    }
 
    void RefreshSignals(std::string bucket);
    void WaitForSignals(std::string bucket, units::second_t timeout);
 
    void RefreshSignals();
 
    void AutoRefreshSignals(bool refresh);
 
    void RemoveSignals(std::string bucketName);
 
   private:
    PhoenixSignalManager();
 
    frc2::CommandPtr refreshSignalsCommand{frc2::cmd::Run([this] { RefreshSignals(); }).IgnoringDisable(true)};
    std::unordered_map<std::string, std::unordered_map<std::string, StatusSignalCollection>> buckets;
    std::unordered_map<std::string, StatusSignalCollection> defaultSignals;
};
 
}  // namespace util
 
}  // namespace valor