core.hpp

// Copyright (c) 2020 INRA Distributed under the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef ORG_VLEPROJECT_IRRITATOR_2020
#define ORG_VLEPROJECT_IRRITATOR_2020

#include <algorithm>
#include <limits>

#ifdef __has_include
#if __has_include(<numbers>)
#include <numbers>
#define irt_have_numbers 1
#else
#define irt_have_numbers 0
#endif
#endif

#include <string_view>
#include <type_traits>

#include <cmath>
#include <cstdint>
#include <cstring>

/*****************************************************************************
 *
 * Helper macros
 *
 ****************************************************************************/

#ifndef irt_assert
#include <cassert>
#define irt_assert(_expr) assert(_expr)
#endif

namespace irt {

static inline bool is_fatal_breakpoint = true;

}

#ifndef NDEBUG
#if (defined(__i386__) || defined(__x86_64__)) && defined(__GNUC__) &&         \
  __GNUC__ >= 2
#define irt_breakpoint()                                                       \
    do {                                                                       \
        if (::irt::is_fatal_breakpoint)                                        \
            __asm__ __volatile__("int $03");                                   \
    } while (0)
#elif (defined(_MSC_VER) || defined(__DMC__)) && defined(_M_IX86)
#define irt_breakpoint()                                                       \
    do {                                                                       \
        if (::irt::is_fatal_breakpoint)                                        \
            __asm int 3h                                                       \
    } while (0)
#elif defined(_MSC_VER)
#define irt_breakpoint()                                                       \
    do {                                                                       \
        if (::irt::is_fatal_breakpoint)                                        \
            __debugbreak();                                                    \
    } while (0)
#elif defined(__alpha__) && !defined(__osf__) && defined(__GNUC__) &&          \
  __GNUC__ >= 2
#define irt_breakpoint()                                                       \
    do {                                                                       \
        if (::irt::is_fatal_breakpoint)                                        \
            __asm__ __volatile__("bpt");                                       \
    } while (0)
#elif defined(__APPLE__)
#define irt_breakpoint()                                                       \
    do {                                                                       \
        if (::irt::is_fatal_breakpoint)                                        \
            __builtin_trap();                                                  \
    } while (0)
#else /* !__i386__ && !__alpha__ */
#define irt_breakpoint()                                                       \
    do {                                                                       \
        if (::irt::is_fatal_breakpoint)                                        \
            raise(SIGTRAP);                                                    \
    } while (0)
#endif /* __i386__ */
#else
#define irt_breakpoint()                                                       \
    do {                                                                       \
    } while (0)
#endif

#define irt_bad_return(status__)                                               \
    do {                                                                       \
        irt_breakpoint();                                                      \
        return status__;                                                       \
    } while (0)

#define irt_return_if_bad(expr__)                                              \
    do {                                                                       \
        auto status__ = (expr__);                                              \
        if (status__ != status::success) {                                     \
            irt_breakpoint();                                                  \
            return status__;                                                   \
        }                                                                      \
    } while (0)

#define irt_return_if_fail(expr__, status__)                                   \
    do {                                                                       \
        if (!(expr__)) {                                                       \
            irt_breakpoint();                                                  \
            return status__;                                                   \
        }                                                                      \
    } while (0)

#if defined(__GNUC__)
#define irt_unreachable() __builtin_unreachable();
#elif defined(_MSC_VER)
#define irt_unreachable() __assume(0)
#else
#define irt_unreachable()
#endif

