Common.h

#pragma once
 
#include <concepts>
#include <cstddef>
#include <string>
#include <tuple>
#include <type_traits>
#include <utility>
 
#include <Eigen/Dense>
#include <fmt/format.h>
 
#include "units/base.h"
 
using namespace units::literals;
 
namespace valor {
namespace EigenUnit {
 
template <typename... UnitTags>
class UnitVector;
 
template <typename OutputTagsTuple, typename InputTagsTuple>
class UnitMatrix;

template <std::size_t Start, std::size_t Length, typename Tuple>
struct SliceTuple {
   private:
    template <typename Indices>
    struct impl;
 
    template <std::size_t... Is>
    struct impl<std::index_sequence<Is...>> {
        using type = std::tuple<std::tuple_element_t<Start + Is, Tuple>...>;
    };
 
   public:
    using type = typename impl<std::make_index_sequence<Length>>::type;
};

template <typename Expected, typename Received>
struct ERROR_UNITS_DO_NOT_MATCH {
    static constexpr bool value = false;
};

template <typename T>
struct ERROR_UNITS_DO_NOT_MATCH<T, T> {
    static constexpr bool value = true;
};

template <typename U>
struct NormalizeUnit {
    using type = U;
};
 
template <typename U>
    requires units::traits::is_dimensionless_unit<units::unit_t<U>>::value
struct NormalizeUnit<U> {
    using type = units::dimensionless::scalar;
};
 
template <typename U>
using Normalize = typename NormalizeUnit<U>::type;
 
template <typename Tuple>
struct NormalizeTuple;
 
template <typename... Ts>
struct NormalizeTuple<std::tuple<Ts...>> {
    using type = std::tuple<Normalize<Ts>...>;
};

template <typename U1, typename U2>
using MultiplyResult = Normalize<typename units::unit_t<units::compound_unit<U1, U2>>::unit_type>;

template <typename U1, typename U2>
using DivideResult = Normalize<typename units::unit_t<units::compound_unit<U1, units::inverse<U2>>>::unit_type>;

namespace detail {
template <std::size_t N, typename T>
using substitution_helper = T;

template <typename T, typename Indices>
struct VectorN_Builder;

template <typename T, std::size_t... Is>
struct VectorN_Builder<T, std::index_sequence<Is...>> {
    using type = UnitVector<substitution_helper<Is, T>...>;
};

struct UnitFormatSpec {
    int precision = -1;
    bool values_only = false;

    template <typename ParseContext>
    constexpr auto parse(ParseContext& ctx) {
        auto it = ctx.begin();
        if (it == ctx.end() || *it == '}')
            return it;
 
        if (*it == 'v') {
            values_only = true;
            ++it;
        }
 
        if (it != ctx.end() && *it == '.') {
            ++it;
            precision = 0;
            while (it != ctx.end() && *it >= '0' && *it <= '9')
                precision = precision * 10 + (*it++ - '0');
        }
 
        if (it != ctx.end() && *it != '}')
            throw fmt::format_error("UnitVector: unexpected format character");
 
        return it;
    }

