Loggable.h

#pragma once
#include <algorithm>
#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include <frc/Errors.h>
#include <frc/Timer.h>
#include <magic_enum/magic_enum.hpp>
#include <networktables/BooleanArrayTopic.h>
#include <networktables/BooleanTopic.h>
#include <networktables/DoubleArrayTopic.h>
#include <networktables/DoubleTopic.h>
#include <networktables/FloatTopic.h>
#include <networktables/IntegerArrayTopic.h>
#include <networktables/IntegerTopic.h>
#include <networktables/NetworkTableInstance.h>
#include <networktables/ProtobufTopic.h>
#include <networktables/StringArrayTopic.h>
#include <networktables/StringTopic.h>
#include <networktables/StructArrayTopic.h>
#include <networktables/StructTopic.h>
#include <units/acceleration.h>
#include <units/angular_acceleration.h>
#include <units/angular_velocity.h>
#include <units/base.h>
#include <units/current.h>
#include <units/temperature.h>
#include <units/time.h>
#include <wpi/json.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/struct/Struct.h>
 
namespace valor {
 
namespace detail {
template <typename T>
inline constexpr bool is_unit_vector = false;
 
template <typename T>
inline constexpr bool is_unit_vector<std::vector<T>> = units::traits::is_unit_t_v<T>;
 
template <typename T>
struct nt_traits {
    static_assert(sizeof(T) == 0, "No valid NetworkTables trait found for the specified data type");
};
 
template <>
struct nt_traits<bool> {
    static constexpr auto func = &nt::NetworkTable::GetBooleanTopic;
    static constexpr std::string_view type = "boolean";
};
 
template <>
struct nt_traits<float> {
    static constexpr auto func = &nt::NetworkTable::GetFloatTopic;
    static constexpr std::string_view type = "float";
};
 
template <>
struct nt_traits<double> {
    static constexpr auto func = &nt::NetworkTable::GetDoubleTopic;
    static constexpr std::string_view type = "double";
};
 
template <typename T>
    requires units::traits::is_unit_t_v<T>
struct nt_traits<T> : nt_traits<double> {};
 
template <std::integral T>
struct nt_traits<T> {
    static constexpr auto func = &nt::NetworkTable::GetIntegerTopic;
    static constexpr std::string_view type = "int";
};
 
template <typename T>
    requires std::convertible_to<T, std::string_view>
struct nt_traits<T> {
    static constexpr auto func = &nt::NetworkTable::GetStringTopic;
    static constexpr std::string_view type = "string";
};
 
template <typename T>
    requires std::is_enum_v<T>
struct nt_traits<T> : nt_traits<std::string_view> {};
 
template <typename T>
    requires wpi::StructSerializable<T> && (!std::is_arithmetic_v<T>)
struct nt_traits<T> {
    static constexpr auto func = &nt::NetworkTable::GetStructTopic<T>;
    inline static std::string type = "struct:" + std::string{wpi::Struct<T>::GetTypeName()};
};
 
template <typename T>
    requires wpi::ProtobufSerializable<T> && (!wpi::StructSerializable<T>)
struct nt_traits<T> {
    static constexpr auto func = &nt::NetworkTable::GetProtobufTopic<T>;
    static constexpr std::string_view type = "proto:";
};
 
template <>
struct nt_traits<std::vector<bool>> {
    static constexpr auto func = &nt::NetworkTable::GetBooleanArrayTopic;
    static constexpr std::string_view type = "boolean[]";
};
 
template <>
struct nt_traits<std::vector<float>> {
    static constexpr auto func = &nt::NetworkTable::GetFloatArrayTopic;
    static constexpr std::string_view type = "float[]";
};
 
template <>
struct nt_traits<std::vector<double>> {
    static constexpr auto func = &nt::NetworkTable::GetDoubleArrayTopic;
    static constexpr std::string_view type = "double[]";
};
 
template <typename T>
    requires is_unit_vector<T>
struct nt_traits<T> : nt_traits<std::vector<double>> {};
 
template <std::integral T>
struct nt_traits<std::vector<T>> {
    static constexpr auto func = &nt::NetworkTable::GetIntegerArrayTopic;
    static constexpr std::string_view type = "int64[]";
};
 
template <typename T>
    requires std::convertible_to<T, std::string_view>
struct nt_traits<std::vector<T>> {
    static constexpr auto func = &nt::NetworkTable::GetStringArrayTopic;
    static constexpr std::string_view type = "string[]";
};
 
template <typename T>
    requires std::is_enum_v<T>
struct nt_traits<std::vector<T>> : nt_traits<std::vector<std::string>> {};
 
template <typename T>
    requires wpi::StructSerializable<T> && (!std::is_arithmetic_v<T>)
struct nt_traits<std::vector<T>> {
    static constexpr auto func = &nt::NetworkTable::GetStructArrayTopic<T>;
    inline static std::string type = "struct:" + std::string{wpi::Struct<T>::GetTypeName()} + "[]";
};
 
template <typename T>
inline constexpr std::string_view unit_string = "";
 
#define UNIT_STRING(unit, str) \
    template <>                \
    inline constexpr std::string_view unit_string<units::unit##_t> = str
UNIT_STRING(turn, "rot");
 
UNIT_STRING(radians_per_second, "rad per sec");
UNIT_STRING(degrees_per_second, "deg per sec");
UNIT_STRING(turns_per_second, "rps");
 
UNIT_STRING(meters_per_second_squared, "mps2");
UNIT_STRING(feet_per_second_squared, "fps2");
UNIT_STRING(standard_gravity, "g");
 
UNIT_STRING(radians_per_second_squared, "rad per sec2");
UNIT_STRING(degrees_per_second_squared, "deg per sec2");
UNIT_STRING(turns_per_second_squared, "rps2");
 
UNIT_STRING(day, "day");
 
UNIT_STRING(celsius, "c");
UNIT_STRING(fahrenheit, "f");
 
UNIT_STRING(ampere, "amp");
#undef UNIT_STRING
 
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

enum class LogLevel {
    Force = 0,
    Normal = 100,
    Important = 50,
    Critical = 20,
};

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

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