namespace irt {

using i8 = int8_t;
using i16 = int16_t;
using i32 = int32_t;
using i64 = int64_t;
using u8 = uint8_t;
using u16 = uint16_t;
using u32 = uint32_t;
using u64 = uint64_t;
using sz = size_t;

inline u16
make_halfword(u8 a, u8 b) noexcept
{
    return static_cast<u16>((a << 8) | b);
}

inline void
unpack_halfword(u16 halfword, u8* a, u8* b) noexcept
{
    *a = static_cast<u8>((halfword >> 8) & 0xff);
    *b = static_cast<u8>(halfword & 0xff);
}

inline u32
make_word(u16 a, u16 b) noexcept
{
    return (static_cast<u32>(a) << 16) | static_cast<u32>(b);
}

inline void
unpack_word(u32 word, u16* a, u16* b) noexcept
{
    *a = static_cast<u16>((word >> 16) & 0xffff);
    *b = static_cast<u16>(word & 0xffff);
}

inline u64
make_doubleword(u32 a, u32 b) noexcept
{
    return (static_cast<u64>(a) << 32) | static_cast<u64>(b);
}

inline void
unpack_doubleword(u64 doubleword, u32* a, u32* b) noexcept
{
    *a = static_cast<u32>((doubleword >> 32) & 0xffffffff);
    *b = static_cast<u32>(doubleword & 0xffffffff);
}

template<typename Integer>
constexpr typename std::make_unsigned<Integer>::type
to_unsigned(Integer value)
{
    irt_assert(value >= 0);
    return static_cast<typename std::make_unsigned<Integer>::type>(value);
}

template<class C>
constexpr int
length(const C& c) noexcept
{
    return static_cast<int>(c.size());
}

template<class T, size_t N>
constexpr int
length(const T (&array)[N]) noexcept
{
    (void)array;

    return static_cast<int>(N);
}

template<typename Identifier>
constexpr Identifier
undefined() noexcept
{
    static_assert(
      std::is_enum<Identifier>::value,
      "Identifier must be a enumeration: enum class id : unsigned {};");

    return static_cast<Identifier>(0);
}

template<typename Identifier>
constexpr bool
is_undefined(Identifier id) noexcept
{
    static_assert(
      std::is_enum<Identifier>::value,
      "Identifier must be a enumeration: enum class id : unsigned {};");

    return id == undefined<Identifier>();
}

template<typename Identifier>
constexpr bool
is_defined(Identifier id) noexcept
{
    static_assert(
      std::is_enum<Identifier>::value,
      "Identifier must be a enumeration: enum class id : unsigned {};");

    return id != undefined<Identifier>();
}

/**
 * @brief Enummeration to integral
 */
template<class Enum, class Integer = typename std::underlying_type<Enum>::type>
constexpr Integer
ordinal(Enum e) noexcept
{
    static_assert(std::is_enum<Enum>::value,
                  "Identifier must be a enumeration");
    return static_cast<Integer>(e);
}

/**
 * @brief Integral to enumeration
 */
template<class Enum, class Integer = typename std::underlying_type<Enum>::type>
constexpr Enum
enum_cast(Integer i) noexcept
{
    static_assert(std::is_enum<Enum>::value,
                  "Identifier must be a enumeration");
    return static_cast<Enum>(i);
}

/**
 * @brief returns an iterator to the result or end if not found
 *
 * Binary search function which returns an iterator to the result or end if
 * not found using the lower_bound standard function.
 */
template<typename Iterator, typename T>
Iterator
binary_find(Iterator begin, Iterator end, const T& value)
{
    Iterator i = std::lower_bound(begin, end, value);
    if (i != end && !(value < *i))
        return i;
    else
        return end;
}

/**
 * @brief returns an iterator to the result or end if not found
 *
 * Binary search function which returns an iterator to the result or end if
 * not found using the lower_bound standard function.
 */
template<typename Iterator, typename T, typename Compare>
Iterator
binary_find(Iterator begin, Iterator end, const T& value, Compare comp)
{
    Iterator i = std::lower_bound(begin, end, value, comp);
    if (i != end && !comp(*i, value))
        return i;
    else
        return end;
}

/*****************************************************************************
 *
 * Return status of many function
 *
 ****************************************************************************/

enum class status
{
    success,
    unknown_dynamics,
    block_allocator_bad_capacity,
    block_allocator_not_enough_memory,
    head_allocator_bad_capacity,
    head_allocator_not_enough_memory,
    simulation_not_enough_model,
    simulation_not_enough_memory_message_list_allocator,
    simulation_not_enough_memory_input_port_list_allocator,
    simulation_not_enough_memory_output_port_list_allocator,
    data_array_init_capacity_error,
    data_array_not_enough_memory,
    data_array_archive_init_capacity_error,
    data_array_archive_not_enough_memory,
    array_init_capacity_zero,
    array_init_capacity_too_big,
    array_init_not_enough_memory,
    vector_init_capacity_zero,
    vector_init_capacity_too_big,
    vector_init_not_enough_memory,
    dynamics_unknown_id,
    dynamics_unknown_port_id,
    dynamics_not_enough_memory,
    model_connect_output_port_unknown,
    model_connect_input_port_unknown,
    model_connect_already_exist,
    model_connect_bad_dynamics,
    model_queue_bad_ta,
    model_queue_empty_allocator,
    model_queue_full,
    model_dynamic_queue_source_is_null,
    model_dynamic_queue_empty_allocator,
    model_dynamic_queue_full,
    model_priority_queue_source_is_null,
    model_priority_queue_empty_allocator,
    model_priority_queue_full,
    model_integrator_dq_error,
    model_integrator_X_error,
    model_integrator_internal_error,
    model_integrator_output_error,
    model_integrator_running_without_x_dot,
    model_integrator_ta_with_bad_x_dot,
    model_generator_null_ta_source,
    model_generator_empty_ta_source,
    model_generator_null_value_source,
    model_generator_empty_value_source,
    model_quantifier_bad_quantum_parameter,
    model_quantifier_bad_archive_length_parameter,
    model_quantifier_shifting_value_neg,
    model_quantifier_shifting_value_less_1,
    model_time_func_bad_init_message,
    model_flow_bad_samplerate,
    model_flow_bad_data,
    gui_not_enough_memory,
    io_not_enough_memory,
    io_file_format_error,
    io_file_format_model_error,
    io_file_format_model_number_error,
    io_file_format_model_unknown,
    io_file_format_dynamics_unknown,
    io_file_format_dynamics_limit_reach,
    io_file_format_dynamics_init_error
};

constexpr i8
status_last() noexcept
{
    return static_cast<i8>(status::io_file_format_dynamics_init_error);
}

constexpr sz
status_size() noexcept
{
    return static_cast<sz>(status_last() + static_cast<i8>(1));
}

constexpr bool
is_success(status s) noexcept
{
    return s == status::success;
}

constexpr bool
is_bad(status s) noexcept
{
    return s != status::success;
}

template<typename... Args>
constexpr bool
is_status_equal(status s, Args... args) noexcept
{
    return ((s == args) || ... || false);
}

inline status
check_return(status s) noexcept
{
    if (s != status::success)
        irt_breakpoint();

    return s;
}

template<typename T, typename... Args>
constexpr bool
match(const T& s, Args... args) noexcept
{
    return ((s == args) || ... || false);
}

template<class T, class... Rest>
constexpr bool
are_all_same() noexcept
{
    return (std::is_same_v<T, Rest> && ...);
}

template<class T>
typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
almost_equal(T x, T y, int ulp)
{
    return std::fabs(x - y) <=
             std::numeric_limits<T>::epsilon() * std::fabs(x + y) * ulp ||
           std::fabs(x - y) < std::numeric_limits<T>::min();
}

/*****************************************************************************
 *
 * Definition of a lightweight std::function
 *
 ****************************************************************************/

template<class F>
class function_ref;

template<class R, class... Args>
class function_ref<R(Args...)>
{
public:
    constexpr function_ref() noexcept = default;

