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>
#include <numbers>
#include <string_view>

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

#include <vector>

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

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

#ifndef NDEBUG
#if (defined(__i386__) || defined(__x86_64__)) && defined(__GNUC__) &&         \
  __GNUC__ >= 2
#define irt_breakpoint()                                                       \
    do {                                                                       \
        __asm__ __volatile__("int $03");                                       \
    } while (0)
#elif (defined(_MSC_VER) || defined(__DMC__)) && defined(_M_IX86)
#define irt_breakpoint()                                                       \
    do {                                                                       \
        __asm int 3h                                                           \
    } while (0)
#elif defined(_MSC_VER)
#define irt_breakpoint()                                                       \
    do {                                                                       \
        __debugbreak();                                                        \
    } while (0)
#elif defined(__alpha__) && !defined(__osf__) && defined(__GNUC__) &&          \
  __GNUC__ >= 2
#define irt_breakpoint()                                                       \
    do {                                                                       \
        __asm__ __volatile__("bpt");                                           \
    } while (0)
#elif defined(__APPLE__)
#define irt_breakpoint()                                                       \
    do {                                                                       \
        __builtin_trap();                                                      \
    } while (0)
#else /* !__i386__ && !__alpha__ */
#define irt_breakpoint()                                                       \
    do {                                                                       \
        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)

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;

