<commit_before>#include "Testing.h"

#ifdef DEBUG
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <cmath>
#include <thread>
#include <iomanip>

#include "Game/Board.h"
#include "Moves/Complete_Move.h"
#include "Players/Genetic_AI.h"
#include "Game/Clock.h"

#include "Utility.h"

#include "Exceptions/Illegal_Move_Exception.h"

// Declaration to silence warnings
bool files_are_identical(const std::string& file_name1, const std::string& file_name2);

bool files_are_identical(const std::string& file_name1, const std::string& file_name2)
{
    std::ifstream file1(file_name1);
    std::ifstream file2(file_name2);
    int line_count = 0;

    while(true)
    {
        std::string line1, line2;
        std::getline(file1, line1);
        std::getline(file2, line2);
        ++line_count;

        if(line1 != line2)
        {
            std::cerr << "Mismatch at line " << line_count << ":\n";
            std::cerr << line1 << " != " << line2 << "\n";
            return false;
        }

        if( ! file1 && ! file2)
        {
            break;
        }
    }

    return true;
}

void run_tests()
{
    bool tests_passed = true;

    // Castling with attacking piece
    for(auto castle_side : std::string("KQ"))
    {
        for(int rook_left_space = 0; rook_left_space < 8; ++rook_left_space)
        {
            auto rook_left  = (rook_left_space > 0 ? std::to_string(rook_left_space) : std::string());
            auto rook_right = (rook_left_space < 7 ? std::to_string(7 - rook_left_space) : std::string());
            Board board("1k6/" + rook_left + "r" + rook_right + "/8/8/8/8/8/R3K2R w KQ - 0 1");

            char final_file = (castle_side == 'K' ? 'g' : 'c');

            try
            {
                board.get_complete_move('e', 1, final_file, 1);
            }
            catch(const Illegal_Move_Exception&)
            {
                if(castle_side == 'K')
                {
                    if(rook_left_space >= 4 && rook_left_space <= 6)
                    {
                        continue;
                    }
                }
                else
                {
                    if(rook_left_space >= 2 && rook_left_space <= 4)
                    {
                        continue;
                    }
                }

                board.ascii_draw(WHITE);
                std::cout << std::string(1, castle_side) + "-castle should be legal here." << std::endl;
                tests_passed = false;
            }

            if(castle_side == 'K')
            {
                if(rook_left_space < 4 || rook_left_space > 6)
                {
                    continue;
                }
            }
            else
            {
                if(rook_left_space < 2 || rook_left_space > 4)
                {
                    continue;
                }
            }

            board.ascii_draw(WHITE);
            std::cout << std::string(1, castle_side) + "-castle should be illegal here." << std::endl;
            tests_passed = false;
        }
    }

    // Test Genetic_AI file loading
    auto pool_file_name = "test_gene_pool.txt";
    auto write_file_name = "test_genome_write.txt";
    auto rewrite_file_name = "test_genome_rewrite.txt";
    remove(pool_file_name);
    remove(write_file_name);
    remove(rewrite_file_name);

    std::vector<Genetic_AI> test_pool(100);
    for(auto& ai : test_pool)
    {
        for(int i = 0; i < 1000; ++i)
        {
            ai.mutate();
        }
        ai.print_genome(pool_file_name);
    }

    auto index = Random::random_integer(0, test_pool.size() - 1);
    test_pool[index].print_genome(write_file_name);
    auto read_ai = Genetic_AI(pool_file_name, index);
    read_ai.print_genome(rewrite_file_name);

    if( ! files_are_identical(write_file_name, rewrite_file_name))
    {
        std::cerr << "Genome loaded from gene pool file not preserved." << std::endl;
        tests_passed = false;
    }
    else
    {
        remove(pool_file_name);
        remove(write_file_name);
        remove(rewrite_file_name);
    }

    // String utilities
    std::string original = "   a    #     b";
    std::string expected = "a";
    std::string result = String::strip_comments(original, '#');
    if(expected != result)
    {
        std::cerr << "\"" << original << "\" --> \"" << result << "\"" << std::endl;
        std::cerr << "Expected result: \"" << expected << "\"" << std::endl;
        tests_passed = false;
    }

    original = "   a    {    b    }    c   {   d  }   ";
    expected = "a c";
    result = String::strip_block_comment(original, '{', '}');
    if(expected != result)
    {
        std::cerr << "\"" << original << "\" --> \"" << result << "\"" << std::endl;
        std::cerr << "Expected result: \"" << expected << "\"" << std::endl;
        tests_passed = false;
    }

    std::string search_string = "abcdefg";
    std::string target = "abc";
    if( ! String::starts_with(search_string, target))
    {
        std::cerr << "\"" << target << "\" not found at beginning of " << search_string << std::endl;
        tests_passed = false;
    }

    std::string wrong_target = "abd";
    if(String::starts_with(search_string, wrong_target))
    {
        std::cerr << "\"" << wrong_target << "\" erroneously found at beginning of " << search_string << std::endl;
        tests_passed = false;
    }

    // Log-Norm distribution check
    const double mean_moves = 26.0;
    const double width = .5;
    const size_t moves_so_far = 22;
    auto moves_left = Math::average_moves_left(mean_moves, width, moves_so_far);
    auto expected_moves_left = 15.2629;
    if(std::abs(moves_left - expected_moves_left) > 1e-4)
    {
        std::cout << "Log-Norm failed: Expected: " << expected_moves_left
                  << " --- Got: " << moves_left << std::endl;
        tests_passed = false;
    }

    // Clock time reset test
    auto time = 30;
    auto moves_to_reset = 40;
    auto clock = Clock(time, moves_to_reset);
    clock.start();
    for(int i = 0; i < 2*moves_to_reset; ++i)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
        clock.punch();
    }
    clock.stop();
    if(clock.time_left(BLACK) != time)
    {
        std::cerr << "Clock incorrect: time left for black is " << clock.time_left(BLACK) << " sec. Should be " << time << "sec." << std::endl;
        tests_passed = false;
    }

    // Clock time increment test
    auto increment = 5;
    auto clock2 = Clock(time, 0, increment);
    clock2.start();
    double expected_time = time;
    for(int i = 0; i < 2*moves_to_reset; ++i)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
        clock2.punch();
        if(i % 2 == 1) // only on black moves
        {
            expected_time += increment - 0.005;
        }
    }
    clock2.stop();
    if(std::abs(clock2.time_left(BLACK) - expected_time) > 0.01)
    {
        std::cerr << "Clock incorrect: time left for black is " << clock2.time_left(BLACK) << " sec. Should be " << expected_time << "sec." << std::endl;
        tests_passed = false;
    }

    Game_Tree_Node_Result r1 = {Complete_Move(),
                                10,
                                WHITE,
                                2,
                                {}};
    Game_Tree_Node_Result r2 = {Complete_Move(),
                                10,
                                BLACK,
                                2,
                                {}};

    if(better_than(r2, r1, WHITE))
    {
        std::cerr << "1. Error in comparing Game Tree Node Results." << std::endl;
        tests_passed = false;
    }

    if(better_than(r1, r2, BLACK))
    {
        std::cerr << "2. Error in comparing Game Tree Node Results." << std::endl;
        tests_passed = false;
    }

    Game_Tree_Node_Result alpha_start = {Complete_Move(),
                                         -Math::infinity,
                                         WHITE,
                                         0,
                                         {}};

    Game_Tree_Node_Result beta_start = {Complete_Move(),
                                        Math::infinity,
                                        WHITE,
                                        0,
                                        {}};
    if(better_than(alpha_start, beta_start, WHITE))
    {
        std::cerr << "3. Error in comparing Game Tree Node Results." << std::endl;
        tests_passed = false;
    }

    if( ! better_than(alpha_start, beta_start, BLACK))
    {
        std::cerr << "4. Error in comparing Game Tree Node Results." << std::endl;
        tests_passed = false;
    }

    Game_Tree_Node_Result white_win4 = {Complete_Move(),
                                        Math::win_score,
                                        WHITE,
                                        4,
                                        {}};
    Game_Tree_Node_Result white_win6 = {Complete_Move(),
                                        Math::win_score,
                                        WHITE,
                                        6,
                                        {}};
    if(better_than(white_win6, white_win4, WHITE))
    {
        std::cerr << "Later win preferred over earlier win." << std::endl;
        tests_passed = false;
    }

    if(better_than(white_win4, white_win6, BLACK))
    {
        std::cerr << "Earlier loss preferred over later win." << std::endl;
        tests_passed = false;
    }

    Game_Tree_Node_Result black_loss6 = {Complete_Move(),
                                         -Math::win_score,
                                         BLACK,
                                         6,
                                         {}};
    if( ! (white_win6 == black_loss6))
    {
        std::cerr << "White win in 6 not equal to black loss in 6." << std::endl;
        tests_passed = false;
    }

    // Move ambiguity check
    Board board;
    bool ambiguous_move_caught = false;
    try
    {
        for(auto move : {"Nc3", "a1",
                         "Nf3", "b1",
                         "Ne4", "c1",
                         "Ng5"})
        {
            board.submit_move(board.get_complete_move(move));
        }
    }
    catch(const Illegal_Move_Exception&)
    {
        ambiguous_move_caught = true;
    }

    if( ! ambiguous_move_caught)
    {
        std::cerr << "Last move should have been an error:" << std::endl;
        board.print_game_record(nullptr, nullptr,
                                "", Game_Result(NONE, "", false), 0, 0, 0, Clock());

        tests_passed = false;
    }

    Board().ascii_draw(WHITE);

    int int_width = 10;
    int real_width = 15;
    int norm_width = 15;
    std::cout << std::setw(int_width) << "Integers"
              << std::setw(real_width) << "Reals"
              << std::setw(norm_width) << "Normals" << '\n';
    std::cout << std::setw(int_width) << "+/-1000"
              << std::setw(real_width) << "[0-2]"
              << std::setw(norm_width) << "sig = 3" << '\n';
    for(int i = 0; i < 10; ++i)
    {
        std::cout << std::setw(int_width) << Random::random_integer(-1000, 1000)
                  << std::setw(real_width) << Random::random_real(0, 2)
                  << std::setw(norm_width) << Random::random_normal(3) << '\n';
    }

    // FEN input/output
    for(std::string test : {"rnbqkbnr/pppppppp/8/8/8/8/PPPPPPPP/RNBQKBNR w KQkq - 0 1",
                            "rnbqkbnr/pppppppp/8/8/4P3/8/PPPP1PPP/RNBQKBNR b KQkq e3 0 1",
                            "rnbqkbnr/pp1ppppp/8/2p5/4P3/8/PPPP1PPP/RNBQKBNR w KQkq c6 0 2",
                            "rnbqkbnr/pp1ppppp/8/2p5/4P3/5N2/PPPP1PPP/RNBQKB1R b KQkq - 1 2"})
    {
        if(Board(test).fen_status() != test)
        {
            std::cerr << test << " -->\n" << Board(test).fen_status() << "\n\n";
            tests_passed = false;
        }
    }

    if(tests_passed)
    {
        std::cout << "All tests passed." << std::endl;
    }
    else
    {
        std::cout << "Tests failed." << std::endl;
    }
}
#else
void run_tests() {}
#endif
<commit_msg>Add basic rules check in tests<commit_after>#include "Testing.h"