    /// Creates a `function_ref` which refers to the same callable as `rhs`.
    constexpr function_ref(const function_ref<R(Args...)>& rhs) noexcept =
      default;

    /// Constructs a `function_ref` referring to `f`.
    ///
    /// \synopsis template <typename F> constexpr function_ref(F &&f) noexcept
    template<
      typename F,
      std::enable_if_t<!std::is_same<std::decay_t<F>, function_ref>::value &&
                       std::is_invocable_r<R, F&&, Args...>::value>* = nullptr>
    constexpr function_ref(F&& f) noexcept
      : obj(const_cast<void*>(reinterpret_cast<const void*>(std::addressof(f))))
    {
        cb = [](void* obj, Args... args) -> R {
            return std::invoke(
              *reinterpret_cast<typename std::add_pointer<F>::type>(obj),
              std::forward<Args>(args)...);
        };
    }

    /// Makes `*this` refer to the same callable as `rhs`.
    constexpr function_ref<R(Args...)>& operator=(
      const function_ref<R(Args...)>& rhs) noexcept = default;

    /// Makes `*this` refer to `f`.
    ///
    /// \synopsis template <typename F> constexpr function_ref &operator=(F &&f)
    /// noexcept;
    template<
      typename F,
      std::enable_if_t<std::is_invocable_r<R, F&&, Args...>::value>* = nullptr>
    constexpr function_ref<R(Args...)>& operator=(F&& f) noexcept
    {
        obj = reinterpret_cast<void*>(std::addressof(f));
        cb = [](void* obj, Args... args) {
            return std::invoke(
              *reinterpret_cast<typename std::add_pointer<F>::type>(obj),
              std::forward<Args>(args)...);
        };

        return *this;
    }