    Loggable();

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

    void LogChild(std::string_view name, wpi::Sendable* child);
 
    static units::millisecond_t GetLoggingTime();

    void SetPeriodicLevel(LogLevel level) { periodicLevel = level; }

    template <typename T>
    inline T WriteLog(std::string_view field, const T& data, LogLevel level = LogLevel::Normal) {
        loggingProfiler.Start();
        if (!table) {
            loggingProfiler.Stop();
            return data;
        }
        auto pub = publishers.find(field);
        if (pub == publishers.end())
            pub = addPublisher<T>(field);
        else if (!inPeriodic && frc::Timer::GetTimestamp() - pub->second.second <
                                    units::millisecond_t{static_cast<double>(level)} - RATE_LIMIT_TIME_TOLERANCE) {
            loggingProfiler.Stop();
            return data;
        }
        // else
        //     checkTopicType<T>(pub->second->GetTopic().GetTypeString(), field);
        WriteLogImpl(pub->second.first.get(), data);
        pub->second.second = frc::Timer::GetTimestamp();
        loggingProfiler.Stop();
        return data;
    }

    template <typename T>
    inline T ReadLog(std::string_view field, const T& defaultValue = {}) {
        loggingProfiler.Start();
        if (!table) {
            loggingProfiler.Stop();
            return defaultValue;
        }
        auto sub = subscribers.find(field);
        if (sub == subscribers.end())
            sub = addSubscriber(field, defaultValue);
        // else
        //     checkTopicType<T>(sub->second->GetTopic().GetTypeString(), field);
        T value = ReadLogImpl<T>(sub->second.get());
        if (value == defaultValue && !publishers.contains(field))
            WriteLog(field, defaultValue);
        loggingProfiler.Stop();
        return value;
    }
 
    static constexpr Loggable& GetRoot() { return rootLoggable; }
 
   protected:
    virtual void OnLoggingStart() {}

    virtual void LoggablePeriodic() {}
 
   private:
    template <typename T>
    using TopicType = decltype((std::declval<nt::NetworkTable>().*detail::nt_traits<T>::func)(""));

    static constexpr units::second_t RATE_LIMIT_TIME_TOLERANCE = 3_ms;

    void EnableLogging();

    template <typename T>
    inline void WriteLogImpl(nt::Publisher* pub, const T& data) {
        static_cast<typename TopicType<T>::PublisherType*>(pub)->Set(data);
    }
 
    template <typename T>
        requires units::traits::is_unit_t_v<T>
    inline void WriteLogImpl(nt::Publisher* pub, const T& data) {
        WriteLogImpl(pub, data.value());
    }
 
    template <typename T>
        requires detail::is_unit_vector<T>
    void WriteLogImpl(nt::Publisher* pub, const T& data) {
        std::vector<double> castedData;
        castedData.reserve(data.size());
        std::transform(data.begin(), data.end(), std::back_inserter(castedData), [](auto val) { return val.value(); });
        WriteLogImpl(pub, castedData);
    }
 
    template <typename T>
        requires std::is_enum_v<T>
    void WriteLogImpl(nt::Publisher* pub, const T& data) {
        WriteLogImpl(pub, magic_enum::enum_name(data));
    }
 
    template <typename T>
        requires std::is_enum_v<T>
    void WriteLogImpl(nt::Publisher* pub, const std::vector<T>& data) {
        std::vector<std::string> castedData;
        castedData.reserve(data.size());
        for (T val : data)
            castedData.emplace_back(magic_enum::enum_name(val));
        WriteLogImpl(pub, castedData);
    }

