text:
#include "Bitstream.hpp"
#include "Chip.hpp"
#include "Util.hpp"
#include <bitset>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/optional.hpp>
#include <cstring>
#include <iostream>

namespace Tang {

static const uint16_t CRC16_POLY = 0x8005;
static const uint16_t CRC16_INIT = 0x0000;

enum class BitstreamCommand : uint8_t {
    DEVICEID = 0xf0,
    VERSION_UCODE =  0xc1,
    CFG_1 = 0xc2,
    CFG_2 = 0xc3,
    FRAMES = 0xc7,
    MEM_FRAME = 0xc8,
    RESET_CRC = 0xf1,
    PROGRAM_DONE = 0xf7,
    CMD_F3 = 0xf3,
    CMD_F5 = 0xf5,
    CMD_C4 = 0xc4,
    CMD_C5 = 0xc5,
    CMD_CA = 0xca,
    FUSE_DATA = 0xec,
    MEMORY_DATA = 0xed,
    //CPLD only
    DEVICEID_CPLD = 0x90,
    RESET_CRC_CPLD = 0xa8,
    CMD_A1 = 0xa1,
    CMD_A3 = 0xa3,
    CMD_AC = 0xac,
    CMD_B1 = 0xb1,
    CPLD_DATA = 0xaa,
};

class Crc16 {
public:
    uint16_t crc16 = CRC16_INIT;

    // Add a single byte to the running CRC16 accumulator
    void update_crc16(uint8_t val) {
        int bit_flag;
        for (int i = 7; i >= 0; i--) {
            bit_flag = crc16 >> 15;

            /* Get next bit: */
            crc16 <<= 1;
            crc16 |= (val >> i) & 1; // item a) work from the least significant bits

            /* Cycle check: */
            if (bit_flag)
                crc16 ^= CRC16_POLY;
        }

    }

    uint16_t finalise_crc16() {
        // item b) "push out" the last 16 bits
        int i;
        bool bit_flag;
        for (i = 0; i < 16; ++i) {
            bit_flag = bool(crc16 >> 15);
            crc16 <<= 1;
            if (bit_flag)
                crc16 ^= CRC16_POLY;
        }

        return crc16;
    }

    void reset_crc16() {
        crc16 = CRC16_INIT;
    }
};

// The BlockReadWriter class stores state (including CRC16) whilst reading
// the bitstream
class BlockReadWriter {
public:
    BlockReadWriter() : data(), iter(data.begin()) {};

    BlockReadWriter(const vector<uint8_t> &data) : data(data), iter(this->data.begin()) {};

    vector<uint8_t> data;
    vector<uint8_t>::iterator iter;
    Crc16 crc16;

    // Return a single byte and update CRC
    inline uint8_t get_byte() {
        assert(iter < data.end());
        uint8_t val = *(iter++);
        crc16.update_crc16(val);
        return val;
    }

    // Read a big endian uint16 from the bitstream and update CRC
    uint16_t get_uint16() {
        uint8_t tmp[2];
        get_bytes(tmp, 2);
        return (tmp[0] << 8UL) | (tmp[1]);
    }

    // The command opcode is a byte so this works like get_byte
    inline uint8_t get_command_opcode() {
        assert(iter < data.end());
        uint8_t val = *(iter++);
        crc16.update_crc16(val);
        return val;
    }

    // Write a single byte and update CRC
    inline void write_byte(uint8_t b) {
        data.push_back(b);
        crc16.update_crc16(b);
    }

    // Copy multiple bytes into an OutputIterator and update CRC
    template<typename T>
    void get_bytes(T out, size_t count) {
        for (size_t i = 0; i < count; i++) {
            *out = get_byte();
            ++out;
        }
    }

    void get_vector(std::vector<uint8_t> &out, size_t count) {
        for (size_t i = 0; i < count; i++) {
            out.push_back(get_byte());
        }
    }

    // Write multiple bytes from an InputIterator and update CRC
    template<typename T>
    void write_bytes(T in, size_t count) {
        for (size_t i = 0; i < count; i++)
            write_byte(*(in++));
    }

    // Skip over bytes while updating CRC
    void skip_bytes(size_t count) {
        for (size_t i = 0; i < count; i++) get_byte();
    }

    // Insert zeros while updating CRC
    void insert_zeros(size_t count) {
        for (size_t i = 0; i < count; i++) write_byte(0x00);
    }

    // Insert dummy bytes into the bitstream, without updating CRC
    void insert_dummy(size_t count) {
        for (size_t i = 0; i < count; i++)
            data.push_back(0xFF);
    }

    // Read a big endian uint32 from the bitstream
    uint32_t get_uint32() {
        uint8_t tmp[4];
        get_bytes(tmp, 4);
        return (tmp[0] << 24UL) | (tmp[1] << 16UL) | (tmp[2] << 8UL) | (tmp[3]);
    }

    // Write a big endian uint16_t into the bitstream
    void write_uint16(uint16_t val) {
        write_byte(uint8_t((val >> 8UL) & 0xFF));
        write_byte(uint8_t(val & 0xFF));
    }
    // Write a big endian uint32_t into the bitstream
    void write_uint32(uint32_t val) {
        write_byte(uint8_t((val >> 24UL) & 0xFF));
        write_byte(uint8_t((val >> 16UL) & 0xFF));
        write_byte(uint8_t((val >> 8UL) & 0xFF));
        write_byte(uint8_t(val & 0xFF));
    }

    // Search for a preamble, setting bitstream position to be after the preamble
    // Returns true on success, false on failure
    bool find_preamble(const vector<uint8_t> &preamble) {
        auto found = search(iter, data.end(), preamble.begin(), preamble.end());
        if (found == data.end())
            return false;
        iter = found + preamble.size();
        return true;
    }

    // Get the offset into the bitstream
    size_t get_offset() {
        return size_t(distance(data.begin(), iter));
    }

    // Check the calculated CRC16 against an actual CRC16, expected in the next 2 bytes
    void check_crc16() {
        uint8_t crc_bytes[2];
        uint16_t actual_crc = crc16.finalise_crc16();
        get_bytes(crc_bytes, 2);
        // cerr << hex << int(crc_bytes[0]) << " " << int(crc_bytes[1]) << endl;
        uint16_t exp_crc = (crc_bytes[0] << 8) | crc_bytes[1];
        if (actual_crc != exp_crc) {
            ostringstream err;
            err << "crc fail, calculated 0x" << hex << actual_crc << " but expecting 0x" << exp_crc;
            throw BitstreamParseError(err.str(), get_offset());
        }
        crc16.reset_crc16();
    }

    // Insert the calculated CRC16 into the bitstream, and then reset it
    void insert_crc16() {
        uint16_t actual_crc = crc16.finalise_crc16();
        write_byte(uint8_t((actual_crc >> 8) & 0xFF));
        write_byte(uint8_t((actual_crc) & 0xFF));
        crc16.reset_crc16();
    }

    bool is_end() {
        return (iter >= data.end());
    }

    const vector<uint8_t> &get() {
        return data;
    };
};

class BitstreamReadWriter {
public:
    BitstreamReadWriter() : data(), iter(data.begin()) {};

    BitstreamReadWriter(const vector<uint8_t> &data) : data(data), iter(this->data.begin()) {};

    void read_block() {
        std::vector<uint8_t> block;
        size_t block_size = data.size();
        get_vector(block, block_size);
        blocks.push_back(block);
    }

    void write_block(std::vector<uint8_t> block) {
        write_uint16(block.size()<<3);
        for (size_t i = 0; i < block.size(); i++)
            write_byte(block[i]);
        blocks.push_back(block);
    }

    // Get the offset into the bitstream
    size_t get_offset() {
        return size_t(distance(data.begin(), iter));
    }

    bool is_end() {
        return (iter >= data.end());
    }

    const std::vector<std::vector<uint8_t>> &get_blocks() {
        return blocks;
    };

    const vector<uint8_t> &get() {
        return data;
    };

    // Insert dummy bytes into the bitstream
    void insert_dummy_block(uint8_t val, uint16_t count) {
        BlockReadWriter blk;
        for (size_t i = 0; i < count; i++)
            blk.write_byte(val);
        write_block(blk.get());
    }

    void insert_cmd_uint32(BitstreamCommand cmd, uint32_t val) {
        BlockReadWriter blk;
        blk.write_byte((uint8_t)cmd);
        blk.write_byte(0x00);
        blk.write_uint16(6);
        blk.write_uint32(val);
        blk.insert_crc16();
        write_block(blk.get());
    }
    void insert_cmd_uint16(BitstreamCommand cmd, uint16_t val) {
        BlockReadWriter blk;
        blk.write_byte((uint8_t)cmd);
        blk.write_byte(0x00);
        blk.write_uint16(4);
        blk.write_uint16(val);
        blk.insert_crc16();
        write_block(blk.get());
    }
private:

    vector<uint8_t> data;
    std::vector<std::vector<uint8_t>> blocks;

    vector<uint8_t>::iterator iter;

    // Return a single byte
    inline uint8_t get_byte() {
        assert(iter < data.end());
        return *(iter++);
    }

    // Read a big endian uint16 from the bitstream
    uint16_t get_uint16() {
        uint8_t tmp[2];
        get_bytes(tmp, 2);
        return (tmp[0] << 8UL) | (tmp[1]);
    }

    inline uint16_t get_block_size() {
        uint16_t len = get_uint16();
        if ((len & 7) != 0)
            throw BitstreamParseError("Invalid size value in bitstream", get_offset());
        return len >> 3;
    }

    // Copy multiple bytes into an OutputIterator
    template<typename T>
    void get_bytes(T out, size_t count) {
        for (size_t i = 0; i < count; i++) {
            *out = get_byte();
            ++out;
        }
    }

    // Read multiple bytes into vector
    void get_vector(std::vector<uint8_t> &out, size_t count) {
        for (size_t i = 0; i < count; i++) {
            out.push_back(get_byte());
        }
    }

    // Write a single byte
    inline void write_byte(uint8_t b) {
        data.push_back(b);
    }

    void write_uint16(uint16_t val) {
        write_byte(uint8_t((val >> 8) & 0xFF));
        write_byte(uint8_t((val) & 0xFF));
    }