/*****************************************************************************
 *
 * 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_adder_empty_init_message,
    model_adder_bad_init_message,

    model_mult_empty_init_message,
    model_mult_bad_init_message,

    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_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,

    gui_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 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> && ...);
}

/*****************************************************************************
 *
 * 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<unsigned char>(copy_length);
    }

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

    constexpr void append(const std::string_view str) noexcept
    {
        const size_t remaining = length - size_;

        if (remaining) {
            size_t copy = std::min(remaining - 1, str.size());
            std::strncpy(buffer_ + size_, str.data(), copy);
            copy += size_;
            size_ = static_cast<unsigned char>(copy);
            assert(size_ < length);

            buffer_[size_] = '\0';
        }
    }

    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[3];
    i8 length;

    constexpr std::size_t size() const noexcept
    {
        assert(length >= 0);
        return static_cast<std::size_t>(length);
    }

    constexpr message() noexcept
      : real{ 0.0, 0.0, 0.0 }
      , length{ 0 }
    {}

    template<typename... T>
    constexpr message(T... args) noexcept
    {
        if constexpr (are_all_same<double, T...>()) {
            static_assert(sizeof...(args) <= 3, "double message limited to 3");
            using unused = double[];
            length = 0;
            (void)unused{ 0.0, (real[length++] = args, 0.0)... };
        }
    }

    double operator[](const difference_type i) const noexcept
    {
        irt_assert(i < static_cast<std::ptrdiff_t>(length));
        return real[i];
    }

    double& operator[](const difference_type i) noexcept
    {
        irt_assert(i < static_cast<std::ptrdiff_t>(length));
        return real[i];
    }
};

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

template<typename T>
class block_allocator
{
public:
    static_assert(std::is_trivially_destructible_v<T>,
                  "block_allocator is only for POD object");

    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)
            std::free(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)
                std::free(blocks);
            blocks =
              static_cast<block*>(std::malloc(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;
        }
    }

    std::pair<bool, T*> try_alloc() noexcept
    {
        if (free_head == nullptr && max_size >= capacity)
            return { false, nullptr };

        return { true, alloc() };
    }

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

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

        return reinterpret_cast<T*>(new_block);
    }

    void free(T* n) noexcept
    {
        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.
        }
    }
};

template<typename T>
class 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 };

    public:
        using iterator_category = std::forward_iterator_tag;
        using value_type = T;
        using pointer = T*;
        using reference = T&;
        using difference_type = std::ptrdiff_t;

        iterator() noexcept = default;

        iterator(node_type* n) noexcept
          : node(n)
        {}

        iterator(const iterator& other) noexcept
          : node(other.node)
        {}

        iterator& operator=(const iterator& other) noexcept
        {
            node = other.node;
            return *this;
        }

        T& operator*() noexcept
        {
            return node->value;
        }

        T* operator->() noexcept
        {
            return &node->value;
        }

        iterator operator++() noexcept
        {
            if (node != nullptr)
                node = node->next;

            return *this;
        }

        iterator operator++(int) noexcept
        {
            iterator tmp(*this);

            if (node != nullptr)
                node = node->next;

            return tmp;
        }

        bool operator==(const iterator& other) const noexcept
        {
            return node == other.node;
        }

        bool operator!=(const iterator& other) const noexcept
        {
            return node != other.node;
        }

        void swap(iterator& other) noexcept
        {
            std::swap(node, other.node);
        }

        friend class flat_list<T>;
    };

    class const_iterator
    {
    private:
        const node_type* node{ nullptr };

    public:
        using iterator_category = std::forward_iterator_tag;
        using value_type = T;
        using pointer = T*;
        using reference = T&;
        using difference_type = std::ptrdiff_t;

        const_iterator() noexcept = default;

        const_iterator(node_type* n) noexcept
          : node(n)
        {}

        const_iterator(const const_iterator& other) noexcept
          : node(other.node)
        {}

        const_iterator& operator=(const const_iterator& other) noexcept
        {
            node = other.node;
            return *this;
        }

        const T& operator*() noexcept
        {
            return node->value;
        }

        const T* operator->() noexcept
        {
            return &node->value;
        }

        const_iterator operator++() noexcept
        {
            if (node != nullptr)
                node = node->next;

            return *this;
        }

        const_iterator operator++(int) noexcept
        {
            const_iterator tmp(*this);

            if (node != nullptr)
                node = node->next;

            return tmp;
        }

        bool operator==(const const_iterator& other) const noexcept
        {
            return node == other.node;
        }

        bool operator!=(const const_iterator& other) const noexcept
        {
            return node != other.node;
        }

        void swap(const_iterator& other) noexcept
        {
            std::swap(node, other.node);
        }

        friend class flat_list<T>;
    };

private:
    allocator_type* allocator{ nullptr };
    node_type* node{ nullptr };

public:
    flat_list() = default;

    flat_list(allocator_type* allocator_new) noexcept
      : allocator(allocator_new)
    {}

    flat_list(const flat_list& other) = delete;
    flat_list& operator=(const flat_list& other) = delete;

    flat_list(flat_list&& other) noexcept
      : allocator(other.allocator)
      , node(other.node)
    {
        other.allocator = nullptr;
        other.node = nullptr;
    }

    void set_allocator(allocator_type* allocator_new) noexcept
    {
        clear();
        allocator = allocator_new;
        node = nullptr;
    }

    flat_list& operator=(flat_list&& other) noexcept
    {
        if (this != &other) {
            clear();
            allocator = other.allocator;
            other.allocator = nullptr;
            node = other.node;
            other.node = nullptr;
        }

        return *this;
    }

    ~flat_list() noexcept
    {
        clear();
    }

    void clear() noexcept
    {
        node_type* prev = node;

        while (node != nullptr) {
            node = node->next;
            allocator->free(prev);
            prev = node;
        }
    }

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

    iterator begin() noexcept
    {
        return iterator(node);
    }

    iterator end() noexcept
    {
        return iterator(nullptr);
    }

    const_iterator begin() const noexcept
    {
        return const_iterator(node);
    }

    const_iterator end() const noexcept
    {
        return const_iterator(nullptr);
    }

    reference front() noexcept
    {
        assert(!empty());

        return node->value;
    }

    const_reference front() const noexcept
    {
        assert(!empty());

        return node->value;
    }

    template<typename... Args>
    iterator emplace_front(Args&&... args) noexcept
    {
        node_type* new_node = allocator->alloc();

        new (&new_node->value) T(std::forward<Args>(args)...);

        new_node->next = node;
        node = new_node;

        return begin();
    }

    template<typename... Args>
    iterator emplace_after(iterator it, Args&&... args) noexcept
    {
        node_type* new_node = allocator->alloc();
        new (&new_node->value) T(std::forward<Args>(args)...);

        if (it->node == nullptr)
            return emplace_front(std::forward<Args>(args)...);

        new_node->next = it->node->next;
        it->node->next = new_node;

        return iterator(new_node);
    }

    template<typename... Args>
    iterator try_emplace_front(Args&&... args) noexcept
    {
        auto [success, new_node] = allocator->try_alloc();

        if (!success)
            return end();

        new (&new_node->value) T(std::forward<Args>(args)...);

        new_node->next = node;
        node = new_node;

        return begin();
    }

    template<typename... Args>
    iterator try_emplace_after(iterator it, Args&&... args) noexcept
    {
        auto [success, new_node] = allocator->try_alloc();

        if (!success)
            return end();

        new (&new_node->value) T(std::forward<Args>(args)...);

        if (it->node == nullptr)
            return emplace_front(std::forward<Args>(args)...);

        new_node->next = it->node->next;
        it->node->next = new_node;

        return iterator(new_node);
    }

    void pop_front() noexcept
    {
        if (node == nullptr)
            return;

        node_type* to_delete = node;
        node = node->next;

        if constexpr (!std::is_trivial_v<T>)
            to_delete->value.~T();

        allocator->free(to_delete);
    }

    iterator erase_after(iterator it) noexcept
    {
        assert(allocator);

        if (it.node == nullptr)
            return end();

        node_type* to_delete = it.node->next;
        if (to_delete == nullptr)
            return end();

        node_type* next = to_delete->next;
        it.node->next = next;

        if constexpr (!std::is_trivial_v<T>)
            to_delete->value.~T();

        allocator->free(to_delete);

        return iterator(next);
    }
};

template<typename T>
class flat_double_list
{
private:
    struct node_type
    {
        T value;
        node_type* next = nullptr;
        node_type* prev = nullptr;
    };

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