<commit_before>#include "ylikuutio_string.hpp"

// Include standard headers
#include <cstdio>   // std::FILE, std::fclose, std::fopen, std::fread, std::getchar, std::printf etc.
#include <cstring>  // std::memcmp, std::strcmp, std::strlen, std::strncmp
#include <iomanip>  // std::setfill, std::setw
#include <iostream> // std::cout, std::cin, std::cerr
#include <list>     // std::list
#include <stdint.h> // uint32_t etc.
#include <sstream>  // std::istringstream, std::ostringstream, std::stringstream
#include <string>   // std::string
#include <vector>   // std::vector

namespace string
{
    bool check_and_report_if_some_string_matches(
            const char* const base_pointer,
            const char* const data_pointer,
            const uint64_t data_size,
            const std::vector<std::string> identifier_strings_vector)
    {
        for (std::string identifier_string : identifier_strings_vector)
        {
            const char* const identifier_string_char = identifier_string.c_str();

            if (data_pointer + identifier_string.length() > base_pointer + data_size)
            {
                // If current `identifier_string` can't fit in the memory region,
                // proceed to the next `identifier_string`, if there is any left.
                continue;
            }

            if (std::strncmp(data_pointer, identifier_string_char, std::strlen(identifier_string_char)) == 0)
            {
                return true;
            }
        }
        return false;
    }

    void extract_string(
            const char* const src_base_pointer,
            char*& src_data_pointer,
            const uint64_t src_data_size,
            const char* const dest_base_pointer,
            char* dest_data_pointer,
            const uint64_t dest_data_size,
            const char* const char_end_string)
    {
        while (src_data_pointer < src_base_pointer + src_data_size &&
                dest_data_pointer + 1 < dest_base_pointer + dest_data_size &&
                std::strncmp(src_data_pointer, char_end_string, std::strlen(char_end_string)) != 0)
        {
            strncpy(dest_data_pointer++, src_data_pointer++, 1);
        }
        *dest_data_pointer = '\0';
    }

    void extract_string_with_several_endings(
            const char* const src_base_pointer,
            char*& src_data_pointer,
            const uint64_t src_data_size,
            const char* const dest_base_pointer,
            char* dest_data_pointer,
            const uint64_t dest_data_size,
            const char* const char_end_string)
    {
        // This function copies characters from `src_mem_pointer` until a character matches.

        while (src_data_pointer < src_base_pointer + src_data_size &&
                dest_data_pointer + 1 < dest_base_pointer + dest_data_size)
        {
            uint32_t n_of_ending_characters = std::strlen(char_end_string);
            const char* end_char_pointer;
            end_char_pointer = char_end_string;

            // Check if current character is any of the ending characters.
            while (*end_char_pointer != '\0')
            {
                if (std::strncmp(src_data_pointer, end_char_pointer, 1) == 0)
                {
                    *dest_data_pointer = '\0';
                    return;
                }
                end_char_pointer++;
            }

            // OK, current character is not any of the ending characters.
            // Copy it and advance the pointers accordingly.
            strncpy(dest_data_pointer++, src_data_pointer++, 1);
        }
        *dest_data_pointer = '\0';
        return;
    }

    int32_t extract_int32_t_value_from_string(
            const char* const src_base_pointer,
            char*& src_data_pointer,
            const uint64_t src_data_size,
            const char* const char_end_string,
            const char* const description)
    {
        char char_number_buffer[1024];
        string::extract_string_with_several_endings(
                src_base_pointer,
                ++src_data_pointer,
                src_data_size,
                char_number_buffer,
                char_number_buffer,
                sizeof(char_number_buffer),
                char_end_string);

        uint32_t value = std::strtoul(char_number_buffer, nullptr, 10); // base 10.

        if (description != nullptr)
        {
            std::printf("%s: %d\n", description, value);
        }
        return value;
    }

    float extract_float_value_from_string(
            const char* const src_base_pointer,
            char*& src_data_pointer,
            const uint64_t src_data_size,
            const char* const char_end_string,
            const char* const description)
    {
        char char_number_buffer[1024];
        string::extract_string_with_several_endings(
                src_base_pointer,
                ++src_data_pointer,
                src_data_size,
                char_number_buffer,
                char_number_buffer,
                sizeof(char_number_buffer),
                char_end_string);

        float value = std::strtof(char_number_buffer, nullptr);

        if (description != nullptr)
        {
            std::printf("%s: %f\n", description, value);
        }
        return value;
    }

    int32_t extract_unicode_value_from_string(const char*& unicode_char_pointer)
    {
        if (*unicode_char_pointer == '\0')
        {
            unicode_char_pointer++;
            std::cerr << "Error: Unicode can not begin with \\0!\n";
            return 0xdfff; // invalid unicode!
        }

        if (*unicode_char_pointer != '&')
        {
            // it's just a character, so return its value,
            // and advance to the next character.
            return (int32_t) *unicode_char_pointer++;
        }

        if (*++unicode_char_pointer != '#')
        {
            // not valid format, must begin `"&#x"`.
            unicode_char_pointer++;
            std::cerr << "Error: Unicode string format not supported!\n";
            return 0xdfff; // invalid unicode!
        }

        if (*++unicode_char_pointer != 'x')
        {
            // not valid format, must begin `"&#x"`.
            unicode_char_pointer++;
            std::cerr << "Error: Unicode string format not supported!\n";
            return 0xdfff; // invalid unicode!
        }

        // valid format.
        std::string hex_string;

        // unicode string beginning with '&'
        while (*++unicode_char_pointer != ';')
        {
            if (*unicode_char_pointer == '\0')
            {
                std::cerr << "Error: Null character \\0 reached before end of Unicode string!\n";
                return 0xdfff; // invalid unicode!
            }

            char current_char = *unicode_char_pointer;
            hex_string.append(unicode_char_pointer);
        }

        // Advance to the next character.
        unicode_char_pointer++;

        // convert hexadecimal string to signed integer.
        // http://stackoverflow.com/questions/1070497/c-convert-hex-string-to-signed-integer/1070499#1070499
        uint32_t unicode_value;
        std::stringstream unicode_stringstream;
        unicode_stringstream << std::hex << hex_string;
        unicode_stringstream >> unicode_value;
        return unicode_value;
    }

    std::string convert_std_list_char_to_std_string(const std::list<char>& std_list_char)
    {
        std::string my_string;

        for (std::list<char>::const_iterator it = std_list_char.begin(); it != std_list_char.end(); it++)
        {
            my_string.push_back(*it);
        }

        return my_string;
    }

    std::string convert_std_list_char_to_std_string(
            const std::list<char>& std_list_char,
            const uint32_t first_line_length,
            const uint32_t line_length)
    {
        std::string my_string;
        uint32_t remaining_characters_on_this_line = first_line_length;

        for (std::list<char>::const_iterator it = std_list_char.begin(); it != std_list_char.end(); it++)
        {
            if (remaining_characters_on_this_line == 0)
            {
                my_string.push_back('\\');
                my_string.push_back('n');
                remaining_characters_on_this_line = line_length;
            }
            my_string.push_back(*it);
            remaining_characters_on_this_line--;
        }
        return my_string;
    }

    std::vector<std::string> convert_std_list_char_to_std_vector_std_string(
            const std::list<char>& std_list_char,
            const uint32_t line_length)
    {
        std::vector<std::string> my_vector;
        std::string my_string;
        uint32_t remaining_characters_on_this_line = line_length;

        for (std::list<char>::const_iterator it = std_list_char.begin(); it != std_list_char.end(); it++)
        {
            if (remaining_characters_on_this_line == 0)
            {
                my_vector.push_back(my_string);
                my_string.clear();
                remaining_characters_on_this_line = line_length;
            }
            my_string.push_back(*it);
            remaining_characters_on_this_line--;
        }

        if (my_string.size() > 0)
        {
            my_vector.push_back(my_string);
        }

        return my_vector;
    }

    bool check_if_float_string(const std::string& my_string)
    {
        int32_t maximum_safe_length_for_float_string = 38;
        return check_if_floating_point_string(my_string, maximum_safe_length_for_float_string);
    }

    bool check_if_double_string(const std::string& my_string)
    {
        int32_t maximum_safe_length_for_double_string = 308;
        return check_if_floating_point_string(my_string, maximum_safe_length_for_double_string);
    }

    bool check_if_floating_point_string(const std::string& my_string, int32_t safe_number_of_chars)
    {
        int32_t n_chars = 0;

        if (my_string.empty())
        {
            return false;
        }

        bool is_dot_found = false;

        for (int32_t i = 0; i < my_string.length(); i++)
        {
            // Each of the characters must be one of the following:
            // 0123456789.-
            //
            // Besides, there may be only 1 dot. The dot may not be the last character.
            // Besides, there may be only 1 minus sign. The minus sign must be the first character.

            if (my_string.at(i) == '-' && i == 0)
            {
                // Minus sign is OK.
                continue;
            }

            if (my_string.at(i) == '.')
            {
                if (is_dot_found)
                {
                    // Two or more dots.
                    return false;
                }

                if (i == my_string.length() - 1)
                {
                    // Last character is dot.
                    return false;
                }

                // OK, decimal point here.
                is_dot_found = true;
                continue;
            }

            if (my_string.at(i) < '0' || my_string.at(i) > '9')
            {
                return false;
            }

            if (!is_dot_found)
            {
                if (++n_chars > safe_number_of_chars)
                {
                    // Too many characters, maximum safe number is 38 characters for float, 308 for double.
                    return false;
                }
            }
        }

        return true;
    }

    bool check_if_signed_integer_string(const std::string& my_string)
    {
        if (my_string.empty())
        {
            return false;
        }

        for (int32_t i = 0; i < my_string.length(); i++)
        {
            // Each of the characters must be one of the following:
            // 0123456789
            if (my_string.at(i) == '-' && i == 0)
            {
                // Minus sign is OK.
                continue;
            }

            if (my_string.at(i) < '0' || my_string.at(i) > '9')
            {
                return false;
            }
        }

        return true;
    }

    bool check_if_unsigned_integer_string(const std::string& my_string)
    {
        if (my_string.empty())
        {
            return false;
        }

        for (int32_t i = 0; i < my_string.length(); i++)
        {
            // Each of the characters must be one of the following:
            // 0123456789

            if (my_string.at(i) < '0' || my_string.at(i) > '9')
            {
                return false;
            }
        }

        return true;
    }