    // Write multiple bytes from an InputIterator
    template<typename T>
    void write_bytes(T in, size_t count) {
        for (size_t i = 0; i < count; i++)
            write_byte(*(in++));
    }

};

Bitstream::Bitstream(const std::vector<uint8_t> &data, const std::vector<std::string> &metadata)
        : data(data), metadata(metadata)
{
    BitstreamReadWriter rd(data);
    while (!rd.is_end()) {
        rd.read_block();
    }
    blocks = rd.get_blocks();
}

Bitstream Bitstream::read(std::istream &in)
{
    std::vector<uint8_t> bytes;
    std::vector<std::string> meta;
    auto hdr1 = uint8_t(in.get());
    auto hdr2 = uint8_t(in.get());
    (void)hdr1;
    (void)hdr2;
    /*
    if (hdr1 != 0x23 || hdr2 != 0x20) {
        throw BitstreamParseError("Anlogic .BIT files must start with comment", 0);
    }
    std::string temp;
    uint8_t c;
    in.seekg(0, in.beg);
    while ((c = uint8_t(in.get())) != 0x00) {
        if (in.eof())
            throw BitstreamParseError("Encountered end of file before start of bitstream data");
        if (c == '\n') {
            meta.push_back(temp);
            temp = "";
        } else {
            temp += char(c);
        }
    }
    */
    size_t start_pos = size_t(in.tellg()) - 1;
    in.seekg(0, in.end);
    size_t length = size_t(in.tellg()) - start_pos;
    in.seekg(start_pos, in.beg);
    bytes.resize(length);
    in.read(reinterpret_cast<char *>(&(bytes[0])), length);
    return Bitstream(bytes, meta);
}

// TODO: replace these macros with something more flexible
#define BITSTREAM_DEBUG(x) if (verbosity >= VerbosityLevel::DEBUG) cerr << "bitstream: " << x << endl
#define BITSTREAM_NOTE(x) if (verbosity >= VerbosityLevel::NOTE) cerr << "bitstream: " << x << endl
#define BITSTREAM_FATAL(x, pos) { ostringstream ss; ss << x; throw BitstreamParseError(ss.str(), pos); }

static const vector<uint8_t> preamble = {0xCC, 0x55, 0xAA, 0x33};

Chip Bitstream::deserialise_chip()
{
    boost::optional<Chip> chip;

    Crc16 data_crc16;
    bool is_cpld = false;
    uint8_t pll_index = 0x00;

    bool found_preamble = false;

    auto it_ = blocks.begin();
    BlockReadWriter rd(*it_);

    while (!rd.is_end()) {
        if (!found_preamble) {
            found_preamble = rd.find_preamble(preamble);
        }

        uint8_t cmd_byte = rd.get_command_opcode();
        switch(cmd_byte) {
            case 0xff:
            case 0xee:
            case 0x00:
                //BITSTREAM_NOTE("padding");
                continue;
                break;
        }

        cerr << "--> Opcode " << hex << (int)cmd_byte << " at offset " << rd.get_offset() << endl;

        // Add highest bit since old tools generate bitstreams with that bit low
        BitstreamCommand cmd = BitstreamCommand(cmd_byte | 0x80);
        bool is_cpld_command = false;
        switch(cmd) {
            case BitstreamCommand::DEVICEID_CPLD:
            case BitstreamCommand::RESET_CRC_CPLD:
            case BitstreamCommand::CMD_A1:
            case BitstreamCommand::CMD_A3:
            case BitstreamCommand::CMD_AC:
            case BitstreamCommand::CMD_B1:
            case BitstreamCommand::CPLD_DATA:
                is_cpld_command = true;
                break;
            case BitstreamCommand::CMD_C4:
                if (is_cpld)
                    is_cpld_command = true;
                break;
            default:
                is_cpld_command = false;
        }
        uint16_t cmd_size = 0;

        if (cmd!=BitstreamCommand::MEMORY_DATA) {
            uint8_t flag = rd.get_byte();
            if (!flag) {
                cmd_size = rd.get_uint16();
            }
        }

        switch (cmd) {
            case BitstreamCommand::RESET_CRC:
                BITSTREAM_DEBUG("reset crc");
                rd.get_uint16();
                data_crc16.reset_crc16();
                break;
            case BitstreamCommand::DEVICEID: {
                uint32_t id = rd.get_uint32();
                BITSTREAM_NOTE("device ID: 0x" << hex << setw(8) << setfill('0') << id);
                chip = boost::make_optional(Chip(id));
                chip->metadata = metadata;
                break;
            }
            case BitstreamCommand::VERSION_UCODE:
                chip->usercode = rd.get_uint32();
                BITSTREAM_NOTE("version and usercode 0x"<< hex << setw(8) << setfill('0') << chip->usercode);
                break;
            case BitstreamCommand::CFG_1:
                if (!chip) {
                    // al3_s10 device bitstreams do not have deviceid set
                    chip = boost::make_optional(Chip(0x12006c31));
                    chip->metadata = metadata;
                }
                BITSTREAM_DEBUG("CFG_1");
                chip->cfg1 = rd.get_uint32();
                break;
            case BitstreamCommand::CFG_2:
                BITSTREAM_DEBUG("CFG_2");
                chip->cfg2 = rd.get_uint32();
                break;
            case BitstreamCommand::FRAMES: {
                uint16_t frames = rd.get_uint16();
                uint32_t bits_per_frame = rd.get_uint16() << 3;
                BITSTREAM_NOTE("frames " << dec << frames << " bits_per_frame " << dec << bits_per_frame);
                BITSTREAM_NOTE("num_frames " << dec << chip->info.num_frames << " bits_per_frame " << dec << chip->info.bits_per_frame);
                if (frames != chip->info.num_frames)
                    throw BitstreamParseError("different number of frames than expected");
                if (bits_per_frame != chip->info.bits_per_frame)
                    throw BitstreamParseError("different bits per frame than expected");
                break;
            }
            case BitstreamCommand::MEM_FRAME: {
                rd.get_uint16();
                uint32_t bram_bits_per_frame = rd.get_uint16() << 3;
                if (bram_bits_per_frame != chip->info.bram_bits_per_frame)
                    throw BitstreamParseError("different BRAM bits per frame than expected");
                BITSTREAM_NOTE("bram_bits_per_frame " << dec << bram_bits_per_frame);
                break;
            }
            case BitstreamCommand::PROGRAM_DONE:
                BITSTREAM_NOTE("program done");
                rd.get_uint16();
                break;
            case BitstreamCommand::CMD_F3:
                BITSTREAM_DEBUG("CMD_F3");
                rd.skip_bytes(cmd_size-2);
                break;
            case BitstreamCommand::CMD_F5:
                BITSTREAM_DEBUG("CMD_F5");
                rd.get_uint16();
                break;
            case BitstreamCommand::CMD_C4:
                BITSTREAM_DEBUG("CMD_C4");
                if (!is_cpld)
                    chip->cfg_c4 = rd.get_uint32();
                else {
                    chip->cfg_c4 = rd.get_uint16();
                }
                break;
            case BitstreamCommand::CMD_C5:
                BITSTREAM_DEBUG("CMD_C5");
                chip->cfg_c5 = rd.get_uint32();
                break;
            case BitstreamCommand::CMD_CA:
                BITSTREAM_DEBUG("CMD_CA");
                chip->cfg_ca = rd.get_uint32();
                break;
            case BitstreamCommand::FUSE_DATA: {
                uint16_t frames = rd.get_uint16();
                uint16_t bytes_per_frame = chip->info.bits_per_frame / 8L;
                unique_ptr<uint8_t[]> frame_bytes = make_unique<uint8_t[]>(bytes_per_frame);
                // taking current CRC16
                data_crc16.crc16 = rd.crc16.crc16;
                for (int idx = 0; idx < frames; idx++) {
                    //it++;
                    //BlockReadWriter rd(*it);
                    rd.get_bytes(frame_bytes.get(), bytes_per_frame);
                    // Update CRC16 for complete frame
                    for (size_t i = 0; i < bytes_per_frame; i++) {
                        data_crc16.update_crc16(frame_bytes[i]);
                    }
                    uint16_t actual_crc = data_crc16.finalise_crc16();
                    uint16_t exp_crc = rd.get_uint16(); // crc
                    if (actual_crc != exp_crc) {
                        ostringstream err;
                        err << "crc fail, calculated 0x" << hex << actual_crc << " but expecting 0x" << exp_crc;
                        throw BitstreamParseError(err.str());
                    }

                    if (rd.get_uint32())
                        throw BitstreamParseError("error parsing fuse data");
                    for (uint32_t j = 0; j < chip->info.bits_per_frame; j++) {
                        chip->cram.bit(idx, j) = (char) ((frame_bytes[(j / 8)]<< (j % 8)) & 0x80);
                    }
                    data_crc16.reset_crc16();
                }
                // zero block, just skip
                //it++;
                break;
            }
            case BitstreamCommand::MEMORY_DATA: {
                uint16_t type = rd.get_uint16();
                uint8_t bram_block_id = rd.get_byte();
                // taking current CRC16
                data_crc16.crc16 = rd.crc16.crc16;
                if (type == 0x0001) {
                    BITSTREAM_NOTE("pll");
                } else {
                    BITSTREAM_NOTE("bram_block_id 0x" << hex << setw(2) << setfill('0') << (int)bram_block_id);
                }
                uint16_t bram_bytes_per_frame = chip->info.bram_bits_per_frame / 8L;
                unique_ptr<uint8_t[]> frame_bytes = make_unique<uint8_t[]>(bram_bytes_per_frame);
                if (type == 0x0001)
                    chip->pll_data[pll_index].resize(bram_bytes_per_frame);
                else
                    chip->bram_data[bram_block_id].resize(bram_bytes_per_frame);
                rd.get_bytes(frame_bytes.get(), bram_bytes_per_frame);
                // Update CRC16 for complete frame
                for (size_t i = 0; i < bram_bytes_per_frame; i++) {
                    data_crc16.update_crc16(frame_bytes[i]);
                }
                if (type == 0x0001) {
                    for(size_t i=0;i<bram_bytes_per_frame;i++)
                        chip->pll_data[pll_index][i] = frame_bytes[i];
                    pll_index++;
                } else {
                    for(size_t i=0;i<bram_bytes_per_frame;i++)
                        chip->bram_data[bram_block_id][i] = frame_bytes[i];
                }
                uint16_t actual_crc = data_crc16.finalise_crc16();
                uint16_t exp_crc = rd.get_uint16(); // crc
                if (actual_crc != exp_crc) {
                    ostringstream err;
                    err << "crc fail, calculated 0x" << hex << actual_crc << " but expecting 0x" << exp_crc;
                    throw BitstreamParseError(err.str());
                }
                rd.skip_bytes(4); // padding
                break;
            }
            // CPLD specific
            case BitstreamCommand::RESET_CRC_CPLD:
                BITSTREAM_DEBUG("reset crc");
                rd.get_byte();
                data_crc16.reset_crc16();
                break;
            case BitstreamCommand::DEVICEID_CPLD: {
                uint32_t id = rd.get_uint32();
                BITSTREAM_NOTE("device ID: 0x" << hex << setw(8) << setfill('0') << id);
                chip = boost::make_optional(Chip(id));
                chip->metadata = metadata;
                is_cpld = true;
                break;
            }

            case BitstreamCommand::CMD_A1:
            case BitstreamCommand::CMD_A3:
            case BitstreamCommand::CMD_AC:
            case BitstreamCommand::CMD_B1:
                //rd.skip_bytes(it->size()-3-3);
                break;
            case BitstreamCommand::CPLD_DATA: {
                uint16_t frames = cmd_size;
                uint16_t bytes_per_frame = chip->info.bits_per_frame / 8L;
                unique_ptr<uint8_t[]> frame_bytes = make_unique<uint8_t[]>(bytes_per_frame);
                // taking current CRC16
                data_crc16.crc16 = rd.crc16.crc16;
                for (int idx = 0; idx < frames; idx++) {
                    //it++;
                    //BlockReadWriter rd(*it);
                    rd.get_bytes(frame_bytes.get(), bytes_per_frame);
                    // Update CRC16 for complete frame
                    for (size_t i = 0; i < bytes_per_frame; i++) {
                        data_crc16.update_crc16(frame_bytes[i]);
                    }
                    uint16_t actual_crc = data_crc16.finalise_crc16();
                    uint16_t exp_crc = rd.get_uint16(); // crc
                    if (actual_crc != exp_crc) {
                        ostringstream err;
                        err << "crc fail, calculated 0x" << hex << actual_crc << " but expecting 0x" << exp_crc;
                        printf("%s\n",err.str().c_str());
                        throw BitstreamParseError(err.str());
                    }

                    for (uint32_t j = 0; j < chip->info.bits_per_frame; j++) {
                        chip->cram.bit(idx, j) = (char) ((frame_bytes[(j / 8)]<< (j % 8)) & 0x80);
                    }
                    data_crc16.reset_crc16();
                }
                break;
            }

            default:
                BITSTREAM_FATAL("unsupported command 0x" << hex << setw(2) << setfill('0') << int(cmd), rd.get_offset());
        }

        if (cmd!=BitstreamCommand::FUSE_DATA && cmd!=BitstreamCommand::CPLD_DATA && cmd!=BitstreamCommand::MEMORY_DATA) {
            rd.check_crc16();
        }
    }

    if (!found_preamble)
        throw BitstreamParseError("preamble not found in bitstream");

    if (chip) {
        return *chip;
    } else {
        throw BitstreamParseError("failed to parse bitstream, no valid payload found");
    }
}

void Bitstream::write_bit(std::ostream &out)
{
    for (const auto &str : metadata) {
        out << str;
        out.put(0x0a);
    }
    // Dump raw bitstream
    out.write(reinterpret_cast<const char *>(&(data[0])), data.size());
}

void Bitstream::write_bin(std::ostream &out)
{
    for (auto &block : blocks) {
        out.write(reinterpret_cast<const char *>(&(block[0])), block.size());
    }
}

void Bitstream::write_bas(std::ostream &out) {
    for (const auto &meta : metadata) {
        out << meta << std::endl;
    }
    for (const auto &block : blocks) {
        for (size_t pos = 0; pos < block.size(); pos++) {
            out << std::bitset<8>(block[pos]);
        }
        out << std::endl;
    }
}

void Bitstream::write_bmk(const Chip &chip, std::ostream &out) {
    for (const auto &meta : metadata) {
        if (boost::starts_with(meta, "# Bitstream CRC:"))
            continue;
        if (boost::starts_with(meta, "# USER CODE:"))
            continue;
        out << meta << std::endl;
    }
    for(auto it = blocks.begin(); it != blocks.end(); ++it) {
        if (it->at(0) == 0x00) continue;
        uint16_t size = it->size() << 3;
        out << uint8_t(size >> 8) << uint8_t(size & 0xff);
        for (size_t pos = 0; pos < it->size(); pos++) {
            out << uint8_t((*it)[pos]);
        }
        if (it->at(0) == (uint8_t)BitstreamCommand::FUSE_DATA) {
            // adding one more block after frames with all zeros
            for(int i=0;i<chip.info.num_frames+1;i++) {
                it++;
                uint16_t size = it->size() << 3;
                out << uint8_t(size >> 8) << uint8_t(size & 0xff);
                for (size_t pos = 0; pos < it->size(); pos++) {
                    out << uint8_t(0);
                }
            }
        }
    }
}

void Bitstream::write_bma(const Chip &chip, std::ostream &out) {
    for (const auto &meta : metadata) {
        if (boost::starts_with(meta, "# Bitstream CRC:"))
            continue;
        if (boost::starts_with(meta, "# USER CODE:"))
            continue;
        out << meta << std::endl;
    }
    for(auto it = blocks.begin(); it != blocks.end(); ++it) {
        if (it->at(0) == 0x00) continue;
        for (size_t pos = 0; pos < it->size(); pos++) {
            out << std::bitset<8>((*it)[pos]);
        }
        out << std::endl;
        if (it->at(0) == (uint8_t)BitstreamCommand::FUSE_DATA) {
            // adding one more block after frames with all zeros
            for(int i=0;i<chip.info.num_frames+1;i++) {
                it++;
                for (size_t pos = 0; pos < it->size(); pos++) {
                    out << std::bitset<8>(0);
                }
                out << std::endl;
            }
        }
    }
}

static uint8_t reverse_byte(uint8_t byte)
{
    uint8_t rev = 0;
    for (int i = 0; i < 8; i++)
        if (byte & (1 << i))
            rev |= (1 << (7 - i));
    return rev;
}

void Bitstream::write_rbf(std::ostream &out)
{
    for (auto &block : blocks) {
        for(auto byte : block) {
            uint8_t val = reverse_byte(byte);
            out.write(reinterpret_cast<const char *>(&val), 1);
        }
    }
}
void Bitstream::write_svf(const Chip &chip, std::ostream &out) {
    out << "// Created using Project Tang Software" << std::endl;
    out << "// Architecture: " << chip.info.name << std::endl;
    //out << "// Package: " << chip.info << std::endl;
    out << std::endl;
    out << "TRST OFF;" << std::endl;
    out << "ENDIR IDLE;" << std::endl;
    out << "ENDDR IDLE;" << std::endl;
    out << "STATE RESET;" << std::endl;
    out << "STATE IDLE;" << std::endl;
    out << "FREQUENCY 1E6 HZ;" << std::endl;
    // Operation: Program
    out << "TIR 0 ;" << std::endl;
    out << "HIR 0 ;" << std::endl;
    out << "TDR 0 ;" << std::endl;
    out << "HDR 0 ;" << std::endl;
    out << "TIR 0 ;" << std::endl;
    out << "HIR 0 ;" << std::endl;
    out << "HDR 0 ;" << std::endl;
    out << "TDR 0 ;" << std::endl;
    // Loading device with 'idcode' instruction.
    out << "SIR 8 TDI (06) SMASK (ff) ;" << std::endl;
    out << "RUNTEST 15 TCK;" << std::endl;
    out << "SDR 32 TDI (00000000) SMASK (ffffffff) TDO (" << std::setw(8) << std::hex << std::setfill('0') << chip.info.idcode
        << ") MASK (ffffffff) ;" << std::endl;
    // Boundary Scan Chain Contents
    // Position 1: BG256
    out << "TIR 0 ;" << std::endl;
    out << "HIR 0 ;" << std::endl;
    out << "TDR 0 ;" << std::endl;
    out << "HDR 0 ;" << std::endl;
    out << "TIR 0 ;" << std::endl;
    out << "HIR 0 ;" << std::endl;
    out << "TDR 0 ;" << std::endl;
    out << "HDR 0 ;" << std::endl;
    out << "TIR 0 ;" << std::endl;
    out << "HIR 0 ;" << std::endl;
    out << "HDR 0 ;" << std::endl;
    out << "TDR 0 ;" << std::endl;
    // Loading device with 'idcode' instruction.
    out << "SIR 8 TDI (06) SMASK (ff) ;" << std::endl;
    out << "SDR 32 TDI (00000000) SMASK (ffffffff) TDO (" << std::setw(8) << std::hex << std::setfill('0') << chip.info.idcode
         << ") MASK (ffffffff) ;" << std::endl;
    // Loading device with 'refresh' instruction.
    out << "SIR 8 TDI (01) SMASK (ff) ;" << std::endl;
    // Loading device with 'bypass' instruction.
    out << "SIR 8 TDI (1f) ;" << std::endl;
    out << "SIR 8 TDI (39) ;" << std::endl;
    out << "RUNTEST 50000 TCK;" << std::endl;
    // Loading device with 'jtag program' instruction.
    out << "SIR 8 TDI (30) SMASK (ff) ;" << std::endl;
    out << "RUNTEST 15 TCK;" << std::endl;
    // Loading device with a `cfg_in` instruction.
    out << "SIR 8 TDI (3b) SMASK (ff) ;" << std::endl;
    out << "RUNTEST 15 TCK;" << std::endl;

    size_t count = 0;
    for (auto const &block : blocks) {
        count += block.size();
    }
    // Begin of bitstream data
    out << "SDR " << std::dec << int(count * 8) << " TDI (" << std::endl;
    int i = 0;
    for (auto it = blocks.rbegin(); it != blocks.rend(); ++it) {
        for (size_t pos = 0; pos < it->size(); pos++) {
            out << std::hex << std::setw(2) << std::setfill('0') << int(reverse_byte(((*it)[it->size() - pos - 1])));
            i++;
            if ((i % 1024) == 0)
                out << std::endl;
        }
    }
    out << std::endl << ") SMASK (" << std::endl;
    for (size_t j = 1; j < count + 1; j++) {
        out << "ff";
        if ((j % 1024) == 0)
            out << std::endl;
    }
    out << std::endl << ") ;" << std::endl;

    out << "RUNTEST 100 TCK;" << std::endl;
    // Loading device with a `jtag start` instruction.
    out << "SIR 8 TDI (3d) ;" << std::endl;
    out << "RUNTEST 15 TCK;" << std::endl;
    // Loading device with 'bypass' instruction.
    out << "SIR 8 TDI (1f) ;" << std::endl;
    out << "RUNTEST 1000 TCK;" << std::endl;
    out << "TIR 0 ;" << std::endl;
    out << "HIR 0 ;" << std::endl;
    out << "HDR 0 ;" << std::endl;
    out << "TDR 0 ;" << std::endl;
    out << "TIR 0 ;" << std::endl;
    out << "HIR 0 ;" << std::endl;
    out << "HDR 0 ;" << std::endl;
    out << "TDR 0 ;" << std::endl;
    // Loading device with 'bypass' instruction.
    out << "SIR 8 TDI (1f) ;" << std::endl;
}

Bitstream Bitstream::serialise_chip(const Chip &chip, const map<string, string>) {
    BitstreamReadWriter wr;
    wr.insert_dummy_block(0xff, 16);
    wr.insert_dummy_block(0xff, 16);
    // Preamble
    {
        BlockReadWriter blk;
        blk.write_bytes(preamble.begin(), preamble.size());
        wr.write_block(blk.get());
    }

    wr.insert_cmd_uint32(BitstreamCommand::DEVICEID, chip.info.idcode);
    wr.insert_cmd_uint32(BitstreamCommand::CFG_1, chip.cfg1);
    wr.insert_cmd_uint32(BitstreamCommand::CFG_2, chip.cfg2);
    wr.insert_cmd_uint32(BitstreamCommand::FRAMES, (chip.info.num_frames << 16) + (chip.info.bits_per_frame >> 3));
    wr.insert_cmd_uint32(BitstreamCommand::MEM_FRAME, chip.info.bram_bits_per_frame >> 3);
    wr.insert_cmd_uint32(BitstreamCommand::VERSION_UCODE, chip.usercode);
    wr.insert_cmd_uint32(BitstreamCommand::CMD_CA, chip.cfg_ca);
    wr.insert_cmd_uint32(BitstreamCommand::CMD_C4, chip.cfg_c4);
    wr.insert_cmd_uint16(BitstreamCommand::CMD_F5, 0x0000);
    wr.insert_cmd_uint16(BitstreamCommand::RESET_CRC, 0x0000);
    uint16_t crc16 = CRC16_INIT;
    {
        BlockReadWriter blk;
        blk.crc16.reset_crc16();
        blk.write_byte((uint8_t)BitstreamCommand::FUSE_DATA);
        blk.write_byte(0xf0);
        blk.write_uint16(chip.info.num_frames);
        crc16 = blk.crc16.crc16;
        wr.write_block(blk.get());
    }
    for (int idx = 0; idx < chip.cram.frames(); idx++) {
        BlockReadWriter blk;
        blk.crc16.crc16 = crc16;
        for (int pos = 0; pos < chip.cram.bits()/8; pos++) {
            uint8_t byte = 0x00;
            for (int i = 0; i < 8; i++)
                byte = (byte << 1) + (chip.cram.get_bit(idx, pos*8+i) ? 1 : 0);
            blk.write_byte(byte);
        }
        blk.insert_crc16();
        blk.write_uint32(0);
        wr.write_block(blk.get());
        crc16 = CRC16_INIT;
    }
    wr.insert_dummy_block(0x00, 15);
    wr.insert_cmd_uint16(BitstreamCommand::PROGRAM_DONE, 0x0000);
    wr.insert_dummy_block(0xff, 16);
    wr.insert_dummy_block(0xff, 16);
    wr.insert_dummy_block(0x00, 1162);
    wr.insert_dummy_block(0x00, 1162);
    wr.insert_dummy_block(0x00, 1162);
    wr.insert_dummy_block(0x00, 1162);
    return Bitstream(wr.get(), chip.metadata);
}

void Bitstream::write_fuse(const Chip &chip, std::ostream &out)
{
    for (int idx = 0; idx < chip.cram.frames(); idx++) {
        for (int pos = 0; pos < chip.cram.bits(); pos++) {
            out << chip.cram.get_bit(idx, pos);
        }
        out << std::endl;
    }
}

BitstreamParseError::BitstreamParseError(const std::string &desc) : runtime_error(desc.c_str()), desc(desc), offset(-1)
{
}

BitstreamParseError::BitstreamParseError(const std::string &desc, size_t offset)
        : runtime_error(desc.c_str()), desc(desc), offset(int(offset))
{
}

const char *BitstreamParseError::what() const noexcept
{
    std::ostringstream ss;
    ss << "Bitstream Parse Error: ";
    ss << desc;
    if (offset != -1)
        ss << " [at 0x" << std::hex << offset << "]";
    return strdup(ss.str().c_str());
}
} // namespace Tang

