text:
/* vim: set sw=4 sts=4 et foldmethod=syntax : */

#include "proof.hh"

#include <algorithm>
#include <iterator>
#include <fstream>
#include <map>
#include <memory>
#include <sstream>
#include <tuple>
#include <utility>

#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filter/bzip2.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/stream.hpp>

using std::copy;
using std::find;
using std::function;
using std::istreambuf_iterator;
using std::make_unique;
using std::map;
using std::max;
using std::min;
using std::move;
using std::ofstream;
using std::optional;
using std::ostream;
using std::ostreambuf_iterator;
using std::pair;
using std::set;
using std::string;
using std::stringstream;
using std::to_string;
using std::tuple;
using std::unique_ptr;
using std::vector;

using boost::iostreams::bzip2_compressor;
using boost::iostreams::file_sink;
using boost::iostreams::filtering_ostream;

const char* endl = "\n";

namespace
{
    auto make_compressed_ostream(const string & fn) -> unique_ptr<ostream>
    {
        auto out = make_unique<filtering_ostream>();
        out->push(bzip2_compressor());
        out->push(file_sink(fn));
        return out;
    }
}

ProofError::ProofError(const string & m) noexcept :
    _message("Proof error: " + m)
{
}

auto ProofError::what() const noexcept -> const char *
{
    return _message.c_str();
}

struct Proof::Imp
{
    string opb_filename, log_filename;
    stringstream model_stream, model_prelude_stream;
    unique_ptr<ostream> proof_stream;
    bool friendly_names;
    bool bz2 = false;

    map<pair<long, long>, string> variable_mappings;
    map<long, string> binary_variable_mappings;
    map<tuple<long, long, long>, string> connected_variable_mappings;
    map<tuple<long, long, long, long>, string> connected_variable_mappings_aux;
    map<long, long> at_least_one_value_constraints, at_most_one_value_constraints, injectivity_constraints;
    map<tuple<long, long, long, long>, long> adjacency_lines;
    map<pair<long, long>, long> eliminations;
    map<pair<long, long>, long> non_edge_constraints;
    long objective_line = 0;

    long nb_constraints = 0;
    long proof_line = 0;
    int largest_level_set = 0;

    bool clique_encoding = false;
};

Proof::Proof(const string & opb_file, const string & log_file, bool f, bool b) :
    _imp(new Imp)
{
    _imp->opb_filename = opb_file;
    _imp->log_filename = log_file;
    _imp->friendly_names = f;
    _imp->bz2 = b;
}

Proof::Proof(Proof &&) = default;

Proof::~Proof() = default;

auto Proof::operator= (Proof &&) -> Proof & = default;

auto Proof::create_cp_variable(int pattern_vertex, int target_size,
        const function<auto (int) -> string> & pattern_name,
        const function<auto (int) -> string> & target_name) -> void
{
    for (int i = 0 ; i < target_size ; ++i)
        if (_imp->friendly_names)
            _imp->variable_mappings.emplace(pair{ pattern_vertex, i }, pattern_name(pattern_vertex) + "_" + target_name(i));
        else
            _imp->variable_mappings.emplace(pair{ pattern_vertex, i }, to_string(_imp->variable_mappings.size() + 1));

    _imp->model_stream << "* vertex " << pattern_vertex << " domain" << endl;
    for (int i = 0 ; i < target_size ; ++i)
        _imp->model_stream << "1 x" << _imp->variable_mappings[{ pattern_vertex, i }] << " ";
    _imp->model_stream << ">= 1 ;" << endl;
    _imp->at_least_one_value_constraints.emplace(pattern_vertex, ++_imp->nb_constraints);

    for (int i = 0 ; i < target_size ; ++i)
        _imp->model_stream << "-1 x" << _imp->variable_mappings[{ pattern_vertex, i }] << " ";
    _imp->model_stream << ">= -1 ;" << endl;
    _imp->at_most_one_value_constraints.emplace(pattern_vertex, ++_imp->nb_constraints);
}

auto Proof::create_injectivity_constraints(int pattern_size, int target_size) -> void
{
    for (int v = 0 ; v < target_size ; ++v) {
        _imp->model_stream << "* injectivity on value " << v << endl;

        for (int p = 0 ; p < pattern_size ; ++p) {
            auto x = _imp->variable_mappings.find(pair{ p, v });
            if (x != _imp->variable_mappings.end())
                _imp->model_stream << "-1 x" << x->second << " ";
        }
        _imp->model_stream << ">= -1 ;" << endl;
        _imp->injectivity_constraints.emplace(v, ++_imp->nb_constraints);
    }
}

auto Proof::create_forbidden_assignment_constraint(int p, int t) -> void
{
    _imp->model_stream << "* forbidden assignment" << endl;
    _imp->model_stream << "1 ~x" << _imp->variable_mappings[pair{ p, t }] << " >= 1 ;" << endl;
    ++_imp->nb_constraints;
    _imp->eliminations.emplace(pair{ p, t }, _imp->nb_constraints);
}

auto Proof::start_adjacency_constraints_for(int p, int t) -> void
{
    _imp->model_stream << "* adjacency " << p << " maps to " << t << endl;
}

auto Proof::create_adjacency_constraint(int p, int q, int t, const vector<int> & uu) -> void
{
    _imp->model_stream << "1 ~x" << _imp->variable_mappings[pair{ p, t }];
    for (auto & u : uu)
        _imp->model_stream << " 1 x" << _imp->variable_mappings[pair{ q, u }];
    _imp->model_stream << " >= 1 ;" << endl;
    _imp->adjacency_lines.emplace(tuple{ 0, p, q, t }, ++_imp->nb_constraints);
}

auto Proof::finalise_model() -> void
{
    unique_ptr<ostream> f = (_imp->bz2 ? make_compressed_ostream(_imp->opb_filename + ".bz2") : make_unique<ofstream>(_imp->opb_filename));

    *f << "* #variable= " << (_imp->variable_mappings.size() + _imp->binary_variable_mappings.size()
            + _imp->connected_variable_mappings.size() + _imp->connected_variable_mappings_aux.size())
        << " #constraint= " << _imp->nb_constraints << endl;
    copy(istreambuf_iterator<char>{ _imp->model_prelude_stream }, istreambuf_iterator<char>{}, ostreambuf_iterator<char>{ *f });
    _imp->model_prelude_stream.clear();
    copy(istreambuf_iterator<char>{ _imp->model_stream }, istreambuf_iterator<char>{}, ostreambuf_iterator<char>{ *f });
    _imp->model_stream.clear();

    if (! *f)
        throw ProofError{ "Error writing opb file to '" + _imp->opb_filename + "'" };

    _imp->proof_stream = (_imp->bz2 ? make_compressed_ostream(_imp->log_filename + ".bz2") : make_unique<ofstream>(_imp->log_filename));

    *_imp->proof_stream << "pseudo-Boolean proof version 1.0" << endl;

    *_imp->proof_stream << "f " << _imp->nb_constraints << " 0" << endl;
    _imp->proof_line += _imp->nb_constraints;

    if (! *_imp->proof_stream)
        throw ProofError{ "Error writing proof file to '" + _imp->log_filename + "'" };
}

auto Proof::finish_unsat_proof() -> void
{
    *_imp->proof_stream << "* asserting that we've proved unsat" << endl;
    *_imp->proof_stream << "u >= 1 ;" << endl;
    ++_imp->proof_line;
    *_imp->proof_stream << "c " << _imp->proof_line << " 0" << endl;
}

auto Proof::failure_due_to_pattern_bigger_than_target() -> void
{
    *_imp->proof_stream << "* failure due to the pattern being bigger than the target" << endl;

    // we get a hall violator by adding up all of the things
    *_imp->proof_stream << "p";
    bool first = true;

    for (auto & [ _, line ] : _imp->at_least_one_value_constraints) {
        if (first) {
            *_imp->proof_stream << " " << line;
            first = false;
        }
        else
            *_imp->proof_stream << " " << line << " +";
    }

    for (auto & [ _, line ] : _imp->injectivity_constraints)
        *_imp->proof_stream << " " << line << " +";
    *_imp->proof_stream << " 0" << endl;
    ++_imp->proof_line;
}

auto Proof::incompatible_by_degrees(
        int g,
        const NamedVertex & p,
        const vector<int> & n_p,
        const NamedVertex & t,
        const vector<int> & n_t) -> void
{
    *_imp->proof_stream << "* cannot map " << p.second << " to " << t.second << " due to degrees in graph pairs " << g << endl;

    *_imp->proof_stream << "p";
    bool first = true;
    for (auto & n : n_p) {
        // due to loops or labels, it might not be possible to map n to t.first
        if (_imp->adjacency_lines.count(tuple{ g, p.first, n, t.first })) {
            if (first) {
                first = false;
                *_imp->proof_stream << " " << _imp->adjacency_lines[tuple{ g, p.first, n, t.first }];
            }
            else
                *_imp->proof_stream << " " << _imp->adjacency_lines[tuple{ g, p.first, n, t.first }] << " +";
        }
    }

    // if I map p to t, I have to map the neighbours of p to neighbours of t
    for (auto & n : n_t)
        *_imp->proof_stream << " " << _imp->injectivity_constraints[n] << " +";

    *_imp->proof_stream << " 0" << endl;
    ++_imp->proof_line;

    *_imp->proof_stream << "j " << _imp->proof_line << " 1 ~x" << _imp->variable_mappings[pair{ p.first, t.first }] << " >= 1 ;" << endl;
    ++_imp->proof_line;
    _imp->eliminations.emplace(pair{ p.first, t.first }, _imp->proof_line);

    *_imp->proof_stream << "d " << _imp->proof_line - 1 << " 0" << endl;
}

auto Proof::incompatible_by_nds(
        int g,
        const NamedVertex & p,
        const NamedVertex & t,
        const vector<int> & p_subsequence,
        const vector<int> & t_subsequence,
        const vector<int> & t_remaining) -> void
{
    *_imp->proof_stream << "* cannot map " << p.second << " to " << t.second << " due to nds in graph pairs " << g << endl;

    // summing up horizontally
    *_imp->proof_stream << "p";
    bool first = true;
    for (auto & n : p_subsequence) {
        // due to loops or labels, it might not be possible to map n to t.first
        if (_imp->adjacency_lines.count(tuple{ g, p.first, n, t.first })) {
            if (first) {
                first = false;
                *_imp->proof_stream << " " << _imp->adjacency_lines[tuple{ g, p.first, n, t.first }];
            }
            else
                *_imp->proof_stream << " " << _imp->adjacency_lines[tuple{ g, p.first, n, t.first }] << " +";
        }
    }

    // injectivity in the square
    for (auto & t : t_subsequence) {
        if (t != t_subsequence.back())
            *_imp->proof_stream << " " << _imp->injectivity_constraints.find(t)->second << " +";
    }

    // block to the right of the failing square
    for (auto & n : p_subsequence) {
        for (auto & u : t_remaining) {
            /* n -> t is already eliminated by degree or loop */
            *_imp->proof_stream << " " << _imp->eliminations[pair{ n, u }] << " +";
        }
    }

    // final column
    for (auto & n : p_subsequence) {
        /* n -> t is already eliminated by degree or loop */
        *_imp->proof_stream << " " << _imp->eliminations[pair{ n, t_subsequence.back() }] << " +";
    }

    *_imp->proof_stream << " 0" << endl;
    ++_imp->proof_line;

    *_imp->proof_stream << "j " << _imp->proof_line << " 1 ~x" << _imp->variable_mappings[pair{ p.first, t.first }] << " >= 1 ;" << endl;
    ++_imp->proof_line;

    *_imp->proof_stream << "d " << _imp->proof_line - 1 << " 0" << endl;
}

