<commit_before>#include <iostream>
#include <sstream>

#include "IRPrinter.h"
#include "IROperator.h"
#include "IR.h"

namespace Halide {

using std::ostream;
using std::vector;
using std::string;
using std::ostringstream;

ostream &operator<<(ostream &out, Type type) {
    switch (type.code) {
    case Type::Int:
        out << "int";
        break;
    case Type::UInt:
        out << "uint";
        break;
    case Type::Float:
        out << "float";
        break;
    case Type::Handle:
        out << "handle";
        break;
    }
    out << type.bits;
    if (type.width > 1) out << 'x' << type.width;
    return out;
}

ostream &operator<<(ostream &stream, Expr ir) {
    if (!ir.defined()) {
        stream << "(undefined)";
    } else {
        Internal::IRPrinter p(stream);
        p.print(ir);
    }
    return stream;
}

namespace Internal {

void IRPrinter::test() {
    Type i32 = Int(32);
    Type f32 = Float(32);
    Expr x = Variable::make(Int(32), "x");
    Expr y = Variable::make(Int(32), "y");
    ostringstream expr_source;
    expr_source << (x + 3) * (y / 2 + 17);
    assert(expr_source.str() == "((x + 3)*((y/2) + 17))");

    Stmt store = Store::make("buf", (x * 17) / (x - 3), y - 1);
    Stmt for_loop = For::make("x", -2, y + 2, For::Parallel, store);
    vector<Expr> args(1); args[0] = x % 3;
    Expr call = Call::make(i32, "buf", args, Call::Extern);
    Stmt store2 = Store::make("out", call + 1, x);
    Stmt for_loop2 = For::make("x", 0, y, For::Vectorized , store2);
    Stmt pipeline = Pipeline::make("buf", for_loop, Stmt(), for_loop2);
    Stmt assertion = AssertStmt::make(y > 3, "y is greater than %d", vec<Expr>(3));
    Stmt block = Block::make(assertion, pipeline);
    Stmt let_stmt = LetStmt::make("y", 17, block);
    Stmt allocate = Allocate::make("buf", f32, vec(Expr(1023)), let_stmt);

    ostringstream source;
    source << allocate;
    std::string correct_source = \
        "allocate buf[float32 * 1023]\n"
        "let y = 17\n"
        "assert((y > 3), \"y is greater than %d\", 3)\n"
        "produce buf {\n"
        "  parallel (x, -2, (y + 2)) {\n"
        "    buf[(y - 1)] = ((x*17)/(x - 3))\n"
        "  }\n"
        "}\n"
        "vectorized (x, 0, y) {\n"
        "  out[x] = (buf((x % 3)) + 1)\n"
        "}\n";

    if (source.str() != correct_source) {
        std::cout << "Correct output:\n" << correct_source;
        std::cout << "Actual output:\n" << source.str();
        assert(false);
    }
    std::cout << "IRPrinter test passed\n";
}

ostream &operator<<(ostream &out, const For::ForType &type) {
    switch (type) {
    case For::Serial:
        out << "for";
        break;
    case For::Parallel:
        out << "parallel";
        break;
    case For::Unrolled:
        out << "unrolled";
        break;
    case For::Vectorized:
        out << "vectorized";
        break;
    }
    return out;
}

ostream &operator<<(ostream &stream, Stmt ir) {
    if (!ir.defined()) {
        stream << "(undefined)\n";
    } else {
        Internal::IRPrinter p(stream);
        p.print(ir);
    }
    return stream;
}

IRPrinter::IRPrinter(ostream &s) : stream(s), indent(0) {
    s.setf(std::ios::fixed, std::ios::floatfield);
}

void IRPrinter::print(Expr ir) {
    ir.accept(this);
}

void IRPrinter::print(Stmt ir) {
    ir.accept(this);
}

void IRPrinter::do_indent() {
    for (int i = 0; i < indent; i++) stream << ' ';
}

void IRPrinter::visit(const IntImm *op) {
    stream << op->value;
}

void IRPrinter::visit(const FloatImm *op) {
    stream << op->value << 'f';
}

void IRPrinter::visit(const StringImm *op) {
    stream << '"';
    for (size_t i = 0; i < op->value.size(); i++) {
        unsigned char c = op->value[i];
        if (c >= ' ' && c <= '~' && c != '\\' && c != '"') {
            stream << c;
        } else {
            stream << '\\';
            switch (c) {
            case '"':
                stream << '"';
                break;
            case '\\':
                stream << '\\';
                break;
            case '\t':
                stream << 't';
                break;
            case '\r':
                stream << 'r';
                break;
            case '\n':
                stream << 'n';
                break;
            default:
                string hex_digits = "0123456789ABCDEF";
                stream << 'x' << hex_digits[c >> 4] << hex_digits[c & 0xf];
            }
        }
    }
    stream << '"';
}

void IRPrinter::visit(const Cast *op) {
    stream << op->type << '(';
    print(op->value);
    stream << ')';
}

void IRPrinter::visit(const Variable *op) {
    // omit the type
    // stream << op->name << "." << op->type;
    stream << op->name;
}

void IRPrinter::visit(const Add *op) {
    stream << '(';
    print(op->a);
    stream << " + ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Sub *op) {
    stream << '(';
    print(op->a);
    stream << " - ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Mul *op) {
    stream << '(';
    print(op->a);
    stream << "*";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Div *op) {
    stream << '(';
    print(op->a);
    stream << "/";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Mod *op) {
    stream << '(';
    print(op->a);
    stream << " % ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Min *op) {
    stream << "min(";
    print(op->a);
    stream << ", ";
    print(op->b);
    stream << ")";
}

void IRPrinter::visit(const Max *op) {
    stream << "max(";
    print(op->a);
    stream << ", ";
    print(op->b);
    stream << ")";
}

void IRPrinter::visit(const EQ *op) {
    stream << '(';
    print(op->a);
    stream << " == ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const NE *op) {
    stream << '(';
    print(op->a);
    stream << " != ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const LT *op) {
    stream << '(';
    print(op->a);
    stream << " < ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const LE *op) {
    stream << '(';
    print(op->a);
    stream << " <= ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const GT *op) {
    stream << '(';
    print(op->a);
    stream << " > ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const GE *op) {
    stream << '(';
    print(op->a);
    stream << " >= ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const And *op) {
    stream << '(';
    print(op->a);
    stream << " && ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Or *op) {
    stream << '(';
    print(op->a);
    stream << " || ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Not *op) {
    stream << '!';
    print(op->a);
}

void IRPrinter::visit(const Select *op) {
    stream << "select(";
    print(op->condition);
    stream << ", ";
    print(op->true_value);
    stream << ", ";
    print(op->false_value);
    stream << ")";
}

void IRPrinter::visit(const Load *op) {
    stream << op->name << "[";
    print(op->index);
    stream << "]";
}

void IRPrinter::visit(const Ramp *op) {
    stream << "ramp(";
    print(op->base);
    stream << ", ";
    print(op->stride);
    stream << ", " << op->width << ")";
}

void IRPrinter::visit(const Broadcast *op) {
    stream << "x" << op->width << "(";
    print(op->value);
    stream << ")";
}

void IRPrinter::visit(const Call *op) {
    stream << op->name << "(";
    for (size_t i = 0; i < op->args.size(); i++) {
        print(op->args[i]);
        if (i < op->args.size() - 1) {
            stream << ", ";
        }
    }
    stream << ")";
}

void IRPrinter::visit(const Let *op) {
    stream << "(let " << op->name << " = ";
    print(op->value);
    stream << " in ";
    print(op->body);
    stream << ")";
}

void IRPrinter::visit(const LetStmt *op) {
    do_indent();
    stream << "let " << op->name << " = ";
    print(op->value);
    stream << '\n';

    print(op->body);
}

void IRPrinter::visit(const AssertStmt *op) {
    do_indent();
    stream << "assert(";
    print(op->condition);
    stream << ", " << '"' << op->message << '"';
    for (size_t i = 0; i < op->args.size(); i++) {
        stream << ", ";
        print(op->args[i]);
    }
    stream << ")\n";
}

void IRPrinter::visit(const Pipeline *op) {

    do_indent();
    stream << "produce " << op->name << " {\n";
    indent += 2;
    print(op->produce);
    indent -= 2;

    if (op->update.defined()) {
        do_indent();
        stream << "} update " << op->name << " {\n";
        indent += 2;
        print(op->update);
        indent -= 2;
    }

    do_indent();
    stream << "}\n";

    print(op->consume);

}

void IRPrinter::visit(const For *op) {

    do_indent();
    stream << op->for_type << " (" << op->name << ", ";
    print(op->min);
    stream << ", ";
    print(op->extent);
    stream << ") {\n";

    indent += 2;
    print(op->body);
    indent -= 2;

    do_indent();
    stream << "}\n";
}

void IRPrinter::visit(const Store *op) {
    do_indent();
    stream << op->name << "[";
    print(op->index);
    stream << "] = ";
    print(op->value);
    stream << '\n';
}

void IRPrinter::visit(const Provide *op) {
    do_indent();
    stream << op->name << "(";
    for (size_t i = 0; i < op->args.size(); i++) {
        print(op->args[i]);
        if (i < op->args.size() - 1) stream << ", ";
    }
    stream << ") = ";
    if (op->values.size() > 1) {
        stream << "{";
    }
    for (size_t i = 0; i < op->values.size(); i++) {
        if (i > 0) {
            stream << ", ";
        }
        print(op->values[i]);
    }
    if (op->values.size() > 1) {
        stream << "}";
    }

    stream << '\n';
}

void IRPrinter::visit(const Allocate *op) {
    do_indent();
    stream << "allocate " << op->name << "[" << op->type;
    for (size_t i = 0; i < op->extents.size(); i++) {
        stream  << " * ";
        print(op->extents[i]);
    }
    stream << "]\n";
    print(op->body);
}

void IRPrinter::visit(const Free *op) {
    do_indent();
    stream << "free " << op->name << '\n';
}

void IRPrinter::visit(const Realize *op) {
    do_indent();
    stream << "realize " << op->name << "(";
    for (size_t i = 0; i < op->bounds.size(); i++) {
        stream << "[";
        print(op->bounds[i].min);
        stream << ", ";
        print(op->bounds[i].extent);
        stream << "]";
        if (i < op->bounds.size() - 1) stream << ", ";
    }
    stream << ") {\n";

    indent += 2;
    print(op->body);
    indent -= 2;

    do_indent();
    stream << "}\n";
}

void IRPrinter::visit(const Block *op) {
    print(op->first);
    if (op->rest.defined()) print(op->rest);
}

void IRPrinter::visit(const IfThenElse *op) {
    do_indent();
    stream << "if (" << op->condition << ") {\n";
    indent += 2;
    print(op->then_case);
    indent -= 2;

    if (op->else_case.defined()) {
        do_indent();
        stream << "} else {\n";
        indent += 2;
        print(op->else_case);
        indent -= 2;
    }

    do_indent();
    stream << "}\n";

}

void IRPrinter::visit(const Evaluate *op) {
    do_indent();
    print(op->value);
    stream << "\n";
}

}}
<commit_msg>Easier-to-read printing of extract_buffer_* intrinsics.<commit_after>#include <iostream>
#include <sstream>

#include "IRPrinter.h"
#include "IROperator.h"
#include "IR.h"

