<commit_before>/*
 * nghttp2 - HTTP/2 C Library
 *
 * Copyright (c) 2014 Tatsuhiro Tsujikawa
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
#include "shrpx_accept_handler.h"

#include <unistd.h>

#include <cerrno>

#include "shrpx_connection_handler.h"
#include "shrpx_config.h"
#include "util.h"

using namespace nghttp2;

namespace shrpx {

namespace {
void acceptcb(struct ev_loop *loop, ev_io *w, int revent) {
  auto h = static_cast<AcceptHandler *>(w->data);
  h->accept_connection();
}
} // namespace

AcceptHandler::AcceptHandler(int fd, ConnectionHandler *h)
    : conn_hnr_(h), fd_(fd) {
  ev_io_init(&wev_, acceptcb, fd_, EV_READ);
  wev_.data = this;
  ev_io_start(conn_hnr_->get_loop(), &wev_);
}

AcceptHandler::~AcceptHandler() {
  ev_io_stop(conn_hnr_->get_loop(), &wev_);
  close(fd_);
}

void AcceptHandler::accept_connection() {
  for (;;) {
    sockaddr_union sockaddr;
    socklen_t addrlen = sizeof(sockaddr);

#ifdef HAVE_ACCEPT4
    auto cfd =
        accept4(fd_, &sockaddr.sa, &addrlen, SOCK_NONBLOCK | SOCK_CLOEXEC);
#else  // !HAVE_ACCEPT4
    auto cfd = accept(fd_, &sockaddr.sa, &addrlen);
#endif // !HAVE_ACCEPT4

    if (cfd == -1) {
      switch (errno) {
      case EINTR:
      case ENETDOWN:
      case EPROTO:
      case ENOPROTOOPT:
      case EHOSTDOWN:
#ifdef ENONET
      case ENONET:
#endif // ENONET
      case EHOSTUNREACH:
      case EOPNOTSUPP:
      case ENETUNREACH:
        continue;
      }

      break;
    }

#ifndef HAVE_ACCEPT4
    util::make_socket_nonblocking(cfd);
    util::make_socket_closeonexec(cfd);
#endif // !HAVE_ACCEPT4

    util::make_socket_nodelay(cfd);

    conn_hnr_->handle_connection(cfd, &sockaddr.sa, addrlen);
  }
}

void AcceptHandler::enable() { ev_io_start(conn_hnr_->get_loop(), &wev_); }

void AcceptHandler::disable() { ev_io_stop(conn_hnr_->get_loop(), &wev_); }

int AcceptHandler::get_fd() const { return fd_; }

} // namespace shrpx
<commit_msg>nghttpx: Disable acceptor temporarily when process runs out of fd<commit_after>/*
 * nghttp2 - HTTP/2 C Library
 *
 * Copyright (c) 2014 Tatsuhiro Tsujikawa
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
#include "shrpx_accept_handler.h"

#include <unistd.h>

#include <cerrno>

#include "shrpx_connection_handler.h"
#include "shrpx_config.h"
#include "util.h"

using namespace nghttp2;

namespace shrpx {

namespace {
void acceptcb(struct ev_loop *loop, ev_io *w, int revent) {
  auto h = static_cast<AcceptHandler *>(w->data);
  h->accept_connection();
}
} // namespace

AcceptHandler::AcceptHandler(int fd, ConnectionHandler *h)
    : conn_hnr_(h), fd_(fd) {
  ev_io_init(&wev_, acceptcb, fd_, EV_READ);
  wev_.data = this;
  ev_io_start(conn_hnr_->get_loop(), &wev_);
}

AcceptHandler::~AcceptHandler() {
  ev_io_stop(conn_hnr_->get_loop(), &wev_);
  close(fd_);
}

void AcceptHandler::accept_connection() {
  for (;;) {
    sockaddr_union sockaddr;
    socklen_t addrlen = sizeof(sockaddr);

#ifdef HAVE_ACCEPT4
    auto cfd =
        accept4(fd_, &sockaddr.sa, &addrlen, SOCK_NONBLOCK | SOCK_CLOEXEC);
#else  // !HAVE_ACCEPT4
    auto cfd = accept(fd_, &sockaddr.sa, &addrlen);
#endif // !HAVE_ACCEPT4

    if (cfd == -1) {
      switch (errno) {
      case EINTR:
      case ENETDOWN:
      case EPROTO:
      case ENOPROTOOPT:
      case EHOSTDOWN:
#ifdef ENONET
      case ENONET:
#endif // ENONET
      case EHOSTUNREACH:
      case EOPNOTSUPP:
      case ENETUNREACH:
        continue;
      case EMFILE:
      case ENFILE:
        LOG(WARN) << "acceptor: running out file descriptor; disable acceptor "
                     "temporarily";
        conn_hnr_->disable_acceptor_temporary(30.);
        break;
      }

      break;
    }

#ifndef HAVE_ACCEPT4
    util::make_socket_nonblocking(cfd);
    util::make_socket_closeonexec(cfd);
#endif // !HAVE_ACCEPT4

    util::make_socket_nodelay(cfd);

    conn_hnr_->handle_connection(cfd, &sockaddr.sa, addrlen);
  }
}

void AcceptHandler::enable() { ev_io_start(conn_hnr_->get_loop(), &wev_); }

void AcceptHandler::disable() { ev_io_stop(conn_hnr_->get_loop(), &wev_); }

int AcceptHandler::get_fd() const { return fd_; }

} // namespace shrpx
<|endoftext|>


<commit_before>#include <smartref/smartref.h>

REFLECTABLE(member);

namespace tests {

using smartref::using_;

//! These tests check for existence of the member() member-function.
namespace test_existence {

template<typename T>
constexpr auto has_member(int) -> decltype(std::declval<T>().member(), bool{}) {return true;}
template<typename T>
constexpr auto has_member(...) {return false;}

struct EmptyClass {};

struct NonConstClass
{
  void member() {}
};

struct ConstClass
{
  void member() const {}
};

struct MixedClass
{
  void member() {}
  void member() const {}
};

////////////////////////////////
// non-const member functions //
////////////////////////////////

static_assert(!has_member<EmptyClass>(0),
              "TEST FAILED: EmptyClass seems to have a member-function!");

static_assert(!has_member<using_<EmptyClass>>(0),
              "TEST FAILED: using_<EmptyClass> seems to have a member-function!");

static_assert(has_member<NonConstClass>(0),
              "TEST FAILED: NonConstClass doesn't seem to have a member-function!");

static_assert(has_member<using_<NonConstClass>>(0),
              "TEST FAILED: using_<NonConstClass> doesn't seem to have a member-function!");

static_assert(has_member<ConstClass>(0),
              "TEST FAILED: ConstClass doesn't seem to have a member-function!");

// TODO: This should currently fail
static_assert(has_member<using_<ConstClass>>(0),
              "TEST FAILED: using_<ConstClass> doesn't seem to have a member-function!");

static_assert(has_member<MixedClass>(0),
              "TEST FAILED: MixedClass doesn't seem to have a member-function!");

static_assert(has_member<using_<MixedClass>>(0),
              "TEST FAILED: using_<MixedClass> doesn't seem to have a member-function!");

////////////////////////////
// const member functions //
////////////////////////////

static_assert(!has_member<const EmptyClass>(0),
              "TEST FAILED: const EmptyClass seems to have a const member-function!");

static_assert(!has_member<const using_<EmptyClass>>(0),
              "TEST FAILED: const using_<EmptyClass> seems to have a const member-function!");

static_assert(!has_member<const NonConstClass>(0),
              "TEST FAILED: const NonConstClass seems to have a const member-function!");

static_assert(!has_member<const using_<NonConstClass>>(0),
              "TEST FAILED: const using_<NonConstClass> seem to have a const member-function!");

} // namespace test_existence
} // namespace tests
<commit_msg>Reworded a TODO.<commit_after>#include <smartref/smartref.h>

REFLECTABLE(member);

namespace tests {

using smartref::using_;

//! These tests check for existence of the member() member-function.
namespace test_existence {

template<typename T>
constexpr auto has_member(int) -> decltype(std::declval<T>().member(), bool{}) {return true;}
template<typename T>
constexpr auto has_member(...) {return false;}

struct EmptyClass {};

struct NonConstClass
{
  void member() {}
};

struct ConstClass
{
  void member() const {}
};

struct MixedClass
{
  void member() {}
  void member() const {}
};

////////////////////////////////
// non-const member functions //
////////////////////////////////

static_assert(!has_member<EmptyClass>(0),
              "TEST FAILED: EmptyClass seems to have a member-function!");

static_assert(!has_member<using_<EmptyClass>>(0),
              "TEST FAILED: using_<EmptyClass> seems to have a member-function!");

static_assert(has_member<NonConstClass>(0),
              "TEST FAILED: NonConstClass doesn't seem to have a member-function!");

static_assert(has_member<using_<NonConstClass>>(0),
              "TEST FAILED: using_<NonConstClass> doesn't seem to have a member-function!");

static_assert(has_member<ConstClass>(0),
              "TEST FAILED: ConstClass doesn't seem to have a member-function!");

// TODO: This reifies a non-const member-function that delegates to the const member-function
//       It should reify a const member-function
static_assert(has_member<using_<ConstClass>>(0),
              "TEST FAILED: using_<ConstClass> doesn't seem to have a member-function!");

static_assert(has_member<MixedClass>(0),
              "TEST FAILED: MixedClass doesn't seem to have a member-function!");

static_assert(has_member<using_<MixedClass>>(0),
              "TEST FAILED: using_<MixedClass> doesn't seem to have a member-function!");

////////////////////////////
// const member functions //
////////////////////////////

static_assert(!has_member<const EmptyClass>(0),
              "TEST FAILED: const EmptyClass seems to have a const member-function!");

static_assert(!has_member<const using_<EmptyClass>>(0),
              "TEST FAILED: const using_<EmptyClass> seems to have a const member-function!");

static_assert(!has_member<const NonConstClass>(0),
              "TEST FAILED: const NonConstClass seems to have a const member-function!");

static_assert(!has_member<const using_<NonConstClass>>(0),
              "TEST FAILED: const using_<NonConstClass> seem to have a const member-function!");

} // namespace test_existence
} // namespace tests
<|endoftext|>


<commit_before>/*
This file is part of Bohrium and copyright (c) 2012 the Bohrium
team <http://www.bh107.org>.

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

Bohrium 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 Lesser General Public License along with Bohrium.

If not, see <http://www.gnu.org/licenses/>.
*/
#include <bh.h>
#include <bh_dag.h>
#include <bh_fuse_cache.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/foreach.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/algorithm/string/predicate.hpp> //For iequals()
#include <boost/graph/connected_components.hpp>
#include <boost/range/adaptors.hpp>
#include <vector>
#include <map>
#include <iterator>
#include <signal.h>
#include <stdio.h>
#include <sys/time.h>