    void print_hexdump(const void* const start_address, const void* const end_address) // `begin` is inclusive, `end is exclusive.
    {
        void* void_start_address = const_cast<void*>(start_address);
        const int32_t line_width_in_bytes = 16;
        int32_t characters_on_this_line = 0;
        std::string current_line_ascii = "";
        std::string current_line_hex = "";

        for (uint8_t* data_pointer = static_cast<uint8_t*>(void_start_address); data_pointer < end_address; data_pointer++)
        {
            const uint8_t data_byte = static_cast<uint8_t>(*data_pointer);
            const char data_char = (data_byte >= 0x20 && data_byte <= 0x7f ? static_cast<char>(data_byte) : '.');
            current_line_ascii += data_char;

            uint32_t data_32_bit = static_cast<uint32_t>(data_byte); // to get the hexadecimal representation instead of the actual value.
            std::stringstream my_stream;
            my_stream << std::setfill('0') << std::setw(2) << std::hex << data_32_bit << std::dec; // std::hex does not work on char values.
            current_line_hex += my_stream.str();
            current_line_hex += " ";

            if (++characters_on_this_line >= line_width_in_bytes)
            {
                std::cout << current_line_hex << " " << current_line_ascii << "\n";
                current_line_hex = "";
                current_line_ascii = "";
                characters_on_this_line = 0;
            }
        }

        if (characters_on_this_line > 0)
        {
            const int32_t number_of_spaces_needed = (line_width_in_bytes - characters_on_this_line) * 3 + 1; // each byte's hexdump takes 3 characters.
            std::cout << current_line_hex << std::string(number_of_spaces_needed, ' ') << current_line_ascii << "\n";
            current_line_hex = "";
            current_line_ascii = "";
            characters_on_this_line = 0;
        }

        std::cout << "\n";
    }
}
<commit_msg>Bugfix: `print_hexdump`: removed dead store x 3.<commit_after>#include "ylikuutio_string.hpp"

// Include standard headers
#include <cstdio>   // std::FILE, std::fclose, std::fopen, std::fread, std::getchar, std::printf etc.
#include <cstring>  // std::memcmp, std::strcmp, std::strlen, std::strncmp
#include <iomanip>  // std::setfill, std::setw
#include <iostream> // std::cout, std::cin, std::cerr
#include <list>     // std::list
#include <stdint.h> // uint32_t etc.
#include <sstream>  // std::istringstream, std::ostringstream, std::stringstream
#include <string>   // std::string
#include <vector>   // std::vector

namespace string
{
    bool check_and_report_if_some_string_matches(
            const char* const base_pointer,
            const char* const data_pointer,
            const uint64_t data_size,
            const std::vector<std::string> identifier_strings_vector)
    {
        for (std::string identifier_string : identifier_strings_vector)
        {
            const char* const identifier_string_char = identifier_string.c_str();

            if (data_pointer + identifier_string.length() > base_pointer + data_size)
            {
                // If current `identifier_string` can't fit in the memory region,
                // proceed to the next `identifier_string`, if there is any left.
                continue;
            }

            if (std::strncmp(data_pointer, identifier_string_char, std::strlen(identifier_string_char)) == 0)
            {
                return true;
            }
        }
        return false;
    }

    void extract_string(
            const char* const src_base_pointer,
            char*& src_data_pointer,
            const uint64_t src_data_size,
            const char* const dest_base_pointer,
            char* dest_data_pointer,
            const uint64_t dest_data_size,
            const char* const char_end_string)
    {
        while (src_data_pointer < src_base_pointer + src_data_size &&
                dest_data_pointer + 1 < dest_base_pointer + dest_data_size &&
                std::strncmp(src_data_pointer, char_end_string, std::strlen(char_end_string)) != 0)
        {
            strncpy(dest_data_pointer++, src_data_pointer++, 1);
        }
        *dest_data_pointer = '\0';
    }

    void extract_string_with_several_endings(
            const char* const src_base_pointer,
            char*& src_data_pointer,
            const uint64_t src_data_size,
            const char* const dest_base_pointer,
            char* dest_data_pointer,
            const uint64_t dest_data_size,
            const char* const char_end_string)
    {
        // This function copies characters from `src_mem_pointer` until a character matches.

        while (src_data_pointer < src_base_pointer + src_data_size &&
                dest_data_pointer + 1 < dest_base_pointer + dest_data_size)
        {
            uint32_t n_of_ending_characters = std::strlen(char_end_string);
            const char* end_char_pointer;
            end_char_pointer = char_end_string;

            // Check if current character is any of the ending characters.
            while (*end_char_pointer != '\0')
            {
                if (std::strncmp(src_data_pointer, end_char_pointer, 1) == 0)
                {
                    *dest_data_pointer = '\0';
                    return;
                }
                end_char_pointer++;
            }

            // OK, current character is not any of the ending characters.
            // Copy it and advance the pointers accordingly.
            strncpy(dest_data_pointer++, src_data_pointer++, 1);
        }
        *dest_data_pointer = '\0';
        return;
    }

    int32_t extract_int32_t_value_from_string(
            const char* const src_base_pointer,
            char*& src_data_pointer,
            const uint64_t src_data_size,
            const char* const char_end_string,
            const char* const description)
    {
        char char_number_buffer[1024];
        string::extract_string_with_several_endings(
                src_base_pointer,
                ++src_data_pointer,
                src_data_size,
                char_number_buffer,
                char_number_buffer,
                sizeof(char_number_buffer),
                char_end_string);

        uint32_t value = std::strtoul(char_number_buffer, nullptr, 10); // base 10.

        if (description != nullptr)
        {
            std::printf("%s: %d\n", description, value);
        }
        return value;
    }

    float extract_float_value_from_string(
            const char* const src_base_pointer,
            char*& src_data_pointer,
            const uint64_t src_data_size,
            const char* const char_end_string,
            const char* const description)
    {
        char char_number_buffer[1024];
        string::extract_string_with_several_endings(
                src_base_pointer,
                ++src_data_pointer,
                src_data_size,
                char_number_buffer,
                char_number_buffer,
                sizeof(char_number_buffer),
                char_end_string);

        float value = std::strtof(char_number_buffer, nullptr);

        if (description != nullptr)
        {
            std::printf("%s: %f\n", description, value);
        }
        return value;
    }

    int32_t extract_unicode_value_from_string(const char*& unicode_char_pointer)
    {
        if (*unicode_char_pointer == '\0')
        {
            unicode_char_pointer++;
            std::cerr << "Error: Unicode can not begin with \\0!\n";
            return 0xdfff; // invalid unicode!
        }

        if (*unicode_char_pointer != '&')
        {
            // it's just a character, so return its value,
            // and advance to the next character.
            return (int32_t) *unicode_char_pointer++;
        }

        if (*++unicode_char_pointer != '#')
        {
            // not valid format, must begin `"&#x"`.
            unicode_char_pointer++;
            std::cerr << "Error: Unicode string format not supported!\n";
            return 0xdfff; // invalid unicode!
        }

        if (*++unicode_char_pointer != 'x')
        {
            // not valid format, must begin `"&#x"`.
            unicode_char_pointer++;
            std::cerr << "Error: Unicode string format not supported!\n";
            return 0xdfff; // invalid unicode!
        }

        // valid format.
        std::string hex_string;

        // unicode string beginning with '&'
        while (*++unicode_char_pointer != ';')
        {
            if (*unicode_char_pointer == '\0')
            {
                std::cerr << "Error: Null character \\0 reached before end of Unicode string!\n";
                return 0xdfff; // invalid unicode!
            }

            char current_char = *unicode_char_pointer;
            hex_string.append(unicode_char_pointer);
        }

        // Advance to the next character.
        unicode_char_pointer++;

        // convert hexadecimal string to signed integer.
        // http://stackoverflow.com/questions/1070497/c-convert-hex-string-to-signed-integer/1070499#1070499
        uint32_t unicode_value;
        std::stringstream unicode_stringstream;
        unicode_stringstream << std::hex << hex_string;
        unicode_stringstream >> unicode_value;
        return unicode_value;
    }

    std::string convert_std_list_char_to_std_string(const std::list<char>& std_list_char)
    {
        std::string my_string;

        for (std::list<char>::const_iterator it = std_list_char.begin(); it != std_list_char.end(); it++)
        {
            my_string.push_back(*it);
        }

        return my_string;
    }

    std::string convert_std_list_char_to_std_string(
            const std::list<char>& std_list_char,
            const uint32_t first_line_length,
            const uint32_t line_length)
    {
        std::string my_string;
        uint32_t remaining_characters_on_this_line = first_line_length;

        for (std::list<char>::const_iterator it = std_list_char.begin(); it != std_list_char.end(); it++)
        {
            if (remaining_characters_on_this_line == 0)
            {
                my_string.push_back('\\');
                my_string.push_back('n');
                remaining_characters_on_this_line = line_length;
            }
            my_string.push_back(*it);
            remaining_characters_on_this_line--;
        }
        return my_string;
    }

    std::vector<std::string> convert_std_list_char_to_std_vector_std_string(
            const std::list<char>& std_list_char,
            const uint32_t line_length)
    {
        std::vector<std::string> my_vector;
        std::string my_string;
        uint32_t remaining_characters_on_this_line = line_length;

        for (std::list<char>::const_iterator it = std_list_char.begin(); it != std_list_char.end(); it++)
        {
            if (remaining_characters_on_this_line == 0)
            {
                my_vector.push_back(my_string);
                my_string.clear();
                remaining_characters_on_this_line = line_length;
            }
            my_string.push_back(*it);
            remaining_characters_on_this_line--;
        }

        if (my_string.size() > 0)
        {
            my_vector.push_back(my_string);
        }

        return my_vector;
    }

    bool check_if_float_string(const std::string& my_string)
    {
        int32_t maximum_safe_length_for_float_string = 38;
        return check_if_floating_point_string(my_string, maximum_safe_length_for_float_string);
    }

    bool check_if_double_string(const std::string& my_string)
    {
        int32_t maximum_safe_length_for_double_string = 308;
        return check_if_floating_point_string(my_string, maximum_safe_length_for_double_string);
    }

    bool check_if_floating_point_string(const std::string& my_string, int32_t safe_number_of_chars)
    {
        int32_t n_chars = 0;

        if (my_string.empty())
        {
            return false;
        }

        bool is_dot_found = false;

        for (int32_t i = 0; i < my_string.length(); i++)
        {
            // Each of the characters must be one of the following:
            // 0123456789.-
            //
            // Besides, there may be only 1 dot. The dot may not be the last character.
            // Besides, there may be only 1 minus sign. The minus sign must be the first character.

            if (my_string.at(i) == '-' && i == 0)
            {
                // Minus sign is OK.
                continue;
            }

            if (my_string.at(i) == '.')
            {
                if (is_dot_found)
                {
                    // Two or more dots.
                    return false;
                }

                if (i == my_string.length() - 1)
                {
                    // Last character is dot.
                    return false;
                }

                // OK, decimal point here.
                is_dot_found = true;
                continue;
            }

            if (my_string.at(i) < '0' || my_string.at(i) > '9')
            {
                return false;
            }

            if (!is_dot_found)
            {
                if (++n_chars > safe_number_of_chars)
                {
                    // Too many characters, maximum safe number is 38 characters for float, 308 for double.
                    return false;
                }
            }
        }

        return true;
    }