namespace Halide {

using std::ostream;
using std::vector;
using std::string;
using std::ostringstream;

ostream &operator<<(ostream &out, Type type) {
    switch (type.code) {
    case Type::Int:
        out << "int";
        break;
    case Type::UInt:
        out << "uint";
        break;
    case Type::Float:
        out << "float";
        break;
    case Type::Handle:
        out << "handle";
        break;
    }
    out << type.bits;
    if (type.width > 1) out << 'x' << type.width;
    return out;
}

ostream &operator<<(ostream &stream, Expr ir) {
    if (!ir.defined()) {
        stream << "(undefined)";
    } else {
        Internal::IRPrinter p(stream);
        p.print(ir);
    }
    return stream;
}

namespace Internal {

void IRPrinter::test() {
    Type i32 = Int(32);
    Type f32 = Float(32);
    Expr x = Variable::make(Int(32), "x");
    Expr y = Variable::make(Int(32), "y");
    ostringstream expr_source;
    expr_source << (x + 3) * (y / 2 + 17);
    assert(expr_source.str() == "((x + 3)*((y/2) + 17))");

    Stmt store = Store::make("buf", (x * 17) / (x - 3), y - 1);
    Stmt for_loop = For::make("x", -2, y + 2, For::Parallel, store);
    vector<Expr> args(1); args[0] = x % 3;
    Expr call = Call::make(i32, "buf", args, Call::Extern);
    Stmt store2 = Store::make("out", call + 1, x);
    Stmt for_loop2 = For::make("x", 0, y, For::Vectorized , store2);
    Stmt pipeline = Pipeline::make("buf", for_loop, Stmt(), for_loop2);
    Stmt assertion = AssertStmt::make(y > 3, "y is greater than %d", vec<Expr>(3));
    Stmt block = Block::make(assertion, pipeline);
    Stmt let_stmt = LetStmt::make("y", 17, block);
    Stmt allocate = Allocate::make("buf", f32, vec(Expr(1023)), let_stmt);

    ostringstream source;
    source << allocate;
    std::string correct_source = \
        "allocate buf[float32 * 1023]\n"
        "let y = 17\n"
        "assert((y > 3), \"y is greater than %d\", 3)\n"
        "produce buf {\n"
        "  parallel (x, -2, (y + 2)) {\n"
        "    buf[(y - 1)] = ((x*17)/(x - 3))\n"
        "  }\n"
        "}\n"
        "vectorized (x, 0, y) {\n"
        "  out[x] = (buf((x % 3)) + 1)\n"
        "}\n";

    if (source.str() != correct_source) {
        std::cout << "Correct output:\n" << correct_source;
        std::cout << "Actual output:\n" << source.str();
        assert(false);
    }
    std::cout << "IRPrinter test passed\n";
}

ostream &operator<<(ostream &out, const For::ForType &type) {
    switch (type) {
    case For::Serial:
        out << "for";
        break;
    case For::Parallel:
        out << "parallel";
        break;
    case For::Unrolled:
        out << "unrolled";
        break;
    case For::Vectorized:
        out << "vectorized";
        break;
    }
    return out;
}

ostream &operator<<(ostream &stream, Stmt ir) {
    if (!ir.defined()) {
        stream << "(undefined)\n";
    } else {
        Internal::IRPrinter p(stream);
        p.print(ir);
    }
    return stream;
}

IRPrinter::IRPrinter(ostream &s) : stream(s), indent(0) {
    s.setf(std::ios::fixed, std::ios::floatfield);
}

void IRPrinter::print(Expr ir) {
    ir.accept(this);
}

void IRPrinter::print(Stmt ir) {
    ir.accept(this);
}

void IRPrinter::do_indent() {
    for (int i = 0; i < indent; i++) stream << ' ';
}

void IRPrinter::visit(const IntImm *op) {
    stream << op->value;
}

void IRPrinter::visit(const FloatImm *op) {
    stream << op->value << 'f';
}

void IRPrinter::visit(const StringImm *op) {
    stream << '"';
    for (size_t i = 0; i < op->value.size(); i++) {
        unsigned char c = op->value[i];
        if (c >= ' ' && c <= '~' && c != '\\' && c != '"') {
            stream << c;
        } else {
            stream << '\\';
            switch (c) {
            case '"':
                stream << '"';
                break;
            case '\\':
                stream << '\\';
                break;
            case '\t':
                stream << 't';
                break;
            case '\r':
                stream << 'r';
                break;
            case '\n':
                stream << 'n';
                break;
            default:
                string hex_digits = "0123456789ABCDEF";
                stream << 'x' << hex_digits[c >> 4] << hex_digits[c & 0xf];
            }
        }
    }
    stream << '"';
}

void IRPrinter::visit(const Cast *op) {
    stream << op->type << '(';
    print(op->value);
    stream << ')';
}

void IRPrinter::visit(const Variable *op) {
    // omit the type
    // stream << op->name << "." << op->type;
    stream << op->name;
}

void IRPrinter::visit(const Add *op) {
    stream << '(';
    print(op->a);
    stream << " + ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Sub *op) {
    stream << '(';
    print(op->a);
    stream << " - ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Mul *op) {
    stream << '(';
    print(op->a);
    stream << "*";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Div *op) {
    stream << '(';
    print(op->a);
    stream << "/";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Mod *op) {
    stream << '(';
    print(op->a);
    stream << " % ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Min *op) {
    stream << "min(";
    print(op->a);
    stream << ", ";
    print(op->b);
    stream << ")";
}

void IRPrinter::visit(const Max *op) {
    stream << "max(";
    print(op->a);
    stream << ", ";
    print(op->b);
    stream << ")";
}

void IRPrinter::visit(const EQ *op) {
    stream << '(';
    print(op->a);
    stream << " == ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const NE *op) {
    stream << '(';
    print(op->a);
    stream << " != ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const LT *op) {
    stream << '(';
    print(op->a);
    stream << " < ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const LE *op) {
    stream << '(';
    print(op->a);
    stream << " <= ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const GT *op) {
    stream << '(';
    print(op->a);
    stream << " > ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const GE *op) {
    stream << '(';
    print(op->a);
    stream << " >= ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const And *op) {
    stream << '(';
    print(op->a);
    stream << " && ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Or *op) {
    stream << '(';
    print(op->a);
    stream << " || ";
    print(op->b);
    stream << ')';
}

void IRPrinter::visit(const Not *op) {
    stream << '!';
    print(op->a);
}

void IRPrinter::visit(const Select *op) {
    stream << "select(";
    print(op->condition);
    stream << ", ";
    print(op->true_value);
    stream << ", ";
    print(op->false_value);
    stream << ")";
}

void IRPrinter::visit(const Load *op) {
    stream << op->name << "[";
    print(op->index);
    stream << "]";
}

void IRPrinter::visit(const Ramp *op) {
    stream << "ramp(";
    print(op->base);
    stream << ", ";
    print(op->stride);
    stream << ", " << op->width << ")";
}

void IRPrinter::visit(const Broadcast *op) {
    stream << "x" << op->width << "(";
    print(op->value);
    stream << ")";
}

void IRPrinter::visit(const Call *op) {
    // Special-case some intrinsics for readability
    if (op->call_type == Call::Intrinsic) {
        if (op->name == Call::extract_buffer_min) {
            print(op->args[0]);
            stream << ".min[" << op->args[1] << "]";
            return;
        } else if (op->name == Call::extract_buffer_extent) {
            print(op->args[0]);
            stream << ".extent[" << op->args[1] << "]";
            return;
        }
    }

    stream << op->name << "(";
    for (size_t i = 0; i < op->args.size(); i++) {
        print(op->args[i]);
        if (i < op->args.size() - 1) {
            stream << ", ";
        }
    }
    stream << ")";
}

void IRPrinter::visit(const Let *op) {
    stream << "(let " << op->name << " = ";
    print(op->value);
    stream << " in ";
    print(op->body);
    stream << ")";
}

void IRPrinter::visit(const LetStmt *op) {
    do_indent();
    stream << "let " << op->name << " = ";
    print(op->value);
    stream << '\n';

    print(op->body);
}

void IRPrinter::visit(const AssertStmt *op) {
    do_indent();
    stream << "assert(";
    print(op->condition);
    stream << ", " << '"' << op->message << '"';
    for (size_t i = 0; i < op->args.size(); i++) {
        stream << ", ";
        print(op->args[i]);
    }
    stream << ")\n";
}

void IRPrinter::visit(const Pipeline *op) {

    do_indent();
    stream << "produce " << op->name << " {\n";
    indent += 2;
    print(op->produce);
    indent -= 2;

    if (op->update.defined()) {
        do_indent();
        stream << "} update " << op->name << " {\n";
        indent += 2;
        print(op->update);
        indent -= 2;
    }

    do_indent();
    stream << "}\n";

    print(op->consume);

}

void IRPrinter::visit(const For *op) {

    do_indent();
    stream << op->for_type << " (" << op->name << ", ";
    print(op->min);
    stream << ", ";
    print(op->extent);
    stream << ") {\n";

    indent += 2;
    print(op->body);
    indent -= 2;

    do_indent();
    stream << "}\n";
}

void IRPrinter::visit(const Store *op) {
    do_indent();
    stream << op->name << "[";
    print(op->index);
    stream << "] = ";
    print(op->value);
    stream << '\n';
}

void IRPrinter::visit(const Provide *op) {
    do_indent();
    stream << op->name << "(";
    for (size_t i = 0; i < op->args.size(); i++) {
        print(op->args[i]);
        if (i < op->args.size() - 1) stream << ", ";
    }
    stream << ") = ";
    if (op->values.size() > 1) {
        stream << "{";
    }
    for (size_t i = 0; i < op->values.size(); i++) {
        if (i > 0) {
            stream << ", ";
        }
        print(op->values[i]);
    }
    if (op->values.size() > 1) {
        stream << "}";
    }

    stream << '\n';
}

void IRPrinter::visit(const Allocate *op) {
    do_indent();
    stream << "allocate " << op->name << "[" << op->type;
    for (size_t i = 0; i < op->extents.size(); i++) {
        stream  << " * ";
        print(op->extents[i]);
    }
    stream << "]\n";
    print(op->body);
}

void IRPrinter::visit(const Free *op) {
    do_indent();
    stream << "free " << op->name << '\n';
}

void IRPrinter::visit(const Realize *op) {
    do_indent();
    stream << "realize " << op->name << "(";
    for (size_t i = 0; i < op->bounds.size(); i++) {
        stream << "[";
        print(op->bounds[i].min);
        stream << ", ";
        print(op->bounds[i].extent);
        stream << "]";
        if (i < op->bounds.size() - 1) stream << ", ";
    }
    stream << ") {\n";

    indent += 2;
    print(op->body);
    indent -= 2;

    do_indent();
    stream << "}\n";
}

void IRPrinter::visit(const Block *op) {
    print(op->first);
    if (op->rest.defined()) print(op->rest);
}

void IRPrinter::visit(const IfThenElse *op) {
    do_indent();
    stream << "if (" << op->condition << ") {\n";
    indent += 2;
    print(op->then_case);
    indent -= 2;

    if (op->else_case.defined()) {
        do_indent();
        stream << "} else {\n";
        indent += 2;
        print(op->else_case);
        indent -= 2;
    }

    do_indent();
    stream << "}\n";

}

void IRPrinter::visit(const Evaluate *op) {
    do_indent();
    print(op->value);
    stream << "\n";
}

}}
<|endoftext|>


<commit_before>/*
LidarLite - C++ wrapper for LIDAR Lite interfacing with RaspberryPi
Created by Produce Consume Robot 2015.
http://produceconsumerobot.com/

This work is licensed under the Creative Commons
Attribution-ShareAlike 3.0 Unported License.
To view a copy of this license, visit http://creativecommons.org/licenses/by-sa/3.0/.

See LIDAR Lite documentation for more info
http://kb.pulsedlight3d.com/support/solutions/articles/5000549565-detailed-register-descriptions-external
*/

#pragma once

#include "LidarLite.hpp"
#include <sstream>