#define VERBOSE

using namespace std;
using namespace boost;
using namespace bohrium;
using namespace bohrium::dag;

/* Help function that fuses the edges in 'edges2explore' where the 'mask' is true */
pair<int64_t,bool> fuse_mask(int64_t best_cost, const vector<EdgeW> &edges2explore,
                             const GraphDW &graph, const vector<bool> &mask, bh_ir &bhir,
                             GraphD &dag)
{
    bool fusibility=true;
    vector<EdgeW> edges2merge;
    unsigned int i=0;
    BOOST_FOREACH(const EdgeW &e, edges2explore)
    {
        if(mask[i++])
            edges2merge.push_back(e);
    }

    //Help function to find the new location
    struct find_new_location
    {
        Vertex operator()(const map<Vertex, Vertex> &loc_map, Vertex v)
        {
            Vertex v_mapped = loc_map.at(v);
            if(v_mapped == v)
                return v;
            else
                return (*this)(loc_map, v_mapped);
        }
    }find_loc;

    //'loc_map' maps a vertex before the merge to the corresponding vertex after the merge
    map<Vertex, Vertex> loc_map;
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        loc_map[v] = v;
    }

    //Lets record the merges into 'loc_map'
    BOOST_FOREACH(const EdgeW &e, edges2merge)
    {
        Vertex v1 = find_loc(loc_map, source(e, graph.bglW()));
        Vertex v2 = find_loc(loc_map, target(e, graph.bglW()));
        loc_map[v1] = v2;
    }

    //Pack 'loc_map' such that all keys maps directly to a new vertex thus after
    //this point there is no need to call find_loc().
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        Vertex v_mapped = find_loc(loc_map, loc_map.at(v));
        if(v_mapped != v)
            loc_map[v] = v_mapped;
    }

    //Create the new vertices and insert instruction topologically
    map<Vertex, bh_ir_kernel> new_vertices;
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        if(loc_map.at(v) == v)
            new_vertices[v] = bh_ir_kernel(bhir);
    }
    vector<Vertex> topological_order;
    topological_sort(dag, back_inserter(topological_order));
    BOOST_REVERSE_FOREACH(Vertex vertex, topological_order)
    {
        Vertex v = loc_map.at(vertex);
        bh_ir_kernel &k = new_vertices.at(v);
        BOOST_FOREACH(uint64_t idx, dag[vertex].instr_indexes)
        {
            if(not k.fusible(idx))
                fusibility = false;
            k.add_instr(idx);
        }
    }

    //TODO: Remove this assert check
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        if(loc_map.at(v) == v)
            assert(new_vertices[v].instr_indexes.size() > 0);
    }

    //Find the total cost
    int64_t cost=0;
    BOOST_FOREACH(const bh_ir_kernel &k, new_vertices | adaptors::map_values)
    {
        cost += k.cost();
    }

    //Check if we need to continue
    if(cost >= best_cost or not fusibility)
        return make_pair(cost,false);

    //Merge the vertice in the DAG
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        Vertex loc_v = loc_map.at(v);
        if(loc_v == v)
        {
            dag[v] = new_vertices.at(v);
            assert(dag[v].instr_indexes.size() > 0);
        }
        else//Lets merge 'v' into 'loc_v'
        {
            BOOST_FOREACH(Vertex a, adjacent_vertices(v, dag))
            {
                a = loc_map.at(a);
                if(a != loc_v)
                    add_edge(loc_v, a, dag);
            }
            BOOST_FOREACH(Vertex a, inv_adjacent_vertices(v, dag))
            {
                a = loc_map.at(a);
                if(a != loc_v)
                    add_edge(a, loc_v, dag);
            }
            clear_vertex(v, dag);
            dag[v] = bh_ir_kernel(bhir);
        }
    }

    //TODO: remove assert check
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        if(dag[loc_map.at(v)].instr_indexes.size() == 0)
        {
            cout << v << endl;
            cout << loc_map.at(v) << endl;
            assert(1 == 2);
            exit(-1);
        }
    }

    //Check for cycles
    if(cycles(dag))
    {
        return make_pair(cost,false);
    }

    assert(cost == (int64_t)dag_cost(dag));
    return make_pair(cost,true);
}

/* Private class to find the optimal solution through branch and bound */
class Solver
{
public:
    bh_ir &bhir;
    const GraphDW &dag;
    const vector<EdgeW> &edges2explore;
    int64_t best_cost;
    GraphD best_dag;
    FuseCache cache;

    #ifdef VERBOSE
        double  purge_count;
        uint64_t explore_count;
    #endif

    /* The constructor */
    Solver(bh_ir &b, const GraphDW &d, const vector<EdgeW> &e, FuseCache &cache, const set<Vertex> &ignores):
           bhir(b),dag(d),edges2explore(e), cache(cache)
    {
        //We use the greedy algorithm to find a good initial guess
        GraphDW new_dag(dag);
        fuse_greedy(new_dag, &ignores);

        best_dag = new_dag.bglD();
        best_cost = dag_cost(best_dag);

        #ifdef VERBOSE
            purge_count=0;
            explore_count=0;
        #endif
    }

    /* Find the optimal solution through branch and bound */
    void branch_n_bound(const vector<bool> &mask, unsigned int offset=0)
    {
        GraphD new_dag(dag.bglD());
        bool fusibility;
        int64_t cost;
        tie(cost, fusibility) = fuse_mask(best_cost, edges2explore, dag, mask, bhir, new_dag);

        #ifdef VERBOSE
            if(explore_count%10000 == 0)
            {
                cout << "[" << explore_count << "] " << "purge count: ";
                cout << purge_count << " / " << pow(2.0,mask.size()) << endl;
                cout << "cost: " << cost << ", best_cost: " << best_cost;
                cout << ", fusibility: " << fusibility << endl;
            }
            ++explore_count;
        #endif

        if(cost >= best_cost)
        {
            #ifdef VERBOSE
                purge_count += pow(2.0, mask.size()-offset);
            #endif
            return;
        }
        if(fusibility)
        {
            //Lets save the new best dag
            best_cost = cost;
            best_dag = new_dag;
            assert(dag_validate(best_dag));

            //Lets write the current best to file
            vector<bh_ir_kernel> kernel_list;
            fill_kernel_list(best_dag, kernel_list);
            const InstrIndexesList &i = cache.insert(bhir.instr_list, kernel_list);
            cache.write_to_files();

            #ifdef VERBOSE
                stringstream ss;
                string filename;
                i.get_filename(filename);
                ss << "new_best_dag-" << filename << ".dot";
                cout << "write file: " << ss.str() << endl;
                pprint(best_dag, ss.str().c_str());
                purge_count += pow(2.0, mask.size()-offset);
            #endif
            return;
        }
        for(unsigned int i=offset; i<mask.size(); ++i)
        {
            vector<bool> m1(mask);
            m1[i] = false;
            branch_n_bound(m1, i+1);
        }
    }
};

void get_edges2explore(const GraphDW &dag, const set<Vertex> &vertices2explore,
                       vector<EdgeW> &edges2explore)
{
    //The list of edges that we should try to merge
    BOOST_FOREACH(const EdgeW &e, edges(dag.bglW()))
    {
        if(vertices2explore.find(source(e, dag.bglW())) != vertices2explore.end() or
           vertices2explore.find(target(e, dag.bglW())) != vertices2explore.end())
            edges2explore.push_back(e);
    }
    sort_weights(dag.bglW(), edges2explore);
    string order;
    {
        const char *t = getenv("BH_FUSER_OPTIMAL_ORDER");
        if(t == NULL)
            order ="regular";
        else
            order = t;
    }
    if(not iequals(order, "regular"))
    {
        if(iequals(order, "reverse"))
        {
            reverse(edges2explore.begin(), edges2explore.end());
        }
        else if(iequals(order, "random"))
        {
            random_shuffle(edges2explore.begin(), edges2explore.end());
        }
        else
        {
            cerr << "FUSER-OPTIMAL: unknown BH_FUSER_OPTIMAL_ORDER: " << order << endl;
            order = "regular";
        }
    }
    cout << "BH_FUSER_OPTIMAL_ORDER: " << order << endl;
}

/* Fuse the 'dag' optimally */
void fuse_optimal(bh_ir &bhir, const GraphDW &dag, const set<Vertex> &vertices2explore,
                  GraphD &output, FuseCache &cache)
{
    //The list of edges that we should try to merge
    vector<EdgeW> edges2explore;
    get_edges2explore(dag, vertices2explore, edges2explore);
    if(edges2explore.size() == 0)
        return;

    //We need the set of vertices that the greedy fusion must ignore
    set<Vertex> ignores;
    BOOST_FOREACH(Vertex v, vertices(dag.bglD()))
    {
        if(vertices2explore.find(v) == vertices2explore.end())
            ignores.insert(v);
    }

    //Check for a preloaded initial condition
    vector<bool> mask(edges2explore.size(), true);
    unsigned int preload_offset=0;
    if(edges2explore.size() > 10)
    {
        const char *t = getenv("BH_FUSER_OPTIMAL_PRELOAD");
        if(t != NULL)
        {
            BOOST_FOREACH(const char &c, string(t))
            {
                mask[preload_offset++] = lexical_cast<bool>(c);
                if(preload_offset == mask.size())
                    break;
            }
            cout << "Preloaded path (" << preload_offset << "): ";
            for(unsigned int j=0; j<preload_offset; ++j)
                cout << mask[j] << ", ";
            cout << endl;
            --preload_offset;
        }
    }

    Solver solver(bhir, dag, edges2explore, cache, ignores);
    if(mask.size() > 100)
    {
        cout << "FUSER-OPTIMAL: ABORT the size of the search space is too large: 2^";
        cout << mask.size() << "!" << endl;
    }
    else
    {
        cout << "FUSER-OPTIMAL: the size of the search space is 2^" << mask.size() << "!" << endl;
        solver.branch_n_bound(mask, preload_offset);
    }
    output = solver.best_dag;
}