max_stars_repo_name: nviennot/prjtang

max_issues_repo_name: nviennot/prjtang

max_forks_repo_name: nviennot/prjtang


text:
#include "logsplitter.hpp"

char const* version()
{
   return "Alfa baby, alfa";
}

#include <boost/python.hpp>
//#include <boost/shared_ptr.hpp>

using namespace nglogger;
using namespace std;

struct python_splitted_post
{
    loggedrowheader header;
    string payload;
    bool checksumok;
};

class python_splitted_reader
{
    logfilemmap iofile;
    logsplitter io;
public:

    python_splitted_reader(const string &filename)
        : iofile(filename), io(iofile)
    {
    }

    shared_ptr<python_splitted_post> read()
    {
        auto rval = make_shared<python_splitted_post>();
        if(io.read_row(rval->header,rval->payload,rval->checksumok))
        {
            return rval;
        }
        return nullptr;
    }
};

shared_ptr<python_splitted_reader> open_splitted_reader(const string &filename)
{
    return make_shared<python_splitted_reader>(filename);
}

BOOST_PYTHON_MODULE(nglogger)
{
    using namespace boost::python;
    def("version", version);

    class_<loggedrowheader>("loggedrowheader")
        .def_readwrite("checksum", &loggedrowheader::checksum)
        .def_readwrite("rowid", &loggedrowheader::rowid)
        .def_readwrite("parent_rowid", &loggedrowheader::parent_rowid)
        .def_readwrite("parts", &loggedrowheader::parts)
        .def_readwrite("when", &loggedrowheader::when)
        .def_readwrite("type", &loggedrowheader::type)
        .def_readwrite("status", &loggedrowheader::status)
    ;
    class_<python_splitted_post>("row")
        .def_readwrite("header", &python_splitted_post::header)
        .def_readwrite("payload", &python_splitted_post::payload)
        .def_readwrite("checksumok", &python_splitted_post::checksumok)
     ;
    class_<python_splitted_reader, boost::noncopyable>("reader", init<const string&>())
        .def( init<const string&>() )
        .def("read", &python_splitted_reader::read)
    ;
    def("open", open_splitted_reader);

    register_ptr_to_python< shared_ptr<python_splitted_post> >();
    register_ptr_to_python< shared_ptr<python_splitted_reader> >();

//    class_<logfilemmap, boost::noncopyable>("logfilemmap", init<const string&>())
//        .def( init<const string&>() )
//    ;

//    class_<logsplitter>("logsplitter", init<logsplitter&>())
//        .def( init<logsplitter&>() )
//        .def( "read", &logsplitter::read_row )
//    ;
}

ext: cpp

max_stars_repo_name: stefandxm/nglogger

max_issues_repo_name: stefandxm/nglogger

max_forks_repo_name: stefandxm/nglogger


text:
#ifndef BOOST_ATOMIC_DETAIL_LOCKPOOL_HPP
#define BOOST_ATOMIC_DETAIL_LOCKPOOL_HPP

//  Copyright (c) 2011 Helge Bahmann
//
//  Distributed under the Boost Software License, Version 1.0.
//  See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#include <boost/atomic/detail/config.hpp>
#ifndef BOOST_ATOMIC_FLAG_LOCK_FREE
#include <boost/thread/mutex.hpp>
#endif

#ifdef BOOST_HAS_PRAGMA_ONCE
#pragma once
#endif

namespace boost {
namespace atomics {
namespace detail {

#ifndef BOOST_ATOMIC_FLAG_LOCK_FREE

class lockpool
{
public:
    typedef mutex lock_type;
    class scoped_lock
    {
    private:
        lock_type& mtx_;

