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 <string_view>

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

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

    model_mult_empty_init_message,
    model_mult_bad_init_message,
    model_mult_bad_external_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_integrator_bad_external_message,

    model_quantifier_bad_quantum_parameter,
    model_quantifier_bad_archive_length_parameter,
    model_quantifier_shifting_value_neg,
    model_quantifier_shifting_value_less_1,
    model_quantifier_bad_external_message,

    model_cross_bad_external_message,

    model_time_func_bad_init_message,

    model_accumulator_bad_external_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);
}

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

#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)

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

    return s;
}

/*****************************************************************************
 *
 * 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
 *
 ****************************************************************************/

enum class value_type : i8
{
    none,
    integer_8,
    integer_32,
    integer_64,
    real_32,
    real_64
};

template<typename T, size_t length>
struct span
{
public:
    using value_type = T;
    using difference_type = std::ptrdiff_t;
    using pointer = T*;
    using reference = T&;
    using const_reference = T&;
    using iterator = T*;
    using const_iterator = const T*;

private:
    const T* data_;

public:
    constexpr span(const T* ptr)
      : data_(ptr)
    {}

    constexpr T* data() noexcept
    {
        return data_;
    }

    constexpr const T* data() const noexcept
    {
        return data_;
    }

    constexpr size_t size() const noexcept
    {
        return length;
    }

    constexpr T operator[](size_t i) const noexcept
    {
        assert(i < length);
        return data_[i];
    }

    constexpr iterator begin() noexcept
    {
        return iterator{ data_ };
    }

    constexpr iterator end() noexcept
    {
        return iterator{ data_ + length };
    }

    constexpr const_iterator begin() const noexcept
    {
        return iterator{ data_ };
    }

    constexpr const_iterator end() const noexcept
    {
        return iterator{ data_ + length };
    }
};

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

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

    union
    {
        i8 integer_8[32];  // 32 bytes
        i32 integer_32[8]; // 8 * 4 bytes
        i64 integer_64[4]; // 4 * 8 bytes
        float real_32[8];  // 8 * 4 bytes
        double real_64[4]; // 4 * 8 bytes
    };

    u8 length;
    value_type type;

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

    constexpr message() noexcept
      : integer_8{ 0 }
      , length{ 0 }
      , type{ value_type::none }
    {}

    template<typename... T>
    constexpr message(T... args) noexcept
    {
        if constexpr (are_all_same<i8, T...>()) {
            static_assert(sizeof...(args) <= 32, "i8 message limited to 32");
            using unused = i8[];
            length = 0;
            (void)unused{ i8(0), (integer_8[length++] = args, i8(0))... };
            type = value_type::integer_8;
        } else if constexpr (are_all_same<i32, T...>()) {
            static_assert(sizeof...(args) <= 8, "i32 message limited to 32");
            using unused = i32[];
            length = 0;
            (void)unused{ i32(0), (integer_32[length++] = args, i32(0))... };
            type = value_type::integer_32;
        } else if constexpr (are_all_same<i64, T...>()) {
            static_assert(sizeof...(args) <= 4, "i64 message limited to 32");
            using unused = i64[];
            length = 0;
            (void)unused{ i64(0), (integer_64[length++] = args, i64(0))... };
            type = value_type::integer_64;
        } else if constexpr (are_all_same<float, T...>()) {
            static_assert(sizeof...(args) <= 8, "float message limited to 8");
            using unused = float[];
            length = 0;
            (void)unused{ 0.0f, (real_32[length++] = args, 0.0f)... };
            type = value_type::real_32;
            return;
        } else if constexpr (are_all_same<double, T...>()) {
            static_assert(sizeof...(args) <= 4, "double message limited to 4");
            using unused = double[];
            length = 0;
            (void)unused{ 0.0, (real_64[length++] = args, 0.0)... };
            type = value_type::real_64;
        }
    }

    constexpr span<i8, 32> to_integer_8() const
    {
        assert(type == value_type::integer_8);
        return span<i8, 32>(integer_8);
    }

    constexpr span<i32, 8> to_integer_32() const
    {
        assert(type == value_type::integer_32);
        return span<i32, 8>(integer_32);
    }

    constexpr span<i64, 4> to_integer_64() const
    {
        assert(type == value_type::integer_64);
        return span<i64, 4>(integer_64);
    }

    constexpr span<float, 8> to_real_32() const
    {
        assert(type == value_type::real_32);
        return span<float, 8>(real_32);
    }

    constexpr span<double, 4> to_real_64() const
    {
        assert(type == value_type::real_64);
        return span<double, 4>(real_64);
    }

    template<typename T>
    constexpr i8 to_integer_8(T i) const
    {
        static_assert(std::is_integral_v<T>, "need [unsigned] integer");

        if constexpr (std::is_signed_v<T>)
            assert(i >= 0);

        assert(type == value_type::integer_8);
        assert(i < static_cast<T>(length));

        return integer_8[i];
    }

    template<typename T>
    constexpr i32 to_integer_32(T i) const
    {
        static_assert(std::is_integral_v<T>, "need [unsigned] integer");

        if constexpr (std::is_signed_v<T>)
            assert(i >= 0);

        assert(type == value_type::integer_32);
        assert(i < static_cast<T>(length));

        return integer_32[i];
    }

    template<typename T>
    constexpr i64 to_integer_64(T i) const
    {
        static_assert(std::is_integral_v<T>, "need [unsigned] integer");

        if constexpr (std::is_signed_v<T>)
            assert(i >= 0);

        assert(type == value_type::integer_64);
        assert(i < static_cast<T>(length));

        return integer_64[i];
    }

    template<typename T>
    constexpr float to_real_32(T i) const
    {
        static_assert(std::is_integral_v<T>, "need [unsigned] integer");

        if constexpr (std::is_signed_v<T>)
            assert(i >= 0);

        assert(type == value_type::real_32);
        assert(i < static_cast<T>(length));

        return real_32[i];
    }

    template<typename T>
    constexpr double to_real_64(T i) const
    {
        static_assert(std::is_integral_v<T>, "need [unsigned] integer");

        if constexpr (std::is_signed_v<T>)
            assert(i >= 0);

        assert(type == value_type::real_64);
        assert(i < static_cast<T>(length));

        return real_64[i];
    }

    template<typename T>
    constexpr double cast_to_real_64(T i) const
    {
        if constexpr (std::is_signed_v<T>)
            assert(i >= 0);

        switch (type) {
        case value_type::integer_8:
            return static_cast<double>(to_integer_8(i));
        case value_type::integer_32:
            return static_cast<double>(to_integer_32(i));
        case value_type::integer_64:
            return static_cast<double>(to_integer_64(i));
        case value_type::real_32:
            return static_cast<double>(to_real_32(i));
        case value_type::real_64:
            return static_cast<double>(to_real_64(i));
        default:
            return 0.0;
        }

        return 0.0;
    }
};

/*****************************************************************************
 *
 * 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>;
    };