//--------------------------------------------------------------
LidarLite::LidarLite() {
	_fd = -1;

	// initialize the LidarLite
	_fd = wiringPiI2CSetup(ADRS_LIDAR_LITE);
	if (_fd > -1) {
		getStatus();  // Dummy request to wake up device
		usleep(100000);
	}

	return;
}

//--------------------------------------------------------------
bool LidarLite::isInitialized() {
	if (_fd > -1) {
		return true;
	}
	return false;
}

//--------------------------------------------------------------
int LidarLite::getStatus() {
	// return the status register result
	return wiringPiI2CReadReg8(_fd, REG_STATUS) ;
}

//--------------------------------------------------------------
int LidarLite::readDistance() {
	int hiVal, loVal, i=0;
	unsigned char val;

	unsigned char nackack = 100; // Setup variable to hold ACK/NACK resopnses

	// trigger the measure lidar lite to measure a value until an ack is received
	while (nackack != 0) {
		nackack = wiringPiI2CWriteReg8(_fd, REG_MEASURE, VAL_MEASURE);
		usleep(1000);
		//delay(1);
		if (++i > MAX_TRIES) return -1; // after many tries return an error
	}

	// Get the low byte, return -1 if error occurred
	if (readByte(_fd, REG_LO_DISTANCE, false, &val) == ERROR_READ) return -1;
	loVal = val;
	if (readByte(_fd, REG_HI_DISTANCE, true, &val) == ERROR_READ) return -1;
	hiVal = val;

	return ( (hiVal << 8) + loVal);
	//return lidar_read(_fd);
}

//--------------------------------------------------------------
int LidarLite::getHardwareVersion() {
	unsigned char val;

	// return zero if we got an error reading
	if (readByte(_fd, REG_VERSION, false, &val) == ERROR_READ) return -1;

	// otherwise return the value
	return (int) val;

	//return lidar_version(_fd);
}

//--------------------------------------------------------------
std::string LidarLite::getStatusString(int status) {
	std::stringstream out;

	unsigned char stat = (unsigned char) status;

	// Print the hex status byte
	out << "STATUS BYTE: 0x";
	// add a zero if necessary to maintain correct formatting
	if ((int) stat < 10) out << "0";
	out << (int) stat;

	if (stat & STATUS_BUSY) out << " busy";
	if (stat & STATUS_REFERENCE_OVERFLOW) out << " reference overflow";
	if (stat & STATUS_SIGNAL_OVERFLOW) out << " signal overflow";
	if (stat & STATUS_PIN) out << " mode select pin";
	if (stat & STATUS_SECOND_PEAK) out << " second peak";
	if (stat & STATUS_TIMESTAMP) out << " active between pairs";
	if (stat & STATUS_SIGNAL_INVALID) out << " no signal";
	if (stat & STATUS_EYE_SAFETY_ON) out << " eye safety";

	return out.str();
}

//--------------------------------------------------------------
int LidarLite::readByte(int fd, int reg,  bool allowZero, unsigned char *val) {
	int i=0, tempVal;

	usleep(1000);
	//delay(1); // ms
	while (true) {
		// read from the register
		tempVal = wiringPiI2CReadReg8(fd, reg);

		// check for error conditions and try again
		if (tempVal == ERROR_READ || (tempVal==0 && !allowZero) ) {
			usleep(1000);
			//delay (1) ;		// ms
			*val = tempVal;
			if (++i > 100) return ERROR_READ; // after many tries return an error
		} else {
			// success!
			*val = tempVal;
			return 0;
		}
	}
}

	<commit_msg>Made some alterations to clean up code.<commit_after>/*
LidarLite - C++ wrapper for LIDAR Lite interfacing with RaspberryPi
Created by Produce Consume Robot 2015.
http://produceconsumerobot.com/

This work is licensed under the Creative Commons
Attribution-ShareAlike 3.0 Unported License.
To view a copy of this license, visit http://creativecommons.org/licenses/by-sa/3.0/.

See LIDAR Lite documentation for more info
http://kb.pulsedlight3d.com/support/solutions/articles/5000549565-detailed-register-descriptions-external
*/

#pragma once

#include "LidarLite.hpp"
#include <sstream>

//--------------------------------------------------------------
LidarLite::LidarLite() {
	_fd = -1;

	// initialize the LidarLite
	_fd = wiringPiI2CSetup(ADRS_LIDAR_LITE);
	if (_fd > -1) {
		getStatus();  // Dummy request to wake up device
		usleep(100000);
	}

	return;
}

//--------------------------------------------------------------
bool LidarLite::isInitialized() {
	if (_fd > -1) {
		return true;
	}
	return false;
}

//--------------------------------------------------------------
int LidarLite::getStatus() {
	// return the status register result
	return wiringPiI2CReadReg8(_fd, REG_STATUS) ;
}

//--------------------------------------------------------------
int LidarLite::readDistance() {
	int hiVal, loVal, i=0;

	unsigned char nackack = 100; // Setup variable to hold ACK/NACK resopnses

	// trigger the measure lidar lite to measure a value until an ack is received
	while (nackack != 0) {
		nackack = wiringPiI2CWriteReg8(_fd, REG_MEASURE, VAL_MEASURE);
		usleep(1000);
		//delay(1);
		if (++i > MAX_TRIES) return ERROR_READ; // after many tries return an error
	}

	// Get the low byte, return -1 if error occurred
	loVal = readByte(_fd, REG_LO_DISTANCE, false);
	if (loVal == ERROR_READ) return ERROR_READ;

	// Get the high byte, return -1 if error occurred
	hiVal = readByte(_fd, REG_HI_DISTANCE, false);
	if (hiVal == ERROR_READ) return ERROR_READ;

	return ( (hiVal << 8) + loVal);
	//return lidar_read(_fd);
}

//--------------------------------------------------------------
int LidarLite::getHardwareVersion() {
	return readByte(_fd, REG_VERSION, false);
}

//--------------------------------------------------------------
std::string LidarLite::getStatusString(int status) {
	std::stringstream out;

	if (status == -1) {
		out << "STATUS: -1 error";
	} else {
		unsigned char stat = (unsigned char) status;

		// Print the hex status byte
		out << "STATUS BYTE: 0x";
		// add a zero if necessary to maintain correct formatting
		if ((int) stat < 10) out << "0";
		out << (int) stat;

		if (stat & STATUS_BUSY) out << " busy";
		if (stat & STATUS_REFERENCE_OVERFLOW) out << " reference overflow";
		if (stat & STATUS_SIGNAL_OVERFLOW) out << " signal overflow";
		if (stat & STATUS_PIN) out << " mode select pin";
		if (stat & STATUS_SECOND_PEAK) out << " second peak";
		if (stat & STATUS_TIMESTAMP) out << " active between pairs";
		if (stat & STATUS_SIGNAL_INVALID) out << " no signal";
		if (stat & STATUS_EYE_SAFETY_ON) out << " eye safety";

		return out.str();
	}
}

//--------------------------------------------------------------
int LidarLite::readByte(int fd, int reg,  bool allowZero) {
	int i=0, tempVal;

	usleep(1000);
	//delay(1); // ms
	while (true) {
		// read from the register
		tempVal = wiringPiI2CReadReg8(fd, reg);

		// check for error conditions and try again
		if (tempVal == ERROR_READ || (tempVal==0 && !allowZero) ) {
			usleep(1000);
			//delay (1) ;		// ms
			if (++i > 100) return ERROR_READ; // after many tries return an error
		} else {
			// success!
			return tempVal;
		}
	}
}

	<|endoftext|>


<commit_before>#include <stdio.h>
#ifdef WIN32
#include <windows.h>
#endif
#include <GL/glut.h>
#include "RayTracer.hpp"

//temporary variables
Vec3Df testRayOrigin;
Vec3Df testRayDestination;
string input;

//use this function for any preprocessing of the mesh.
void init(int argc, char **argv){
    //load the mesh file
    //feel free to replace cube by a path to another model
    //please realize that not all OBJ files will successfully load.
    //Nonetheless, if they come from Blender, they should.
    //there is a difference between windows written objs and Linux written objs.
    //hence, some models might not yet work well.

    RayTracingResolutionX = 1000; // These resolutions should be the same as the window,
    RayTracingResolutionY = 1000; // otherwise unexpected behaviour occurs.

    if(argc == 2){
        input = argv[1];
    }else{
        cout << "Mesh file name: (0: cube, 1: monkey, 2: dodgeColorTest)"
                << endl << "You can omit the mesh/ path and the .obj extension."
                << endl;
        cin >> input;
    }

	if (input == "0")
		input = "cube";
	else if (input == "1")
	input = "monkey";
	else if (input == "2")
		input = "dodgeColorTest";
	input = string("mesh/").append(input).append(".obj");
    MyMesh.loadMesh(input.c_str(), true);

    MyMesh.computeVertexNormals();

    //one first move: initialize the first light source
    //at least ONE light source has to be in the scene!!!
    //here, we set it to the current location of the camera
    MyLightPositions.push_back(MyCameraPosition);
}

#define dot Vec3Df::dotProduct
#define cross Vec3Df::crossProduct

#define v0 t.vertices[0].p
#define v1 t.vertices[1].p
#define v2 t.vertices[2].p

#define rayOrig ray.orig
#define rayDir ray.dir

/** Calculate intersection between triangle and ray */
float intersect(const Triangle& t, const Ray& ray){

	float angle = dot(t.normal, rayDir); // The cosine of the angle of the vectors (dotproduct of the vectors).

	/* Floats are only rarely exactly 0, are you sure this is correct? */
	if (angle == 0) // If the ray and the plane are parallel (so their angle is 0), they won't intersect.
		return 10e6f;

	float rayD = -(dot(t.normal, rayOrig) + dot(t.normal, v0)) / angle; // The distance of the ray's origin
	// to the plane in which lies the triangle.

	Vec3Df rayHit = rayOrig + rayD * rayDir; // The intersection point of the ray and the plane in which lies the triangle.

	if (dot(t.normal, cross(v1 - v0, rayHit - v0)) < 0)
		return 10e6f;
	if (dot(t.normal, cross(v2 - v1, rayHit - v1)) < 0)
		return 10e6f;
	if (dot(t.normal, cross(v0 - v2, rayHit - v2)) < 0)
		return 10e6f;

	return rayD;
}

//return the color of your pixel.
const Vec3Df& performRayTracing(const Vec3Df & origin, const Vec3Df & dest){
    Vec3Df color = Vec3Df(0, 1, 0);
    Ray ray = Ray(color, origin, dest);

	/* Actual ray tracing code */
	float hit = 10e6f;
	unsigned int amountOfTriangles = MyMesh.triangles.size();
	for (unsigned int i = 0; i < amountOfTriangles; i++){
		float ins = intersect(MyMesh.triangles[i], ray);
		if (ins < hit && ins > 0)
			hit = ins;
	}
	//hit = 1 / ((hit * 2) + 1); // Arithmetic function for getting a usable color.
	ray.setColor(Vec3Df(hit, hit / 5, hit * 5));

	return Vec3Df(hit, hit / 5, hit * 5);
}

void yourDebugDraw(){
    //draw open gl debug stuff
    //this function is called every frame

    //as an example:
    glPushAttrib(GL_ALL_ATTRIB_BITS);
    glDisable(GL_LIGHTING);
    glColor3f(1, 0, 1);
    glBegin(GL_LINES);
    glVertex3f(testRayOrigin[0], testRayOrigin[1], testRayOrigin[2]);
    glVertex3f(testRayDestination[0], testRayDestination[1],
            testRayDestination[2]);
    glEnd();
    glPointSize(10);
    glBegin(GL_POINTS);
    glVertex3fv(MyLightPositions[0].pointer());
    glEnd();
    glPopAttrib();

}