    public:
        explicit
        scoped_lock(const volatile void * addr) : mtx_(get_lock_for(addr))
        {
            mtx_.lock();
        }
        ~scoped_lock()
        {
            mtx_.unlock();
        }

        BOOST_DELETED_FUNCTION(scoped_lock(scoped_lock const&))
        BOOST_DELETED_FUNCTION(scoped_lock& operator=(scoped_lock const&))
    };

private:
    static BOOST_ATOMIC_DECL lock_type& get_lock_for(const volatile void * addr);
};

#else

class lockpool
{
public:
    typedef atomic_flag lock_type;

    class scoped_lock
    {
    private:
        atomic_flag& flag_;

    public:
        explicit
        scoped_lock(const volatile void * addr) : flag_(get_lock_for(addr))
        {
            for (; flag_.test_and_set(memory_order_acquire);)
            {
#if defined(BOOST_ATOMIC_X86_PAUSE)
                BOOST_ATOMIC_X86_PAUSE();
#endif
            }
        }

        ~scoped_lock(void)
        {
            flag_.clear(memory_order_release);
        }

        BOOST_DELETED_FUNCTION(scoped_lock(const scoped_lock &))
        BOOST_DELETED_FUNCTION(scoped_lock& operator=(const scoped_lock &))
    };

private:
    static BOOST_ATOMIC_DECL lock_type& get_lock_for(const volatile void * addr);
};

#endif

}
}
}

#endif

ext: hpp

max_stars_repo_name: graehl/boost

max_issues_repo_name: graehl/boost

max_forks_repo_name: graehl/boost


text:
// Copyright 2006. Peter Gottschling, Matthias Troyer, Rolf Bonderer

#ifndef LA_ETS_CONCEPTS_INCLUDE
#define LA_ETS_CONCEPTS_INCLUDE

#include <boost/numeric/linear_algebra/concepts.hpp>

#ifdef  __GXX_CONCEPTS__

//"Namespace ETS = Expression Templates Support":
// This file contains concepts that support the concepts defined in the namespace math.
// They are required since the math-concepts do not support ExpressionTemplates so far.
// The list of valid expressions is in fact infinite, so we just name some of them.
// Once ExpressionTemplates are supported by the math-concepts, the ets-concepts are no longer required.

namespace ets {

  auto concept Field<typename Element>
  {
    requires std::Assignable<Element, math::AdditivePartiallyInvertibleMonoid<Element>::unary_result_type>; //"x=-y" valid
  };

  auto concept VectorSpace<typename Vector, typename Scalar>
  {
    // valid expression: "vector2 += scalar*vector1"
    typename res_type_1;
    res_type_1 operator+=(Vector&, math::Multiplicable<Scalar, Vector>::result_type);

    // valid expression: "vector2 -= scalar*vector1"
    typename res_type_2;
    res_type_2 operator-=(Vector&, math::Multiplicable<Scalar, Vector>::result_type);

    //valid expression: "vector *= -scalar"
    typename res_type_3;
    res_type_3 operator*=(Vector&, const math::AdditivePartiallyInvertibleMonoid<Scalar>::unary_result_type&);

    //valid expression: "vector3 = vector1 + scalar*vector2"
    requires math::Addable<Vector, math::Multiplicable<Scalar, Vector>::result_type>; //"vector1+scalar*vector2" valid
    requires std::Assignable<Vector, math::Addable<Vector, math::Multiplicable<Scalar, Vector>::result_type>::result_type>; //"vector3 = vector1 + scalar*vector2" valid

  };

} //namespace ets

#endif //__GXX_CONCEPTS__

#endif //LA_ETS_CONCEPTS_INCLUDE


text:
/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <iostream>
#include <boost/math/tools/roots.hpp>

#include "NOpenMeshType.hpp"
//#include "NOpenMeshDesc.hpp"
#include "NOpenMeshBisect.hpp"

template <typename T>
NOpenMeshBisect<T>::NOpenMeshBisect( const SDF& sdf, const P& a, const P& b , float tolerance )
    :
    func(sdf, a, b ),
    tol(tolerance),
    invalid( func(0)*func(1) > 0 ),  // <-- sdf must have different signs at a and b : ie bracket zero
    degenerate( fabs(func(0)) < tolerance && fabs(func(1)) <  tolerance ),  // <-- no root to find, already there
    iterations(15)
{
}

template <typename T>
void NOpenMeshBisect<T>::bisect( P& frontier, float& t )
{
    std::pair<float, float> root = boost::math::tools::bisect(func, 0.f, 1.f, tol, iterations );

    t = (root.first + root.second)/2. ;

    func.position(frontier, t );
}

template <typename T>
NOpenMeshBisectFunc<T>::NOpenMeshBisectFunc( const SDF& sdf, const P& a, const P& b )
    :
    sdf(sdf),
    a(a),
    b(b)
{
}
template <typename T>
void NOpenMeshBisectFunc<T>::position(P& tp, const float t) const
{
    // parameterized position along a -> b line segment
    //    a(1-t)+t*b   t=0 -> a,   t=1 -> b

    const float s = 1.f - t ;
    tp[0] = a[0]*s + b[0]*t ;
    tp[1] = a[1]*s + b[1]*t ;
    tp[2] = a[2]*s + b[2]*t ;
}

template <typename T>
float NOpenMeshBisectFunc<T>::operator()(const float t) const
{
    // signed distance to CSG left/right/composite object from position on line segement
    P tp ;
    position(tp, t );

    float d = sdf( tp[0], tp[1], tp[2] ) ;

/*
    std::cout << "NOpenMeshBisectFunc<T>"
              << " t " << t
              << " tp " << NOpenMeshDesc<T>::desc_point(tp,8,2)
              << " d " << d
              << std::endl ;
*/

    return d ;
}

template struct NOpenMeshBisectFunc<NOpenMeshType> ;
template struct NOpenMeshBisect<NOpenMeshType> ;

max_stars_repo_name: hanswenzel/opticks

max_issues_repo_name: hanswenzel/opticks

max_forks_repo_name: hanswenzel/opticks


text:
#include <iostream>
#include <chrono>
#include <memory>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Dense>
#include "minimize.h"

using Matrix = minimize::Matrix;
using Vector = minimize::Vector;

// Define interpolation procedure for minimization algorithm
double minimize::interpolate(double x2, double f2, double d2, double x3, double f3, double d3, double f0, double INT, double RHO)
{

  // choose subinterval
  // move point 3 to point 4
  double x4 = x3;
  double f4 = f3;
  double d4 = d3;

  //double tolerance = 1e-32;
  double tolerance = 1e-64;

  if ( f4 > f0 )
    {
      double denom = f4-f2-d2*(x4-x2);
      if ( std::abs(denom) < tolerance )
        // bisect
        x3 = (x2+x4)/2;
      else
        // quadratic interpolation
        x3 = x2-(0.5*d2*std::pow(x4-x2,2))/(denom);
    }
  else
    {
      // cubic interpolation
      double A = 6*(f2-f4)/(x4-x2)+3*(d4+d2);
      double B = 3*(f4-f2)-(2*d2+d4)*(x4-x2);
      double radical = B*B-A*d2*std::pow(x4-x2,2);

      if ( ( radical < 0 ) || ( std::abs(A) < tolerance ) )
        x3 = (x2+x4)/2;
      else
        x3 = x2+( std::sqrt(radical) - B)/A;
    }

  // don't accept too close
  if ( x4-INT*(x4-x2) < x3 )
    x3 = x4-INT*(x4-x2);

  if ( x2+INT*(x4-x2) > x3 )
    x3 = x2+INT*(x4-x2);

  return x3;
};

// Define cubic extrapolation routine for minimization algorithm
double minimize::cubic_extrap(double x1, double x2, double f1, double f2, double d1, double d2, double EXT, double INT)
{
  // make cubic extrapolation
  double A = 6*(f1-f2)+3*(d2+d1)*(x2-x1);
  double B = 3*(f2-f1)-(2*d1+d2)*(x2-x1);

  double x3;
  //double tolerance = 1e-32;
  double tolerance = 1e-64;
  double radical = B*B-A*d1*(x2-x1);

  if ( radical < 0.0 )
    x3 = x2*EXT;
  else if ( B + std::sqrt(radical) < tolerance )
    x3 = x2*EXT;
  else
    {
      x3 = x1-d1*std::pow(x2-x1,2)/( B + std::sqrt(radical) );

      if ( ( x3 < 0 ) || ( x3 > x2*EXT ) )
        x3 = x2*EXT;
      else if ( x3 < x2+INT*(x2-x1) )
        x3 = x2+INT*(x2-x1);
    }

  return x3;
};

//
//  ORIGINAL CODE BY CARL EDWARD RASMUSSEN
//  http://learning.eng.cam.ac.uk/carl/code/minimize/
//
//  % Minimize a differentiable multivariate function.
//  %
//  % Usage: [X, fX, i] = minimize(X, f, length, P1, P2, P3, ... )
//  %
//  % where the starting point is given by "X" (D by 1), and the function named in
//  % the string "f", must return a function value and a vector of partial
//  % derivatives of f wrt X, the "length" gives the length of the run: if it is
//  % positive, it gives the maximum number of line searches, if negative its
//  % absolute gives the maximum allowed number of function evaluations. You can
//  % (optionally) give "length" a second component, which will indicate the
//  % reduction in function value to be expected in the first line-search (defaults
//  % to 1.0). The parameters P1, P2, P3, ... are passed on to the function f.
//  %
//  % The function returns when either its length is up, or if no further progress
//  % can be made (ie, we are at a (local) minimum, or so close that due to
//  % numerical problems, we cannot get any closer). NOTE: If the function
//  % terminates within a few iterations, it could be an indication that the
//  % function values and derivatives are not consistent (ie, there may be a bug in
//  % the implementation of your "f" function). The function returns the found
//  % solution "X", a vector of function values "fX" indicating the progress made
//  % and "i" the number of iterations (line searches or function evaluations,
//  % depending on the sign of "length") used.
//  %
//  % The Polack-Ribiere flavour of conjugate gradients is used to compute search
//  % directions, and a line search using quadratic and cubic polynomial
//  % approximations and the Wolfe-Powell stopping criteria is used together with
//  % the slope ratio method for guessing initial step sizes. Additionally a bunch
//  % of checks are made to make sure that exploration is taking place and that
//  % extrapolation will not be unboundedly large.
//  %
//  % See also: checkgrad
//  %
//  % Copyright (C) 2001 - 2006 by Carl Edward Rasmussen (2006-09-08).
//
//  INT = 0.1;    % don't reevaluate within 0.1 of the limit of the current bracket
//  EXT = 3.0;                  % extrapolate maximum 3 times the current step-size
//  MAX = 20;                         % max 20 function evaluations per line search
//  RATIO = 10;                                       % maximum allowed slope ratio
//  SIG = 0.1; RHO = SIG/2; % SIG and RHO are the constants controlling the Wolfe-
//  % Powell conditions. SIG is the maximum allowed absolute ratio between
//  % previous and new slopes (derivatives in the search direction), thus setting
//  % SIG to low (positive) values forces higher precision in the line-searches.
//  % RHO is the minimum allowed fraction of the expected (from the slope at the
//  % initial point in the linesearch). Constants must satisfy 0 < RHO < SIG < 1.
//  % Tuning of SIG (depending on the nature of the function to be optimized) may
//  % speed up the minimization; it is probably not worth playing much with RHO.
//
//  % The code falls naturally into 3 parts, after the initial line search is
//  % started in the direction of steepest descent. 1) we first enter a while loop
//  % which uses point 1 (p1) and (p2) to compute an extrapolation (p3), until we
//  % have extrapolated far enough (Wolfe-Powell conditions). 2) if necessary, we
//  % enter the second loop which takes p2, p3 and p4 chooses the subinterval
//  % containing a (local) minimum, and interpolates it, unil an acceptable point
//  % is found (Wolfe-Powell conditions). Note, that points are always maintained
//  % in order p0 <= p1 <= p2 < p3 < p4. 3) compute a new search direction using
//  % conjugate gradients (Polack-Ribiere flavour), or revert to steepest if there
//  % was a problem in the previous line-search. Return the best value so far, if
//  % two consecutive line-searches fail, or whenever we run out of function
//  % evaluations or line-searches. During extrapolation, the "f" function may fail
//  % either with an error or returning Nan or Inf, and minimize should handle this
//  % gracefully.
//