#ifdef DEBUG
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <cmath>
#include <thread>
#include <iomanip>

#include "Game/Board.h"
#include "Moves/Complete_Move.h"
#include "Players/Genetic_AI.h"
#include "Game/Clock.h"

#include "Utility.h"

#include "Exceptions/Illegal_Move_Exception.h"

// Declaration to silence warnings
bool files_are_identical(const std::string& file_name1, const std::string& file_name2);

bool files_are_identical(const std::string& file_name1, const std::string& file_name2)
{
    std::ifstream file1(file_name1);
    std::ifstream file2(file_name2);
    int line_count = 0;

    while(true)
    {
        std::string line1, line2;
        std::getline(file1, line1);
        std::getline(file2, line2);
        ++line_count;

        if(line1 != line2)
        {
            std::cerr << "Mismatch at line " << line_count << ":\n";
            std::cerr << line1 << " != " << line2 << "\n";
            return false;
        }

        if( ! file1 && ! file2)
        {
            break;
        }
    }

    return true;
}

void run_tests()
{
    bool tests_passed = true;

    // Basic chess rules check
    Board starting_board;
    auto starting_move_count = starting_board.legal_moves().size();
    auto correct_move_count = 20;
    if(starting_move_count != correct_move_count)
    {
        std::cerr << "Wrong number of legal moves at beginning of game. Got " << starting_move_count
                  << ", should be " << correct_move_count << std::endl;
        std::cerr << "Legal moves found:" << std::endl;
        auto move_count = 0;
        for(const auto& move : starting_board.legal_moves())
        {
            std::cerr << ++move_count << ". " << move.game_record_item(starting_board) << std::endl;
        }

        tests_passed = false;
    }

    // Castling with attacking piece
    for(auto castle_side : std::string("KQ"))
    {
        for(int rook_left_space = 0; rook_left_space < 8; ++rook_left_space)
        {
            auto rook_left  = (rook_left_space > 0 ? std::to_string(rook_left_space) : std::string());
            auto rook_right = (rook_left_space < 7 ? std::to_string(7 - rook_left_space) : std::string());
            Board board("1k6/" + rook_left + "r" + rook_right + "/8/8/8/8/8/R3K2R w KQ - 0 1");

            char final_file = (castle_side == 'K' ? 'g' : 'c');

            try
            {
                board.get_complete_move('e', 1, final_file, 1);
            }
            catch(const Illegal_Move_Exception&)
            {
                if(castle_side == 'K')
                {
                    if(rook_left_space >= 4 && rook_left_space <= 6)
                    {
                        continue;
                    }
                }
                else
                {
                    if(rook_left_space >= 2 && rook_left_space <= 4)
                    {
                        continue;
                    }
                }

                board.ascii_draw(WHITE);
                std::cout << std::string(1, castle_side) + "-castle should be legal here." << std::endl;
                tests_passed = false;
            }

            if(castle_side == 'K')
            {
                if(rook_left_space < 4 || rook_left_space > 6)
                {
                    continue;
                }
            }
            else
            {
                if(rook_left_space < 2 || rook_left_space > 4)
                {
                    continue;
                }
            }

            board.ascii_draw(WHITE);
            std::cout << std::string(1, castle_side) + "-castle should be illegal here." << std::endl;
            tests_passed = false;
        }
    }

    // Test Genetic_AI file loading
    auto pool_file_name = "test_gene_pool.txt";
    auto write_file_name = "test_genome_write.txt";
    auto rewrite_file_name = "test_genome_rewrite.txt";
    remove(pool_file_name);
    remove(write_file_name);
    remove(rewrite_file_name);

    std::vector<Genetic_AI> test_pool(100);
    for(auto& ai : test_pool)
    {
        for(int i = 0; i < 1000; ++i)
        {
            ai.mutate();
        }
        ai.print_genome(pool_file_name);
    }

    auto index = Random::random_integer(0, test_pool.size() - 1);
    test_pool[index].print_genome(write_file_name);
    auto read_ai = Genetic_AI(pool_file_name, index);
    read_ai.print_genome(rewrite_file_name);

    if( ! files_are_identical(write_file_name, rewrite_file_name))
    {
        std::cerr << "Genome loaded from gene pool file not preserved." << std::endl;
        tests_passed = false;
    }
    else
    {
        remove(pool_file_name);
        remove(write_file_name);
        remove(rewrite_file_name);
    }

    // String utilities
    std::string original = "   a    #     b";
    std::string expected = "a";
    std::string result = String::strip_comments(original, '#');
    if(expected != result)
    {
        std::cerr << "\"" << original << "\" --> \"" << result << "\"" << std::endl;
        std::cerr << "Expected result: \"" << expected << "\"" << std::endl;
        tests_passed = false;
    }

    original = "   a    {    b    }    c   {   d  }   ";
    expected = "a c";
    result = String::strip_block_comment(original, '{', '}');
    if(expected != result)
    {
        std::cerr << "\"" << original << "\" --> \"" << result << "\"" << std::endl;
        std::cerr << "Expected result: \"" << expected << "\"" << std::endl;
        tests_passed = false;
    }

    std::string search_string = "abcdefg";
    std::string target = "abc";
    if( ! String::starts_with(search_string, target))
    {
        std::cerr << "\"" << target << "\" not found at beginning of " << search_string << std::endl;
        tests_passed = false;
    }

    std::string wrong_target = "abd";
    if(String::starts_with(search_string, wrong_target))
    {
        std::cerr << "\"" << wrong_target << "\" erroneously found at beginning of " << search_string << std::endl;
        tests_passed = false;
    }

    // Log-Norm distribution check
    const double mean_moves = 26.0;
    const double width = .5;
    const size_t moves_so_far = 22;
    auto moves_left = Math::average_moves_left(mean_moves, width, moves_so_far);
    auto expected_moves_left = 15.2629;
    if(std::abs(moves_left - expected_moves_left) > 1e-4)
    {
        std::cout << "Log-Norm failed: Expected: " << expected_moves_left
                  << " --- Got: " << moves_left << std::endl;
        tests_passed = false;
    }

    // Clock time reset test
    auto time = 30;
    auto moves_to_reset = 40;
    auto clock = Clock(time, moves_to_reset);
    clock.start();
    for(int i = 0; i < 2*moves_to_reset; ++i)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
        clock.punch();
    }
    clock.stop();
    if(clock.time_left(BLACK) != time)
    {
        std::cerr << "Clock incorrect: time left for black is " << clock.time_left(BLACK) << " sec. Should be " << time << "sec." << std::endl;
        tests_passed = false;
    }

    // Clock time increment test
    auto increment = 5;
    auto clock2 = Clock(time, 0, increment);
    clock2.start();
    double expected_time = time;
    for(int i = 0; i < 2*moves_to_reset; ++i)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
        clock2.punch();
        if(i % 2 == 1) // only on black moves
        {
            expected_time += increment - 0.005;
        }
    }
    clock2.stop();
    if(std::abs(clock2.time_left(BLACK) - expected_time) > 0.01)
    {
        std::cerr << "Clock incorrect: time left for black is " << clock2.time_left(BLACK) << " sec. Should be " << expected_time << "sec." << std::endl;
        tests_passed = false;
    }

    Game_Tree_Node_Result r1 = {Complete_Move(),
                                10,
                                WHITE,
                                2,
                                {}};
    Game_Tree_Node_Result r2 = {Complete_Move(),
                                10,
                                BLACK,
                                2,
                                {}};

    if(better_than(r2, r1, WHITE))
    {
        std::cerr << "1. Error in comparing Game Tree Node Results." << std::endl;
        tests_passed = false;
    }

    if(better_than(r1, r2, BLACK))
    {
        std::cerr << "2. Error in comparing Game Tree Node Results." << std::endl;
        tests_passed = false;
    }

    Game_Tree_Node_Result alpha_start = {Complete_Move(),
                                         -Math::infinity,
                                         WHITE,
                                         0,
                                         {}};

    Game_Tree_Node_Result beta_start = {Complete_Move(),
                                        Math::infinity,
                                        WHITE,
                                        0,
                                        {}};
    if(better_than(alpha_start, beta_start, WHITE))
    {
        std::cerr << "3. Error in comparing Game Tree Node Results." << std::endl;
        tests_passed = false;
    }

    if( ! better_than(alpha_start, beta_start, BLACK))
    {
        std::cerr << "4. Error in comparing Game Tree Node Results." << std::endl;
        tests_passed = false;
    }

    Game_Tree_Node_Result white_win4 = {Complete_Move(),
                                        Math::win_score,
                                        WHITE,
                                        4,
                                        {}};
    Game_Tree_Node_Result white_win6 = {Complete_Move(),
                                        Math::win_score,
                                        WHITE,
                                        6,
                                        {}};
    if(better_than(white_win6, white_win4, WHITE))
    {
        std::cerr << "Later win preferred over earlier win." << std::endl;
        tests_passed = false;
    }

    if(better_than(white_win4, white_win6, BLACK))
    {
        std::cerr << "Earlier loss preferred over later win." << std::endl;
        tests_passed = false;
    }

    Game_Tree_Node_Result black_loss6 = {Complete_Move(),
                                         -Math::win_score,
                                         BLACK,
                                         6,
                                         {}};
    if( ! (white_win6 == black_loss6))
    {
        std::cerr << "White win in 6 not equal to black loss in 6." << std::endl;
        tests_passed = false;
    }

    // Move ambiguity check
    Board board;
    bool ambiguous_move_caught = false;
    try
    {
        for(auto move : {"Nc3", "a1",
                         "Nf3", "b1",
                         "Ne4", "c1",
                         "Ng5"})
        {
            board.submit_move(board.get_complete_move(move));
        }
    }
    catch(const Illegal_Move_Exception&)
    {
        ambiguous_move_caught = true;
    }

    if( ! ambiguous_move_caught)
    {
        std::cerr << "Last move should have been an error:" << std::endl;
        board.print_game_record(nullptr, nullptr,
                                "", Game_Result(NONE, "", false), 0, 0, 0, Clock());

        tests_passed = false;
    }

    Board().ascii_draw(WHITE);

    int int_width = 10;
    int real_width = 15;
    int norm_width = 15;
    std::cout << std::setw(int_width) << "Integers"
              << std::setw(real_width) << "Reals"
              << std::setw(norm_width) << "Normals" << '\n';
    std::cout << std::setw(int_width) << "+/-1000"
              << std::setw(real_width) << "[0-2]"
              << std::setw(norm_width) << "sig = 3" << '\n';
    for(int i = 0; i < 10; ++i)
    {
        std::cout << std::setw(int_width) << Random::random_integer(-1000, 1000)
                  << std::setw(real_width) << Random::random_real(0, 2)
                  << std::setw(norm_width) << Random::random_normal(3) << '\n';
    }

    // FEN input/output
    for(std::string test : {"rnbqkbnr/pppppppp/8/8/8/8/PPPPPPPP/RNBQKBNR w KQkq - 0 1",
                            "rnbqkbnr/pppppppp/8/8/4P3/8/PPPP1PPP/RNBQKBNR b KQkq e3 0 1",
                            "rnbqkbnr/pp1ppppp/8/2p5/4P3/8/PPPP1PPP/RNBQKBNR w KQkq c6 0 2",
                            "rnbqkbnr/pp1ppppp/8/2p5/4P3/5N2/PPPP1PPP/RNBQKB1R b KQkq - 1 2"})
    {
        if(Board(test).fen_status() != test)
        {
            std::cerr << test << " -->\n" << Board(test).fen_status() << "\n\n";
            tests_passed = false;
        }
    }

    if(tests_passed)
    {
        std::cout << "All tests passed." << std::endl;
    }
    else
    {
        std::cout << "Tests failed." << std::endl;
    }
}
#else
void run_tests() {}
#endif
<|endoftext|>