void do_fusion(bh_ir &bhir, FuseCache &cache)
{
    vector<bh_ir_kernel> kernel_list;
    {
        GraphDW dag;
        from_bhir(bhir, dag);
        fill_kernel_list(dag.bglD(), kernel_list);
    }
    while(true)
    {
        cout << endl << "Starting new round." << endl;
        GraphDW dag;
        from_kernels(kernel_list, dag);
        fuse_gentle(dag);
        dag.transitive_reduction();
        assert(dag_validate(dag.bglD()));

        vector<set<Vertex> > component2vertices;
        {
            vector<Vertex> vertex2component(num_vertices(dag.bglW()));
            uint64_t num = connected_components(dag.bglW(), &vertex2component[0]);
            component2vertices.resize(num);
            for(Vertex v=0; v<vertex2component.size(); ++v)
            {
                component2vertices[vertex2component[v]].insert(v);
            }
        }

        uint64_t component_id = 0;
        BOOST_FOREACH(set<Vertex> &vertices, component2vertices)
        {
            cout << "Component " << component_id << ": ";
            BOOST_FOREACH(Vertex v, vertices)
            {
                cout << v << ", ";
            }
            cout << endl;
            ++component_id;
        }

        //Find the smallest component with more than one vertex
        uint64_t comp_size=num_vertices(dag.bglW())+1;
        int comp_id=-1;
        for(uint64_t i=0; i < component2vertices.size(); ++i)
        {
            const uint64_t size = component2vertices[i].size();
           if(1 < size and size < comp_size)
           {
               comp_size = size;
               comp_id = i;
           }
        }
        if(comp_id == -1)
            break;//No more components to fuse

        GraphD output;
        cout << "Fusing component: " << comp_id << endl;
        fuse_optimal(bhir, dag, component2vertices[comp_id], output, cache);
        assert(num_vertices(output) > 0);
        assert(dag_validate(output));
        kernel_list.clear();
        fill_kernel_list(output, kernel_list);
    }
    bhir.kernel_list = kernel_list;
}

void fuser(bh_ir &bhir, FuseCache &cache)
{
    if(bhir.kernel_list.size() != 0)
        throw logic_error("The kernel_list is not empty!");

    BatchHash batch(bhir.instr_list);
    //if(not cache.lookup(batch, bhir, bhir.kernel_list))
    {
        do_fusion(bhir, cache);
        cache.insert(batch, bhir.kernel_list);
    }
}

<commit_msg>optimal-fuser: now using a task stack<commit_after>/*
This file is part of Bohrium and copyright (c) 2012 the Bohrium
team <http://www.bh107.org>.

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

Bohrium 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 Lesser General Public License along with Bohrium.

If not, see <http://www.gnu.org/licenses/>.
*/
#include <bh.h>
#include <bh_dag.h>
#include <bh_fuse_cache.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/foreach.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/algorithm/string/predicate.hpp> //For iequals()
#include <boost/graph/connected_components.hpp>
#include <boost/range/adaptors.hpp>
#include <vector>
#include <map>
#include <iterator>
#include <signal.h>
#include <stdio.h>
#include <sys/time.h>

#define VERBOSE

using namespace std;
using namespace boost;
using namespace bohrium;
using namespace bohrium::dag;

/* Help function that fuses the edges in 'edges2explore' where the 'mask' is true */
pair<int64_t,bool> fuse_mask(int64_t best_cost, const vector<EdgeW> &edges2explore,
                             const GraphDW &graph, const vector<bool> &mask, bh_ir &bhir,
                             GraphD &dag)
{
    bool fusibility=true;
    vector<EdgeW> edges2merge;
    unsigned int i=0;
    BOOST_FOREACH(const EdgeW &e, edges2explore)
    {
        if(mask[i++])
            edges2merge.push_back(e);
    }

    //Help function to find the new location
    struct find_new_location
    {
        Vertex operator()(const map<Vertex, Vertex> &loc_map, Vertex v)
        {
            Vertex v_mapped = loc_map.at(v);
            if(v_mapped == v)
                return v;
            else
                return (*this)(loc_map, v_mapped);
        }
    }find_loc;

    //'loc_map' maps a vertex before the merge to the corresponding vertex after the merge
    map<Vertex, Vertex> loc_map;
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        loc_map[v] = v;
    }

    //Lets record the merges into 'loc_map'
    BOOST_FOREACH(const EdgeW &e, edges2merge)
    {
        Vertex v1 = find_loc(loc_map, source(e, graph.bglW()));
        Vertex v2 = find_loc(loc_map, target(e, graph.bglW()));
        loc_map[v1] = v2;
    }

    //Pack 'loc_map' such that all keys maps directly to a new vertex thus after
    //this point there is no need to call find_loc().
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        Vertex v_mapped = find_loc(loc_map, loc_map.at(v));
        if(v_mapped != v)
            loc_map[v] = v_mapped;
    }

    //Create the new vertices and insert instruction topologically
    map<Vertex, bh_ir_kernel> new_vertices;
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        if(loc_map.at(v) == v)
            new_vertices[v] = bh_ir_kernel(bhir);
    }
    vector<Vertex> topological_order;
    topological_sort(dag, back_inserter(topological_order));
    BOOST_REVERSE_FOREACH(Vertex vertex, topological_order)
    {
        Vertex v = loc_map.at(vertex);
        bh_ir_kernel &k = new_vertices.at(v);
        BOOST_FOREACH(uint64_t idx, dag[vertex].instr_indexes)
        {
            if(not k.fusible(idx))
                fusibility = false;
            k.add_instr(idx);
        }
    }

    //TODO: Remove this assert check
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        if(loc_map.at(v) == v)
            assert(new_vertices[v].instr_indexes.size() > 0);
    }

    //Find the total cost
    int64_t cost=0;
    BOOST_FOREACH(const bh_ir_kernel &k, new_vertices | adaptors::map_values)
    {
        cost += k.cost();
    }

    //Check if we need to continue
    if(cost >= best_cost or not fusibility)
        return make_pair(cost,false);

    //Merge the vertice in the DAG
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        Vertex loc_v = loc_map.at(v);
        if(loc_v == v)
        {
            dag[v] = new_vertices.at(v);
            assert(dag[v].instr_indexes.size() > 0);
        }
        else//Lets merge 'v' into 'loc_v'
        {
            BOOST_FOREACH(Vertex a, adjacent_vertices(v, dag))
            {
                a = loc_map.at(a);
                if(a != loc_v)
                    add_edge(loc_v, a, dag);
            }
            BOOST_FOREACH(Vertex a, inv_adjacent_vertices(v, dag))
            {
                a = loc_map.at(a);
                if(a != loc_v)
                    add_edge(a, loc_v, dag);
            }
            clear_vertex(v, dag);
            dag[v] = bh_ir_kernel(bhir);
        }
    }

    //TODO: remove assert check
    BOOST_FOREACH(Vertex v, vertices(dag))
    {
        if(dag[loc_map.at(v)].instr_indexes.size() == 0)
        {
            cout << v << endl;
            cout << loc_map.at(v) << endl;
            assert(1 == 2);
            exit(-1);
        }
    }

    //Check for cycles
    if(cycles(dag))
    {
        return make_pair(cost,false);
    }

    assert(cost == (int64_t)dag_cost(dag));
    return make_pair(cost,true);
}

/* Private class to find the optimal solution through branch and bound */
class Solver
{
public:
    bh_ir &bhir;
    const GraphDW &dag;
    const vector<EdgeW> &edges2explore;
    int64_t best_cost;
    GraphD best_dag;
    FuseCache cache;

    #ifdef VERBOSE
        double  purge_count;
        uint64_t explore_count;
    #endif

    /* The constructor */
    Solver(bh_ir &b, const GraphDW &d, const vector<EdgeW> &e, FuseCache &cache, const set<Vertex> &ignores):
           bhir(b),dag(d),edges2explore(e), cache(cache)
    {
        //We use the greedy algorithm to find a good initial guess
        GraphDW new_dag(dag);
        fuse_greedy(new_dag, &ignores);

        best_dag = new_dag.bglD();
        best_cost = dag_cost(best_dag);

        #ifdef VERBOSE
            purge_count=0;
            explore_count=0;
        #endif
    }

    /* Find the optimal solution through branch and bound */
    void branch_n_bound(const vector<bool> &init_mask, unsigned int init_offset=0)
    {
        vector<pair<const vector<bool>, const unsigned int> > tasks;
        tasks.push_back(make_pair(init_mask, init_offset));
        while(tasks.size() > 0)
        {
            //Pop a task
            vector<bool> mask;
            unsigned int offset;
            tie(mask, offset) = tasks.back();
            tasks.pop_back();

            //Fuse the task
            GraphD new_dag(dag.bglD());
            bool fusibility;
            int64_t cost;
            tie(cost, fusibility) = fuse_mask(best_cost, edges2explore, dag, mask, bhir, new_dag);

            #ifdef VERBOSE
                if(explore_count%10000 == 0)
                {
                    cout << "[" << explore_count << "][";
                    BOOST_FOREACH(bool b, mask)
                    {
                        if(b){cout << "1";}else{cout << "0";}
                    }
                    cout << "] purge count: ";
                    cout << purge_count << " / " << pow(2.0,mask.size());
                    cout << ", cost: " << cost << ", best_cost: " << best_cost;
                    cout << ", fusibility: " << fusibility << endl;
                }
                ++explore_count;
            #endif

            if(cost >= best_cost)
            {
                #ifdef VERBOSE
                    purge_count += pow(2.0, mask.size()-offset);
                #endif
                continue;
            }
            if(fusibility)
            {
                //Lets save the new best dag
                best_cost = cost;
                best_dag = new_dag;
                assert(dag_validate(best_dag));

                //Lets write the current best to file
                vector<bh_ir_kernel> kernel_list;
                fill_kernel_list(best_dag, kernel_list);
                const InstrIndexesList &i = cache.insert(bhir.instr_list, kernel_list);
                cache.write_to_files();

                #ifdef VERBOSE
                    stringstream ss;
                    string filename;
                    i.get_filename(filename);
                    ss << "new_best_dag-" << filename << ".dot";
                    cout << "write file: " << ss.str() << endl;
                    pprint(best_dag, ss.str().c_str());
                    purge_count += pow(2.0, mask.size()-offset);
                #endif
                continue;
            }
            //for(unsigned int i=offset; i<mask.size(); ++i) //breadth first
            for(int i=mask.size()-1; i>= (int)offset; --i)   //depth first
            {
                vector<bool> m1(mask);
                m1[i] = false;
                tasks.push_back(make_pair(m1, i+1));
            }
        }
    }
};