    bool check_if_signed_integer_string(const std::string& my_string)
    {
        if (my_string.empty())
        {
            return false;
        }

        for (int32_t i = 0; i < my_string.length(); i++)
        {
            // Each of the characters must be one of the following:
            // 0123456789
            if (my_string.at(i) == '-' && i == 0)
            {
                // Minus sign is OK.
                continue;
            }

            if (my_string.at(i) < '0' || my_string.at(i) > '9')
            {
                return false;
            }
        }

        return true;
    }

    bool check_if_unsigned_integer_string(const std::string& my_string)
    {
        if (my_string.empty())
        {
            return false;
        }

        for (int32_t i = 0; i < my_string.length(); i++)
        {
            // Each of the characters must be one of the following:
            // 0123456789

            if (my_string.at(i) < '0' || my_string.at(i) > '9')
            {
                return false;
            }
        }

        return true;
    }

    void print_hexdump(const void* const start_address, const void* const end_address) // `begin` is inclusive, `end is exclusive.
    {
        void* void_start_address = const_cast<void*>(start_address);
        const int32_t line_width_in_bytes = 16;
        int32_t characters_on_this_line = 0;
        std::string current_line_ascii = "";
        std::string current_line_hex = "";

        for (uint8_t* data_pointer = static_cast<uint8_t*>(void_start_address); data_pointer < end_address; data_pointer++)
        {
            const uint8_t data_byte = static_cast<uint8_t>(*data_pointer);
            const char data_char = (data_byte >= 0x20 && data_byte <= 0x7f ? static_cast<char>(data_byte) : '.');
            current_line_ascii += data_char;

            uint32_t data_32_bit = static_cast<uint32_t>(data_byte); // to get the hexadecimal representation instead of the actual value.
            std::stringstream my_stream;
            my_stream << std::setfill('0') << std::setw(2) << std::hex << data_32_bit << std::dec; // std::hex does not work on char values.
            current_line_hex += my_stream.str();
            current_line_hex += " ";

            if (++characters_on_this_line >= line_width_in_bytes)
            {
                std::cout << current_line_hex << " " << current_line_ascii << "\n";
                current_line_hex = "";
                current_line_ascii = "";
                characters_on_this_line = 0;
            }
        }

        if (characters_on_this_line > 0)
        {
            const int32_t number_of_spaces_needed = (line_width_in_bytes - characters_on_this_line) * 3 + 1; // each byte's hexdump takes 3 characters.
            std::cout << current_line_hex << std::string(number_of_spaces_needed, ' ') << current_line_ascii << "\n";
        }

        std::cout << "\n";
    }
}
<|endoftext|>


<commit_before>class RegisterClassDetails
{
public:
private:
    BitSet _members;
};

class RegisterClass
{
public:
private:
    int _index;
};

class ConstantClassDetails
{
public:
    bool matches(int c)
    {
        if (c < _minimum || c > _maximum)
            return false;
        return (c - _minimum) % _step == 0;
    }
private:
    int _minimum;
    int _maximum;
    int _step;
};

class ConstantClass
{
public:
private:
    int _index;
};

class Register
{
public:
private:
    int _index;
};

class InstructionPattern
{
public:
    bool matches(Byte* data)
    {

    }
private:

    Array<RegisterClass> _registerHoles;
    Array<ConstantClass> _constantHoles;
};

class Instruction
{
public:
private:
    InstructionPattern* _patter;
    Array<Register> _registers;
    Array<int> _constants;
};

class MachineDescription
{
public:
    Array<InstructionPattern> _patterns;
    Array<RegisterClassDetails> _registerClassDetails;
    Array<ConstantClassDetails> _constantClassDetails;
};
<commit_msg>More machine descriptions<commit_after>class RegisterClassDetails
{
public:
private:
    BitSet _members;
};

class RegisterClass
{
public:
private:
    int _index;
};

class ConstantClassDetails
{
public:
    bool matches(int c)
    {
        if (c < _minimum || c > _maximum)
            return false;
        return (c - _minimum) % _step == 0;
    }
private:
    int _minimum;
    int _maximum;
    int _step;
};

class ConstantClass
{
public:
private:
    int _index;
};

class Register
{
public:
private:
    int _index;
};

class BinaryMatcher
{
public:
private:
};

class InstructionPattern
{
public:
    bool matches(Byte* data)
    {

    }
private:
    Array<BinaryMatcher> _matchers;
    Array<RegisterClass> _registerHoles;
    Array<ConstantClass> _constantHoles;
};

class Instruction
{
public:
private:
    InstructionPattern* _patter;
    Array<Register> _registers;
    Array<int> _constants;
};

class MachineDescription
{
public:
    Array<InstructionPattern> _patterns;
    Array<RegisterClassDetails> _registerClassDetails;
    Array<ConstantClassDetails> _constantClassDetails;
};
<|endoftext|>


<commit_before>

#include <stdio.h>
#include <ssn_log.h>
#include <ssn_port.h>
#include <ssn_sys.h>
#include <ssn_common.h>

#include <stdio.h>
#include <string>

#include <slankdev/exception.h>
#include <slankdev/util.h>
#include <slankdev/extra/dpdk.h>

#include <ssn_sys.h>
#include <ssn_log.h>
#include <ssn_ring.h>
#include <ssn_port.h>
#include <ssn_port_stat.h>
#include <ssn_timer.h>
#include <ssn_native_thread.h>
#include <ssn_green_thread.h>

#include "vnf.h"
#include "vty_config.h"
#include "rest_config.h"
#include "stageio.h"

constexpr uint16_t vty_port = 9999;
constexpr uint16_t rest_port = 8888;
constexpr size_t   required_nb_cores = 4;
constexpr size_t   green_thread_lcoreid = 1;
constexpr size_t   timer_lcoreid = 2;

void print(void* arg)
{
  vnf* v = reinterpret_cast<vnf*>(arg);
  while (true) {
    v->debug_dump(stdout);
    printf("-------------\n");
    ssn_sleep(1000);
    ssn_yield();
  }
}

void init(ssn_timer_sched* timer_sched, ssn_vty* vty, ssn_rest* rest, int argc, char** argv);
void fini(ssn_timer_sched* timer_sched, ssn_vty* vty, ssn_rest* rest);

/*
 * VNF IMPLEMENTATION BEGIN
 */
class func_wk : public func {
  bool run;
 public:
  stageio_rx* rx[2];
  stageio_tx* tx[2];
  virtual void poll_exe() override
  {
    ssn_log(SSN_LOG_INFO, "func_wk: INCLUDE DELAY\r\n");
    size_t nb_ports = ssn_dev_count();
    rte_mbuf* mbufs[32];
    while (run) {
      for (size_t p=0; p<nb_ports; p++) {
        size_t deqlen = rx[p]->rx_burst(mbufs, 32);
        for (size_t i=0; i<deqlen; i++) {
          // for (size_t j=0; j<100; j++) ; // DELAY
          for (size_t j=0; j<30; j++) ; // DELAY
          int ret = tx[p^1]->tx_shot(mbufs[i]);
          if (ret < 0) rte_pktmbuf_free(mbufs[i]);
        }
      }
    }
  }
  virtual void stop() override { run = false; }
};

class func_rx : public func {
  bool run;
 public:
  stageio_rx* rx[2];
  stageio_tx* tx[2];
  virtual void poll_exe() override
  {
    size_t nb_ports = ssn_dev_count();
    while (run) {
      for (size_t p=0; p<nb_ports; p++) {
        rte_mbuf* mbufs[32];
        size_t recvlen = rx[p]->rx_burst(mbufs, 32);
        if (recvlen == 0) continue;
        size_t enqlen = tx[p]->tx_burst(mbufs, recvlen);
        if (recvlen > enqlen) {
          slankdev::rte_pktmbuf_free_bulk(&mbufs[enqlen], recvlen-enqlen);
        }
      }
    }
  }
  virtual void stop() override { run = false; }
};

class func_tx : public func {
  bool run;
 public:
  stageio_rx* rx[2];
  stageio_tx* tx[2];
  virtual void poll_exe() override
  {
    size_t nb_ports = ssn_dev_count();
    while (run) {
      for (size_t p=0; p<nb_ports; p++) {
        rte_mbuf* mbufs[32];
        size_t deqlen = rx[p]->rx_burst(mbufs, 32);
        if (deqlen == 0) continue;
        printf("stop4 deqlen=%zd p=%zd\n", deqlen, p);
        size_t sendlen = tx[p]->tx_burst(mbufs, deqlen);
        printf("stop10\n");
        if (deqlen > sendlen) {
          slankdev::rte_pktmbuf_free_bulk(&mbufs[sendlen], deqlen-sendlen);
        }
      }
    }
  }
  virtual void stop() override { run = false; }
};

class stage_rx : public stage {
 public:
  stageio_rx_port rx[2];
  stageio_tx_ring tx[2];
  stage_rx(const char* n) : stage(n) {}
  virtual func* allocate() override
  {
    func_rx* f_rx = new func_rx;
    f_rx->rx[0] = &rx[0];
    f_rx->rx[1] = &rx[1];
    f_rx->tx[0] = &tx[0];
    f_rx->tx[1] = &tx[1];
    return f_rx;
  }
  virtual size_t throughput_pps() const override
  {
    size_t sum_rx_pps = 0;
    size_t nb_ports = ssn_dev_count();
    for (size_t i=0; i<nb_ports; i++) {
      sum_rx_pps += rx[i].rx_pps();
    }
    return sum_rx_pps;
  }
};

class stage_wk : public stage {
 public:
  stage_wk(const char* n) : stage(n) {}
  stageio_rx_ring rx[2];
  stageio_tx_ring tx[2];
  virtual func* allocate() override
  {
    func_wk* wk = new func_wk;
    wk->rx[0] = &rx[0];
    wk->rx[1] = &rx[1];
    wk->tx[0] = &tx[0];
    wk->tx[1] = &tx[1];
    return wk;
  }
  virtual size_t throughput_pps() const override
  {
    size_t sum_pps = 0;
    size_t nb_ports = ssn_dev_count();
    for (size_t i=0; i<nb_ports; i++) {
      sum_pps += rx[i].rx_pps();
    }
    return sum_pps;
  }
};

class stage_tx : public stage {
 public:
  stage_tx(const char* n) : stage(n) {}
  stageio_rx_ring rx[2];
  stageio_tx_port tx[2];

  virtual func* allocate() override
  {
    func_tx* f_tx = new func_tx;
    f_tx->rx[0] = &rx[0];
    f_tx->rx[1] = &rx[1];
    f_tx->tx[0] = &tx[0];
    f_tx->tx[1] = &tx[2];
    return f_tx;
  }
  virtual size_t throughput_pps() const override
  {
    size_t sum_pps = 0;
    size_t nb_ports = ssn_dev_count();
    for (size_t i=0; i<nb_ports; i++) {
      sum_pps += rx[i].rx_pps();
    }
    return sum_pps;
  }
};