    constexpr void swap(function_ref<R(Args...)>& rhs) noexcept
    {
        std::swap(obj, rhs.obj);
        std::swap(cb, rhs.cb);
    }

    R operator()(Args... args) const
    {
        return cb(obj, std::forward<Args>(args)...);
    }

    constexpr bool empty() const noexcept
    {
        return obj == nullptr;
    }

    constexpr void reset() noexcept
    {
        obj = nullptr;
        cb = nullptr;
    }

private:
    void* obj = nullptr;
    R (*cb)(void*, Args...) = nullptr;
};

/// Swaps the referred callables of `lhs` and `rhs`.
template<typename R, typename... Args>
constexpr void
swap(function_ref<R(Args...)>& lhs, function_ref<R(Args...)>& rhs) noexcept
{
    lhs.swap(rhs);
}

template<typename R, typename... Args>
function_ref(R (*)(Args...)) -> function_ref<R(Args...)>;

template<typename R, typename... Args>
function_ref(R (*)(Args...) noexcept) -> function_ref<R(Args...) noexcept>;

/*****************************************************************************
 *
 * Allocator
 *
 ****************************************************************************/

using global_alloc_function_type = function_ref<void*(sz size)>;
using global_free_function_type = function_ref<void(void* ptr)>;

static inline void*
malloc_wrapper(sz size)
{
    return std::malloc(size);
}

static inline void
free_wrapper(void* ptr)
{
    if (ptr)
        std::free(ptr);
}

static inline global_alloc_function_type g_alloc_fn{ malloc_wrapper };
static inline global_free_function_type g_free_fn{ free_wrapper };

/*****************************************************************************
 *
 * Definition of Time
 *
 * TODO:
 * - enable template definition of float or [float|double,bool absolute]
 *   representation of time?
 *
 ****************************************************************************/

using time = double;

template<typename T>
struct time_domain
{};

template<>
struct time_domain<time>
{
    using time_type = time;

    static constexpr const double infinity =
      std::numeric_limits<double>::infinity();
    static constexpr const double negative_infinity =
      -std::numeric_limits<double>::infinity();
    static constexpr const double zero = 0;

    static constexpr bool is_infinity(time t) noexcept
    {
        return t == infinity || t == negative_infinity;
    }

    static constexpr bool is_zero(time t) noexcept
    {
        return t == zero;
    }
};

/*****************************************************************************
 *
 * Small string
 *
 ****************************************************************************/

template<size_t length = 8>
class small_string
{
    char buffer_[length];
    u8 size_;

public:
    using iterator = char*;
    using const_iterator = const char*;
    using size_type = u8;

    static_assert(length > size_t{ 1 } && length < size_t{ 254 });

    constexpr small_string() noexcept
    {
        clear();
    }

    constexpr small_string(const small_string& str) noexcept
    {
        std::copy_n(str.buffer_, str.size_, buffer_);
        buffer_[str.size_] = '\0';
        size_ = str.size_;
    }

    constexpr small_string(small_string&& str) noexcept
    {
        std::copy_n(str.buffer_, str.size_, buffer_);
        buffer_[str.size_] = '\0';
        size_ = str.size_;
        str.clear();
    }