<commit_before>/*
 * goal_orientation_constraint.cpp
 *
 *  Created on: Apr 21, 2016
 *      Author: Jorge Nicho
 */

#include <ros/console.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>
#include <pluginlib/class_list_macros.h>
#include <stomp_moveit/noisy_filters/underconstrained_goal.h>
#include <XmlRpcException.h>

PLUGINLIB_EXPORT_CLASS(stomp_moveit::noisy_filters::UnderconstrainedGoal,stomp_moveit::noisy_filters::StompNoisyFilter);

const static unsigned int DOF_SIZE = 6;
const static double EPSILON = 0.1;
const static double LAMBDA = 0.01;

static void reduceJacobian(const Eigen::MatrixXd& jacb,
                                          const std::vector<int>& indices,Eigen::MatrixXd& jacb_reduced)
{
  jacb_reduced.resize(indices.size(),jacb.cols());
  for(auto i = 0u; i < indices.size(); i++)
  {
    jacb_reduced.row(i) = jacb.row(indices[i]);
  }
}

static void calculateMoorePenrosePseudoInverse(const Eigen::MatrixXd& jacb,Eigen::MatrixXd& jacb_pseudo_inv)
{
  Eigen::MatrixXd jacb_transpose = jacb.transpose();
  jacb_pseudo_inv = (jacb_transpose) * ((jacb * jacb_transpose).inverse());
}

static void calculateDampedPseudoInverse(const Eigen::MatrixXd &jacb, Eigen::MatrixXd &jacb_pseudo_inv,
                                         double eps = 0.011, double lambda = 0.01)
{
  using namespace Eigen;

  //Calculate A+ (pseudoinverse of A) = V S+ U*, where U* is Hermition of U (just transpose if all values of U are real)
  //in order to solve Ax=b -> x*=A+ b
  Eigen::JacobiSVD<MatrixXd> svd(jacb, Eigen::ComputeThinU | Eigen::ComputeThinV);
  const MatrixXd &U = svd.matrixU();
  const VectorXd &Sv = svd.singularValues();
  const MatrixXd &V = svd.matrixV();

  // calculate the reciprocal of Singular-Values
  // damp inverse with lambda so that inverse doesn't oscillate near solution
  size_t nSv = Sv.size();
  VectorXd inv_Sv(nSv);
  for(size_t i=0; i< nSv; ++i)
  {
    if (fabs(Sv(i)) > eps)
    {
      inv_Sv(i) = 1/Sv(i);
    }
    else
    {
      inv_Sv(i) = Sv(i) / (Sv(i)*Sv(i) + lambda*lambda);
    }
  }

  jacb_pseudo_inv = V * inv_Sv.asDiagonal() * U.transpose();
}

static void computeTwist(const Eigen::Affine3d& p0,
                                        const Eigen::Affine3d& pf,
                                        const Eigen::ArrayXi& nullity,Eigen::VectorXd& twist)
{
  twist.resize(nullity.size());
  twist.setConstant(0);
  Eigen::Vector3d twist_pos = pf.translation() - p0.translation();

  // relative rotation -> R = inverse(R0) * Rf
  Eigen::AngleAxisd relative_rot(p0.rotation().transpose() * pf.rotation());
  double angle = relative_rot.angle();
  Eigen::Vector3d axis = relative_rot.axis();

  // forcing angle to range [-pi , pi]
  while( (angle > M_PI) || (angle < -M_PI))
  {
    angle = (angle >  M_PI) ? (angle - 2*M_PI) : angle;
    angle = (angle < -M_PI )? (angle + 2*M_PI) : angle;
  }

  // creating twist rotation relative to tool
  Eigen::Vector3d twist_rot = axis * angle;

  // assigning into full 6dof twist vector
  twist.head(3) = twist_pos;
  twist.tail(3) = twist_rot;

  // zeroing all underconstrained cartesian dofs
  twist = (nullity == 0).select(0,twist);
}