class vnf_l2fwd : public vnf {
 public:
  ssn_ring* ring_prewk[2];
  ssn_ring* ring_poswk[2];
  vnf_l2fwd()
  {
    ring_prewk[0] = new ssn_ring("prewk0");
    ring_prewk[1] = new ssn_ring("prewk1");
    ring_poswk[0] = new ssn_ring("poswk0");
    ring_poswk[1] = new ssn_ring("poswk1");

    stage_rx* rx = new stage_rx("rx");
    rx->rx[0].set(0, 0);
    rx->rx[1].set(1, 0);
    rx->tx[0].set(ring_prewk[0]);
    rx->tx[1].set(ring_prewk[1]);

    stage_wk* wk = new stage_wk("wk");
    wk->rx[0].set(ring_prewk[0]);
    wk->rx[1].set(ring_prewk[1]);
    wk->tx[0].set(ring_poswk[0]);
    wk->tx[1].set(ring_poswk[1]);

    stage_tx* tx = new stage_tx("tx");
    tx->rx[0].set(ring_poswk[0]);
    tx->rx[1].set(ring_poswk[1]);
    tx->tx[0].set(0, 0);
    tx->tx[1].set(1, 0);

    stages.push_back(rx);
    stages.push_back(wk);
    stages.push_back(tx);
  }
};
/*
 * VNF IMPLEMENTATION END
 */

int main(int argc, char** argv)
{
  ssn_timer_sched* timer_sched;
  ssn_vty* vty;
  ssn_rest* rest;

  init(timer_sched, vty, rest, argc, argv);

  /* Init VNF */
  vnf_l2fwd* vnf1 = new vnf_l2fwd;
  vnf1->deploy();
  ssn_green_thread_launch(print, vnf1, green_thread_lcoreid);

  /* Loop */
  while (true) {
    char c = getchar();
    if (c == 'q') {
      printf("quit\n");
      break;
    }
    vnf1->tuneup();
  }

  fini(timer_sched, vty, rest);
}

void init(ssn_timer_sched* timer_sched, ssn_vty* vty, ssn_rest* rest, int argc, char** argv)
{
  ssn_log_set_level(SSN_LOG_EMERG);

  /*
   * Main Routine
   */
  ssn_init(argc, argv);
  if (ssn_lcore_count() <= required_nb_cores)
    throw slankdev::exception("cpu tarinai");

  /* Init Green Thread Scheduler */
  ssn_green_thread_sched_register(1);

  /* Init Timer Scheduler */
  timer_sched = new ssn_timer_sched(2);
  ssn_native_thread_launch(ssn_timer_sched_poll_thread, timer_sched, 2);
  uint64_t hz = ssn_timer_get_hz();
  timer_sched->add(new ssn_timer(ssn_port_stat_update, nullptr, hz));
  timer_sched->add(new ssn_timer(ssn_ring_getstats_timer_cb, nullptr, hz));

  /* Init VTY */
  vty = new ssn_vty(0x0, vty_port);
  vty->install_command(vtymt_slank()    , vtycb_slank    , nullptr);
  vty->install_command(vtymt_show_port(), vtycb_show_port, nullptr);
  vty->install_command(vtymt_show_ring(), vtycb_show_ring, nullptr );
  // vty->install_command(vtymt_show_vnf() , vtycb_show_vnf , &vnf0  );
  ssn_green_thread_launch(ssn_vty_poll_thread, vty, 1);

  /* Init REST API Server */
  rest = new ssn_rest(0x0, rest_port);
  rest->add_route("/"       , restcb_root  , nullptr);
  rest->add_route("/stats"  , restcb_stats , nullptr);
  rest->add_route("/author" , restcb_author, nullptr);
  ssn_green_thread_launch(ssn_rest_poll_thread, rest,1);

  /* Port Configuration */
  ssn_port_conf conf;
  size_t nb_ports = ssn_dev_count();
  for (size_t i=0; i<nb_ports; i++) {
    ssn_port_configure(i, &conf);
    ssn_port_dev_up(i);
    ssn_port_promisc_on(i);
  }
}

void fini(ssn_timer_sched* timer_sched, ssn_vty* vty, ssn_rest* rest)
{
  /*
   * Finilize
   */
  ssn_wait_all_lcore();
  ssn_green_thread_sched_unregister(1);
  delete timer_sched;
  ssn_wait_all_lcore();
  ssn_fin();
}
<commit_msg>Refact<commit_after>

#include <stdio.h>
#include <ssn_log.h>
#include <ssn_port.h>
#include <ssn_sys.h>
#include <ssn_common.h>

#include <stdio.h>
#include <string>

#include <slankdev/exception.h>
#include <slankdev/util.h>
#include <slankdev/extra/dpdk.h>

#include <ssn_sys.h>
#include <ssn_log.h>
#include <ssn_ring.h>
#include <ssn_port.h>
#include <ssn_port_stat.h>
#include <ssn_timer.h>
#include <ssn_native_thread.h>
#include <ssn_green_thread.h>

#include "vnf.h"
#include "vty_config.h"
#include "rest_config.h"
#include "stageio.h"

constexpr uint16_t vty_port = 9999;
constexpr uint16_t rest_port = 8888;
constexpr size_t   required_nb_cores = 4;
constexpr size_t   green_thread_lcoreid = 1;
constexpr size_t   timer_lcoreid = 2;

void print(void* arg)
{
  vnf* v = reinterpret_cast<vnf*>(arg);
  while (true) {
    v->debug_dump(stdout);
    printf("-------------\n");
    ssn_sleep(1000);
    ssn_yield();
  }
}

void init(ssn_timer_sched* timer_sched, ssn_vty* vty, ssn_rest* rest, int argc, char** argv);
void fini(ssn_timer_sched* timer_sched, ssn_vty* vty, ssn_rest* rest);

/*
 * VNF IMPLEMENTATION BEGIN
 */
class func_wk : public func {
  bool run;
 public:
  stageio_rx_ring rx[2];
  stageio_tx_ring tx[2];
  virtual void poll_exe() override
  {
    ssn_log(SSN_LOG_INFO, "func_wk: INCLUDE DELAY\r\n");
    size_t nb_ports = ssn_dev_count();
    rte_mbuf* mbufs[32];
    while (run) {
      for (size_t p=0; p<nb_ports; p++) {
        size_t deqlen = rx[p].rx_burst(mbufs, 32);
        for (size_t i=0; i<deqlen; i++) {
          // for (size_t j=0; j<100; j++) ; // DELAY
          for (size_t j=0; j<30; j++) ; // DELAY
          int ret = tx[p^1].tx_shot(mbufs[i]);
          if (ret < 0) rte_pktmbuf_free(mbufs[i]);
        }
      }
    }
  }
  virtual void stop() override { run = false; }
};

class func_rx : public func {
  bool run;
 public:
  stageio_rx_port rx[2];
  stageio_tx_ring tx[2];
  virtual void poll_exe() override
  {
    size_t nb_ports = ssn_dev_count();
    while (run) {
      for (size_t p=0; p<nb_ports; p++) {
        rte_mbuf* mbufs[32];
        size_t recvlen = rx[p].rx_burst(mbufs, 32);
        if (recvlen == 0) continue;
        size_t enqlen = tx[p].tx_burst(mbufs, recvlen);
        if (recvlen > enqlen) {
          slankdev::rte_pktmbuf_free_bulk(&mbufs[enqlen], recvlen-enqlen);
        }
      }
    }
  }
  virtual void stop() override { run = false; }
};

class func_tx : public func {
  bool run;
 public:
  stageio_rx_ring rx[2];
  stageio_tx_port tx[2];
  virtual void poll_exe() override
  {
    size_t nb_ports = ssn_dev_count();
    while (run) {
      for (size_t p=0; p<nb_ports; p++) {
        rte_mbuf* mbufs[32];
        size_t deqlen = rx[p].rx_burst(mbufs, 32);
        if (deqlen == 0) continue;
        printf("stop4 deqlen=%zd p=%zd\n", deqlen, p);
        size_t sendlen = tx[p].tx_burst(mbufs, deqlen);
        printf("stop10\n");
        if (deqlen > sendlen) {
          slankdev::rte_pktmbuf_free_bulk(&mbufs[sendlen], deqlen-sendlen);
        }
      }
    }
  }
  virtual void stop() override { run = false; }
};

class stage_rx : public stage {
 public:
  stageio_rx_port rx[2];
  stageio_tx_ring tx[2];
  stage_rx(const char* n) : stage(n) {}
  virtual func* allocate() override
  {
    func_rx* f_rx = new func_rx;
    f_rx->rx[0] = rx[0];
    f_rx->rx[1] = rx[1];
    f_rx->tx[0] = tx[0];
    f_rx->tx[1] = tx[1];
    return f_rx;
  }
  virtual size_t throughput_pps() const override
  {
    size_t sum_pps = 0;
    size_t nb_ports = ssn_dev_count();
    for (size_t i=0; i<nb_ports; i++) {
      sum_pps += rx[i].rx_pps();
    }
    return sum_pps;
  }
};

class stage_wk : public stage {
 public:
  stage_wk(const char* n) : stage(n) {}
  stageio_rx_ring rx[2];
  stageio_tx_ring tx[2];
  virtual func* allocate() override
  {
    func_wk* wk = new func_wk;
    wk->rx[0] = rx[0];
    wk->rx[1] = rx[1];
    wk->tx[0] = tx[0];
    wk->tx[1] = tx[1];
    return wk;
  }
  virtual size_t throughput_pps() const override
  {
    size_t sum_pps = 0;
    size_t nb_ports = ssn_dev_count();
    for (size_t i=0; i<nb_ports; i++) {
      sum_pps += rx[i].rx_pps();
    }
    return sum_pps;
  }
};

class stage_tx : public stage {
 public:
  stage_tx(const char* n) : stage(n) {}
  stageio_rx_ring rx[2];
  stageio_tx_port tx[2];

  virtual func* allocate() override
  {
    func_tx* f_tx = new func_tx;
    f_tx->rx[0] = rx[0];
    f_tx->rx[1] = rx[1];
    f_tx->tx[0] = tx[0];
    f_tx->tx[1] = tx[2];
    return f_tx;
  }
  virtual size_t throughput_pps() const override
  {
    size_t sum_pps = 0;
    size_t nb_ports = ssn_dev_count();
    for (size_t i=0; i<nb_ports; i++) {
      sum_pps += rx[i].rx_pps();
    }
    return sum_pps;
  }
};