    std::string format_value(double v) const {
        if (precision >= 0)
            return fmt::vformat("{:.{}f}", fmt::make_format_args(v, precision));
        return fmt::format("{}", v);
    }
};
}  // namespace detail

template <size_t N, typename... Ts>
concept ValidIndex = (N < sizeof...(Ts));

template <size_t N, typename... Ts>
concept ValidSize = (N <= sizeof...(Ts));

template <typename... Ts>
concept AllSameUnits = sizeof...(Ts) > 0 && (std::is_same_v<std::tuple_element_t<0, std::tuple<Ts...>>, Ts> && ...);

template <typename... Ts>
concept ThreeDimensionalVector = sizeof...(Ts) == 3;

template <typename T1, typename T2>
concept UnitEquality = ERROR_UNITS_DO_NOT_MATCH<T1, T2>::value;

template <typename T>
concept HomogeneousTuple = []<typename... Ts>(std::tuple<Ts...>) { return AllSameUnits<Ts...>; }(T{});

template <typename M>
concept SquareMatrix = requires {
    typename M::OutTuple;
    typename M::InTuple;
} && (std::tuple_size_v<typename M::OutTuple> == std::tuple_size_v<typename M::InTuple>);

template <typename M>
concept MatchingSquareMatrix = requires {
    typename M::OutTuple;
    typename M::InTuple;
} && std::is_same_v<typename M::OutTuple, typename M::InTuple>;

template <typename M>
concept HomogeneousMatrix = HomogeneousTuple<typename M::InTuple> && HomogeneousTuple<typename M::OutTuple>;

template <typename TL, typename TR, typename BL, typename BR, std::size_t TotalRows, std::size_t TotalCols>
concept ValidBlockLayout =
    requires {
        { TL::RowCount } -> std::convertible_to<std::size_t>;
        { TL::ColCount } -> std::convertible_to<std::size_t>;
        { TR::RowCount } -> std::convertible_to<std::size_t>;
        { TR::ColCount } -> std::convertible_to<std::size_t>;
        { BL::RowCount } -> std::convertible_to<std::size_t>;
        { BL::ColCount } -> std::convertible_to<std::size_t>;
        { BR::RowCount } -> std::convertible_to<std::size_t>;
        { BR::ColCount } -> std::convertible_to<std::size_t>;
    } && (TL::RowCount + BL::RowCount == TotalRows) && (TR::RowCount + BR::RowCount == TotalRows) &&
    (TL::ColCount + TR::ColCount == TotalCols) && (BL::ColCount + BR::ColCount == TotalCols);

template <typename Vec, typename ExpectedTuple>
concept MatchingUnitTuple = requires { typename Vec::UnitTuple; } && std::is_same_v<typename Vec::UnitTuple, ExpectedTuple>;

template <typename OutTuple, typename... ColVecs>
concept ColVecsMatchOutput = (MatchingUnitTuple<ColVecs, OutTuple> && ...);

template <typename InTuple, typename... RowVecs>
concept RowVecsMatchInput = (MatchingUnitTuple<RowVecs, InTuple> && ...);

template <std::size_t I, typename OutTuple, typename InTuple>
using MatrixElemUnit = DivideResult<std::tuple_element_t<I / std::tuple_size_v<InTuple>, OutTuple>,
                                    std::tuple_element_t<I % std::tuple_size_v<InTuple>, InTuple>>;

template <std::size_t I, typename OutTuple, typename InTuple, typename Arg>
inline constexpr bool ValidElemArg = std::is_same_v<std::decay_t<Arg>, units::unit_t<MatrixElemUnit<I, OutTuple, InTuple>>>;

template <typename OutTuple, typename InTuple, typename ArgsTuple, typename IS>
struct ValidElemArgsImpl;

template <typename OutTuple, typename InTuple, typename ArgsTuple, std::size_t... Is>
struct ValidElemArgsImpl<OutTuple, InTuple, ArgsTuple, std::index_sequence<Is...>> {
    static constexpr bool value = (ValidElemArg<Is, OutTuple, InTuple, std::tuple_element_t<Is, ArgsTuple>> && ...);
};

template <typename OutTuple, typename InTuple, typename... Args>
concept ValidElemArgs = (sizeof...(Args) == std::tuple_size_v<OutTuple> * std::tuple_size_v<InTuple>) &&
                        ValidElemArgsImpl<OutTuple, InTuple, std::tuple<Args...>, std::make_index_sequence<sizeof...(Args)>>::value;
 
template <std::size_t N, typename IS = std::make_index_sequence<N>>
struct ScalarTuple_Builder;
 
template <std::size_t N, std::size_t... Is>
struct ScalarTuple_Builder<N, std::index_sequence<Is...>> {
    using type = std::tuple<detail::substitution_helper<Is, units::dimensionless::scalar>...>;
};
 
template <typename OutTuple, typename InTuple>
struct NormalizeBlockTuples {
    using NormalizedOut = OutTuple;
    using NormalizedIn = InTuple;
};
 
template <typename... Ts>
struct NormalizeBlockTuples<std::tuple<Ts...>, std::tuple<Ts...>> {
    using NormalizedOut = typename ScalarTuple_Builder<sizeof...(Ts)>::type;
    using NormalizedIn = typename ScalarTuple_Builder<sizeof...(Ts)>::type;
};
 
}  // namespace EigenUnit
}  // namespace valor