namespace stomp_moveit
{
namespace noisy_filters
{

UnderconstrainedGoal::UnderconstrainedGoal():
    name_("UnderconstrainedGoal")
{

}

UnderconstrainedGoal::~UnderconstrainedGoal()
{
  // TODO Auto-generated destructor stub
}

bool UnderconstrainedGoal::initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
                        const std::string& group_name,const XmlRpc::XmlRpcValue& config)
{
  group_name_ = group_name;
  robot_model_ = robot_model_ptr;

  return configure(config);
}

bool UnderconstrainedGoal::configure(const XmlRpc::XmlRpcValue& config)
{
  using namespace XmlRpc;

  try
  {
    XmlRpcValue params = config;

    XmlRpcValue dof_nullity_param = params["constrained_dofs"];
    XmlRpcValue dof_thresholds_param = params["cartesian_convergence"];
    XmlRpcValue joint_updates_param = params["joint_update_rates"];
    if((dof_nullity_param.getType() != XmlRpcValue::TypeArray) ||
        dof_nullity_param.size() < DOF_SIZE ||
        dof_thresholds_param.getType() != XmlRpcValue::TypeArray ||
        dof_thresholds_param.size() < DOF_SIZE  ||
        joint_updates_param.getType() != XmlRpcValue::TypeArray ||
        joint_updates_param.size() == 0)
    {
      ROS_ERROR("UnderconstrainedGoal received invalid array parameters");
      return false;
    }

    dof_nullity_.resize(DOF_SIZE);
    for(auto i = 0u; i < dof_nullity_param.size(); i++)
    {
      dof_nullity_(i) = static_cast<int>(dof_nullity_param[i]);
    }

    cartesian_convergence_thresholds_.resize(DOF_SIZE);
    for(auto i = 0u; i < dof_thresholds_param.size(); i++)
    {
      cartesian_convergence_thresholds_(i) = static_cast<double>(dof_thresholds_param[i]);
    }

    joint_update_rates_.resize(joint_updates_param.size());
    for(auto i = 0u; i < joint_updates_param.size(); i++)
    {
      joint_update_rates_(i) = static_cast<double>(joint_updates_param[i]);
    }

    //update_weight_ = static_cast<double>(params["update_weight"]);
    max_iterations_ = static_cast<int>(params["max_ik_iterations"]);
  }
  catch(XmlRpc::XmlRpcException& e)
  {
    ROS_ERROR("UnderconstrainedGoal failed to load parameters, %s",e.getMessage().c_str());
    return false;
  }

  return true;
}

bool UnderconstrainedGoal::setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
                 const moveit_msgs::MotionPlanRequest &req,
                 const stomp_core::StompConfiguration &config,
                 moveit_msgs::MoveItErrorCodes& error_code)
{
  using namespace Eigen;
  using namespace moveit::core;

  const JointModelGroup* joint_group = robot_model_->getJointModelGroup(group_name_);
  tool_link_ = joint_group->getLinkModelNames().back();
  state_.reset(new RobotState(robot_model_));
  robotStateMsgToRobotState(req.start_state,*state_);

  const std::vector<moveit_msgs::Constraints>& goals = req.goal_constraints;
  if(goals.empty())
  {
    ROS_ERROR("A goal constraint was not provided");
    error_code.val = error_code.INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  // storing tool goal pose
  if(goals.front().position_constraints.empty() ||
      goals.front().orientation_constraints.empty())
  {
    ROS_WARN("A goal constraint for the tool link was not provided, using forward kinematics");

    // check joint constraints
    if(goals.front().joint_constraints.empty())
    {
      ROS_ERROR_STREAM("No joint values for the goal were found");
      error_code.val = error_code.INVALID_GOAL_CONSTRAINTS;
      return false;
    }

    // compute FK to obtain tool pose
    const std::vector<moveit_msgs::JointConstraint>& joint_constraints = goals.front().joint_constraints;

    // copying goal values into state
    for(auto& jc: joint_constraints)
    {
      state_->setVariablePosition(jc.joint_name,jc.position);
    }

    state_->update(true);
    state_->enforceBounds(joint_group);
    tool_goal_pose_ = state_->getGlobalLinkTransform(tool_link_);
  }
  else
  {
    // tool goal
    const moveit_msgs::PositionConstraint& pos_constraint = goals.front().position_constraints.front();
    const moveit_msgs::OrientationConstraint& orient_constraint = goals.front().orientation_constraints.front();

    geometry_msgs::Pose pose;
    pose.position = pos_constraint.constraint_region.primitive_poses[0].position;
    pose.orientation = orient_constraint.orientation;
    tf::poseMsgToEigen(pose,tool_goal_pose_);
  }

  error_code.val = error_code.SUCCESS;
  return true;
}

bool UnderconstrainedGoal::filter(std::size_t start_timestep,
                    std::size_t num_timesteps,
                    int iteration_number,
                    int rollout_number,
                    Eigen::MatrixXd& parameters,
                    bool& filtered)
{
  using namespace Eigen;
  using namespace moveit::core;

  VectorXd init_joint_pose = parameters.rightCols(1);
  VectorXd joint_pose;

  if(!runIK(tool_goal_pose_,init_joint_pose,joint_pose))
  {
    ROS_ERROR("UnderconstrainedGoal failed to find valid ik close to reference pose");
    filtered = false;
    return false;
  }

  filtered = true;
  ROS_DEBUG_STREAM("Last joint pose changes "<<(parameters.rightCols(1) - joint_pose).transpose());
  parameters.rightCols(1) = joint_pose;

  return true;
}

bool UnderconstrainedGoal::runIK(const Eigen::Affine3d& tool_goal_pose,const Eigen::VectorXd& init_joint_pose,
                                       Eigen::VectorXd& joint_pose)
{
  using namespace Eigen;
  using namespace moveit::core;

  // joint variables
  VectorXd delta_j = VectorXd::Zero(init_joint_pose.size());
  joint_pose = init_joint_pose;
  const JointModelGroup* joint_group = robot_model_->getJointModelGroup(group_name_);
  state_->setJointGroupPositions(joint_group,joint_pose);
  Affine3d tool_current_pose = state_->getGlobalLinkTransform(tool_link_);

  // tool twist variables
  VectorXd tool_twist, tool_twist_reduced;
  std::vector<int> indices;
  for(auto i = 0u; i < dof_nullity_.size(); i++)
  {
    if(dof_nullity_(i) != 0)
    {
      indices.push_back(i);
    }
  }
  tool_twist_reduced = VectorXd::Zero(indices.size());

  // jacobian calculation variables
  MatrixXd identity = MatrixXd::Identity(init_joint_pose.size(),init_joint_pose.size());
  MatrixXd jacb, jacb_reduced, jacb_pseudo_inv;

  unsigned int iteration_count = 0;
  bool converged = false;
  while(iteration_count < max_iterations_)
  {

    // computing twist vector
    computeTwist(tool_current_pose,tool_goal_pose,dof_nullity_,tool_twist);

    // check convergence
    if((tool_twist.cwiseAbs().array() < cartesian_convergence_thresholds_).all())
    {
      // converged
      converged = true;
      ROS_DEBUG("Found numeric ik solution after %i iterations",iteration_count);
      break;
    }

    // updating reduced tool twist
    for(auto i = 0u; i < indices.size(); i++)
    {
      tool_twist_reduced(i) = tool_twist(indices[i]);
    }

    // computing jacobian
    if(!state_->getJacobian(joint_group,state_->getLinkModel(tool_link_),Vector3d::Zero(),jacb))
    {
      ROS_ERROR("Failed to get Jacobian for link %s",tool_link_.c_str());
      return false;
    }

    // transform jacobian rotational part to tool coordinates
    jacb.bottomRows(3) = tool_current_pose.rotation().transpose()*jacb.bottomRows(3);

    // reduce jacobian and compute its pseudo inverse
    reduceJacobian(jacb,indices,jacb_reduced);
    calculateDampedPseudoInverse(jacb_reduced,jacb_pseudo_inv,EPSILON,LAMBDA);

    // computing joint change
    delta_j = (jacb_pseudo_inv*tool_twist_reduced);

    // updating joint values
    joint_pose += (joint_update_rates_* delta_j.array()).matrix();

    // updating tool pose
    state_->setJointGroupPositions(joint_group,joint_pose);
    tool_current_pose = state_->getGlobalLinkTransform(tool_link_);

    iteration_count++;
  }

  ROS_DEBUG_STREAM("Final tool twist "<<tool_twist.transpose());

  return converged;
}

} /* namespace filters */
} /* namespace stomp_moveit */
<commit_msg>Remove print statement<commit_after>/*
 * goal_orientation_constraint.cpp
 *
 *  Created on: Apr 21, 2016
 *      Author: Jorge Nicho
 */

#include <ros/console.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>
#include <pluginlib/class_list_macros.h>
#include <stomp_moveit/noisy_filters/underconstrained_goal.h>
#include <XmlRpcException.h>

PLUGINLIB_EXPORT_CLASS(stomp_moveit::noisy_filters::UnderconstrainedGoal,stomp_moveit::noisy_filters::StompNoisyFilter);

const static unsigned int DOF_SIZE = 6;
const static double EPSILON = 0.1;
const static double LAMBDA = 0.01;

static void reduceJacobian(const Eigen::MatrixXd& jacb,
                                          const std::vector<int>& indices,Eigen::MatrixXd& jacb_reduced)
{
  jacb_reduced.resize(indices.size(),jacb.cols());
  for(auto i = 0u; i < indices.size(); i++)
  {
    jacb_reduced.row(i) = jacb.row(indices[i]);
  }
}

static void calculateMoorePenrosePseudoInverse(const Eigen::MatrixXd& jacb,Eigen::MatrixXd& jacb_pseudo_inv)
{
  Eigen::MatrixXd jacb_transpose = jacb.transpose();
  jacb_pseudo_inv = (jacb_transpose) * ((jacb * jacb_transpose).inverse());
}

static void calculateDampedPseudoInverse(const Eigen::MatrixXd &jacb, Eigen::MatrixXd &jacb_pseudo_inv,
                                         double eps = 0.011, double lambda = 0.01)
{
  using namespace Eigen;

  //Calculate A+ (pseudoinverse of A) = V S+ U*, where U* is Hermition of U (just transpose if all values of U are real)
  //in order to solve Ax=b -> x*=A+ b
  Eigen::JacobiSVD<MatrixXd> svd(jacb, Eigen::ComputeThinU | Eigen::ComputeThinV);
  const MatrixXd &U = svd.matrixU();
  const VectorXd &Sv = svd.singularValues();
  const MatrixXd &V = svd.matrixV();

  // calculate the reciprocal of Singular-Values
  // damp inverse with lambda so that inverse doesn't oscillate near solution
  size_t nSv = Sv.size();
  VectorXd inv_Sv(nSv);
  for(size_t i=0; i< nSv; ++i)
  {
    if (fabs(Sv(i)) > eps)
    {
      inv_Sv(i) = 1/Sv(i);
    }
    else
    {
      inv_Sv(i) = Sv(i) / (Sv(i)*Sv(i) + lambda*lambda);
    }
  }

  jacb_pseudo_inv = V * inv_Sv.asDiagonal() * U.transpose();
}

static void computeTwist(const Eigen::Affine3d& p0,
                                        const Eigen::Affine3d& pf,
                                        const Eigen::ArrayXi& nullity,Eigen::VectorXd& twist)
{
  twist.resize(nullity.size());
  twist.setConstant(0);
  Eigen::Vector3d twist_pos = pf.translation() - p0.translation();

  // relative rotation -> R = inverse(R0) * Rf
  Eigen::AngleAxisd relative_rot(p0.rotation().transpose() * pf.rotation());
  double angle = relative_rot.angle();
  Eigen::Vector3d axis = relative_rot.axis();

  // forcing angle to range [-pi , pi]
  while( (angle > M_PI) || (angle < -M_PI))
  {
    angle = (angle >  M_PI) ? (angle - 2*M_PI) : angle;
    angle = (angle < -M_PI )? (angle + 2*M_PI) : angle;
  }

  // creating twist rotation relative to tool
  Eigen::Vector3d twist_rot = axis * angle;

  // assigning into full 6dof twist vector
  twist.head(3) = twist_pos;
  twist.tail(3) = twist_rot;

  // zeroing all underconstrained cartesian dofs
  twist = (nullity == 0).select(0,twist);
}