class vnf_l2fwd : public vnf {
 public:
  ssn_ring* ring_prewk[2];
  ssn_ring* ring_poswk[2];
  vnf_l2fwd()
  {
    ring_prewk[0] = new ssn_ring("prewk0");
    ring_prewk[1] = new ssn_ring("prewk1");
    ring_poswk[0] = new ssn_ring("poswk0");
    ring_poswk[1] = new ssn_ring("poswk1");

    stage_rx* rx = new stage_rx("rx");
    rx->rx[0].set(0, 0);
    rx->rx[1].set(1, 0);
    rx->tx[0].set(ring_prewk[0]);
    rx->tx[1].set(ring_prewk[1]);

    stage_wk* wk = new stage_wk("wk");
    wk->rx[0].set(ring_prewk[0]);
    wk->rx[1].set(ring_prewk[1]);
    wk->tx[0].set(ring_poswk[0]);
    wk->tx[1].set(ring_poswk[1]);

    stage_tx* tx = new stage_tx("tx");
    tx->rx[0].set(ring_poswk[0]);
    tx->rx[1].set(ring_poswk[1]);
    tx->tx[0].set(0, 0);
    tx->tx[1].set(1, 0);

    stages.push_back(rx);
    stages.push_back(wk);
    stages.push_back(tx);
  }
};
/*
 * VNF IMPLEMENTATION END
 */

int main(int argc, char** argv)
{
  ssn_timer_sched* timer_sched;
  ssn_vty* vty;
  ssn_rest* rest;

  init(timer_sched, vty, rest, argc, argv);

  /* Init VNF */
  vnf_l2fwd* vnf1 = new vnf_l2fwd;
  vnf1->deploy();
  ssn_green_thread_launch(print, vnf1, green_thread_lcoreid);

  /* Loop */
  while (true) {
    char c = getchar();
    if (c == 'q') {
      printf("quit\n");
      break;
    }
    vnf1->tuneup();
  }

  fini(timer_sched, vty, rest);
}

void init(ssn_timer_sched* timer_sched, ssn_vty* vty, ssn_rest* rest, int argc, char** argv)
{
  ssn_log_set_level(SSN_LOG_EMERG);

  /*
   * Main Routine
   */
  ssn_init(argc, argv);
  if (ssn_lcore_count() <= required_nb_cores)
    throw slankdev::exception("cpu tarinai");

  /* Init Green Thread Scheduler */
  ssn_green_thread_sched_register(1);

  /* Init Timer Scheduler */
  timer_sched = new ssn_timer_sched(2);
  ssn_native_thread_launch(ssn_timer_sched_poll_thread, timer_sched, 2);
  uint64_t hz = ssn_timer_get_hz();
  timer_sched->add(new ssn_timer(ssn_port_stat_update, nullptr, hz));
  timer_sched->add(new ssn_timer(ssn_ring_getstats_timer_cb, nullptr, hz));

  /* Init VTY */
  vty = new ssn_vty(0x0, vty_port);
  vty->install_command(vtymt_slank()    , vtycb_slank    , nullptr);
  vty->install_command(vtymt_show_port(), vtycb_show_port, nullptr);
  vty->install_command(vtymt_show_ring(), vtycb_show_ring, nullptr );
  // vty->install_command(vtymt_show_vnf() , vtycb_show_vnf , &vnf0  );
  ssn_green_thread_launch(ssn_vty_poll_thread, vty, 1);

  /* Init REST API Server */
  rest = new ssn_rest(0x0, rest_port);
  rest->add_route("/"       , restcb_root  , nullptr);
  rest->add_route("/stats"  , restcb_stats , nullptr);
  rest->add_route("/author" , restcb_author, nullptr);
  ssn_green_thread_launch(ssn_rest_poll_thread, rest,1);

  /* Port Configuration */
  ssn_port_conf conf;
  size_t nb_ports = ssn_dev_count();
  for (size_t i=0; i<nb_ports; i++) {
    ssn_port_configure(i, &conf);
    ssn_port_dev_up(i);
    ssn_port_promisc_on(i);
  }
}

void fini(ssn_timer_sched* timer_sched, ssn_vty* vty, ssn_rest* rest)
{
  /*
   * Finilize
   */
  ssn_wait_all_lcore();
  ssn_green_thread_sched_unregister(1);
  delete timer_sched;
  ssn_wait_all_lcore();
  ssn_fin();
}
<|endoftext|>


<commit_before>#ifndef HMLIB_EXCEPTIONS_EXCEPTION_INC
#define HMLIB_EXCEPTIONS_EXCEPTION_INC 100
#
#ifndef HMLIB_CONFIG_INC
#	include "../hmLib_config.h"
#endif
/*===exceptions::exception===
基本的なマクロ宣言
*/
#include"../static_buffer.hpp"
#include <exception>
#include <string>

namespace hmLib{
	namespace exceptions{
		namespace details{
			struct exception_identifier{};
			using buffer = static_buffer<char, 4096, exception_identifier>;
		}
		struct exception : public std::exception{
		private:
			details::buffer Buffer;
		public:
			exception()noexcept{}
			explicit exception(const char* What_)noexcept{
				auto Itr = Buffer.begin();
				while(*What_ != 0 && Itr != Buffer.end()){
					*(Itr++) = *(What_++);
				}
			}
			exception(const exception&) = default;
			exception& operator=(const exception&) = default;
		public:
			const char* what() const noexcept override{ return Buffer.cbegin(); }
		};
		template<typename identifier_,typename base_type_=exception>
		struct exception_pattern :public base_type_ {
			typedef base_type_ base_type;
		public:
			explicit exception_pattern(const std::string& What_)noexcept :base_type(What_.c_str()) {}
			explicit exception_pattern(const char* What_)noexcept :base_type(What_) {}
		};
	}
}
#ifndef  HMLIB_NOEXCEPT
#	define _hmLib_exceptions_to_str(num) #num
#	define hmLib_exceptions_to_str(num) _hmLib_exceptions_to_str(num)
#	define hmLib_throwformat(str) (std::string(__FILE__)+"("+hmLib_exceptions_to_str(__LINE__)+") in "+__FUNCTION__+": "+str)
#	define hmLib_throw(except,str) throw except((hmLib_throwformat(str)).c_str())
#	define hmLib_thrownext(except,nextexcept,str) throw nextexcept((std::string(except.what())+"\n"+hmLib_throwformat(str)).c_str())
#	define hmLib_assert(condition,except,str) if(!(condition))hmLib_throw(except,str)
#	define hmLib_assert_any(condition,str) hmLib_assert(condition,hmLib::exceptions::exception,str)
#else
#	define _hmLib_exceptions_to_str(num)
#	define hmLib_exceptions_to_str(num)
#	define hmLib_throwformat(str)
#	define hmLib_throw(except,str)
#	define hmLib_thrownext(except,nextexcept,str)
#	define hmLib_assert(condition,except,str)
#	define hmLib_assert_any(condition,str)
#endif

#
#endif
<commit_msg>hmLib_nothrow is now disable only assert.<commit_after>#ifndef HMLIB_EXCEPTIONS_EXCEPTION_INC
#define HMLIB_EXCEPTIONS_EXCEPTION_INC 100
#
#ifndef HMLIB_CONFIG_INC
#	include "../hmLib_config.h"
#endif
/*===exceptions::exception===
基本的なマクロ宣言
*/
#include"../static_buffer.hpp"
#include <exception>
#include <string>

namespace hmLib{
	namespace exceptions{
		namespace details{
			struct exception_identifier{};
			using buffer = static_buffer<char, 4096, exception_identifier>;
		}
		struct exception : public std::exception{
		private:
			details::buffer Buffer;
		public:
			exception()noexcept{}
			explicit exception(const char* What_)noexcept{
				auto Itr = Buffer.begin();
				while(*What_ != 0 && Itr != Buffer.end()){
					*(Itr++) = *(What_++);
				}
			}
			exception(const exception&) = default;
			exception& operator=(const exception&) = default;
		public:
			const char* what() const noexcept override{ return Buffer.cbegin(); }
		};
		template<typename identifier_,typename base_type_=exception>
		struct exception_pattern :public base_type_ {
			typedef base_type_ base_type;
		public:
			explicit exception_pattern(const std::string& What_)noexcept :base_type(What_.c_str()) {}
			explicit exception_pattern(const char* What_)noexcept :base_type(What_) {}
		};
	}
}
#define _hmLib_exceptions_to_str(num) #num
#define hmLib_exceptions_to_str(num) _hmLib_exceptions_to_str(num)
#define hmLib_throwformat(str) (std::string(__FILE__)+"("+hmLib_exceptions_to_str(__LINE__)+") in "+__FUNCTION__+": "+str)
#define hmLib_throw(except,str) throw except((hmLib_throwformat(str)).c_str())
#define hmLib_thrownext(except,nextexcept,str) throw nextexcept((std::string(except.what())+"\n"+hmLib_throwformat(str)).c_str())
#ifndef  HMLIB_NOEXCEPT
#	define hmLib_assert(condition,except,str) if(!(condition))hmLib_throw(except,str)
#	define hmLib_assert_any(condition,str) hmLib_assert(condition,hmLib::exceptions::exception,str)
#else
#	define hmLib_assert(condition,except,str)
#	define hmLib_assert_any(condition,str)
#endif

#
#endif
<|endoftext|>


<commit_before>// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.h"

#include <Eigen/Core>

#include <fstream>

#include "SurgSim/DataStructures/DataGroupBuilder.h"
#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/SharedInstance.h"

namespace
{
/// Missing istream "operator >>" for Eigen matrix type
/// http://eigen.tuxfamily.org/bz/show_bug.cgi?id=622
template<typename Derived>
std::istream& operator >>(std::istream& s, Eigen::MatrixBase<Derived>& m)
{
	for (int i = 0; i < m.rows(); ++i)
	{
		for (int j = 0; j < m.cols(); j++)
		{
			s >> m(i, j);
		}
	}
	return s;
}
}

namespace SurgSim
{
namespace Devices
{

class ReplayPoseDevice;

ReplayPoseScaffold::ReplayPoseScaffold() :
	Framework::BasicThread("ReplayPoseScaffold"),
	m_logger(Framework::Logger::getLogger("Devices/ReplayPoseScaffold"))
{
	SURGSIM_LOG_DEBUG(m_logger) << "ReplayPose scaffold created.";
}

ReplayPoseScaffold::~ReplayPoseScaffold()
{
	if (m_device != nullptr)
	{
		unregisterDevice();
	}
	SURGSIM_LOG_DEBUG(m_logger) << "ReplayPose scaffold destroyed.";
}

std::shared_ptr<ReplayPoseScaffold> ReplayPoseScaffold::getOrCreateSharedInstance()
{
	static Framework::SharedInstance<ReplayPoseScaffold> sharedInstance;
	return sharedInstance.get();
}

bool ReplayPoseScaffold::doInitialize() { return true; }

bool ReplayPoseScaffold::doStartUp() { return true; }

bool ReplayPoseScaffold::doUpdate(double dt)
{
	SURGSIM_ASSERT(m_device != nullptr) << "DeviceData not properly allocated";

	return updateDevice(m_device.get());
}

struct ReplayPoseScaffold::DeviceData
{
	/// Constructor
	/// \param device Device to be managed by this scaffold
	explicit DeviceData(ReplayPoseDevice* device) : deviceObject(device), m_timestamp(0), m_index(0), m_fileLoaded(false)
	{
		m_fileLoaded = loadFile(deviceObject->getFileName());
		m_timer.start();
	}