auto Proof::initial_domain_is_empty(int p) -> void
{
    *_imp->proof_stream << "* failure due to domain " << p << " being empty" << endl;
}

auto Proof::emit_hall_set_or_violator(const vector<int> & lhs, const vector<int> & rhs) -> void
{
    *_imp->proof_stream << "* hall set or violator size " << lhs.size() << "/" << rhs.size() << endl;
    *_imp->proof_stream << "p";
    bool first = true;
    for (auto & l : lhs) {
        if (first) {
            first = false;
            *_imp->proof_stream << " " << _imp->at_least_one_value_constraints[l];
        }
        else
            *_imp->proof_stream << " " << _imp->at_least_one_value_constraints[l] << " +";
    }
    for (auto & r : rhs)
        *_imp->proof_stream << " " << _imp->injectivity_constraints[r] << " +";
    *_imp->proof_stream << " 0" << endl;
    ++_imp->proof_line;
}

auto Proof::root_propagation_failed() -> void
{
    *_imp->proof_stream << "* root node propagation failed" << endl;
}

auto Proof::guessing(int depth, const NamedVertex & branch_v, const NamedVertex & val) -> void
{
    *_imp->proof_stream << "* [" << depth << "] guessing " << branch_v.second << "=" << val.second << endl;
}

auto Proof::propagation_failure(const vector<pair<int, int> > & decisions, const NamedVertex & branch_v, const NamedVertex & val) -> void
{
    *_imp->proof_stream << "* [" << decisions.size() << "] propagation failure on " << branch_v.second << "=" << val.second << endl;
    *_imp->proof_stream << "u ";
    for (auto & [ var, val ] : decisions)
        *_imp->proof_stream << " 1 ~x" << _imp->variable_mappings[pair{ var, val }];
    *_imp->proof_stream << " >= 1 ;" << endl;
    ++_imp->proof_line;
}

auto Proof::incorrect_guess(const vector<pair<int, int> > & decisions, bool failure) -> void
{
    if (failure)
        *_imp->proof_stream << "* [" << decisions.size() << "] incorrect guess" << endl;
    else
        *_imp->proof_stream << "* [" << decisions.size() << "] backtracking" << endl;

    *_imp->proof_stream << "u";
    for (auto & [ var, val ] : decisions)
        *_imp->proof_stream << " 1 ~x" << _imp->variable_mappings[pair{ var, val }];
    *_imp->proof_stream << " >= 1 ;" << endl;
    ++_imp->proof_line;
}

auto Proof::out_of_guesses(const vector<pair<int, int> > &) -> void
{
}

auto Proof::unit_propagating(const NamedVertex & var, const NamedVertex & val) -> void
{
    *_imp->proof_stream << "* unit propagating " << var.second << "=" << val.second << endl;
}

auto Proof::start_level(int l) -> void
{
    *_imp->proof_stream << "# " << l << endl;
    _imp->largest_level_set = max(_imp->largest_level_set, l);
}

auto Proof::back_up_to_level(int l) -> void
{
    *_imp->proof_stream << "# " << l << endl;
    _imp->largest_level_set = max(_imp->largest_level_set, l);
}

auto Proof::forget_level(int l) -> void
{
    if (_imp->largest_level_set >= l)
        *_imp->proof_stream << "w " << l << endl;
}

auto Proof::back_up_to_top() -> void
{
    *_imp->proof_stream << "# " << 0 << endl;
}

auto Proof::post_restart_nogood(const vector<pair<int, int> > & decisions) -> void
{
    *_imp->proof_stream << "* [" << decisions.size() << "] restart nogood" << endl;
    *_imp->proof_stream << "u";
    for (auto & [ var, val ] : decisions)
        *_imp->proof_stream << " 1 ~x" << _imp->variable_mappings[pair{ var, val }];
    *_imp->proof_stream << " >= 1 ;" << endl;
    ++_imp->proof_line;
}

auto Proof::post_solution(const vector<pair<NamedVertex, NamedVertex> > & decisions) -> void
{
    *_imp->proof_stream << "* found solution";
    for (auto & [ var, val ] : decisions)
        *_imp->proof_stream << " " << var.second << "=" << val.second;
    *_imp->proof_stream << endl;

    *_imp->proof_stream << "v";
    for (auto & [ var, val ] : decisions)
        *_imp->proof_stream << " x" << _imp->variable_mappings[pair{ var.first, val.first }];
    *_imp->proof_stream << endl;
    ++_imp->proof_line;
}

auto Proof::post_solution(const vector<int> & solution) -> void
{
    *_imp->proof_stream << "v";
    for (auto & v : solution)
        *_imp->proof_stream << " x" << _imp->binary_variable_mappings[v];
    *_imp->proof_stream << endl;
    ++_imp->proof_line;
}

auto Proof::new_incumbent(const vector<pair<int, bool> > & solution) -> void
{
    *_imp->proof_stream << "o";
    for (auto & [ v, t ] : solution)
        *_imp->proof_stream << " " << (t ? "" : "~") << "x" << _imp->binary_variable_mappings[v];
    *_imp->proof_stream << endl;
    _imp->objective_line = ++_imp->proof_line;
}

auto Proof::new_incumbent(const vector<tuple<NamedVertex, NamedVertex, bool> > & decisions) -> void
{
    *_imp->proof_stream << "o";
    for (auto & [ var, val, t ] : decisions)
        *_imp->proof_stream << " " << (t ? "" : "~") << "x" << _imp->variable_mappings[pair{ var.first, val.first }];
    *_imp->proof_stream << endl;
    _imp->objective_line = ++_imp->proof_line;
}

auto Proof::create_exact_path_graphs(
        int g,
        const NamedVertex & p,
        const NamedVertex & q,
        const vector<NamedVertex> & between_p_and_q,
        const NamedVertex & t,
        const vector<NamedVertex> & n_t,
        const vector<pair<NamedVertex, vector<NamedVertex> > > & two_away_from_t,
        const vector<NamedVertex> & d_n_t
        ) -> void
{
    *_imp->proof_stream << "* adjacency " << p.second << " maps to " << t.second <<
        " in G^[" << g << "x2] so " << q.second << " maps to one of..." << endl;

    *_imp->proof_stream << "# 1" << endl;

    *_imp->proof_stream << "p";

    // if p maps to t then things in between_p_and_q have to go to one of these...
    bool first = true;
    for (auto & b : between_p_and_q) {
        *_imp->proof_stream << " " << _imp->adjacency_lines[tuple{ 0, p.first, b.first, t.first }];
        if (! first)
            *_imp->proof_stream << " +";
        first = false;
    }

    // now go two hops out: cancel between_p_and_q things with where can q go
    for (auto & b : between_p_and_q) {
        for (auto & w : n_t) {
            // due to loops or labels, it might not be possible to map to w
            auto i = _imp->adjacency_lines.find(tuple{ 0, b.first, q.first, w.first });
            if (i != _imp->adjacency_lines.end())
                *_imp->proof_stream << " " << i->second << " +";
        }
    }

    *_imp->proof_stream << " 0" << endl;
    ++_imp->proof_line;

    // first tidy-up step: if p maps to t then q maps to something a two-walk away from t
    *_imp->proof_stream << "j " << _imp->proof_line << " 1 ~x" << _imp->variable_mappings[pair{ p.first, t.first }];
    for (auto & u : two_away_from_t)
        *_imp->proof_stream << " 1 x" << _imp->variable_mappings[pair{ q.first, u.first.first }];
    *_imp->proof_stream << " >= 1 ;" << endl;
    ++_imp->proof_line;

    // if p maps to t then q does not map to t
    *_imp->proof_stream << "p " << _imp->proof_line << " " << _imp->injectivity_constraints[t.first] << " + 0" << endl;
    ++_imp->proof_line;

    // and cancel out stray extras from injectivity
    *_imp->proof_stream << "j " << _imp->proof_line << " 1 ~x" << _imp->variable_mappings[pair{ p.first, t.first }];
    for (auto & u : two_away_from_t)
        if (u.first != t)
            *_imp->proof_stream << " 1 x" << _imp->variable_mappings[pair{ q.first, u.first.first }];
    *_imp->proof_stream << " >= 1 ;" << endl;
    ++_imp->proof_line;

    vector<long> things_to_add_up;
    things_to_add_up.push_back(_imp->proof_line);

    // cancel out anything that is two away from t, but by insufficiently many paths
    for (auto & u : two_away_from_t) {
        if ((u.first == t) || (d_n_t.end() != find(d_n_t.begin(), d_n_t.end(), u.first)))
            continue;

        *_imp->proof_stream << "p";
        bool first = true;
        for (auto & b : between_p_and_q) {
            *_imp->proof_stream << " " << _imp->adjacency_lines[tuple{ 0, p.first, b.first, t.first }];
            if (! first)
                *_imp->proof_stream << " +";
            first = false;
            *_imp->proof_stream << " " << _imp->adjacency_lines[tuple{ 0, q.first, b.first, u.first.first }] << " +";
            *_imp->proof_stream << " " << _imp->at_most_one_value_constraints[b.first] << " +";
        }

        for (auto & z : u.second)
            *_imp->proof_stream << " " << _imp->injectivity_constraints[z.first] << " +";

        *_imp->proof_stream << " 0" << endl;
        ++_imp->proof_line;

        // want: ~x_p_t + ~x_q_u >= 1
        *_imp->proof_stream << "j " << _imp->proof_line << " 1 ~x" << _imp->variable_mappings[pair{ p.first, t.first }]
            << " 1 ~x" << _imp->variable_mappings[pair{ q.first, u.first.first }] << " >= 1 ;" << endl;
        things_to_add_up.push_back(++_imp->proof_line);
    }

    // do the getting rid of
    if (things_to_add_up.size() > 1) {
        bool first = true;
        *_imp->proof_stream << "p";
        for (auto & t : things_to_add_up) {
            *_imp->proof_stream << " " << t;
            if (! first)
                *_imp->proof_stream << " +";
            first = false;
        }
        *_imp->proof_stream << " 0" << endl;
        ++_imp->proof_line;
    }

    *_imp->proof_stream << "# 0" << endl;

    // and finally, tidy up to get what we wanted
    *_imp->proof_stream << "j " << _imp->proof_line << " 1 ~x" << _imp->variable_mappings[pair{ p.first, t.first }];
    for (auto & u : d_n_t)
        if (u != t)
            *_imp->proof_stream << " 1 x" << _imp->variable_mappings[pair{ q.first, u.first }];
    *_imp->proof_stream << " >= 1 ;" << endl;
    ++_imp->proof_line;

    _imp->adjacency_lines.emplace(tuple{ g, p.first, q.first, t.first }, _imp->proof_line);

    *_imp->proof_stream << "w 1" << endl;
}

auto Proof::create_binary_variable(int vertex,
                const function<auto (int) -> string> & name) -> void
{
    if (_imp->friendly_names)
        _imp->binary_variable_mappings.emplace(vertex, name(vertex));
    else
        _imp->binary_variable_mappings.emplace(vertex, to_string(_imp->binary_variable_mappings.size() + 1));
}