void get_edges2explore(const GraphDW &dag, const set<Vertex> &vertices2explore,
                       vector<EdgeW> &edges2explore)
{
    //The list of edges that we should try to merge
    BOOST_FOREACH(const EdgeW &e, edges(dag.bglW()))
    {
        if(vertices2explore.find(source(e, dag.bglW())) != vertices2explore.end() or
           vertices2explore.find(target(e, dag.bglW())) != vertices2explore.end())
            edges2explore.push_back(e);
    }
    sort_weights(dag.bglW(), edges2explore);
    string order;
    {
        const char *t = getenv("BH_FUSER_OPTIMAL_ORDER");
        if(t == NULL)
            order ="regular";
        else
            order = t;
    }
    if(not iequals(order, "regular"))
    {
        if(iequals(order, "reverse"))
        {
            reverse(edges2explore.begin(), edges2explore.end());
        }
        else if(iequals(order, "random"))
        {
            random_shuffle(edges2explore.begin(), edges2explore.end());
        }
        else
        {
            cerr << "FUSER-OPTIMAL: unknown BH_FUSER_OPTIMAL_ORDER: " << order << endl;
            order = "regular";
        }
    }
    cout << "BH_FUSER_OPTIMAL_ORDER: " << order << endl;
}

/* Fuse the 'dag' optimally */
void fuse_optimal(bh_ir &bhir, const GraphDW &dag, const set<Vertex> &vertices2explore,
                  GraphD &output, FuseCache &cache)
{
    //The list of edges that we should try to merge
    vector<EdgeW> edges2explore;
    get_edges2explore(dag, vertices2explore, edges2explore);
    if(edges2explore.size() == 0)
        return;

    //We need the set of vertices that the greedy fusion must ignore
    set<Vertex> ignores;
    BOOST_FOREACH(Vertex v, vertices(dag.bglD()))
    {
        if(vertices2explore.find(v) == vertices2explore.end())
            ignores.insert(v);
    }

    //Check for a preloaded initial condition
    vector<bool> mask(edges2explore.size(), true);
    unsigned int preload_offset=0;
    if(edges2explore.size() > 10)
    {
        const char *t = getenv("BH_FUSER_OPTIMAL_PRELOAD");
        if(t != NULL)
        {
            BOOST_FOREACH(const char &c, string(t))
            {
                mask[preload_offset++] = lexical_cast<bool>(c);
                if(preload_offset == mask.size())
                    break;
            }
            cout << "Preloaded path (" << preload_offset << "): ";
            for(unsigned int j=0; j<preload_offset; ++j)
                cout << mask[j] << ", ";
            cout << endl;
            --preload_offset;
        }
    }

    Solver solver(bhir, dag, edges2explore, cache, ignores);
    if(mask.size() > 100)
    {
        cout << "FUSER-OPTIMAL: ABORT the size of the search space is too large: 2^";
        cout << mask.size() << "!" << endl;
    }
    else
    {
        cout << "FUSER-OPTIMAL: the size of the search space is 2^" << mask.size() << "!" << endl;
        solver.branch_n_bound(mask, preload_offset);
    }
    output = solver.best_dag;
}

void do_fusion(bh_ir &bhir, FuseCache &cache)
{
    vector<bh_ir_kernel> kernel_list;
    {
        GraphDW dag;
        from_bhir(bhir, dag);
        fill_kernel_list(dag.bglD(), kernel_list);
    }
    while(true)
    {
        cout << endl << "Starting new round." << endl;
        GraphDW dag;
        from_kernels(kernel_list, dag);
        fuse_gentle(dag);
        dag.transitive_reduction();
        assert(dag_validate(dag.bglD()));

        vector<set<Vertex> > component2vertices;
        {
            vector<Vertex> vertex2component(num_vertices(dag.bglW()));
            uint64_t num = connected_components(dag.bglW(), &vertex2component[0]);
            component2vertices.resize(num);
            for(Vertex v=0; v<vertex2component.size(); ++v)
            {
                component2vertices[vertex2component[v]].insert(v);
            }
        }

        uint64_t component_id = 0;
        BOOST_FOREACH(set<Vertex> &vertices, component2vertices)
        {
            cout << "Component " << component_id << ": ";
            BOOST_FOREACH(Vertex v, vertices)
            {
                cout << v << ", ";
            }
            cout << endl;
            ++component_id;
        }

        //Find the smallest component with more than one vertex
        uint64_t comp_size=num_vertices(dag.bglW())+1;
        int comp_id=-1;
        for(uint64_t i=0; i < component2vertices.size(); ++i)
        {
            const uint64_t size = component2vertices[i].size();
           if(1 < size and size < comp_size)
           {
               comp_size = size;
               comp_id = i;
           }
        }
        if(comp_id == -1)
            break;//No more components to fuse

        GraphD output;
        cout << "Fusing component: " << comp_id << endl;
        fuse_optimal(bhir, dag, component2vertices[comp_id], output, cache);
        assert(num_vertices(output) > 0);
        assert(dag_validate(output));
        kernel_list.clear();
        fill_kernel_list(output, kernel_list);
    }
    bhir.kernel_list = kernel_list;
}

void fuser(bh_ir &bhir, FuseCache &cache)
{
    if(bhir.kernel_list.size() != 0)
        throw logic_error("The kernel_list is not empty!");

    BatchHash batch(bhir.instr_list);
    //if(not cache.lookup(batch, bhir, bhir.kernel_list))
    {
        do_fusion(bhir, cache);
        cache.insert(batch, bhir.kernel_list);
    }
}

<|endoftext|>


<commit_before>// This file is part of the dune-pymor project:
//   https://github.com/pyMor/dune-pymor
// Copyright Holders: Felix Albrecht, Stephan Rave
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifdef HAVE_CMAKE_CONFIG
  #include "cmake_config.h"
#elif defined (HAVE_CONFIG_H)
  #include <config.h>
#endif // HAVE_CMAKE_CONFIG

#include <utility>

#include <dune/stuff/test/test_common.hh>

#if HAVE_EIGEN
#include <dune/pymor/operators/eigen.hh>
#include <dune/pymor/common/exceptions.hh>
#include <dune/common/float_cmp.hh>

using namespace Dune;
using namespace Dune::Pymor;

typedef testing::Types< std::pair< Dune::Pymor::Operators::EigenDenseMatrix,
                                   Dune::Pymor::LA::EigenDenseVector >
                      , std::pair< Dune::Pymor::Operators::EigenRowMajorSparseMatrix,
                                   Dune::Pymor::LA::EigenDenseVector >
                      > LinearOperatorTypes;

template< class T >
T* createIdentityMatrix(const T&);

Dune::Pymor::Operators::EigenDenseMatrix* createIdentityMatrix(const Dune::Pymor::Operators::EigenDenseMatrix&,
                                                               const size_t dim)
{
  Dune::Pymor::Operators::EigenDenseMatrix* ret = new Dune::Pymor::Operators::EigenDenseMatrix(dim, dim);
  for (size_t ii = 0; ii < dim; ++ii)
    ret->set(ii, ii, 1.0);
  return ret;
}

Dune::Pymor::Operators::EigenRowMajorSparseMatrix* createIdentityMatrix(const Dune::Pymor::Operators::EigenRowMajorSparseMatrix&,
                                                                        const size_t dim)
{
  return new Dune::Pymor::Operators::EigenRowMajorSparseMatrix(*(Dune::Stuff::LA::createIdentityEigenRowMajorSparseMatrix(dim)));
}

template< class TypePair >
struct LinearOperatorTest
  : public ::testing::Test
{
  void check() const
  {
    typedef typename TypePair::first_type   OperatorType;
    typedef typename TypePair::second_type  VectorType;
    const size_t dim = 2;
    VectorType* U = new VectorType(dim, 1.0);
    OperatorType* op = createIdentityMatrix(OperatorType(), dim);
    if (!op->linear()) DUNE_PYMOR_THROW(PymorException, "");
    if (op->parametric()) DUNE_PYMOR_THROW(PymorException, "");
    if (op->dim_source() != dim) DUNE_PYMOR_THROW(PymorException, "");
    if (op->dim_range() != dim) DUNE_PYMOR_THROW(PymorException, "");
    if (op->type_source() != U->type()) DUNE_PYMOR_THROW(PymorException, "");
    if (op->type_range() != U->type()) DUNE_PYMOR_THROW(PymorException, "");
    VectorType* V = op->apply(U);
    if (V->dim() != op->dim_range()) DUNE_PYMOR_THROW(PymorException, "");
    if (!Dune::FloatCmp::eq(V->get(0), 1.0) || !Dune::FloatCmp::eq(V->get(1), 1.0))
      DUNE_PYMOR_THROW(PymorException, "");
    const double res = op->apply2(U, V);
    if (!Dune::FloatCmp::eq(res, 2.0)) DUNE_PYMOR_THROW(PymorException, "");
    delete V;
    delete op;
    delete U;
  }
}; // struct LinearOperatorTest

TYPED_TEST_CASE(LinearOperatorTest, LinearOperatorTypes);
TYPED_TEST(LinearOperatorTest, OPERATORS) {
  this->check();
}

int main(int argc, char** argv)
{
//  try {
    test_init(argc, argv);
    return RUN_ALL_TESTS();
//  } catch (Dune::PymorException &e) {
//    std::cerr << Dune::Stuff::Common::colorStringRed("dune-pymor reported: ") << e << std::endl;
//  } catch (Dune::Exception &e) {
//    std::cerr << Dune::Stuff::Common::colorStringRed("Dune reported error: ") << e << std::endl;
//  } catch (...) {
//    std::cerr << Dune::Stuff::Common::colorStringRed("Unknown exception thrown!") << std::endl;
//  }
}

#else // HAVE_EIGEN

int main(int /*argc*/, char** /*argv*/)
{
  return 0;
}

#endif // HAVE_EIGEN
<commit_msg>[test.operators] update<commit_after>// This file is part of the dune-pymor project:
//   https://github.com/pyMor/dune-pymor
// Copyright Holders: Felix Albrecht, Stephan Rave
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifdef HAVE_CMAKE_CONFIG
  #include "cmake_config.h"
#elif defined (HAVE_CONFIG_H)
  #include <config.h>
#endif // HAVE_CMAKE_CONFIG

#include <utility>

#include <dune/stuff/test/test_common.hh>

#if HAVE_EIGEN
#include <dune/pymor/operators/eigen.hh>
#include <dune/pymor/common/exceptions.hh>
#include <dune/common/float_cmp.hh>

using namespace Dune;
using namespace Dune::Pymor;

typedef testing::Types< std::pair< Dune::Pymor::Operators::EigenDenseMatrix,
                                   Dune::Pymor::LA::EigenDenseVector >
                      , std::pair< Dune::Pymor::Operators::EigenRowMajorSparseMatrix,
                                   Dune::Pymor::LA::EigenDenseVector >
                      > LinearOperatorTypes;

template< class T >
T* createIdentityMatrix(const T&);