    template <typename T>
    inline T ReadLogImpl(nt::Subscriber* sub) {
        return static_cast<TopicType<T>::SubscriberType*>(sub)->Get();
    }
 
    template <typename T>
        requires units::traits::is_unit_t_v<T>
    inline T ReadLogImpl(nt::Subscriber* sub) {
        return T{ReadLogImpl<double>(sub)};
    }
 
    template <typename T>
        requires detail::is_unit_vector<T>
    T ReadLogImpl(nt::Subscriber* sub) {
        auto rawData = ReadLogImpl<std::vector<double>>(sub);
        T castedData;
        castedData.reserve(rawData.size());
        for (double data : rawData)
            castedData.emplace_back(data);
        return castedData;
    }
 
    template <typename T>
        requires std::is_enum_v<T>
    inline T ReadLogImpl(nt::Subscriber* sub) {
        return magic_enum::enum_cast<T>(ReadLogImpl<std::string_view>(sub)).value_or(T{});
    }

    template <typename T>
    inline decltype(publishers)::iterator addPublisher(std::string_view field) {
        return publishers
            .try_emplace(
                std::string{field},
                std::make_unique<typename TopicType<T>::PublisherType>((table.get()->*detail::nt_traits<T>::func)(field).Publish()), 0_s)
            .first;
    }
 
    template <typename T>
        requires units::traits::is_unit_t_v<T>
    decltype(publishers)::iterator addPublisher(std::string_view field) {
        std::string unitValue{detail::unit_string<T>};
        if (unitValue.empty())
            unitValue = T{}.abbreviation();
 
        return publishers
            .try_emplace(std::string{field},
                         std::make_unique<nt::DoublePublisher>(table->GetDoubleTopic(field).PublishEx("double", {{"unit", unitValue}})),
                         0_s)
            .first;
    }

    template <typename T>
    inline decltype(subscribers)::iterator addSubscriber(std::string_view field, const T& defaultValue) {
        return subscribers
            .emplace(field, std::make_unique<typename TopicType<T>::SubscriberType>(
                                (table.get()->*detail::nt_traits<T>::func)(field).Subscribe(defaultValue)))
            .first;
    }
 
    template <typename T>
        requires units::traits::is_unit_t_v<T>
    inline decltype(subscribers)::iterator addSubscriber(std::string_view field, const T& defaultValue) {
        return addSubscriber(field, defaultValue.value());
    }
 
    template <typename T>
        requires detail::is_unit_vector<T>
    decltype(subscribers)::iterator addSubscriber(std::string_view field, const T& defaultValue) {
        thread_local std::vector<double> defaultData;
        defaultData.clear();
        defaultData.reserve(defaultValue.size());
        std::transform(defaultValue.begin(), defaultValue.end(), std::back_inserter(defaultData), [](auto val) { return val.value(); });
        return addSubscriber(field, defaultData);
    }
 
    template <typename T>
        requires std::is_enum_v<T>
    decltype(subscribers)::iterator addSubscriber(std::string_view field, const T& defaultValue) {
        return addSubscriber(field, magic_enum::enum_name(defaultValue));
    }

    template <typename T>
    void checkTopicType(std::string_view topicType, std::string_view field) {
        if (topicType.empty())
            return;
        FRC_AssertMessage(topicType == detail::nt_traits<T>::type,
                          "Current type of '{}' doesn't match original topic type (received '{}', expected '{}')", field,
                          detail::nt_traits<T>::type, topicType);
    }
 
    std::shared_ptr<nt::NetworkTable> table;
    std::unordered_map<std::string, Loggable*> loggableChildren;
    std::unordered_map<std::string, wpi::Sendable*> sendableChildren;
    frc::Timer periodicProfiler;
    bool inPeriodic = false;
    LogLevel periodicLevel = LogLevel::Normal;
    units::second_t lastPeriodicTime = 0_s;
    static frc::Timer loggingProfiler;
    static Loggable rootLoggable;
};
 
template <>
inline decltype(Loggable::subscribers)::iterator Loggable::addSubscriber(std::string_view field, const std::vector<bool>& defaultValue) {
    return subscribers
        .emplace(field, std::make_unique<nt::BooleanArraySubscriber>(
                            table->GetBooleanArrayTopic(field).Subscribe(std::vector<int>{defaultValue.begin(), defaultValue.end()})))
        .first;
}
 
template <>
inline void Loggable::WriteLogImpl(nt::Publisher* pub, const std::vector<bool>& data) {
    static_cast<nt::BooleanArrayPublisher*>(pub)->Set(std::vector<int>{data.begin(), data.end()});
}
 
template <>
inline std::vector<bool> Loggable::ReadLogImpl(nt::Subscriber* sub) {
    auto data = static_cast<nt::BooleanArraySubscriber*>(sub)->Get();
    return {data.begin(), data.end()};
}
 
}  // namespace valor