Policies.h

Go to the documentation of this file.

#pragma once

 
#include <array>
#include <cstddef>
#include <functional>
#include <optional>
#include <tuple>
#include <type_traits>
#include <utility>
 
#include <EmrysFilter/Core/Forwarding.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Distributions/Gaussian.h>
#include <EmrysFilter/Distributions/Mixture.h>
#include <EmrysFilter/Distributions/Product.h>
#include <EmrysFilter/Spaces/Composite.h>
#include <EmrysFilter/Spaces/Coordinates.h>
#include <EmrysFilter/Spaces/Space.h>
#include <EmrysFilter/Spaces/Subspace.h>
#include <EmrysFilter/Spaces/TagPath.h>
 
namespace Emrys
{
 
namespace internal
{

template <typename Path, typename Name>
struct ReadItem
{
    using PathType = Path;  
    using NameType = Name;  

    static constexpr bool SingleTag = std::is_same_v<Path, TagPath<Name>>;
};

template <typename T>
struct ReadItemOf
{
    static constexpr bool valid = false;  
};

template <PathValue T>
struct ReadItemOf<T>
{
    static constexpr bool valid = true;                    
    using type = ReadItem<PathOf<T>, PathTag<PathOf<T>>>;  
};

template <PathValue X, typename Name>
struct ReadItemOf<OnPath<X, Name>>
{
    static constexpr bool valid = true;      
    using type = ReadItem<PathOf<X>, Name>;  
};

template <typename... Ts>
consteval bool
distinctTypes()
{
    if constexpr (sizeof...(Ts) < 2)
    {
        return true;
    }
    else
    {
        return []<typename First, typename... Rest>(std::type_identity<First>, std::type_identity<Rest>...) {
            return (!std::is_same_v<First, Rest> && ...) && distinctTypes<Rest...>();
        }(std::type_identity<Ts>{}...);
    }
}

template <typename T>
struct LibraryBaseOf
{
    using type = T;
};

template <IsComposite T>
struct LibraryBaseOf<T>
{
    template <typename... Blocks>
    static Composite<Blocks...> *
    of(std::tuple<Blocks...> *);
 
    using type = std::remove_pointer_t<decltype(of(static_cast<CompositeBlocks<T> *>(nullptr)))>;
};
 
template <IsRepeated T>
struct LibraryBaseOf<T>
{
    using type = Repeated<RepeatedBlock<T>, T::MaxCount>;
};
 
template <IsUniformLeaf T>
    requires(!IsPeriodic<T>)
struct LibraryBaseOf<T>
{
    using type = Linear<T::ValueDimensionStep == 0 ? T::FixedValueDimension : Eigen::Dynamic, T::MaxValueDimension>;
};
 
template <IsPeriodic T>
struct LibraryBaseOf<T>
{
    template <PeriodicInterval Interval, int Dimension, int MaxDimension>
    static Periodic<Interval, Dimension, MaxDimension> *
    of(Periodic<Interval, Dimension, MaxDimension> const *);
 
    using type = std::remove_pointer_t<decltype(of(static_cast<T const *>(nullptr)))>;
};


template <typename T>
using LibraryBase = typename LibraryBaseOf<T>::type;

template <typename Name, typename Block>
inline constexpr bool sameLeaf = std::derived_from<Name, Block> || std::derived_from<Block, Name>
                              || std::is_same_v<LibraryBase<Name>, LibraryBase<Block>>;
 
}  // namespace internal

template <typename... Items>
struct Reads
{
    static constexpr std::size_t Count = sizeof...(Items);  

    using InputSpace = Composite<typename Items::NameType...>;

    template <std::size_t I>
    using Item = std::tuple_element_t<I, std::tuple<Items...>>;
};

template <typename... Items>
[[nodiscard]] auto
reads(Items const &...)
{
    static_assert(
        (internal::ReadItemOf<std::remove_cvref_t<Items>>::valid && ...),
        "reads() takes paths, path<Tags...> or a tag's value, and on<Name>(path) for a path read under another "
        "name"
    );
    using Result = Reads<typename internal::ReadItemOf<std::remove_cvref_t<Items>>::type...>;
    static_assert(
        internal::distinctTypes<typename internal::ReadItemOf<std::remove_cvref_t<Items>>::type::NameType...>(),
        "two items of reads() share a name, their paths' last tag; read one under another name with on<Name>(path)"
    );
    return Result{};
}

template <typename... Paths>
struct Writes
{
    static constexpr std::size_t Count = sizeof...(Paths);  