Dune::Pymor::Operators::EigenDenseMatrix* createIdentityMatrix(const Dune::Pymor::Operators::EigenDenseMatrix&,
                                                               const size_t dim)
{
  Dune::Pymor::Operators::EigenDenseMatrix* ret = new Dune::Pymor::Operators::EigenDenseMatrix(dim, dim);
  for (size_t ii = 0; ii < dim; ++ii)
    ret->set(ii, ii, 1.0);
  return ret;
}

Dune::Pymor::Operators::EigenRowMajorSparseMatrix* createIdentityMatrix(const Dune::Pymor::Operators::EigenRowMajorSparseMatrix&,
                                                                        const size_t dim)
{
  return new Dune::Pymor::Operators::EigenRowMajorSparseMatrix(*(Dune::Stuff::LA::createIdentityEigenRowMajorSparseMatrix(dim)));
}

template< class TypePair >
struct LinearOperatorTest
  : public ::testing::Test
{
  void check() const
  {
    typedef typename TypePair::first_type   OperatorType;
    typedef typename TypePair::second_type  VectorType;
    const size_t dim = 2;
    VectorType* U = new VectorType(dim, 1.0);
    VectorType* V = new VectorType(dim, 1.0);
    OperatorType* op = createIdentityMatrix(OperatorType(), dim);
    if (!op->linear()) DUNE_PYMOR_THROW(PymorException, "");
    if (op->parametric()) DUNE_PYMOR_THROW(PymorException, "");
    if (op->dim_source() != dim) DUNE_PYMOR_THROW(PymorException, "");
    if (op->dim_range() != dim) DUNE_PYMOR_THROW(PymorException, "");
    if (op->type_source() != U->type()) DUNE_PYMOR_THROW(PymorException, "");
    if (op->type_range() != U->type()) DUNE_PYMOR_THROW(PymorException, "");
    op->apply(U, V);
    if (V->dim() != op->dim_range()) DUNE_PYMOR_THROW(PymorException, "");
    if (!Dune::FloatCmp::eq(V->get(0), 1.0) || !Dune::FloatCmp::eq(V->get(1), 1.0))
      DUNE_PYMOR_THROW(PymorException, "");
    const double res = op->apply2(U, V);
    if (!Dune::FloatCmp::eq(res, 2.0)) DUNE_PYMOR_THROW(PymorException, "");
    delete V;
    delete op;
    delete U;
  }
}; // struct LinearOperatorTest

TYPED_TEST_CASE(LinearOperatorTest, LinearOperatorTypes);
TYPED_TEST(LinearOperatorTest, OPERATORS) {
  this->check();
}

int main(int argc, char** argv)
{
//  try {
    test_init(argc, argv);
    return RUN_ALL_TESTS();
//  } catch (Dune::PymorException &e) {
//    std::cerr << Dune::Stuff::Common::colorStringRed("dune-pymor reported: ") << e << std::endl;
//  } catch (Dune::Exception &e) {
//    std::cerr << Dune::Stuff::Common::colorStringRed("Dune reported error: ") << e << std::endl;
//  } catch (...) {
//    std::cerr << Dune::Stuff::Common::colorStringRed("Unknown exception thrown!") << std::endl;
//  }
}

#else // HAVE_EIGEN

int main(int /*argc*/, char** /*argv*/)
{
  return 0;
}

#endif // HAVE_EIGEN
<|endoftext|>


<commit_before>/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC Development Team
 * 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.
 *
 * 3. Neither the name of the copyright holder 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 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.
 ******************************************************************************/

/*******************************************************************************
 * \file qfilter.c
 *
 * \author MAV'RIC Team
 * \author Felix Schill
 *
 * \brief This file implements a complementary filter for the attitude estimation
 *
 ******************************************************************************/

#include "qfilter.hpp"

extern "C"
{
	#include "coord_conventions.h"
	#include "print_util.h"
	#include "time_keeper.hpp"
	#include <math.h>
	#include "maths.h"
	#include "constants.h"
}

//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS DECLARATION
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// PUBLIC FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

bool qfilter_init(qfilter_t* qf, const qfilter_conf_t config, const Imu* imu, ahrs_t* ahrs)
{
	bool init_success = true;

	//Init dependencies
	qf->imu = imu;
	qf->ahrs = ahrs;

	//init qfilter gains according to provided configuration
	qf->kp = config.kp;
	qf->ki = config.ki;
	qf->kp_mag = config.kp_mag;
	qf->ki_mag = config.ki_mag;

	qf->gyro_bias = std::array<float,3>{{0.0f, 0.0f, 0.0f}};

	return init_success;
}

void qfilter_update(qfilter_t *qf)
{
	static uint32_t convergence_update_count = 0;
	float  omc[3], omc_mag[3] , tmp[3], snorm, norm, s_acc_norm, acc_norm, s_mag_norm, mag_norm;
	quat_t qed, qtmp1, up, up_bf;
	quat_t mag_global, mag_corrected_local;

	quat_t front_vec_global = {};
	front_vec_global.s    = 0.0f;
	front_vec_global.v[0] = 1.0f;
	front_vec_global.v[1] = 0.0f;
	front_vec_global.v[2] = 0.0f;

	float kp 	 = qf->kp;
	float kp_mag = qf->kp_mag;
	float ki 	 = qf->ki;
	float ki_mag = qf->ki_mag;

	// Update time in us
	float now_us 	= time_keeper_get_us();

	// Delta t in seconds
	float dt     = 1e-6 * (float)(now_us - qf->ahrs->last_update);

	// Write to ahrs structure
	qf->ahrs->dt 		  = dt;
	qf->ahrs->last_update = now_us;

	// up_bf = qe^-1 *(0,0,0,-1) * qe
	up.s = 0; up.v[X] = UPVECTOR_X; up.v[Y] = UPVECTOR_Y; up.v[Z] = UPVECTOR_Z;
	up_bf = quaternions_global_to_local(qf->ahrs->qe, up);

	// Get data from IMU
	std::array<float,3> acc  = qf->imu->acc();
	std::array<float,3> gyro = qf->imu->gyro();
	std::array<float,3> mag  = qf->imu->mag();

	// Remove estimated bias from gyros
	gyro[X] -= qf->gyro_bias[X];
	gyro[Y] -= qf->gyro_bias[Y];
	gyro[Z] -= qf->gyro_bias[Z];

	// calculate norm of acceleration vector
	s_acc_norm =  acc[X]*acc[X] + acc[Y]*acc[Y] + acc[Z]*acc[Z];
	if ( (s_acc_norm > 0.7f * 0.7f) && (s_acc_norm < 1.3f * 1.3f) )
	{
		// approximate square root by running 2 iterations of newton method
		acc_norm = maths_fast_sqrt(s_acc_norm);

		tmp[X] = acc[X] / acc_norm;
		tmp[Y] = acc[Y] / acc_norm;
		tmp[Z] = acc[Z] / acc_norm;

		// omc = a x up_bf.v
		CROSS(tmp, up_bf.v, omc);
	}
	else
	{
		omc[X] = 0;
		omc[Y] = 0;
		omc[Z] = 0;
	}

	// Heading computation
	// transfer
	qtmp1 = quat_t{0.0f, {mag[X], mag[Y], mag[Z]}};
	mag_global = quaternions_local_to_global(qf->ahrs->qe, qtmp1);

	// calculate norm of compass vector
	//s_mag_norm = SQR(mag_global.v[X]) + SQR(mag_global.v[Y]) + SQR(mag_global.v[Z]);
	s_mag_norm = SQR(mag_global.v[X]) + SQR(mag_global.v[Y]);

	if ( (s_mag_norm > 0.004f * 0.004f) && (s_mag_norm < 1.8f * 1.8f) )
	{
		mag_norm = maths_fast_sqrt(s_mag_norm);

		mag_global.v[X] /= mag_norm;
		mag_global.v[Y] /= mag_norm;
		mag_global.v[Z] = 0.0f;   // set z component in global frame to 0

		// transfer magneto vector back to body frame
		quat_t north_vec = quaternions_global_to_local(qf->ahrs->qe, front_vec_global);

		mag_corrected_local = quaternions_global_to_local(qf->ahrs->qe, mag_global);

		// omc = a x up_bf.v
		CROSS(mag_corrected_local.v, north_vec.v,  omc_mag);

	}
	else
	{
		omc_mag[X] = 0;
		omc_mag[Y] = 0;
		omc_mag[Z] = 0;
	}

	// get error correction gains depending on mode
	switch (qf->ahrs->internal_state)
	{
		case AHRS_UNLEVELED:
			kp = qf->kp * 10.0f;
			kp_mag = qf->kp_mag * 10.0f;

			ki = 0.5f * qf->ki;
			ki_mag = 0.5f * qf->ki_mag;

			convergence_update_count += 1;
			if( convergence_update_count > 2000 )
			{
				convergence_update_count = 0;
				qf->ahrs->internal_state = AHRS_CONVERGING;
				print_util_dbg_print("[QFILTER] End of AHRS attitude initialization.\r\n");
			}
			break;

		case AHRS_CONVERGING:
			kp = qf->kp;
			kp_mag = qf->kp_mag;

			ki = qf->ki * 3.0f;
			ki_mag = qf->ki_mag * 3.0f;

			convergence_update_count += 1;
			if( convergence_update_count > 2000 )
			{
				convergence_update_count = 0;
				qf->ahrs->internal_state = AHRS_READY;
				print_util_dbg_print("[QFILTER] End of AHRS leveling.\r\n");
			}
			break;

		case AHRS_READY:
			kp = qf->kp;
			kp_mag = qf->kp_mag;
			ki = qf->ki;
			ki_mag = qf->ki_mag;
			break;
	}

	// apply error correction with appropriate gains for accelerometer and compass

	for (uint8_t i = 0; i < 3; i++)
	{
		qtmp1.v[i] = 0.5f * (gyro[i] + kp * omc[i] + kp_mag * omc_mag[i]);
	}
	qtmp1.s = 0;

	// apply step rotation with corrections
	qed = quaternions_multiply(qf->ahrs->qe,qtmp1);

	// TODO: correct this formulas!
	qf->ahrs->qe.s = qf->ahrs->qe.s + qed.s * dt;
	qf->ahrs->qe.v[X] += qed.v[X] * dt;
	qf->ahrs->qe.v[Y] += qed.v[Y] * dt;
	qf->ahrs->qe.v[Z] += qed.v[Z] * dt;

	snorm = qf->ahrs->qe.s * qf->ahrs->qe.s + qf->ahrs->qe.v[X] * qf->ahrs->qe.v[X] + qf->ahrs->qe.v[Y] * qf->ahrs->qe.v[Y] + qf->ahrs->qe.v[Z] * qf->ahrs->qe.v[Z];
	if (snorm < 0.0001f)
	{
		norm = 0.01f;
	}
	else
	{
		// approximate square root by running 2 iterations of newton method
		norm = maths_fast_sqrt(snorm);
	}
	qf->ahrs->qe.s /= norm;
	qf->ahrs->qe.v[X] /= norm;
	qf->ahrs->qe.v[Y] /= norm;
	qf->ahrs->qe.v[Z] /= norm;

	// bias estimate update
	qf->gyro_bias[X] += - dt * ki * omc[X];
	qf->gyro_bias[Y] += - dt * ki * omc[Y];
	qf->gyro_bias[Z] += - dt * ki * omc[Z];

	qf->gyro_bias[X] += - dt * ki_mag * omc_mag[X];
	qf->gyro_bias[Y] += - dt * ki_mag * omc_mag[Y];
	qf->gyro_bias[Z] += - dt * ki_mag * omc_mag[Z];

	// Update linear acceleration
	qf->ahrs->linear_acc[X] = 9.81f * (acc[X] - up_bf.v[X]) ;
	qf->ahrs->linear_acc[Y] = 9.81f * (acc[Y] - up_bf.v[Y]) ;
	qf->ahrs->linear_acc[Z] = 9.81f * (acc[Z] - up_bf.v[Z]) ;

	// Update angular_speed.
	qf->ahrs->angular_speed[X] = gyro[X] - qf->gyro_bias[X];
	qf->ahrs->angular_speed[Y] = gyro[Y] - qf->gyro_bias[Y];
	qf->ahrs->angular_speed[Z] = gyro[Z] - qf->gyro_bias[Z];
}<commit_msg>Fix bug in qfilter, gyro bias was removed twice<commit_after>/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC Development Team
 * 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.
 *
 * 3. Neither the name of the copyright holder 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 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.
 ******************************************************************************/