void yourKeyboardFunc(char t, int x, int y){
    // do what you want with the keyboard input t.
    // x, y are the screen position

    //here I use it to get the coordinates of a ray, which I then draw in the debug function.
    produceRay(x, y, testRayOrigin, testRayDestination);

    std::cout << t << " pressed! The mouse was in location " << x << "," << y
            << "!" << std::endl;
}
<commit_msg>Placed "out of range" value in intersect to preprocessor define.<commit_after>#include <stdio.h>
#ifdef WIN32
#include <windows.h>
#endif
#include <GL/glut.h>
#include "RayTracer.hpp"

//temporary variables
Vec3Df testRayOrigin;
Vec3Df testRayDestination;
string input;

//use this function for any preprocessing of the mesh.
void init(int argc, char **argv){
    //load the mesh file
    //feel free to replace cube by a path to another model
    //please realize that not all OBJ files will successfully load.
    //Nonetheless, if they come from Blender, they should.
    //there is a difference between windows written objs and Linux written objs.
    //hence, some models might not yet work well.

    RayTracingResolutionX = 1000; // These resolutions should be the same as the window,
    RayTracingResolutionY = 1000; // otherwise unexpected behaviour occurs.

    if(argc == 2){
        input = argv[1];
    }else{
        cout << "Mesh file name: (0: cube, 1: monkey, 2: dodgeColorTest)"
                << endl << "You can omit the mesh/ path and the .obj extension."
                << endl;
        cin >> input;
    }

	if (input == "0")
		input = "cube";
	else if (input == "1")
	input = "monkey";
	else if (input == "2")
		input = "dodgeColorTest";
	input = string("mesh/").append(input).append(".obj");
    MyMesh.loadMesh(input.c_str(), true);

    MyMesh.computeVertexNormals();

    //one first move: initialize the first light source
    //at least ONE light source has to be in the scene!!!
    //here, we set it to the current location of the camera
    MyLightPositions.push_back(MyCameraPosition);
}

#define dot Vec3Df::dotProduct
#define cross Vec3Df::crossProduct

#define v0 t.vertices[0].p
#define v1 t.vertices[1].p
#define v2 t.vertices[2].p

#define rayOrig ray.orig
#define rayDir ray.dir
#define VEWY_HIGH 10e6f

/** Calculate intersection between triangle and ray */
float intersect(const Triangle& t, const Ray& ray){

	float angle = dot(t.normal, rayDir); // The cosine of the angle of the vectors (dotproduct of the vectors).

	/* Floats are only rarely exactly 0, are you sure this is correct? */
	if (angle == 0) // If the ray and the plane are parallel (so their angle is 0), they won't intersect.
		return VEWY_HIGH;

	float rayD = -(dot(t.normal, rayOrig) + dot(t.normal, v0)) / angle; // The distance of the ray's origin
	// to the plane in which lies the triangle.

	Vec3Df rayHit = rayOrig + rayD * rayDir; // The intersection point of the ray and the plane in which lies the triangle.

	if (dot(t.normal, cross(v1 - v0, rayHit - v0)) < 0)
		return VEWY_HIGH;
	if (dot(t.normal, cross(v2 - v1, rayHit - v1)) < 0)
		return VEWY_HIGH;
	if (dot(t.normal, cross(v0 - v2, rayHit - v2)) < 0)
		return VEWY_HIGH;

	return rayD;
}

//return the color of your pixel.
const Vec3Df& performRayTracing(const Vec3Df & origin, const Vec3Df & dest){
    Vec3Df color = Vec3Df(0, 1, 0);
    Ray ray = Ray(color, origin, dest);

	/* Actual ray tracing code */
	float hit = 10e6f;
	unsigned int amountOfTriangles = MyMesh.triangles.size();
	for (unsigned int i = 0; i < amountOfTriangles; i++){
		float ins = intersect(MyMesh.triangles[i], ray);
		if (ins < hit && ins > 0)
			hit = ins;
	}
	//hit = 1 / ((hit * 2) + 1); // Arithmetic function for getting a usable color.
	ray.setColor(Vec3Df(hit, hit / 5, hit * 5));

	return Vec3Df(hit, hit / 5, hit * 5);
}

void yourDebugDraw(){
    //draw open gl debug stuff
    //this function is called every frame

    //as an example:
    glPushAttrib(GL_ALL_ATTRIB_BITS);
    glDisable(GL_LIGHTING);
    glColor3f(1, 0, 1);
    glBegin(GL_LINES);
    glVertex3f(testRayOrigin[0], testRayOrigin[1], testRayOrigin[2]);
    glVertex3f(testRayDestination[0], testRayDestination[1],
            testRayDestination[2]);
    glEnd();
    glPointSize(10);
    glBegin(GL_POINTS);
    glVertex3fv(MyLightPositions[0].pointer());
    glEnd();
    glPopAttrib();

}

void yourKeyboardFunc(char t, int x, int y){
    // do what you want with the keyboard input t.
    // x, y are the screen position

    //here I use it to get the coordinates of a ray, which I then draw in the debug function.
    produceRay(x, y, testRayOrigin, testRayDestination);

    std::cout << t << " pressed! The mouse was in location " << x << "," << y
            << "!" << std::endl;
}
<|endoftext|>


<commit_before>// Conversion of words to word indices and vice versa.
#include <errno.h>
#include "misc/io.hh"
#include "Vocabulary.hh"
#include "misc/str.hh"

using namespace std;
Vocabulary::Vocabulary()
{
  m_words.push_back("<UNK>");
  m_indices["<UNK>"] = 0;
}

void Vocabulary::reset()
{
	clear_words();
	m_words.push_back("<UNK>");
	m_indices["<UNK>"] = 0;
}

int
Vocabulary::add_word(const std::string &word)
{
  const vocabmap::iterator i = m_indices.find(word);
  if (i != m_indices.end())
    return (*i).second;

  int index = m_words.size();
  m_indices[word] = index;
  m_words.push_back(word);
  return index;
}

void
Vocabulary::set_oov(const std::string &word)
{
  clear_words();
  m_words.push_back(word);
  m_indices[word] = 0;
}

void
Vocabulary::read(FILE *file)
{
  std::string word;

  while (str::read_line(word, file, true)) {

    // Remove comments
    int comment = word.find('#');
    if (comment >= 0)
      word = word.substr(0, comment);

    // Remove leading and trailing spaces.  Skip if word is just
    // spaces.
    int start = word.find_first_not_of("\t\n\r ");
    if (start < 0)
      continue;
    int end = word.find_last_not_of("\t\n\r ");

    // Check if " " or "(" is present and truncate the word to
    // that symbol. You can use dictionary files in CMUdict
    // and Decoder formats this way.
    int end_2 = word.find_first_of(" (")-1;
    if ((end_2>-1) and (end_2<end))
      end=end_2;
    word = word.substr(start, end - start + 1);

    if (word.compare("_")==0 or word.compare("__")==0)
      continue;

    // Insert word
    add_word(word);
  }
}

void
Vocabulary::read(const std::string &filename)
{
  io::Stream file(filename, "r");
  read(file.file);
}

void
Vocabulary::write(FILE *file) const
{
  for (unsigned int i = 1; i < m_words.size(); i++)
    fprintf(file, "%s\n", m_words[i].c_str());
}

void
Vocabulary::clear_words()
{
  m_indices.clear();
  m_words.clear();
}
<commit_msg>*** empty log message ***<commit_after>// Conversion of words to word indices and vice versa.
#include <errno.h>
#include "misc/io.hh"
#include "Vocabulary.hh"
#include "misc/str.hh"

Vocabulary::Vocabulary()
{
  m_words.push_back("<UNK>");
  m_indices["<UNK>"] = 0;
}

void Vocabulary::reset()
{
	clear_words();
	m_words.push_back("<UNK>");
	m_indices["<UNK>"] = 0;
}

int
Vocabulary::add_word(const std::string &word)
{
  const vocabmap::iterator i = m_indices.find(word);
  if (i != m_indices.end())
    return (*i).second;

  int index = m_words.size();
  m_indices[word] = index;
  m_words.push_back(word);
  return index;
}

void
Vocabulary::set_oov(const std::string &word)
{
  clear_words();
  m_words.push_back(word);
  m_indices[word] = 0;
}

void
Vocabulary::read(FILE *file)
{
  std::string word;

  while (str::read_line(word, file, true)) {

    // Remove comments
    int comment = word.find('#');
    if (comment >= 0)
      word = word.substr(0, comment);

    // Remove leading and trailing spaces.  Skip if word is just
    // spaces.
    int start = word.find_first_not_of("\t\n\r ");
    if (start < 0)
      continue;
    int end = word.find_last_not_of("\t\n\r ");

    // Check if " " or "(" is present and truncate the word to
    // that symbol. You can use dictionary files in CMUdict
    // and Decoder formats this way.
    int end_2 = word.find_first_of(" (")-1;
    if ((end_2>-1) and (end_2<end))
      end=end_2;
    word = word.substr(start, end - start + 1);

    if (word.compare("_")==0 or word.compare("__")==0)
      continue;

    // Insert word
    add_word(word);
  }
}

void
Vocabulary::read(const std::string &filename)
{
  io::Stream file(filename, "r");
  read(file.file);
}

void
Vocabulary::write(FILE *file) const
{
  for (unsigned int i = 1; i < m_words.size(); i++)
    fprintf(file, "%s\n", m_words[i].c_str());
}

void
Vocabulary::clear_words()
{
  m_indices.clear();
  m_words.clear();
}
<|endoftext|>


<commit_before>#include "algorithm8.hpp"
#include <unordered_set>
#include <distributions/random.hpp>
#include <distributions/vector_math.hpp>
#include <distributions/trivial_hash.hpp>

#define LOOM_ASSERT_CLOSE(x, y) LOOM_ASSERT_LT(fabs((x) - (y)), 1e-4)

namespace loom
{

void Algorithm8::clear ()
{
    model.clear();
    kinds.clear();
}

void Algorithm8::model_load (CrossCat &)
{
    TODO("load model");
}

void Algorithm8::mixture_init_empty (rng_t & rng, size_t kind_count)
{
    LOOM_ASSERT_LT(0, kind_count);
    kinds.resize(kind_count);
    for (auto & kind : kinds) {
        kind.mixture.init_empty(model, rng);
    }
}

//----------------------------------------------------------------------------
// Block Pitman-Yor Sampler
//
// This sampler follows the math in
// $DISTRIBUTIONS_PATH/src/clustering.hpp
// distributions::Clustering<int>::PitmanYor::sample_assignments(...)

class Algorithm8::BlockPitmanYorSampler
{
public:

    BlockPitmanYorSampler (
            const distributions::Clustering<int>::PitmanYor & clustering,
            const std::vector<VectorFloat> & likelihoods,
            std::vector<uint32_t> & assignments);

    void run (size_t iterations, rng_t & rng);

private:

    void validate () const;

    float get_likelihood_empty () const;

    static float compute_posterior (
            const VectorFloat & prior_in,
            const VectorFloat & likelihood_in,
            VectorFloat & posterior_out);

