text:
#include <algorithm>
#include <ranges>
#include <tuple>

#include <renderer/renderer.hxx>

#include "graphics/graphics_pipeline.hxx"

namespace renderer
{
    template<class T>
    template<class L, class R> requires mpl::are_same_v<T, std::remove_cvref_t<L>, std::remove_cvref_t<R>>
    constexpr bool draw_commands_holder::comparator<T>::operator() (L &&lhs, R &&rhs) const
    {
        if constexpr (std::is_same_v<T, renderer::indexed_draw_command>) {
            if (lhs.index_buffer->index_type() != rhs.index_buffer->index_type())
                return lhs.index_buffer->index_type() < rhs.index_buffer->index_type();

            if (lhs.index_buffer->device_buffer()->handle() != rhs.index_buffer->device_buffer()->handle())
                return lhs.index_buffer->device_buffer()->handle() < rhs.index_buffer->device_buffer()->handle();
        }

        if (lhs.vertex_input_binding_index != rhs.vertex_input_binding_index)
            return lhs.vertex_input_binding_index < rhs.vertex_input_binding_index;

        if (lhs.vertex_buffer->device_buffer()->handle() != rhs.vertex_buffer->device_buffer()->handle())
            return lhs.vertex_buffer->device_buffer()->handle() < rhs.vertex_buffer->device_buffer()->handle();

        return lhs.first_vertex < rhs.first_vertex;
    }

    void draw_commands_holder::add_draw_command(renderer::nonindexed_draw_command const &draw_command)
    {
        nonindexed_draw_commands_.push_back(draw_command);
    }

    void draw_commands_holder::add_draw_command(renderer::indexed_draw_command const &draw_command)
    {
        indexed_draw_commands_.push_back(draw_command);
    }

    std::vector<renderer::vertex_buffers_bind_range>
    draw_commands_holder::get_primitives_buffers_bind_ranges()
    {
        auto &draw_commands = nonindexed_draw_commands_;

        std::stable_sort(std::begin(draw_commands), std::end(draw_commands), comparator<renderer::nonindexed_draw_command>{});

        std::vector<renderer::vertex_buffers_bind_range> buffers_bind_range;

        partion_vertex_buffers_binds<renderer::nonindexed_draw_command>(draw_commands, [&buffers_bind_range] (auto &&buffer_handles, auto range)
        {
            buffers_bind_range.push_back({
                range.front().vertex_input_binding_index,
                buffer_handles,
                std::vector<VkDeviceSize>(std::size(buffer_handles), 0u),
                range
            });
        });

        return buffers_bind_range;
    }

    std::vector<renderer::indexed_primitives_buffers_bind_range>
    draw_commands_holder::get_indexed_primitives_buffers_bind_range()
    {
        auto &draw_commands = indexed_draw_commands_;

        std::stable_sort(std::begin(draw_commands), std::end(draw_commands), comparator<renderer::indexed_draw_command>{});

        std::vector<renderer::indexed_primitives_buffers_bind_range> indexed_buffers_bind_range;

        for (auto it_begin = std::begin(draw_commands); it_begin != std::end(draw_commands);) {
            auto it = std::adjacent_find(it_begin, std::end(draw_commands), [] (auto &&lhs, auto &&rhs)
            {
                if (lhs.index_buffer->device_buffer()->handle() != rhs.index_buffer->device_buffer()->handle())
                    return true;

                return lhs.index_buffer->index_type() != rhs.index_buffer->index_type();
            });

            if (it != std::end(draw_commands))
                it = std::next(it);

            std::vector<renderer::vertex_buffers_bind_range> vertex_buffers_bind_ranges;

            partion_vertex_buffers_binds<renderer::indexed_draw_command>(std::span{it_begin, it}, [&vertex_buffers_bind_ranges] (auto &&buffer_handles, auto range)
            {
                vertex_buffers_bind_ranges.push_back({
                    range.front().vertex_input_binding_index,
                    buffer_handles,
                    std::vector<VkDeviceSize>(std::size(buffer_handles), 0u),
                    range
                });
            });
;
            indexed_buffers_bind_range.push_back({
                it_begin->index_buffer->index_type(),
                it_begin->index_buffer->device_buffer()->handle(),
                0u,
                vertex_buffers_bind_ranges
            });

            it_begin = it;
        }

        return indexed_buffers_bind_range;
    }

    template<class T>
    void draw_commands_holder::partion_vertex_buffers_binds(std::span<T> draw_commands, std::function<void(std::vector<VkBuffer> &&, std::span<T>)> callback)
    {
        for (auto it_begin = std::begin(draw_commands); it_begin != std::end(draw_commands);) {
            auto h = it_begin->vertex_buffer->device_buffer()->handle();
            auto i = it_begin->vertex_input_binding_index;

            std::vector<VkBuffer> buffer_handles(1, h);

            auto it_end = std::stable_partition(it_begin, std::end(draw_commands), [&h, &i, &buffer_handles] (auto &&b)
            {
                if (i == b.vertex_input_binding_index)
                    return h == b.vertex_buffer->device_buffer()->handle();

                else if (b.vertex_input_binding_index - i == 1) {
                    h = b.vertex_buffer->device_buffer()->handle();
                    i = b.vertex_input_binding_index;

                    buffer_handles.push_back(h);

                    return true;
                }

                return false;
            });

            callback(std::move(buffer_handles), std::span{it_begin, it_end});

            it_begin = it_end;
        }
    }

    void draw_commands_holder::clear()
    {
        nonindexed_draw_commands_.clear();
        indexed_draw_commands_.clear();
    }
}

#if 0

#include <algorithm>
#include <iostream>
#include <variant>
#include <memory>
#include <string>
#include <vector>
#include <ranges>
#include <span>
#include <map>
#include <set>

#include <boost/range/algorithm.hpp>

enum INDEX_TYPE {
    UNDEFINED = 0,
    UINT_16,
    UINT_32
};

namespace resource
{
    struct vertex_buffer final {
        vertex_buffer(std::uint32_t h = 0) : handle{h} { }
        std::uint32_t handle;
    };

    struct index_buffer final {
        index_buffer(std::uint32_t h, INDEX_TYPE i) : handle{h}, index_type{i} { }
        INDEX_TYPE index_type;
        std::uint32_t handle;
    };
}

namespace renderer
{
    struct nonindexed_draw_command final {
        std::shared_ptr<resource::vertex_buffer> vertex_buffer;
        std::uint32_t vertex_input_binding_index{0};
        std::uint32_t first_vertex{0};
    };

    struct indexed_draw_command final {
        std::shared_ptr<resource::vertex_buffer> vertex_buffer;
        std::shared_ptr<resource::index_buffer> index_buffer;
        std::uint32_t vertex_input_binding_index{0};
        std::uint32_t first_vertex{0};
        std::uint32_t first_index{0};
    };

    struct vertex_buffers_bind_range final {
        std::uint32_t first_binding;
        std::vector<std::uint32_t> buffer_handles;
        std::vector<std::uint32_t> buffer_offsets;
        std::span<renderer::nonindexed_draw_command> draw_commands;
    };

    struct indexed_primitives_buffers_bind_range final {
        INDEX_TYPE index_type;
        std::uint32_t index_buffer_handle;
        std::uint32_t index_buffer_offset;
        std::span<renderer::nonindexed_draw_command> draw_commands;
    };

    template<class T>
    struct comparator final {
        template<class L, class R>
        constexpr bool operator() (L &&lhs, R &&rhs) const
        {
            if constexpr (std::is_same_v<T, renderer::indexed_draw_command>) {
                if (lhs.index_buffer->index_type != rhs.index_buffer->index_type)
                    return lhs.index_buffer->index_type < rhs.index_buffer->index_type;

                if (lhs.index_buffer->handle != rhs.index_buffer->handle)
                    return lhs.index_buffer->handle < rhs.index_buffer->handle;
            }

            if (lhs.vertex_input_binding_index != rhs.vertex_input_binding_index)
                return lhs.vertex_input_binding_index < rhs.vertex_input_binding_index;

            if (lhs.vertex_buffer->handle != rhs.vertex_buffer->handle)
                return lhs.vertex_buffer->handle < rhs.vertex_buffer->handle;

            return lhs.first_vertex < rhs.first_vertex;
        }
    };
}

namespace renderer
{
    std::ostream &operator<<(std::ostream &out, renderer::nonindexed_draw_command const &c)
    {
        return out << "h" << c.vertex_buffer->handle << " l" << c.vertex_input_binding_index;// << " v" << c.first_vertex;
    }
    std::ostream &operator<<(std::ostream &out, renderer::indexed_draw_command const &c)
    {
        return out << "h" << c.index_buffer->handle << "idx" << c.index_buffer->index_type << " h" << c.vertex_buffer->handle << "L" << c.vertex_input_binding_index;// << " v" << c.first_vertex;
    }
    std::ostream &operator<<(std::ostream &out, renderer::vertex_buffers_bind_range const &r)
    {
        std::ranges::copy(r.buffer_handles, std::ostream_iterator<int>(out << "fbnd " << r.first_binding << " [", " "));
        return out << "]";
    }
    // std::ostream &operator<<(std::ostream &out, renderer::indexed_primitives_buffers_bind_range const &r)
    // {
    //     out << "idx " << r.index_type <<
    //     std::ranges::copy(r.buffer_handles, std::ostream_iterator<int>(out << "fbnd " << r.first_binding << " [", " "));
    //     return out << "]";
    // }
}

template<class T>
auto get_primitives_buffers_bind_range(std::span<T> draw_commands)
{
    ;
}