/*******************************************************************************
 * \file qfilter.c
 *
 * \author MAV'RIC Team
 * \author Felix Schill
 *
 * \brief This file implements a complementary filter for the attitude estimation
 *
 ******************************************************************************/

#include "qfilter.hpp"

extern "C"
{
	#include "coord_conventions.h"
	#include "print_util.h"
	#include "time_keeper.hpp"
	#include <math.h>
	#include "maths.h"
	#include "constants.h"
}

//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS DECLARATION
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// PUBLIC FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

bool qfilter_init(qfilter_t* qf, const qfilter_conf_t config, const Imu* imu, ahrs_t* ahrs)
{
	bool init_success = true;

	//Init dependencies
	qf->imu = imu;
	qf->ahrs = ahrs;

	//init qfilter gains according to provided configuration
	qf->kp = config.kp;
	qf->ki = config.ki;
	qf->kp_mag = config.kp_mag;
	qf->ki_mag = config.ki_mag;

	qf->gyro_bias = std::array<float,3>{{0.0f, 0.0f, 0.0f}};

	return init_success;
}

void qfilter_update(qfilter_t *qf)
{
	static uint32_t convergence_update_count = 0;
	float  omc[3], omc_mag[3] , tmp[3], snorm, norm, s_acc_norm, acc_norm, s_mag_norm, mag_norm;
	quat_t qed, qtmp1, up, up_bf;
	quat_t mag_global, mag_corrected_local;

	quat_t front_vec_global = {};
	front_vec_global.s    = 0.0f;
	front_vec_global.v[0] = 1.0f;
	front_vec_global.v[1] = 0.0f;
	front_vec_global.v[2] = 0.0f;

	float kp 	 = qf->kp;
	float kp_mag = qf->kp_mag;
	float ki 	 = qf->ki;
	float ki_mag = qf->ki_mag;

	// Update time in us
	float now_us 	= time_keeper_get_us();

	// Delta t in seconds
	float dt     = 1e-6 * (float)(now_us - qf->ahrs->last_update);

	// Write to ahrs structure
	qf->ahrs->dt 		  = dt;
	qf->ahrs->last_update = now_us;

	// up_bf = qe^-1 *(0,0,0,-1) * qe
	up.s = 0; up.v[X] = UPVECTOR_X; up.v[Y] = UPVECTOR_Y; up.v[Z] = UPVECTOR_Z;
	up_bf = quaternions_global_to_local(qf->ahrs->qe, up);

	// Get data from IMU
	std::array<float,3> acc  = qf->imu->acc();
	std::array<float,3> gyro = qf->imu->gyro();
	std::array<float,3> mag  = qf->imu->mag();

	// Remove estimated bias from gyros
	gyro[X] -= qf->gyro_bias[X];
	gyro[Y] -= qf->gyro_bias[Y];
	gyro[Z] -= qf->gyro_bias[Z];

	// calculate norm of acceleration vector
	s_acc_norm =  acc[X]*acc[X] + acc[Y]*acc[Y] + acc[Z]*acc[Z];
	if ( (s_acc_norm > 0.7f * 0.7f) && (s_acc_norm < 1.3f * 1.3f) )
	{
		// approximate square root by running 2 iterations of newton method
		acc_norm = maths_fast_sqrt(s_acc_norm);

		tmp[X] = acc[X] / acc_norm;
		tmp[Y] = acc[Y] / acc_norm;
		tmp[Z] = acc[Z] / acc_norm;

		// omc = a x up_bf.v
		CROSS(tmp, up_bf.v, omc);
	}
	else
	{
		omc[X] = 0;
		omc[Y] = 0;
		omc[Z] = 0;
	}

	// Heading computation
	// transfer
	qtmp1 = quat_t{0.0f, {mag[X], mag[Y], mag[Z]}};
	mag_global = quaternions_local_to_global(qf->ahrs->qe, qtmp1);

	// calculate norm of compass vector
	//s_mag_norm = SQR(mag_global.v[X]) + SQR(mag_global.v[Y]) + SQR(mag_global.v[Z]);
	s_mag_norm = SQR(mag_global.v[X]) + SQR(mag_global.v[Y]);

	if ( (s_mag_norm > 0.004f * 0.004f) && (s_mag_norm < 1.8f * 1.8f) )
	{
		mag_norm = maths_fast_sqrt(s_mag_norm);

		mag_global.v[X] /= mag_norm;
		mag_global.v[Y] /= mag_norm;
		mag_global.v[Z] = 0.0f;   // set z component in global frame to 0

		// transfer magneto vector back to body frame
		quat_t north_vec = quaternions_global_to_local(qf->ahrs->qe, front_vec_global);

		mag_corrected_local = quaternions_global_to_local(qf->ahrs->qe, mag_global);

		// omc = a x up_bf.v
		CROSS(mag_corrected_local.v, north_vec.v,  omc_mag);

	}
	else
	{
		omc_mag[X] = 0;
		omc_mag[Y] = 0;
		omc_mag[Z] = 0;
	}

	// get error correction gains depending on mode
	switch (qf->ahrs->internal_state)
	{
		case AHRS_UNLEVELED:
			kp = qf->kp * 10.0f;
			kp_mag = qf->kp_mag * 10.0f;

			ki = 0.5f * qf->ki;
			ki_mag = 0.5f * qf->ki_mag;

			convergence_update_count += 1;
			if( convergence_update_count > 2000 )
			{
				convergence_update_count = 0;
				qf->ahrs->internal_state = AHRS_CONVERGING;
				print_util_dbg_print("[QFILTER] End of AHRS attitude initialization.\r\n");
			}
			break;

		case AHRS_CONVERGING:
			kp = qf->kp;
			kp_mag = qf->kp_mag;

			ki = qf->ki * 3.0f;
			ki_mag = qf->ki_mag * 3.0f;

			convergence_update_count += 1;
			if( convergence_update_count > 2000 )
			{
				convergence_update_count = 0;
				qf->ahrs->internal_state = AHRS_READY;
				print_util_dbg_print("[QFILTER] End of AHRS leveling.\r\n");
			}
			break;

		case AHRS_READY:
			kp = qf->kp;
			kp_mag = qf->kp_mag;
			ki = qf->ki;
			ki_mag = qf->ki_mag;
			break;
	}

	// apply error correction with appropriate gains for accelerometer and compass

	for (uint8_t i = 0; i < 3; i++)
	{
		qtmp1.v[i] = 0.5f * (gyro[i] + kp * omc[i] + kp_mag * omc_mag[i]);
	}
	qtmp1.s = 0;

	// apply step rotation with corrections
	qed = quaternions_multiply(qf->ahrs->qe,qtmp1);

	// TODO: correct this formulas!
	qf->ahrs->qe.s = qf->ahrs->qe.s + qed.s * dt;
	qf->ahrs->qe.v[X] += qed.v[X] * dt;
	qf->ahrs->qe.v[Y] += qed.v[Y] * dt;
	qf->ahrs->qe.v[Z] += qed.v[Z] * dt;

	snorm = qf->ahrs->qe.s * qf->ahrs->qe.s + qf->ahrs->qe.v[X] * qf->ahrs->qe.v[X] + qf->ahrs->qe.v[Y] * qf->ahrs->qe.v[Y] + qf->ahrs->qe.v[Z] * qf->ahrs->qe.v[Z];
	if (snorm < 0.0001f)
	{
		norm = 0.01f;
	}
	else
	{
		// approximate square root by running 2 iterations of newton method
		norm = maths_fast_sqrt(snorm);
	}
	qf->ahrs->qe.s /= norm;
	qf->ahrs->qe.v[X] /= norm;
	qf->ahrs->qe.v[Y] /= norm;
	qf->ahrs->qe.v[Z] /= norm;

	// bias estimate update
	qf->gyro_bias[X] += - dt * ki * omc[X];
	qf->gyro_bias[Y] += - dt * ki * omc[Y];
	qf->gyro_bias[Z] += - dt * ki * omc[Z];

	qf->gyro_bias[X] += - dt * ki_mag * omc_mag[X];
	qf->gyro_bias[Y] += - dt * ki_mag * omc_mag[Y];
	qf->gyro_bias[Z] += - dt * ki_mag * omc_mag[Z];

	// Update linear acceleration
	qf->ahrs->linear_acc[X] = 9.81f * (acc[X] - up_bf.v[X]) ;
	qf->ahrs->linear_acc[Y] = 9.81f * (acc[Y] - up_bf.v[Y]) ;
	qf->ahrs->linear_acc[Z] = 9.81f * (acc[Z] - up_bf.v[Z]) ;

	// Update angular_speed.
	qf->ahrs->angular_speed[X] = gyro[X];
	qf->ahrs->angular_speed[Y] = gyro[Y];
	qf->ahrs->angular_speed[Z] = gyro[Z];
}<|endoftext|>