namespace stomp_moveit
{
namespace noisy_filters
{

UnderconstrainedGoal::UnderconstrainedGoal():
    name_("UnderconstrainedGoal")
{

}

UnderconstrainedGoal::~UnderconstrainedGoal()
{
  // TODO Auto-generated destructor stub
}

bool UnderconstrainedGoal::initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
                        const std::string& group_name,const XmlRpc::XmlRpcValue& config)
{
  group_name_ = group_name;
  robot_model_ = robot_model_ptr;

  return configure(config);
}

bool UnderconstrainedGoal::configure(const XmlRpc::XmlRpcValue& config)
{
  using namespace XmlRpc;

  try
  {
    XmlRpcValue params = config;

    XmlRpcValue dof_nullity_param = params["constrained_dofs"];
    XmlRpcValue dof_thresholds_param = params["cartesian_convergence"];
    XmlRpcValue joint_updates_param = params["joint_update_rates"];
    if((dof_nullity_param.getType() != XmlRpcValue::TypeArray) ||
        dof_nullity_param.size() < DOF_SIZE ||
        dof_thresholds_param.getType() != XmlRpcValue::TypeArray ||
        dof_thresholds_param.size() < DOF_SIZE  ||
        joint_updates_param.getType() != XmlRpcValue::TypeArray ||
        joint_updates_param.size() == 0)
    {
      ROS_ERROR("UnderconstrainedGoal received invalid array parameters");
      return false;
    }

    dof_nullity_.resize(DOF_SIZE);
    for(auto i = 0u; i < dof_nullity_param.size(); i++)
    {
      dof_nullity_(i) = static_cast<int>(dof_nullity_param[i]);
    }

    cartesian_convergence_thresholds_.resize(DOF_SIZE);
    for(auto i = 0u; i < dof_thresholds_param.size(); i++)
    {
      cartesian_convergence_thresholds_(i) = static_cast<double>(dof_thresholds_param[i]);
    }

    joint_update_rates_.resize(joint_updates_param.size());
    for(auto i = 0u; i < joint_updates_param.size(); i++)
    {
      joint_update_rates_(i) = static_cast<double>(joint_updates_param[i]);
    }

    //update_weight_ = static_cast<double>(params["update_weight"]);
    max_iterations_ = static_cast<int>(params["max_ik_iterations"]);
  }
  catch(XmlRpc::XmlRpcException& e)
  {
    ROS_ERROR("UnderconstrainedGoal failed to load parameters, %s",e.getMessage().c_str());
    return false;
  }

  return true;
}

bool UnderconstrainedGoal::setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
                 const moveit_msgs::MotionPlanRequest &req,
                 const stomp_core::StompConfiguration &config,
                 moveit_msgs::MoveItErrorCodes& error_code)
{
  using namespace Eigen;
  using namespace moveit::core;

  const JointModelGroup* joint_group = robot_model_->getJointModelGroup(group_name_);
  tool_link_ = joint_group->getLinkModelNames().back();
  state_.reset(new RobotState(robot_model_));
  robotStateMsgToRobotState(req.start_state,*state_);

  const std::vector<moveit_msgs::Constraints>& goals = req.goal_constraints;
  if(goals.empty())
  {
    ROS_ERROR("A goal constraint was not provided");
    error_code.val = error_code.INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  // storing tool goal pose
  if(goals.front().position_constraints.empty() ||
      goals.front().orientation_constraints.empty())
  {
    ROS_WARN("A goal constraint for the tool link was not provided, using forward kinematics");

    // check joint constraints
    if(goals.front().joint_constraints.empty())
    {
      ROS_ERROR_STREAM("No joint values for the goal were found");
      error_code.val = error_code.INVALID_GOAL_CONSTRAINTS;
      return false;
    }

    // compute FK to obtain tool pose
    const std::vector<moveit_msgs::JointConstraint>& joint_constraints = goals.front().joint_constraints;

    // copying goal values into state
    for(auto& jc: joint_constraints)
    {
      state_->setVariablePosition(jc.joint_name,jc.position);
    }

    state_->update(true);
    state_->enforceBounds(joint_group);
    tool_goal_pose_ = state_->getGlobalLinkTransform(tool_link_);
  }
  else
  {
    // tool goal
    const moveit_msgs::PositionConstraint& pos_constraint = goals.front().position_constraints.front();
    const moveit_msgs::OrientationConstraint& orient_constraint = goals.front().orientation_constraints.front();

    geometry_msgs::Pose pose;
    pose.position = pos_constraint.constraint_region.primitive_poses[0].position;
    pose.orientation = orient_constraint.orientation;
    tf::poseMsgToEigen(pose,tool_goal_pose_);
  }

  error_code.val = error_code.SUCCESS;
  return true;
}

bool UnderconstrainedGoal::filter(std::size_t start_timestep,
                    std::size_t num_timesteps,
                    int iteration_number,
                    int rollout_number,
                    Eigen::MatrixXd& parameters,
                    bool& filtered)
{
  using namespace Eigen;
  using namespace moveit::core;

  VectorXd init_joint_pose = parameters.rightCols(1);
  VectorXd joint_pose;

  if(!runIK(tool_goal_pose_,init_joint_pose,joint_pose))
  {
    ROS_ERROR("UnderconstrainedGoal failed to find valid ik close to reference pose");
    filtered = false;
    return false;
  }

  filtered = true;
  parameters.rightCols(1) = joint_pose;

  return true;
}

bool UnderconstrainedGoal::runIK(const Eigen::Affine3d& tool_goal_pose,const Eigen::VectorXd& init_joint_pose,
                                       Eigen::VectorXd& joint_pose)
{
  using namespace Eigen;
  using namespace moveit::core;

  // joint variables
  VectorXd delta_j = VectorXd::Zero(init_joint_pose.size());
  joint_pose = init_joint_pose;
  const JointModelGroup* joint_group = robot_model_->getJointModelGroup(group_name_);
  state_->setJointGroupPositions(joint_group,joint_pose);
  Affine3d tool_current_pose = state_->getGlobalLinkTransform(tool_link_);

  // tool twist variables
  VectorXd tool_twist, tool_twist_reduced;
  std::vector<int> indices;
  for(auto i = 0u; i < dof_nullity_.size(); i++)
  {
    if(dof_nullity_(i) != 0)
    {
      indices.push_back(i);
    }
  }
  tool_twist_reduced = VectorXd::Zero(indices.size());

  // jacobian calculation variables
  MatrixXd identity = MatrixXd::Identity(init_joint_pose.size(),init_joint_pose.size());
  MatrixXd jacb, jacb_reduced, jacb_pseudo_inv;

  unsigned int iteration_count = 0;
  bool converged = false;
  while(iteration_count < max_iterations_)
  {

    // computing twist vector
    computeTwist(tool_current_pose,tool_goal_pose,dof_nullity_,tool_twist);

    // check convergence
    if((tool_twist.cwiseAbs().array() < cartesian_convergence_thresholds_).all())
    {
      // converged
      converged = true;
      ROS_DEBUG("Found numeric ik solution after %i iterations",iteration_count);
      break;
    }

    // updating reduced tool twist
    for(auto i = 0u; i < indices.size(); i++)
    {
      tool_twist_reduced(i) = tool_twist(indices[i]);
    }

    // computing jacobian
    if(!state_->getJacobian(joint_group,state_->getLinkModel(tool_link_),Vector3d::Zero(),jacb))
    {
      ROS_ERROR("Failed to get Jacobian for link %s",tool_link_.c_str());
      return false;
    }

    // transform jacobian rotational part to tool coordinates
    jacb.bottomRows(3) = tool_current_pose.rotation().transpose()*jacb.bottomRows(3);

    // reduce jacobian and compute its pseudo inverse
    reduceJacobian(jacb,indices,jacb_reduced);
    calculateDampedPseudoInverse(jacb_reduced,jacb_pseudo_inv,EPSILON,LAMBDA);

    // computing joint change
    delta_j = (jacb_pseudo_inv*tool_twist_reduced);

    // updating joint values
    joint_pose += (joint_update_rates_* delta_j.array()).matrix();

    // updating tool pose
    state_->setJointGroupPositions(joint_group,joint_pose);
    tool_current_pose = state_->getGlobalLinkTransform(tool_link_);

    iteration_count++;
  }

  ROS_DEBUG_STREAM("Final tool twist "<<tool_twist.transpose());

  return converged;
}

} /* namespace filters */
} /* namespace stomp_moveit */
<|endoftext|>


<commit_before>#include "Tracing.h"
#include "IRMutator.h"
#include "IROperator.h"
#include "runtime/HalideRuntime.h"

namespace Halide {
namespace Internal {

int tracing_level() {
    char *trace = getenv("HL_TRACE");
    return trace ? atoi(trace) : 0;
}

using std::vector;
using std::map;
using std::string;

class InjectTracing : public IRMutator {
public:
    const map<string, Function> &env;
    int global_level;
    InjectTracing(const map<string, Function> &e)
        : env(e),
          global_level(tracing_level()) {}

private:
    using IRMutator::visit;

    void visit(const Call *op) {

        // Calls inside of an address_of don't count, but we want to
        // visit the args of the inner call.
        if (op->call_type == Call::Intrinsic && op->name == Call::address_of) {
            internal_assert(op->args.size() == 1);
            const Call *c = op->args[0].as<Call>();
            const Load *l = op->args[0].as<Load>();

            internal_assert(c || l);

            std::vector<Expr> args;
            if (c) {
                args = c->args;
            } else {
                args.push_back(l->index);
            }

            bool unchanged = true;
            vector<Expr> new_args(args.size());
            for (size_t i = 0; i < args.size(); i++) {
                new_args[i] = mutate(args[i]);
                unchanged = unchanged && (new_args[i].same_as(args[i]));
            }

            if (unchanged) {
                expr = op;
                return;
            } else {
                Expr inner;
                if (c) {
                    inner = Call::make(c->type, c->name, new_args, c->call_type,
                                       c->func, c->value_index, c->image, c->param);
                } else {
                    Expr inner = Load::make(l->type, l->name, new_args[0], l->image, l->param);
                }
                expr = Call::make(Handle(), Call::address_of, {inner}, Call::Intrinsic);
                return;
            }
        }

        IRMutator::visit(op);
        op = expr.as<Call>();
        internal_assert(op);

        bool trace_it = false;
        Expr trace_parent;
        if (op->call_type == Call::Halide) {
            Function f = op->func;
            bool inlined = f.schedule().compute_level().is_inline();
            trace_it = f.is_tracing_loads() || (global_level > 2 && !inlined);
            trace_parent = Variable::make(Int(32), op->name + ".trace_id");
        } else if (op->call_type == Call::Image) {
            trace_it = global_level > 2;
            trace_parent = -1;
        }

        if (trace_it) {
            // Wrap the load in a call to trace_load
            vector<Expr> args;
            args.push_back(op->name);
            args.push_back(halide_trace_load);
            args.push_back(trace_parent);
            args.push_back(op->value_index);
            args.push_back(op);
            args.insert(args.end(), op->args.begin(), op->args.end());

            expr = Call::make(op->type, Call::trace_expr, args, Call::Intrinsic);
        }

    }

    void visit(const Provide *op) {
        IRMutator::visit(op);
        op = stmt.as<Provide>();
        internal_assert(op);

        map<string, Function>::const_iterator iter = env.find(op->name);
        if (iter == env.end()) return;
        Function f = iter->second;
        bool inlined = f.schedule().compute_level().is_inline();

        if (f.is_tracing_stores() || (global_level > 1 && !inlined)) {
            // Wrap each expr in a tracing call

            const vector<Expr> &values = op->values;
            vector<Expr> traces(op->values.size());

            for (size_t i = 0; i < values.size(); i++) {
                vector<Expr> args;
                args.push_back(f.name());
                args.push_back(halide_trace_store);
                args.push_back(Variable::make(Int(32), op->name + ".trace_id"));
                args.push_back((int)i);
                args.push_back(values[i]);
                args.insert(args.end(), op->args.begin(), op->args.end());
                traces[i] = Call::make(values[i].type(), Call::trace_expr, args, Call::Intrinsic);
            }

            stmt = Provide::make(op->name, traces, op->args);
        }
    }