	/// Retrieve the pose for the given time stamp "m_timestamp"
	/// \return The requested pose, Identity if none could be loaded
	Math::RigidTransform3d getPose()
	{
		if (m_motion.size() == 0)
		{
			return Math::RigidTransform3d::Identity();
		}

		if (m_motion.size() == 1 || (m_index == 0 && m_timestamp <= m_motion[0].first))
		{
			return m_motion[0].second;
		}

		while (m_motion.size() > m_index && m_timestamp > m_motion[m_index].first)
		{
			m_index++;
		}

		// Requesting a timestamp over the higher limit recorded in the file
		if (m_motion.size() <= m_index)
		{
			return m_motion[m_motion.size() - 1].second;
		}

		// Are we requesting exactly the timestamp we just indexed
		if (m_timestamp == m_motion[m_index].first)
		{
			return m_motion[m_index].second;
		}

		// Otherwise, we need to interpolate between the index and the previous index
		double range = m_motion[m_index].first - m_motion[m_index - 1].first;
		double time = (m_timestamp - m_motion[m_index - 1].first) / range;
		return Math::interpolate(m_motion[m_index - 1].second, m_motion[m_index].second, time);
	}

	/// Device object managed by this scaffold.
	ReplayPoseDevice* const deviceObject;

	/// Timer to keep track of the time stamp and keep the replay in sync with the recording
	Framework::Timer m_timer;

	/// Time stamp for the device (which time stamp are we reproducing?)
	double m_timestamp;

	/// Series of poses through time loaded from the input file
	std::vector<std::pair<double, Math::RigidTransform3d>> m_motion;

	/// Index of the latest pose used
	size_t m_index;

	/// Valid file loaded successfully
	bool m_fileLoaded;

private:
	// Prevent copy construction and copy assignment.  (VS2012 does not support "= delete" yet.)
	DeviceData(const DeviceData&) /*= delete*/;
	DeviceData& operator=(const DeviceData&) /*= delete*/;

	/// \param fileName the file to be open and loaded
	/// \return True if the file was loaded successfully, False otherwise
	bool loadFile(const std::string& fileName)
	{
		auto logger = Framework::Logger::getLogger("Devices/ReplayPoseScaffold");

		std::ifstream inputFile;
		bool result = true;

		inputFile.open(fileName, std::ios::in);

		if (!inputFile.is_open())
		{
			SURGSIM_LOG_WARNING(logger) << "Could not find or open the file " << fileName <<
				"; Replay will use Identity pose";
			result = false;
		}
		else
		{
			while (!inputFile.eof())
			{
				double timeStamp;
				Math::RigidTransform3d pose;
				inputFile >> timeStamp >> pose.matrix();
				m_motion.emplace_back(timeStamp, pose);
			}
			SURGSIM_LOG_INFO(logger) << "Loaded " << m_motion.size() << " timestamps";
			if (m_motion.size() >= 1)
			{
				SURGSIM_LOG_INFO(logger) << "The loaded timestamps cover a range of " <<
					m_motion[m_motion.size() - 1].first - m_motion[0].first << " second(s)";
			}
			SURGSIM_LOG_IF(m_motion.size() == 0, logger, WARNING) <<
				"No poses could be properly loaded, Identity pose will be used";

			inputFile.close();
		}

		return result;
	}
};

bool ReplayPoseScaffold::registerDevice(ReplayPoseDevice* device)
{
	SURGSIM_ASSERT(m_device == nullptr) << "Can't register two ReplayPoseDevice.";

	m_device.reset(new ReplayPoseScaffold::DeviceData(device));
	if (m_device == nullptr)
	{
		SURGSIM_LOG_CRITICAL(m_logger) << "Failed to create a DeviceData";
		return false;
	}
	if (!m_device->m_fileLoaded)
	{
		SURGSIM_LOG_CRITICAL(m_logger) << "Failed to load the file to replay";
		return false;
	}
	SURGSIM_LOG_DEBUG(m_logger) << "Registered device " << device->getName();

	bool success = true;
	if (!isRunning())
	{
		std::shared_ptr<Framework::Barrier> barrier = std::make_shared<Framework::Barrier>(2);
		start(barrier);
		barrier->wait(true); // Wait for initialize
		barrier->wait(true); // Wait for startup
		success = isInitialized();
	}

	if (success)
	{
		setRate(device->getRate());
	}

	return success;
}

bool ReplayPoseScaffold::unregisterDevice()
{
	// #threadsafety After unregistering, another thread could be in the process of registering.
	if (isRunning())
	{
		stop();
	}

	m_device.reset();
	if (m_device == nullptr)
	{
		SURGSIM_LOG_DEBUG(m_logger) << "Unregistered device";
		return true;
	}
	SURGSIM_LOG_DEBUG(m_logger) << "There is no device to unregister.";

	return false;
}

bool ReplayPoseScaffold::updateDevice(ReplayPoseScaffold::DeviceData* info)
{
	DataStructures::DataGroup& inputData = m_device->deviceObject->getInputData();

	info->m_timer.markFrame();
	info->m_timestamp += info->m_timer.getLastFramePeriod();

	Math::RigidTransform3d pose = info->getPose();
	inputData.poses().set(DataStructures::Names::POSE, pose);

	m_device->deviceObject->pushInput();

	return true;
}

DataStructures::DataGroup ReplayPoseScaffold::buildDeviceInputData()
{
	DataStructures::DataGroupBuilder builder;
	builder.addPose(DataStructures::Names::POSE);
	return builder.createData();
}

};  // namespace Devices
};  // namespace SurgSim
<commit_msg>Clean up<commit_after>// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.h"

#include <Eigen/Core>

#include <fstream>

#include "SurgSim/DataStructures/DataGroupBuilder.h"
#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/SharedInstance.h"

namespace
{
/// Missing istream "operator >>" for Eigen matrix type
/// http://eigen.tuxfamily.org/bz/show_bug.cgi?id=622
template<typename Derived>
std::istream& operator >>(std::istream& s, Eigen::MatrixBase<Derived>& m)
{
	for (int i = 0; i < m.rows(); ++i)
	{
		for (int j = 0; j < m.cols(); j++)
		{
			s >> m(i, j);
		}
	}
	return s;
}
}

namespace SurgSim
{
namespace Devices
{

ReplayPoseScaffold::ReplayPoseScaffold() :
	Framework::BasicThread("ReplayPoseScaffold"),
	m_logger(Framework::Logger::getLogger("Devices/ReplayPoseScaffold"))
{
	SURGSIM_LOG_DEBUG(m_logger) << "ReplayPose scaffold created.";
}

ReplayPoseScaffold::~ReplayPoseScaffold()
{
	if (m_device != nullptr)
	{
		unregisterDevice();
	}
	SURGSIM_LOG_DEBUG(m_logger) << "ReplayPose scaffold destroyed.";
}

std::shared_ptr<ReplayPoseScaffold> ReplayPoseScaffold::getOrCreateSharedInstance()
{
	static Framework::SharedInstance<ReplayPoseScaffold> sharedInstance;
	return sharedInstance.get();
}

bool ReplayPoseScaffold::doInitialize() { return true; }

bool ReplayPoseScaffold::doStartUp() { return true; }

bool ReplayPoseScaffold::doUpdate(double dt)
{
	SURGSIM_ASSERT(m_device != nullptr) << "DeviceData not properly allocated";

	return updateDevice(m_device.get());
}

struct ReplayPoseScaffold::DeviceData
{
	/// Constructor
	/// \param device Device to be managed by this scaffold
	explicit DeviceData(ReplayPoseDevice* device) : deviceObject(device), m_timestamp(0), m_index(0), m_fileLoaded(false)
	{
		m_fileLoaded = loadFile(deviceObject->getFileName());
		m_timer.start();
	}

	/// Retrieve the pose for the given time stamp "m_timestamp"
	/// \return The requested pose, Identity if none could be loaded
	Math::RigidTransform3d getPose()
	{
		if (m_motion.size() == 0)
		{
			return Math::RigidTransform3d::Identity();
		}

		if (m_motion.size() == 1 || (m_index == 0 && m_timestamp <= m_motion[0].first))
		{
			return m_motion[0].second;
		}

		while (m_motion.size() > m_index && m_timestamp > m_motion[m_index].first)
		{
			m_index++;
		}

		// Requesting a timestamp over the higher limit recorded in the file
		if (m_motion.size() <= m_index)
		{
			return m_motion[m_motion.size() - 1].second;
		}

		// Are we requesting exactly the timestamp we just indexed
		if (m_timestamp == m_motion[m_index].first)
		{
			return m_motion[m_index].second;
		}

		// Otherwise, we need to interpolate between the index and the previous index
		double range = m_motion[m_index].first - m_motion[m_index - 1].first;
		double time = (m_timestamp - m_motion[m_index - 1].first) / range;
		return Math::interpolate(m_motion[m_index - 1].second, m_motion[m_index].second, time);
	}

	/// Device object managed by this scaffold.
	ReplayPoseDevice* const deviceObject;

	/// Timer to keep track of the time stamp and keep the replay in sync with the recording
	Framework::Timer m_timer;

	/// Time stamp for the device (which time stamp are we reproducing?)
	double m_timestamp;

	/// Series of poses through time loaded from the input file
	std::vector<std::pair<double, Math::RigidTransform3d>> m_motion;

	/// Index of the latest pose used
	size_t m_index;

	/// Valid file loaded successfully
	bool m_fileLoaded;

private:
	// Prevent copy construction and copy assignment.  (VS2012 does not support "= delete" yet.)
	DeviceData(const DeviceData&) /*= delete*/;
	DeviceData& operator=(const DeviceData&) /*= delete*/;