auto Proof::create_objective(int n, optional<int> d) -> void
{
    if (d) {
        _imp->model_stream << "* objective" << endl;
        for (int v = 0 ; v < n ; ++ v)
            _imp->model_stream << "1 x" << _imp->binary_variable_mappings[v] << " ";
        _imp->model_stream << ">= " << *d << ";" << endl;
        _imp->objective_line = ++_imp->nb_constraints;
    }
    else {
        _imp->model_prelude_stream << "min:";
        for (int v = 0 ; v < n ; ++ v)
            _imp->model_prelude_stream << " -1 x" << _imp->binary_variable_mappings[v];
        _imp->model_prelude_stream << " ;" << endl;
    }
}

auto Proof::create_non_edge_constraint(int p, int q) -> void
{
    _imp->model_stream << "-1 x" << _imp->binary_variable_mappings[p] << " -1 x" << _imp->binary_variable_mappings[q] << " >= -1 ;" << endl;

    ++_imp->nb_constraints;
    _imp->non_edge_constraints.emplace(pair{ p, q }, _imp->nb_constraints);
    _imp->non_edge_constraints.emplace(pair{ q, p }, _imp->nb_constraints);
}

auto Proof::create_non_null_decision_bound(int p, int t, optional<int> d) -> void
{
    if (d) {
        _imp->model_stream << "* objective" << endl;
        for (int v = 0 ; v < p ; ++v)
            for (int w = 0 ; w < t ; ++w)
                _imp->model_stream << "1 x" << _imp->variable_mappings[pair{ v, w }] << " ";
        _imp->model_stream << ">= " << *d << " ;" << endl;
        _imp->objective_line = ++_imp->nb_constraints;
    }
    else {
        _imp->model_prelude_stream << "min:";
        for (int v = 0 ; v < p ; ++v)
            for (int w = 0 ; w < t ; ++w)
                _imp->model_prelude_stream << " -1 x" << _imp->variable_mappings[pair{ v, w }] << " ";
        _imp->model_prelude_stream << " ;" << endl;
    }
}

auto Proof::backtrack_from_binary_variables(const vector<int> & v) -> void
{
    *_imp->proof_stream << "u";
    for (auto & w : v)
        *_imp->proof_stream << " 1 ~x" << _imp->binary_variable_mappings[w];
    *_imp->proof_stream << " >= 1 ;" << endl;
    ++_imp->proof_line;
}

auto Proof::colour_bound(const vector<vector<int> > & ccs) -> void
{
    *_imp->proof_stream << "* bound using";
    for (auto & cc : ccs) {
        *_imp->proof_stream << " [";
        for (auto & c : cc)
            *_imp->proof_stream << " x" << _imp->binary_variable_mappings[c];
        *_imp->proof_stream << " ]";
    }
    *_imp->proof_stream << endl;

    vector<long> to_sum;
    for (auto & cc : ccs) {
        if (cc.size() > 2) {
            *_imp->proof_stream << "p " << _imp->non_edge_constraints[pair{ cc[0], cc[1] }];

            for (unsigned i = 2 ; i < cc.size() ; ++i) {
                *_imp->proof_stream << " " << i << " *";
                for (unsigned j = 0 ; j < i ; ++j)
                    *_imp->proof_stream << " " << _imp->non_edge_constraints[pair{ cc[i], cc[j] }] << " +";
                *_imp->proof_stream << " " << (i + 1) << " d";
            }

            *_imp->proof_stream << endl;
            to_sum.push_back(++_imp->proof_line);
        }
        else if (cc.size() == 2) {
            to_sum.push_back(_imp->non_edge_constraints[pair{ cc[0], cc[1] }]);
        }

        *_imp->proof_stream << "p " << _imp->objective_line;
        for (auto & t : to_sum)
            *_imp->proof_stream << " " << t << " +";
        *_imp->proof_stream << endl;
        ++_imp->proof_line;
    }
}

auto Proof::mcs_bound(
        const vector<pair<set<int>, set<int> > > & partitions) -> void
{
    *_imp->proof_stream << "* failed bound" << endl;

    vector<string> to_sum;
    for (auto & [ l, r ] : partitions) {
        if (r.size() >= l.size())
            continue;

        *_imp->proof_stream << "p";
        bool first = true;
        for (auto & v : l) {
            *_imp->proof_stream << " " << _imp->at_least_one_value_constraints[v];
           if (first)
              first = false;
           else
              *_imp->proof_stream << " +";
        }
        for (auto & v : r)
            *_imp->proof_stream << " " << _imp->injectivity_constraints[v] << " +";

        *_imp->proof_stream << endl;
        to_sum.push_back(to_string(++_imp->proof_line));
    }

    if (! to_sum.empty()) {
        *_imp->proof_stream << "p " << _imp->objective_line;
        for (auto & t : to_sum)
            *_imp->proof_stream << " " << t << " +";
        *_imp->proof_stream << endl;
        ++_imp->proof_line;
    }
}

auto Proof::rewrite_mcs_objective(int pattern_size) -> void
{
    *_imp->proof_stream << "* get the objective function to talk about nulls, not non-nulls" << endl;
    *_imp->proof_stream << "p " << _imp->objective_line;
    for (int v = 0 ; v < pattern_size ; ++v)
        *_imp->proof_stream << " " << _imp->at_most_one_value_constraints[v] << " +";
    *_imp->proof_stream << endl;
    _imp->objective_line = ++_imp->proof_line;
}

auto Proof::create_connected_constraints(int p, int t, const function<auto (int, int) -> bool> & adj) -> void
{
    _imp->model_stream << "* selected vertices must be connected, walk 1" << endl;
    int mapped_to_null = t;
    int cnum = _imp->variable_mappings.size();

    for (int v = 0 ; v < p ; ++v)
        for (int w = 0 ; w < v ; ++w) {
            string n = _imp->friendly_names ? ("conn1_" + to_string(v) + "_" + to_string(w)) : to_string(++cnum);
            _imp->connected_variable_mappings.emplace(tuple{ 1, v, w }, n);
            if (! adj(v, w)) {
                // v not adjacent to w, so the walk does not exist
                _imp->model_stream << "1 ~x" << n << " >= 1 ;" << endl;
                ++_imp->nb_constraints;
            }
            else {
                // v = null -> the walk does not exist
                _imp->model_stream << "1 ~x" << n << " 1 ~x" << _imp->variable_mappings[pair{ v, mapped_to_null }] << " >= 1 ;" << endl;
                // w = null -> the walk does not exist
                _imp->model_stream << "1 ~x" << n << " 1 ~x" << _imp->variable_mappings[pair{ w, mapped_to_null }] << " >= 1 ;" << endl;
                // either v = null, or w = null, or the walk exists
                _imp->model_stream << "1 x" << n << " 1 x" << _imp->variable_mappings[pair{ v, mapped_to_null }]
                    << " 1 x" << _imp->variable_mappings[pair{ w, mapped_to_null }] << " >= 1 ;" << endl;
                _imp->nb_constraints += 3;
            }
        }

    int last_k = 0;
    for (int k = 2 ; k < 2 * t ; k *= 2) {
        last_k = k;
        _imp->model_stream << "* selected vertices must be connected, walk " << k << endl;
        for (int v = 0 ; v < p ; ++v)
            for (int w = 0 ; w < v ; ++w) {
                string n = _imp->friendly_names ? ("conn" + to_string(k) + "_" + to_string(v) + "_" + to_string(w)) : to_string(++cnum);
                _imp->connected_variable_mappings.emplace(tuple{ k, v, w }, n);

                vector<string> ors;
                for (int u = 0 ; u < p ; ++u) {
                    if (v != w && v != u && u != w) {
                        string m = _imp->friendly_names ?
                            ("conn" + to_string(k) + "_" + to_string(v) + "_" + to_string(w) + "_via_" + to_string(u)) :
                            to_string(++cnum);
                        ors.push_back(m);
                        _imp->connected_variable_mappings_aux.emplace(tuple{ k, v, w, u }, m);
                        // either the first half walk exists, or the via term is false
                        _imp->model_stream << "1 x" << _imp->connected_variable_mappings[tuple{ k / 2, max(u, v), min(u, v) }]
                            << " 1 ~x" << m << " >= 1 ;" << endl;
                        // either the second half walk exists, or the via term is false
                        _imp->model_stream << "1 x" << _imp->connected_variable_mappings[tuple{ k / 2, max(u, w), min(u, w) }]
                            << " 1 ~x" << m << " >= 1 ;" << endl;
                        // one of the half walks is false, or the via term must be true
                        _imp->model_stream << "1 x" << m
                            << " 1 ~x" << _imp->connected_variable_mappings[tuple{ k / 2, max(v, u), min(v, u) }]
                            << " 1 ~x" << _imp->connected_variable_mappings[tuple{ k / 2, max(u, w), min(u, w) }] << " >= 1 ;" << endl;
                        _imp->nb_constraints += 3;
                    }
                }

                // one of the vias must be true, or a shorter walk exists, or the entry is false
                _imp->model_stream << "1 ~x" << n;
                for (auto & o : ors)
                    _imp->model_stream << " 1 x" << o;
                _imp->model_stream << " 1 x" << _imp->connected_variable_mappings[tuple{ k / 2, v, w }];
                _imp->model_stream << " >= 1 ;" << endl;
                ++_imp->nb_constraints;

                // if the entry is false, then all of the vias must be false and the shorter walk must be false
                for (auto & o : ors) {
                    _imp->model_stream << "1 x" << n << " 1 ~x" << o << " >= 1 ;" << endl;
                    ++_imp->nb_constraints;
                }
                _imp->model_stream << "1 x" << n << " 1 ~x" << _imp->connected_variable_mappings[tuple{ k / 2, v, w }] << " >= 1 ;" << endl;
                ++_imp->nb_constraints;
            }
    }

    _imp->model_stream << "* if two vertices are used, they must be connected" << endl;
    for (int v = 0 ; v < p ; ++v)
        for (int w = 0 ; w < v ; ++w) {
            _imp->model_stream << "1 x" << _imp->variable_mappings[pair{ v, mapped_to_null }]
                << " 1 x" << _imp->variable_mappings[pair{ w, mapped_to_null }]
                << " 1 x" << _imp->connected_variable_mappings[tuple{ last_k, v, w }] << " >= 1 ;" << endl;
            ++_imp->nb_constraints;
        }
}

auto Proof::not_connected_in_underlying_graph(const std::vector<int> & x, int y) -> void
{
    *_imp->proof_stream << "u 1 ~x" << _imp->binary_variable_mappings[y];
    for (auto & v : x)
        *_imp->proof_stream << " 1 ~x" << _imp->binary_variable_mappings[v];
    *_imp->proof_stream << " >= 1 ;" << endl;
    ++_imp->proof_line;
}

auto Proof::has_clique_model() const -> bool
{
    return _imp->clique_encoding;
}

auto Proof::create_clique_encoding(
        const vector<pair<int, int> > & enc) -> void
{
    _imp->clique_encoding = true;
    for (unsigned i = 0 ; i < enc.size() ; ++i)
        _imp->binary_variable_mappings.emplace(i, _imp->variable_mappings[enc[i]]);
}