    const float alpha_;
    const float d_;
    const size_t feature_count_;
    const size_t kind_count_;
    size_t empty_kind_count_;
    const std::vector<VectorFloat> & likelihoods_;
    std::vector<uint32_t> & assignments_;
    std::vector<uint32_t> counts_;
    std::unordered_set<uint32_t, distributions::TrivialHash<uint32_t>>
        empty_kinds_;
    VectorFloat prior_;
    VectorFloat posterior_;
};

Algorithm8::BlockPitmanYorSampler::BlockPitmanYorSampler (
        const distributions::Clustering<int>::PitmanYor & clustering,
        const std::vector<VectorFloat> & likelihoods,
        std::vector<uint32_t> & assignments) :
    alpha_(clustering.alpha),
    d_(clustering.d),
    feature_count_(likelihoods.size()),
    kind_count_(likelihoods[0].size()),
    empty_kind_count_(0),
    likelihoods_(likelihoods),
    assignments_(assignments),
    counts_(kind_count_, 0),
    prior_(kind_count_),
    posterior_(kind_count_)
{
    LOOM_ASSERT_LT(0, likelihoods.size());
    LOOM_ASSERT_EQ(likelihoods.size(), assignments.size());

    for (size_t f = 0; f < feature_count_; ++f) {
        size_t k = assignments[f];
        ++counts_[k];
    }

    for (size_t k = 0; k < kind_count_; ++k) {
        if (counts_[k] == 0) {
            empty_kinds_.insert(k);
            ++empty_kind_count_;
        }
    }

    const float likelihood_empty = get_likelihood_empty();
    for (size_t k = 0; k < kind_count_; ++k) {
        if (auto count = counts_[k]) {
            prior_[k] = count - d_;
        } else {
            prior_[k] = likelihood_empty;
        }
    }
}

inline void Algorithm8::BlockPitmanYorSampler::validate () const
{
    const float likelihood_empty = get_likelihood_empty();
    for (size_t k = 0; k < kind_count_; ++k) {
        if (auto count = counts_[k]) {
            LOOM_ASSERT_CLOSE(prior_[k], count - d_);
        } else {
            LOOM_ASSERT_CLOSE(prior_[k], likelihood_empty);
        }
    }
}

inline float Algorithm8::BlockPitmanYorSampler::get_likelihood_empty () const
{
    if (empty_kind_count_) {
        float nonempty_kind_count = kind_count_ - empty_kind_count_;
        return (alpha_ + d_ * nonempty_kind_count) / empty_kind_count_;
    } else {
        return 0.f;
    }
}

inline float Algorithm8::BlockPitmanYorSampler::compute_posterior (
        const VectorFloat & prior_in,
        const VectorFloat & likelihood_in,
        VectorFloat & posterior_out)
{
    const size_t size = prior_in.size();
    const float * __restrict__ prior =
        DIST_ASSUME_ALIGNED(prior_in.data());
    const float * __restrict__ likelihood =
        DIST_ASSUME_ALIGNED(likelihood_in.data());
    float * __restrict__ posterior =
        DIST_ASSUME_ALIGNED(posterior_out.data());

    float total = 0;
    for (size_t i = 0; i < size; ++i) {
        total += posterior[i] = prior[i] * likelihood[i];
    }
    return total;
}

using distributions::sample_from_likelihoods;

void Algorithm8::BlockPitmanYorSampler::run (
        size_t iterations,
        rng_t & rng)
{
    LOOM_ASSERT_LT(0, iterations);

    for (size_t i = 0; i < iterations; ++i) {
        for (size_t f = 0; f < feature_count_; ++f) {

            const VectorFloat & likelihood = likelihoods_[f];
            float total = compute_posterior(prior_, likelihood, posterior_);
            size_t new_k = sample_from_likelihoods(rng, posterior_, total);
            size_t old_k = assignments_[f];
            if (LOOM_UNLIKELY(new_k != old_k)) {
                assignments_[f] = new_k;

                size_t old_empty_kind_count = empty_kind_count_;
                const float old_likelihood_empty = get_likelihood_empty();
                if (--counts_[old_k] == 0) {
                    prior_[old_k] = old_likelihood_empty;
                    empty_kinds_.insert(old_k);
                    ++empty_kind_count_;
                } else {
                    prior_[old_k] = counts_[old_k] - d_;
                }
                if (counts_[new_k]++ == 0) {
                    empty_kinds_.erase(new_k);
                    --empty_kind_count_;
                }
                prior_[new_k] = counts_[new_k] - d_;

                size_t new_empty_kind_count = empty_kind_count_;
                if (new_empty_kind_count != old_empty_kind_count) {
                    const float likelihood_empty = get_likelihood_empty();
                    for (auto k : empty_kinds_) {
                        prior_[k] = likelihood_empty;
                    }
                }
            }

            if (LOOM_DEBUG_LEVEL >= 3) {
                validate();
            }
        }
    }
}

void Algorithm8::infer_assignments (
        std::vector<uint32_t> & featureid_to_kindid,
        size_t iterations,
        rng_t & rng) const
{
    LOOM_ASSERT_LT(0, iterations);

    const size_t feature_count = featureid_to_kindid.size();
    const size_t kind_count = kinds.size();
    std::vector<VectorFloat> likelihoods(feature_count);

    #pragma omp parallel for schedule(dynamic, 1)
    for (size_t featureid = 0; featureid < feature_count; ++featureid) {
        VectorFloat & scores = likelihoods[featureid];
        scores.resize(kind_count);
        for (size_t kindid = 0; kindid < feature_count; ++kindid) {
            const auto & mixture = kinds[kindid].mixture;
            scores[kindid] = mixture.score_feature(model, featureid, rng);
        }
        distributions::scores_to_likelihoods(scores);
    }

    BlockPitmanYorSampler sampler(
            model.clustering,
            likelihoods,
            featureid_to_kindid);

    sampler.run(iterations, rng);
}

} // namespace loom
<commit_msg>Add more validation to BlockPitmanYorSampler<commit_after>#include "algorithm8.hpp"
#include <unordered_set>
#include <distributions/random.hpp>
#include <distributions/vector_math.hpp>
#include <distributions/trivial_hash.hpp>

#define LOOM_ASSERT_CLOSE(x, y) \
    LOOM_ASSERT_LT(fabs((x) - (y)) / ((x) + (y) + 1e-20), 1e-4)

namespace loom
{

void Algorithm8::clear ()
{
    model.clear();
    kinds.clear();
}

void Algorithm8::model_load (CrossCat &)
{
    TODO("load model");
}

void Algorithm8::mixture_init_empty (rng_t & rng, size_t kind_count)
{
    LOOM_ASSERT_LT(0, kind_count);
    kinds.resize(kind_count);
    for (auto & kind : kinds) {
        kind.mixture.init_empty(model, rng);
    }
}

//----------------------------------------------------------------------------
// Block Pitman-Yor Sampler
//
// This sampler follows the math in
// $DISTRIBUTIONS_PATH/src/clustering.hpp
// distributions::Clustering<int>::PitmanYor::sample_assignments(...)

class Algorithm8::BlockPitmanYorSampler
{
public:

    BlockPitmanYorSampler (
            const distributions::Clustering<int>::PitmanYor & clustering,
            const std::vector<VectorFloat> & likelihoods,
            std::vector<uint32_t> & assignments);

    void run (size_t iterations, rng_t & rng);

private:

    void validate () const;

    float get_likelihood_empty () const;
    std::vector<uint32_t> get_counts () const;
    VectorFloat get_prior () const;

    static float compute_posterior (
            const VectorFloat & prior_in,
            const VectorFloat & likelihood_in,
            VectorFloat & posterior_out);

    const float alpha_;
    const float d_;
    const size_t feature_count_;
    const size_t kind_count_;
    size_t empty_kind_count_;
    const std::vector<VectorFloat> & likelihoods_;
    std::vector<uint32_t> & assignments_;
    std::vector<uint32_t> counts_;
    std::unordered_set<uint32_t, distributions::TrivialHash<uint32_t>>
        empty_kinds_;
    VectorFloat prior_;
    VectorFloat posterior_;
};

Algorithm8::BlockPitmanYorSampler::BlockPitmanYorSampler (
        const distributions::Clustering<int>::PitmanYor & clustering,
        const std::vector<VectorFloat> & likelihoods,
        std::vector<uint32_t> & assignments) :
    alpha_(clustering.alpha),
    d_(clustering.d),
    feature_count_(likelihoods.size()),
    kind_count_(likelihoods[0].size()),
    empty_kind_count_(0),
    likelihoods_(likelihoods),
    assignments_(assignments),
    counts_(get_counts()),
    empty_kinds_(),
    prior_(get_prior()),
    posterior_(kind_count_)
{
    LOOM_ASSERT_LT(0, likelihoods.size());
    LOOM_ASSERT_EQ(likelihoods.size(), assignments.size());

    for (size_t k = 0; k < kind_count_; ++k) {
        if (counts_[k] == 0) {
            empty_kinds_.insert(k);
            ++empty_kind_count_;
        }
    }
}

inline std::vector<uint32_t>
Algorithm8::BlockPitmanYorSampler::get_counts () const
{
    std::vector<uint32_t> counts(kind_count_, 0);
    for (size_t f = 0; f < feature_count_; ++f) {
        size_t k = assignments_[f];
        ++counts[k];
    }
    return counts;
}

inline VectorFloat Algorithm8::BlockPitmanYorSampler::get_prior () const
{
    VectorFloat prior(kind_count_);
    const float likelihood_empty = get_likelihood_empty();
    for (size_t k = 0; k < kind_count_; ++k) {
        if (auto count = counts_[k]) {
            prior[k] = count - d_;
        } else {
            prior[k] = likelihood_empty;
        }
    }
    return prior;
}

inline void Algorithm8::BlockPitmanYorSampler::validate () const
{
    std::vector<uint32_t> expected_counts = get_counts();
    for (size_t k = 0; k < kind_count_; ++k) {
        LOOM_ASSERT_EQ(counts_[k], expected_counts[k]);
    }

    VectorFloat expected_prior = get_prior();
    for (size_t k = 0; k < kind_count_; ++k) {
        LOOM_ASSERT_CLOSE(prior_[k], expected_prior[k]);
    }

    LOOM_ASSERT_EQ(empty_kind_count_, empty_kinds_.size());
    for (size_t k = 0; k < kind_count_; ++k) {
        bool in_empty_kinds = (empty_kinds_.find(k) != empty_kinds_.end());
        bool has_zero_count = (counts_[k] == 0);
        LOOM_ASSERT_EQ(in_empty_kinds, has_zero_count);
    }
}

inline float Algorithm8::BlockPitmanYorSampler::get_likelihood_empty () const
{
    if (empty_kind_count_) {
        float nonempty_kind_count = kind_count_ - empty_kind_count_;
        return (alpha_ + d_ * nonempty_kind_count) / empty_kind_count_;
    } else {
        return 0.f;
    }
}

inline float Algorithm8::BlockPitmanYorSampler::compute_posterior (
        const VectorFloat & prior_in,
        const VectorFloat & likelihood_in,
        VectorFloat & posterior_out)
{
    const size_t size = prior_in.size();
    const float * __restrict__ prior =
        DIST_ASSUME_ALIGNED(prior_in.data());
    const float * __restrict__ likelihood =
        DIST_ASSUME_ALIGNED(likelihood_in.data());
    float * __restrict__ posterior =
        DIST_ASSUME_ALIGNED(posterior_out.data());

    float total = 0;
    for (size_t i = 0; i < size; ++i) {
        total += posterior[i] = prior[i] * likelihood[i];
    }
    return total;
}

using distributions::sample_from_likelihoods;

void Algorithm8::BlockPitmanYorSampler::run (
        size_t iterations,
        rng_t & rng)
{
    LOOM_ASSERT_LT(0, iterations);

    for (size_t i = 0; i < iterations; ++i) {
        for (size_t f = 0; f < feature_count_; ++f) {

            const VectorFloat & likelihood = likelihoods_[f];
            float total = compute_posterior(prior_, likelihood, posterior_);
            size_t new_k = sample_from_likelihoods(rng, posterior_, total);
            size_t old_k = assignments_[f];
            if (LOOM_UNLIKELY(new_k != old_k)) {
                assignments_[f] = new_k;

                size_t old_empty_kind_count = empty_kind_count_;
                const float old_likelihood_empty = get_likelihood_empty();
                if (--counts_[old_k] == 0) {
                    prior_[old_k] = old_likelihood_empty;
                    empty_kinds_.insert(old_k);
                    ++empty_kind_count_;
                } else {
                    prior_[old_k] = counts_[old_k] - d_;
                }
                if (counts_[new_k]++ == 0) {
                    empty_kinds_.erase(new_k);
                    --empty_kind_count_;
                }
                prior_[new_k] = counts_[new_k] - d_;

                size_t new_empty_kind_count = empty_kind_count_;
                if (new_empty_kind_count != old_empty_kind_count) {
                    const float likelihood_empty = get_likelihood_empty();
                    for (auto k : empty_kinds_) {
                        prior_[k] = likelihood_empty;
                    }
                }
            }

            if (LOOM_DEBUG_LEVEL >= 3) {
                validate();
            }
        }
    }
}

void Algorithm8::infer_assignments (
        std::vector<uint32_t> & featureid_to_kindid,
        size_t iterations,
        rng_t & rng) const
{
    LOOM_ASSERT_LT(0, iterations);

    const size_t feature_count = featureid_to_kindid.size();
    const size_t kind_count = kinds.size();
    std::vector<VectorFloat> likelihoods(feature_count);

    #pragma omp parallel for schedule(dynamic, 1)
    for (size_t featureid = 0; featureid < feature_count; ++featureid) {
        VectorFloat & scores = likelihoods[featureid];
        scores.resize(kind_count);
        for (size_t kindid = 0; kindid < feature_count; ++kindid) {
            const auto & mixture = kinds[kindid].mixture;
            scores[kindid] = mixture.score_feature(model, featureid, rng);
        }
        distributions::scores_to_likelihoods(scores);
    }

    BlockPitmanYorSampler sampler(
            model.clustering,
            likelihoods,
            featureid_to_kindid);

    sampler.run(iterations, rng);
}

} // namespace loom
<|endoftext|>