std::vector<renderer::vertex_buffers_bind_range> get_primitives_buffers_bind_ranges(std::span<renderer::nonindexed_draw_command> nonindexed_draw_commands_)
{
    std::stable_sort(std::begin(nonindexed_draw_commands_), std::end(nonindexed_draw_commands_), renderer::comparator<renderer::nonindexed_draw_command>{});

    std::ranges::copy(nonindexed_draw_commands_, std::ostream_iterator<renderer::nonindexed_draw_command>(std::cout, "|"));
    std::cout << std::endl;

    std::vector<renderer::vertex_buffers_bind_range> buffers_bind_range;

    for (auto it_begin = std::begin(nonindexed_draw_commands_); it_begin != std::end(nonindexed_draw_commands_);) {
        auto h = it_begin->vertex_buffer->handle;
        auto i = it_begin->vertex_input_binding_index;
        // std::cout << "a " << h << ' ' << i << std::endl;

        std::vector<std::uint32_t> buffer_handles(1, h);

        auto it_end = std::stable_partition(it_begin, std::end(nonindexed_draw_commands_), [&h, &i, &buffer_handles] (auto &&b)
        {
            if (i == b.vertex_input_binding_index) {
                // std::cout << "bf " << h << ' ' << i << '|' << b.vertex_buffer->handle << ' ' << b.vertex_input_binding_index << std::boolalpha << (h == b.vertex_buffer->handle) << std::endl;
                return h == b.vertex_buffer->handle;
            }

            else if (b.vertex_input_binding_index - i == 1) {
                // std::cout << "bs " << h << ' ' << i << '|' << b.vertex_buffer->handle << ' ' << b.vertex_input_binding_index << std::endl;
                h = b.vertex_buffer->handle;
                i = b.vertex_input_binding_index;

                buffer_handles.push_back(h);
                return true;
            }

            return false;
        });

        std::copy(it_begin, it_end, std::ostream_iterator<renderer::nonindexed_draw_command>(std::cout, "|"));
        std::cout << std::endl;

        std::ranges::copy(buffer_handles, std::ostream_iterator<std::uint32_t>(std::cout, " "));
        std::cout << std::endl;

        buffers_bind_range.push_back(renderer::vertex_buffers_bind_range{
            it_begin->vertex_input_binding_index,
            buffer_handles,
            std::vector<std::uint32_t>(std::size(buffer_handles), 0u),
            std::span{it_begin, it_end}
                                     });

        it_begin = it_end;
    }

    return buffers_bind_range;
}

std::vector<renderer::indexed_primitives_buffers_bind_range> get_indexed_primitives_buffers_bind_range(std::span<renderer::indexed_draw_command> draw_commands_)
{
    std::stable_sort(std::begin(draw_commands_), std::end(draw_commands_), renderer::comparator<renderer::indexed_draw_command>{});

    std::ranges::copy(draw_commands_, std::ostream_iterator<renderer::indexed_draw_command>(std::cout, "|"));
    std::cout << std::endl;

    auto it_begin = std::begin(draw_commands_);

    while (it_begin != std::end(draw_commands_)) {
        auto it = std::adjacent_find(it_begin, std::end(draw_commands_), [] (auto &&lhs, auto &&rhs)
        {
            return lhs.index_buffer->index_type != rhs.index_buffer->index_type || lhs.index_buffer->handle != rhs.index_buffer->handle;
        });

        if (it != std::end(draw_commands_))
            it = std::next(it);

        auto dcs = std::span{it_begin, it};

        // auto buffers_bind_range = get_primitives_buffers_bind_ranges(dcs);

        // std::ranges::copy(dcs, std::ostream_iterator<renderer::indexed_draw_command>(std::cout, "|"));
        // std::cout << std::endl;

        for (auto it_begin2 = std::begin(dcs); it_begin2 != std::end(dcs);) {
            auto h = it_begin2->vertex_buffer->handle;
            auto i = it_begin2->vertex_input_binding_index;
            // std::cout << "a " << h << ' ' << i << std::endl;

            std::vector<std::uint32_t> buffer_handles(1, h);

            auto it_end2 = std::stable_partition(it_begin2, std::end(dcs), [&h, &i, &buffer_handles] (auto &&b)
            {
                if (i == b.vertex_input_binding_index) {
                    // std::cout << "bf " << h << ' ' << i << '|' << b.vertex_buffer->handle << ' ' << b.vertex_input_binding_index << std::boolalpha << (h == b.vertex_buffer->handle) << std::endl;
                    return h == b.vertex_buffer->handle;
                }

                else if (b.vertex_input_binding_index - i == 1) {
                    // std::cout << "bs " << h << ' ' << i << '|' << b.vertex_buffer->handle << ' ' << b.vertex_input_binding_index << std::endl;
                    h = b.vertex_buffer->handle;
                    i = b.vertex_input_binding_index;

                    buffer_handles.push_back(h);
                    return true;
    }

                return false;
});

            std::copy(it_begin2, it_end2, std::ostream_iterator<renderer::indexed_draw_command>(std::cout, "|"));
            std::cout << std::endl;

            // std::ranges::copy(buffer_handles, std::ostream_iterator<std::uint32_t>(std::cout, " "));
            // std::cout << std::endl;

            // buffers_bind_range.push_back(renderer::vertex_buffers_bind_range{
            //     it_begin2->vertex_input_binding_index,
            //     buffer_handles,
            //     std::vector<std::uint32_t>(std::size(buffer_handles), 0u),
            //     std::span{it_begin2, it_end2}
            // });

            it_begin2 = it_end2;
        }

        // std::ranges::copy(dcs, std::ostream_iterator<renderer::indexed_draw_command>(std::cout, "|"));
        std::cout << std::endl;

        it_begin = it;
    }

    return {};
}

int main()
{
    // h0 i0|h0 i0|h0 i1|
    // h4 i0|h1 i1|h2 i1|h2 i1|
    // h7 i1|
    // h5 i3|h6 i4|
    // h8 i6|

    // std::vector<renderer::nonindexed_draw_command> commands{
    //     {std::make_shared<resource::vertex_buffer>(0), 0},
    //     {std::make_shared<resource::vertex_buffer>(0), 0},
    //     {std::make_shared<resource::vertex_buffer>(2), 1},
    //     {std::make_shared<resource::vertex_buffer>(4), 0},
    //     {std::make_shared<resource::vertex_buffer>(1), 1},
    //     // {std::make_shared<resource::vertex_buffer>(1), 2},
    //     {std::make_shared<resource::vertex_buffer>(2), 1},
    //     {std::make_shared<resource::vertex_buffer>(0), 1},
    //     {std::make_shared<resource::vertex_buffer>(5), 3},
    //     {std::make_shared<resource::vertex_buffer>(6), 4},
    //     {std::make_shared<resource::vertex_buffer>(8), 6},
    //     {std::make_shared<resource::vertex_buffer>(0), 1},
    //     {std::make_shared<resource::vertex_buffer>(7), 1}
    // };

    // auto buffers_bind_range = get_primitives_buffers_bind_ranges(commands);

    // std::ranges::copy(commands, std::ostream_iterator<renderer::nonindexed_draw_command>(std::cout, "|"));
    // std::cout << std::endl;

    // std::ranges::copy(buffers_bind_range, std::ostream_iterator<renderer::vertex_buffers_bind_range>(std::cout, "|"));
    // std::cout << std::endl;

    std::vector<renderer::indexed_draw_command> idxcommands{
        {std::make_shared<resource::vertex_buffer>(0), std::make_shared<resource::index_buffer>(0, INDEX_TYPE::UINT_16), 0, 0},
        {std::make_shared<resource::vertex_buffer>(0), std::make_shared<resource::index_buffer>(1, INDEX_TYPE::UINT_16), 0, 0},
        {std::make_shared<resource::vertex_buffer>(2), std::make_shared<resource::index_buffer>(0, INDEX_TYPE::UINT_16), 1, 0},
        {std::make_shared<resource::vertex_buffer>(4), std::make_shared<resource::index_buffer>(1, INDEX_TYPE::UINT_16), 0, 0},
        {std::make_shared<resource::vertex_buffer>(1), std::make_shared<resource::index_buffer>(3, INDEX_TYPE::UINT_16), 1, 0},
        {std::make_shared<resource::vertex_buffer>(1), std::make_shared<resource::index_buffer>(2, INDEX_TYPE::UINT_16), 2, 0},
        {std::make_shared<resource::vertex_buffer>(2), std::make_shared<resource::index_buffer>(4, INDEX_TYPE::UINT_16), 1, 0},
        {std::make_shared<resource::vertex_buffer>(0), std::make_shared<resource::index_buffer>(1, INDEX_TYPE::UINT_16), 1, 0},
        {std::make_shared<resource::vertex_buffer>(5), std::make_shared<resource::index_buffer>(1, INDEX_TYPE::UINT_16), 3, 0},
        {std::make_shared<resource::vertex_buffer>(6), std::make_shared<resource::index_buffer>(1, INDEX_TYPE::UINT_16), 4, 0},
        {std::make_shared<resource::vertex_buffer>(8), std::make_shared<resource::index_buffer>(2, INDEX_TYPE::UINT_16), 6, 0},
        {std::make_shared<resource::vertex_buffer>(0), std::make_shared<resource::index_buffer>(0, INDEX_TYPE::UINT_16), 1, 0},
        {std::make_shared<resource::vertex_buffer>(7), std::make_shared<resource::index_buffer>(1, INDEX_TYPE::UINT_16), 1, 0}
    };

    get_indexed_primitives_buffers_bind_range(idxcommands);

    // std::ranges::copy(commands, std::ostream_iterator<renderer::nonindexed_draw_command>(std::cout, "|"));
    // std::cout << std::endl;
}
#endif

std::pair<std::span<draw_command>, std::span<draw_command>> separate_indexed_and_nonindexed(std::span<draw_command> draw_commands)
{
    auto it = std::stable_partition(std::begin(draw_commands), std::end(draw_commands), [] (auto &&draw_command)
    {
        return draw_command.index_buffer != nullptr;
    });

    return std::pair{std::span{std::begin(draw_commands), it}, std::span{it, std::end(draw_commands)}};
}