auto Proof::create_clique_nonedge(int v, int w) -> void
{
    *_imp->proof_stream << "u 1 ~x" << _imp->binary_variable_mappings[v] <<
        " 1 ~x" << _imp->binary_variable_mappings[w] << " >= 1 ;" << endl;
    ++_imp->proof_line;
    _imp->non_edge_constraints.emplace(pair{ v, w }, _imp->proof_line);
    _imp->non_edge_constraints.emplace(pair{ w, v }, _imp->proof_line);
}

ext: cc

max_stars_repo_name: StephanGocht/glasgow-subgraph-solver

max_issues_repo_name: StephanGocht/glasgow-subgraph-solver

max_forks_repo_name: StephanGocht/glasgow-subgraph-solver


text:
// -*- mode: C++ -*-
/**
   Dietrich Bollmann, Kamakura, 2015/01/01

   Spring-test.h

   Unit tests for class: Spring

   Copyright (c) 2015 Dietrich Bollmann

   This software may be modified and distributed under the terms
   of the MIT license.  See the LICENSE file for details.
*/

#define BOOST_TEST_DYN_LINK
#ifdef STAND_ALONE
#   define BOOST_TEST_MODULE Main
#endif
#include <boost/test/unit_test.hpp>

#include "Spring.h"

BOOST_AUTO_TEST_SUITE(TestSuite_Spring)

BOOST_AUTO_TEST_CASE(Test_Spring_add)
{
  // TODO
}

BOOST_AUTO_TEST_SUITE_END()

/* fin */

ext: cpp


text:
/*
Copyright (c) 2015-2019, Florian Sittel (www.lettis.net) and Daniel Nagel
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.

2. 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.

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

#include "noise.hpp"

#include "logger.hpp"
#include "tools.hpp"

#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>

#include <boost/filesystem.hpp>
#include <omp.h>

namespace Clustering {
namespace Noise {
  void
  main(boost::program_options::variables_map args) {
    using namespace Clustering::Tools;
    namespace b_fs = boost::filesystem;
    namespace b_po = boost::program_options;
    // load states
    Clustering::logger(std::cout) << "~~~ reading files\n"
                                  << "    trajectory from: " << args["states"].as<std::string>()
                                  << std::endl;
    std::vector<std::size_t> states = Clustering::Tools::read_clustered_trajectory(args["states"].as<std::string>());
    std::vector<std::size_t> states_without_noise;
    std::copy(states.begin(), states.end(), std::back_inserter(states_without_noise));
    std::set<std::size_t> state_names(states.begin(), states.end());
    std::size_t n_frames = states.size();
    // shift cmin fomr [0,100] -> [0,1]
    float cmin = 0.01*args["cmin"].as<float>();
    std::string basename = args["basename"].as<std::string>();
    basename.append(".");
    Clustering::verbose = args["verbose"].as<bool>();
    // generate header comment
    std::string header_comment = args["header"].as<std::string>();
    std::map<std::string,float> commentsMap = args["commentsMap"].as<std::map<std::string,float>>();
    // read previously used parameters
    read_comments(args["states"].as<std::string>(), commentsMap);
    commentsMap["cmin"] = cmin;
    // noise state is 1 lower than lowest
    auto lowestState = std::min_element(states.begin(), states.end());
    std::size_t noiseState = *lowestState-1;

    if (args.count("output") || args.count("cores")) {
      // load concatenation limits to treat concatenated trajectories correctly
      // when performing dynamical corrections
      std::vector<std::size_t> concat_limits;
      if (args.count("concat-limits")) {
        Clustering::logger(std::cout) << "    limits from: "
                                      << args["concat-limits"].as<std::string>() << std::endl;
        concat_limits = Clustering::Tools::read_concat_limits(args["concat-limits"].as<std::string>());
      } else if (args.count("concat-nframes")) {
        std::size_t n_frames_per_subtraj = args["concat-nframes"].as<std::size_t>();
        for (std::size_t i=n_frames_per_subtraj; i <= n_frames; i += n_frames_per_subtraj) {
          concat_limits.push_back(i);
        }
      } else {
        concat_limits = {n_frames};
      }
      // check if concat_limits are well definied
      Clustering::Tools::check_concat_limits(concat_limits, n_frames);
      Clustering::logger(std::cout) << "    interpret data as " << concat_limits.size()
                                    << " trajectories" << std::endl;
      if (commentsMap["limits"] == 0) {
        commentsMap["limits"] = concat_limits.size();
      } else if (std::abs(commentsMap["limits"]-concat_limits.size()) > 0.001) {
        Clustering::logger(std::cout) << "warning: the number of limits are not in agreement\n"
                                      << "         " << commentsMap["limits"] << " vs. "
                                      << concat_limits.size() << std::endl;
      }
      // findest highest clust file
      b_fs::path cwd(b_fs::current_path());
      b_fs::path wd_basename(basename);

      // if relative or absolute path is given, use this instead of cwd
      if (wd_basename.parent_path() != "") {
        cwd = wd_basename.parent_path();
      }

      typedef std::vector<b_fs::path> vec;             // store paths,
      vec v;                                           // so we can sort them later

      std::copy(b_fs::directory_iterator(cwd), b_fs::directory_iterator(), std::back_inserter(v));

      std::sort(v.begin(), v.end());             // sort, since directory iteration
                                                 // is not ordered on some file systems
//      std::cout << " basename: " << basename << "\n";
//      std::cout << " v.end(): " << v.back().string() << "\n";
      // iterate reverse over all files in directory
      std::string clust_filename;
      std::size_t found = std::string::npos;
      for (vec::reverse_iterator it(v.rbegin()), it_end(v.rend()); it != it_end; ++it)
      {
        std::ostringstream oss;
        oss << *it;
        std::string file_str = oss.str();

        found = file_str.rfind(basename);
        if (found!=std::string::npos) {
          // if commentsMap is provided, check if found fe > fe_max
          if (commentsMap.count("screening_to") && commentsMap.count("screening_step")) {
            float fe_max;
            try{
              fe_max = std::stof(file_str.substr(found+basename.length(),file_str.length()-found-basename.length()-1));
              if ((fe_max > commentsMap["screening_to"] + commentsMap["screening_step"]) ||
                 (fe_max < commentsMap["screening_to"])){
                continue;
              }
            } catch (...) {
                continue;
            }
          }
          clust_filename = file_str.substr(found,file_str.length()-found-1);
          break;
        }
      }
      // catch if file not found
      if (found == std::string::npos) {
        std::cerr << "\n" << "error (noise): cluster file of type " << basename << " not found\n\n";
        exit(EXIT_FAILURE);
      }

      // open highest clust file
      header_comment.append(Clustering::Tools::stringprintf("#\n# Execution remarks:\n"
                            "# used for highest cluster file: %s\n", clust_filename.c_str()));
      Clustering::logger(std::cout) << "    highest cluster: " << clust_filename  << std::endl;
      std::vector<std::size_t> clust = Clustering::Tools::read_clustered_trajectory(clust_filename);
      // check if parameters are identical to states ones
      read_comments(clust_filename, commentsMap);
      if (n_frames != clust.size()) {
        std::cerr << "\n" << "error (noise): clust file is not of same length as state trajectory." << "\n\n";
        exit(EXIT_FAILURE);
      }
      // generate counts of each cluster id
      CounterClustMap counts;
      for (std::size_t i=0; i < n_frames; ++i) {
        CounterClustMap::iterator it(counts.find(clust[i]));
        if (it != counts.end()){
          it->second++;
        } else {
          counts[clust[i]] = 1;
        }
      }
      Clustering::logger(std::cout) << "~~~ noise assignment" << std::endl;
      std::size_t noise_frames = 0;
      // define noise frames as state 0
      for (std::size_t i=0; i < n_frames; ++i) {
        if (counts[clust[i]] < cmin*n_frames){
          states[i] = noiseState;
          ++noise_frames;
        }
      }
      float noise_frames_per = 100.*noise_frames / n_frames;
      Clustering::logger(std::cout) << Clustering::Tools::stringprintf("    %.2f", noise_frames_per)
                                    << "% of frames were identified as noise" << std::endl;
      header_comment.append(Clustering::Tools::stringprintf("# %.2f", noise_frames_per) +
                            "% of frames were identified as noise\n");
      // TODO: remove following line. Should we keep with argument?
      // Clustering::Tools::write_clustered_trajectory("microstatesNoiseDef", states);
      // noise core trajectory
      std::vector<std::size_t> noise_traj(n_frames);
      std::size_t current_core = states[0];
      std::vector<long> cores(n_frames);
      std::size_t changed_frames = 0;
      // honour concatenation limits, i.e. treat every concatenated trajectory-part on its own
      std::size_t last_limit = 0;
      for (std::size_t next_limit: concat_limits) {
        std::size_t next_limit_corrected = std::min(next_limit, n_frames);
        for (std::size_t i=last_limit; i < next_limit_corrected; ++i) {
          if (states[i] != noiseState) {
            current_core = states[i];
            break;
          }
        }
        for (std::size_t i=last_limit; i < next_limit_corrected; ++i) {
          // coring window
          if (states[i] != noiseState) {
            current_core = states[i];
            cores[i] = current_core;
          } else {
            cores[i] = -1;
          }
          if (current_core != states_without_noise[i]) {
            ++changed_frames;
          }
          noise_traj[i] = current_core;
        }
        last_limit = next_limit_corrected;
      }
      float changed_frames_per = 100.*changed_frames / n_frames;
      Clustering::logger(std::cout) << Clustering::Tools::stringprintf("    %.2f", changed_frames_per)
                                    << "% of frames were reassigned\n"
                                    << "    store result in: " << args["output"].as<std::string>()
                                    << std::endl;
      header_comment.append(Clustering::Tools::stringprintf("# %.2f", changed_frames_per) +
                            "% of frames were reassigned\n");
      // write cored trajectory to file
      if (args.count("output")) {
        Clustering::Tools::write_clustered_trajectory(args["output"].as<std::string>(),
                                                      noise_traj,
                                                      header_comment,
                                                      commentsMap);
      }
      // write core information to file
      if (args.count("cores")) {
        append_commentsMap(header_comment, commentsMap);
        Clustering::Tools::write_single_column<long>(args["cores"].as<std::string>(),
                                                     cores,
                                                     header_comment,
                                                     false);
      }
    } else {
      std::cerr << "\n" << "error (noise): nothing to do! please define '--output' or '--cores'" << "\n\n";
      exit(EXIT_FAILURE);
    }
  }
} // end namespace Noise
} // end namespace Clustering

max_stars_repo_name: gregorweiss/Clustering

max_issues_repo_name: gregorweiss/Clustering

max_forks_repo_name: gregorweiss/Clustering


text:
/*Deep Euler implementation of a sonochemical bubble model*/

//#define EIGEN_NO_DEBUG
#include <iostream>
#include <fstream>
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <string>
#include <chrono>

#include <torch/script.h>
#include <Eigen/Core>
#include <boost/numeric/odeint.hpp>

const double rho_L = 9.970639504998557e+02;
const double p_inf = 1.0e+5;
const double sigma = 0.071977583160056;
const double gamma = 1.33;
const double c_L = 1.497251785455527e+03; // water 25 Celsius
const double mu_L = 8.902125058209557e-04; //25 Celsius
const double lambda = 0.6084; //water 25 Celsius
const double T_inf = 298.15; // 25 Celsius

const double R_E = 10e-6; //1...10u
const double p_A = 0.5e5; //0.5..2 bar
const double f = 100e3; //20 kHz... 2 MHz