    using OutputSpace = Composite<internal::PathTag<Paths>...>;

    using PathList = TagPaths<Paths...>;

    template <std::size_t I>
    using Path = std::tuple_element_t<I, std::tuple<Paths...>>;
};
 
namespace internal
{

template <typename Paths_, typename Out_>
struct WritesAs
{
    using PathList = Paths_;   
    using OutputSpace = Out_;  
};
 
}  // namespace internal

template <PathValue... Paths>
[[nodiscard]] auto
writes(Paths const &...)
{
    static_assert(sizeof...(Paths) > 0, "writes() names at least one block");
    static_assert(internal::distinctTypes<internal::PathOf<Paths>...>(), "writes() names each block once");
    return Writes<internal::PathOf<Paths>...>{};
}

template <typename... Names>
struct AffineIn
{
    static constexpr std::size_t Count = sizeof...(Names);  
};

template <PathValue... Names>
[[nodiscard]] auto
affineIn(Names const &...)
{
    static_assert(
        (std::is_same_v<internal::PathOf<Names>, TagPath<internal::PathTag<internal::PathOf<Names>>>> && ...),
        "affineIn() names an input of the model by the one tag it is read under in reads(), never by a path into the "
        "state"
    );
    static_assert(
        internal::distinctTypes<internal::PathTag<internal::PathOf<Names>>...>(),
        "affineIn() names each input once"
    );
    return AffineIn<internal::PathTag<internal::PathOf<Names>>...>{};
}
 
namespace internal
{

template <typename T>
inline constexpr bool isGaussianMixture = false;

template <typename Scalar, typename Space, typename Allocator>
inline constexpr bool isGaussianMixture<GaussianMixtureParams<Space, Scalar, Allocator>>{true};

template <typename T>
inline constexpr bool isNoiseCovariance = MatrixExpression<T>;

template <typename T>
inline constexpr bool placesCovariance = false;

template <typename X, typename Path>
inline constexpr bool placesCovariance<OnPath<X, Path>> = isNoiseCovariance<X>;
 
template <typename X, typename Path>
inline constexpr bool placesCovariance<OnEach<X, Path>> = isNoiseCovariance<X>;

template <typename T>
inline constexpr bool isPlacedNoise =
    isPlacedFactor<std::remove_cvref_t<T>> || placesCovariance<std::remove_cvref_t<T>>;

template <typename T>
inline constexpr bool placesMixture = false;

template <typename X, typename Path>
inline constexpr bool placesMixture<OnPath<X, Path>> = isGaussianMixture<std::remove_cvref_t<X>>;
 
template <typename X, typename Path>
inline constexpr bool placesMixture<OnEach<X, Path>> = isGaussianMixture<std::remove_cvref_t<X>>;
 
}  // namespace internal
 
template <typename Axis, typename Part>
class ModeNoise;

template <typename... Parts>
class AdditiveNoise
{
    static_assert(sizeof...(Parts) > 0, "additiveNoise() takes a distribution, or parts placed by tag path");
    static_assert(
        (internal::isPlacedNoise<Parts> && ...) || (sizeof...(Parts) == 1 && !(internal::isPlacedNoise<Parts> || ...)),
        "additiveNoise() with no placement covers the whole output space, so it takes one distribution alone; place "
        "each part with on<Path>() or onEach<Path>() to give several"
    );
    static_assert(
        (!internal::placesMixture<std::remove_cvref_t<Parts>> && ...),
        "a mixture is additive noise over the whole output space, unplaced; a placed part is one Gaussian"
    );
 
public:

    static constexpr bool Placed = (internal::isPlacedNoise<Parts> || ...);

    using Distribution = std::remove_cvref_t<std::tuple_element_t<0, std::tuple<Parts...>>>;

    static constexpr bool ZeroMean = !Placed && internal::isNoiseCovariance<Distribution>;