    void visit(const Realize *op) {
        IRMutator::visit(op);
        op = stmt.as<Realize>();
        internal_assert(op);

        map<string, Function>::const_iterator iter = env.find(op->name);
        if (iter == env.end()) return;
        Function f = iter->second;
        if (f.is_tracing_realizations() || global_level > 0) {

            // Throw a tracing call before and after the realize body
            vector<Expr> args;
            args.push_back(op->name);
            args.push_back(halide_trace_begin_realization); // event type for begin realization
            args.push_back(0); // realization id
            args.push_back(0); // value index
            args.push_back(0); // value

            for (size_t i = 0; i < op->bounds.size(); i++) {
                args.push_back(op->bounds[i].min);
                args.push_back(op->bounds[i].extent);
            }

            // Begin realization returns a unique token to pass to further trace calls affecting this buffer.

            Expr call_before = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
            args[1] = halide_trace_end_realization;
            args[2] = Variable::make(Int(32), op->name + ".trace_id");
            Expr call_after = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
            Stmt new_body = op->body;
            new_body = Block::make(new_body, Evaluate::make(call_after));
            new_body = LetStmt::make(op->name + ".trace_id", call_before, new_body);
            stmt = Realize::make(op->name, op->types, op->bounds, op->condition, new_body);
        } else if (f.is_tracing_stores() || f.is_tracing_loads()) {
            // We need a trace id defined to pass to the loads and stores
            Stmt new_body = op->body;
            new_body = LetStmt::make(op->name + ".trace_id", 0, new_body);
            stmt = Realize::make(op->name, op->types, op->bounds, op->condition, new_body);
        }

    }

    void visit(const ProducerConsumer *op) {
        IRMutator::visit(op);
        op = stmt.as<ProducerConsumer>();
        internal_assert(op);
        map<string, Function>::const_iterator iter = env.find(op->name);
        if (iter == env.end()) return;
        Function f = iter->second;
        if (f.is_tracing_realizations() || global_level > 0) {
            // Throw a tracing call around each pipeline event
            vector<Expr> args;
            args.push_back(op->name);
            args.push_back(0);
            args.push_back(Variable::make(Int(32), op->name + ".trace_id"));
            args.push_back(0); // value index
            args.push_back(0); // value

            // Use the size of the pure step

            for (int i = 0; i < f.dimensions(); i++) {
                Expr min = Variable::make(Int(32), f.name() + ".s0." + f.args()[i] + ".min");
                Expr max = Variable::make(Int(32), f.name() + ".s0." + f.args()[i] + ".max");
                Expr extent = (max + 1) - min;
                args.push_back(min);
                args.push_back(extent);
            }

            Expr call;
            Stmt new_update;
            if (op->update.defined()) {
                args[1] = halide_trace_update;
                call = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
                new_update = Block::make(Evaluate::make(call), op->update);
            }

            args[1] = halide_trace_consume;
            call = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
            Stmt new_consume = Block::make(Evaluate::make(call), op->consume);

            args[1] = halide_trace_end_consume;
            call = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
            new_consume = Block::make(new_consume, Evaluate::make(call));

            stmt = ProducerConsumer::make(op->name, op->produce, new_update, new_consume);

            args[1] = halide_trace_produce;
            call = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
            stmt = LetStmt::make(f.name() + ".trace_id", call, stmt);
        }

    }
};

class RemoveRealizeOverOutput : public IRMutator {
    using IRMutator::visit;
    const vector<Function> &outputs;

    void visit(const Realize *op) {
        for (Function f : outputs) {
            if (op->name == f.name()) {
                stmt = mutate(op->body);
                return;
            }
        }
        IRMutator::visit(op);
    }

public:
    RemoveRealizeOverOutput(const vector<Function> &o) : outputs(o) {}
};

Stmt inject_tracing(Stmt s, const map<string, Function> &env, const vector<Function> &outputs) {
    Stmt original = s;
    InjectTracing tracing(env);

    // Add a dummy realize block for the output buffers
    for (Function output : outputs) {
        Region output_region;
        Parameter output_buf = output.output_buffers()[0];
        internal_assert(output_buf.is_buffer());
        for (int i = 0; i < output.dimensions(); i++) {
            string d = std::to_string(i);
            Expr min = Variable::make(Int(32), output_buf.name() + ".min." + d);
            Expr extent = Variable::make(Int(32), output_buf.name() + ".extent." + d);
            output_region.push_back(Range(min, extent));
        }
        s = Realize::make(output.name(), output.output_types(), output_region, const_true(), s);
    }

    // Inject tracing calls
    s = tracing.mutate(s);

    // Strip off the dummy realize blocks
    s = RemoveRealizeOverOutput(outputs).mutate(s);

    // Unless tracing was a no-op, add a call to shut down the trace
    // (which flushes the output stream)
    if (!s.same_as(original)) {
        Expr flush = Call::make(Int(32), "halide_shutdown_trace", vector<Expr>(), Call::Extern);
        s = Block::make(s, Evaluate::make(flush));
    }
    return s;
}

}
}
<commit_msg>Trace inlined reductions too<commit_after>#include "Tracing.h"
#include "IRMutator.h"
#include "IROperator.h"
#include "runtime/HalideRuntime.h"

namespace Halide {
namespace Internal {

int tracing_level() {
    char *trace = getenv("HL_TRACE");
    return trace ? atoi(trace) : 0;
}

using std::vector;
using std::map;
using std::string;

class InjectTracing : public IRMutator {
public:
    const map<string, Function> &env;
    int global_level;
    InjectTracing(const map<string, Function> &e)
        : env(e),
          global_level(tracing_level()) {}

private:
    using IRMutator::visit;

    void visit(const Call *op) {

        // Calls inside of an address_of don't count, but we want to
        // visit the args of the inner call.
        if (op->call_type == Call::Intrinsic && op->name == Call::address_of) {
            internal_assert(op->args.size() == 1);
            const Call *c = op->args[0].as<Call>();
            const Load *l = op->args[0].as<Load>();

            internal_assert(c || l);

            std::vector<Expr> args;
            if (c) {
                args = c->args;
            } else {
                args.push_back(l->index);
            }

            bool unchanged = true;
            vector<Expr> new_args(args.size());
            for (size_t i = 0; i < args.size(); i++) {
                new_args[i] = mutate(args[i]);
                unchanged = unchanged && (new_args[i].same_as(args[i]));
            }

            if (unchanged) {
                expr = op;
                return;
            } else {
                Expr inner;
                if (c) {
                    inner = Call::make(c->type, c->name, new_args, c->call_type,
                                       c->func, c->value_index, c->image, c->param);
                } else {
                    Expr inner = Load::make(l->type, l->name, new_args[0], l->image, l->param);
                }
                expr = Call::make(Handle(), Call::address_of, {inner}, Call::Intrinsic);
                return;
            }
        }

        IRMutator::visit(op);
        op = expr.as<Call>();
        internal_assert(op);

        bool trace_it = false;
        Expr trace_parent;
        if (op->call_type == Call::Halide) {
            Function f = op->func;
            bool inlined = f.schedule().compute_level().is_inline();
            if (f.has_update_definition()) inlined = false;
            trace_it = f.is_tracing_loads() || (global_level > 2 && !inlined);
            trace_parent = Variable::make(Int(32), op->name + ".trace_id");
        } else if (op->call_type == Call::Image) {
            trace_it = global_level > 2;
            trace_parent = -1;
        }

        if (trace_it) {
            // Wrap the load in a call to trace_load
            vector<Expr> args;
            args.push_back(op->name);
            args.push_back(halide_trace_load);
            args.push_back(trace_parent);
            args.push_back(op->value_index);
            args.push_back(op);
            args.insert(args.end(), op->args.begin(), op->args.end());

            expr = Call::make(op->type, Call::trace_expr, args, Call::Intrinsic);
        }

    }

    void visit(const Provide *op) {
        IRMutator::visit(op);
        op = stmt.as<Provide>();
        internal_assert(op);

        map<string, Function>::const_iterator iter = env.find(op->name);
        if (iter == env.end()) return;
        Function f = iter->second;
        bool inlined = f.schedule().compute_level().is_inline();
        if (f.has_update_definition()) inlined = false;

        if (f.is_tracing_stores() || (global_level > 1 && !inlined)) {
            // Wrap each expr in a tracing call

            const vector<Expr> &values = op->values;
            vector<Expr> traces(op->values.size());

            for (size_t i = 0; i < values.size(); i++) {
                vector<Expr> args;
                args.push_back(f.name());
                args.push_back(halide_trace_store);
                args.push_back(Variable::make(Int(32), op->name + ".trace_id"));
                args.push_back((int)i);
                args.push_back(values[i]);
                args.insert(args.end(), op->args.begin(), op->args.end());
                traces[i] = Call::make(values[i].type(), Call::trace_expr, args, Call::Intrinsic);
            }

            stmt = Provide::make(op->name, traces, op->args);
        }
    }

    void visit(const Realize *op) {
        IRMutator::visit(op);
        op = stmt.as<Realize>();
        internal_assert(op);

        map<string, Function>::const_iterator iter = env.find(op->name);
        if (iter == env.end()) return;
        Function f = iter->second;
        if (f.is_tracing_realizations() || global_level > 0) {

            // Throw a tracing call before and after the realize body
            vector<Expr> args;
            args.push_back(op->name);
            args.push_back(halide_trace_begin_realization); // event type for begin realization
            args.push_back(0); // realization id
            args.push_back(0); // value index
            args.push_back(0); // value

            for (size_t i = 0; i < op->bounds.size(); i++) {
                args.push_back(op->bounds[i].min);
                args.push_back(op->bounds[i].extent);
            }

            // Begin realization returns a unique token to pass to further trace calls affecting this buffer.

            Expr call_before = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
            args[1] = halide_trace_end_realization;
            args[2] = Variable::make(Int(32), op->name + ".trace_id");
            Expr call_after = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
            Stmt new_body = op->body;
            new_body = Block::make(new_body, Evaluate::make(call_after));
            new_body = LetStmt::make(op->name + ".trace_id", call_before, new_body);
            stmt = Realize::make(op->name, op->types, op->bounds, op->condition, new_body);
        } else if (f.is_tracing_stores() || f.is_tracing_loads()) {
            // We need a trace id defined to pass to the loads and stores
            Stmt new_body = op->body;
            new_body = LetStmt::make(op->name + ".trace_id", 0, new_body);
            stmt = Realize::make(op->name, op->types, op->bounds, op->condition, new_body);
        }

    }