<commit_before>// Open the mesh named in command line arguments,
// update elems in oldid to newid

#include <iostream>

#include "libmesh.h"

#include "mesh.h"
#include "elem.h"
#include "getpot.h"
#include "boundary_info.h"

void usage_error(const char *progname)
{
  std::cout << "Usage: " << progname
            << " --input inputmesh --output outputmesh --oldid --newid"
            << std::endl;

  exit(1);
}

int main(int argc, char** argv)
{
  LibMeshInit init(argc, argv);

  GetPot cl(argc, argv);

  Mesh mesh;

  if(!cl.search("--input"))
  {
    std::cerr << "No --input argument found!" << std::endl;
    usage_error(argv[0]);
  }
  const char* meshname = cl.next("");

  mesh.read(meshname);
  std::cout << "Loaded mesh " << meshname << std::endl;

  if(!cl.search("--oldid"))
  {
    std::cerr << "No --oldid argument found!" << std::endl;
    usage_error(argv[0]);
  }
  unsigned int oldid = cl.next(0);

  if(!cl.search("--newid"))
  {
    std::cerr << "No --newid argument found!" << std::endl;
    usage_error(argv[0]);
  }
  unsigned int newid = cl.next(0);

  MeshBase::element_iterator           el = mesh.elements_begin();
  const MeshBase::element_iterator end_el = mesh.elements_end();

  for (; el != end_el; ++el)
  {
    Elem *elem = *el;

    // Update the elements in old subdomain to the new subdomain
    if (elem->subdomain_id() == oldid)
      elem->subdomain_id() = newid;

    // Update sides that match this id
    unsigned int n_sides = elem->n_sides();
    for (unsigned int s=0; s != n_sides; ++s)
    {
      std::vector<short int> boundary_ids = mesh.boundary_info->boundary_ids(elem, s);
      if (std::find(boundary_ids.begin(), boundary_ids.end(), oldid) != boundary_ids.end())
      {
        mesh.boundary_info->remove_side(elem, s, oldid);
        mesh.boundary_info->add_side(elem, s, newid);
      }
    }
  }
  // Finally update all the nodesets
  mesh.boundary_info->clear_boundary_node_ids();
  mesh.boundary_info->build_node_list_from_side_list();
//  mesh.prepare_for_use();

  std::string outputname;
  if(cl.search("--output"))
  {
    outputname = cl.next("");
  }
  else
  {
    outputname = "new.";
    outputname += meshname;
  }

  mesh.write(outputname.c_str());
  std::cout << "Wrote mesh " << outputname << std::endl;

  return 0;
}
<commit_msg>Utility to renumber blocks/sidesets/nodesets in exodus meshes<commit_after>// Open the mesh named in command line arguments,
// update ids of one or more of the following enties
// as perscribed on the command line:
// blocks, sidesets and nodesets

#include <iostream>

#include "exodusII.h"
#include "exodusII_int.h"
#include "getpot.h"

#define BLOCKS  0x4
#define SIDES   0x2
#define NODES   0x1

void handle_error(int error)
{
  std::cout << "An error occured while working with the netCDF API" << std::endl;

  exit(1);
}

void usage_error(const char *progname)
{
  std::cout << "Usage: " << progname
            << " --input inputmesh --oldid <n> --newid <n> [--nodesetonly | --sidesetonly | --blockonly]"
            << std::endl;
  exit(1);
}

int main(int argc, char** argv)
{
  GetPot cl(argc, argv);

  // Command line parsing
  if(!cl.search("--input"))
  {
    std::cerr << "No --input argument found!" << std::endl;
    usage_error(argv[0]);
  }
  const char* meshname = cl.next("");

  if(!cl.search("--oldid"))
  {
    std::cerr << "No --oldid argument found!" << std::endl;
    usage_error(argv[0]);
  }
  long oldid = cl.next(0);

  if(!cl.search("--newid"))
  {
    std::cerr << "No --newid argument found!" << std::endl;
    usage_error(argv[0]);
  }
  long newid = cl.next(0);

  unsigned char flags = 0;
  if (cl.search("--nodesetonly"))
    flags |= NODES;
  if (cl.search("--sidesetonly"))
    flags |= SIDES;
  if (cl.search("--blockonly"))
    flags |= BLOCKS;
  if (!flags)
    flags = NODES | SIDES | BLOCKS; // ALL ON

  // flags are exclusive
  if (flags != NODES && flags != SIDES && flags != BLOCKS  && flags != (NODES | SIDES | BLOCKS))
  {
    std::cerr << "Only one of the following options may be active [--nodesetonly | --sidesetonly | --blockonly]!" << std::endl;
    usage_error(argv[0]);
  }

  // Processing
  std::string var_name, dim_name;
  int status;
  int nc_id, var_id, dim_id, var_len;
  size_t dim_len;

  status = nc_open (meshname, NC_WRITE, &nc_id);
  if (status != NC_NOERR) handle_error(status);

  for (unsigned char mask = 1<<2; mask; mask>>=1)
  {
    switch (flags & mask)
    {
    case BLOCKS:
      dim_name = DIM_NUM_EL_BLK;
      var_name = VAR_ID_EL_BLK;
      break;
    case SIDES:
      dim_name = DIM_NUM_SS;
      var_name = VAR_SS_IDS;
      break;
    case NODES:
      dim_name = DIM_NUM_NS;
      var_name = VAR_NS_IDS;
      break;
    default:
      continue;
    }

    // Get the length of the "variable" in question - stored in a dimension field
    status = nc_inq_dimid (nc_id, dim_name.c_str(), &dim_id);
    if (status != NC_NOERR) handle_error(status);

    status = nc_inq_dimlen (nc_id, dim_id, &dim_len);
    if (status != NC_NOERR) handle_error(status);

    // Now get the variable values themselves
    std::vector<long> var_vals(dim_len);

    status = nc_inq_varid (nc_id, var_name.c_str(), &var_id);
    if (status != NC_NOERR) handle_error(status);

    status = nc_get_var_long (nc_id, var_id, &var_vals[0]);
    if (status != NC_NOERR) handle_error(status);

    // Update the variable value specified on the command line
    for (unsigned int i=0; i<dim_len; ++i)
      if (var_vals[i] == oldid)
        var_vals[i] = newid;

    // Save that value back to the NetCDF database
    status = nc_put_var_long (nc_id, var_id, &var_vals[0]);
    if (status != NC_NOERR) handle_error(status);
  }

  // Write out the dataset
  status = nc_close(nc_id);

  exit(0);
}
<|endoftext|>


<commit_before>// Copyright (c) 2018 The Bitcoin developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <avalanche.h>

#include <chain.h>
#include <config/bitcoin-config.h>
#include <netmessagemaker.h>
#include <reverse_iterator.h>
#include <scheduler.h>
#include <validation.h>

#include <tuple>

/**
 * Run the avalanche event loop every 10ms.
 */
static const int64_t AVALANCHE_TIME_STEP_MILLISECONDS = 10;

/**
 * Maximum item count that can be polled at once.
 */
static const size_t AVALANCHE_MAX_ELEMENT_POLL = 4096;