    static constexpr bool HasOwnSpace = !Placed && !ZeroMean;

    explicit AdditiveNoise(Parts... parts)
        : m_parts{std::forward<Parts>(parts)...}
    {}

    [[nodiscard]] std::tuple<Parts...> const &
    parts() const
    {
        return m_parts;
    }

    [[nodiscard]] Distribution const &
    distribution() const &
        requires(!Placed)
    {
        return std::get<0>(m_parts);
    }

    [[nodiscard]] std::tuple_element_t<0, std::tuple<Parts...>>
    distribution() &&
        requires(!Placed)
    {
        return std::get<0>(std::move(m_parts));
    }

    template <typename Axis>
    [[nodiscard]] auto
    byMode() const &
    {
        checkByMode();
        return ModeNoise<Axis, std::tuple_element_t<0, std::tuple<Parts...>>>{*this};
    }

    template <typename Axis>
    [[nodiscard]] auto
    byMode() &&
    {
        checkByMode();
        return ModeNoise<Axis, std::tuple_element_t<0, std::tuple<Parts...>>>{std::move(*this)};
    }
 
private:

    static constexpr void
    checkByMode()
    {
        static_assert(
            !Placed && internal::isGaussianMixture<Distribution>,
            "byMode() takes additive noise that is one mixture over the whole output, with one component per value "
            "of the mode axis"
        );
    }
 
    std::tuple<Parts...> m_parts;  
};

template <typename Axis, typename Part>
class ModeNoise : public AdditiveNoise<Part>
{
public:
 
    using ModeAxisType = Axis;  

    explicit ModeNoise(AdditiveNoise<Part> noise)
        : AdditiveNoise<Part>{std::move(noise)}
    {}
};

template <typename... Parts>
[[nodiscard]] AdditiveNoise<typename internal::HeldOf<std::unwrap_ref_decay_t<Parts>>::type...>
additiveNoise(Parts &&... parts)
{
    return AdditiveNoise<typename internal::HeldOf<std::unwrap_ref_decay_t<Parts>>::type...>{
        std::forward<Parts>(parts)...
    };
}

template <typename... Parts>
class ContinuousNoise
{
    static_assert(sizeof...(Parts) > 0, "continuousNoise() takes a spectral density, or parts placed by tag path");
    static_assert(
        (internal::isPlacedByPath<std::remove_cvref_t<Parts>> && ...)
            || (sizeof...(Parts) == 1 && !(internal::isPlacedByPath<std::remove_cvref_t<Parts>> || ...)),
        "continuousNoise() with no placement covers the whole output space, so it takes one spectral density alone; "
        "place each part with on<Path>() or onEach<Path>() to give several"
    );
 
public:

    static constexpr bool Placed = (internal::isPlacedByPath<std::remove_cvref_t<Parts>> || ...);

    explicit ContinuousNoise(Parts... parts)
        : m_parts{std::move(parts)...}
    {}

    [[nodiscard]] std::tuple<Parts...> const &
    parts() const
    {
        return m_parts;
    }
 
private:
 
    std::tuple<Parts...> m_parts;  
};

template <typename... Parts>
[[nodiscard]] ContinuousNoise<std::remove_cvref_t<Parts>...>
continuousNoise(Parts &&... parts)
{
    return ContinuousNoise<std::remove_cvref_t<Parts>...>{std::forward<Parts>(parts)...};
}
 
namespace internal
{

template <typename G>
struct DiscreteNoise
{
    G function;  
};

template <typename T>
inline constexpr bool isModel = requires { requires std::remove_cvref_t<T>::IsModel; };
 
}  // namespace internal

template <typename G>
struct LogLikelihood
{
    G function;  
};

template <typename G>
    requires(!MatrixExpression<G> && !Space<std::remove_cvref_t<G>>)
[[nodiscard]] LogLikelihood<std::remove_cvref_t<G>>
logLikelihood(G && function)
{
    return {std::forward<G>(function)};
}

template <typename G>
struct Sample
{
    G function;  
};

template <typename G>
    requires(!MatrixExpression<G> && !Space<std::remove_cvref_t<G>>)
[[nodiscard]] Sample<std::remove_cvref_t<G>>
sample(G && function)
{
    return {std::forward<G>(function)};
}

template <typename X>
struct OutputSpace
{
    static constexpr bool Computed = !Space<X>;
 
