Loggable.h

#pragma once
#include <algorithm>
#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include <frc/smartdashboard/SmartDashboard.h>
#include <magic_enum/magic_enum.hpp>
#include <networktables/StructArrayTopic.h>
#include <networktables/StructTopic.h>
#include <units/base.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/struct/Struct.h>
 
namespace valor {
 
namespace detail {
 
struct string_hash {
    using is_transparent = void;
 
    size_t operator()(std::string_view sv) const { return std::hash<std::string_view>{}(sv); }
 
    size_t operator()(const std::string& s) const { return std::hash<std::string>{}(s); }
};
 
struct string_equal {
    using is_transparent = void;
 
    bool operator()(std::string_view lhs, std::string_view rhs) const { return lhs == rhs; }
};
 
}  // namespace detail

class Loggable {
    std::unordered_map<std::string, std::unique_ptr<nt::Publisher>, detail::string_hash, detail::string_equal> publishers;
 
   public:
    explicit Loggable(std::string_view name);

    explicit Loggable(std::shared_ptr<nt::NetworkTable> table);

    Loggable() = default;

    void LogChild(std::string_view name, Loggable* child);

    void LogChild(std::string_view name, wpi::Sendable* child);
 
    inline bool WriteLog(std::string_view field, bool data) {
        if (table)
            frc::SmartDashboard::PutBoolean(smartDashboardPrefix + std::string{field}, data);
        return data;
    }
 
    inline double WriteLog(std::string_view field, double data) {
        if (table)
            frc::SmartDashboard::PutNumber(smartDashboardPrefix + std::string{field}, data);
        return data;
    }
 
    inline void WriteLog(std::string_view field, const std::vector<double>& data) {
        if (table)
            frc::SmartDashboard::PutNumberArray(smartDashboardPrefix + std::string{field}, data);
    }
 
    template <typename U>
        requires units::traits::is_unit_t_v<U>
    inline U WriteLog(std::string_view field, U data) {
        WriteLog(field, data.value());
        return data;
    }
 
    inline void WriteLog(std::string_view field, std::string_view data) {
        if (table)
            frc::SmartDashboard::PutString(smartDashboardPrefix + std::string{field}, data);
    }
 
    template <typename E>
        requires std::is_enum_v<E>
    inline void WriteLog(std::string_view field, E data) {
        WriteLog(field, magic_enum::enum_name(data));
    }
 
    template <typename T>
        requires wpi::StructSerializable<T> && (!std::is_arithmetic_v<T>)
    inline void WriteLog(std::string_view field, const T& data) {
        if (!table)
            return;
        auto pub = publishers.find(field);
        if (pub == publishers.end())
            pub = publishers
                      .try_emplace(std::string{field}, std::make_unique<nt::StructPublisher<T>>(table->GetStructTopic<T>(field).Publish()))
                      .first;
        static_cast<nt::StructPublisher<T>*>(pub->second.get())->Set(data);
    }
 
    template <typename T>
        requires wpi::StructSerializable<T> && (!std::is_arithmetic_v<T>)
    inline void WriteLog(std::string_view field, const std::vector<T>& data) {
        if (!table)
            return;
        auto pub = publishers.find(field);
        if (pub == publishers.end())
            pub = publishers
                      .try_emplace(std::string{field},
                                   std::make_unique<nt::StructArrayPublisher<T>>(table->GetStructArrayTopic<T>(field).Publish()))
                      .first;
        static_cast<nt::StructArrayPublisher<T>*>(pub->second.get())->Set(data);
    }
 
    inline bool ReadLog(std::string_view field, bool defaultValue) {
        if (table)
            return frc::SmartDashboard::GetBoolean(smartDashboardPrefix + std::string{field}, defaultValue);
        return defaultValue;
    }
 
    inline double ReadLog(std::string_view field, double defaultValue) {
        if (table)
            return frc::SmartDashboard::GetNumber(smartDashboardPrefix + std::string{field}, defaultValue);
        return defaultValue;
    }
 
    template <typename U>
        requires units::traits::is_unit_t_v<U>
    inline U ReadLog(std::string_view field, U defaultValue) {
        return U{ReadLog(field, defaultValue.value())};
    }
 
    static constexpr Loggable& GetRoot() { return rootLoggable; }
 
   protected:
    virtual void OnLoggingStart() {}

    virtual void LoggablePeriodic() {}
 
   private:
    std::string GetSmartDashboardPrefix() const;
 
    std::shared_ptr<nt::NetworkTable> table;
    std::string smartDashboardPrefix;
    std::unordered_map<std::string, Loggable*> loggableChildren;
    std::unordered_map<std::string, wpi::Sendable*> sendableChildren;
    static Loggable rootLoggable;
};
 
}  // namespace valor