static uint32_t countBits(uint32_t v) {
#if HAVE_DECL___BUILTIN_POPCOUNT
    return __builtin_popcount(v);
#else
    /**
     * Computes the number of bits set in each group of 8bits then uses a
     * multiplication to sum all of them in the 8 most significant bits and
     * return these.
     * More detailed explanation can be found at
     * https://www.playingwithpointers.com/blog/swar.html
     */
    v = v - ((v >> 1) & 0x55555555);
    v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
    return (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
#endif
}

bool VoteRecord::registerVote(NodeId nodeid, uint32_t error) {
    // We just got a new vote, so there is one less inflight request.
    clearInflightRequest();

    // We want to avoid having the same node voting twice in a quorum.
    if (!addNodeToQuorum(nodeid)) {
        return false;
    }

    /**
     * The result of the vote is determined from the error code. If the error
     * code is 0, there is no error and therefore the vote is yes. If there is
     * an error, we check the most significant bit to decide if the vote is a no
     * (for instance, the block is invalid) or is the vote inconclusive (for
     * instance, the queried node does not have the block yet).
     */
    votes = (votes << 1) | (error == 0);
    consider = (consider << 1) | (int32_t(error) >= 0);

    /**
     * We compute the number of yes and/or no votes as follow:
     *
     * votes:     1010
     * consider:  1100
     *
     * yes votes: 1000 using votes & consider
     * no votes:  0100 using ~votes & consider
     */
    bool yes = countBits(votes & consider & 0xff) > 6;
    if (!yes) {
        bool no = countBits(~votes & consider & 0xff) > 6;
        if (!no) {
            // The round is inconclusive.
            return false;
        }
    }

    // If the round is in agreement with previous rounds, increase confidence.
    if (isAccepted() == yes) {
        confidence += 2;
        return getConfidence() == AVALANCHE_FINALIZATION_SCORE;
    }

    // The round changed our state. We reset the confidence.
    confidence = yes;
    return true;
}

bool VoteRecord::addNodeToQuorum(NodeId nodeid) {
    if (nodeid == NO_NODE) {
        // Helpful for testing.
        return true;
    }

    // MMIX Linear Congruent Generator.
    const uint64_t r1 = 6364136223846793005 * nodeid + 1442695040888963407;
    // Fibonacci hashing.
    const uint64_t r2 = 11400714819323198485ull * (nodeid ^ seed);
    // Combine and extract hash.
    const uint16_t h = (r1 + r2) >> 48;

    /**
     * Check if the node is in the filter.
     */
    for (size_t i = 1; i < nodeFilter.size(); i++) {
        if (nodeFilter[(successfulVotes + i) % nodeFilter.size()] == h) {
            return false;
        }
    }

    /**
     * Add the node which just voted to the filter.
     */
    nodeFilter[successfulVotes % nodeFilter.size()] = h;
    successfulVotes++;
    return true;
}

bool VoteRecord::registerPoll() const {
    uint8_t count = inflight.load();
    while (count < AVALANCHE_MAX_INFLIGHT_POLL) {
        if (inflight.compare_exchange_weak(count, count + 1)) {
            return true;
        }
    }

    return false;
}

static bool IsWorthPolling(const CBlockIndex *pindex) {
    AssertLockHeld(cs_main);

    if (pindex->nStatus.isInvalid()) {
        // No point polling invalid blocks.
        return false;
    }

    if (IsBlockFinalized(pindex)) {
        // There is no point polling finalized block.
        return false;
    }

    return true;
}

bool AvalancheProcessor::addBlockToReconcile(const CBlockIndex *pindex) {
    bool isAccepted;

    {
        LOCK(cs_main);
        if (!IsWorthPolling(pindex)) {
            // There is no point polling this block.
            return false;
        }

        isAccepted = chainActive.Contains(pindex);
    }

    return vote_records.getWriteView()
        ->insert(std::make_pair(pindex, VoteRecord(isAccepted)))
        .second;
}

bool AvalancheProcessor::isAccepted(const CBlockIndex *pindex) const {
    auto r = vote_records.getReadView();
    auto it = r->find(pindex);
    if (it == r.end()) {
        return false;
    }

    return it->second.isAccepted();
}

int AvalancheProcessor::getConfidence(const CBlockIndex *pindex) const {
    auto r = vote_records.getReadView();
    auto it = r->find(pindex);
    if (it == r.end()) {
        return -1;
    }

    return it->second.getConfidence();
}

bool AvalancheProcessor::registerVotes(
    NodeId nodeid, const AvalancheResponse &response,
    std::vector<AvalancheBlockUpdate> &updates) {
    {
        // Save the time at which we can query again.
        auto w = peerSet.getWriteView();
        auto it = w->find(nodeid);
        if (it != w->end()) {
            w->modify(it, [&response](Peer &p) {
                // FIXME: This will override the time even when we received an
                // old stale message. This should check that the message is
                // indeed the most up to date one before updating the time.
                p.nextRequestTime =
                    std::chrono::steady_clock::now() +
                    std::chrono::milliseconds(response.getCooldown());
            });
        }
    }

    std::vector<CInv> invs;

    {
        // Check that the query exists.
        auto w = queries.getWriteView();
        auto it = w->find(std::make_tuple(nodeid, response.getRound()));
        if (it == w.end()) {
            // NB: The request may be old, so we don't increase banscore.
            return false;
        }

        invs = std::move(it->invs);
        w->erase(it);
    }

    // Verify that the request and the vote are consistent.
    const std::vector<AvalancheVote> &votes = response.GetVotes();
    size_t size = invs.size();
    if (votes.size() != size) {
        // TODO: increase banscore for inconsistent response.
        // NB: This isn't timeout but actually node misbehaving.
        return false;
    }

    for (size_t i = 0; i < size; i++) {
        if (invs[i].hash != votes[i].GetHash()) {
            // TODO: increase banscore for inconsistent response.
            // NB: This isn't timeout but actually node misbehaving.
            return false;
        }
    }

    std::map<CBlockIndex *, AvalancheVote> responseIndex;

    {
        LOCK(cs_main);
        for (auto &v : votes) {
            BlockMap::iterator mi = mapBlockIndex.find(v.GetHash());
            if (mi == mapBlockIndex.end()) {
                // This should not happen, but just in case...
                continue;
            }

            CBlockIndex *pindex = mi->second;
            if (!IsWorthPolling(pindex)) {
                // There is no point polling this block.
                continue;
            }

            responseIndex.insert(std::make_pair(pindex, v));
        }
    }

    {
        // Register votes.
        auto w = vote_records.getWriteView();
        for (auto &p : responseIndex) {
            CBlockIndex *pindex = p.first;
            const AvalancheVote &v = p.second;

            auto it = w->find(pindex);
            if (it == w.end()) {
                // We are not voting on that item anymore.
                continue;
            }

            auto &vr = it->second;
            if (!vr.registerVote(nodeid, v.GetError())) {
                // This vote did not provide any extra information, move on.
                continue;
            }

            if (!vr.hasFinalized()) {
                // This item has note been finalized, so we have nothing more to
                // do.
                updates.emplace_back(
                    pindex, vr.isAccepted()
                                ? AvalancheBlockUpdate::Status::Accepted
                                : AvalancheBlockUpdate::Status::Rejected);
                continue;
            }

            // We just finalized a vote. If it is valid, then let the caller
            // know. Either way, remove the item from the map.
            updates.emplace_back(pindex,
                                 vr.isAccepted()
                                     ? AvalancheBlockUpdate::Status::Finalized
                                     : AvalancheBlockUpdate::Status::Invalid);
            w->erase(it);
        }
    }

    return true;
}

bool AvalancheProcessor::addPeer(NodeId nodeid, int64_t score) {
    return peerSet.getWriteView()
        ->insert({nodeid, score, std::chrono::steady_clock::now()})
        .second;
}

bool AvalancheProcessor::startEventLoop(CScheduler &scheduler) {
    LOCK(cs_running);
    if (running) {
        // Do not start the event loop twice.
        return false;
    }

    running = true;

    // Start the event loop.
    scheduler.scheduleEvery(
        [this]() -> bool {
            runEventLoop();
            if (!stopRequest) {
                return true;
            }

            LOCK(cs_running);
            running = false;

            cond_running.notify_all();

            // A stop request was made.
            return false;
        },
        AVALANCHE_TIME_STEP_MILLISECONDS);

    return true;
}

bool AvalancheProcessor::stopEventLoop() {
    WAIT_LOCK(cs_running, lock);
    if (!running) {
        return false;
    }

    // Request avalanche to stop.
    stopRequest = true;

    // Wait for avalanche to stop.
    cond_running.wait(lock, [this]() EXCLUSIVE_LOCKS_REQUIRED(cs_running) {
        return !running;
    });

    stopRequest = false;
    return true;
}

std::vector<CInv> AvalancheProcessor::getInvsForNextPoll(bool forPoll) const {
    std::vector<CInv> invs;

    auto r = vote_records.getReadView();
    for (const std::pair<const CBlockIndex *const, VoteRecord> &p :
         reverse_iterate(r)) {
        const CBlockIndex *pindex = p.first;

        {
            LOCK(cs_main);
            if (!IsWorthPolling(pindex)) {
                // Obviously do not poll if the block is not worth polling.
                continue;
            }
        }

        // Check if we can run poll.
        const bool shouldPoll =
            forPoll ? p.second.registerPoll() : p.second.shouldPoll();
        if (!shouldPoll) {
            continue;
        }

        // We don't have a decision, we need more votes.
        invs.emplace_back(MSG_BLOCK, pindex->GetBlockHash());
        if (invs.size() >= AVALANCHE_MAX_ELEMENT_POLL) {
            // Make sure we do not produce more invs than specified by the
            // protocol.
            return invs;
        }
    }

    return invs;
}

NodeId AvalancheProcessor::getSuitableNodeToQuery() {
    auto r = peerSet.getReadView();
    auto it = r->get<next_request_time>().begin();
    if (it == r->get<next_request_time>().end()) {
        return NO_NODE;
    }

    if (it->nextRequestTime <= std::chrono::steady_clock::now()) {
        return it->nodeid;
    }

    return NO_NODE;
}

void AvalancheProcessor::clearTimedoutRequests() {
    auto now = std::chrono::steady_clock::now();
    std::map<CInv, uint8_t> timedout_items{};

    {
        // Clear expired requests.
        auto w = queries.getWriteView();
        auto it = w->get<query_timeout>().begin();
        while (it != w->get<query_timeout>().end() && it->timeout < now) {
            for (auto &i : it->invs) {
                timedout_items[i]++;
            }

            w->get<query_timeout>().erase(it++);
        }
    }

    if (timedout_items.empty()) {
        return;
    }

    // In flight request accounting.
    for (const auto &p : timedout_items) {
        const CInv &inv = p.first;
        assert(inv.type == MSG_BLOCK);

        CBlockIndex *pindex;

        {
            LOCK(cs_main);
            BlockMap::iterator mi = mapBlockIndex.find(inv.hash);
            if (mi == mapBlockIndex.end()) {
                continue;
            }

            pindex = mi->second;
        }

        auto w = vote_records.getWriteView();
        auto it = w->find(pindex);
        if (it == w.end()) {
            continue;
        }

        it->second.clearInflightRequest(p.second);
    }
}

void AvalancheProcessor::runEventLoop() {
    // First things first, check if we have requests that timed out and clear
    // them.
    clearTimedoutRequests();

    while (true) {
        NodeId nodeid = getSuitableNodeToQuery();
        if (nodeid == NO_NODE) {
            return;
        }

        /**
         * If we lost contact to that node, then we remove it from nodeids, but
         * never add the request to queries, which ensures bad nodes get cleaned
         * up over time.
         */
        std::vector<CInv> invs;
        bool hasSent = connman->ForNode(nodeid, [this, &invs](CNode *pnode) {
            invs = getInvsForNextPoll();
            if (invs.empty()) {
                return false;
            }

            uint64_t current_round = round++;

            {
                // Compute the time at which this requests times out.
                auto timeout =
                    std::chrono::steady_clock::now() + queryTimeoutDuration;
                // Register the query.
                queries.getWriteView()->insert(
                    {pnode->GetId(), current_round, timeout, invs});
                // Set the timeout.
                auto w = peerSet.getWriteView();
                auto it = w->find(pnode->GetId());
                if (it != w->end()) {
                    w->modify(it, [&timeout](Peer &p) {
                        p.nextRequestTime = timeout;
                    });
                }
            }

            // Send the query to the node.
            connman->PushMessage(
                pnode,
                CNetMsgMaker(pnode->GetSendVersion())
                    .Make(NetMsgType::AVAPOLL,
                          AvalanchePoll(current_round, std::move(invs))));
            return true;
        });

        // Success!
        if (hasSent || invs.empty()) {
            return;
        }

        // This node is obsolete, delete it.
        peerSet.getWriteView()->erase(nodeid);
    }
}
<commit_msg>Fix undefined behavior in avalanche.cpp<commit_after>// Copyright (c) 2018 The Bitcoin developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <avalanche.h>

#include <chain.h>
#include <config/bitcoin-config.h>
#include <netmessagemaker.h>
#include <reverse_iterator.h>
#include <scheduler.h>
#include <validation.h>

#include <tuple>

/**
 * Run the avalanche event loop every 10ms.
 */
static const int64_t AVALANCHE_TIME_STEP_MILLISECONDS = 10;

/**
 * Maximum item count that can be polled at once.
 */
static const size_t AVALANCHE_MAX_ELEMENT_POLL = 4096;

static uint32_t countBits(uint32_t v) {
#if HAVE_DECL___BUILTIN_POPCOUNT
    return __builtin_popcount(v);
#else
    /**
     * Computes the number of bits set in each group of 8bits then uses a
     * multiplication to sum all of them in the 8 most significant bits and
     * return these.
     * More detailed explanation can be found at
     * https://www.playingwithpointers.com/blog/swar.html
     */
    v = v - ((v >> 1) & 0x55555555);
    v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
    return (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
#endif
}

bool VoteRecord::registerVote(NodeId nodeid, uint32_t error) {
    // We just got a new vote, so there is one less inflight request.
    clearInflightRequest();

    // We want to avoid having the same node voting twice in a quorum.
    if (!addNodeToQuorum(nodeid)) {
        return false;
    }

    /**
     * The result of the vote is determined from the error code. If the error
     * code is 0, there is no error and therefore the vote is yes. If there is
     * an error, we check the most significant bit to decide if the vote is a no
     * (for instance, the block is invalid) or is the vote inconclusive (for
     * instance, the queried node does not have the block yet).
     */
    votes = (votes << 1) | (error == 0);
    consider = (consider << 1) | (int32_t(error) >= 0);

    /**
     * We compute the number of yes and/or no votes as follow:
     *
     * votes:     1010
     * consider:  1100
     *
     * yes votes: 1000 using votes & consider
     * no votes:  0100 using ~votes & consider
     */
    bool yes = countBits(votes & consider & 0xff) > 6;
    if (!yes) {
        bool no = countBits(~votes & consider & 0xff) > 6;
        if (!no) {
            // The round is inconclusive.
            return false;
        }
    }

    // If the round is in agreement with previous rounds, increase confidence.
    if (isAccepted() == yes) {
        confidence += 2;
        return getConfidence() == AVALANCHE_FINALIZATION_SCORE;
    }

    // The round changed our state. We reset the confidence.
    confidence = yes;
    return true;
}

bool VoteRecord::addNodeToQuorum(NodeId nodeid) {
    if (nodeid == NO_NODE) {
        // Helpful for testing.
        return true;
    }

    // MMIX Linear Congruent Generator.
    const uint64_t r1 =
        6364136223846793005 * uint64_t(nodeid) + 1442695040888963407;
    // Fibonacci hashing.
    const uint64_t r2 = 11400714819323198485ull * (nodeid ^ seed);
    // Combine and extract hash.
    const uint16_t h = (r1 + r2) >> 48;

    /**
     * Check if the node is in the filter.
     */
    for (size_t i = 1; i < nodeFilter.size(); i++) {
        if (nodeFilter[(successfulVotes + i) % nodeFilter.size()] == h) {
            return false;
        }
    }

    /**
     * Add the node which just voted to the filter.
     */
    nodeFilter[successfulVotes % nodeFilter.size()] = h;
    successfulVotes++;
    return true;
}

bool VoteRecord::registerPoll() const {
    uint8_t count = inflight.load();
    while (count < AVALANCHE_MAX_INFLIGHT_POLL) {
        if (inflight.compare_exchange_weak(count, count + 1)) {
            return true;
        }
    }

    return false;
}

static bool IsWorthPolling(const CBlockIndex *pindex) {
    AssertLockHeld(cs_main);

    if (pindex->nStatus.isInvalid()) {
        // No point polling invalid blocks.
        return false;
    }

    if (IsBlockFinalized(pindex)) {
        // There is no point polling finalized block.
        return false;
    }

    return true;
}

bool AvalancheProcessor::addBlockToReconcile(const CBlockIndex *pindex) {
    bool isAccepted;

    {
        LOCK(cs_main);
        if (!IsWorthPolling(pindex)) {
            // There is no point polling this block.
            return false;
        }

        isAccepted = chainActive.Contains(pindex);
    }

    return vote_records.getWriteView()
        ->insert(std::make_pair(pindex, VoteRecord(isAccepted)))
        .second;
}

bool AvalancheProcessor::isAccepted(const CBlockIndex *pindex) const {
    auto r = vote_records.getReadView();
    auto it = r->find(pindex);
    if (it == r.end()) {
        return false;
    }

    return it->second.isAccepted();
}

int AvalancheProcessor::getConfidence(const CBlockIndex *pindex) const {
    auto r = vote_records.getReadView();
    auto it = r->find(pindex);
    if (it == r.end()) {
        return -1;
    }

    return it->second.getConfidence();
}

bool AvalancheProcessor::registerVotes(
    NodeId nodeid, const AvalancheResponse &response,
    std::vector<AvalancheBlockUpdate> &updates) {
    {
        // Save the time at which we can query again.
        auto w = peerSet.getWriteView();
        auto it = w->find(nodeid);
        if (it != w->end()) {
            w->modify(it, [&response](Peer &p) {
                // FIXME: This will override the time even when we received an
                // old stale message. This should check that the message is
                // indeed the most up to date one before updating the time.
                p.nextRequestTime =
                    std::chrono::steady_clock::now() +
                    std::chrono::milliseconds(response.getCooldown());
            });
        }
    }

    std::vector<CInv> invs;

    {
        // Check that the query exists.
        auto w = queries.getWriteView();
        auto it = w->find(std::make_tuple(nodeid, response.getRound()));
        if (it == w.end()) {
            // NB: The request may be old, so we don't increase banscore.
            return false;
        }

        invs = std::move(it->invs);
        w->erase(it);
    }

    // Verify that the request and the vote are consistent.
    const std::vector<AvalancheVote> &votes = response.GetVotes();
    size_t size = invs.size();
    if (votes.size() != size) {
        // TODO: increase banscore for inconsistent response.
        // NB: This isn't timeout but actually node misbehaving.
        return false;
    }

    for (size_t i = 0; i < size; i++) {
        if (invs[i].hash != votes[i].GetHash()) {
            // TODO: increase banscore for inconsistent response.
            // NB: This isn't timeout but actually node misbehaving.
            return false;
        }
    }

    std::map<CBlockIndex *, AvalancheVote> responseIndex;

    {
        LOCK(cs_main);
        for (auto &v : votes) {
            BlockMap::iterator mi = mapBlockIndex.find(v.GetHash());
            if (mi == mapBlockIndex.end()) {
                // This should not happen, but just in case...
                continue;
            }

            CBlockIndex *pindex = mi->second;
            if (!IsWorthPolling(pindex)) {
                // There is no point polling this block.
                continue;
            }

            responseIndex.insert(std::make_pair(pindex, v));
        }
    }

    {
        // Register votes.
        auto w = vote_records.getWriteView();
        for (auto &p : responseIndex) {
            CBlockIndex *pindex = p.first;
            const AvalancheVote &v = p.second;

            auto it = w->find(pindex);
            if (it == w.end()) {
                // We are not voting on that item anymore.
                continue;
            }

            auto &vr = it->second;
            if (!vr.registerVote(nodeid, v.GetError())) {
                // This vote did not provide any extra information, move on.
                continue;
            }

            if (!vr.hasFinalized()) {
                // This item has note been finalized, so we have nothing more to
                // do.
                updates.emplace_back(
                    pindex, vr.isAccepted()
                                ? AvalancheBlockUpdate::Status::Accepted
                                : AvalancheBlockUpdate::Status::Rejected);
                continue;
            }

            // We just finalized a vote. If it is valid, then let the caller
            // know. Either way, remove the item from the map.
            updates.emplace_back(pindex,
                                 vr.isAccepted()
                                     ? AvalancheBlockUpdate::Status::Finalized
                                     : AvalancheBlockUpdate::Status::Invalid);
            w->erase(it);
        }
    }

    return true;
}

bool AvalancheProcessor::addPeer(NodeId nodeid, int64_t score) {
    return peerSet.getWriteView()
        ->insert({nodeid, score, std::chrono::steady_clock::now()})
        .second;
}

bool AvalancheProcessor::startEventLoop(CScheduler &scheduler) {
    LOCK(cs_running);
    if (running) {
        // Do not start the event loop twice.
        return false;
    }

    running = true;

    // Start the event loop.
    scheduler.scheduleEvery(
        [this]() -> bool {
            runEventLoop();
            if (!stopRequest) {
                return true;
            }

            LOCK(cs_running);
            running = false;

            cond_running.notify_all();

            // A stop request was made.
            return false;
        },
        AVALANCHE_TIME_STEP_MILLISECONDS);

    return true;
}

bool AvalancheProcessor::stopEventLoop() {
    WAIT_LOCK(cs_running, lock);
    if (!running) {
        return false;
    }

    // Request avalanche to stop.
    stopRequest = true;

    // Wait for avalanche to stop.
    cond_running.wait(lock, [this]() EXCLUSIVE_LOCKS_REQUIRED(cs_running) {
        return !running;
    });

    stopRequest = false;
    return true;
}

std::vector<CInv> AvalancheProcessor::getInvsForNextPoll(bool forPoll) const {
    std::vector<CInv> invs;

    auto r = vote_records.getReadView();
    for (const std::pair<const CBlockIndex *const, VoteRecord> &p :
         reverse_iterate(r)) {
        const CBlockIndex *pindex = p.first;

        {
            LOCK(cs_main);
            if (!IsWorthPolling(pindex)) {
                // Obviously do not poll if the block is not worth polling.
                continue;
            }
        }

        // Check if we can run poll.
        const bool shouldPoll =
            forPoll ? p.second.registerPoll() : p.second.shouldPoll();
        if (!shouldPoll) {
            continue;
        }

        // We don't have a decision, we need more votes.
        invs.emplace_back(MSG_BLOCK, pindex->GetBlockHash());
        if (invs.size() >= AVALANCHE_MAX_ELEMENT_POLL) {
            // Make sure we do not produce more invs than specified by the
            // protocol.
            return invs;
        }
    }

    return invs;
}

NodeId AvalancheProcessor::getSuitableNodeToQuery() {
    auto r = peerSet.getReadView();
    auto it = r->get<next_request_time>().begin();
    if (it == r->get<next_request_time>().end()) {
        return NO_NODE;
    }

    if (it->nextRequestTime <= std::chrono::steady_clock::now()) {
        return it->nodeid;
    }

    return NO_NODE;
}

void AvalancheProcessor::clearTimedoutRequests() {
    auto now = std::chrono::steady_clock::now();
    std::map<CInv, uint8_t> timedout_items{};

    {
        // Clear expired requests.
        auto w = queries.getWriteView();
        auto it = w->get<query_timeout>().begin();
        while (it != w->get<query_timeout>().end() && it->timeout < now) {
            for (auto &i : it->invs) {
                timedout_items[i]++;
            }

            w->get<query_timeout>().erase(it++);
        }
    }

    if (timedout_items.empty()) {
        return;
    }

    // In flight request accounting.
    for (const auto &p : timedout_items) {
        const CInv &inv = p.first;
        assert(inv.type == MSG_BLOCK);

        CBlockIndex *pindex;

        {
            LOCK(cs_main);
            BlockMap::iterator mi = mapBlockIndex.find(inv.hash);
            if (mi == mapBlockIndex.end()) {
                continue;
            }

            pindex = mi->second;
        }

        auto w = vote_records.getWriteView();
        auto it = w->find(pindex);
        if (it == w.end()) {
            continue;
        }

        it->second.clearInflightRequest(p.second);
    }
}

void AvalancheProcessor::runEventLoop() {
    // First things first, check if we have requests that timed out and clear
    // them.
    clearTimedoutRequests();

    while (true) {
        NodeId nodeid = getSuitableNodeToQuery();
        if (nodeid == NO_NODE) {
            return;
        }

        /**
         * If we lost contact to that node, then we remove it from nodeids, but
         * never add the request to queries, which ensures bad nodes get cleaned
         * up over time.
         */
        std::vector<CInv> invs;
        bool hasSent = connman->ForNode(nodeid, [this, &invs](CNode *pnode) {
            invs = getInvsForNextPoll();
            if (invs.empty()) {
                return false;
            }

            uint64_t current_round = round++;

            {
                // Compute the time at which this requests times out.
                auto timeout =
                    std::chrono::steady_clock::now() + queryTimeoutDuration;
                // Register the query.
                queries.getWriteView()->insert(
                    {pnode->GetId(), current_round, timeout, invs});
                // Set the timeout.
                auto w = peerSet.getWriteView();
                auto it = w->find(pnode->GetId());
                if (it != w->end()) {
                    w->modify(it, [&timeout](Peer &p) {
                        p.nextRequestTime = timeout;
                    });
                }
            }

            // Send the query to the node.
            connman->PushMessage(
                pnode,
                CNetMsgMaker(pnode->GetSendVersion())
                    .Make(NetMsgType::AVAPOLL,
                          AvalanchePoll(current_round, std::move(invs))));
            return true;
        });

        // Success!
        if (hasSent || invs.empty()) {
            return;
        }

        // This node is obsolete, delete it.
        peerSet.getWriteView()->erase(nodeid);
    }
}
<|endoftext|>