    X value;  

    template <typename StateSpace>
    [[nodiscard]] auto
    in([[maybe_unused]] StateSpace const & stateSpace) const
    {
        if constexpr (Computed)
        {
            static_assert(
                std::invocable<X const &, StateSpace const &>,
                "a model's outputSpace() function takes the state's space instance"
            );
            using Result = std::remove_cvref_t<std::invoke_result_t<X const &, StateSpace const &>>;
            static_assert(Space<Result>, "a model's outputSpace() function returns the output space instance");
            return value(stateSpace);
        }
        else
        {
            return value;
        }
    }
};

template <typename X>
    requires(!MatrixExpression<X>)
[[nodiscard]] OutputSpace<std::remove_cvref_t<X>>
outputSpace(X && x)
{
    return {std::forward<X>(x)};
}
 
namespace internal
{

enum class PolicyKind
{
    reads,
    writes,
    affineIn,
    additiveNoise,
    continuousNoise,
    logLikelihood,
    sample,
    outputSpace,
    discreteNoise  
};

template <typename T>
inline constexpr std::optional<PolicyKind> policyKind = std::nullopt;

template <typename... Items>
inline constexpr std::optional<PolicyKind> policyKind<Reads<Items...>>{PolicyKind::reads};
 
template <typename... Paths>
inline constexpr std::optional<PolicyKind> policyKind<Writes<Paths...>>{PolicyKind::writes};
 
template <typename Paths, typename Out>
inline constexpr std::optional<PolicyKind> policyKind<WritesAs<Paths, Out>>{PolicyKind::writes};
 
template <typename... Names>
inline constexpr std::optional<PolicyKind> policyKind<AffineIn<Names...>>{PolicyKind::affineIn};
 
template <typename... Parts>
inline constexpr std::optional<PolicyKind> policyKind<AdditiveNoise<Parts...>>{PolicyKind::additiveNoise};
 
template <typename Axis, typename Part>
inline constexpr std::optional<PolicyKind> policyKind<ModeNoise<Axis, Part>>{PolicyKind::additiveNoise};
 
template <typename... Parts>
inline constexpr std::optional<PolicyKind> policyKind<ContinuousNoise<Parts...>>{PolicyKind::continuousNoise};
 
template <typename G>
inline constexpr std::optional<PolicyKind> policyKind<LogLikelihood<G>>{PolicyKind::logLikelihood};
 
template <typename G>
inline constexpr std::optional<PolicyKind> policyKind<Sample<G>>{PolicyKind::sample};
 
template <typename X>
inline constexpr std::optional<PolicyKind> policyKind<OutputSpace<X>>{PolicyKind::outputSpace};
 
template <typename G>
inline constexpr std::optional<PolicyKind> policyKind<DiscreteNoise<G>>{PolicyKind::discreteNoise};

template <typename T>
concept ModelPolicy = policyKind<std::remove_cvref_t<T>>.has_value();

template <typename T>
inline constexpr bool isAdditiveNoise = policyKind<std::remove_cvref_t<T>> == PolicyKind::additiveNoise;

template <typename Noise>
struct NoiseModeAxisOf
{
    using type = void;
};

template <typename Axis, typename Part>
struct NoiseModeAxisOf<ModeNoise<Axis, Part>>
{
    using type = Axis;  
};


template <typename Noise>
using NoiseModeAxis = typename NoiseModeAxisOf<Noise>::type;

struct NoPolicy
{};

template <typename... Policies>
struct PolicySet
{
    static_assert(
        (ModelPolicy<Policies> && ...),
        "a model takes its function and then its policies, in any order: reads(), writes(), affineIn(), "
        "additiveNoise(), continuousNoise(), logLikelihood(), and outputSpace()"
    );

    static constexpr std::array<PolicyKind, sizeof...(Policies)> kinds{*policyKind<std::remove_cvref_t<Policies>>...};

    [[nodiscard]] static consteval std::size_t
    position(PolicyKind kind)
    {
        for (std::size_t i = 0; i < kinds.size(); ++i)
        {
            if (kinds[i] == kind)
            {
                return i;
            }
        }
        return kinds.size();
    }