<commit_before>/*++
Copyright (c) 2013 Microsoft Corporation

Module Name:

    smt_farkas_util.cpp

Abstract:

    Utility for combining inequalities using coefficients obtained from Farkas lemmas.

Author:

    Nikolaj Bjorner (nbjorner) 2013-11-2.

Revision History:

    NB. This utility is specialized to proofs generated by the arithmetic solvers.

--*/

#include "smt_farkas_util.h"
#include "ast_pp.h"
#include "th_rewriter.h"
#include "bool_rewriter.h"

namespace smt {

    farkas_util::farkas_util(ast_manager& m):
        m(m),
        a(m),
        m_ineqs(m),
        m_split_literals(false),
        m_time(0) {
    }

    void farkas_util::mk_coerce(expr*& e1, expr*& e2) {
        if (a.is_int(e1) && a.is_real(e2)) {
            e1 = a.mk_to_real(e1);
        }
        else if (a.is_int(e2) && a.is_real(e1)) {
            e2 = a.mk_to_real(e2);
        }
    }

    // TBD: arith_decl_util now supports coercion, so this should be deprecated.
    app* farkas_util::mk_add(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_add(e1, e2);
    }

    app* farkas_util::mk_mul(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_mul(e1, e2);
    }

    app* farkas_util::mk_le(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_le(e1, e2);
    }

    app* farkas_util::mk_ge(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_gt(e1, e2);
    }

    app* farkas_util::mk_gt(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_gt(e1, e2);
    }

    app* farkas_util::mk_lt(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_lt(e1, e2);
    }

    void farkas_util::mul(rational const& c, expr* e, expr_ref& res) {
        expr_ref tmp(m);
        if (c.is_one()) {
            tmp = e;
        }
        else {
            tmp = mk_mul(a.mk_numeral(c, c.is_int() && a.is_int(e)), e);
        }
        res = mk_add(res, tmp);
    }

    bool farkas_util::is_int_sort(app* c) {
        SASSERT(m.is_eq(c) || a.is_le(c) || a.is_lt(c) || a.is_gt(c) || a.is_ge(c));
        SASSERT(a.is_int(c->get_arg(0)) || a.is_real(c->get_arg(0)));
        return a.is_int(c->get_arg(0));
    }

    bool farkas_util::is_int_sort() {
        SASSERT(!m_ineqs.empty());
        return is_int_sort(m_ineqs[0].get());
    }

    void farkas_util::normalize_coeffs() {
        rational l(1);
        for (unsigned i = 0; i < m_coeffs.size(); ++i) {
            l = lcm(l, denominator(m_coeffs[i]));
        }
        if (!l.is_one()) {
            for (unsigned i = 0; i < m_coeffs.size(); ++i) {
                m_coeffs[i] *= l;
            }
        }
        m_normalize_factor = l;
    }

    app* farkas_util::mk_one() {
        return a.mk_numeral(rational(1), true);
    }

    app* farkas_util::fix_sign(bool is_pos, app* c) {
        expr* x, *y;
        SASSERT(m.is_eq(c) || a.is_le(c) || a.is_lt(c) || a.is_gt(c) || a.is_ge(c));
        bool is_int = is_int_sort(c);
        if (is_int && is_pos && (a.is_lt(c, x, y) || a.is_gt(c, y, x))) {
            return mk_le(mk_add(x, mk_one()), y);
        }
        if (is_int && !is_pos && (a.is_le(c, x, y) || a.is_ge(c, y, x))) {
            // !(x <= y) <=> x > y <=> x >= y + 1
            return mk_ge(x, mk_add(y, mk_one()));
        }
        if (is_pos) {
            return c;
        }
        if (a.is_le(c, x, y)) return mk_gt(x, y);
        if (a.is_lt(c, x, y)) return mk_ge(x, y);
        if (a.is_ge(c, x, y)) return mk_lt(x, y);
        if (a.is_gt(c, x, y)) return mk_le(x, y);
        UNREACHABLE();
        return c;
    }

    void farkas_util::partition_ineqs() {
        m_reps.reset();
        m_his.reset();
        ++m_time;
        for (unsigned i = 0; i < m_ineqs.size(); ++i) {
            m_reps.push_back(process_term(m_ineqs[i].get()));
        }
        unsigned head = 0;
        while (head < m_ineqs.size()) {
            unsigned r = find(m_reps[head]);
            unsigned tail = head;
            for (unsigned i = head+1; i < m_ineqs.size(); ++i) {
                if (find(m_reps[i]) == r) {
                    ++tail;
                    if (tail != i) {
                        SASSERT(tail < i);
                        std::swap(m_reps[tail], m_reps[i]);
                        app_ref tmp(m);
                        tmp = m_ineqs[i].get();
                        m_ineqs[i] = m_ineqs[tail].get();
                        m_ineqs[tail] = tmp;
                        std::swap(m_coeffs[tail], m_coeffs[i]);
                    }
                }
            }
            head = tail + 1;
            m_his.push_back(head);
        }
    }

    unsigned farkas_util::find(unsigned idx) {
        if (m_ts.size() <= idx) {
            m_roots.resize(idx+1);
            m_size.resize(idx+1);
            m_ts.resize(idx+1);
            m_roots[idx] = idx;
            m_ts[idx] = m_time;
            m_size[idx] = 1;
            return idx;
        }
        if (m_ts[idx] != m_time) {
            m_size[idx] = 1;
            m_ts[idx]    = m_time;
            m_roots[idx] = idx;
            return idx;
        }
        while (true) {
            if (m_roots[idx] == idx) {
                return idx;
            }
            idx = m_roots[idx];
        }
    }

    void farkas_util::merge(unsigned i, unsigned j) {
        i = find(i);
        j = find(j);
        if (i == j) {
            return;
        }
        if (m_size[i] > m_size[j]) {
            std::swap(i, j);
        }
        m_roots[i] = j;
        m_size[j] += m_size[i];
    }
    unsigned farkas_util::process_term(expr* e) {
        unsigned r = e->get_id();
        ptr_vector<expr> todo;
        ast_mark mark;
        todo.push_back(e);
        while (!todo.empty()) {
            e = todo.back();
            todo.pop_back();
            if (mark.is_marked(e)) {
                continue;
            }
            mark.mark(e, true);
            if (is_uninterp(e)) {
                merge(r, e->get_id());
            }
            if (is_app(e)) {
                app* a = to_app(e);
                for (unsigned i = 0; i < a->get_num_args(); ++i) {
                    todo.push_back(a->get_arg(i));
                }
            }
        }
        return r;
    }
    expr_ref farkas_util::extract_consequence(unsigned lo, unsigned hi) {
        bool is_int = is_int_sort();
        app_ref zero(a.mk_numeral(rational::zero(), is_int), m);
        expr_ref res(m);
        res = zero;
        bool is_strict = false;
        bool is_eq     = true;
        expr* x, *y;
        for (unsigned i = lo; i < hi; ++i) {
            app* c = m_ineqs[i].get();
            if (m.is_eq(c, x, y)) {
                mul(m_coeffs[i],  x, res);
                mul(-m_coeffs[i], y, res);
            }
            if (a.is_lt(c, x, y) || a.is_gt(c, y, x)) {
                mul(m_coeffs[i],  x, res);
                mul(-m_coeffs[i], y, res);
                is_strict = true;
                is_eq = false;
            }
            if (a.is_le(c, x, y) || a.is_ge(c, y, x)) {
                mul(m_coeffs[i],  x, res);
                mul(-m_coeffs[i], y, res);
                is_eq = false;
            }
        }

        zero = a.mk_numeral(rational::zero(), a.is_int(res));
        if (is_eq) {
            res = m.mk_eq(res, zero);
        }
        else if (is_strict) {
            res = mk_lt(res, zero);
        }
        else {
            res = mk_le(res, zero);
        }
        res = m.mk_not(res);
        th_rewriter rw(m);
        params_ref params;
        params.set_bool("gcd_rounding", true);
        rw.updt_params(params);
        proof_ref pr(m);
        expr_ref result(m);
        rw(res, result, pr);
        fix_dl(result);
        return result;
    }

    void farkas_util::fix_dl(expr_ref& r) {
        expr* e;
        if (m.is_not(r, e)) {
            r = e;
            fix_dl(r);
            r = m.mk_not(r);
            return;
        }
        expr* e1, *e2, *e3, *e4;
        if ((m.is_eq(r, e1, e2) || a.is_lt(r, e1, e2) || a.is_gt(r, e1, e2) ||
             a.is_le(r, e1, e2) || a.is_ge(r, e1, e2))) {
            if (a.is_add(e1, e3, e4) && a.is_mul(e3)) {
                r = m.mk_app(to_app(r)->get_decl(), a.mk_add(e4,e3), e2);
            }
        }
    }

    void farkas_util::reset() {
        m_ineqs.reset();
        m_coeffs.reset();
    }

    void farkas_util::add(rational const & coef, app * c) {
        bool is_pos = true;
        expr* e;
        while (m.is_not(c, e)) {
            is_pos = !is_pos;
            c = to_app(e);
        }

        if (!coef.is_zero() && !m.is_true(c)) {
            m_coeffs.push_back(coef);
            m_ineqs.push_back(fix_sign(is_pos, c));
        }
    }

    expr_ref farkas_util::get() {
        m_normalize_factor = rational::one();
        expr_ref res(m);
        if (m_coeffs.empty()) {
            res = m.mk_false();
            return res;
        }
        bool is_int = is_int_sort();
        if (is_int) {
            normalize_coeffs();
        }

        if (m_split_literals) {
            // partition equalities into variable disjoint sets.
            // take the conjunction of these instead of the
            // linear combination.
            partition_ineqs();
            expr_ref_vector lits(m);
            unsigned lo = 0;
            for (unsigned i = 0; i < m_his.size(); ++i) {
                unsigned hi = m_his[i];
                lits.push_back(extract_consequence(lo, hi));
                lo = hi;
            }
            bool_rewriter(m).mk_or(lits.size(), lits.c_ptr(), res);
            IF_VERBOSE(2, { if (lits.size() > 1) { verbose_stream() << "combined lemma: " << mk_pp(res, m) << "\n"; } });
        }
        else {
            res = extract_consequence(0, m_coeffs.size());
        }

        TRACE("arith",
              for (unsigned i = 0; i < m_coeffs.size(); ++i) {
                  tout << m_coeffs[i] << " * (" << mk_pp(m_ineqs[i].get(), m) << ") ";
              }
              tout << "\n";
              tout << res << "\n";
              );

        return res;
    }
}