// Conjugate gradient minimization algorithm
void minimize::cg_minimize(Vector & X, minimize::GradientObj * target, Vector & D, int length, double SIG, double EXT, double INT, int MAX)
{
  // specify optimization hyperparameters
  //double RATIO = 10.0;
  double RATIO = 100.0;
  double RHO = SIG/2;

  // determine problem dimension
  int N = static_cast<int>(X.size());

  // initialize values
  int i = 0;
  bool ls_failed = false;
  Vector df0(N);
  double f0;
  (*target).computeValueAndGradient(X, f0, df0);

  // initial search direction (steepest) and slope
  Vector s = -df0;
  double d0 = -s.transpose()*s;

  // initial step is 1/(|s|+1)
  double x3 = 1/(1-d0);

  // declare placeholders for storing optimal values
  Vector X0(N);
  double F0;
  Vector dF0(N);

  // declare variables in main loop
  int M;
  bool continue_extrap;
  double x1, f1, d1, x2, f2, d2, f3, d3;
  Vector df3(N);

  // "realmin" = smallest positive normalized floating-point number in IEEE double precision format
  double realmin = 2.2251e-308;

  // MAIN LOOP
  bool request_break = false;
  while ( ( i < length ) && ( !request_break ) )
    {
      i++;

      // make a copy of current values
      X0 = X;
      F0 = f0;
      dF0 = df0;

      // Display current parameter values
      //std::cout << " X  =  " << X.transpose().array().exp().matrix() << std::endl;

      // initialize iteration count
      M = MAX;

      // EXTRAPOLATE
      continue_extrap = true;
      while ( continue_extrap )
        {
          x2 = 0.0;
          f2 = f0;
          d2 = d0;
          M = M - 1;
          (*target).computeValueAndGradient(X+x3*s, f3, df3);

          // keep best values
          if ( f3 < F0 )
            {
              X0 = X+x3*s;
              F0 = f3;
              dF0 = df3;
            }

          // new slope
          d3 = df3.transpose()*s;

          // are we done extrapolating?
          if ( ( ( d3 > SIG*d0 ) || ( f3 > f0+x3*RHO*d0 ) ) || ( M == 0 ) )
              continue_extrap = false;

          // move point 2 to point 1
          x1 = x2;
          f1 = f2;
          d1 = d2;
          // move point 3 to point 2
          x2 = x3;
          f2 = f3;
          d2 = d3;

          // cubic extrapolation
          x3 = cubic_extrap(x1, x2, f1, f2, d1, d2, EXT, INT);

        } // END EXTRAPOLATE

      // INTERPOLATE
      while ( ( ( std::abs(d3) > -SIG*d0 ) || ( f3 > f0+x3*RHO*d0) )  &&  ( M > 0 ) )
        {

          x3 = interpolate(x2, f2, d2, x3, f3, d3, f0, INT, RHO);

          (*target).computeValueAndGradient(X+x3*s, f3, df3);

          // keep best values
          if ( f3 < F0 )
            {
              X0 = X+x3*s;
              F0 = f3;
              dF0 = df3;
            }

          // decrement line-search count
          M = M - 1;

          // new slope
          d3 = df3.transpose()*s;

        } // END INTERPOLATE

      //  START COMPUTE NEW SEARCH DIRECTION
      if ( ( std::abs(d3) < -SIG*d0 ) && ( f3 < f0+x3*RHO*d0 ) )
        {
          // if line search succeeded
          // update variables
          X = X+x3*s;
          f0 = f3;

          // Polack-Ribiere CG direction
          s = ( (df3.transpose()*df3 - df0.transpose()*df3)(0) / (df0.transpose()*df0)(0) )*s - df3;

          // swap derivatives
          df0 = df3;
          d3 = d0;
          d0 = df0.transpose()*s;

          // new slope must be negative
          if ( d0 > 0 )
            {
              // otherwise use steepest direction
              s = -df0;
              d0 = -s.transpose()*s;
            }

          // slope ratio but max RATIO
          if ( RATIO <  d3/(d0-realmin) )
            {
              x3 = x3 * RATIO;
              //std::cout << "\n[*] RATIO parameter enforced\n";
            }
          else
              x3 = x3 * d3/(d0-realmin);

          // this line search did not fail
          ls_failed = false;
        }

      else
        {
          // restore best point so far
          X = X0;
          f0 = F0;
          df0 = dF0;

          // line search failed twice in a row
          // or we ran out of time, so we give up
          if ( ( ls_failed ) || ( i > length ) )
              request_break = true;

          // DEBUGGING INFO TO SEE WHY OPTIMIZATION EXITS EARLY
          /*
          if ( ls_failed )
            {
              std::cout << "\n[*] Line Search Failed  ( i = " << i << " )\n";
              std::cout << "\n The following conditions failed:   [ SIG = " << SIG << " , RHO = " << RHO << " ]\n";
              if ( std::abs(d3) >= -SIG*d0 )
                {
                  double lhs = std::abs(d3);
                  double rhs = -SIG*d0;
                  std::cout << "abs(" << d3 << ")   <   -SIG * " << d0 << " [   i.e. " << lhs << " < " << rhs << " ]\n";
                }
              if ( f3 >= f0+x3*RHO*d0 )
                {
                  double lhs = f3;
                  double rhs = f0+x3*RHO*d0;
                  std::cout <<  f3 << "   <   " << f0 << " + " << x3 << " * RHO * " << d0 << "   [ i.e. " << lhs << " < " << rhs << " ]\n";
                }
            }
          if ( i > length )
            std::cout << "\n[*] Exceeded 'length' value\n";
          */

          // try steepest
          s = -df0;
          d0 = -s.transpose()*s;
          x3 = 1/(1-d0);

          // this line search failed
          ls_failed = true;

        } // END COMPUTE NEW SEARCH DIRECTION

    }  // END MAIN LOOP

  //std::cout << "\nMinimized Function Value (???) :\n";
  //std::cout << f0 << std::endl;

};


text:
//
// Created by jan on 28.10.21.
//

#include "Client.h"
#include "Mumble.pb.h"
#include "protocol/protocol.h"

#include <array>
#include <chrono>
#include <deque>
#include <memory>
#include <mutex>
#include <span>
#include <string>
#include <type_traits>
#include <vector>

#include <asio/ssl.hpp>
#include <asio/ts/net.hpp>
#include <boost/log/core.hpp>
#include <boost/log/expressions.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/manipulators/dump.hpp>

namespace mumble_client::core {

using namespace std::chrono_literals;

void InitLogging() {
#ifdef NDEBUG
	boost::log::core::get()->set_filter(boost::log::trivial::severity >= boost::log::trivial::info);
#else
	boost::log::core::get()->set_filter(boost::log::trivial::severity >= boost::log::trivial::debug);
#endif
}

// Client::Impl

struct Client::Impl {
	static constexpr auto PingPeriod = 20s;

	Impl();

	asio::io_context io_context;
	asio::ssl::context tls_context;
	asio::ssl::stream<asio::ip::tcp::socket> tls_socket;

	asio::steady_timer ping_timer;

	std::array<std::byte, protocol::control::MaxPacketLength> receive_buffer;

	std::deque<std::vector<std::byte>> send_queue;
	std::mutex send_queue_mutex;

	std::unique_ptr<protocol::control::VersionPacket> server_version;

	void ReadCompletionHandler(const std::error_code &ec, std::size_t bytes_transferred);

	void QueueMessage(const std::vector<std::byte> &message);

	void WriteCompletionHandler(const std::error_code &ec, std::size_t bytes_transferred);

	void PingTimerCompletionHandler();

	void Connect(std::string_view user_name, std::string_view server, uint16_t port, bool validate_server_certificate);

	void HandleVersionPacket(std::span<std::byte> payload);

	void SendVersionPacket();

	void SendAuthenticatePaket(std::string_view user_name);

	static void HandlePingPacket(std::span<std::byte> payload);

	static void HandleRejectPacket(std::span<std::byte> payload);

	static void HandleServerSyncPacket(std::span<std::byte> payload);

	static void HandleChannelStatePacket(std::span<std::byte> payload);

	static void HandleUserStatePacket(std::span<std::byte> payload);