    [[nodiscard]] static consteval bool
    distinct()
    {
        for (std::size_t i = 0; i < kinds.size(); ++i)
        {
            for (std::size_t j = i + 1; j < kinds.size(); ++j)
            {
                if (kinds[i] == kinds[j])
                {
                    return false;
                }
            }
        }
        return true;
    }
 
    static_assert(distinct(), "a model takes each policy at most once");

    template <PolicyKind K>
    static constexpr bool has = position(K) < sizeof...(Policies);

    template <PolicyKind K>
    using Type = std::conditional_t<
        has<K>,
        std::tuple_element_t<has<K> ? position(K) : 0, std::tuple<Policies..., NoPolicy>>,
        NoPolicy>;

    template <PolicyKind K, typename Tuple>
    [[nodiscard]] static auto const &
    get(Tuple const & policies)
    {
        static_assert(has<K>, "the model was not given this policy");
        return std::get<has<K> ? position(K) : 0>(policies);
    }
};

template <typename Space, typename Part>
[[nodiscard]] Part const &
noiseFactor(Space const &, Part const & part)
{
    return part;
}

template <typename X>
using ZeroMeanOf =
    Eigen::Matrix<typename X::Scalar, X::RowsAtCompileTime, 1, Eigen::ColMajor, X::MaxRowsAtCompileTime, 1>;

template <typename Space, MatrixExpression X, typename Path>
[[nodiscard]] auto
noiseFactor(Space const & space, OnPath<X, Path> const & part)
{
    auto block = pathSubspace<Path>(space);
    using Noise = GaussianParams<decltype(block), typename X::Scalar>;
    return OnPath<Noise, Path>{Noise{std::move(block), ZeroMeanOf<X>::Zero(part.value.rows()), part.value}};
}

template <typename Space, MatrixExpression X, typename Path>
[[nodiscard]] auto
noiseFactor(Space const &, OnEach<X, Path> const & part)
{
    using Block = typename EachBlockOf<Space, Path>::type;
    if constexpr (requires { typename Block::BlockType; })
    {
        using Noise = GaussianParams<typename Block::BlockType, typename X::Scalar>;
        return OnEach<Noise, Path>{Noise{ZeroMeanOf<X>::Zero(part.value.rows()), part.value}};
    }
    else
    {
        static_assert(
            std::is_void_v<X>,
            "onEach<Path>(R) in additiveNoise() places R on each repetition of a repeated block"
        );
    }
}

template <typename Space, typename... Parts>
[[nodiscard]] auto
placeNoise(Space const & space, AdditiveNoise<Parts...> const & noise)
{
    if constexpr (AdditiveNoise<Parts...>::ZeroMean)
    {
        auto const & cov = noise.distribution();
        using Cov = typename AdditiveNoise<Parts...>::Distribution;
        using Differences = DifferenceSpace<Space>;
        constexpr int Rows = valueDimensionAtCompileTime<Differences>;
        static_assert(
            Rows == Eigen::Dynamic || Cov::RowsAtCompileTime == Eigen::Dynamic || Cov::RowsAtCompileTime == Rows,
            "additiveNoise(R) is over the output's differences, so R has a row for each"
        );
        EMRYS_FILTER_ASSERT(
            cov.rows() == differenceDimension(space) && cov.cols() == cov.rows()
            && "additiveNoise(R) is over the output's differences, so R is square with a row for each"
        );
        using Noise = GaussianParams<Linear<Rows, maxValueDimensionAtCompileTime<Differences>>, typename Cov::Scalar>;
        return AdditiveNoise<Noise>{Noise{ZeroMeanOf<Cov>::Zero(cov.rows()), cov}};
    }
    else if constexpr (AdditiveNoise<Parts...>::Placed)
    {
        auto const product = std::apply(
            [&](auto const &... parts) {
                return Product{space, noiseFactor(space, parts)...};
            },
            noise.parts()
        );
        using Scalar = typename decltype(product)::Scalar;
        using Noise =
            GaussianParams<Linear<valueDimensionAtCompileTime<Space>, maxValueDimensionAtCompileTime<Space>>, Scalar>;
        auto const moments = product.moments();
        if constexpr (isExpected<std::remove_cvref_t<decltype(moments)>>)
        {
            EMRYS_FILTER_ASSERT(moments && "the noise's parts must have moments");
            return AdditiveNoise<Noise>{Noise{moments->mean(), moments->covarianceLower()}};
        }
        else
        {
            return AdditiveNoise<Noise>{Noise{moments.mean(), moments.covarianceLower()}};
        }
    }
    else
    {
        return AdditiveNoise<typename AdditiveNoise<Parts...>::Distribution const &>{noise.distribution()};
    }
}

template <typename R, typename Space>
[[nodiscard]] consteval bool
checkReadNames()
{
    return [&]<std::size_t... I>(std::index_sequence<I...>) {
        auto const check = []<typename Item>(std::type_identity<Item>) {
            using Path = typename Item::PathType;
            using Name = typename Item::NameType;
            constexpr bool inState = [] {
                return []<typename First, typename... Rest>(TagPath<First, Rest...> *) {
                    return countTag<First, Space>() > 0;
                }(static_cast<Path *>(nullptr));
            }();
            if constexpr (inState)
            {
                static_assert(
                    sameLeaf<Name, PathTag<Path>>,
                    "a name given by on<Name>() in reads() must have the same leaf space beneath it as the block it "
                    "names, such as Linear<2> for a Position : Linear<2>"
                );
            }
            return true;
        };
        return (check(std::type_identity<typename R::template Item<I>>{}) && ...);
    }(std::make_index_sequence<R::Count>{});
}
 
}  // namespace internal

template <Space S, typename Vector>
struct Sure
{
    using SpaceType = S;
    using VectorType = Vector;
 
