UnitMap.h

#pragma once
 
#include "valkyrie/util/eigenunit/UnitVector.h"
 
namespace valor {
namespace EigenUnit {

template <typename... UnitTags>
class UnitMap {
   public:
    static constexpr std::size_t Size = sizeof...(UnitTags);
    using StorageType = Eigen::Matrix<double, Size, 1>;
    using MapType = Eigen::Map<StorageType>;
    using ConstMapType = Eigen::Map<const StorageType>;
    using UnitTuple = std::tuple<UnitTags...>;
 
    // -----------------------------------------------------------------------
    // Construction
    // -----------------------------------------------------------------------

    explicit UnitMap(double* ptr) : map(ptr) {}
 
    // Non-copyable: copying a map would alias the same buffer under two
    // objects, which is confusing. Use UnitVector for owned copies.
    UnitMap(const UnitMap&) = delete;
    UnitMap& operator=(const UnitMap&) = delete;

    UnitMap(UnitMap&&) = default;
    UnitMap& operator=(UnitMap&&) = default;
 
    // -----------------------------------------------------------------------
    // Read access
    // -----------------------------------------------------------------------

    template <std::size_t N>
        requires ValidIndex<N, UnitTags...>
    constexpr auto Get() const {
        using T = std::tuple_element_t<N, UnitTuple>;
        return units::unit_t<T>(map[N]);
    }

    ConstMapType Raw() const { return ConstMapType(map.data()); }

    MapType Raw() { return map; }
 
    // -----------------------------------------------------------------------
    // Write access
    // -----------------------------------------------------------------------

    template <std::size_t N>
        requires ValidIndex<N, UnitTags...>
    void Set(units::unit_t<std::tuple_element_t<N, UnitTuple>> val) {
        map[N] = val.value();
    }

    template <typename... OtherTags>
    UnitMap& operator=(const UnitVector<OtherTags...>& vec)
        requires UnitEquality<UnitTuple, std::tuple<OtherTags...>>
    {
        map = vec.Raw();
        return *this;
    }
 
    // -----------------------------------------------------------------------
    // Conversion
    // -----------------------------------------------------------------------

    UnitVector<UnitTags...> ToVector() const { return UnitVector<UnitTags...>(StorageType(map)); }

    operator UnitVector<UnitTags...>() const { return ToVector(); }
 
    // -----------------------------------------------------------------------
    // Named accessors (mirrors UnitVector)
    // -----------------------------------------------------------------------

    constexpr auto X() const
        requires(Size >= 1)
    {
        return this->Get<0>();
    }

    void X(units::unit_t<std::tuple_element_t<Size >= 1 ? 0 : 0, UnitTuple>> val)
        requires(Size >= 1)
    {
        this->Set<0>(val);
    }

    constexpr auto Y() const
        requires(Size >= 2)
    {
        return this->Get<1>();
    }

    void Y(units::unit_t<std::tuple_element_t<Size >= 2 ? 1 : 0, UnitTuple>> val)
        requires(Size >= 2)
    {
        this->Set<1>(val);
    }

    constexpr auto Z() const
        requires(Size >= 3)
    {
        return this->Get<2>();
    }

    void Z(units::unit_t<std::tuple_element_t<Size >= 3 ? 2 : 0, UnitTuple>> val)
        requires(Size >= 3)
    {
        this->Set<2>(val);
    }

    constexpr auto W() const
        requires(Size >= 4)
    {
        return this->Get<3>();
    }

    void W(units::unit_t<std::tuple_element_t<Size >= 4 ? 3 : 0, UnitTuple>> val)
        requires(Size >= 4)
    {
        this->Set<3>(val);
    }
 
    // -----------------------------------------------------------------------
    // Debug
    // -----------------------------------------------------------------------

    friend std::ostream& operator<<(std::ostream& os, const UnitMap& m) {
        os << "[";
        m.print_impl(os, std::make_index_sequence<Size>{});
        os << "]";
        return os;
    }
 
   private:
    template <std::size_t... Is>
    void print_impl(std::ostream& os, std::index_sequence<Is...>) const {
        ((os << units::unit_t<std::tuple_element_t<Is, UnitTuple>>(map[Is]).value() << (Is == Size - 1 ? "" : ", ")), ...);
    }
 
    MapType map;
};

template <typename... UnitTags>
class ConstUnitMap {
   public:
    static constexpr std::size_t Size = sizeof...(UnitTags);
    using StorageType = Eigen::Matrix<double, Size, 1>;
    using ConstMapType = Eigen::Map<const StorageType>;
    using UnitTuple = std::tuple<UnitTags...>;

    explicit ConstUnitMap(const double* ptr) : map(ptr) {}

    explicit ConstUnitMap(const UnitMap<UnitTags...>& m) : map(m.Raw().data()) {}
 
    ConstUnitMap(const ConstUnitMap&) = delete;
    ConstUnitMap& operator=(const ConstUnitMap&) = delete;
    ConstUnitMap(ConstUnitMap&&) = default;
    ConstUnitMap& operator=(ConstUnitMap&&) = default;

    template <std::size_t N>
        requires ValidIndex<N, UnitTags...>
    constexpr auto Get() const {
        using T = std::tuple_element_t<N, UnitTuple>;
        return units::unit_t<T>(map[N]);
    }

    ConstMapType Raw() const { return map; }

    UnitVector<UnitTags...> ToVector() const { return UnitVector<UnitTags...>(StorageType(map)); }

    operator UnitVector<UnitTags...>() const { return ToVector(); }

    constexpr auto X() const
        requires(Size >= 1)
    {
        return this->Get<0>();
    }

    constexpr auto Y() const
        requires(Size >= 2)
    {
        return this->Get<1>();
    }

    constexpr auto Z() const
        requires(Size >= 3)
    {
        return this->Get<2>();
    }

    constexpr auto W() const
        requires(Size >= 4)
    {
        return this->Get<3>();
    }

    friend std::ostream& operator<<(std::ostream& os, const ConstUnitMap& m) {
        os << "[";
        m.print_impl(os, std::make_index_sequence<Size>{});
        os << "]";
        return os;
    }
 
   private:
    template <std::size_t... Is>
    void print_impl(std::ostream& os, std::index_sequence<Is...>) const {
        ((os << units::unit_t<std::tuple_element_t<Is, UnitTuple>>(map[Is]).value() << (Is == Size - 1 ? "" : ", ")), ...);
    }
 
    ConstMapType map;
};
 
}  // namespace EigenUnit
}  // namespace valor