	static void HandleCryptSetupPacket(std::span<std::byte> payload);
};

Client::Impl::Impl()
	: io_context(), tls_context(asio::ssl::context::tlsv12), tls_socket(io_context, tls_context),
	  ping_timer(io_context), receive_buffer(), send_queue(), send_queue_mutex(), server_version(nullptr) {}

void Client::Impl::ReadCompletionHandler(const std::error_code &ec, std::size_t bytes_transferred) {
	if (ec) {
		BOOST_LOG_TRIVIAL(error) << "Error reading from socket: " << ec.message();
		throw std::system_error(ec);
	}

	BOOST_LOG_TRIVIAL(debug) << "Read " << bytes_transferred << " bytes from Socket: "
							 << boost::log::dump(receive_buffer.data(), protocol::control::HeaderLength);

	const std::span buffer_span{receive_buffer};
	const protocol::control::Header header{buffer_span};
	BOOST_LOG_TRIVIAL(debug) << "Packet type " << static_cast<uint16_t>(header.packet_type) << ", length "
							 << header.packet_length;

	const auto payload = buffer_span.subspan(protocol::control::HeaderLength, header.packet_length);
	switch (header.packet_type) {
		case protocol::control::PacketType::Version: HandleVersionPacket(payload); break;
		case protocol::control::PacketType::UDPTunnel:
		case protocol::control::PacketType::Authenticate:
			BOOST_LOG_TRIVIAL(error) << "No handler implemented for control packet type: "
									 << static_cast<std::underlying_type_t<protocol::control::PacketType>>(
											header.packet_type);
			break;
		case protocol::control::PacketType::Ping: HandlePingPacket(payload); break;
		case protocol::control::PacketType::Reject: HandleRejectPacket(payload); break;
		case protocol::control::PacketType::ServerSync: HandleServerSyncPacket(payload); break;
		case protocol::control::PacketType::ChannelRemove:
			BOOST_LOG_TRIVIAL(error) << "No handler implemented for control packet type: "
									 << static_cast<std::underlying_type_t<protocol::control::PacketType>>(
											header.packet_type);
			break;
		case protocol::control::PacketType::ChannelState: HandleChannelStatePacket(payload); break;
		case protocol::control::PacketType::UserRemove:
			BOOST_LOG_TRIVIAL(error) << "No handler implemented for control packet type: "
									 << static_cast<std::underlying_type_t<protocol::control::PacketType>>(
											header.packet_type);
			break;
		case protocol::control::PacketType::UserState: HandleUserStatePacket(payload); break;
		case protocol::control::PacketType::BanList:
		case protocol::control::PacketType::TextMessage:
		case protocol::control::PacketType::PermissionDenied:
		case protocol::control::PacketType::ACL:
		case protocol::control::PacketType::QueryUsers:
			BOOST_LOG_TRIVIAL(error) << "No handler implemented for control packet type: "
									 << static_cast<std::underlying_type_t<protocol::control::PacketType>>(
											header.packet_type);
			break;
		case protocol::control::PacketType::CryptSetup: HandleCryptSetupPacket(payload); break;
		case protocol::control::PacketType::ContextActionModify:
		case protocol::control::PacketType::ContextAction:
		case protocol::control::PacketType::UserList:
		case protocol::control::PacketType::VoiceTarget:
		case protocol::control::PacketType::PermissionQuery:
		case protocol::control::PacketType::CodecVersion:
		case protocol::control::PacketType::UserStats:
		case protocol::control::PacketType::RequestBlob:
		case protocol::control::PacketType::ServerConfig:
		case protocol::control::PacketType::SuggestConfig:
			BOOST_LOG_TRIVIAL(error) << "No handler implemented for control packet type: "
									 << static_cast<std::underlying_type_t<protocol::control::PacketType>>(
											header.packet_type);
			break;
	}

	asio::async_read(tls_socket, asio::buffer(receive_buffer), asio::transfer_at_least(protocol::control::HeaderLength),
					 [this](const std::error_code &error, std::size_t bytes_transferred) {
						 ReadCompletionHandler(error, bytes_transferred);
					 });
}

void Client::Impl::QueueMessage(const std::vector<std::byte> &message) {
	if (std::lock_guard{send_queue_mutex}; send_queue.empty()) {
		asio::async_write(tls_socket, asio::buffer(message),
						  [this](const std::error_code &ec, std::size_t bytes_transferred) {
							  WriteCompletionHandler(ec, bytes_transferred);
						  });
	} else {
		send_queue.push_back(message);
	}
}

void Client::Impl::WriteCompletionHandler(const std::error_code &ec, std::size_t bytes_transferred) {
	if (ec) {
		BOOST_LOG_TRIVIAL(error) << "Error writing to socket: " << ec.message();
		throw std::system_error(ec);
	}

	BOOST_LOG_TRIVIAL(debug) << "Wrote " << bytes_transferred << " bytes to socket";

	if (std::lock_guard{send_queue_mutex}; !send_queue.empty()) {
		const auto message = send_queue.front();
		send_queue.pop_front();
		asio::async_write(tls_socket, asio::buffer(message),
						  [this](const std::error_code &ec, std::size_t bytes_transferred) {
							  WriteCompletionHandler(ec, bytes_transferred);
						  });
	}
}

void Client::Impl::PingTimerCompletionHandler() {

	MumbleProto::Ping ping;
	const auto Timestamp = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
	ping.set_timestamp(static_cast<uint64_t>(Timestamp));
	const auto PingAsString = ping.SerializeAsString();
	const auto PingBuffer = std::as_bytes(std::span(PingAsString));

	protocol::control::Header header{protocol::control::PacketType::Ping,
									 static_cast<uint32_t>(std::size(PingAsString))};
	const auto HeaderBuffer = header.Serialize();

	std::vector<std::byte> message{std::cbegin(HeaderBuffer), std::cend(HeaderBuffer)};
	message.insert(std::cend(message), std::cbegin(PingBuffer), std::cend(PingBuffer));

	QueueMessage(message);

	ping_timer.expires_after(PingPeriod);
	ping_timer.async_wait([this](const std::error_code &ec) {
		if (ec) {
			BOOST_LOG_TRIVIAL(error) << "Timer wait failed with " << ec.message();
			throw std::system_error(ec);
		}
		PingTimerCompletionHandler();
	});
}

void Client::Impl::Connect(const std::string_view user_name, const std::string_view server, const uint16_t port,
						   const bool validate_server_certificate) {
	tls_context.set_default_verify_paths();
	tls_socket.set_verify_mode(validate_server_certificate ? asio::ssl::verify_peer : asio::ssl::verify_none);
	tls_socket.set_verify_callback(asio::ssl::host_name_verification(std::string{server}));

	asio::ip::tcp::resolver resolver{io_context};
	const auto &endpoints = resolver.resolve(server, std::to_string(port));

	const auto connected_endpoint = asio::connect(tls_socket.next_layer(), endpoints);
	BOOST_LOG_TRIVIAL(debug) << "Connected to endpoint: " << connected_endpoint.address().to_string()
							 << ", port: " << connected_endpoint.port();
	tls_socket.lowest_layer().set_option(asio::ip::tcp::no_delay(true));
	tls_socket.handshake(asio::ssl::stream_base::client);

	// start periodic ping sender
	ping_timer.expires_after(PingPeriod);
	ping_timer.async_wait([this](const std::error_code &ec) {
		if (ec) {
			BOOST_LOG_TRIVIAL(error) << "Timer wait failed with " << ec.message();
			throw std::system_error(ec);
		}
		PingTimerCompletionHandler();
	});

	asio::async_read(tls_socket, asio::buffer(receive_buffer), asio::transfer_at_least(protocol::control::HeaderLength),
					 [this](const std::error_code &error, std::size_t bytes_transferred) {
						 ReadCompletionHandler(error, bytes_transferred);
					 });

	// begin Mumble handshake protocol
	SendVersionPacket();
	SendAuthenticatePaket(user_name);
}

void Client::Impl::HandleVersionPacket(const std::span<std::byte> payload) {
	MumbleProto::Version version;
	version.ParseFromArray(payload.data(), static_cast<int>(payload.size_bytes()));

	BOOST_LOG_TRIVIAL(debug) << "Version packet: " << version.DebugString();
	server_version = std::make_unique<protocol::control::VersionPacket>(version.version(), version.release(),
																		version.os(), version.os_version());
}

void Client::Impl::SendVersionPacket() {
	protocol::control::VersionPacket::Version client_version{1, 3, 4};
	const uint32_t NumericVersion(client_version);

	MumbleProto::Version version{};
	version.set_version(NumericVersion);
	version.set_release("1.3.4");
	version.set_os("Linux");
	version.set_os_version("5.4.32");
	const auto VersionAsString = version.SerializeAsString();
	const auto VersionBuffer = std::as_bytes(std::span(VersionAsString));

	protocol::control::Header header{protocol::control::PacketType::Version,
									 static_cast<uint32_t>(std::size(VersionAsString))};
	const auto HeaderBuffer = header.Serialize();

	std::vector<std::byte> message{std::cbegin(HeaderBuffer), std::cend(HeaderBuffer)};
	message.insert(std::cend(message), std::cbegin(VersionBuffer), std::cend(VersionBuffer));

	QueueMessage(message);
}

void Client::Impl::SendAuthenticatePaket(const std::string_view user_name) {
	MumbleProto::Authenticate authenticate{};
	authenticate.set_username(std::string(user_name));
	authenticate.set_opus(true);
	const auto AuthenticateAsString = authenticate.SerializeAsString();
	const auto AuthenticateBuffer = std::as_bytes(std::span(AuthenticateAsString));

	protocol::control::Header header{protocol::control::PacketType::Authenticate,
									 static_cast<uint32_t>(std::size(AuthenticateAsString))};
	const auto HeaderBuffer = header.Serialize();

	std::vector<std::byte> message{std::cbegin(HeaderBuffer), std::cend(HeaderBuffer)};
	message.insert(std::cend(message), std::cbegin(AuthenticateBuffer), std::cend(AuthenticateBuffer));

	QueueMessage(message);
}

void Client::Impl::HandlePingPacket(const std::span<std::byte> payload) {
	MumbleProto::Ping ping;
	ping.ParseFromArray(payload.data(), static_cast<int>(payload.size_bytes()));

	BOOST_LOG_TRIVIAL(trace) << "Received server ping: " << std::endl << ping.DebugString();
}

void Client::Impl::HandleRejectPacket(std::span<std::byte> payload) {
	MumbleProto::Reject reject;
	reject.ParseFromArray(payload.data(), static_cast<int>(payload.size_bytes()));

	BOOST_LOG_TRIVIAL(debug) << "Received server reject: " << std::endl << reject.DebugString();
}

void Client::Impl::HandleServerSyncPacket(std::span<std::byte> payload) {
	MumbleProto::ServerSync server_sync;
	server_sync.ParseFromArray(payload.data(), static_cast<int>(payload.size_bytes()));

	BOOST_LOG_TRIVIAL(debug) << "Received server sync: " << std::endl << server_sync.DebugString();
}

void Client::Impl::HandleChannelStatePacket(std::span<std::byte> payload) {
	MumbleProto::ChannelState channel_state;
	channel_state.ParseFromArray(payload.data(), static_cast<int>(payload.size_bytes()));

	BOOST_LOG_TRIVIAL(debug) << "Received channel state: " << std::endl << channel_state.DebugString();
}

void Client::Impl::HandleUserStatePacket(std::span<std::byte> payload) {
	MumbleProto::UserState user_state;
	user_state.ParseFromArray(payload.data(), static_cast<int>(payload.size_bytes()));

	BOOST_LOG_TRIVIAL(debug) << "Received user state: " << std::endl << user_state.DebugString();
}

void Client::Impl::HandleCryptSetupPacket(std::span<std::byte> payload) {
	MumbleProto::CryptSetup crypt_setup;
	crypt_setup.ParseFromArray(payload.data(), static_cast<int>(payload.size_bytes()));

	BOOST_LOG_TRIVIAL(debug) << "Received crypt setup: " << std::endl << crypt_setup.DebugString();
}

// Client

Client::Client(std::string_view user_name, const std::string_view server, const uint16_t port,
			   const bool validate_server_certificate)
	: PImpl(std::make_unique<Client::Impl>()) {
	// Verify that the version of the library that we linked against is
	// compatible with the version of the headers we compiled against.
	GOOGLE_PROTOBUF_VERIFY_VERSION;

	PImpl->Connect(user_name, server, port, validate_server_certificate);
}

Client::~Client() = default;

void Client::operator()() {
	InitLogging();

	PImpl->io_context.run();
}
}// namespace mumble_client::core


text:
/**
   \file
   \author Shin'ichiro Nakaoka
*/

#include "Camera.h"
#include <boost/make_shared.hpp>

using namespace cnoid;

Camera::Camera()
{
    on_ = true;
    imageType_ = COLOR_IMAGE;
    isShotDataSetAsState_ = false;
    resolutionX_ = 640;
    resolutionY_ = 480;
    fieldOfView_ = 0.785398;
    nearDistance_ = 0.01;
    farDistance_ = 100.0;
    frameRate_ = 30.0;
    image_ = boost::make_shared<Image>();
}

void Camera::copyStateFrom(const Camera& other)
{
    VisionSensor::copyStateFrom(other);

    on_ = other.on_;
    imageType_ = other.imageType_;
    resolutionX_ = other.resolutionX_;
    resolutionY_ = other.resolutionY_;
    fieldOfView_ = other.fieldOfView_;
    nearDistance_ = other.nearDistance_;
    farDistance_ = other.farDistance_;
    frameRate_ = other.frameRate_;

    if(other.isShotDataSetAsState_){
        image_ = other.image_;
    } else {
        image_ = boost::make_shared<Image>();
    }
}

void Camera::copyStateFrom(const DeviceState& other)
{
    if(typeid(other) != typeid(Camera)){
        throw std::invalid_argument("Type mismatch in the Device::copyStateFrom function");
    }
    copyStateFrom(static_cast<const Camera&>(other));
}

Camera::Camera(const Camera& org, bool copyAll)
    : VisionSensor(org, copyAll)
{
    isShotDataSetAsState_ = org.isShotDataSetAsState_;
    copyStateFrom(org);
}

DeviceState* Camera::cloneState() const
{
    return new Camera(*this, false);
}

Device* Camera::clone() const
{
    return new Camera(*this);
}

void Camera::forEachActualType(boost::function<bool(const std::type_info& type)> func)
{
    if(!func(typeid(Camera))){
        VisionSensor::forEachActualType(func);
    }
}

Image& Camera::image()
{
    if(image_.use_count() > 1){
        image_ = boost::make_shared<Image>(*image_);
    }
    return *image_;
}

Image& Camera::newImage()
{
    image_ = boost::make_shared<Image>();
    return *image_;
}

void Camera::setImage(boost::shared_ptr<Image>& image)
{
    if(image.use_count() == 1){
        image_ = image;
    } else {
        image_ = boost::make_shared<Image>(*image);
    }
    image.reset();
}

void Camera::clearState()
{
    if(image_.use_count() == 1){
        image_->clear();
    } else {
        image_ = boost::make_shared<Image>();
    }
}

int Camera::stateSize() const
{
    return 7;
}

const double* Camera::readState(const double* buf)
{
    buf = VisionSensor::readState(buf);
    on_ = buf[0];
    resolutionX_ = buf[1];
    resolutionY_ = buf[2];
    nearDistance_ = buf[3];
    farDistance_ = buf[4];
    fieldOfView_ = buf[5];
    frameRate_ = buf[6];
    return buf + 7;
}

double* Camera::writeState(double* out_buf) const
{
    out_buf = VisionSensor::writeState(out_buf);
    out_buf[0] = on_ ? 1.0 : 0.0;
    out_buf[1] = resolutionX_;
    out_buf[2] = resolutionY_;
    out_buf[3] = nearDistance_;
    out_buf[4] = farDistance_;
    out_buf[5] = fieldOfView_;
    out_buf[6] = frameRate_;
    return out_buf + 7;
}

RangeCamera::RangeCamera()
{
    setImageType(NO_IMAGE);
    points_ = boost::make_shared<PointData>();
    isOrganized_ = false;
}

void RangeCamera::copyStateFrom(const RangeCamera& other)
{
    Camera::copyStateFrom(other);

    if(other.isShotDataSetAsState()){
        points_ = other.points_;
    } else {
        points_ = boost::make_shared<PointData>();
    }
    isOrganized_ = other.isOrganized_;
}

void RangeCamera::copyStateFrom(const DeviceState& other)
{
    if(typeid(other) != typeid(RangeCamera)){
        throw std::invalid_argument("Type mismatch in the Device::copyStateFrom function");
    }
    copyStateFrom(static_cast<const RangeCamera&>(other));
}

RangeCamera::RangeCamera(const RangeCamera& org, bool copyAll)
    : Camera(org, copyAll)
{
    if(org.isShotDataSetAsState()){
        points_ = org.points_;
    } else {
        points_ = boost::make_shared<PointData>();
    }
    isOrganized_ = org.isOrganized_;
}

DeviceState* RangeCamera::cloneState() const
{
    return new RangeCamera(*this, false);
}

Device* RangeCamera::clone() const
{
    return new RangeCamera(*this);
}

void RangeCamera::forEachActualType(boost::function<bool(const std::type_info& type)> func)
{
    if(!func(typeid(RangeCamera))){
        Camera::forEachActualType(func);
    }
}

RangeCamera::PointData& RangeCamera::points()
{
    if(points_.use_count() > 1){
        points_ = boost::make_shared<PointData>(*points_);
    }
    return *points_;
}

RangeCamera::PointData& RangeCamera::newPoints()
{
    points_ = boost::make_shared<PointData>();
    return *points_;
}

void RangeCamera::setPoints(boost::shared_ptr<PointData>& points)
{
    if(points.use_count() == 1){
        points_ = points;
    } else {
        points_ = boost::make_shared<PointData>(*points);
    }
    points.reset();
}

void RangeCamera::clearState()
{
    Camera::clearState();

    if(points_.use_count() == 1){
        points_->clear();
    } else {
        points_ = boost::make_shared<PointData>();
    }
}

void RangeCamera::setOrganized(bool on)
{
    if(on != isOrganized_){
        clearState();
    }
    isOrganized_ = on;
}

max_stars_repo_name: snozawa/choreonoid

max_issues_repo_name: snozawa/choreonoid

max_forks_repo_name: snozawa/choreonoid


text:
#define BOOST_TEST_MODULE "test_read_lennard_jones_wall_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/input/read_external_potential.hpp>
#include <tuple>

using test_types = std::tuple<double, float>;

constexpr inline float  tolerance_value(float)  noexcept {return 1e-4;}
constexpr inline double tolerance_value(double) noexcept {return 1e-8;}