    constexpr small_string& operator=(const small_string& str) noexcept
    {
        if (&str != this) {
            std::copy_n(str.buffer_, str.size_, buffer_);
            buffer_[str.size_] = '\0';
            size_ = str.size_;
        }

        return *this;
    }

    constexpr small_string& operator=(small_string&& str) noexcept
    {
        if (&str != this) {
            std::copy_n(str.buffer_, str.size_, buffer_);
            buffer_[str.size_] = '\0';
            size_ = str.size_;
        }

        return *this;
    }

    constexpr small_string(const char* str) noexcept
    {
        std::strncpy(buffer_, str, length - 1);
        buffer_[length - 1] = '\0';
        size_ = static_cast<u8>(std::strlen(buffer_));
    }

    constexpr small_string(const std::string_view str) noexcept
    {
        assign(str);
    }

    constexpr bool empty() const noexcept
    {
        constexpr unsigned char zero{ 0 };

        return zero == size_;
    }

    constexpr void size(std::size_t sz) noexcept
    {
        size_ = static_cast<u8>(std::min(sz, length - 1));
        buffer_[size_] = '\0';
    }

    constexpr std::size_t size() const noexcept
    {
        return size_;
    }

    constexpr std::size_t capacity() const noexcept
    {
        return length;
    }

    constexpr void assign(const std::string_view str) noexcept
    {
        const size_t copy_length = std::min(str.size(), length - 1);

        std::memcpy(buffer_, str.data(), copy_length);
        buffer_[copy_length] = '\0';

        size_ = static_cast<u8>(copy_length);
    }

    constexpr std::string_view sv() const noexcept
    {
        return { buffer_, size_ };
    }

    constexpr void clear() noexcept
    {
        std::fill_n(buffer_, length, '\0');
        size_ = 0;
    }

    constexpr const char* c_str() const noexcept
    {
        return buffer_;
    }

    constexpr iterator begin() noexcept
    {
        return buffer_;
    }

    constexpr iterator end() noexcept
    {
        return buffer_ + size_;
    }

    constexpr const_iterator begin() const noexcept
    {
        return buffer_;
    }

    constexpr const_iterator end() const noexcept
    {
        return buffer_ + size_;
    }

    constexpr bool operator==(const small_string& rhs) const noexcept
    {
        return std::strncmp(buffer_, rhs.buffer_, length) == 0;
    }

    constexpr bool operator!=(const small_string& rhs) const noexcept
    {
        return std::strncmp(buffer_, rhs.buffer_, length) != 0;
    }

    constexpr bool operator>(const small_string& rhs) const noexcept
    {
        return std::strncmp(buffer_, rhs.buffer_, length) > 0;
    }

    constexpr bool operator<(const small_string& rhs) const noexcept
    {
        return std::strncmp(buffer_, rhs.buffer_, length) < 0;
    }

    constexpr bool operator==(const char* rhs) const noexcept
    {
        return std::strncmp(buffer_, rhs, length) == 0;
    }

    constexpr bool operator!=(const char* rhs) const noexcept
    {
        return std::strncmp(buffer_, rhs, length) != 0;
    }

    constexpr bool operator>(const char* rhs) const noexcept
    {
        return std::strncmp(buffer_, rhs, length) > 0;
    }

    constexpr bool operator<(const char* rhs) const noexcept
    {
        return std::strncmp(buffer_, rhs, length) < 0;
    }
};

/*****************************************************************************
 *
 * value
 *
 ****************************************************************************/

struct message
{
    using size_type = std::size_t;
    using difference_type = std::ptrdiff_t;

    double real[4];

    constexpr size_type size() const noexcept
    {
        return real[3] ? 4u : real[2] ? 3u : real[1] ? 2u : real[0] ? 1u : 0u;
    }

    constexpr difference_type ssize() const noexcept
    {
        return real[3] ? 4 : real[2] ? 3 : real[1] ? 2 : real[0] ? 1 : 0;
    }

    constexpr message() noexcept
      : real{ 0., 0., 0., 0. }
    {}

    template<typename... Args>
    constexpr message(Args&&... args)
      : real{ std::forward<Args>(args)... }
    {
        auto size = sizeof...(args);
        for (; size != std::size(real); ++size)
            real[size] = 0.;
    }