    void visit(const ProducerConsumer *op) {
        IRMutator::visit(op);
        op = stmt.as<ProducerConsumer>();
        internal_assert(op);
        map<string, Function>::const_iterator iter = env.find(op->name);
        if (iter == env.end()) return;
        Function f = iter->second;
        if (f.is_tracing_realizations() || global_level > 0) {
            // Throw a tracing call around each pipeline event
            vector<Expr> args;
            args.push_back(op->name);
            args.push_back(0);
            args.push_back(Variable::make(Int(32), op->name + ".trace_id"));
            args.push_back(0); // value index
            args.push_back(0); // value

            // Use the size of the pure step

            for (int i = 0; i < f.dimensions(); i++) {
                Expr min = Variable::make(Int(32), f.name() + ".s0." + f.args()[i] + ".min");
                Expr max = Variable::make(Int(32), f.name() + ".s0." + f.args()[i] + ".max");
                Expr extent = (max + 1) - min;
                args.push_back(min);
                args.push_back(extent);
            }

            Expr call;
            Stmt new_update;
            if (op->update.defined()) {
                args[1] = halide_trace_update;
                call = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
                new_update = Block::make(Evaluate::make(call), op->update);
            }

            args[1] = halide_trace_consume;
            call = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
            Stmt new_consume = Block::make(Evaluate::make(call), op->consume);

            args[1] = halide_trace_end_consume;
            call = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
            new_consume = Block::make(new_consume, Evaluate::make(call));

            stmt = ProducerConsumer::make(op->name, op->produce, new_update, new_consume);

            args[1] = halide_trace_produce;
            call = Call::make(Int(32), Call::trace, args, Call::Intrinsic);
            stmt = LetStmt::make(f.name() + ".trace_id", call, stmt);
        }

    }
};

class RemoveRealizeOverOutput : public IRMutator {
    using IRMutator::visit;
    const vector<Function> &outputs;

    void visit(const Realize *op) {
        for (Function f : outputs) {
            if (op->name == f.name()) {
                stmt = mutate(op->body);
                return;
            }
        }
        IRMutator::visit(op);
    }

public:
    RemoveRealizeOverOutput(const vector<Function> &o) : outputs(o) {}
};

Stmt inject_tracing(Stmt s, const map<string, Function> &env, const vector<Function> &outputs) {
    Stmt original = s;
    InjectTracing tracing(env);

    // Add a dummy realize block for the output buffers
    for (Function output : outputs) {
        Region output_region;
        Parameter output_buf = output.output_buffers()[0];
        internal_assert(output_buf.is_buffer());
        for (int i = 0; i < output.dimensions(); i++) {
            string d = std::to_string(i);
            Expr min = Variable::make(Int(32), output_buf.name() + ".min." + d);
            Expr extent = Variable::make(Int(32), output_buf.name() + ".extent." + d);
            output_region.push_back(Range(min, extent));
        }
        s = Realize::make(output.name(), output.output_types(), output_region, const_true(), s);
    }

    // Inject tracing calls
    s = tracing.mutate(s);

    // Strip off the dummy realize blocks
    s = RemoveRealizeOverOutput(outputs).mutate(s);

    // Unless tracing was a no-op, add a call to shut down the trace
    // (which flushes the output stream)
    if (!s.same_as(original)) {
        Expr flush = Call::make(Int(32), "halide_shutdown_trace", vector<Expr>(), Call::Extern);
        s = Block::make(s, Evaluate::make(flush));
    }
    return s;
}

}
}
<|endoftext|>


<commit_before>#include "Version.h"

#include "Hash.h"

#include <ostream>
#include <sstream>
#include <iostream>
#include <string>
#include <regex>

using namespace Arbiter;

size_t std::hash<ArbiterSemanticVersion>::operator() (const ArbiterSemanticVersion &version) const
{
  return hashOf(version._major)
    ^ hashOf(version._minor)
    ^ hashOf(version._patch)
    ^ hashOf(version._prereleaseVersion)
    ^ hashOf(version._buildMetadata);
}

size_t std::hash<ArbiterSelectedVersion>::operator() (const ArbiterSelectedVersion &version) const
{
  return hashOf(version._semanticVersion);
}

Optional<ArbiterSemanticVersion> ArbiterSemanticVersion::fromString (const std::string &versionString)
{
  // Versions and identifiers cannot have a leading zero.
  #define VERSION "(0|[1-9][0-9]*)"
  #define IDENTIFIER "(?:0|[1-9A-Za-z-][0-9A-Za-z-]*)"
  #define DOT_SEPARATED_IDENTIFIER "(" IDENTIFIER "(?:\\." IDENTIFIER ")*)"

  std::regex pattern {
    VERSION "\\." VERSION "\\." VERSION
    // prerelease begins with a hyphen followed by a dot separated identifier
    "(?:"   "-" DOT_SEPARATED_IDENTIFIER ")?"
    // metadata begins with a plus sign followed by a dot separated identifier
    "(?:" "\\+" DOT_SEPARATED_IDENTIFIER ")?"
  };

  #undef DOT_SEPARATED_IDENTIFIER
  #undef IDENTIFIER
  #undef VERSION

  std::smatch match;
  if (!std::regex_match(versionString, match, pattern)) {
    return Optional<ArbiterSemanticVersion>();
  }

  unsigned major = stoi(match.str(1));
  unsigned minor = stoi(match.str(2));
  unsigned patch = stoi(match.str(3));

  Optional<std::string> prereleaseVersion;
  Optional<std::string> buildMetadata;

  if (match.length(4) > 0) {
    prereleaseVersion = Optional<std::string>(match.str(4));
  }
  if (match.length(5) > 0) {
    buildMetadata = Optional<std::string>(match.str(5));
  }

  return ArbiterSemanticVersion(major, minor, patch, prereleaseVersion, buildMetadata);
}

bool ArbiterSemanticVersion::operator< (const ArbiterSemanticVersion &other) const noexcept
{
  if (_major < other._major) {
    return true;
  } else if (_major > other._major) {
    return false;
  }

  if (_minor < other._minor) {
    return true;
  } else if (_minor > other._minor) {
    return false;
  }

  if (_patch < other._patch) {
    return true;
  } else if (_patch > other._patch) {
    return false;
  }

  if (_prereleaseVersion) {
    if (!other._prereleaseVersion) {
      return true;
    }

    // FIXME: This should compare numbers naturally, not lexically
    return _prereleaseVersion.value() < other._prereleaseVersion.value();
  }

  // Build metadata does not participate in precedence.
  return false;
}

std::ostream &operator<< (std::ostream &os, const ArbiterSemanticVersion &version)
{
  os << version._major << '.' << version._minor << '.' << version._patch;

  if (version._prereleaseVersion) {
    os << '-' << version._prereleaseVersion.value();
  }

  if (version._buildMetadata) {
    os << '+' << version._buildMetadata.value();
  }

  return os;
}

std::ostream &operator<< (std::ostream &os, const ArbiterSelectedVersion &version)
{
  return os << version._semanticVersion << " (" << version._metadata << ")";
}

std::ostream &operator<< (std::ostream &os, const ArbiterSelectedVersionList &versionList)
{
  os << "Version list:";

  for (const auto &version : versionList._versions) {
    os << "\n" << version;
  }

  return os;
}

ArbiterSemanticVersion *ArbiterCreateSemanticVersion (unsigned major, unsigned minor, unsigned patch, const char *prereleaseVersion, const char *buildMetadata)
{
  return new ArbiterSemanticVersion(
    major,
    minor,
    patch,
    (prereleaseVersion ? Optional<std::string>(prereleaseVersion) : Optional<std::string>()),
    (buildMetadata ? Optional<std::string>(buildMetadata) : Optional<std::string>())
  );
}

ArbiterSemanticVersion *ArbiterCreateSemanticVersionFromString (const char *string)
{
  auto version = ArbiterSemanticVersion::fromString(string);
  if (version) {
    return new ArbiterSemanticVersion(std::move(version.value()));
  } else {
    return nullptr;
  }
}

void ArbiterFreeSemanticVersion (ArbiterSemanticVersion *version)
{
  delete version;
}

unsigned ArbiterGetMajorVersion (const ArbiterSemanticVersion *version)
{
  return version->_major;
}

unsigned ArbiterGetMinorVersion (const ArbiterSemanticVersion *version)
{
  return version->_minor;
}

unsigned ArbiterGetPatchVersion (const ArbiterSemanticVersion *version)
{
  return version->_patch;
}

const char *ArbiterGetPrereleaseVersion (const ArbiterSemanticVersion *version)
{
  if (version->_prereleaseVersion) {
    return version->_prereleaseVersion->c_str();
  } else {
    return nullptr;
  }
}

const char *ArbiterGetBuildMetadata (const ArbiterSemanticVersion *version)
{
  if (version->_buildMetadata) {
    return version->_buildMetadata->c_str();
  } else {
    return nullptr;
  }
}

bool ArbiterEqualVersions (const ArbiterSemanticVersion *lhs, const ArbiterSemanticVersion *rhs)
{
  return *lhs == *rhs;
}

int ArbiterCompareVersionOrdering (const ArbiterSemanticVersion *lhs, const ArbiterSemanticVersion *rhs)
{
  if (*lhs < *rhs) {
    return -1;
  } else if (*lhs > *rhs) {
    return 1;
  } else {
    return 0;
  }
}

ArbiterSelectedVersion *ArbiterCreateSelectedVersion (const ArbiterSemanticVersion *semanticVersion, ArbiterUserValue metadata)
{
  return new ArbiterSelectedVersion(*semanticVersion, ArbiterSelectedVersion::Metadata(metadata));
}