using namespace std;

const int N = 16;
const int N_z = N / 2 - 1;

const int nn_inputs = 2 + 3 + N_z;
const int nn_outputs = N_z;
const int system_order = 4+N_z;
c10::TensorOptions global_tensor_op;

//Modify these to load the correct model
string file_name = "../simulations/bub_hybrid_test.txt";
string model_file = "../../../training/traced_model_bub0.5_hybrid_e51_2112021549.pt";
string scaler_file = "../../../training/scaler_bub0.5_hybrid_2112021549.psca";

typedef double value_type;
typedef vector<value_type> state_type;
typedef Eigen::Matrix<value_type, N / 2, N / 2> matrix_type;

struct std_scaler {
	torch::Tensor mean;
	torch::Tensor scale;

	torch::Tensor operator()(torch::Tensor tensor) {
		return (tensor - mean) / scale;
	}
	torch::Tensor inverse_transform(torch::Tensor tensor) {
		return tensor * scale + mean;
	}
	void parse(istream& is, int numel) {
		mean = torch::ones({ 1, numel });
		is.get();
		double temp = 0.0;
		for (int i = 0; i < numel; i++) {
			is >> temp;
			mean[0][i] = temp;
		}
		is.get();
		is.get();
		scale = torch::ones({ 1, numel });
		is.get();
		for (int i = 0; i < numel; i++) {
			is >> temp;
			scale[0][i] = temp;
		}
		is.get();
		is.get();
	}
};

struct norm_scaler {
	torch::Tensor data_min;
	torch::Tensor data_max;
	double min = 0;
	double max = 0;

	torch::Tensor operator()(torch::Tensor tensor) {
		torch::Tensor X_std = (tensor - data_min) / (data_max - data_min);
		return X_std * (max - min) + min;
	}
	torch::Tensor inverse_transform(torch::Tensor tensor) {
		torch::Tensor Y_std = (tensor - min) / (max - min);
		return Y_std * (data_max - data_min) + data_min;
	}
	void parse(istream& is) {
		data_min = torch::ones({ 1,nn_outputs });
		is.get();
		double temp = 0.0;
		for (int i = 0; i < nn_outputs; i++) {
			is >> temp;
			data_min[0][i] = temp;
		}
		is.get();
		is.get();
		data_max = torch::ones({ 1, nn_outputs });
		is.get();
		for (int i = 0; i < nn_outputs; i++) {
			is >> temp;
			data_max[0][i] = temp;
		}
		is.get(); //']'
		is.get(); //'\n'
		is >> min;
		is >> max;
	}
};

//ode function of Van der Pol equation
class BubDyn {
	double mu = 1.5;
	std::vector<torch::jit::IValue> inps; //reused neural network input vector
	torch::Tensor inputs; //reused tensor of inputs
public:
	torch::jit::script::Module model; //the neural network
	std_scaler in_transf;
	std_scaler out_transf;

	std::array<double, 5> stage_times;
	std::vector<value_type> C; //constants of the right hand side
	Eigen::Matrix<value_type, N / 2, 1> y; //collocation points (half)
	Eigen::Matrix<value_type, N / 2, 1> y_sq; //same, every entry squared
	matrix_type D_E; //Derivative matrix for even functions
	matrix_type D_O; //Derivative matrix for odd functions

	BubDyn() {
		inps = std::vector<torch::jit::IValue>(1);
		inputs = torch::ones({ 5, nn_inputs }, global_tensor_op);

		//----------------------------------------------------------
		//bubblemodel initializations
		//----------------------------------------------------------
		const double omega = 2 * M_PI * f;
		value_type pi2wRE = 2 * M_PI / (omega * R_E);

		//constants
		C = std::vector<value_type>(13);
		C[0] = omega * R_E / (2 * M_PI * c_L);
		C[1] = 4 * mu_L / (c_L * rho_L * R_E);
		C[2] = 4 * mu_L / (rho_L * R_E) * pi2wRE;
		C[3] = 2 * sigma * pi2wRE * pi2wRE / (rho_L * R_E);
		C[4] = p_inf / rho_L * pi2wRE * pi2wRE;
		C[5] = p_A / rho_L * pi2wRE * pi2wRE;
		C[6] = pi2wRE * p_inf / (c_L * rho_L);
		C[7] = pi2wRE * p_A / (c_L * rho_L);
		C[8] = 2 * M_PI * pi2wRE * p_A / (c_L * rho_L);
		C[9] = lambda * (gamma - 1) / gamma * pi2wRE / R_E * T_inf / p_inf;
		C[10] = lambda * (gamma - 1) * pi2wRE / R_E * T_inf / p_inf;
		C[11] = (gamma - 1) / gamma;
		C[12] = 1.0 / (3 * gamma);

		//Derivative matrices
		Eigen::Matrix<value_type, N, 1> y_full(N);
		value_type rec_cpn = 1.0 / (N - 1);
		for (int i = 0; i < N; i++) {
			y_full[i] = cos(M_PI * i * rec_cpn);
			//std::cout << y_full[i] << std::endl;
		}
		Eigen::Matrix<value_type, N, N> D(N, N);
		for (int i = 0; i < N; i++) {
			for (int j = 0; j < N; j++) {
				if (i == j) {
					if (i == N - 1) {
						D(N - 1, N - 1) = -(1 + 2 * (N - 1) * (N - 1)) / 6.0;
					}
					else if (i == 0) {
						D(0, 0) = (1 + 2 * (N - 1) * (N - 1)) / 6.0;
					}
					else {
						D(i, i) = -y_full[i] / (2.0 * (1.0 - y_full[i] * y_full[i]));
					}
				}
				else {
					D(i, j) = std::pow(-1, i + j) * (i == 0 || i == N - 1 ? 2.0 : 1.0)
						/ ((j == 0 || j == N - 1 ? 2.0 : 1.0) * (y_full[i] - y_full[j]));
				}
			}
		}
		D_E = matrix_type(N / 2, N / 2);
		D_O = matrix_type(N / 2, N / 2);
		for (int i = 0; i < N / 2; i++) {
			for (int j = 0; j < N / 2; j++) {
				D_E(i, j) = D(i, j) + D(i, N - 1 - j);
			}
		}
		for (int i = 0; i < N / 2; i++) {
			for (int j = 0; j < N / 2; j++) {
				D_O(i, j) = D(i, j) - D(i, N - 1 - j);
			}
		}
		y = y_full.head<N / 2>();
		y_sq = y.cwiseProduct(y);

		//---------------------------------------------------------
		//neural network initializations
		//---------------------------------------------------------
		torch::Tensor inputs = torch::ones({ 1, nn_inputs }, global_tensor_op);

		try {
			model = torch::jit::load(model_file);
			std::vector<torch::jit::IValue> inp;
			inp.push_back(torch::ones({ 1, nn_inputs }, global_tensor_op));
			std::cout << inp << endl;
			// Execute the model and turn its output into a tensor.
			at::Tensor output = model.forward(inp).toTensor().detach();
			std::cout << output << endl;
		}
		catch (const c10::Error& e) {
			std::cerr << "Error loading the model: " << e.what() << endl;
			exit(-1);
		}
		ifstream in(scaler_file);
		if (!in) {
			std::cerr << "Error loading the scalers." << endl;
			exit(-1);
		}
		out_transf.parse(in, nn_outputs);
		in_transf.parse(in, nn_inputs);
		in.close();
	}

	//Rewrites the errors array with the predicted local truncation errors
	torch::Tensor local_error(double t, double dt, const double * x, const double* z) {
		double sinpi = sin(2 * M_PI * t);

		for (int jj = 0; jj < 5; jj++) {
			inputs[jj][1] = x[0];
			inputs[jj][2] = x[1];
			inputs[jj][3] = x[2];
			inputs[jj][nn_inputs - 1] = sinpi;
			//timestep
			inputs[jj][0] = stage_times[jj] * dt;
			//temperature
			for (int i = 0; i < N_z; i++) {
				inputs[jj][i + 4] = z[i+1];
			}
			//scaling
			inputs.index_put_({jj, torch::indexing::Slice()}, in_transf(inputs.index({ jj, torch::indexing::Slice() })));
		}
		inps[0] = inputs;
		//evaluating
		torch::Tensor loc_trun_err = model.forward(inps).toTensor().detach();
		for (int i = 0; i < 5; i++) {
			loc_trun_err.index_put_({ i, torch::indexing::Slice() }, out_transf.inverse_transform(loc_trun_err.index({ i, torch::indexing::Slice() })));
		}
		return loc_trun_err;
	}
	//ODE function of discretized temperature (z)
	void temperature(double t, const double * x, const double* z, double* dzdt) {
		Eigen::Map<const Eigen::Matrix<value_type, N / 2, 1>> z_vector(z);
		Eigen::Map<Eigen::Matrix<value_type, N / 2, 1>> dzdt_vector(dzdt);

		Eigen::Matrix<value_type, N / 2, 1> De_x = D_E * z_vector; //derivative of z (dimless temperature)

		value_type rec_xR = 1.0 / x[0];
		value_type rec_xp = 1.0 / x[2];
		value_type dxdt2 = 3 * rec_xR * (C[10] * rec_xR * De_x[0] - gamma * x[1] * x[2]);

		//discretized PDE of bubble temperature
		dzdt_vector = De_x.cwiseProduct(x[1] * rec_xR * y - C[9] * rec_xR * rec_xR * rec_xp * De_x //this might show error, but it will not fail at compile time, valid syntax
			+ C[12] * rec_xp * dxdt2 * y)
			+ C[11] * rec_xp * dxdt2 * z_vector + C[9] * rec_xp * rec_xR * rec_xR * z_vector
			.cwiseProduct(y_sq.cwiseInverse()).cwiseProduct(D_O * (y_sq.cwiseProduct(De_x)));
		dzdt_vector[0] = 0.0; //Boundary condition

		return;
	}

	//ODE function of bubbledynamics (x). In the pointer x the values are rewritten with the computed slopes
	void operator()(double t, const double* x, const double * z, double* dxdt) {
		Eigen::Map<const Eigen::Matrix<value_type, N / 2, 1>> z_vector(z);
		value_type rec_xR = 1.0 / x[0];
		value_type rec_xp = 1.0 / x[2];

		//bubble pressure evolution
		dxdt[2] = 3 * rec_xR * (C[10] * rec_xR * (D_E * z_vector)[0] - gamma * x[1] * x[2]);

		//Keller-Miksis equation
		dxdt[0] = x[1];
		value_type sin2pit = sin(2 * M_PI * t);
		value_type den = x[0] - C[0] * x[0] * x[1] + C[1];
		value_type num = 0.5 * C[0] * x[1] * x[1] * x[1] - 1.5 * x[1] * x[1] - C[2] * x[1] * rec_xR - C[3] * rec_xR
			+ C[4] * x[2] - C[4] - C[5] * sin2pit + C[6] * x[1] * x[2] - C[6] * x[1] - C[7] * x[1] * sin2pit
			- C[8] * x[0] * cos(2 * M_PI * t) + C[6] * x[0] * dxdt[2];
		dxdt[1] = num / den;
	}
};

class BubbleSolver
{
public:

	BubbleSolver(int temperature_size) :z_size(temperature_size) {};