	/// \param fileName the file to be open and loaded
	/// \return True if the file was loaded successfully, False otherwise
	bool loadFile(const std::string& fileName)
	{
		auto logger = Framework::Logger::getLogger("Devices/ReplayPoseScaffold");

		std::ifstream inputFile;
		bool result = true;

		inputFile.open(fileName, std::ios::in);

		if (!inputFile.is_open())
		{
			SURGSIM_LOG_WARNING(logger) << "Could not find or open the file " << fileName <<
				"; Replay will use Identity pose";
			result = false;
		}
		else
		{
			while (!inputFile.eof())
			{
				double timeStamp;
				Math::RigidTransform3d pose;
				inputFile >> timeStamp >> pose.matrix();
				m_motion.emplace_back(timeStamp, pose);
			}
			SURGSIM_LOG_INFO(logger) << "Loaded " << m_motion.size() << " timestamps";
			if (m_motion.size() >= 1)
			{
				SURGSIM_LOG_INFO(logger) << "The loaded timestamps cover a range of " <<
					m_motion[m_motion.size() - 1].first - m_motion[0].first << " second(s)";
			}
			SURGSIM_LOG_IF(m_motion.size() == 0, logger, WARNING) <<
				"No poses could be properly loaded, Identity pose will be used";

			inputFile.close();
		}

		return result;
	}
};

bool ReplayPoseScaffold::registerDevice(ReplayPoseDevice* device)
{
	SURGSIM_ASSERT(m_device == nullptr) << "Can't register two ReplayPoseDevice.";

	m_device.reset(new ReplayPoseScaffold::DeviceData(device));
	if (m_device == nullptr)
	{
		SURGSIM_LOG_CRITICAL(m_logger) << "Failed to create a DeviceData";
		return false;
	}
	if (!m_device->m_fileLoaded)
	{
		SURGSIM_LOG_CRITICAL(m_logger) << "Failed to load the file to replay";
		return false;
	}
	SURGSIM_LOG_DEBUG(m_logger) << "Registered device " << device->getName();

	bool success = true;
	if (!isRunning())
	{
		std::shared_ptr<Framework::Barrier> barrier = std::make_shared<Framework::Barrier>(2);
		start(barrier);
		barrier->wait(true); // Wait for initialize
		barrier->wait(true); // Wait for startup
		success = isInitialized();
	}

	if (success)
	{
		setRate(device->getRate());
	}

	return success;
}

bool ReplayPoseScaffold::unregisterDevice()
{
	// #threadsafety After unregistering, another thread could be in the process of registering.
	if (isRunning())
	{
		stop();
	}

	m_device.reset();
	SURGSIM_LOG_DEBUG(m_logger) << "Unregistered device";
	return true;
}

bool ReplayPoseScaffold::updateDevice(ReplayPoseScaffold::DeviceData* info)
{
	DataStructures::DataGroup& inputData = m_device->deviceObject->getInputData();

	info->m_timer.markFrame();
	info->m_timestamp += info->m_timer.getLastFramePeriod();

	Math::RigidTransform3d pose = info->getPose();
	inputData.poses().set(DataStructures::Names::POSE, pose);

	m_device->deviceObject->pushInput();

	return true;
}

DataStructures::DataGroup ReplayPoseScaffold::buildDeviceInputData()
{
	DataStructures::DataGroupBuilder builder;
	builder.addPose(DataStructures::Names::POSE);
	return builder.createData();
}

};  // namespace Devices
};  // namespace SurgSim
<|endoftext|>


<commit_before>// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.h"

#include <Eigen/Core>

#include <fstream>

#include "SurgSim/DataStructures/DataGroupBuilder.h"
#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/SharedInstance.h"

namespace
{
/// Missing istream "operator >>" for Eigen matrix type
/// http://eigen.tuxfamily.org/bz/show_bug.cgi?id=622
template<typename Derived>
std::istream& operator >>(std::istream& s, Eigen::MatrixBase<Derived>& m)
{
	for (int i = 0; i < m.rows(); ++i)
	{
		for (int j = 0; j < m.cols(); j++)
		{
			s >> m(i, j);
		}
	}
	return s;
}
}

namespace SurgSim
{
namespace Devices
{

ReplayPoseScaffold::ReplayPoseScaffold() :
	Framework::BasicThread("ReplayPoseScaffold"),
	m_logger(Framework::Logger::getLogger("Devices/ReplayPoseScaffold"))
{
	SURGSIM_LOG_DEBUG(m_logger) << "ReplayPose scaffold created.";
}

ReplayPoseScaffold::~ReplayPoseScaffold()
{
	if (m_device != nullptr)
	{
		unregisterDevice();
	}
	SURGSIM_LOG_DEBUG(m_logger) << "ReplayPose scaffold destroyed.";
}

std::shared_ptr<ReplayPoseScaffold> ReplayPoseScaffold::getOrCreateSharedInstance()
{
	static Framework::SharedInstance<ReplayPoseScaffold> sharedInstance;
	return sharedInstance.get();
}

bool ReplayPoseScaffold::doInitialize()
{
	return true;
}

bool ReplayPoseScaffold::doStartUp()
{
	return true;
}

bool ReplayPoseScaffold::doUpdate(double dt)
{
	boost::unique_lock<boost::mutex> scopedLock(m_deviceLock);

	SURGSIM_ASSERT(m_device != nullptr) << "DeviceData not properly allocated";

	return updateDevice(m_device.get());
}

struct ReplayPoseScaffold::DeviceData
{
	/// Constructor
	/// \param device Device to be managed by this scaffold
	explicit DeviceData(ReplayPoseDevice* device) :
		deviceObject(device),
		m_timestamp(0),
		m_index(0),
		m_fileLoaded(false)
	{
		m_fileLoaded = loadFile(deviceObject->getFileName());
		m_timer.start();
	}

	/// Retrieve the pose for the given time stamp "m_timestamp"
	/// \return The requested pose, Identity if none could be loaded
	Math::RigidTransform3d getPose()
	{
		if (m_motion.size() == 0)
		{
			return Math::RigidTransform3d::Identity();
		}

		if (m_motion.size() == 1 || (m_index == 0 && m_timestamp <= m_motion[0].first))
		{
			return m_motion[0].second;
		}

		while (m_motion.size() > m_index && m_timestamp > m_motion[m_index].first)
		{
			m_index++;
		}

		// Requesting a timestamp over the higher limit recorded in the file
		if (m_motion.size() <= m_index)
		{
			return m_motion[m_motion.size() - 1].second;
		}

		// Are we requesting exactly the timestamp we just indexed
		if (m_timestamp == m_motion[m_index].first)
		{
			return m_motion[m_index].second;
		}

		// Otherwise, we need to interpolate between the index and the previous index
		double range = m_motion[m_index].first - m_motion[m_index - 1].first;
		double time = (m_timestamp - m_motion[m_index - 1].first) / range;
		return Math::interpolate(m_motion[m_index - 1].second, m_motion[m_index].second, time);
	}

	/// Device object managed by this scaffold.
	ReplayPoseDevice* const deviceObject;

	/// Timer to keep track of the time stamp and keep the replay in sync with the recording
	Framework::Timer m_timer;

	/// Time stamp for the device (which time stamp are we reproducing?)
	double m_timestamp;

	/// Series of poses through time loaded from the input file
	std::vector<std::pair<double, Math::RigidTransform3d>> m_motion;

	/// Index of the latest pose used
	size_t m_index;

	/// Valid file loaded successfully
	bool m_fileLoaded;

private:
	// Prevent copy construction and copy assignment.  (VS2012 does not support "= delete" yet.)
	DeviceData(const DeviceData&) /*= delete*/;
	DeviceData& operator=(const DeviceData&) /*= delete*/;

	/// \param fileName the file to be open and loaded
	/// \return True if the file was loaded successfully, False otherwise
	bool loadFile(const std::string& fileName)
	{
		auto logger = Framework::Logger::getLogger("Devices/ReplayPoseScaffold");

		std::ifstream inputFile;
		bool result = true;

		inputFile.open(fileName, std::ios::in);

		if (!inputFile.is_open())
		{
			SURGSIM_LOG_WARNING(logger) << "Could not find or open the file " << fileName <<
										"; Replay will use Identity pose";
			result = false;
		}
		else
		{
			while (!inputFile.eof())
			{
				double timeStamp;
				Math::RigidTransform3d pose;
				inputFile >> timeStamp >> pose.matrix();
				m_motion.emplace_back(timeStamp, pose);
			}
			SURGSIM_LOG_INFO(logger) << "Loaded " << m_motion.size() << " timestamps";
			if (m_motion.size() >= 1)
			{
				SURGSIM_LOG_INFO(logger) << "The loaded timestamps cover a range of " <<
										 m_motion[m_motion.size() - 1].first - m_motion[0].first << " second(s)";
			}
			SURGSIM_LOG_IF(m_motion.size() == 0, logger, WARNING) <<
					"No poses could be properly loaded, Identity pose will be used";

			inputFile.close();
		}

		return result;
	}
};

bool ReplayPoseScaffold::registerDevice(ReplayPoseDevice* device)
{
	SURGSIM_ASSERT(m_device == nullptr) << "Can't register two ReplayPoseDevice.";

	m_device.reset(new ReplayPoseScaffold::DeviceData(device));
	if (m_device == nullptr)
	{
		SURGSIM_LOG_CRITICAL(m_logger) << "Failed to create a DeviceData";
		return false;
	}
	if (!m_device->m_fileLoaded)
	{
		SURGSIM_LOG_CRITICAL(m_logger) << "Failed to load the file to replay";
		return false;
	}
	SURGSIM_LOG_DEBUG(m_logger) << "Registered device " << device->getName();

	bool success = true;
	if (!isRunning())
	{
		std::shared_ptr<Framework::Barrier> barrier = std::make_shared<Framework::Barrier>(2);
		start(barrier);
		barrier->wait(true); // Wait for initialize
		barrier->wait(true); // Wait for startup
		success = isInitialized();
	}

	if (success)
	{
		setRate(device->getRate());
	}

	return success;
}

bool ReplayPoseScaffold::unregisterDevice()
{
	boost::unique_lock<boost::mutex> scopedLock(m_deviceLock);

	// #threadsafety After unregistering, another thread could be in the process of registering.
	if (isRunning())
	{
		stop();
	}

	m_device.reset();
	SURGSIM_LOG_DEBUG(m_logger) << "Unregistered device";
	return true;
}

bool ReplayPoseScaffold::updateDevice(ReplayPoseScaffold::DeviceData* info)
{
	DataStructures::DataGroup& inputData = m_device->deviceObject->getInputData();

	info->m_timer.markFrame();
	info->m_timestamp += info->m_timer.getLastFramePeriod();

	Math::RigidTransform3d pose = info->getPose();
	inputData.poses().set(DataStructures::Names::POSE, pose);

	m_device->deviceObject->pushInput();

	return true;
}

DataStructures::DataGroup ReplayPoseScaffold::buildDeviceInputData()
{
	DataStructures::DataGroupBuilder builder;
	builder.addPose(DataStructures::Names::POSE);
	return builder.createData();
}

};  // namespace Devices
};  // namespace SurgSim
<commit_msg>Fix possible deadlock in destruction of device<commit_after>// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.h"

#include <Eigen/Core>

#include <fstream>