std::vector<vertex_buffers_bind_ranges>
separate_nonindexed_by_binds(graphics::vertex_input_state_manager &vertex_input_state_manager, std::span<draw_command> draw_commands)
{
    std::stable_sort(std::begin(draw_commands), std::end(draw_commands), [&] (auto &&lhs, auto &&rhs)
    {
        auto lhs_binding_index = vertex_input_state_manager.binding_index(lhs.vertex_buffer->vertex_layout());
        auto rhs_binding_index = vertex_input_state_manager.binding_index(rhs.vertex_buffer->vertex_layout());

        if (lhs_binding_index == rhs_binding_index)
            return lhs.vertex_buffer->device_buffer()->handle() < rhs.vertex_buffer->device_buffer()->handle();

        return lhs_binding_index < rhs_binding_index;
    });

    std::vector<draw_command> rc(std::begin(draw_commands), std::end(draw_commands));
    std::span<draw_command> newrc{rc};
    std::vector<std::span<draw_command>> subranges;

    auto it_out = std::begin(draw_commands);

    for (auto it_begin = std::begin(newrc); it_begin != std::end(newrc); it_begin = std::begin(newrc)) {
        auto it_temp = it_out;

        it_out = std::unique_copy(it_begin, std::end(newrc), it_out, [&] (auto &&lhs, auto &&rhs)
        {
            auto lhs_binding_index = vertex_input_state_manager.binding_index(lhs.vertex_buffer->vertex_layout());
            auto rhs_binding_index = vertex_input_state_manager.binding_index(rhs.vertex_buffer->vertex_layout());

            if (lhs_binding_index == rhs_binding_index)
                return lhs.vertex_buffer->device_buffer()->handle() != rhs.vertex_buffer->device_buffer()->handle();

            return false;
        });

        auto subrange = std::span{it_temp, it_out};

        auto it_end = std::set_difference(std::begin(newrc), std::end(newrc),
                                          std::begin(subrange), std::end(subrange),
                                          std::begin(newrc), [&] (auto &&lhs, auto &&rhs)
        {
            auto lhs_binding_index = vertex_input_state_manager.binding_index(lhs.vertex_buffer->vertex_layout());
            auto rhs_binding_index = vertex_input_state_manager.binding_index(rhs.vertex_buffer->vertex_layout());

            return lhs_binding_index < rhs_binding_index;
        });

        subranges.push_back(subrange);

        newrc = std::span{std::begin(newrc), it_end};
    }

    std::vector<vertex_buffers_bind_ranges> bind_ranges;

    for (auto subrange : subranges) {
        for (auto it_begin = std::begin(subrange); it_begin != std::end(subrange);) {
            auto it = std::adjacent_find(it_begin, std::end(subrange), [&] (auto &&lhs, auto &&rhs)
            {
                auto lhs_binding_index = vertex_input_state_manager.binding_index(lhs.vertex_buffer->vertex_layout());
                auto rhs_binding_index = vertex_input_state_manager.binding_index(rhs.vertex_buffer->vertex_layout());

                return (rhs_binding_index - lhs_binding_index) > 1;
            });

            if (it != std::end(subrange))
                it = std::next(it);

            std::span<draw_command> consecutive_binds{it_begin, it};

            std::vector<std::pair<std::uint32_t, VkBuffer>> layout_index_and_buffer_pairs;

            std::transform(it_begin, it, std::back_inserter(layout_index_and_buffer_pairs), [&] (auto &&lhs)
            {
                auto binding_index = vertex_input_state_manager.binding_index(lhs.vertex_buffer->vertex_layout());

                return std::pair{binding_index, lhs.vertex_buffer->device_buffer()->handle()};
            });

            auto itx = std::unique(std::begin(layout_index_and_buffer_pairs), std::end(layout_index_and_buffer_pairs), [] (auto lhs, auto rhs)
            {
                return lhs == rhs;
            });

            /*auto handles = consecutive_binds | std::views::transform([] (auto &&lhs)
            {
                return lhs.vertex_buffer->device_buffer()->handle();
            });*/

            std::vector<VkBuffer> vertex_buffer_handles;

            std::transform(std::begin(layout_index_and_buffer_pairs), itx, std::back_inserter(vertex_buffer_handles), [] (auto &&lhs)
            {
                return lhs.second;
            });

            //std::unique_copy(std::begin(handles), std::end(handles), std::back_inserter(vertex_buffer_handles));

            auto first_binding_index = vertex_input_state_manager.binding_index(consecutive_binds.front().vertex_buffer->vertex_layout());
            auto last_binding_index = vertex_input_state_manager.binding_index(consecutive_binds.back().vertex_buffer->vertex_layout());

            bind_ranges.push_back(vertex_buffers_bind_ranges{
                first_binding_index,
                last_binding_index - first_binding_index + 1,
                vertex_buffer_handles,
                std::vector<VkDeviceSize>(std::size(vertex_buffer_handles), 0),
                consecutive_binds
            });

            it_begin = it;
        }
    }

    return bind_ranges;
}

std::vector<indexed_primitives_draw> separate_indexed_by_binds(std::span<draw_command> draw_commands)
{
    std::stable_sort(std::begin(draw_commands), std::end(draw_commands), [] (auto &&lhs, auto &&rhs)
    {
        if (lhs.index_buffer->index_type() == rhs.index_buffer->index_type())
            return lhs.index_buffer->device_buffer()->handle() < rhs.index_buffer->device_buffer()->handle();

        return lhs.index_buffer->index_type() < rhs.index_buffer->index_type();
    });

#if 0

    std::vector<draw_command> draw_commands_copy(std::begin(draw_commands), std::end(draw_commands));
    std::span<draw_command> copy_range{draw_commands_copy};

    std::vector<std::span<draw_command>> unique_subranges;

    auto it_out = std::begin(draw_commands);

    for (auto it_begin = std::begin(copy_range); it_begin != std::end(copy_range); it_begin = std::begin(copy_range)) {
        auto it = it_out;

        it_out = std::unique_copy(it_begin, std::begin(copy_range), it_out, [] (auto &&lhs, auto &&rhs)
        {
            if (lhs.index_buffer->index_type() == rhs.index_buffer->index_type())
                return lhs.index_buffer->device_buffer()->handle() != rhs.index_buffer->device_buffer()->handle();

            return false;
        });

        auto subrange = std::span{it, it_out};

        auto it_end = std::set_difference(std::begin(copy_range), std::end(copy_range),
                                          std::begin(subrange), std::end(subrange),
                                          std::begin(copy_range), [] (auto &&lhs, auto &&rhs)
        {
            return lhs.index_buffer->index_type() < rhs.index_buffer->index_type();
        });

        subranges.push_back(subrange);

        copy_range = std::span{std::begin(copy_range), it_end};
    }

    std::vector<indexed_primitives_draw> bind_ranges;

    for (auto subrange : subranges) {
        for (auto it_begin = std::begin(subrange); it_begin != std::end(subrange);) {
            auto it = std::adjacent_find(it_begin, std::end(subrange), [&] (auto &&lhs, auto &&rhs)
            {
                return (rhs.index_buffer->index_type() - lhs.index_buffer->index_type()) > 1;
            });

            if (it != std::end(subrange))
                it = std::next(it);

            std::span<draw_command> consecutive_binds{it_begin, it};

            auto handles = consecutive_binds | std::views::transform([] (auto &&lhs)
            {
                return lhs.index_buffer->device_buffer()->handle();
            });

            std::vector<VkBuffer> index_buffer_handles;

            std::unique_copy(std::begin(handles), std::end(handles), std::back_inserter(index_buffer_handles), [] (auto lhs, auto rhs)
            {
                return lhs == rhs;
            });

            bind_ranges.push_back(indexed_primitives_draw{
                first_binding_index,
                last_binding_index - first_binding_index + 1,
                vertex_buffer_handles,
                std::vector<VkDeviceSize>(std::size(vertex_buffer_handles), 0),
                consecutive_binds
            });

            /*auto first_binding_index = vertex_input_state_manager.binding_index(consecutive_binds.front().vertex_buffer->vertex_layout());
            auto last_binding_index = vertex_input_state_manager.binding_index(consecutive_binds.back().vertex_buffer->vertex_layout());

            bind_ranges.push_back(indexed_primitives_draw{
                first_binding_index,
                last_binding_index - first_binding_index + 1,
                vertex_buffer_handles,
                std::vector<VkDeviceSize>(std::size(vertex_buffer_handles), 0),
                consecutive_binds
            });*/

            it_begin = it;
        }
    }
#endif

    /*auto it = std::adjacent_find(std::begin(draw_commands), std::end(draw_commands), [] (auto &&lhs, auto &&rhs)
    {
        return lhs.index_buffer->index_type() != rhs.index_buffer->index_type();
    });

    if (it != std::end(subrange))
        it = std::next(it);*/

    return {};
}

ext: cxx

max_stars_repo_name: Alabuta/VulkanIsland

max_issues_repo_name: Alabuta/VulkanIsland

max_forks_repo_name: Alabuta/VulkanIsland


text:
#pragma once

#include <boost/iostreams/constants.hpp>
#include <boost/iostreams/categories.hpp>
#include <openssl/sha.h>

class sha256_filter {
public:
	SHA256_CTX hasher;
	unsigned char digest[SHA256_DIGEST_LENGTH]{};
	typedef char char_type;

	struct category :
		boost::iostreams::output,
		boost::iostreams::input,
		boost::iostreams::filter_tag,
		boost::iostreams::multichar_tag,
		boost::iostreams::closable_tag { };

	/* FIXME TODO Signal that errors happened somehow */
	sha256_filter()
	{
		SHA256_Init(&hasher);
	}

	template<typename Sink>
	std::streamsize write(Sink& dest, const char* s, std::streamsize n)
	{
		SHA256_Update(&hasher, s, n);
		boost::iostreams::write(dest, s, n);
		return n;
	}

	template<typename Source>
	std::streamsize read(Source& src, char* s, std::streamsize n)
	{
		std::streamsize result = boost::iostreams::read(src, s, n);

		if (result == -1) return result;

		SHA256_Update(&hasher, s, result);
		return result;
	}

	template <class Device>
	void close(Device &device)
	{
		SHA256_Final(&digest[0], &hasher);
	}
};

ext: hpp

max_stars_repo_name: stream-labs/a-files-updater

max_issues_repo_name: stream-labs/a-files-updater

max_forks_repo_name: stream-labs/a-files-updater


text:
/*
	This file is part of cpp-ethereum.

	cpp-ethereum is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	cpp-ethereum is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file difficulty.cpp
 * @author Christoph Jentzsch <cj@ethdev.com>
 * @date 2015
 * difficulty calculation tests.
 */

#include <boost/test/unit_test.hpp>
#include <test/libtesteth/TestHelper.h>
#include <test/fuzzTesting/fuzzHelper.h>
#include <libethashseal/Ethash.h>
#include <libethashseal/GenesisInfo.h>
#include <cpp-ethereum/libethereum/ChainParams.h>
using namespace std;
using namespace dev;
using namespace dev::eth;

namespace js = json_spirit;
std::string const c_testDifficulty = R"(
 "DifficultyTest[N]" : {
		"parentTimestamp" : "[PSTAMP]",
		"parentDifficulty" : "[PDIFF]",
		"currentTimestamp" : "[СSTAMP]",
		"currentBlockNumber" : "[CNUM]",
		"currentDifficulty" : "[CDIFF]"
	},
)";