const ArbiterSemanticVersion *ArbiterSelectedVersionSemanticVersion (const ArbiterSelectedVersion *version)
{
  return &version->_semanticVersion;
}

const void *ArbiterSelectedVersionMetadata (const ArbiterSelectedVersion *version)
{
  return version->_metadata.data();
}

bool ArbiterEqualSelectedVersions (const ArbiterSelectedVersion *lhs, const ArbiterSelectedVersion *rhs)
{
  return *lhs == *rhs;
}

void ArbiterFreeSelectedVersion (ArbiterSelectedVersion *version)
{
  delete version;
}
<commit_msg>Less verbose pattern name<commit_after>#include "Version.h"

#include "Hash.h"

#include <ostream>
#include <sstream>
#include <iostream>
#include <string>
#include <regex>

using namespace Arbiter;

size_t std::hash<ArbiterSemanticVersion>::operator() (const ArbiterSemanticVersion &version) const
{
  return hashOf(version._major)
    ^ hashOf(version._minor)
    ^ hashOf(version._patch)
    ^ hashOf(version._prereleaseVersion)
    ^ hashOf(version._buildMetadata);
}

size_t std::hash<ArbiterSelectedVersion>::operator() (const ArbiterSelectedVersion &version) const
{
  return hashOf(version._semanticVersion);
}

Optional<ArbiterSemanticVersion> ArbiterSemanticVersion::fromString (const std::string &versionString)
{
  // Versions and identifiers cannot have a leading zero.
  #define VERSION "(0|[1-9][0-9]*)"
  #define IDENTIFIER "(?:0|[1-9A-Za-z-][0-9A-Za-z-]*)"
  #define DOTTED_IDENTIFIER "(" IDENTIFIER "(?:\\." IDENTIFIER ")*)"

  std::regex pattern {
    VERSION "\\." VERSION "\\." VERSION
    // prerelease begins with a hyphen followed by a dot separated identifier
    "(?:"   "-" DOTTED_IDENTIFIER ")?"
    // metadata begins with a plus sign followed by a dot separated identifier
    "(?:" "\\+" DOTTED_IDENTIFIER ")?"
  };

  #undef DOTTED_IDENTIFIER
  #undef IDENTIFIER
  #undef VERSION

  std::smatch match;
  if (!std::regex_match(versionString, match, pattern)) {
    return Optional<ArbiterSemanticVersion>();
  }

  unsigned major = stoi(match.str(1));
  unsigned minor = stoi(match.str(2));
  unsigned patch = stoi(match.str(3));

  Optional<std::string> prereleaseVersion;
  Optional<std::string> buildMetadata;

  if (match.length(4) > 0) {
    prereleaseVersion = Optional<std::string>(match.str(4));
  }
  if (match.length(5) > 0) {
    buildMetadata = Optional<std::string>(match.str(5));
  }

  return ArbiterSemanticVersion(major, minor, patch, prereleaseVersion, buildMetadata);
}

bool ArbiterSemanticVersion::operator< (const ArbiterSemanticVersion &other) const noexcept
{
  if (_major < other._major) {
    return true;
  } else if (_major > other._major) {
    return false;
  }

  if (_minor < other._minor) {
    return true;
  } else if (_minor > other._minor) {
    return false;
  }

  if (_patch < other._patch) {
    return true;
  } else if (_patch > other._patch) {
    return false;
  }

  if (_prereleaseVersion) {
    if (!other._prereleaseVersion) {
      return true;
    }

    // FIXME: This should compare numbers naturally, not lexically
    return _prereleaseVersion.value() < other._prereleaseVersion.value();
  }

  // Build metadata does not participate in precedence.
  return false;
}

std::ostream &operator<< (std::ostream &os, const ArbiterSemanticVersion &version)
{
  os << version._major << '.' << version._minor << '.' << version._patch;

  if (version._prereleaseVersion) {
    os << '-' << version._prereleaseVersion.value();
  }

  if (version._buildMetadata) {
    os << '+' << version._buildMetadata.value();
  }

  return os;
}

std::ostream &operator<< (std::ostream &os, const ArbiterSelectedVersion &version)
{
  return os << version._semanticVersion << " (" << version._metadata << ")";
}

std::ostream &operator<< (std::ostream &os, const ArbiterSelectedVersionList &versionList)
{
  os << "Version list:";

  for (const auto &version : versionList._versions) {
    os << "\n" << version;
  }

  return os;
}

ArbiterSemanticVersion *ArbiterCreateSemanticVersion (unsigned major, unsigned minor, unsigned patch, const char *prereleaseVersion, const char *buildMetadata)
{
  return new ArbiterSemanticVersion(
    major,
    minor,
    patch,
    (prereleaseVersion ? Optional<std::string>(prereleaseVersion) : Optional<std::string>()),
    (buildMetadata ? Optional<std::string>(buildMetadata) : Optional<std::string>())
  );
}

ArbiterSemanticVersion *ArbiterCreateSemanticVersionFromString (const char *string)
{
  auto version = ArbiterSemanticVersion::fromString(string);
  if (version) {
    return new ArbiterSemanticVersion(std::move(version.value()));
  } else {
    return nullptr;
  }
}

void ArbiterFreeSemanticVersion (ArbiterSemanticVersion *version)
{
  delete version;
}

unsigned ArbiterGetMajorVersion (const ArbiterSemanticVersion *version)
{
  return version->_major;
}

unsigned ArbiterGetMinorVersion (const ArbiterSemanticVersion *version)
{
  return version->_minor;
}

unsigned ArbiterGetPatchVersion (const ArbiterSemanticVersion *version)
{
  return version->_patch;
}

const char *ArbiterGetPrereleaseVersion (const ArbiterSemanticVersion *version)
{
  if (version->_prereleaseVersion) {
    return version->_prereleaseVersion->c_str();
  } else {
    return nullptr;
  }
}

const char *ArbiterGetBuildMetadata (const ArbiterSemanticVersion *version)
{
  if (version->_buildMetadata) {
    return version->_buildMetadata->c_str();
  } else {
    return nullptr;
  }
}

bool ArbiterEqualVersions (const ArbiterSemanticVersion *lhs, const ArbiterSemanticVersion *rhs)
{
  return *lhs == *rhs;
}

int ArbiterCompareVersionOrdering (const ArbiterSemanticVersion *lhs, const ArbiterSemanticVersion *rhs)
{
  if (*lhs < *rhs) {
    return -1;
  } else if (*lhs > *rhs) {
    return 1;
  } else {
    return 0;
  }
}

ArbiterSelectedVersion *ArbiterCreateSelectedVersion (const ArbiterSemanticVersion *semanticVersion, ArbiterUserValue metadata)
{
  return new ArbiterSelectedVersion(*semanticVersion, ArbiterSelectedVersion::Metadata(metadata));
}

const ArbiterSemanticVersion *ArbiterSelectedVersionSemanticVersion (const ArbiterSelectedVersion *version)
{
  return &version->_semanticVersion;
}

const void *ArbiterSelectedVersionMetadata (const ArbiterSelectedVersion *version)
{
  return version->_metadata.data();
}

bool ArbiterEqualSelectedVersions (const ArbiterSelectedVersion *lhs, const ArbiterSelectedVersion *rhs)
{
  return *lhs == *rhs;
}

void ArbiterFreeSelectedVersion (ArbiterSelectedVersion *version)
{
  delete version;
}
<|endoftext|>


<commit_before>#include "ascii_in.h"

#include <iostream>

namespace redsea {

AsciiBits::AsciiBits() : is_eof_(false) {

}

AsciiBits::~AsciiBits() {

}

int AsciiBits::getNextBit() {
  int result = 0;
  while (result != '0' && result != '1' && result != EOF)
    result = getchar();

  if (result == EOF) {
    is_eof_ = true;
    return 0;
  }

  return (result == '1');

}

bool AsciiBits::isEOF() const {
  return is_eof_;
}

std::vector<uint16_t> getNextGroupRSpy() {
  std::vector<uint16_t> result;

  bool finished = false;

  while (! (finished || std::cin.eof())) {
    std::string line;
    std::getline(std::cin, line);
    if (line.length() < 16)
      continue;

    for (int nblok=0; nblok<4; nblok++) {
      uint16_t bval=0;
      int nyb=0;

      while (nyb < 4) {

        if (line.length() < 1) {
          finished = true;
          break;
        }

        std::string single = line.substr(0,1);

        if (single.compare(" ") != 0) {
          try {
            int nval = std::stoi(std::string(single), nullptr, 16);
            bval = (bval << 4) + nval;
            nyb++;
          } catch (std::invalid_argument) {
            finished = true;
            break;
          }
        }
        line = line.substr(1);
      }

      if (finished)
        break;

      result.push_back(bval);

      if (nblok==3)
        finished = true;
    }
  }

  return result;

}

} // namespace redsea
<commit_msg>fix compile error on RasPi<commit_after>#include "ascii_in.h"

#include <iostream>
#include <stdexcept>

namespace redsea {

AsciiBits::AsciiBits() : is_eof_(false) {

}

AsciiBits::~AsciiBits() {

}

int AsciiBits::getNextBit() {
  int result = 0;
  while (result != '0' && result != '1' && result != EOF)
    result = getchar();

  if (result == EOF) {
    is_eof_ = true;
    return 0;
  }

  return (result == '1');

}

bool AsciiBits::isEOF() const {
  return is_eof_;
}

std::vector<uint16_t> getNextGroupRSpy() {
  std::vector<uint16_t> result;

  bool finished = false;

  while (! (finished || std::cin.eof())) {
    std::string line;
    std::getline(std::cin, line);
    if (line.length() < 16)
      continue;

    for (int nblok=0; nblok<4; nblok++) {
      uint16_t bval=0;
      int nyb=0;

      while (nyb < 4) {

        if (line.length() < 1) {
          finished = true;
          break;
        }

        std::string single = line.substr(0,1);

        if (single.compare(" ") != 0) {
          try {
            int nval = std::stoi(std::string(single), nullptr, 16);
            bval = (bval << 4) + nval;
            nyb++;
          } catch (std::invalid_argument) {
            finished = true;
            break;
          }
        }
        line = line.substr(1);
      }

      if (finished)
        break;

      result.push_back(bval);

      if (nblok==3)
        finished = true;
    }
  }

  return result;

}

} // namespace redsea
<|endoftext|>