template<typename Real>
decltype(boost::test_tools::tolerance(std::declval<Real>()))
tolerance() {return boost::test_tools::tolerance(tolerance_value(Real()));}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_lennard_jones_wall_noenv, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_lennard_jones_wall.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction    = "Distance"
            potential      = "LennardJonesWall"
            shape.name     = "AxisAlignedPlane"
            shape.axis     = "+X"
            shape.position = 1.0
            shape.margin   = 0.5
            parameters     = [
                {index =  0, sigma = 2.0, epsilon = 1.5},
                {index =  2, "σ"   = 2.0, epsilon = 1.5},
                {index =  9, sigma = 2.0, "ε"     = 1.5},
                {index = 10, "σ"   = 2.0, "ε"     = 1.5},
            ]
        )"_toml;

        const auto g = mjolnir::read_lennard_jones_wall_potential<real_type>(v);

        BOOST_TEST(g.participants().size() ==  4u);
        BOOST_TEST(g.participants().at(0)  ==  0u);
        BOOST_TEST(g.participants().at(1)  ==  2u);
        BOOST_TEST(g.participants().at(2)  ==  9u);
        BOOST_TEST(g.participants().at(3)  == 10u);

        BOOST_TEST(g.parameters().at( 0).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 0).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.cutoff() == real_type(2.5), tolerance<real_type>());
    }

    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction    = "Distance"
            potential      = "LennardJonesWall"
            shape.name     = "AxisAlignedPlane"
            shape.axis     = "+X"
            shape.position = 1.0
            shape.margin   = 0.5
            cutoff = 3.14
            parameters     = [
                {index =  0, sigma = 2.0, epsilon = 1.5},
                {index =  2, "σ"   = 2.0, epsilon = 1.5},
                {index =  9, sigma = 2.0, "ε"     = 1.5},
                {index = 10, "σ"   = 2.0, "ε"     = 1.5},
            ]
        )"_toml;

        const auto g = mjolnir::read_lennard_jones_wall_potential<real_type>(v);

        BOOST_TEST(g.participants().size() ==  4u);
        BOOST_TEST(g.participants().at(0)  ==  0u);
        BOOST_TEST(g.participants().at(1)  ==  2u);
        BOOST_TEST(g.participants().at(2)  ==  9u);
        BOOST_TEST(g.participants().at(3)  == 10u);

        BOOST_TEST(g.parameters().at( 0).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).first  == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 0).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).second == real_type(1.5), tolerance<real_type>());
        BOOST_TEST(g.cutoff() == real_type(3.14), tolerance<real_type>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_lennard_jones_wall_env, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_lennard_jones_wall.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction    = "Distance"
            potential      = "LennardJonesWall"
            shape.name     = "AxisAlignedPlane"
            shape.axis     = "+X"
            shape.position = 1.0
            shape.margin   = 0.5
            env.large  = 10.0
            env.small  = 0.01
            env.strong = 100.0
            env.weak   = 0.001
            cutoff = 3.14
            parameters     = [
                {index =  0, sigma = "large", epsilon = "strong"},
                {index =  2, "σ"   = "large", epsilon = "weak"},
                {index =  9, sigma = "small", "ε"     = "strong"},
                {index = 10, "σ"   = "small", "ε"     = "weak"},
            ]
        )"_toml;

        const auto g = mjolnir::read_lennard_jones_wall_potential<real_type>(v);

        BOOST_TEST(g.participants().size() ==  4u);
        BOOST_TEST(g.participants().at(0)  ==  0u);
        BOOST_TEST(g.participants().at(1)  ==  2u);
        BOOST_TEST(g.participants().at(2)  ==  9u);
        BOOST_TEST(g.participants().at(3)  == 10u);

        BOOST_TEST(g.parameters().at( 0).first  == real_type(10.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).first  == real_type(10.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).first  == real_type(0.01), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).first  == real_type(0.01), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 0).second == real_type(100.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 2).second == real_type(0.001), tolerance<real_type>());
        BOOST_TEST(g.parameters().at( 9).second == real_type(100.0), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(10).second == real_type(0.001), tolerance<real_type>());
        BOOST_TEST(g.cutoff() == real_type(3.14), tolerance<real_type>());
    }
}

max_issues_repo_name: Mjolnir-MD/Mjolnir


text:
// Copyright 2014 Hottinger Baldwin Messtechnik
// Distributed under MIT license
// See file LICENSE provided

#ifndef _WIN32
#define BOOST_TEST_DYN_LINK
#endif
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE socketnonblocking tests

#include <boost/test/unit_test.hpp>

#include <string>
#include <functional>
#include <memory>

#include "hbm/communication/socketnonblocking.h"
#include "hbm/communication/tcpserver.h"
#include "socketnonblocking_test.h"
#include "hbm/sys/eventloop.h"
#include "hbm/sys/timer.h"

#ifndef _WIN32
static const char server[] = "::1";
#else
static const char server[] = "127.0.0.1";

#endif

namespace hbm {
	namespace communication {
		namespace test {

			static const unsigned int PORT = 22222;

			static void executionTimerCb(bool fired, hbm::sys::EventLoop& eventloop)
			{
				if (fired) {
					eventloop.stop();
				}
			}

			serverFixture::serverFixture()
				: m_server(m_eventloop)
			{
				BOOST_TEST_MESSAGE("setup Fixture1");
				int result = m_server.start(PORT, 3, std::bind(&serverFixture::acceptCb, this, std::placeholders::_1));
				BOOST_CHECK_MESSAGE(result == 0, strerror(errno));
			}

			serverFixture::~serverFixture()
			{
				BOOST_TEST_MESSAGE("teardown serverFixture");
				stop();
			}

			size_t serverFixture::getClientCount() const
			{
				return m_workers.size();
			}

			void serverFixture::start()
			{
				m_serverWorker = std::async(std::launch::async, std::bind(&hbm::sys::EventLoop::execute, std::ref(m_eventloop)));
			}

			void serverFixture::stop()
			{
				m_eventloop.stop();
				try {
					m_serverWorker.wait();
				} catch (...) {
				}

			}

			void serverFixture::acceptCb(clientSocket_t worker)
			{
				static int clientId = 0;
				++clientId;
				m_workers[clientId] = std::move(worker);
				m_workers[clientId]->setDataCb(std::bind(&serverFixture::serverEcho, this, clientId));
			}