    EMRYS_FILTER_NO_UNIQUE_ADDRESS S space;  
    Vector value;                            
};

template <FixedSizeSpace S, MatrixExpression V>
    requires Space<S>
[[nodiscard]] auto
sure(V && value)
{
    using Plain = typename internal::CrtpDerived<V>::PlainObject;
    static_assert(!internal::IsUntagged<S>, "a sure input is named by a tag, a type derived from a space");
    static_assert(Plain::ColsAtCompileTime == 1, "a sure input is a column vector");
    static_assert(
        Plain::RowsAtCompileTime == Eigen::Dynamic || Plain::RowsAtCompileTime == valueDimensionAtCompileTime<S>,
        "a sure input's rows do not match its space's dimension at compile time"
    );
    EMRYS_FILTER_ASSERT(value.rows() == valueDimension(S{}) && "a sure input must be a value in its space");
    return Sure<S, Plain>{S{}, Plain{std::forward<V>(value)}};
}

template <Space S, MatrixExpression V>
[[nodiscard]] auto
sure(S space, V && value)
{
    using Plain = typename internal::CrtpDerived<V>::PlainObject;
    static_assert(!internal::IsUntagged<S>, "a sure input is named by a tag, a type derived from a space");
    static_assert(Plain::ColsAtCompileTime == 1, "a sure input is a column vector");
    EMRYS_FILTER_ASSERT(value.rows() == valueDimension(space) && "a sure input must be a value in its space");
    return Sure<S, Plain>{std::move(space), Plain{std::forward<V>(value)}};
}
 
namespace internal
{

template <typename T>
inline constexpr bool isSure = false;

template <typename S, typename Vector>
inline constexpr bool isSure<Sure<S, Vector>>{true};
 
}  // namespace internal

template <typename... Paths>
struct Consider
{
    static constexpr std::size_t Count = sizeof...(Paths);  
};

template <PathValue... Paths>
[[nodiscard]] constexpr Consider<internal::PathOf<Paths>...>
consider(Paths const &...)
{
    static_assert(sizeof...(Paths) > 0, "consider() names at least one block of the state");
    static_assert(internal::distinctTypes<internal::PathOf<Paths>...>(), "consider() names each block once");
    return {};
}
 
namespace internal
{

template <typename T>
inline constexpr bool isConsider = false;

template <typename... Paths>
inline constexpr bool isConsider<Consider<Paths...>>{true};
 
}  // namespace internal
 
}  // namespace Emrys