	bool setInitialConditions(const double* conds_x, const double * conds_z, const double at) {
		begin_t = at;
		x_init = (double*)malloc(sizeof(double) * x_size);
		for (int u = 0; u < x_size; u++) {
			x_init[u] = conds_x[u];
		}
		z_init = (double*)malloc(sizeof(double) * z_size);
		for (int u = 0; u < z_size; u++) {
			z_init[u] = conds_z[u];
		}
		return true;
	}
	void setMaxTime(double max_t) {
		t_max = max_t;
	}
	void setTolerances(double rel, double abs) {
		abs_tol = abs;
		rel_tol = rel;
	}

	void solve(BubDyn& sys, ostream& os) {

		double* x = (double*)malloc(sizeof(double) * x_size);
		for (int u = 0; u < x_size; u++) {
			x[u] = x_init[u];
		}
		double* z = (double*)malloc(sizeof(double) * z_size);
		for (int u = 0; u < z_size; u++) {
			z[u] = z_init[u];
		}

		//preparations
		double* z_stage = (double*)malloc(sizeof(double) * z_size);
		double* z_next = (double*)malloc(sizeof(double) * z_size);
		double* dzdt = (double*)malloc(sizeof(double) * z_size);
		double* x_stage = (double*)malloc(sizeof(double) * x_size);
		double* x_tmp = (double*)malloc(sizeof(double) * x_size);
		double* k1 = (double*)malloc(sizeof(double) * x_size);
		double* k2 = (double*)malloc(sizeof(double) * x_size);
		double* k3 = (double*)malloc(sizeof(double) * x_size);
		double* k4 = (double*)malloc(sizeof(double) * x_size);
		double* k5 = (double*)malloc(sizeof(double) * x_size);
		double t = begin_t;
		int l = 0;
		torch::Tensor errors;
		bool accept = true;
		bool nan_detect = false;
		value_type rel_err = 0.0;
		value_type coeff = 1.0;
		sys.stage_times = { 0.2, 0.3, 0.6, 1.0, 7.0 / 8.0 };
		z_stage[0] = 1.0; //fixed, boundary condition
		z_next[0] = 1.0; //same

		os << t;
		for (int i = 0; i < x_size; i++) {
			os << " " << x[i];
		}
		for (int i = 0; i < z_size; i++) {
			os << " " << z[i];
		}
		os << endl;

		if (t_max - t < delta_t) delta_t = t_max - t;

		while (t < t_max) {

			//DEM for temperature----------------------------------------------
			errors = sys.local_error(t, delta_t, x, z); //neural network
			sys.temperature(t, x, z, dzdt);
			//DOPRI for bubbleradius-------------------------------------------
			sys(t, x, z, k1);

			//k2
			for (int j = 1; j < z_size; j++) {
				z_stage[j] = z[j] + 0.2 * delta_t * dzdt[j] +0.04 * delta_t * delta_t * errors[0][j - 1].item<double>();
			}
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + 0.2 * delta_t * k1[j];
			}
			sys(t + 0.2 * delta_t, x_stage, z_stage,  k2);

			//k3
			for (int j = 1; j < z_size; j++) {
				z_stage[j] = z[j] + 0.3 * delta_t * dzdt[j] + 0.09 * delta_t * delta_t * errors[1][j-1].item<double>();
			}
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + 3.0 / 40.0 * delta_t * k1[j] + 9.0 / 40.0 * delta_t * k2[j];
			}
			sys(t + 0.3 * delta_t, x_stage, z_stage, k3); //k3

			//k4
			for (int j = 1; j < z_size; j++) {
				z_stage[j] = z[j] + 0.6 * delta_t * dzdt[j] + 0.36 * delta_t * delta_t * errors[2][j-1].item<double>();
			}
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + delta_t * (0.3 * k1[j] - 0.9 * k2[j] + 6.0 / 5.0 * k3[j]);
			}
			sys(t + 0.6 * delta_t, x_stage, z_stage, k4);

			//k5
			for (int j = 1; j < z_size; j++) {
				z_next[j] = z[j] + delta_t * dzdt[j] + delta_t * delta_t * errors[3][j-1].item<double>();
				//this is the DEM solution
			}
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + delta_t * (-11.0 / 54.0 * k1[j] + 5.0 / 2.0 * k2[j] - 70.0 / 27.0 * k3[j] + 35.0 / 27.0 * k4[j]);
			}
			sys(t + delta_t, x_stage, z_next, k5);

			//k6
			for (int j = 1; j < z_size; j++) {
				z_stage[j] = z[j] + 7.0/8.0 * delta_t * dzdt[j] + 49.0/64.0 * delta_t * delta_t * errors[4][j-1].item<double>();
			}
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + delta_t * (1631.0 / 55296.0 * k1[j] + 175.0 / 512.0 * k2[j] + 575.0 / 13824.0 * k3[j] + 44275.0 / 110592.0 * k4[j] + 253.0 / 4096.0 * k5[j]);
			}
			sys(t + 7.0 / 8.0 * delta_t, x_stage, z_stage, k2); //k6

			//solution--------------------------------------------
			for (int j = 0; j < x_size; j++) {
				//Main solution:
				x_tmp[j] = x[j] + delta_t * (37.0 / 378.0 * k1[j] + 250.0 / 621.0 * k3[j] + 125.0 / 594.0 * k4[j] + 512.0 / 1771.0 * k2[j]); //k2=k6
				//main solution end
			}
			//secondary solution
			for (int j = 0; j < x_size; j++) {
				x_stage[j] = x[j] + delta_t * (2825.0 / 27648.0 * k1[j] + 18575.0 / 48384.0 * k3[j] + 13525.0 / 55296.0 * k4[j] + 277.0 / 14336.0 * k5[j] + 0.25 * k2[j]); //k2=k6
			}

			//error control---------------------------------------
			accept = true;
			nan_detect = false;
			rel_err = 0;
			for (int j = 0; j < x_size; j++) {
				if (!std::isfinite(x_tmp[j]) || !isfinite(x_stage[j])) {
					accept = false;
					std::cout << "NaN detected!" << std::endl;
					nan_detect = true;
					break;
				}
				k2[j] = distance(x_tmp[j], x_stage[j]); // local error
				k4[j] = abs_tol + std::fmax(std::fabs(x_tmp[j]), std::fabs(x[j])) * rel_tol; //tolerance
				if (k2[j] > k4[j]) {
					accept = false;
				}
				rel_err = std::fmax(rel_err, k2[j] / k4[j]);
			}
			/*if (std::isfinite(rel_err))
				for (int j = 1; j < z_size; j++) {
					if ( !std::isfinite(z_next[j]) ) {
						accept = false;
						std::cout << "NaN detected!" << std::endl;
						nan_detect = true;
						break;
					}
				}*/

			if (!accept) {
				//1/(q+1) = 1/5 = 0.2;
				coeff = safety_factor * std::pow(1.0 / rel_err, 0.2);
				if (!std::isfinite(coeff) || nan_detect) coeff = 0.1;
				delta_t = coeff * delta_t;
				//std::cout << "Not good\n";
				continue;
				//redo this step
			}

			//save------------------------------------------------
			t += delta_t;
			l++;
			coeff = safety_factor * std::pow(1.0 / rel_err, 0.2);
			if (!std::isfinite(coeff)) coeff = 0.1;
			else if (coeff < 0.1) coeff = 0.1;
			else if (coeff > 5.0) coeff = 5.0;

			if (t + coeff * delta_t > t_max)delta_t = t_max - t;
			else delta_t = coeff * delta_t;

			os << t;
			for (int i = 0; i < x_size; i++) {
				x[i] = x_tmp[i];
				os << " " << x[i];
			}
			for (int i = 0; i < z_size; i++) {
				z[i] = z_next[i];
				os << " " << z[i];
			}
			os << endl;
		}
		free(x); free(z);
		free(x_stage); free(x_tmp);
		free(dzdt);
		free(z_stage); free(z_next);
		free(k1); free(k2); free(k3); free(k4); free(k5);
	}

	~BubbleSolver() {
		free(x_init);
		free(z_init);
	}
private:
	const int x_size = 3;
	int z_size = 8;
	double* x_init = 0;
	double* z_init = 0;
	double begin_t = 0;
	double delta_t = 1e-4;
	double abs_tol = 1e-6;
	double rel_tol = 1e-6;
	const double safety_factor = 0.8;
	int t_max = 10;
	double distance(double a, double b) {
		if (a < b)return b - a;
		else return a - b;
	}
};

int main() {
	global_tensor_op = torch::TensorOptions().dtype(torch::kFloat64);
	std::cout << "BubbleDynamics with DEM started\n" << setprecision(17) << endl;

	ofstream ofs(file_name);
	if (!ofs.is_open()) {
		std::cout << "File could not be opened: " << file_name << endl;
		exit(-1);
	}
	ofs.precision(17);
	ofs.flags(ios::scientific);
	std::cout << "Writing file: " << file_name << endl;

	//initial conditions
	double* x = new double[3] {1.0, 0.0, 1.0 + 2.0 * sigma / (R_E * p_inf)};
	double* z = new double[N/2] {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
	double t_start = 0.0;
	std::cout << "Rarrrr" << endl;
	BubDyn bubi;

	BubbleSolver solver(N/2);
	solver.setInitialConditions(x, z, 0.0);
	solver.setTolerances(1e-8, 1e-8);
	solver.setMaxTime(5.0);

	cout << "Solving..." << endl;
	auto t1 = chrono::high_resolution_clock::now();
	solver.solve(bubi, ofs);
	auto t2 = chrono::high_resolution_clock::now();
	//Not valid measurement of DEM computational time. Just a slight indicator
	cout << "Time (ms):" << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << endl;

	ofs.flush();
	ofs.close();

	cout << "Ready" << endl;
	return 0;
}

max_stars_repo_name: plaveczlambert/nonlinearbubbledynamics

max_issues_repo_name: plaveczlambert/nonlinearbubbledynamics

max_forks_repo_name: plaveczlambert/nonlinearbubbledynamics


text:
#include <doctest/doctest.h>

#define EIGEN_INITIALIZE_MATRICES_BY_ZERO

#include <Eigen/Core>
#include <ode/ode.hpp>

TEST_CASE("Coupled Test")
{
  using method_type               = ode::explicit_method<ode::forward_euler_tableau<float>>;
  using second_order_problem_type = ode::higher_order_initial_value_problem<float, Eigen::Vector3f, 2>;
  using coupled_problem_type      = second_order_problem_type::coupled_type;

  constexpr auto alpha = 1.0f;

  const auto second_order_problem = second_order_problem_type
  {
    0.0f,                                                                              /* t0 */
    std::array {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 0.0f)}, /* y0, y'0 */
    [&] (const float h, const Eigen::Vector3f& y, const Eigen::Vector3f& dydt)         /* y'' = f(t, y, y') */
    {
      return 2.0f * h * dydt + 2.0f * alpha * y; /* Hermite differential equation */
    }
  };

  auto iterator = ode::coupled_fixed_step_iterator<method_type, coupled_problem_type>(second_order_problem.to_coupled_first_order(), 1.0f /* h */);
  for (auto i = 0; i < 1000; ++i)
    ++iterator;
}

max_stars_repo_name: acdemiralp/ode

max_issues_repo_name: acdemiralp/ode

max_forks_repo_name: acdemiralp/ode


text:
#pragma once