			ssize_t serverFixture::serverEcho(int clientId)
			{
				char buffer[1024];
				ssize_t result;

				do {
					result = m_workers[clientId]->receive(buffer, sizeof(buffer));
					if (result>0) {
						result = m_workers[clientId]->sendBlock(buffer, result, false);
					} else if (result==0) {
						// socket got closed
						m_workers.erase(clientId);
					} else {
#ifdef _WIN32
						int retVal = WSAGetLastError();
						if ((retVal != WSAEWOULDBLOCK) && (retVal != ERROR_IO_PENDING) && (retVal != WSAEINPROGRESS)) {
#else
						if ((errno != EAGAIN) && (errno != EWOULDBLOCK)) {
#endif

							// a real error
							m_workers.erase(clientId);
						}
					}
				} while (result>0);
				return result;
			}

			int serverFixture::clientReceive(hbm::communication::SocketNonblocking& socket)
			{
				char buffer[1024];
				ssize_t result;

				result = socket.receive(buffer, sizeof(buffer));
				if (result>0) {
					m_answer += buffer;
				}

				return result;
			}

			int serverFixture::clientReceiveTarget(hbm::communication::SocketNonblocking& socket, std::string& target)
			{
				char buffer[1024];
				ssize_t result;

				result = socket.receive(buffer, sizeof(buffer));
				if (result>0) {
					target += buffer;
				}

				return result;
			}

			int serverFixture::clientReceiveSingleBytes(hbm::communication::SocketNonblocking& socket)
			{
				char buffer;
				ssize_t result;

				result = socket.receive(&buffer, sizeof(buffer));
				if (result > 0) {
					if (buffer != '\0') {
						m_answer += buffer;
					}
				}

				return result;
			}

		BOOST_AUTO_TEST_CASE(check_leak)
		{
			static const std::chrono::milliseconds waitDuration(1);

			char readBuffer[1024] = "";

#ifdef _WIN32
			DWORD fdCountBefore;
			DWORD fdCountAfter;

			{
				// Do one create and destruct cycle under windows before retrieving the number of handles before. This is important beacuse a lot of handles will be created on the first run.
				hbm::sys::EventLoop eventloop;
				hbm::sys::Timer executionTimer(eventloop);
				hbm::communication::SocketNonblocking socket(eventloop);
				executionTimer.set(waitDuration, false, std::bind(&executionTimerCb, std::placeholders::_1, std::ref(eventloop)));
				eventloop.execute();
			}
			GetProcessHandleCount(GetCurrentProcess(), &fdCountBefore);
#else
			unsigned int fdCountBefore;
			unsigned int fdCountAfter;

			FILE* pipe;
			std::string cmd;
			pid_t processId = getpid();

			// the numbe of file descriptors of this process
			cmd = "ls -1 /proc/" + std::to_string(processId) + "/fd | wc -l";
			pipe = popen(cmd.c_str(), "r");
			if(fgets(readBuffer, sizeof(readBuffer), pipe)==nullptr) {
				BOOST_ASSERT("Could not get number of fds of process");
			}
			fdCountBefore = std::stoul(readBuffer);
			fclose(pipe);
#endif

			for (unsigned cycle = 0; cycle<10; ++cycle) {
				hbm::sys::EventLoop eventloop;
				hbm::sys::Timer executionTimer(eventloop);
				hbm::communication::SocketNonblocking socket(eventloop);
				executionTimer.set(waitDuration, false, std::bind(&executionTimerCb, std::placeholders::_1, std::ref(eventloop)));
				eventloop.execute();
			}

#ifdef _WIN32
			GetProcessHandleCount( GetCurrentProcess(), &fdCountAfter);
#else
			pipe = popen(cmd.c_str(), "r");
			if(fgets(readBuffer, sizeof(readBuffer), pipe)==nullptr) {
				BOOST_ASSERT("Could not get number of fds of process");
			}
			fdCountAfter = std::stoul(readBuffer);
			fclose(pipe);
#endif

			BOOST_CHECK_EQUAL(fdCountBefore, fdCountAfter);
		}

			BOOST_FIXTURE_TEST_SUITE( socket_test, serverFixture )

			BOOST_AUTO_TEST_CASE(connect_test)
			{
				int result;
				start();
				hbm::communication::SocketNonblocking client(m_eventloop);
				for (unsigned int cycleIndex = 0; cycleIndex < 1000; ++cycleIndex) {
					result = client.connect(server, std::to_string(PORT));
					BOOST_CHECK_MESSAGE(result == 0, strerror(errno));

					client.disconnect();
				}

				// wrong port! Should fail
				result = client.connect(server, std::to_string(PORT + 1));
				BOOST_CHECK_MESSAGE(result == -1, strerror(errno));
			}

			BOOST_AUTO_TEST_CASE(connect_multiple_test)
			{
				static const unsigned int CLIENT_COUNT = 32;
				static const unsigned int CYCLE_COUNT = 100;

				int result;
				start();

				std::vector < std::unique_ptr < hbm::communication::SocketNonblocking > > clients;

				for (size_t clientIndex = 0; clientIndex < CLIENT_COUNT; ++clientIndex) {
					clients.emplace_back(std::make_unique < hbm::communication::SocketNonblocking >(m_eventloop));
				}

				for (unsigned int cycleIndex = 0; cycleIndex < CYCLE_COUNT; ++cycleIndex) {
					for (unsigned int clientIndex = 0; clientIndex < CLIENT_COUNT; ++clientIndex) {
						result = clients[clientIndex]->connect(server, std::to_string(PORT));
						BOOST_CHECK_MESSAGE(result == 0, strerror(errno));

						clients[clientIndex]->disconnect();
					}
				}
			}

			BOOST_AUTO_TEST_CASE(send_recv_test)
			{
				int result;
				static const char msg[] = "hallo";
				char response[1024];

				start();

				hbm::communication::SocketNonblocking client(m_eventloop);
				result = client.connect(server, std::to_string(PORT));
				BOOST_CHECK_MESSAGE(result == 0, strerror(errno));

				result = client.send(msg, sizeof(msg), false);
				std::this_thread::sleep_for(std::chrono::milliseconds(1));
				BOOST_CHECK_MESSAGE(result == sizeof(msg), strerror(errno));
				result = client.receive(response, sizeof(msg));
				BOOST_CHECK_MESSAGE(result == sizeof(msg), strerror(errno));
				BOOST_CHECK_EQUAL(response, msg);

				result = client.send(msg, sizeof(msg), false);
				std::this_thread::sleep_for(std::chrono::milliseconds(1));
				BOOST_CHECK_MESSAGE(result == sizeof(msg), strerror(errno));
				for( unsigned int cycleIndex=0; cycleIndex<sizeof(msg); ++cycleIndex) {
					result = client.receive(response, 1);
					BOOST_CHECK_MESSAGE(result == 1, strerror(errno));
					BOOST_CHECK_EQUAL(response[0], msg[cycleIndex]);
				}
				result = client.receive(response, 1);
				BOOST_CHECK_EQUAL(result, -1);

				client.disconnect();

				stop();
			}

			BOOST_AUTO_TEST_CASE(sendblock_recvblock_test)
			{
				int result;
				static const char msg[] = "hallo";
				char response[1024];

				start();

				hbm::communication::SocketNonblocking client(m_eventloop);
				//client.setDataCb(std::bind(&serverFixture::clientReceiveSingleBytes, this, std::placeholders::_1));
				result = client.connect(server, std::to_string(PORT));
				BOOST_CHECK_MESSAGE(result == 0, strerror(errno));

				for( unsigned int cycleIndex=0; cycleIndex<100; ++cycleIndex) {
					result = client.sendBlock(msg, sizeof(msg), false);
					BOOST_CHECK_MESSAGE(result == sizeof(msg), strerror(errno));
					result = client.receiveComplete(response, sizeof(msg), 100);
					BOOST_CHECK_MESSAGE(result == sizeof(msg), strerror(errno));
					BOOST_CHECK_EQUAL(response, msg);
				}

				result = client.sendBlock(msg, sizeof(msg), false);
				BOOST_CHECK_MESSAGE(result == sizeof(msg), strerror(errno));
				for( unsigned int cycleIndex=0; cycleIndex<sizeof(msg); ++cycleIndex) {
					result = client.receiveComplete(response, 1, 100);
					BOOST_CHECK_MESSAGE(result == 1, strerror(errno));
					BOOST_CHECK_EQUAL(response[0], msg[cycleIndex]);
				}
				result = client.receiveComplete(response, 1, 100);
				BOOST_CHECK_EQUAL(result, -1);

				// force timeout!
				result = client.receiveComplete(response, sizeof(msg), 100);
				BOOST_CHECK_EQUAL(result, -1);

				// nothing to read and no time to wait
				result = client.receiveComplete(response, sizeof(msg), 0);
				BOOST_CHECK_EQUAL(result, -1);

				client.disconnect();

				stop();
			}

			BOOST_AUTO_TEST_CASE(echo_test)
			{
				int result;
				static const char msg[] = "hallo";
				static const char msg2[] = "!";

				start();

				hbm::communication::SocketNonblocking client(m_eventloop);
				client.setDataCb(std::bind(&serverFixture::clientReceiveSingleBytes, this, std::placeholders::_1));
				result = client.connect(server, std::to_string(PORT));
				BOOST_CHECK_MESSAGE(result == 0, strerror(errno));

				clearAnswer();
				result = client.sendBlock(msg, sizeof(msg), false);
				BOOST_CHECK_MESSAGE(result == sizeof(msg), strerror(errno));
				std::this_thread::sleep_for(std::chrono::milliseconds(100));
				std::string answer = getAnswer();
				BOOST_CHECK_EQUAL(answer, msg);

				clearAnswer();
				result = client.sendBlock(msg2, sizeof(msg2), false);
				BOOST_CHECK_MESSAGE(result == sizeof(msg2), strerror(errno));
				std::this_thread::sleep_for(std::chrono::milliseconds(100));
				answer = getAnswer();
				BOOST_CHECK_EQUAL(answer, msg2);

				clearAnswer();
				result = client.sendBlock(msg, sizeof(msg), false);
				result = client.sendBlock(msg2, sizeof(msg2), false);

				std::this_thread::sleep_for(std::chrono::milliseconds(100));
				answer = getAnswer();
				BOOST_CHECK_EQUAL(answer, std::string(msg) + msg2);

				client.disconnect();

				stop();
			}

			BOOST_AUTO_TEST_CASE(multiclient_echo_test)
			{
				static const size_t clientCount = 10;
				int result;
				static const std::string msgPrefix = "hallo";

				start();

				std::vector < std::unique_ptr < hbm::communication::SocketNonblocking > > clients;

				for (size_t clientIndex=0; clientIndex<clientCount; ++clientIndex) {
					clients.emplace_back( std::make_unique < hbm::communication::SocketNonblocking> (m_eventloop));
				}

				unsigned int index = 0;
				for (auto &iter: clients) {
					std::string msg = msgPrefix + std::to_string(index++);
					result = iter->connect(server, std::to_string(PORT));
					BOOST_CHECK_MESSAGE(result == 0, strerror(errno));
					iter->setDataCb(std::bind(&serverFixture::clientReceiveSingleBytes, this, std::placeholders::_1));

					clearAnswer();
					result = iter->sendBlock(msg.c_str(), msg.length(), false);
					BOOST_CHECK_MESSAGE(static_cast < size_t > (result) == msg.length(), strerror(errno));
					std::this_thread::sleep_for(std::chrono::milliseconds(100));
					std::string answer = getAnswer();
					BOOST_CHECK_EQUAL(msg, answer);
				}

				for (auto &iter: clients) {
					iter->disconnect();
				}

				stop();
			}

#ifndef _WIN32
			// under windows tcpserver does not support ipv6 yet
			BOOST_AUTO_TEST_CASE(echo_test_ipv6)
			{
				int result;
				static const char msg[] = "hallo";

				start();

				hbm::communication::SocketNonblocking client(m_eventloop);
				result = client.connect("::1", std::to_string(PORT));
				BOOST_CHECK_MESSAGE(result == 0, strerror(errno));
				client.setDataCb(std::bind(&serverFixture::clientReceiveSingleBytes, this, std::placeholders::_1));

				clearAnswer();
				result = client.sendBlock(msg, sizeof(msg), false);
				BOOST_CHECK_MESSAGE(result == sizeof(msg), strerror(errno));
				std::this_thread::sleep_for(std::chrono::milliseconds(100));
				std::string answer = getAnswer();
				BOOST_CHECK_EQUAL(answer, msg);

				client.disconnect();

				stop();
			}
#endif

			BOOST_AUTO_TEST_CASE(setting_data_callback)
			{
				int result;
				static const char msg[] = "hallo";

				start();

				hbm::communication::SocketNonblocking client(m_eventloop);
				result = client.connect(server, std::to_string(PORT));
				BOOST_CHECK_MESSAGE(result == 0, strerror(errno));
				client.setDataCb(std::bind(&serverFixture::clientReceiveSingleBytes, this, std::placeholders::_1));

				clearAnswer();
				result = client.sendBlock(msg, sizeof(msg), false);
				BOOST_CHECK_MESSAGE(result == sizeof(msg), strerror(errno));
				std::this_thread::sleep_for(std::chrono::milliseconds(100));
				std::string answer = getAnswer();
				BOOST_CHECK_EQUAL(answer, msg);

				// set callback function again
				client.setDataCb(std::bind(&serverFixture::clientReceive, this, std::placeholders::_1));
				clearAnswer();
				result = client.sendBlock(msg, sizeof(msg), false);

				std::this_thread::sleep_for(std::chrono::milliseconds(100));

				BOOST_CHECK_MESSAGE(result == sizeof(msg), strerror(errno));
				std::this_thread::sleep_for(std::chrono::milliseconds(100));
				answer = getAnswer();
				clearAnswer();
				BOOST_CHECK_EQUAL(answer, msg);

				client.disconnect();

				stop();
			}

			BOOST_AUTO_TEST_CASE(writev_test_list)
			{
				ssize_t result;
				static const size_t bufferSize = 100000;
				static const size_t blockCount = 10;
				static const size_t blockSize = bufferSize/blockCount;
				char buffer[bufferSize] = "a";
				uint8_t smallBuffer[] = {"hallo"};

				hbm::communication::dataBlocks_t dataBlocks;

				for(unsigned int i=0; i<blockCount; ++i) {
					hbm::communication::dataBlock_t dataBlock(&buffer[i*blockSize], blockSize);
					dataBlocks.push_back(dataBlock);
				}

				start();

				hbm::communication::SocketNonblocking client(m_eventloop);
				result = client.connect(server, std::to_string(PORT));
				BOOST_CHECK_MESSAGE(result == 0, strerror(errno));

				client.setDataCb(std::bind(&serverFixture::clientReceive, this, std::placeholders::_1));

				clearAnswer();
				result = client.sendBlocks(dataBlocks);
				std::this_thread::sleep_for(std::chrono::milliseconds(10));
				BOOST_CHECK_MESSAGE(result == bufferSize, strerror(errno));

				clearAnswer();

				hbm::communication::dataBlock_t smallDatablocks[4];
				size_t bufferCount = sizeof(smallDatablocks)/sizeof(hbm::communication::dataBlock_t);

				for (size_t blockIndex = 0; blockIndex<bufferCount; ++blockIndex) {
					smallDatablocks[blockIndex].pData = smallBuffer;
					smallDatablocks[blockIndex].size = sizeof(smallBuffer);
				}

				result = client.sendBlocks(smallDatablocks, bufferCount, true);
				BOOST_CHECK_MESSAGE((size_t)result == sizeof(smallBuffer)*bufferCount, strerror(errno));
				result = client.sendBlocks(smallDatablocks, bufferCount, false);
				BOOST_CHECK_MESSAGE((size_t)result == sizeof(smallBuffer)*bufferCount, strerror(errno));

				client.disconnect();

				stop();
			}

#define BLOCKCOUNT 10

			BOOST_AUTO_TEST_CASE(writev_test_array)
			{
				int result;
				static const size_t bufferSize = 100000;
				static const size_t blockSize = bufferSize/BLOCKCOUNT;
				char buffer[bufferSize] = "a";

				hbm::communication::dataBlock_t dataBlockArray[BLOCKCOUNT];
				hbm::communication::dataBlocks_t dataBlocks;

				for(unsigned int i=0; i<BLOCKCOUNT; ++i) {
					dataBlockArray[i].pData = &buffer[i*blockSize];
					dataBlockArray[i].size = blockSize;
				}

				start();

				hbm::communication::SocketNonblocking client(m_eventloop);
				result = client.connect(server, std::to_string(PORT));
				BOOST_CHECK_MESSAGE(result == 0, strerror(errno));

				client.setDataCb(std::bind(&serverFixture::clientReceive, this, std::placeholders::_1));

				clearAnswer();
				result = client.sendBlocks(dataBlockArray, 10);
				std::this_thread::sleep_for(std::chrono::milliseconds(10));
				client.disconnect();

				BOOST_CHECK_MESSAGE(result == bufferSize, strerror(errno));

				stop();
			}

			BOOST_AUTO_TEST_CASE(receive_wouldblock)
			{
				char buffer[1000];
				hbm::communication::SocketNonblocking client(m_eventloop);
				client.connect(server, std::to_string(PORT));

				ssize_t result = client.receive(buffer, sizeof(buffer));
				BOOST_CHECK_EQUAL(result, -1);
#ifndef _WIN32
				BOOST_CHECK_EQUAL(errno, EWOULDBLOCK);
#else
				BOOST_CHECK_EQUAL(WSAGetLastError(), WSA_IO_PENDING);
#endif
			}

#ifndef _WIN32
			BOOST_AUTO_TEST_CASE(send_wouldblock)
			{
				size_t bytesSend = 0;
				unsigned int sendBufferSize = 1000;
				socklen_t len;
				char data[1000000] = { 0 };
				hbm::communication::SocketNonblocking client(m_eventloop);
				client.connect(server, std::to_string(PORT));
				unsigned int notifiyCount = 0;

				auto lambda = [&notifiyCount](hbm::communication::SocketNonblocking&)
				{
					++notifiyCount;
					return 0;
				};

				client.setOutDataCb(lambda);
				// callback function gets called once to check for pending work...
				BOOST_CHECK_EQUAL(notifiyCount, 1);

				int result = setsockopt(client.getEvent(), SOL_SOCKET, SO_SNDBUF, &sendBufferSize, sizeof(sendBufferSize));
				BOOST_CHECK_EQUAL(result, 0);
				len = sizeof(sendBufferSize);
				getsockopt(client.getEvent(), SOL_SOCKET, SO_SNDBUF, &sendBufferSize, &len);

				static const unsigned int CYCLECOUNT = 10;
				for (unsigned int cycleIndex=0; cycleIndex<CYCLECOUNT; ++cycleIndex) {
					notifiyCount = 0;
					ssize_t sendResult;
					// send until send buffer is full
					do {
						sendResult = client.send(data, sizeof(data), false);
						bytesSend += static_cast < size_t > (sendResult);
					} while (sendResult>0);
					BOOST_CHECK_GE(bytesSend, sendBufferSize);
					BOOST_CHECK_EQUAL(errno, EWOULDBLOCK);

					// server is not receiving, hence socket won't become writable and callback function is not being called.
					BOOST_CHECK_EQUAL(notifiyCount, 0);
					start();
					// server is receiving. As a result socket becomes wriable again. Callback function is called!
					std::this_thread::sleep_for(std::chrono::milliseconds(100));

					BOOST_CHECK_EQUAL(notifiyCount, 1);

					// We should be able to send again
					sendResult = client.send(data, sizeof(data), false);
					BOOST_CHECK_GT(bytesSend, 0);
					stop();
				}

			}
#endif

			BOOST_AUTO_TEST_SUITE_END()
		}
	}
}

max_stars_repo_name: mloy/hbm

max_issues_repo_name: mloy/hbm

max_forks_repo_name: mloy/hbm