    constexpr double operator[](const difference_type i) const noexcept
    {
        return real[i];
    }

    constexpr double& operator[](const difference_type i) noexcept
    {
        return real[i];
    }

    constexpr void reset() noexcept
    {
        std::fill_n(std::data(real), std::size(real), 0.);
    }
};

struct dated_message
{
    using size_type = std::size_t;
    using difference_type = std::ptrdiff_t;

    double real[5];

    constexpr dated_message() noexcept
      : real{ 0., 0., 0., 0., 0. }
    {}

    template<typename... Args>
    constexpr dated_message(Args&&... args)
      : real{ std::forward<Args>(args)... }
    {
        auto size = sizeof...(args);
        for (; size != std::size(real); ++size)
            real[size] = 0.;
    }

    constexpr double operator[](const difference_type i) const noexcept
    {
        return real[i];
    }

    constexpr double& operator[](const difference_type i) noexcept
    {
        return real[i];
    }

    constexpr void reset() noexcept
    {
        std::fill_n(std::data(real), std::size(real), 0.);
    }

    inline bool operator<(const dated_message& rhs) const noexcept
    {
        return real[0] < rhs.real[0];
    }

    inline bool operator==(const dated_message& rhs) const noexcept
    {
        return real[0] == rhs.real[0];
    }
};

/*****************************************************************************
 *
 * Flat list
 *
 ****************************************************************************/

template<typename T>
class block_allocator
{
public:
    using value_type = T;

    union block
    {
        block* next;
        typename std::aligned_storage<sizeof(T), alignof(T)>::type storage;
    };

private:
    block* blocks{ nullptr };    // contains all preallocated blocks
    block* free_head{ nullptr }; // a free list
    sz size{ 0 };                // number of active elements allocated
    sz max_size{ 0 }; // number of elements allocated (with free_head)
    sz capacity{ 0 }; // capacity of the allocator

public:
    block_allocator() = default;

    block_allocator(const block_allocator&) = delete;
    block_allocator& operator=(const block_allocator&) = delete;

    ~block_allocator() noexcept
    {
        if (blocks)
            g_free_fn(blocks);
    }

    status init(std::size_t new_capacity) noexcept
    {
        if (new_capacity == 0)
            return status::block_allocator_bad_capacity;

        if (new_capacity != capacity) {
            if (blocks)
                g_free_fn(blocks);

            blocks =
              static_cast<block*>(g_alloc_fn(new_capacity * sizeof(block)));
            if (blocks == nullptr)
                return status::block_allocator_not_enough_memory;
        }

        size = 0;
        max_size = 0;
        capacity = new_capacity;
        free_head = nullptr;

        return status::success;
    }

    void reset() noexcept
    {
        if (capacity > 0) {
            size = 0;
            max_size = 0;
            free_head = nullptr;
        }
    }

    T* alloc() noexcept
    {
        block* new_block = nullptr;

        if (free_head != nullptr) {
            new_block = free_head;
            free_head = free_head->next;
        } else {
            irt_assert(max_size < capacity);
            new_block = reinterpret_cast<block*>(&blocks[max_size++]);
        }
        ++size;

        return reinterpret_cast<T*>(new_block);
    }

    bool can_alloc() noexcept
    {
        return free_head != nullptr || max_size < capacity;
    }

    void free(T* n) noexcept
    {
        irt_assert(n);

        block* ptr = reinterpret_cast<block*>(n);

        ptr->next = free_head;
        free_head = ptr;

        --size;

        if (size == 0) {         // A special part: if it no longer exists
            max_size = 0;        // we reset the free list and the number
            free_head = nullptr; // of elements allocated.
        }
    }

    bool can_alloc(size_t number) const noexcept
    {
        return number + size < capacity;
    }
};

template<typename T>
class shared_flat_list
{
public:
    struct node_type
    {
        T value;
        node_type* next = nullptr;
    };

public:
    using allocator_type = block_allocator<node_type>;
    using value_type = T;
    using reference = T&;
    using const_reference = const T&;
    using pointer = T*;

    class iterator
    {
    private:
        node_type* node{ nullptr };