#include <crab/common/bignums.hpp>
#include <crab/domains/intervals.hpp>
#include <crab/common/debug.hpp>
#include <crab/cfg/cfg.hpp>
#include <crab/iterators/wto.hpp>

#include <boost/unordered_map.hpp>
#include <boost/range/iterator_range.hpp>

#include <algorithm>
#include <climits>

namespace crab {

   namespace iterators {

     /**
	 Class that represents a thresholds used by the widening operator
     **/
     template<typename Number>
     class thresholds {

      private:

       /// XXX: internal representation of a threshold
       typedef ikos::bound <Number> bound_t;

       std::vector<bound_t> m_thresholds;
       unsigned int m_size;

       template<class B1, class B2>
       static B2 convert_bounds_impl (B1 b1) {
	 B2 b2 (0); // some initial value it doesn't matter which one
	 ikos::bounds_impl::convert_bounds (b1, b2);
	 return b2;
       }

      public:

       thresholds (int size = UINT_MAX) : m_size (size) {
         m_thresholds.push_back (bound_t::minus_infinity ());
         m_thresholds.push_back (0);
	 // useful thresholds for wrapped domains
	 #if 1
	 m_thresholds.push_back (bound_t("-2147483648"));
	 m_thresholds.push_back (bound_t("-32768"));
	 m_thresholds.push_back (bound_t("-128"));
	 m_thresholds.push_back (bound_t("127"));
	 m_thresholds.push_back (bound_t("255"));
	 m_thresholds.push_back (bound_t("32767"));
	 m_thresholds.push_back (bound_t("65535"));
	 m_thresholds.push_back (bound_t("2147483647"));
	 #endif
         m_thresholds.push_back (bound_t::plus_infinity ());
       }

       unsigned size (void) const { return m_thresholds.size(); }

       template<typename N>
       void add (ikos::bound<N> v1) {
         if (m_thresholds.size () < m_size) {
       	   bound_t v = convert_bounds_impl<ikos::bound<N>, bound_t> (v1);
           if (std::find
	       (m_thresholds.begin (), m_thresholds.end (), v) == m_thresholds.end ()) {
             auto ub = std::upper_bound (m_thresholds.begin (), m_thresholds.end (), v);
             m_thresholds.insert (ub, v);
           }
         }
       }

       template<typename N>
       ikos::bound<N> get_next (ikos::bound<N> v1) const {
	 if (v1.is_plus_infinity()) return v1;
	 bound_t v = convert_bounds_impl<ikos::bound<N>, bound_t> (v1);
	 bound_t t = m_thresholds [m_thresholds.size () - 1];
         auto ub = std::upper_bound (m_thresholds.begin (), m_thresholds.end (), v);
         if (ub != m_thresholds.end ())
	   t = *ub;
	 return convert_bounds_impl<bound_t, ikos::bound<N> > (t);
       }

       template<typename N>
       ikos::bound<N> get_prev (ikos::bound<N> v1) const {
	 if (v1.is_minus_infinity()) return v1;
	 bound_t v = convert_bounds_impl<ikos::bound<N>, bound_t> (v1);
         auto lb = std::lower_bound (m_thresholds.begin (), m_thresholds.end (), v);
         if (lb != m_thresholds.end ()) {
           --lb;
           if (lb != m_thresholds.end ()) {
	     return convert_bounds_impl<bound_t, ikos::bound<N> > (*lb);
	   }
         }
	 return convert_bounds_impl<bound_t, ikos::bound<N> > (m_thresholds [0]);
       }

       void write (crab_os &o) const {
         o << "{";
         for (typename std::vector<bound_t>::const_iterator it = m_thresholds.begin (),
                  et= m_thresholds.end (); it != et; ) {
           bound_t b (*it);
           b.write (o);
           ++it;
           if (it != m_thresholds.end ())
             o << ",";
         }
         o << "}";
       }

     };

     template<typename Number>
     crab_os& operator<< (crab_os& o, const thresholds<Number>& t) {
       t.write (o);
       return o;
     }

     // explicit instantiation
     typedef thresholds<ikos::q_number> thresholds_t;

     /**
	Collect thresholds per wto cycle  (i.e. loop)
     **/
     template< typename NodeName, typename CFG>
     class wto_thresholds: public ikos::wto_component_visitor< NodeName, CFG > {

     public:

       typedef ikos::wto_vertex< NodeName, CFG > wto_vertex_t;
       typedef ikos::wto_cycle< NodeName, CFG > wto_cycle_t;
       typedef crab::iterators::thresholds_t thresholds_t;
       typedef boost::unordered_map<NodeName, thresholds_t> thresholds_map_t;

     private:

       // the cfg
       CFG m_cfg;
       // maximum number of thresholds
       size_t m_max_size;
       // keep a set of thresholds per wto head
       thresholds_map_t m_head_to_thresholds;
       // the top of the stack is the current wto head
       std::vector<NodeName> m_stack;

       typedef typename CFG::basic_block_t basic_block_t;
       typedef typename CFG::number_t number_t;
       typedef typename CFG::varname_t varname_t;
       typedef ikos::bound<number_t> bound_t;

       typedef crab::cfg::assume_stmt<number_t,varname_t> assume_t;
       typedef crab::cfg::select_stmt<number_t,varname_t> select_t;
       typedef crab::cfg::assignment<number_t,varname_t>  assign_t;

       void get_thresholds(const basic_block_t &bb, thresholds_t &thresholds) {
	 for (auto const&i : boost::make_iterator_range (bb.begin (), bb.end ())) {
	   bound_t t = bound_t::plus_infinity ();
	   if (i.is_assume ()) {
	     auto a = static_cast<const assume_t*> (&i);
	     t = bound_t(-(a->constraint ().expression ().constant ()));
	   }
	   else if (i.is_select ()) {
	     auto s = static_cast<const select_t*> (&i);
	     t = bound_t(-(s->cond ().expression ().constant ()));
	   }
	   else if (i.is_assign ()) {
	     auto a = static_cast<const assign_t*> (&i);
	     if (a->rhs ().is_constant ())
	       t = bound_t(-(a->rhs().constant ()));
	   }

	   if (t != bound_t::plus_infinity ()) {
	     // XXX: for code pattern like this "while(x<t) {x+=k;}"
	     // note that the condition (x<t) is translated to
	     // (x<=t-1) so an useful threshold would be t+1+k.
	     // Since we don't keep track of how x is incremented or
	     // decremented we choose arbitrarily k=1.
	     thresholds.add(t+2);
	   }
	 }
       }

     public:

       wto_thresholds(CFG cfg, size_t max_size)
	 : m_cfg(cfg), m_max_size (max_size) { }

       void visit(wto_vertex_t& vertex) {
	 if (m_stack.empty()) return;

	 NodeName head = m_stack.back();
	 auto it = m_head_to_thresholds.find(head);
	 if (it != m_head_to_thresholds.end()) {
	   thresholds_t & thresholds = it->second;
	   typename CFG::basic_block_t & bb = m_cfg.get_node(vertex.node());
	   get_thresholds(bb, thresholds);
	 } else {
	   CRAB_ERROR("No head found while gathering thresholds");
	 }
       }

       void visit(wto_cycle_t& cycle) {
	 thresholds_t thresholds(m_max_size);
	 typename CFG::basic_block_t & bb = m_cfg.get_node(cycle.head());
	 get_thresholds(bb, thresholds);

	 #if 1
	 // XXX: if we want to consider constants from loop
	 // initializations
	 for (auto pre : boost::make_iterator_range(bb.prev_blocks())) {
	   if (pre != cycle.head()) {
	     typename CFG::basic_block_t & pred_bb = m_cfg.get_node(pre);
	     get_thresholds(pred_bb, thresholds);
	   }
	 }
	 #endif

	 m_head_to_thresholds.insert(std::make_pair(cycle.head(), thresholds));
	 m_stack.push_back(cycle.head());
	 for (typename wto_cycle_t::iterator it = cycle.begin(); it != cycle.end(); ++it) {
	   it->accept(this);
	 }
	 m_stack.pop_back();
       }

       const thresholds_map_t& get_thresholds_map() const {
	 return m_head_to_thresholds;
       }

       void write (crab::crab_os& o) const {
	 for (auto &kv: m_head_to_thresholds) {
	   o << crab::cfg_impl::get_label_str(kv.first) << "=" << kv.second << "\n";
	 }
       }

     }; // class wto_thresholds

     template<typename NodeName, typename CFG>
     crab::crab_os& operator<<(crab::crab_os& o, const wto_thresholds<NodeName, CFG>& t) {
       t.write (o);
       return o;
     }

   } // end namespace iterators
} // end namespace crab

ext: hpp

max_stars_repo_name: bishoksan/crab-latest

max_issues_repo_name: bishoksan/crab-latest

max_forks_repo_name: bishoksan/crab-latest


text:
#pragma once

#include <vector>

#include <Eigen/Core>

namespace ipc {

std::vector<size_t>
vertex_to_min_edge(size_t num_vertices, const Eigen::MatrixXi& E);

} // namespace ipc

max_stars_repo_name: ipc-sim/ipc-toolk

max_issues_repo_name: dbelgrod/ipc-toolkit

max_forks_repo_name: dbelgrod/ipc-toolkit


text:
/*
 * Copyright 2018-2019 Autoware Foundation. 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.
 */
#include <autoware_planning_msgs/Trajectory.h>
#include <geometry_msgs/TwistStamped.h>
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <tf2/utils.h>
#include <tf2_ros/transform_listener.h>
#include <boost/shared_ptr.hpp>
#include <iostream>
#include <mutex>
#include <string>

#include <dynamic_reconfigure/server.h>
#include <motion_velocity_optimizer/MotionVelocityOptimizerConfig.h>
#include <motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp>
#include <motion_velocity_optimizer/optimizer/optimizer_base.hpp>

#include <osqp_interface/osqp_interface.h>

class MotionVelocityOptimizer
{
public:
  MotionVelocityOptimizer();
  ~MotionVelocityOptimizer();

private:
  ros::NodeHandle nh_;
  ros::NodeHandle pnh_;
  ros::Publisher pub_trajectory_;           //!< @brief publisher for output trajectory
  ros::Publisher pub_over_stop_velocity_;   //!< @brief publisher for over stop velocity warning
  ros::Subscriber sub_current_velocity_;    //!< @brief subscriber for current velocity
  ros::Subscriber sub_current_trajectory_;  //!< @brief subscriber for reference trajectory
  ros::Subscriber sub_external_velocity_limit_;  //!< @brief subscriber for external velocity limit
  tf2_ros::Buffer tf_buffer_;                    //!< @brief tf butter
  tf2_ros::TransformListener tf_listener_;       //!< @brief tf listener
  ros::Timer timer_;
  std::mutex mutex_;

  geometry_msgs::PoseStamped::ConstPtr current_pose_ptr_;           // current vehicle pose
  geometry_msgs::TwistStamped::ConstPtr current_velocity_ptr_;      // current vehicle twist
  autoware_planning_msgs::Trajectory::ConstPtr base_traj_raw_ptr_;  // current base_waypoints
  std_msgs::Float32::ConstPtr external_velocity_limit_ptr_;  // current external_velocity_limit
  boost::shared_ptr<double> external_velocity_limit_filtered_;

  autoware_planning_msgs::Trajectory prev_output_;  // previously published trajectory