#include "SurgSim/DataStructures/DataGroupBuilder.h"
#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/SharedInstance.h"

namespace
{
/// Missing istream "operator >>" for Eigen matrix type
/// http://eigen.tuxfamily.org/bz/show_bug.cgi?id=622
template<typename Derived>
std::istream& operator >>(std::istream& s, Eigen::MatrixBase<Derived>& m)
{
	for (int i = 0; i < m.rows(); ++i)
	{
		for (int j = 0; j < m.cols(); j++)
		{
			s >> m(i, j);
		}
	}
	return s;
}
}

namespace SurgSim
{
namespace Devices
{

ReplayPoseScaffold::ReplayPoseScaffold() :
	Framework::BasicThread("ReplayPoseScaffold"),
	m_logger(Framework::Logger::getLogger("Devices/ReplayPoseScaffold"))
{
	SURGSIM_LOG_DEBUG(m_logger) << "ReplayPose scaffold created.";
}

ReplayPoseScaffold::~ReplayPoseScaffold()
{
	if (m_device != nullptr)
	{
		unregisterDevice();
	}
	SURGSIM_LOG_DEBUG(m_logger) << "ReplayPose scaffold destroyed.";
}

std::shared_ptr<ReplayPoseScaffold> ReplayPoseScaffold::getOrCreateSharedInstance()
{
	static Framework::SharedInstance<ReplayPoseScaffold> sharedInstance;
	return sharedInstance.get();
}

bool ReplayPoseScaffold::doInitialize()
{
	return true;
}

bool ReplayPoseScaffold::doStartUp()
{
	return true;
}

bool ReplayPoseScaffold::doUpdate(double dt)
{
	boost::unique_lock<boost::mutex> scopedLock(m_deviceLock);

	SURGSIM_ASSERT(m_device != nullptr) << "DeviceData not properly allocated";

	return updateDevice(m_device.get());
}

struct ReplayPoseScaffold::DeviceData
{
	/// Constructor
	/// \param device Device to be managed by this scaffold
	explicit DeviceData(ReplayPoseDevice* device) :
		deviceObject(device),
		m_timestamp(0),
		m_index(0),
		m_fileLoaded(false)
	{
		m_fileLoaded = loadFile(deviceObject->getFileName());
		m_timer.start();
	}

	/// Retrieve the pose for the given time stamp "m_timestamp"
	/// \return The requested pose, Identity if none could be loaded
	Math::RigidTransform3d getPose()
	{
		if (m_motion.size() == 0)
		{
			return Math::RigidTransform3d::Identity();
		}

		if (m_motion.size() == 1 || (m_index == 0 && m_timestamp <= m_motion[0].first))
		{
			return m_motion[0].second;
		}

		while (m_motion.size() > m_index && m_timestamp > m_motion[m_index].first)
		{
			m_index++;
		}

		// Requesting a timestamp over the higher limit recorded in the file
		if (m_motion.size() <= m_index)
		{
			return m_motion[m_motion.size() - 1].second;
		}

		// Are we requesting exactly the timestamp we just indexed
		if (m_timestamp == m_motion[m_index].first)
		{
			return m_motion[m_index].second;
		}

		// Otherwise, we need to interpolate between the index and the previous index
		double range = m_motion[m_index].first - m_motion[m_index - 1].first;
		double time = (m_timestamp - m_motion[m_index - 1].first) / range;
		return Math::interpolate(m_motion[m_index - 1].second, m_motion[m_index].second, time);
	}

	/// Device object managed by this scaffold.
	ReplayPoseDevice* const deviceObject;

	/// Timer to keep track of the time stamp and keep the replay in sync with the recording
	Framework::Timer m_timer;

	/// Time stamp for the device (which time stamp are we reproducing?)
	double m_timestamp;

	/// Series of poses through time loaded from the input file
	std::vector<std::pair<double, Math::RigidTransform3d>> m_motion;

	/// Index of the latest pose used
	size_t m_index;

	/// Valid file loaded successfully
	bool m_fileLoaded;

private:
	// Prevent copy construction and copy assignment.  (VS2012 does not support "= delete" yet.)
	DeviceData(const DeviceData&) /*= delete*/;
	DeviceData& operator=(const DeviceData&) /*= delete*/;

	/// \param fileName the file to be open and loaded
	/// \return True if the file was loaded successfully, False otherwise
	bool loadFile(const std::string& fileName)
	{
		auto logger = Framework::Logger::getLogger("Devices/ReplayPoseScaffold");

		std::ifstream inputFile;
		bool result = true;

		inputFile.open(fileName, std::ios::in);

		if (!inputFile.is_open())
		{
			SURGSIM_LOG_WARNING(logger) << "Could not find or open the file " << fileName <<
										"; Replay will use Identity pose";
			result = false;
		}
		else
		{
			while (!inputFile.eof())
			{
				double timeStamp;
				Math::RigidTransform3d pose;
				inputFile >> timeStamp >> pose.matrix();
				m_motion.emplace_back(timeStamp, pose);
			}
			SURGSIM_LOG_INFO(logger) << "Loaded " << m_motion.size() << " timestamps";
			if (m_motion.size() >= 1)
			{
				SURGSIM_LOG_INFO(logger) << "The loaded timestamps cover a range of " <<
										 m_motion[m_motion.size() - 1].first - m_motion[0].first << " second(s)";
			}
			SURGSIM_LOG_IF(m_motion.size() == 0, logger, WARNING) <<
					"No poses could be properly loaded, Identity pose will be used";

			inputFile.close();
		}

		return result;
	}
};

bool ReplayPoseScaffold::registerDevice(ReplayPoseDevice* device)
{
	boost::unique_lock<boost::mutex> scopedLock(m_deviceLock);
	SURGSIM_ASSERT(m_device == nullptr) << "Can't register two ReplayPoseDevice.";

	m_device.reset(new ReplayPoseScaffold::DeviceData(device));
	if (m_device == nullptr)
	{
		SURGSIM_LOG_CRITICAL(m_logger) << "Failed to create a DeviceData";
		return false;
	}
	if (!m_device->m_fileLoaded)
	{
		SURGSIM_LOG_CRITICAL(m_logger) << "Failed to load the file to replay";
		return false;
	}
	SURGSIM_LOG_DEBUG(m_logger) << "Registered device " << device->getName();

	bool success = true;
	if (!isRunning())
	{
		std::shared_ptr<Framework::Barrier> barrier = std::make_shared<Framework::Barrier>(2);
		start(barrier);
		barrier->wait(true); // Wait for initialize
		barrier->wait(true); // Wait for startup
		success = isInitialized();
	}

	if (success)
	{
		setRate(device->getRate());
	}

	return success;
}

bool ReplayPoseScaffold::unregisterDevice()
{

	if (isRunning())
	{
		stop();
	}

	// #threadsafety After unregistering, another thread could be in the process of registering.
	boost::unique_lock<boost::mutex> scopedLock(m_deviceLock);
	m_device.reset();
	SURGSIM_LOG_DEBUG(m_logger) << "Unregistered device";
	return true;
}

bool ReplayPoseScaffold::updateDevice(ReplayPoseScaffold::DeviceData* info)
{
	DataStructures::DataGroup& inputData = m_device->deviceObject->getInputData();

	info->m_timer.markFrame();
	info->m_timestamp += info->m_timer.getLastFramePeriod();

	Math::RigidTransform3d pose = info->getPose();
	inputData.poses().set(DataStructures::Names::POSE, pose);

	m_device->deviceObject->pushInput();

	return true;
}

DataStructures::DataGroup ReplayPoseScaffold::buildDeviceInputData()
{
	DataStructures::DataGroupBuilder builder;
	builder.addPose(DataStructures::Names::POSE);
	return builder.createData();
}

};  // namespace Devices
};  // namespace SurgSim
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkSumProjectionImageFilterTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "itkCommand.h"
#include "itkSimpleFilterWatcher.h"

#include "itkSumProjectionImageFilter.h"

int itkSumProjectionImageFilterTest(int argc, char * argv[])
{
  if( argc < 3 )
    {
    std::cerr << "Missing Parameters " << std::endl;
    std::cerr << "Usage: " << argv[0];
    std::cerr << " InputImage OutputImage  " << std::endl;
    return EXIT_FAILURE;
    }

  const int dim = 3;

  typedef unsigned char PixelType;
  typedef itk::Image< PixelType, dim > ImageType;

  typedef itk::ImageFileReader< ImageType > ReaderType;
  ReaderType::Pointer reader = ReaderType::New();
  reader->SetFileName( argv[1] );

  typedef itk::SumProjectionImageFilter< ImageType, ImageType > FilterType;
  FilterType::Pointer filter = FilterType::New();
  filter->SetInput( reader->GetOutput() );

  itk::SimpleFilterWatcher watcher(filter, "filter");

  typedef itk::ImageFileWriter< ImageType > WriterType;
  WriterType::Pointer writer = WriterType::New();
  writer->SetInput( filter->GetOutput() );
  writer->SetFileName( argv[2] );

  try
    {
    writer->Update();
    }
  catch ( itk::ExceptionObject & excp )
    {
    std::cerr << excp << std::endl;
    return EXIT_FAILURE;
    }

  return EXIT_SUCCESS;
}

<commit_msg>STYLE: code style fix<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkSumProjectionImageFilterTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "itkCommand.h"
#include "itkSimpleFilterWatcher.h"

#include "itkSumProjectionImageFilter.h"

int itkSumProjectionImageFilterTest(int argc, char * argv[])
{
  if( argc < 3 )
    {
    std::cerr << "Missing Parameters " << std::endl;
    std::cerr << "Usage: " << argv[0];
    std::cerr << " InputImage OutputImage  " << std::endl;
    return EXIT_FAILURE;
    }

  const int dim = 3;

  typedef unsigned char InputPixelType;
  typedef itk::Image< InputPixelType, dim > InputImageType;

  typedef unsigned short OutpuPixelType;
  typedef itk::Image< OutpuPixelType, dim > OutputImageType;

  typedef itk::ImageFileReader< InputImageType > ReaderType;
  ReaderType::Pointer reader = ReaderType::New();
  reader->SetFileName( argv[1] );

  typedef itk::SumProjectionImageFilter< InputImageType, OutputImageType > FilterType;
  FilterType::Pointer filter = FilterType::New();
  filter->SetInput( reader->GetOutput() );

  itk::SimpleFilterWatcher watcher(filter, "filter");

  typedef itk::ImageFileWriter< OutputImageType > WriterType;
  WriterType::Pointer writer = WriterType::New();
  writer->SetInput( filter->GetOutput() );
  writer->SetFileName( argv[2] );

  try
    {
    writer->Update();
    }
  catch ( itk::ExceptionObject & excp )
    {
    std::cerr << excp << std::endl;
    return EXIT_FAILURE;
    }

  return EXIT_SUCCESS;
}

<|endoftext|>