<commit_msg>typo: gt -> ge<commit_after>/*++
Copyright (c) 2013 Microsoft Corporation

Module Name:

    smt_farkas_util.cpp

Abstract:

    Utility for combining inequalities using coefficients obtained from Farkas lemmas.

Author:

    Nikolaj Bjorner (nbjorner) 2013-11-2.

Revision History:

    NB. This utility is specialized to proofs generated by the arithmetic solvers.

--*/

#include "smt_farkas_util.h"
#include "ast_pp.h"
#include "th_rewriter.h"
#include "bool_rewriter.h"

namespace smt {

    farkas_util::farkas_util(ast_manager& m):
        m(m),
        a(m),
        m_ineqs(m),
        m_split_literals(false),
        m_time(0) {
    }

    void farkas_util::mk_coerce(expr*& e1, expr*& e2) {
        if (a.is_int(e1) && a.is_real(e2)) {
            e1 = a.mk_to_real(e1);
        }
        else if (a.is_int(e2) && a.is_real(e1)) {
            e2 = a.mk_to_real(e2);
        }
    }

    // TBD: arith_decl_util now supports coercion, so this should be deprecated.
    app* farkas_util::mk_add(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_add(e1, e2);
    }

    app* farkas_util::mk_mul(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_mul(e1, e2);
    }

    app* farkas_util::mk_le(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_le(e1, e2);
    }

    app* farkas_util::mk_ge(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_ge(e1, e2);
    }

    app* farkas_util::mk_gt(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_gt(e1, e2);
    }

    app* farkas_util::mk_lt(expr* e1, expr* e2) {
        mk_coerce(e1, e2);
        return a.mk_lt(e1, e2);
    }

    void farkas_util::mul(rational const& c, expr* e, expr_ref& res) {
        expr_ref tmp(m);
        if (c.is_one()) {
            tmp = e;
        }
        else {
            tmp = mk_mul(a.mk_numeral(c, c.is_int() && a.is_int(e)), e);
        }
        res = mk_add(res, tmp);
    }

    bool farkas_util::is_int_sort(app* c) {
        SASSERT(m.is_eq(c) || a.is_le(c) || a.is_lt(c) || a.is_gt(c) || a.is_ge(c));
        SASSERT(a.is_int(c->get_arg(0)) || a.is_real(c->get_arg(0)));
        return a.is_int(c->get_arg(0));
    }

    bool farkas_util::is_int_sort() {
        SASSERT(!m_ineqs.empty());
        return is_int_sort(m_ineqs[0].get());
    }

    void farkas_util::normalize_coeffs() {
        rational l(1);
        for (unsigned i = 0; i < m_coeffs.size(); ++i) {
            l = lcm(l, denominator(m_coeffs[i]));
        }
        if (!l.is_one()) {
            for (unsigned i = 0; i < m_coeffs.size(); ++i) {
                m_coeffs[i] *= l;
            }
        }
        m_normalize_factor = l;
    }

    app* farkas_util::mk_one() {
        return a.mk_numeral(rational(1), true);
    }

    app* farkas_util::fix_sign(bool is_pos, app* c) {
        expr* x, *y;
        SASSERT(m.is_eq(c) || a.is_le(c) || a.is_lt(c) || a.is_gt(c) || a.is_ge(c));
        bool is_int = is_int_sort(c);
        if (is_int && is_pos && (a.is_lt(c, x, y) || a.is_gt(c, y, x))) {
            return mk_le(mk_add(x, mk_one()), y);
        }
        if (is_int && !is_pos && (a.is_le(c, x, y) || a.is_ge(c, y, x))) {
            // !(x <= y) <=> x > y <=> x >= y + 1
            return mk_ge(x, mk_add(y, mk_one()));
        }
        if (is_pos) {
            return c;
        }
        if (a.is_le(c, x, y)) return mk_gt(x, y);
        if (a.is_lt(c, x, y)) return mk_ge(x, y);
        if (a.is_ge(c, x, y)) return mk_lt(x, y);
        if (a.is_gt(c, x, y)) return mk_le(x, y);
        UNREACHABLE();
        return c;
    }

    void farkas_util::partition_ineqs() {
        m_reps.reset();
        m_his.reset();
        ++m_time;
        for (unsigned i = 0; i < m_ineqs.size(); ++i) {
            m_reps.push_back(process_term(m_ineqs[i].get()));
        }
        unsigned head = 0;
        while (head < m_ineqs.size()) {
            unsigned r = find(m_reps[head]);
            unsigned tail = head;
            for (unsigned i = head+1; i < m_ineqs.size(); ++i) {
                if (find(m_reps[i]) == r) {
                    ++tail;
                    if (tail != i) {
                        SASSERT(tail < i);
                        std::swap(m_reps[tail], m_reps[i]);
                        app_ref tmp(m);
                        tmp = m_ineqs[i].get();
                        m_ineqs[i] = m_ineqs[tail].get();
                        m_ineqs[tail] = tmp;
                        std::swap(m_coeffs[tail], m_coeffs[i]);
                    }
                }
            }
            head = tail + 1;
            m_his.push_back(head);
        }
    }

    unsigned farkas_util::find(unsigned idx) {
        if (m_ts.size() <= idx) {
            m_roots.resize(idx+1);
            m_size.resize(idx+1);
            m_ts.resize(idx+1);
            m_roots[idx] = idx;
            m_ts[idx] = m_time;
            m_size[idx] = 1;
            return idx;
        }
        if (m_ts[idx] != m_time) {
            m_size[idx] = 1;
            m_ts[idx]    = m_time;
            m_roots[idx] = idx;
            return idx;
        }
        while (true) {
            if (m_roots[idx] == idx) {
                return idx;
            }
            idx = m_roots[idx];
        }
    }

    void farkas_util::merge(unsigned i, unsigned j) {
        i = find(i);
        j = find(j);
        if (i == j) {
            return;
        }
        if (m_size[i] > m_size[j]) {
            std::swap(i, j);
        }
        m_roots[i] = j;
        m_size[j] += m_size[i];
    }
    unsigned farkas_util::process_term(expr* e) {
        unsigned r = e->get_id();
        ptr_vector<expr> todo;
        ast_mark mark;
        todo.push_back(e);
        while (!todo.empty()) {
            e = todo.back();
            todo.pop_back();
            if (mark.is_marked(e)) {
                continue;
            }
            mark.mark(e, true);
            if (is_uninterp(e)) {
                merge(r, e->get_id());
            }
            if (is_app(e)) {
                app* a = to_app(e);
                for (unsigned i = 0; i < a->get_num_args(); ++i) {
                    todo.push_back(a->get_arg(i));
                }
            }
        }
        return r;
    }
    expr_ref farkas_util::extract_consequence(unsigned lo, unsigned hi) {
        bool is_int = is_int_sort();
        app_ref zero(a.mk_numeral(rational::zero(), is_int), m);
        expr_ref res(m);
        res = zero;
        bool is_strict = false;
        bool is_eq     = true;
        expr* x, *y;
        for (unsigned i = lo; i < hi; ++i) {
            app* c = m_ineqs[i].get();
            if (m.is_eq(c, x, y)) {
                mul(m_coeffs[i],  x, res);
                mul(-m_coeffs[i], y, res);
            }
            if (a.is_lt(c, x, y) || a.is_gt(c, y, x)) {
                mul(m_coeffs[i],  x, res);
                mul(-m_coeffs[i], y, res);
                is_strict = true;
                is_eq = false;
            }
            if (a.is_le(c, x, y) || a.is_ge(c, y, x)) {
                mul(m_coeffs[i],  x, res);
                mul(-m_coeffs[i], y, res);
                is_eq = false;
            }
        }

        zero = a.mk_numeral(rational::zero(), a.is_int(res));
        if (is_eq) {
            res = m.mk_eq(res, zero);
        }
        else if (is_strict) {
            res = mk_lt(res, zero);
        }
        else {
            res = mk_le(res, zero);
        }
        res = m.mk_not(res);
        th_rewriter rw(m);
        params_ref params;
        params.set_bool("gcd_rounding", true);
        rw.updt_params(params);
        proof_ref pr(m);
        expr_ref result(m);
        rw(res, result, pr);
        fix_dl(result);
        return result;
    }

    void farkas_util::fix_dl(expr_ref& r) {
        expr* e;
        if (m.is_not(r, e)) {
            r = e;
            fix_dl(r);
            r = m.mk_not(r);
            return;
        }
        expr* e1, *e2, *e3, *e4;
        if ((m.is_eq(r, e1, e2) || a.is_lt(r, e1, e2) || a.is_gt(r, e1, e2) ||
             a.is_le(r, e1, e2) || a.is_ge(r, e1, e2))) {
            if (a.is_add(e1, e3, e4) && a.is_mul(e3)) {
                r = m.mk_app(to_app(r)->get_decl(), a.mk_add(e4,e3), e2);
            }
        }
    }

    void farkas_util::reset() {
        m_ineqs.reset();
        m_coeffs.reset();
    }

    void farkas_util::add(rational const & coef, app * c) {
        bool is_pos = true;
        expr* e;
        while (m.is_not(c, e)) {
            is_pos = !is_pos;
            c = to_app(e);
        }

        if (!coef.is_zero() && !m.is_true(c)) {
            m_coeffs.push_back(coef);
            m_ineqs.push_back(fix_sign(is_pos, c));
        }
    }

    expr_ref farkas_util::get() {
        m_normalize_factor = rational::one();
        expr_ref res(m);
        if (m_coeffs.empty()) {
            res = m.mk_false();
            return res;
        }
        bool is_int = is_int_sort();
        if (is_int) {
            normalize_coeffs();
        }

        if (m_split_literals) {
            // partition equalities into variable disjoint sets.
            // take the conjunction of these instead of the
            // linear combination.
            partition_ineqs();
            expr_ref_vector lits(m);
            unsigned lo = 0;
            for (unsigned i = 0; i < m_his.size(); ++i) {
                unsigned hi = m_his[i];
                lits.push_back(extract_consequence(lo, hi));
                lo = hi;
            }
            bool_rewriter(m).mk_or(lits.size(), lits.c_ptr(), res);
            IF_VERBOSE(2, { if (lits.size() > 1) { verbose_stream() << "combined lemma: " << mk_pp(res, m) << "\n"; } });
        }
        else {
            res = extract_consequence(0, m_coeffs.size());
        }

        TRACE("arith",
              for (unsigned i = 0; i < m_coeffs.size(); ++i) {
                  tout << m_coeffs[i] << " * (" << mk_pp(m_ineqs[i].get(), m) << ") ";
              }
              tout << "\n";
              tout << res << "\n";
              );

        return res;
    }
}

<|endoftext|>