  enum class InitializeType {
    INIT = 0,
    LARGE_DEVIATION_REPLAN = 1,
    ENGAGING = 2,
    NORMAL = 3,
  };
  InitializeType initialize_type_;

  boost::shared_ptr<OptimizerBase> optimizer_;

  bool publish_debug_trajs_;  // publish planned trajectories

  double over_stop_velocity_warn_thr_;  // threshold to publish over velocity warn

  struct MotionVelocityOptimizerParam
  {
    double max_velocity;                 // max velocity [m/s]
    double max_accel;                    // max acceleration in planning [m/s2] > 0
    double min_decel;                    // min deceleration in planning [m/s2] < 0
    double max_lateral_accel;            // max lateral acceleration [m/ss] > 0
    double min_curve_velocity;           // min velocity at curve [m/s]
    double decel_distance_before_curve;  // distance before slow down for lateral acc at a curve
    double decel_distance_after_curve;   // distance after slow down for lateral acc at a curve
    double replan_vel_deviation;  // if speed error exceeds this [m/s], replan from current velocity
    double engage_velocity;       // use this speed when start moving [m/s]
    double engage_acceleration;   // use this acceleration when start moving [m/ss]
    double engage_exit_ratio;     // exit engage sequence when the speed exceeds ratio x engage_vel.
    double stopping_velocity;     // change target velocity to this value before v=0 point.
    double stopping_distance;     // distance for the stopping_velocity
    double extract_ahead_dist;    // forward waypoints distance from current position [m]
    double extract_behind_dist;   // backward waypoints distance from current position [m]
    double max_trajectory_length;             // max length of the objective trajectory for resample
    double min_trajectory_length;             // min length of the objective trajectory for resample
    double resample_time;                     // max time to calculate trajectory length
    double resample_dt;                       // dt to calculate trajectory length
    double min_trajectory_interval_distance;  // for resampling
    double stop_dist_to_prohibit_engage;      // prevent to move toward close stop point
    double delta_yaw_threshold;               // for closest index calculation
    std::string algorithm_type;               // Option : Linf, L2
  } planning_param_;

  /* topic callback */
  void callbackCurrentVelocity(const geometry_msgs::TwistStamped::ConstPtr msg);
  void callbackCurrentTrajectory(const autoware_planning_msgs::Trajectory::ConstPtr msg);
  void callbackExternalVelocityLimit(const std_msgs::Float32::ConstPtr msg);
  void timerCallback(const ros::TimerEvent & e);

  /* non-const methods */
  void run();

  void updateCurrentPose();

  autoware_planning_msgs::Trajectory calcTrajectoryVelocity(
    const autoware_planning_msgs::Trajectory & base_traj);
  void updateExternalVelocityLimit(const double dt);

  autoware_planning_msgs::Trajectory optimizeVelocity(
    const autoware_planning_msgs::Trajectory & input, const int input_closest,
    const autoware_planning_msgs::Trajectory & prev_output_traj, const int prev_output_closest);

  void calcInitialMotion(
    const double & base_speed, const autoware_planning_msgs::Trajectory & base_waypoints,
    const int base_closest, const autoware_planning_msgs::Trajectory & prev_replanned_traj,
    const int prev_replanned_traj_closest, double & initial_vel, double & initial_acc);

  /* const methods */
  bool resampleTrajectory(
    const autoware_planning_msgs::Trajectory & input,
    autoware_planning_msgs::Trajectory & output) const;

  bool lateralAccelerationFilter(
    const autoware_planning_msgs::Trajectory & input,
    autoware_planning_msgs::Trajectory & output) const;

  bool extractPathAroundIndex(
    const autoware_planning_msgs::Trajectory & input, const int index,
    autoware_planning_msgs::Trajectory & output) const;

  bool externalVelocityLimitFilter(
    const autoware_planning_msgs::Trajectory & input,
    autoware_planning_msgs::Trajectory & output) const;

  void preventMoveToCloseStopLine(
    const int closest, autoware_planning_msgs::Trajectory & trajectory) const;

  void publishTrajectory(const autoware_planning_msgs::Trajectory & traj) const;

  void publishStopDistance(
    const autoware_planning_msgs::Trajectory & trajectory, const int closest) const;

  void insertBehindVelocity(
    const int prev_out_closest, const autoware_planning_msgs::Trajectory & prev_output,
    const int output_closest, autoware_planning_msgs::Trajectory & output) const;

  void applyStoppingVelocity(autoware_planning_msgs::Trajectory * traj) const;

  void overwriteStopPoint(
    const autoware_planning_msgs::Trajectory & input,
    autoware_planning_msgs::Trajectory * output) const;

  std_msgs::Float32 createFloat32Msg(const double value)
  {
    std_msgs::Float32 msg;
    msg.data = value;
    return msg;
  }

  /* dynamic reconfigure */
  dynamic_reconfigure::Server<motion_velocity_optimizer::MotionVelocityOptimizerConfig>
    dyncon_server_;
  void dynamicRecofCallback(
    motion_velocity_optimizer::MotionVelocityOptimizerConfig & config, uint32_t level)
  {
    std::lock_guard<std::mutex> lock(mutex_);
    planning_param_.max_velocity = config.max_velocity;
    planning_param_.max_accel = config.max_accel;
    planning_param_.min_decel = config.min_decel;
    planning_param_.max_lateral_accel = config.max_lateral_accel;
    planning_param_.min_curve_velocity = config.min_curve_velocity;
    planning_param_.decel_distance_before_curve = config.decel_distance_before_curve;
    planning_param_.decel_distance_after_curve = config.decel_distance_after_curve;
    planning_param_.replan_vel_deviation = config.replan_vel_deviation;
    planning_param_.engage_velocity = config.engage_velocity;
    planning_param_.engage_acceleration = config.engage_acceleration;
    planning_param_.engage_exit_ratio = config.engage_exit_ratio;
    planning_param_.stopping_velocity = config.stopping_velocity;
    planning_param_.stopping_distance = config.stopping_distance;
    planning_param_.extract_ahead_dist = config.extract_ahead_dist;
    planning_param_.extract_behind_dist = config.extract_behind_dist;
    planning_param_.stop_dist_to_prohibit_engage = config.stop_dist_to_prohibit_engage;
    planning_param_.delta_yaw_threshold = config.delta_yaw_threshold;

    planning_param_.resample_time = config.resample_time;
    planning_param_.resample_dt = config.resample_dt;
    planning_param_.max_trajectory_length = config.max_trajectory_length;
    planning_param_.min_trajectory_length = config.min_trajectory_length;
    planning_param_.min_trajectory_interval_distance = config.min_trajectory_interval_distance;

    OptimizerParam p;
    p.max_accel = config.max_accel;
    p.min_decel = config.min_decel;
    p.pseudo_jerk_weight = config.pseudo_jerk_weight;
    p.over_v_weight = config.over_v_weight;
    p.over_a_weight = config.over_a_weight;
    optimizer_->setParam(p);
  }

  /* debug */
  ros::Publisher pub_dist_to_stopline_;  //!< @brief publisher for stop distance
  ros::Publisher pub_trajectory_raw_;
  ros::Publisher pub_velocity_limit_;
  ros::Publisher pub_trajectory_vel_lim_;
  ros::Publisher pub_trajectory_latcc_filtered_;
  ros::Publisher pub_trajectory_resampled_;
  ros::Publisher debug_closest_velocity_;
  ros::Publisher debug_closest_acc_;
  void publishFloat(const double & data, const ros::Publisher & pub) const;

  // jerk calc
  ros::Publisher debug_closest_jerk_;
  std::shared_ptr<ros::Time> prev_time_;
  double prev_acc_;
  void publishClosestJerk(const double curr_acc);
};


text:
// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Domain/Creators/ExpandOverBlocks.hpp"

#include <array>
#include <boost/algorithm/string/join.hpp>
#include <cstddef>
#include <exception>
#include <string>
#include <vector>

#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/MakeArray.hpp"

namespace domain {

template <typename T, size_t Dim>
ExpandOverBlocks<T, Dim>::ExpandOverBlocks(size_t num_blocks) noexcept
    : num_blocks_(num_blocks) {}

template <typename T, size_t Dim>
ExpandOverBlocks<T, Dim>::ExpandOverBlocks(
    std::vector<std::string> block_names,
    std::unordered_map<std::string, std::unordered_set<std::string>>
        block_groups) noexcept
    : num_blocks_(block_names.size()),
      block_names_(std::move(block_names)),
      block_groups_(std::move(block_groups)) {}

template <typename T, size_t Dim>
std::vector<std::array<T, Dim>> ExpandOverBlocks<T, Dim>::operator()(
    const T& value) const {
  return {num_blocks_, make_array<Dim>(value)};
}

template <typename T, size_t Dim>
std::vector<std::array<T, Dim>> ExpandOverBlocks<T, Dim>::operator()(
    const std::array<T, Dim>& value) const {
  return {num_blocks_, value};
}

template <typename T, size_t Dim>
std::vector<std::array<T, Dim>> ExpandOverBlocks<T, Dim>::operator()(
    const std::vector<std::array<T, Dim>> value) const {
  if (value.size() != num_blocks_) {
    throw std::length_error{"You supplied " + std::to_string(value.size()) +
                            " values, but the domain creator has " +
                            std::to_string(num_blocks_) + " blocks."};
  }
  return value;
}

template <typename T, size_t Dim>
std::vector<std::array<T, Dim>> ExpandOverBlocks<T, Dim>::operator()(
    const std::unordered_map<std::string, std::array<T, Dim>>& value) const {
  ASSERT(num_blocks_ == block_names_.size(),
         "Construct 'ExpandOverBlocks' with block names to use the "
         "map-over-block-names feature.");
  // Expand group names
  auto value_per_block = value;
  for (const auto& [name, block_value] : value) {
    const auto found_group = block_groups_.find(name);
    if (found_group != block_groups_.end()) {
      for (const auto& expanded_name : found_group->second) {
        if (value_per_block.count(expanded_name) == 0) {
          value_per_block[expanded_name] = block_value;
        } else {
          throw std::invalid_argument{
              "Duplicate block name '" + expanded_name +
              // NOLINTNEXTLINE(performance-inefficient-string-concatenation)
              "' (expanded from '" + name + "')."};
        }
      }
      value_per_block.erase(name);
    }
  }
  if (value_per_block.size() != num_blocks_) {
    throw std::length_error{
        "You supplied " + std::to_string(value_per_block.size()) +
        " values, but the domain creator has " + std::to_string(num_blocks_) +
        " blocks: " + boost::algorithm::join(block_names_, ", ")};
  }
  std::vector<std::array<T, Dim>> result{};
  for (const auto& block_name : block_names_) {
    const auto found_value = value_per_block.find(block_name);
    if (found_value != value_per_block.end()) {
      result.push_back(found_value->second);
    } else {
      throw std::out_of_range{"Value for block '" + block_name +
                              "' is missing. Did you misspell its name?"};
    }
  }
  return result;
}

#define DTYPE(data) BOOST_PP_TUPLE_ELEM(0, data)
#define DIM(data) BOOST_PP_TUPLE_ELEM(1, data)
#define INSTANTIATE(_, data) \
  template class ExpandOverBlocks<DTYPE(data), DIM(data)>;

GENERATE_INSTANTIATIONS(INSTANTIATE, (size_t), (1, 2, 3))

#undef DIM
#undef DTYPE
#undef INSTANTIATE

}  // namespace domain