void checkCalculatedDifficulty(BlockHeader const& _bi, BlockHeader const& _parent, Network _n, ChainOperationParams const& _p, string const& _testName = "")
{
	u256 difficulty = _bi.difficulty();
	u256 frontierDiff = _p.u256Param("homsteadForkBlock");

	//The ultimate formula (Homestead)
	if (_bi.number() > frontierDiff)
	{
		u256 minimumDifficulty = _p.u256Param("minimumDifficulty");
		bigint block_diff = _parent.difficulty();

		bigint a = (_parent.difficulty() / 2048);
		int b = 1 - int(_bi.timestamp() - _parent.timestamp()) / 10;
		bigint c = (_bi.number() / 100000) - 2;

		block_diff += a * max<int>(b, -99);
		block_diff += u256(1) << (unsigned)c;
		block_diff = max<bigint>(minimumDifficulty, block_diff);

		BOOST_CHECK_MESSAGE(difficulty == block_diff, "Homestead Check Calculated diff = " << difficulty << " expected diff = " << block_diff << _testName);
		return;
	}

	u256 durationLimit;
	u256 minimumDifficulty;
	u256 difficultyBoundDivisor;
	switch(_n)
	{
	case Network::MainNetwork:
	case Network::FrontierTest:
	case Network::HomesteadTest:
	case Network::Ropsten:
	case Network::TransitionnetTest:
		durationLimit = 13;
		minimumDifficulty = 131072;
		difficultyBoundDivisor = 2048;
	break;
	default:
		cerr << "testing undefined network difficulty";
		durationLimit = _p.u256Param("durationLimit");
		minimumDifficulty = _p.u256Param("minimumDifficulty");
		difficultyBoundDivisor = _p.u256Param("difficultyBoundDivisor");
		break;
	}

	//Frontier Era
	bigint block_diff = _parent.difficulty();

	bigint a = (_parent.difficulty() / difficultyBoundDivisor);
	bigint b = ((_bi.timestamp() - _parent.timestamp()) < durationLimit) ?  1 : -1;
	bigint c = (_bi.number() / 100000) - 2;

	block_diff += a * b;
	block_diff += u256(1) << (unsigned)c;
	block_diff = max<bigint>(minimumDifficulty, block_diff);

	BOOST_CHECK_MESSAGE(difficulty == block_diff, "Check Calculated diff = " << difficulty << " expected diff = " << block_diff << _testName);
	return;
}

void fillDifficulty(string const& _testFileFullName, Ethash& _sealEngine)
{
	int testN = 0;
	ostringstream finalTest;
	finalTest << "{" << std::endl;
	dev::test::TestOutputHelper::initTest(900);

	for (int stampDelta = 0; stampDelta < 45; stampDelta+=2)
	{
		for (u256 blockNumber = 1; blockNumber < 1500000; blockNumber += 25000)
		{
			testN++;
			json_spirit::mObject m;
			string testName = "DifficultyTest"+toString(testN);
			if (!dev::test::TestOutputHelper::passTest(m, testName))
				continue;

			u256 pStamp = dev::test::RandomCode::randomUniInt();
			u256 pDiff = dev::test::RandomCode::randomUniInt();
			u256 cStamp = pStamp + stampDelta;
			u256 cNum = blockNumber;

			BlockHeader parent;
			parent.setTimestamp(pStamp);
			parent.setDifficulty(pDiff);
			parent.setNumber(cNum - 1);

			BlockHeader current;
			current.setTimestamp(cStamp);
			current.setNumber(cNum);

			string tmptest = c_testDifficulty;
			std::map<string, string> replaceMap;
			replaceMap["[N]"] = toString(testN);
			replaceMap["[PDIFF]"] = toCompactHex(pDiff, HexPrefix::Add);
			replaceMap["[PSTAMP]"] = toCompactHex(pStamp, HexPrefix::Add);
			replaceMap["[СSTAMP]"] = toCompactHex(cStamp, HexPrefix::Add);
			replaceMap["[CNUM]"] = toCompactHex(cNum, HexPrefix::Add);
			replaceMap["[CDIFF]"] = toCompactHex(_sealEngine.calculateDifficulty(current, parent), HexPrefix::Add);

			dev::test::RandomCode::parseTestWithTypes(tmptest, replaceMap);
			finalTest << tmptest;
		}
	}

	finalTest << std::endl << "}";
	string testFile = finalTest.str();
	testFile = testFile.replace(testFile.find_last_of(","), 1, "");
	writeFile(_testFileFullName, asBytes(testFile));
}

void testDifficulty(string const& _testFileFullName, Ethash& _sealEngine, Network _n)
{
	//Test File
	js::mValue v;
	string s = contentsString(_testFileFullName);
	BOOST_REQUIRE_MESSAGE(s.length() > 0, "Contents of '" << _testFileFullName << "' is empty. Have you cloned the 'tests' repo branch develop?");
	js::read_string(s, v);
	dev::test::TestOutputHelper::initTest(v);

	for (auto& i: v.get_obj())
	{
		js::mObject o = i.second.get_obj();
		string testname = i.first;
		if (!dev::test::TestOutputHelper::passTest(o, testname))
			continue;

		BlockHeader parent;
		parent.setTimestamp(test::toInt(o["parentTimestamp"]));
		parent.setDifficulty(test::toInt(o["parentDifficulty"]));
		parent.setNumber(test::toInt(o["currentBlockNumber"]) - 1);

		BlockHeader current;
		current.setTimestamp(test::toInt(o["currentTimestamp"]));
		current.setNumber(test::toInt(o["currentBlockNumber"]));

		u256 difficulty = _sealEngine.calculateDifficulty(current, parent);
		current.setDifficulty(difficulty);
		BOOST_CHECK_EQUAL(difficulty, test::toInt(o["currentDifficulty"]));

		//Manual formula test
		checkCalculatedDifficulty(current, parent, _n, _sealEngine.chainParams(), "(" + i.first + ")");
	}
	dev::test::TestOutputHelper::finishTest();
}

BOOST_AUTO_TEST_SUITE(DifficultyTests)

BOOST_AUTO_TEST_CASE(difficultyTestsFrontier)
{
	string testFileFullName = test::getTestPath();
	testFileFullName += "/BasicTests/difficultyFrontier.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::FrontierTest)));

	if (dev::test::Options::get().filltests)
		fillDifficulty(testFileFullName, sealEngine);

	testDifficulty(testFileFullName, sealEngine, Network::FrontierTest);
}

BOOST_AUTO_TEST_CASE(difficultyTestsRopsten)
{
	string testFileFullName = test::getTestPath();
	testFileFullName += "/BasicTests/difficultyRopsten.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::Ropsten)));

	if (dev::test::Options::get().filltests)
		fillDifficulty(testFileFullName, sealEngine);

	testDifficulty(testFileFullName, sealEngine, Network::Ropsten);
}

BOOST_AUTO_TEST_CASE(difficultyTestsHomestead)
{
	string testFileFullName = test::getTestPath();
	testFileFullName += "/BasicTests/difficultyHomestead.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::HomesteadTest)));

	if (dev::test::Options::get().filltests)
		fillDifficulty(testFileFullName, sealEngine);

	testDifficulty(testFileFullName, sealEngine, Network::HomesteadTest);
}

BOOST_AUTO_TEST_CASE(difficultyTestsMainNetwork)
{
	string testFileFullName = test::getTestPath();
	testFileFullName += "/BasicTests/difficultyMainNetwork.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::MainNetwork)));

	if (dev::test::Options::get().filltests)
		fillDifficulty(testFileFullName, sealEngine);

	testDifficulty(testFileFullName, sealEngine, Network::MainNetwork);
}

BOOST_AUTO_TEST_CASE(difficultyTestsCustomMainNetwork)
{
	string testFileFullName = test::getTestPath();
	testFileFullName += "/BasicTests/difficultyCustomMainNetwork.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::MainNetwork)));

	if (dev::test::Options::get().filltests)
	{
		u256 homsteadBlockNumber = 1000000;
		std::vector<u256> blockNumberVector = {homsteadBlockNumber - 100000, homsteadBlockNumber, homsteadBlockNumber + 100000};
		std::vector<u256> parentDifficultyVector = {1000, 2048, 4000, 1000000};
		std::vector<int> timestampDeltaVector = {0, 1, 8, 10, 13, 20, 100, 800, 1000, 1500};

		int testN = 0;
		ostringstream finalTest;
		finalTest << "{" << std::endl;

		for (size_t bN = 0; bN < blockNumberVector.size(); bN++)
			for (size_t pdN = 0; pdN < parentDifficultyVector.size(); pdN++)
				for (size_t tsN = 0; tsN < timestampDeltaVector.size(); tsN++)
				{
					testN++;
					int stampDelta = timestampDeltaVector.at(tsN);
					u256 blockNumber = blockNumberVector.at(bN);
					u256 pDiff = parentDifficultyVector.at(pdN);

					u256 pStamp = dev::test::RandomCode::randomUniInt();
					u256 cStamp = pStamp + stampDelta;
					u256 cNum = blockNumber;

					BlockHeader parent;
					parent.setTimestamp(pStamp);
					parent.setDifficulty(pDiff);
					parent.setNumber(cNum - 1);

					BlockHeader current;
					current.setTimestamp(cStamp);
					current.setNumber(cNum);

					string tmptest = c_testDifficulty;
					std::map<string, string> replaceMap;
					replaceMap["[N]"] = toString(testN);
					replaceMap["[PDIFF]"] = toCompactHex(pDiff, HexPrefix::Add);
					replaceMap["[PSTAMP]"] = toCompactHex(pStamp, HexPrefix::Add);
					replaceMap["[СSTAMP]"] = toCompactHex(cStamp, HexPrefix::Add);
					replaceMap["[CNUM]"] = toCompactHex(cNum, HexPrefix::Add);
					replaceMap["[CDIFF]"] = toCompactHex(sealEngine.calculateDifficulty(current, parent), HexPrefix::Add);

					dev::test::RandomCode::parseTestWithTypes(tmptest, replaceMap);
					finalTest << tmptest;
				}

		finalTest << std::endl << "}";
		string testFile = finalTest.str();
		testFile = testFile.replace(testFile.find_last_of(","), 1, "");
		writeFile(testFileFullName, asBytes(testFile));
	}

	testDifficulty(testFileFullName, sealEngine, Network::MainNetwork);
}

BOOST_AUTO_TEST_CASE(basicDifficultyTest)
{
	string testPath = test::getTestPath();
	testPath += "/BasicTests/difficulty.json";

	Ethash sealEngine;
	sealEngine.setChainParams(ChainParams(genesisInfo(Network::MainNetwork)));

	testDifficulty(testPath, sealEngine, Network::MainNetwork);
}

BOOST_AUTO_TEST_SUITE_END()

ext: cpp

max_stars_repo_name: nexalt-project/nexalt-core

max_issues_repo_name: nexalt-project/nexalt-core

max_forks_repo_name: nexalt-project/nexalt-core


text:
#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../include/structures.h"
#include "../include/transformations.h"
#include "../../rectangular_object_with_unknown_width_height_tait_bryan_wc_jacobian.h"
#include "../../rectangular_object_with_unknown_width_height_rodrigues_wc_jacobian.h"
#include "../../rectangular_object_with_unknown_width_height_quaternion_wc_jacobian.h"
#include "../../quaternion_constraint_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

std::vector<std::vector<Eigen::Affine3d>> bundle_of_rays;
struct Rectangle{
	std::vector<Eigen::Vector3d> corners_local;
	double scale_x;
	double scale_y;
	Eigen::Affine3d pose;
};
Rectangle rectangle;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

