<commit_before>/*
Copyright (C) 2016 Thiemar Pty Ltd

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#ifndef STATEVECTOR_H
#define STATEVECTOR_H

#include <limits>
#include <tuple>
#include <cstddef>
#include <utility>
#include <Eigen/Core>
#include "UKF/Types.h"
#include "UKF/Integrator.h"

namespace UKF {

    namespace Detail {

    /*
    This variable template defines the dimensionality of a particular
    StateVector field. For a normal Eigen column vector, the default
    implementation returns the number of rows at compile time.

    Template specialisations can be used to define the size of fields which
    don't have a MaxRowsAtCompileTime member, or need a custom size for some
    reason (eg. the Eigen Quaternion classes).
    */
    template <typename T>
    constexpr std::size_t StateVectorDimension = T::MaxRowsAtCompileTime;

    template <>
    constexpr std::size_t StateVectorDimension<Quaternion> = 4;

    template <>
    constexpr std::size_t StateVectorDimension<real_t> = 1;

    /*
    This variable template defines the dimensionality of a particular
    StateVector field as it applies to the covariance matrix. For most types,
    this is just the same as the StateVectorDimension.

    An example of a type which needs a special case is a quaternion – it
    needs more special treatment than a simple additive update, so its
    representation in the covariance matrix is different to its
    representation in the state vector.
    */
    template <typename T>
    constexpr std::size_t CovarianceDimension = StateVectorDimension<T>;

    template <>
    constexpr std::size_t CovarianceDimension<Quaternion> = 3;

    template <typename T>
    constexpr T adder(T v) {
        return v;
    }

    template <typename T, typename... Args>
    constexpr T adder(T first, Args... args) {
        return first + adder(args...);
    }

    /*
    Get the dimension of the state vector by summing the dimension of all
    fields.
    */
    template <typename... Fields>
    constexpr std::size_t get_composite_vector_dimension() {
        return adder(StateVectorDimension<Fields>...);
    }

    /*
    Get the dimension of the covariance matrix by summing the dimension of
    all fields.
    */
    template <typename... Fields>
    constexpr std::size_t get_covariance_dimension() {
        return adder(CovarianceDimension<Fields>...);
    }

    /*
    Get the offset of a particular field in a parameter pack of Field objects
    by matching the provided key parameter.
    */
    template <std::size_t Offset, typename T>
    constexpr std::size_t get_field_offset(int Key) {
        return Key == T::key ? Offset : std::numeric_limits<std::size_t>::max();
    }

    template <std::size_t Offset, typename T1, typename T2, typename... Fields>
    constexpr std::size_t get_field_offset(int Key) {
        return Key == T1::key ? Offset : get_field_offset<
            Offset + StateVectorDimension<typename T1::type>, T2, Fields...>(Key);
    }

    /* Do the same as above, but for the covariance matrix. */
    template <std::size_t Offset, typename T>
    constexpr std::size_t get_field_covariance_offset(int Key) {
        return Key == T::key ? Offset : std::numeric_limits<std::size_t>::max();
    }

    template <std::size_t Offset, typename T1, typename T2, typename... Fields>
    constexpr std::size_t get_field_covariance_offset(int Key) {
        return Key == T1::key ? Offset : get_field_covariance_offset<
            Offset + CovarianceDimension<typename T1::type>, T2, Fields...>(Key);
    }

    /* Get the order of the specified key within the field pack. */
    template <std::size_t Offset, typename T>
    constexpr std::size_t get_field_order(int Key) {
        return Key == T::key ? Offset : std::numeric_limits<std::size_t>::max();
    }

    template <std::size_t Offset, typename T1, typename T2, typename... Fields>
    constexpr std::size_t get_field_order(int Key) {
        return Key == T1::key ? Offset : get_field_order<Offset + 1, T2, Fields...>(Key);
    }

    /*
    These helper structs allow various functions to determine field types at
    compile time based only on the integer key parameter.
    */
    template <bool condition, class T, class U>
    struct IfHelper {
      using type = U;
    };
     
    template <class T, class U>
    struct IfHelper<true, T, U> {
      using type = T;
    };

    template <int Key, typename... Fields>
    struct FieldTypes {
        using type = void;
    };

    template <int Key, typename T, typename... Fields>
    struct FieldTypes<Key, T, Fields...> {
        using type = typename IfHelper<
            Key == T::key,
            typename T::type,
            typename FieldTypes<Key, Fields...>::type>::type;
    };

    template <typename T>
    constexpr T convert_from_segment(const Vector<StateVectorDimension<T>>& state) {
        return static_cast<T>(state);
    }

    template <>
    constexpr real_t convert_from_segment<real_t>(const Vector<1>& state) {
        return static_cast<real_t>(state(0));
    }

    /*
    Get the size of a particular field in a parameter pack of Field objects
    by matching the provided key parameter.
    */
    template <typename T>
    constexpr std::size_t get_field_size(int Key) {
        return Key == T::key ? StateVectorDimension<typename T::type> : std::numeric_limits<std::size_t>::max();
    }

    template <typename T1, typename T2, typename... Fields>
    constexpr std::size_t get_field_size(int Key) {
        return Key == T1::key ? StateVectorDimension<typename T1::type> : get_field_size<T2, Fields...>(Key);
    }

    /* Do the same as above, but for the covariance matrix. */
    template <typename T>
    constexpr std::size_t get_field_covariance_size(int Key) {
        return Key == T::key ? CovarianceDimension<typename T::type> : std::numeric_limits<std::size_t>::max();
    }

    template <typename T1, typename T2, typename... Fields>
    constexpr std::size_t get_field_covariance_size(int Key) {<commit_msg>Make convert_from_segment non-constexpr<commit_after>/*
Copyright (C) 2016 Thiemar Pty Ltd

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#ifndef STATEVECTOR_H
#define STATEVECTOR_H

#include <limits>
#include <tuple>
#include <cstddef>
#include <utility>
#include <Eigen/Core>
#include "UKF/Types.h"
#include "UKF/Integrator.h"

namespace UKF {

    namespace Detail {

    /*
    This variable template defines the dimensionality of a particular
    StateVector field. For a normal Eigen column vector, the default
    implementation returns the number of rows at compile time.

    Template specialisations can be used to define the size of fields which
    don't have a MaxRowsAtCompileTime member, or need a custom size for some
    reason (eg. the Eigen Quaternion classes).
    */
    template <typename T>
    constexpr std::size_t StateVectorDimension = T::MaxRowsAtCompileTime;

    template <>
    constexpr std::size_t StateVectorDimension<Quaternion> = 4;

    template <>
    constexpr std::size_t StateVectorDimension<real_t> = 1;

    /*
    This variable template defines the dimensionality of a particular
    StateVector field as it applies to the covariance matrix. For most types,
    this is just the same as the StateVectorDimension.

    An example of a type which needs a special case is a quaternion – it
    needs more special treatment than a simple additive update, so its
    representation in the covariance matrix is different to its
    representation in the state vector.
    */
    template <typename T>
    constexpr std::size_t CovarianceDimension = StateVectorDimension<T>;

    template <>
    constexpr std::size_t CovarianceDimension<Quaternion> = 3;

    template <typename T>
    constexpr T adder(T v) {
        return v;
    }

    template <typename T, typename... Args>
    constexpr T adder(T first, Args... args) {
        return first + adder(args...);
    }

    /*
    Get the dimension of the state vector by summing the dimension of all
    fields.
    */
    template <typename... Fields>
    constexpr std::size_t get_composite_vector_dimension() {
        return adder(StateVectorDimension<Fields>...);
    }

    /*
    Get the dimension of the covariance matrix by summing the dimension of
    all fields.
    */
    template <typename... Fields>
    constexpr std::size_t get_covariance_dimension() {
        return adder(CovarianceDimension<Fields>...);
    }

    /*
    Get the offset of a particular field in a parameter pack of Field objects
    by matching the provided key parameter.
    */
    template <std::size_t Offset, typename T>
    constexpr std::size_t get_field_offset(int Key) {
        return Key == T::key ? Offset : std::numeric_limits<std::size_t>::max();
    }

    template <std::size_t Offset, typename T1, typename T2, typename... Fields>
    constexpr std::size_t get_field_offset(int Key) {
        return Key == T1::key ? Offset : get_field_offset<
            Offset + StateVectorDimension<typename T1::type>, T2, Fields...>(Key);
    }

    /* Do the same as above, but for the covariance matrix. */
    template <std::size_t Offset, typename T>
    constexpr std::size_t get_field_covariance_offset(int Key) {
        return Key == T::key ? Offset : std::numeric_limits<std::size_t>::max();
    }

    template <std::size_t Offset, typename T1, typename T2, typename... Fields>
    constexpr std::size_t get_field_covariance_offset(int Key) {
        return Key == T1::key ? Offset : get_field_covariance_offset<
            Offset + CovarianceDimension<typename T1::type>, T2, Fields...>(Key);
    }

    /* Get the order of the specified key within the field pack. */
    template <std::size_t Offset, typename T>
    constexpr std::size_t get_field_order(int Key) {
        return Key == T::key ? Offset : std::numeric_limits<std::size_t>::max();
    }

    template <std::size_t Offset, typename T1, typename T2, typename... Fields>
    constexpr std::size_t get_field_order(int Key) {
        return Key == T1::key ? Offset : get_field_order<Offset + 1, T2, Fields...>(Key);
    }

    /*
    These helper structs allow various functions to determine field types at
    compile time based only on the integer key parameter.
    */
    template <bool condition, class T, class U>
    struct IfHelper {
      using type = U;
    };
     
    template <class T, class U>
    struct IfHelper<true, T, U> {
      using type = T;
    };

    template <int Key, typename... Fields>
    struct FieldTypes {
        using type = void;
    };

    template <int Key, typename T, typename... Fields>
    struct FieldTypes<Key, T, Fields...> {
        using type = typename IfHelper<
            Key == T::key,
            typename T::type,
            typename FieldTypes<Key, Fields...>::type>::type;
    };

    template <typename T>
    T convert_from_segment(const Vector<StateVectorDimension<T>>& state) {
        return static_cast<T>(state);
    }

    template <>
    real_t convert_from_segment<real_t>(const Vector<1>& state) {
        return static_cast<real_t>(state(0));
    }

    /*
    Get the size of a particular field in a parameter pack of Field objects
    by matching the provided key parameter.
    */
    template <typename T>
    constexpr std::size_t get_field_size(int Key) {
        return Key == T::key ? StateVectorDimension<typename T::type> : std::numeric_limits<std::size_t>::max();
    }

    template <typename T1, typename T2, typename... Fields>
    constexpr std::size_t get_field_size(int Key) {
        return Key == T1::key ? StateVectorDimension<typename T1::type> : get_field_size<T2, Fields...>(Key);
    }

    /* Do the same as above, but for the covariance matrix. */
    template <typename T>
    constexpr std::size_t get_field_covariance_size(int Key) {
        return Key == T::key ? CovarianceDimension<typename T::type> : std::numeric_limits<std::size_t>::max();
    }

    template <typename T1, typename T2, typename... Fields>
    constexpr std::size_t get_field_covariance_size(int Key) {
{'fraction_non_alphanumeric': 0.09151212553495007, 'fraction_numerical': 0.00442225392296719, 'mean_word_length': 3.266889835666464, 'pattern_counts': {'":': 0, '<': 123, '<?xml version=': 0, '>': 123, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3679114', 'n_tokens_mistral': 4150, 'n_tokens_neox': 3907, 'n_words': 1486}