int main(int argc, char *argv[]){

	std::vector<Eigen::Affine3d> br;
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -2;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -2;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();

	rectangle.scale_x = 1.0;
	rectangle.scale_y = 1.0;
	rectangle.pose = Eigen::Affine3d::Identity();
	rectangle.corners_local.emplace_back(-1,-1, 0);
	rectangle.corners_local.emplace_back( 1,-1, 0);
	rectangle.corners_local.emplace_back( 1, 1, 0);
	rectangle.corners_local.emplace_back(-1, 1, 0);

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("rectangular_object_with_unknown_width_height");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glColor3f(0,1,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < bundle_of_rays.size(); i++){
		for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
			Eigen::Vector3d z_begin(0, 0,-100);
			Eigen::Vector3d z_end(0, 0, 100);

			Eigen::Vector3d z_begin_t = bundle_of_rays[i][j] * z_begin;
			Eigen::Vector3d z_end_t = bundle_of_rays[i][j] * z_end;

			glVertex3f(z_begin_t.x(), z_begin_t.y(), z_begin_t.z());
			glVertex3f(z_end_t.x(), z_end_t.y(), z_end_t.z());
		}
	}
	glEnd();

	std::vector<Eigen::Vector3d> corners_global;
	for(size_t i = 0; i < rectangle.corners_local.size(); i++){
		Eigen::Vector3d v(rectangle.corners_local[i].x() * rectangle.scale_x, rectangle.corners_local[i].y() * rectangle.scale_y, rectangle.corners_local[i].z());
		corners_global.push_back(rectangle.pose * v);
	}

	glLineWidth(5);
	glColor3f(1,0,0);
	glBegin(GL_LINE_STRIP);
		for(size_t i = 0 ; i < corners_global.size(); i++){
			glVertex3f(corners_global[i].x(), corners_global[i].y(), corners_global[i].z());
		}
		glVertex3f(corners_global[0].x(), corners_global[0].y(), corners_global[0].z());
	glEnd();
	glLineWidth(1);
	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(rectangle.pose);

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					Eigen::Vector3d vx(bundle_of_rays[i][j](0,0), bundle_of_rays[i][j](1,0), bundle_of_rays[i][j](2,0));
					Eigen::Vector3d vy(bundle_of_rays[i][j](0,1), bundle_of_rays[i][j](1,1), bundle_of_rays[i][j](2,1));

					Eigen::Matrix<double, 2, 1> delta;
					rectangular_object_with_unknown_width_height_tait_bryan_wc(delta,
							pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					Eigen::Matrix<double, 2, 8> delta_jacobian;
					rectangular_object_with_unknown_width_height_tait_bryan_wc_jacobian(delta_jacobian,
							pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					int ir = tripletListB.size();
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir, k, -delta_jacobian(0,k));
					}
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir + 1, k, -delta_jacobian(1,k));
					}

					tripletListP.emplace_back(ir    , ir    ,  1);
					tripletListP.emplace_back(ir + 1, ir + 1,  1);

					tripletListB.emplace_back(ir    , 0,  delta(0,0));
					tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 8);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(8, 8);
			Eigen::SparseMatrix<double> AtPB(8, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
					std::cout << it.value() << std::endl;
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			if(h_x.size() == 8){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;
				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(rectangle.pose);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.om += h_x[counter++];
				pose.fi += h_x[counter++];
				pose.ka += h_x[counter++];
				rectangle.pose = affine_matrix_from_pose_tait_bryan(pose);
				rectangle.scale_x += h_x[counter++];
				rectangle.scale_y += h_x[counter++];
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			TaitBryanPose pose_rand;
			pose_rand.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			rectangle.pose = rectangle.pose * affine_matrix_from_pose_tait_bryan(pose_rand);

			RodriguesPose pose = pose_rodrigues_from_affine_matrix(rectangle.pose);

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					Eigen::Vector3d vx(bundle_of_rays[i][j](0,0), bundle_of_rays[i][j](1,0), bundle_of_rays[i][j](2,0));
					Eigen::Vector3d vy(bundle_of_rays[i][j](0,1), bundle_of_rays[i][j](1,1), bundle_of_rays[i][j](2,1));

					Eigen::Matrix<double, 2, 1> delta;
					rectangular_object_with_unknown_width_height_rodrigues_wc(delta,
							pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					Eigen::Matrix<double, 2, 8> delta_jacobian;
					rectangular_object_with_unknown_width_height_rodrigues_wc_jacobian(delta_jacobian,
							pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					int ir = tripletListB.size();
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir, k, -delta_jacobian(0,k));
					}
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir + 1, k, -delta_jacobian(1,k));
					}

					tripletListP.emplace_back(ir    , ir    ,  1);
					tripletListP.emplace_back(ir + 1, ir + 1,  1);

					tripletListB.emplace_back(ir    , 0,  delta(0,0));
					tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 8);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(8, 8);
			Eigen::SparseMatrix<double> AtPB(8, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
					std::cout << it.value() << std::endl;
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			if(h_x.size() == 8){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;
				RodriguesPose pose = pose_rodrigues_from_affine_matrix(rectangle.pose);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.sx += h_x[counter++];
				pose.sy += h_x[counter++];
				pose.sz += h_x[counter++];
				rectangle.pose = affine_matrix_from_pose_rodrigues(pose);
				rectangle.scale_x += h_x[counter++];
				rectangle.scale_y += h_x[counter++];
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			TaitBryanPose pose_rand;
			pose_rand.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			rectangle.pose = rectangle.pose * affine_matrix_from_pose_tait_bryan(pose_rand);

			QuaternionPose pose = pose_quaternion_from_affine_matrix(rectangle.pose);

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					Eigen::Vector3d vx(bundle_of_rays[i][j](0,0), bundle_of_rays[i][j](1,0), bundle_of_rays[i][j](2,0));
					Eigen::Vector3d vy(bundle_of_rays[i][j](0,1), bundle_of_rays[i][j](1,1), bundle_of_rays[i][j](2,1));

					Eigen::Matrix<double, 2, 1> delta;
					rectangular_object_with_unknown_width_height_quaternion_wc(delta,
							pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					Eigen::Matrix<double, 2, 9> delta_jacobian;
					rectangular_object_with_unknown_width_height_quaternion_wc_jacobian(delta_jacobian,
							pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					int ir = tripletListB.size();
					for(size_t k = 0 ; k < 9; k++){
						tripletListA.emplace_back(ir, k, -delta_jacobian(0,k));
					}
					for(size_t k = 0 ; k < 9; k++){
						tripletListA.emplace_back(ir + 1, k, -delta_jacobian(1,k));
					}

					tripletListP.emplace_back(ir    , ir    ,  1);
					tripletListP.emplace_back(ir + 1, ir + 1,  1);

					tripletListB.emplace_back(ir    , 0,  delta(0,0));
					tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
				}
			}

			int ir = tripletListB.size();

			double delta;
			quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

			Eigen::Matrix<double, 1, 4> jacobian;
			quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

			tripletListA.emplace_back(ir, 3 , -jacobian(0,0));
			tripletListA.emplace_back(ir, 4 , -jacobian(0,1));
			tripletListA.emplace_back(ir, 5 , -jacobian(0,2));
			tripletListA.emplace_back(ir, 6 , -jacobian(0,3));

			tripletListP.emplace_back(ir, ir, 1000000.0);

			tripletListB.emplace_back(ir, 0, delta);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 9);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(9, 9);
			Eigen::SparseMatrix<double> AtPB(9, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
					std::cout << it.value() << std::endl;
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			if(h_x.size() == 9){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;
				QuaternionPose pose = pose_quaternion_from_affine_matrix(rectangle.pose);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.q0 += h_x[counter++];
				pose.q1 += h_x[counter++];
				pose.q2 += h_x[counter++];
				pose.q3 += h_x[counter++];
				rectangle.pose = affine_matrix_from_pose_quaternion(pose);
				rectangle.scale_x += h_x[counter++];
				rectangle.scale_y += h_x[counter++];
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'n':{
			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					TaitBryanPose pose;

					pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.05;
					pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.05;
					pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.05;

					pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
					pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
					pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

					bundle_of_rays[i][j] = bundle_of_rays[i][j] * affine_matrix_from_pose_tait_bryan(pose);
				}
			}
			TaitBryanPose pose;

			pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;

			pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
			pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
			pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

			rectangle.pose = rectangle.pose * affine_matrix_from_pose_tait_bryan(pose);

			rectangle.scale_x += ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			rectangle.scale_y += ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: modify rays" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize (Rodrigues)" << std::endl;
	std::cout << "q: optimize (Quaternion)" << std::endl;
}


text:
// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NETKET_ABSTRACTMACHINE_HPP
#define NETKET_ABSTRACTMACHINE_HPP

#include <Eigen/Dense>
#include <complex>
#include <fstream>
#include <vector>
#include "Utils/lookup.hpp"
#include "Utils/random_utils.hpp"

namespace netket {
/**
  Abstract class for Machines.
  This class prototypes the methods needed
  by a class satisfying the Machine concept.
*/
class AbstractMachine {
 public:
  using VectorType = Eigen::Matrix<Complex, Eigen::Dynamic, 1>;
  using MatrixType = Eigen::Matrix<Complex, Eigen::Dynamic, Eigen::Dynamic>;
  using LookupType = Lookup<Complex>;
  using VectorRefType = Eigen::Ref<VectorType>;
  using VectorConstRefType = Eigen::Ref<const VectorType>;
  using VisibleConstType = Eigen::Ref<const Eigen::VectorXd>;

  /**
  Member function returning the number of variational parameters.
  @return Number of variational parameters in the Machine.
  */
  virtual int Npar() const = 0;

  /**
  Member function returning the current set of parameters in the machine.
  @return Current set of variational parameters of the machine.
  */
  virtual VectorType GetParameters() = 0;

  /**
  Member function setting the current set of parameters in the machine.
  */
  virtual void SetParameters(VectorConstRefType pars) = 0;

  /**
  Member function providing a random initialization of the parameters.
  @param seed is the see of the random number generator.
  @param sigma is the variance of the gaussian.
  */
  virtual void InitRandomPars(int seed, double sigma) = 0;

  /**
  Member function returning the number of visible units.
  @return Number of visible units in the Machine.
  */
  virtual int Nvisible() const = 0;

  /**
  Member function computing the logarithm of the wave function for a given
  visible vector. Given the current set of parameters, this function should
  comput the value of the logarithm of the wave function from scratch.
  @param v a constant reference to a visible configuration.
  @return Logarithm of the wave function.
  */
  virtual Complex LogVal(VisibleConstType v) = 0;

  /**
  Member function computing the logarithm of the wave function for a given
  visible vector. Given the current set of parameters, this function should
  comput the value of the logarithm of the wave function using the information
  provided in the look-up table, to speed up the computation.
  @param v a constant reference to a visible configuration.
  @param lt a constant eference to the look-up table.
  @return Logarithm of the wave function.
  */
  virtual Complex LogVal(VisibleConstType v, const LookupType &lt) = 0;

  /**
  Member function initializing the look-up tables.
  If needed, a Machine can make use of look-up tables
  to speed up some critical functions. For example,
  to speed up the calculation of wave-function ratios.
  The state of a look-up table depends on the visible units.
  This function should initialize the look-up tables
  making sure that memory in the table is also being allocated.
  @param v a constant reference to the visible configuration.
  @param lt a reference to the look-up table to be initialized.
  */
  virtual void InitLookup(VisibleConstType v, LookupType &lt) = 0;

  /**
  Member function updating the look-up tables.
  If needed, a Machine can make use of look-up tables
  to speed up some critical functions. For example,
  to speed up the calculation of wave-function ratios.
  The state of a look-up table depends on the visible units.
  This function should update the look-up tables
  when the state of visible units is changed according
  to the information stored in toflip and newconf
  @param v a constant reference to the current visible configuration.
  @param tochange a constant reference to a vector containing the indeces of the
  units to be modified.
  @param newconf a constant reference to a vector containing the new values of
  the visible units: here newconf(i)=v'(tochange(i)), where v' is the new
  visible state.
  @param lt a reference to the look-up table to be updated.
  */
  virtual void UpdateLookup(VisibleConstType v,
                            const std::vector<int> &tochange,
                            const std::vector<double> &newconf,
                            LookupType &lt) = 0;

  /**
  Member function computing the difference between the logarithm of the
  wave-function computed at different values of the visible units (v, and a set
  of v').
  @param v a constant reference to the current visible configuration.
  @param tochange a constant reference to a vector containing the indeces of the
  units to be modified.
  @param newconf a constant reference to a vector containing the new values of
  the visible units: here for each v', newconf(i)=v'(tochange(i)), where v' is
  the new visible state.
  @return A vector containing, for each v', log(Psi(v')) - log(Psi(v))
  */
  virtual VectorType LogValDiff(
      VisibleConstType v, const std::vector<std::vector<int>> &tochange,
      const std::vector<std::vector<double>> &newconf) = 0;

  /**
  Member function computing the difference between the logarithm of the
  wave-function computed at different values of the visible units (v, and a
  single v'). This version uses the look-up tables to speed-up the calculation.
  @param v a constant reference to the current visible configuration.
  @param tochange a constant reference to a vector containing the indeces of the
  units to be modified.
  @param newconf a constant reference to a vector containing the new values of
  the visible units: here newconf(i)=v'(tochange(i)), where v' is the new
  visible state.
  @param lt a constant eference to the look-up table.
  @return The value of log(Psi(v')) - log(Psi(v))
  */
  virtual Complex LogValDiff(VisibleConstType v, const std::vector<int> &toflip,
                             const std::vector<double> &newconf,
                             const LookupType &lt) = 0;

  /**
  Member function computing the derivative of the logarithm of the wave function
  for a given visible vector.
  @param v a constant reference to a visible configuration.
  @return Derivatives of the logarithm of the wave function with respect to the
  set of parameters.
  */
  virtual VectorType DerLog(VisibleConstType v) = 0;

  virtual void to_json(json &j) const = 0;
  virtual void from_json(const json &j) = 0;

  void Save(const std::string &filename) const {
    std::ofstream filewf(filename);
    Save(filewf);
    filewf.close();
  }

  void Save(std::ofstream &stream) const {
    json j;
    to_json(j);
    stream << j << std::endl;
  }

  virtual const AbstractHilbert &GetHilbert() const noexcept = 0;

  virtual ~AbstractMachine() {}
};
}  // namespace netket

#endif

max_stars_repo_name: flatironinstitute/netket

max_issues_repo_name: flatironinstitute/netket

max_forks_repo_name: flatironinstitute/netket


text:
/***************************************************************************
 * \file CWToneGenComponent.cpp
 * \brief Defines the Continuous-wave (CW) tone generator interface
 *    for the NDR651.
 * \author DA
 * \author NH
 * \author MN
 * \copyright (c) 2017 CyberRadio Solutions, Inc.  All rights reserved.
 *
 ***************************************************************************/

#include "LibCyberRadio/Driver/NDR651/CWToneGenComponent.h"
#include "LibCyberRadio/Driver/RadioHandler.h"
#include <boost/format.hpp>

namespace LibCyberRadio
{
    namespace Driver
    {

        namespace NDR651
        {
            CWToneGenComponent::CWToneGenComponent(
                    int index,
                    ::LibCyberRadio::Driver::RadioHandler* parent,
                     bool debug,
                     int txIndex,
                     double frequency,
                     double amplitude,
                     double phase,
                     double sweepStart,
                     double sweepStop,
                     double sweepStep,
                     double dwellTime ) :
                ::LibCyberRadio::Driver::CWToneGenComponent(
                        /* const std::string& name */ ( boost::format("NDR651-TX%02d-CW%02d") % txIndex % index ).str(),
                        /* int index */ index,
                        /* RadioHandler* parent */ parent,
                        /* bool debug */ debug,
                        /* int txIndex */ txIndex,
                        /* double freqRangeMin */ -512e5,
                        /* double freqRangeMax */ 512e5,
                        /* double freqRes */ 1.0,
                        /* double freqUnits */ 1.0,
                        /* double ampRangeMin */ 0.0,
                        /* double ampRangeMax */ 65535.0,
                        /* double ampRes */ 1.0,
                        /* double phaseRangeMin */ -180.0,
                        /* double phaseRangeMax */ 180.0,
                        /* double phaseRes */ 1.0,
                        /* double sweepStartRangeMin */ -512e5,
                        /* double sweepStartRangeMax */ 512e5,
                        /* double sweepStartRes */ 1.0,
                        /* double sweepStopRangeMin */ -512e5,
                        /* double sweepStopRangeMax */ 512e5,
                        /* double sweepStopRes */ 1.0,
                        /* double sweepStepRangeMin */ -512e5,
                        /* double sweepStepRangeMax */ 512e5,
                        /* double sweepStepRes */ 1.0,
                        /* double dwellTimeRangeMin */ 0.0,
                        /* double dwellTimeRangeMax */ (double)0xFFFFFFFF,
                        /* double dwellTimeRes */ 1.0,
                        /* double frequency */ frequency,
                        /* double amplitude */ amplitude,
                        /* double phase */ phase,
                        /* double sweepStart */ sweepStart,
                        /* double sweepStop */ sweepStop,
                        /* double sweepStep */ sweepStep,
                        /* double dwellTime */ dwellTime )
            {
                initConfigurationDict();
            }

            CWToneGenComponent::~CWToneGenComponent()
            {
            }

            CWToneGenComponent::CWToneGenComponent(const CWToneGenComponent& other) :
                ::LibCyberRadio::Driver::CWToneGenComponent(other)
            {
            }

            CWToneGenComponent& CWToneGenComponent::operator=(const CWToneGenComponent& other)
            {
                ::LibCyberRadio::Driver::CWToneGenComponent::operator=(other);
                if ( this != &other )
                {
                }
                return *this;
            }

        } // namespace NDR651

    } // namespace Driver

} // namespace LibCyberRadio

max_stars_repo_name: CyberRadio/libcyberradio

max_issues_repo_name: CyberRadio/libcyberradio

max_forks_repo_name: CyberRadio/libcyberradio


text:
#pragma once
#include <Shlwapi.h>
#include <cassert>
#include <string>
#include <vector>
#include <fstream>

#include "imgui.h"
#include "imgui_impl_dx9.h"
#include "imgui_impl_dx10.h"
#include "imgui_impl_dx11.h"
#include "imgui_impl_win32.h"
#include "misc/freetype/imgui_freetype.h"

#include <Indicium/Engine/IndiciumCore.h>
#include <boost/algorithm/string.hpp>
#include <luajit/lua.hpp>

#include "../lua-cjson/lua-cjson.h"
#include "../lua-compat-5.3/c-api/compat-5.3.h"

using namespace std;

constexpr char* DEFAULT_RENDER_FUNCTION = "__LuaRender__";

// Find DLL's parent folder path
// From <https://stackoverflow.com/questions/6924195/get-dll-path-at-runtime/6924332>
string thisDllDirPath()
{
    char path[MAX_PATH * 5];
    HMODULE hm;
    if (GetModuleHandleExA(GET_MODULE_HANDLE_EX_FLAG_FROM_ADDRESS |
        GET_MODULE_HANDLE_EX_FLAG_UNCHANGED_REFCOUNT,
        (LPSTR)& thisDllDirPath, &hm)) {
        GetModuleFileNameA(hm, path, sizeof(path));
        PathRemoveFileSpecA(path);
        return path;
    }
    return "";
}

class LuaVM {
private:
    lua_State* stack_ = nullptr;
    string response_;
    string lastError_;

public:
    LuaVM() {
        stack_ = luaL_newstate();
        if (stack_ == nullptr) {
            throw std::runtime_error("create Lua VM failed, please restart your game or system and try again.");
        }

        luaL_openlibs(stack_);
        luaL_requiref(stack_, "cjson", luaopen_cjson, 0);
    }

    ~LuaVM() {
        if (stack_ != nullptr) {
            lua_close(stack_);
        }
    }

    bool Init() {
        ClearResponse();
        ClearLastError();

        string dllBaseDir = thisDllDirPath();
        dllBaseDir = boost::replace_all_copy(dllBaseDir, "\\", "/");
        dllBaseDir = boost::replace_all_copy(dllBaseDir, "'", "\\'");
        IndiciumEngineLogInfo(("Lua Base Dir: " + dllBaseDir).c_str());

        string initCode = "local ffi = require('ffi');\n";
        initCode += "package.path = package.path .. ';" + dllBaseDir + "/?.lua';\n";
        initCode += "ThisLuaVM = ffi.new('uintptr_t', " + std::to_string(reinterpret_cast<uintptr_t>(this)) + ");\n";

        if (!ExecuteString(initCode)) {
            IndiciumEngineLogInfo("Registering LuaVM functions failed");
            return false;
        }

        if (!ExecuteFile(dllBaseDir + "/imgui/init.lua")) {
            IndiciumEngineLogInfo("Load imgui/init.lua failed");
            return false;
        }

        if (!GetLastError().empty()) {
            LogAndSetLastError("LuaVM initialized failed: " + GetLastError());
            return false;
        }

        IndiciumEngineLogInfo("LuaVM initialized successfully");
        return true;
    }

    bool SetFunction(string && code, const string &functionName = DEFAULT_RENDER_FUNCTION) {
        ClearLastError();

        code = "function " + functionName + "()\n" + code + "\nend";
        const int status = luaL_dostring(stack_, code.c_str());
        if (status) {
            LogAndSetLastError("Couldn't set Lua function: " + luaLastError());
            lua_settop(stack_, 0);
            return false;
        }
        lua_settop(stack_, 0);
        return true;
    }

    bool CallFunction(const string& functionName = DEFAULT_RENDER_FUNCTION) {
        lua_getglobal(stack_, functionName.c_str());
        if (lua_type(stack_, -1) != LUA_TFUNCTION) {
            lua_settop(stack_, 0);
            return false;
        }
        const int status = lua_pcall(stack_, 0, 0, 0);
        if (status) {
            LogAndSetLastError("Couldn't call Lua function: " + luaLastError());
            lua_settop(stack_, 0);
            return false;
        }
        lua_settop(stack_, 0);
        return true;
    }

    bool ExecuteFile(const string& file) {
        const int status = luaL_dofile(stack_, file.c_str());
        if (status) {
            LogAndSetLastError("Couldn't execute Lua file: " + luaLastError());
            lua_settop(stack_, 0);
            return false;
        }
        lua_settop(stack_, 0);
        return true;
    }

    bool ExecuteString(const string & code) {
        const int status = luaL_dostring(stack_, code.c_str());
        if (status) {
            LogAndSetLastError("Couldn't execute Lua script: " + luaLastError());
            lua_settop(stack_, 0);
            return false;
        }
        lua_settop(stack_, 0);
        return true;
    }

    void AppendResponse(const string& msg) {
        response_ += msg;
    }

    void SetResponse(const string& msg) {
        response_ = msg;
    }

    void ClearResponse() {
        response_.clear();
    }

    string GetResponse() {
        return response_;
    }

    void SetLastError(const string& msg) {
        lastError_ = msg;
    }

    void LogAndSetLastError(const string& msg) {
        IndiciumEngineLogInfo(msg.c_str());
        lastError_ = msg;
    }

    void ClearLastError() {
        lastError_.clear();
    }

    string GetLastError() {
        return lastError_;
    }

    string GetLastErrorClear() {
        string error = lastError_;
        lastError_.clear();
        return error;
    }

private:
    string luaLastError() {
        return lua_gettop(stack_) ? lua_tostring(stack_, -1) : "unknown error";
    }
};

extern "C"
{
    __declspec(dllexport) void __cdecl LuaVM_AppendResponse(uintptr_t vm, const char* msg) {
        reinterpret_cast<LuaVM*>(vm)->AppendResponse(msg);
    }

    __declspec(dllexport) void __cdecl LuaVM_SetResponse(uintptr_t vm, const char* msg) {
        reinterpret_cast<LuaVM*>(vm)->SetResponse(msg);
    }

    __declspec(dllexport) void __cdecl LuaVM_ClearResponse(uintptr_t vm) {
        reinterpret_cast<LuaVM*>(vm)->ClearResponse();
    }

    __declspec(dllexport) const char* __cdecl LuaVM_GetResponse(uintptr_t vm) {
        return reinterpret_cast<LuaVM*>(vm)->GetResponse().c_str();
    }

    __declspec(dllexport) void __cdecl LuaVM_SetLastError(uintptr_t vm, const char* msg) {
        reinterpret_cast<LuaVM*>(vm)->SetLastError(msg);
    }

    __declspec(dllexport) void __cdecl LuaVM_LogAndSaveLastError(uintptr_t vm, const char* msg) {
        reinterpret_cast<LuaVM*>(vm)->LogAndSetLastError(msg);
    }

    __declspec(dllexport) void __cdecl LuaVM_ClearLastError(uintptr_t vm) {
        reinterpret_cast<LuaVM*>(vm)->ClearLastError();
    }

    __declspec(dllexport) const char* __cdecl LuaVM_GetLastError(uintptr_t vm) {
        return reinterpret_cast<LuaVM*>(vm)->GetLastError().c_str();
    }

    __declspec(dllexport) void __cdecl LogLine(const char *msg) {
        IndiciumEngineLogInfo(msg);
    }

    __declspec(dllexport) void __cdecl UpdateFontCache() {
        ImGuiIO& io = ImGui::GetIO();
        ImGuiFreeType::BuildFontAtlas(io.Fonts);

        ImGui_ImplDX9_InvalidateDeviceObjects();
        ImGui_ImplDX10_InvalidateDeviceObjects();
        ImGui_ImplDX11_InvalidateDeviceObjects();
    }
}

max_stars_repo_name: SwimmingTiger/OverlayDisplayServer

max_issues_repo_name: SwimmingTiger/OverlayDisplayServer

max_forks_repo_name: SwimmingTiger/OverlayDisplayServer


text:
#include "math/rotation.h"

#include <ceres/rotation.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <limits>

namespace GraphSfM {
// Eigen::Vector3d MultiplyRotations(const Eigen::Vector3d& rotation1,
//                                   const Eigen::Vector3d& rotation2) {
//   const double theta1_sq = rotation1.squaredNorm();
//   const double theta2_sq = rotation2.squaredNorm();

//   // Compute the sin and cosine terms below. Take care to ensure that there will
//   // not be any divide-by-zeros when the rotations are small.
//   double cos_a;
//   double cos_b;
//   Eigen::Vector3d sin_a_times_v1;
//   Eigen::Vector3d sin_b_times_v2;

//   // We need to explicity handle the cases when the rotations are near zero.
//   // Near zero, the first order Taylor approximation of the rotation matrix R
//   // corresponding to a vector w and angle w is
//   //
//   //   R = I + hat(w) * sin(theta)
//   //
//   // But sintheta ~ theta and theta * w = angle_axis, which gives us
//   //
//   //  R = I + hat(angle_axis)
//   //
//   // We will use this in the special cases below to ensure stable computation
//   // when composing the rotations and avoid dividing by zero when theta is
//   // small.
//   if (theta1_sq < std::numeric_limits<double>::epsilon()) {
//     // Use the fact that theta ~ sin(theta) when theta is small. Since
//     // a = 0.5 * theta1, we end up with:
//     //   sin(a) * v1 = sin(theta / 2) * v1 = theta / 2 * v1 = rotation1 / 2.
//     sin_a_times_v1 = 0.5 * rotation1;
//     // When a is small, cos(a) ~ 1.0 by the first order taylor approximation.
//     cos_a = 1.0;
//   } else {
//     const double theta1 = std::sqrt(theta1_sq);
//     const double sin_a = std::sin(0.5 * theta1);
//     cos_a = std::cos(0.5 * theta1);
//     sin_a_times_v1 = sin_a * rotation1 / theta1;
//   }

//   // Same as above, but for theta2.
//   if (theta2_sq < std::numeric_limits<double>::epsilon()) {
//     sin_b_times_v2 = 0.5 * rotation2;
//     cos_b = 1.0;
//   } else {
//     const double theta2 = std::sqrt(theta2_sq);
//     const double sin_b = std::sin(0.5 * theta2);
//     cos_b = std::cos(0.5 * theta2);
//     sin_b_times_v2 = sin_b * rotation2 / theta2;
//   }

//   // Compute sin(c) * v3 using the formula above.
//   const Eigen::Vector3d sin_c_times_v3 = cos_b * sin_a_times_v1 +
//                                          cos_a * sin_b_times_v2 +
//                                          sin_a_times_v1.cross(sin_b_times_v2);

//   // If sin(c) is near zero then we again need to take care to avoid dividing by
//   // zero. We can use the first order Taylor approximation again, noting that
//   // sin(c) ~ c, which gives us:
//   //   rotation3 = theta * v3 = 2 * c * v3 ~ 2 * sin(c) * v3
//   const double sin_c_sq = sin_c_times_v3.squaredNorm();
//   if (sinc_c < std::numeric_limits<double>::epsilon()) {
//     const double diff = (2.0 * sin_c_times_v3 - rotation_aa).norm();
//     return 2.0 * sin_c_times_v3;
//   } else {
//     // Otherwise, we use the formula above. The angle axis rotation is the axis
//     // (v3) times the angle theta3.
//     const double sin_c = std::sqrt(sin_c_sq);
//     const double theta3 = 2.0 * std::asin(sin_c);
//     const Eigen::Vector3d v3 = sin_c_times_v3 / sin_c;

//     return theta3 * v3;
//   }
// }

// Use Ceres to perform a stable composition of rotations. This is not as
// efficient as directly composing angle axis vectors (see the old
// implementation commented above) but is more stable.
Eigen::Vector3d MultiplyRotations(const Eigen::Vector3d& rotation1,
                                  const Eigen::Vector3d& rotation2) {
  Eigen::Matrix3d rotation1_mat, rotation2_mat;
  ceres::AngleAxisToRotationMatrix(rotation1.data(), rotation1_mat.data());
  ceres::AngleAxisToRotationMatrix(rotation2.data(), rotation2_mat.data());

  const Eigen::Matrix3d rotation = rotation1_mat * rotation2_mat;
  Eigen::Vector3d rotation_aa;
  ceres::RotationMatrixToAngleAxis(rotation.data(), rotation_aa.data());
  return rotation_aa;
}

}  // namespace GraphSfM


text:
/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Sep 12, 2013
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file ceres/MarginalizationError.hpp
 * @brief Header file for the MarginalizationError class.
 * @author Stefan Leutenegger
 */

#ifndef INCLUDE_OKVIS_CERES_MARGINALIZATIONERROR_HPP_
#define INCLUDE_OKVIS_CERES_MARGINALIZATIONERROR_HPP_

#include <vector>
#include <deque>
#include <memory>
#include <Eigen/Core>
#include <okvis/kinematics/Transformation.hpp>
#include <okvis/Measurements.hpp>
#include <okvis/Variables.hpp>
#include "ceres/ceres.h"
#include <okvis/kinematics/Transformation.hpp>
#include <okvis/assert_macros.hpp>
#include <okvis/ceres/HomogeneousPointParameterBlock.hpp>
#include <okvis/ceres/HomogeneousPointLocalParameterization.hpp>
#include <okvis/ceres/PoseParameterBlock.hpp>
#include <okvis/ceres/SpeedAndBiasParameterBlock.hpp>
#include <okvis/ceres/Map.hpp>
#include <okvis/ceres/ErrorInterface.hpp>

/// \brief okvis Main namespace of this package.
namespace okvis {
/// \brief ceres Namespace for ceres-related functionality implemented in okvis.
namespace ceres {

// not sized, in order to be flexible.
class MarginalizationError : public ::ceres::CostFunction, public ErrorInterface
{
 public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)

  /// \brief The base class type.
  typedef ::ceres::CostFunction base_t;

  /// \brief Trivial destructor.
  virtual ~MarginalizationError()
  {
  }

  /// \brief Default constructor. Initialises a new okvis::ceres::Map.
  MarginalizationError();

  /// \brief Constructor from okvis::ceres::Map.
  /// @param[in] map The underlying okvis::ceres::Map.
  MarginalizationError(Map& map);

  /// \brief Constructor from okvis::ceres::Map and directly add some residuals
  /// @param[in] map The underlying okvis::ceres::Map.
  /// @param[in] residualBlockIds Residual block IDs to be added directly (\see okvis::ceres::addResidualBlocks)
  MarginalizationError(
      Map& map, std::vector< ::ceres::ResidualBlockId > & residualBlockIds);

  // initialization
  /// \brief Set the underlying okvis::ceres::Map.
  /// @param[in] map The underlying okvis::ceres::Map.
  void setMap(Map& map);

  /// \brief Add some residuals to this marginalisation error. This means, they will get linearised.
  /// \warning Note that once added here, they will be removed from the okvis::ceres::Map and stay linerised
  ///          at exactly the points passed here.
  /// @param[in] residualBlockIds Vector of residual block ids, the corresponding terms of which will be added.
  /// @param[in] keepResidualBlocks Currently not in use.
  bool addResidualBlocks(
      const std::vector< ::ceres::ResidualBlockId > & residualBlockIds,
      const std::vector<bool> & keepResidualBlocks = std::vector<bool>());

  /// \brief Add one residual to this marginalisation error. This means, it will get linearised.
  /// \warning Note that once added here, it will be removed from the okvis::ceres::Map and stay linerised
  ///          at exactly the point passed here.
  /// @param[in] residualBlockId Residual block id, the corresponding term of which will be added.
  /// @param[in] keepResidualBlock Currently not in use.
  bool addResidualBlock(::ceres::ResidualBlockId residualBlockId,
                        bool keepResidualBlock = false);

  /// \brief Info: is this parameter block connected to this marginalization error?
  /// @param[in] parameterBlockId Parameter block id of interest.
  bool isParameterBlockConnected(uint64_t parameterBlockId);

  // marginalization

  /// \brief Marginalise out a set of parameter blocks.
  /// \warning The parameter blocks to be marginalised must have been added before.
  ///          Also: make sure to add all the needed residual blocks beforehand for this to make sense.
  /// \return False if not all necessary residual blocks were added before.
  bool marginalizeOut(const std::vector<uint64_t> & parameterBlockIds,
                      const std::vector<bool> & keepParameterBlocks =
                          std::vector<bool>());

  /// \brief This must be called before optimization after adding residual blocks and/or marginalizing,
  ///        since it performs all the lhs and rhs computations on from a given _H and _b.
  void updateErrorComputation();

  /// \brief Call this in order to (re-)add this error term after whenever it had been modified.
  /// @param[in] parameterBlockPtrs Parameter block pointers in question.
  void getParameterBlockPtrs(
      std::vector<std::shared_ptr<okvis::ceres::ParameterBlock> >& parameterBlockPtrs);

  // error term and Jacobian implementation (inherited pure virtuals from ::ceres::CostFunction)
  /**
    * @brief This evaluates the error term and additionally computes the Jacobians.
    * @param parameters Pointer to the parameters (see ceres)
    * @param residuals Pointer to the residual vector (see ceres)
    * @param jacobians Pointer to the Jacobians (see ceres)
    * @return success of th evaluation.
    */
  virtual bool Evaluate(double const* const * parameters, double* residuals,
                        double** jacobians) const;

  /**
   * @brief This evaluates the error term and additionally computes
   *        the Jacobians in the minimal internal representation.
   * @param parameters Pointer to the parameters (see ceres)
   * @param residuals Pointer to the residual vector (see ceres)
   * @param jacobians Pointer to the Jacobians (see ceres)
   * @param jacobiansMinimal Pointer to the minimal Jacobians (equivalent to jacobians).
   * @return Success of the evaluation.
   */
  bool EvaluateWithMinimalJacobians(double const* const * parameters,
                                    double* residuals, double** jacobians,
                                    double** jacobiansMinimal) const;

  // sizes (inherited pure virtuals from ::okvis::ceres::ErrorInterface)
  /// \brief Residual dimension.
  size_t residualDim() const
  {
    return base_t::num_residuals();
  }

  /// \brief Number of parameter blocks.
  size_t parameterBlocks() const
  {
    return base_t::parameter_block_sizes().size();
  }

  /// \brief Dimension of an individual parameter block.
  /// @param[in] parameterBlockId ID of the parameter block of interest.
  /// \return The dimension.
  size_t parameterBlockDim(size_t parameterBlockId) const
  {
    return base_t::parameter_block_sizes().at(parameterBlockId);
  }

  /// @brief Return parameter block type as string
  virtual std::string typeInfo() const
  {
    return "MarginalizationError";
  }

  const Eigen::MatrixXd& H() const {
    return H_;
  }

 protected:
  Map* mapPtr_; ///< The underlying map.
  ::ceres::ResidualBlockId residualBlockId_; ///< The residual block id of this.

  /// \brief Checks the internal datastructure (debug)
  void check();

  /// \brief Computes the linearized deviation from the references (linearization points)
  bool computeDeltaChi(Eigen::VectorXd& DeltaChi) const;  // use the stored estimates

  /// \brief Computes the linearized deviation from the references (linearization points)
  bool computeDeltaChi(double const* const * parameters,
                       Eigen::VectorXd& DeltaChi) const;  // use the provided estimates

  /// \brief Split for Schur complement op.
  template<typename Derived_A, typename Derived_U, typename Derived_W,
      typename Derived_V>
  static void splitSymmetricMatrix(
      const std::vector<std::pair<int, int> >& marginalizationStartIdxAndLengthPairs,
      const Eigen::MatrixBase<Derived_A>& A,  // input
      const Eigen::MatrixBase<Derived_U>& U,  // output
      const Eigen::MatrixBase<Derived_W>& W,  // output
      const Eigen::MatrixBase<Derived_V>& V);  // output

  /// \brief Split for Schur complement op.
  template<typename Derived_b, typename Derived_b_a, typename Derived_b_b>
  static void splitVector(
      const std::vector<std::pair<int, int> >& marginalizationStartIdxAndLengthPairs,
      const Eigen::MatrixBase<Derived_b>& b,  // input
      const Eigen::MatrixBase<Derived_b_a>& b_a,  // output
      const Eigen::MatrixBase<Derived_b_b>& b_b);  // output

  /// @name The internal storage of the linearised system.
  /// lhs and rhs:
  /// H_*delta_Chi = _b - H_*Delta_Chi .
  /// the lhs Hessian matrix is decomposed as _H = J^T*J = _U*S*_U^T ,
  /// the rhs is decomposed as _b - _H*Delta_Chi = -J^T * (-pinv(J^T) * _b + J*Delta_Chi) ,
  /// i.e. we have the ceres standard form with weighted Jacobians _J,
  /// an identity information matrix, and an error
  /// _e = -pinv(J^T) * _b + J*Delta_Chi .
  /// _e = _e0 + J*Delta_Chi .
  /// @{
  Eigen::MatrixXd H_;  ///< lhs - Hessian
  Eigen::VectorXd b0_;  ///<  rhs constant part
  Eigen::VectorXd e0_;  ///<  _e0 := pinv(J^T) * _b0
  Eigen::MatrixXd J_;  ///<  Jacobian such that _J^T * J == _H
  Eigen::MatrixXd U_;  ///<  H_ = _U*_S*_U^T lhs Eigen decomposition
  Eigen::VectorXd S_;  ///<  singular values
  Eigen::VectorXd S_sqrt_;  ///<  cwise sqrt of _S, i.e. _S_sqrt*_S_sqrt=_S; _J=_U^T*_S_sqrt
  Eigen::VectorXd S_pinv_;  ///<  pseudo inverse of _S
  Eigen::VectorXd S_pinv_sqrt_;  ///<  cwise sqrt of _S_pinv, i.e. pinv(J^T)=_U^T*_S_pinv_sqrt
  Eigen::VectorXd p_;
  Eigen::VectorXd p_inv_;
  volatile bool errorComputationValid_;  ///<  adding residual blocks will invalidate this. before optimizing, call updateErrorComputation()

  /// \brief Book-keeping of the ordering.
  struct ParameterBlockInfo
  {
    OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
    uint64_t parameterBlockId;
    std::shared_ptr<ParameterBlock> parameterBlockPtr;
    size_t orderingIdx;
    size_t dimension;
    size_t minimalDimension;
    size_t localDimension;
    std::shared_ptr<double> linearizationPoint;
    bool isLandmark;
    ParameterBlockInfo()
        : parameterBlockId(0),
          parameterBlockPtr(std::shared_ptr<ParameterBlock>()),
          orderingIdx(0),
          dimension(0),
          minimalDimension(0),
          localDimension(0),
          isLandmark(false)
    {
    }
    ParameterBlockInfo(uint64_t parameterBlockId,
                       std::shared_ptr<ParameterBlock> parameterBlockPtr,
                       size_t orderingIdx, bool isLandmark)
        : parameterBlockId(parameterBlockId),
          parameterBlockPtr(parameterBlockPtr),
          orderingIdx(orderingIdx),
          isLandmark(isLandmark)
    {
      dimension = parameterBlockPtr->dimension();
      minimalDimension = parameterBlockPtr->minimalDimension();
      if (parameterBlockPtr->localParameterizationPtr()) {
        localDimension = parameterBlockPtr->localParameterizationPtr()
            ->LocalSize();
      } else {
        localDimension = minimalDimension;
      }
      if (parameterBlockPtr->fixed()) {
        minimalDimension = 0;
        localDimension = 0;
		  }
      linearizationPoint.reset(new double[dimension],
                               std::default_delete<double[]>());
      memcpy(linearizationPoint.get(), parameterBlockPtr->parameters(),
             dimension * sizeof(double));
    }

    /// \brief Reset the linearisation point. Use with caution.
    void resetLinearizationPoint(
        std::shared_ptr<ParameterBlock> parameterBlockPtr)
    {
      OKVIS_ASSERT_TRUE_DBG(Exception,dimension==parameterBlockPtr->dimension(),"not initialised.")
      memcpy(linearizationPoint.get(), parameterBlockPtr->parameters(),
             dimension * sizeof(double));
    }
  };

  std::vector<ParameterBlockInfo> parameterBlockInfos_;  ///< Book keeper.
  std::map<uint64_t, size_t> parameterBlockId2parameterBlockInfoIdx_;  ///< Maps parameter block Ids to index in _parameterBlockInfos
  size_t denseIndices_;  ///< Keep track of the size of the dense part of the equation system

  /// @}

};

}  // namespace ceres
}  // namespace okvis

#include "implementation/MarginalizationError.hpp"

#endif /* INCLUDE_OKVIS_CERES_MARGINALIZATIONERROR_HPP_ */


text: #include <boost/algorithm/string.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino