text:
/**
 * Copyright (C) 2015 Dato, Inc.
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD license. See the LICENSE file for details.
 */
#include <cassert>
#include <sstream>
#include <zmq.h>
#include <boost/algorithm/string.hpp>
#include <boost/integer_traits.hpp>
#include <util/md5.hpp>

namespace libfault {
int SEND_TIMEOUT = 3000;
int RECV_TIMEOUT = 7000;

void set_send_timeout(int ms) {
  SEND_TIMEOUT = ms;
}
void set_recv_timeout(int ms) {
  RECV_TIMEOUT = ms;
}

void set_conservative_socket_parameters(void* z_socket) {
  int lingerval = 500;
  int timeoutms = 500;
  int hwm = 0;
  int rc = zmq_setsockopt(z_socket, ZMQ_LINGER, &lingerval, sizeof(lingerval));
  assert(rc == 0);
  rc = zmq_setsockopt(z_socket, ZMQ_RCVTIMEO, &timeoutms, sizeof(timeoutms));
  assert(rc == 0);
  rc = zmq_setsockopt(z_socket, ZMQ_SNDTIMEO, &timeoutms, sizeof(timeoutms));
  assert(rc == 0);

  rc = zmq_setsockopt(z_socket, ZMQ_SNDHWM, &hwm, sizeof(hwm));
  assert(rc == 0);
  rc = zmq_setsockopt(z_socket, ZMQ_RCVHWM, &hwm, sizeof(hwm));
  assert(rc == 0);

}

/**
 * Given a string, returns a zeromq localhost tcp address (ex:
 * tcp://127.15.21.22:11111).
 *
 * On windows we don't get zeromq IPC sockets. Hence, the easiest thing to do
 * is to remap ipc sockets to tcp addresses.
 *
 * When the server is started with address=default mode, on *nix systems we
 * map to ipc://something or order. Instead, on windows systems
 * we must map to tcp://[arbitrary local IP] where there is optimally,
 * a 1-1 correspondence between the local IP and the PID.
 *
 * So, here are the rules:
 * - We cannot generate any port number <= 1024
 * - Lets about 127.0.0.1 because too many stuff like to live there
 * - 127.0.0.0 is invalid. (network address)
 * - 127.255.255.255 is invalid (broadcast address)
 */
std::string hash_string_to_tcp_address(const std::string& s) {
  std::string md5sum = graphlab::md5_raw(s);
  // we get approximately 5 bytes of entropy (yes really somewhat less)

  unsigned char addr[4];
  addr[0] = 127;
  addr[1] = md5sum[0];
  addr[2] = md5sum[1];
  addr[3] = md5sum[2];
  uint16_t port = (uint16_t)(md5sum[3]) * 256 + md5sum[4];

  // validate
  if ((addr[1] == 0 && addr[2] == 0 && addr[3] == 0) ||  // network address
      (addr[1] == 0 && addr[2] == 0 && addr[3] == 1) ||  // common address
      (addr[1] == 255 && addr[2] == 255 && addr[3] == 255) ||  // broadcast
      (port <= 1024)) { // bad port
    // bad. this is network name
    // rehash
    return hash_string_to_tcp_address(md5sum);
  }

  // ok generate the string
  std::stringstream strm;
  strm << "tcp://" << (int)(addr[0]) << "."
                   << (int)(addr[1]) << "."
                   << (int)(addr[2]) << "."
                   << (int)(addr[3]) << ":"
                   << (int)(port);
  return strm.str();
}

std::string normalize_address(const std::string& address) {
#ifdef _WIN32
  if (boost::starts_with(address, "ipc://")) {
    return hash_string_to_tcp_address(address);
  } else {
    return address;
  }
#endif
  return address;
}
};

max_stars_repo_name: parquette/ParFrame

max_issues_repo_name: parquette/ParFrame

max_forks_repo_name: parquette/ParFrame


text:
///////////////////////////////////////
///////////////////////////////////////
///////////////////////////////////////
//
// Created by root on 01.12.17.
//
#include <mutex>
#include <vector>
#include <iostream>
#include <thread>
#include <enet/enet.h>
#include <cstring>
#include <portaudio.h>
#include <opus/opus.h>
#include <boost/circular_buffer.hpp>

using namespace std;
using namespace chrono;

#define SAMPLE opus_int16

#define CHECK(x) { if(!(x)) { \
fprintf(stderr, "%s:%i: failure at: %s\n", __FILE__, __LINE__, #x); \
_exit(1); } }

enum MsgId {
    MsgSoundData    = 0x0,
    MsgDataRequest  = 0x1,
    MsgDataFaster   = 0x2,
    MsgDataSlower   = 0x3,
    MsgDataPing     = 0x4,
};

struct MsgHeader {
    MsgId id;
    enet_uint32 seqnum;
    enet_uint32 flags;
    enet_uint32 misc2;
};

OpusDecoder *decoder;
ENetPeer *peer;
ENetHost *host;
bool optionReliable = true;

enet_uint16 PORT            = 59010;
int         SAMPLE_RATE     = 48000;
int         FRAME_SIZE      = 480;
int         CHANNELS        = 2;
size_t      MAX_FRAME_SIZE  = 0;
#define LEN(a) (sizeof(a)/sizeof(*(a)))

boost::circular_buffer<SAMPLE> CaptureBuffer;

bool isIdle = true;

int openedTimes = 0;

/// FIXME:XXX: portaudio not working at all
static int playCallback(
        [[gnu::unused]] const void *inputBuffer,
        [[gnu::unused]] void *outputBuffer,
        [[gnu::unused]] unsigned long framesPerBuffer,
        [[gnu::unused]] const PaStreamCallbackTimeInfo *timeInfo,
        [[gnu::unused]] PaStreamCallbackFlags statusFlags,
        [[gnu::unused]] void *userData)
{
    SAMPLE *out = (SAMPLE *) outputBuffer;

    ENetEvent event;
    MsgHeader *h;
    SAMPLE *b;
    int b_frames = 0;
    int status;
    while (true) {
        status = enet_host_service(host, &event, 1000);
        if (status < 0) {
            puts("Error returned from service call");
            continue;
        } else if (status == 0) {
            break;
        } else
            break;
    }

    switch(event.type) {
        case ENET_EVENT_TYPE_RECEIVE:
            h = (MsgHeader *)(event.packet->data);
            b = (SAMPLE *)(event.packet->data + sizeof(MsgHeader));
            b_frames = (int)(event.packet->dataLength-sizeof(MsgHeader))/CHANNELS/sizeof(SAMPLE);
            //todo msgheader contains channels
            for(int i=0; i<FRAME_SIZE*CHANNELS; i++) {
                CaptureBuffer.push_back(b[i]);
            }

            if (CaptureBuffer.empty())
                isIdle = true;
            else if (CaptureBuffer.full())
                isIdle = false;

            if (isIdle) {
                break;
            }

            for(size_t i=0; i < framesPerBuffer*CHANNELS;) {
                out[i++] = CaptureBuffer.front();
                CaptureBuffer.pop_front();
            }

            enet_packet_destroy(event.packet);
            return paContinue;

        case ENET_EVENT_TYPE_DISCONNECT:
            puts("Disconnection succeeded.");
            break;

        case ENET_EVENT_TYPE_NONE:
            puts("Timeout while waiting ENet event.");
            break;

        case ENET_EVENT_TYPE_CONNECT:
            MsgHeader header;
            memset(&header, 0, sizeof(header));
            header.id = MsgDataRequest;
            header.flags = 0x1;

            //TODO while(true): обрабатываем запросы на сервер, например, с командной строки
            ENetPacket *packet = enet_packet_create((void *)&header, sizeof(header), ENET_PACKET_FLAG_RELIABLE);
            enet_peer_send(peer, 0, packet);
            enet_host_flush(host);
            clog<<"Connection success"<<endl;
            break;
    }

    //memset(out, 0, sizeof(SAMPLE)*framesPerBuffer*CHANNELS);
    return paContinue;
}

PaStream *Open() {
    /* Initialize. */
    CHECK(Pa_Initialize() == paNoError);

    PaStreamParameters output_parameters;
    output_parameters.device = Pa_GetDefaultOutputDevice();
    CHECK(output_parameters.device != paNoDevice);
    CHECK(output_parameters.device != paUseHostApiSpecificDeviceSpecification);
    output_parameters.channelCount = CHANNELS;
    output_parameters.sampleFormat = paInt16;
    output_parameters.hostApiSpecificStreamInfo = NULL;

    const PaDeviceInfo *input_info = Pa_GetDeviceInfo(output_parameters.device);
    CHECK(input_info != nullptr);
    output_parameters.suggestedLatency = input_info->defaultLowOutputLatency;

    /* Create and start stream */
    PaStream *stream;
    PaError ret = Pa_OpenStream(&stream,
                                NULL,
                                &output_parameters,
                                input_info->defaultSampleRate,
                                (unsigned)(FRAME_SIZE),
                                paClipOff,
                                playCallback,
                                NULL);

    if (ret != paNoError) {
        fprintf(stderr, "Pa_OpenStream failed: (err %i) %s\n", ret, Pa_GetErrorText(ret));
        if (stream)
            Pa_CloseStream(stream);
        return nullptr;
    }

    CHECK(Pa_StartStream(stream) == paNoError);
    openedTimes++;

    const PaStreamInfo *streamInfo = Pa_GetStreamInfo(stream);
    clog<<"[!] Audio stream info: sample rate = "<<streamInfo->sampleRate
            <<", input latency = "<<streamInfo->inputLatency<<" sec"<<endl;

    return stream;
}

int main(int argc, char *argv[]) {
    int err;
    OpusDecoder *decoder = opus_decoder_create(SAMPLE_RATE, CHANNELS, &err);
    if (!decoder || err != OPUS_OK) {
        std::cerr << "Opus decoder create failed: " << opus_strerror(err) << std::endl;
        return EXIT_FAILURE;
    }

    std::clog << "Starting network client" << std::endl;
    if (enet_initialize() != 0) {
        std::cerr << "An error occurred while initializing ENet" << std::endl;
        return EXIT_FAILURE;
    }
    atexit(enet_deinitialize);

    errno = 0;
    host = enet_host_create(
            nullptr,  // the address to bind the server host to
            1,        // allow up to 32 clients and/or outgoing connections
            1,        // allow up to 2 channels to be used, 0 and 1
            0,        // assume any amount of incoming bandwidth
            0         // assume any amount of outgoing bandwidth
    );
    if (!host) {
        std::cerr << "An error occurred while trying to create an ENet client host: " << std::strerror(errno) << std::endl;
        return EXIT_FAILURE;
    }

    ENetAddress address;
    const char* ip = "192.168.31.21";
    enet_address_set_host(&address, ip);
    address.port = PORT;

    //подключаемся
    errno = 0;
    peer = enet_host_connect(host, &address, 2, 0/*42*/);
    if (!peer) {
        std::cerr << "No available peers for initiating an ENet connection: " << std::strerror(errno) << std::endl;
        return EXIT_FAILURE;
    }

    ENetEvent event;
    ssize_t ping = -1;
    high_resolution_clock::time_point t1;

    while (ping < 0) {
        errno = 0;
        int status = enet_host_service(host, &event, 1);
        if (status < 0) {
            std::cerr << "Error returned from service call: " << std::strerror(errno) << std::endl;
            return EXIT_FAILURE;
        }

        switch(event.type) {
            case ENET_EVENT_TYPE_CONNECT: {
                MsgHeader header{};
                // allow server to send us audio
                header.id = MsgDataRequest;
                header.flags = optionReliable?0x1:0;  // reliable or best-effort mode
                ENetPacket *packet = enet_packet_create(&header, sizeof(header), ENET_PACKET_FLAG_RELIABLE);
                enet_peer_send(peer, 0, packet);
                enet_host_flush(host);

                // send server ping request
                header.id = MsgDataPing;
                packet = enet_packet_create((void *)&header, sizeof(header), ENET_PACKET_FLAG_RELIABLE);
                enet_peer_send(peer, 0, packet);
                enet_host_flush(host);
                t1 = high_resolution_clock::now();
                clog<<"ping sent"<<endl;
                break;
            }
            case ENET_EVENT_TYPE_RECEIVE: {
                MsgHeader *header = reinterpret_cast<MsgHeader *>(event.packet->data + 0);
                switch(header->id) {
                    case MsgSoundData:break;
                    case MsgDataRequest:break;
                    case MsgDataFaster:break;
                    case MsgDataSlower:break;
                    case MsgDataPing:
                        ping = duration_cast<duration<typeof(ping)>>(high_resolution_clock::now() - t1).count();
                        clog<<"ping: "<<ping<<" ms."<<endl;
                        clog<<"formula: samples = ms * sample_rate"<<endl;
                        MAX_FRAME_SIZE = (static_cast<size_t>((ping*SAMPLE_RATE)/FRAME_SIZE)+1)*FRAME_SIZE;
                        clog<<"formula: finally = "<<MAX_FRAME_SIZE<<endl;

                        //allocate memory
                        CaptureBuffer.resize(MAX_FRAME_SIZE);
                        clog<<"CaptureBuffer.size():  "<<CaptureBuffer.size()<<endl;
                        break;
                }
                break;
            }
            case ENET_EVENT_TYPE_DISCONNECT:
                return -111;

            case ENET_EVENT_TYPE_NONE:
                break;
        }
    }

    PaStream *stream = Open();
    while (true) {
        if (Pa_IsStreamActive(stream)) {
            usleep(10*1000);
        } else {
            Pa_StopStream(stream);
            stream = Open();
            if (!stream)
                break;
        }
    }
}

ext: cc

max_stars_repo_name: ivanstepanovftw/output-audio-stream

max_issues_repo_name: ivanstepanovftw/output-audio-stream

max_forks_repo_name: AshFTW/output-audio-stream


text:
#pragma once

#include <terark/valvec.hpp>
#include <terark/util/refcount.hpp>
#include <boost/noncopyable.hpp>

namespace terark {

class SingleLruReadonlyCache;
class  MultiLruReadonlyCache;
class TERARK_DLL_EXPORT LruReadonlyCache : public RefCounter {
public:
	class Buffer : private boost::noncopyable {
        friend class SingleLruReadonlyCache;
        friend class  MultiLruReadonlyCache;
		enum CacheType : unsigned char {
			hit,
			evicted_others,
			initial_free,
			dropped_free,
			hit_others_load,
			mix, // for multi page only
		};
        SingleLruReadonlyCache* owner;
        valvec<byte_t>*         rdbuf;
		uint32_t  index; // index == 0 indicate not ref any page
		CacheType cache_type;
	//	byte_t    reserved;
	//	uint16_t  missed_pages;
        void discard_impl();
    public:
        explicit
         Buffer(valvec<byte_t>* rb) : rdbuf(rb), index(0) { assert(rb); }
        ~Buffer() { discard(); }
        void discard() { if (index) discard_impl(); }
	};
	static LruReadonlyCache*
	create(size_t totalcapacityBytes, size_t shards, size_t maxFiles, bool aio);

	virtual const byte_t* pread(intptr_t fi, size_t offset, size_t len, Buffer*) = 0;
	virtual intptr_t open(intptr_t fd) = 0;
	virtual void close(intptr_t fi) = 0;
	virtual bool safe_close(intptr_t fi) = 0;
	virtual void print_stat_cnt(FILE*) const = 0;
};

TERARK_DLL_EXPORT
void fdpread(intptr_t fd, void* buf, size_t len, size_t offset);

} // namespace terark

ext: hpp

max_stars_repo_name: rockeet/terark-zip

max_issues_repo_name: rockeet/terark-zip

max_forks_repo_name: rockeet/terark-zip


text:
#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
//#include <boost/math/special_functions/binomial.hpp>
#include <cmath>
#include <Eigen/Dense>
#include <limits>
#include <vector>

#include <fstream>

#include "kalman_filter.hpp"
#include "kalman_filter_cache.hpp"
#include "model.hpp"
#include "partition.hpp"
#include "io.hpp"

//using boost::math::binomial_coefficient;

namespace biggles
{

namespace model
{

namespace detail
{

// log multivariate gamma function with p == 2
inline float lgamma_2(float x)
{
    // see http://en.wikipedia.org/wiki/Multivariate_gamma_function
    return 0.5f*logf(M_PI) + lgammaf(x) + lgammaf(x-0.5f);
}

// log of the inverse Wishart PDF.
inline float log_inverse_wishart_pdf(const Eigen::Matrix2f& B, const Eigen::Matrix2f& Phi, int m)
{
    // see http://en.wikipedia.org/wiki/Inverse-Wishart_distribution

    float log_det_B = logf(B.determinant());
    float log_det_Phi = logf(Phi.determinant());
    float trace_Phi_B_inv = (Phi * B.inverse()).trace();

    return 0.5f*m*log_det_Phi - 0.5f*(m+2+1)*log_det_B - 0.5f*trace_Phi_B_inv - 0.5f*(2*m)*logf(2.f) - lgamma_2(0.5f*m);
}

}

/// \brief calculates p(partition | clutter rate)
///
/// This function seems to be uncontroversial
float log_partition_given_clutter_rate_density(const partition& part_sample, const model::parameters& para_sample) {
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    /*
    std::vector<size_t> clutter_counts(part_duration);
    BOOST_FOREACH(const observation& obs, part_sample.clutter()) {
        ++clutter_counts[t(obs) - part_first_ts];
    }
    */
    float lambda_f = clutter_rate(para_sample);
    /*
    if (not std::isfinite(lambda_f)) {
        std::stringstream sstr;
        sstr << "clutter rate is not finite: " << lambda_f;
        throw std::runtime_error(sstr.str()) ;
    }
    */
    BOOST_ASSERT(std::isfinite(lambda_f));
    BOOST_ASSERT(lambda_f >= 0);
    float log_part_clutter = part_sample.clutter().size() * logf(lambda_f) - part_duration * lambda_f;
    for (clutter_t::const_iterator it = part_sample.clutter().begin(); it not_eq part_sample.clutter().end(); ++it) {
        log_part_clutter += -lgammaf(1.f + it->second.size());
    }
    return log_part_clutter;
}

float log_partition_given_survival_prob_density(const partition& part_sample, const model::parameters& para_sample) {
    binomial_coefficient& binom_coeff = binomial_coefficient::get();
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    const int min_surv = 0; // number of guaranteed survivals
    std::vector<size_t> survival_events(part_duration);
    std::vector<size_t> death_events(part_duration);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        BOOST_ASSERT(t_ptr->duration() > min_surv);
        time_stamp track_first_ts = t_ptr->first_time_stamp();
        time_stamp track_last_ts = t_ptr->last_time_stamp();
        for (time_stamp ts = track_first_ts - part_first_ts + 1 + min_surv; ts < track_last_ts - part_first_ts; ++ts)
            survival_events[ts]++;
        if (track_last_ts < part_last_ts)
            death_events[track_last_ts - part_first_ts]++;
    }
    float log_part_surv = 0.f;
    float log_p_s = logf(survival_probability(para_sample));
    float log_1m_p_s = logf(1.f - survival_probability(para_sample));
    for ( time_stamp i = 0; i < part_duration; ++i) {
        log_part_surv += logf(binom_coeff(survival_events[i] + death_events[i], survival_events[i])) +
            survival_events[i] * log_p_s + death_events[i] * log_1m_p_s;
    }
    return log_part_surv;
}

float log_partition_given_observation_prob_density(const partition& part_sample, const model::parameters& para_sample) {
    binomial_coefficient& binom_coeff = binomial_coefficient::get();
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    const size_t min_obs = 0; //number of guaranteed observations
    std::vector<size_t> track_events(part_duration);
    std::vector<size_t> observation_events(part_duration);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        time_stamp track_first_ts = t_ptr->first_time_stamp();
        time_stamp track_last_ts = t_ptr->last_time_stamp();
        for (time_stamp ts = track_first_ts - part_first_ts; ts < track_last_ts - part_first_ts; ++ts)
            track_events[ts]++;
        BOOST_ASSERT(t_ptr->observations().size() >= min_obs);
        observation_collection::const_iterator obs_iter = t_ptr->observations().begin();
        for (size_t i = 0; i < min_obs; ++i) {
            track_events[t(*obs_iter) - part_first_ts]--;
            obs_iter++;
        }
        for (; obs_iter != t_ptr->observations().end(); ++obs_iter)
            observation_events[t(*obs_iter) - part_first_ts]++;
    }
    float log_part_obs = 0.f;
    float log_p_o = logf(observation_probability(para_sample));
    float log_1m_p_o = logf(1.f - observation_probability(para_sample));
    for (time_stamp i = 0; i < part_duration; ++i) {
        log_part_obs += logf(binom_coeff(track_events[i], observation_events[i])) +
            observation_events[i] * log_p_o + (track_events[i] - observation_events[i]) * log_1m_p_o;
    }
    return log_part_obs;
}

float log_partition_given_birth_rate_density(const partition& part_sample, const model::parameters& para_sample) {
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    const int min_surv = 0; // number of guaranteed survivals
    std::vector<size_t> birth_events(part_duration);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        BOOST_ASSERT(t_ptr->duration() > min_surv);
        time_stamp track_first_ts = t_ptr->first_time_stamp();
        birth_events[track_first_ts - part_first_ts]++;
    }
    float lambda_b = birth_rate(para_sample);
    size_t n_total_born(part_sample.tracks().size());
    float log_part_birth = n_total_born * logf(lambda_b) - (part_duration - min_surv) * lambda_b;
    // TODO: the value for lgammaf may be stored and recalled rather than recalculated; could be faster.
    for (time_stamp i = 0; i < part_duration; ++i)
        log_part_birth += -lgammaf(1.f + birth_events[i]);
    return log_part_birth;
}

float log_partition_given_parameters_density_orig(const partition& partition, const model::parameters& parameters)
{
    float log_pdf = 0.f;

    off_t n_frames = partition.duration();

    // ensemble statistics over all time stamps
    off_t n_total_born(partition.tracks().size());
    off_t n_total_false(partition.clutter().size());
    off_t n_total_died(partition.tracks().size()); // everything that has a beginning has an end, Neo.
    off_t n_total_survived(0);
    off_t n_total_generated(0);

    // calculate n_total_survived and n_total_generated. record birth times for tracks
    std::vector<size_t> birth_counts(partition.last_time_stamp() - partition.first_time_stamp());
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, partition.tracks())
    {
        BOOST_ASSERT(t_ptr->duration() > 1);
        // a track 'survives' for one fewer ticks than it's duration
        n_total_survived += t_ptr->duration() - 1;

        // the track's size is the number of observations within it
        n_total_generated += t_ptr->size();

        // record birth time
        BOOST_ASSERT(t_ptr->first_time_stamp() >= partition.first_time_stamp());
        BOOST_ASSERT(t_ptr->first_time_stamp() - partition.first_time_stamp() < static_cast<time_stamp>(birth_counts.size()));
        ++birth_counts[t_ptr->first_time_stamp() - partition.first_time_stamp()];
    }

    // record clutter counts
    std::vector<size_t> clutter_counts(partition.last_time_stamp() - partition.first_time_stamp());
    for (clutter_t::const_iterator it = partition.clutter().begin(); it not_eq partition.clutter().end(); ++it) {
        clutter_counts[it->first - partition.first_time_stamp()] = it->second.size();
    }

    // p_s term
    float p_s = frame_to_frame_survival_probability(parameters);
    BOOST_ASSERT(std::isfinite(p_s) and p_s >= 0);
    log_pdf += n_total_survived * logf(p_s) + n_total_died * logf(1.f - p_s);

    // p_d term
    float p_d = generate_observation_probability(parameters);
    BOOST_ASSERT(std::isfinite(p_d) and p_d >= 0);
    log_pdf += n_total_generated * logf(p_d) + (n_total_survived + n_total_born - n_total_generated) * logf(1.f - p_d);

    // start of lambda_b term
    float lambda_b = mean_new_tracks_per_frame(parameters);
    BOOST_ASSERT(std::isfinite(lambda_b) and lambda_b >= 0);
    log_pdf += n_total_born * logf(lambda_b) - n_frames * lambda_b; // ... still to calculate log gamma terms

    // start of lambda_f term
    float lambda_f = mean_false_observations_per_frame(parameters);
    if (not std::isfinite(lambda_f)) {
        std::stringstream sstr;
        sstr << "clutter rate is not finite: " << lambda_f;
        throw std::runtime_error(sstr.str()) ; // TODO debug cleaning up
    }
    BOOST_ASSERT(std::isfinite(lambda_f));
    BOOST_ASSERT(lambda_f >= 0);
    log_pdf += n_total_false * logf(lambda_f) - n_frames * lambda_f; // ... still to calculate log gamma terms

    // finish lambda_b and lambda_f terms
    for(time_stamp ts = partition.first_time_stamp(); ts < partition.last_time_stamp(); ++ts)
    {
        // finish lambda_b term
        log_pdf += -lgammaf(1.f + birth_counts[ts - partition.first_time_stamp()]);

        // finish lambda_f term
        log_pdf += -lgammaf(1.f + clutter_counts[ts - partition.first_time_stamp()]);
    }

    return log_pdf; // the rest is just a repeat of P(T|clutter_rate)

}

float log_partition_given_parameters_density(
        const partition& partition_sample, const model::parameters& parameter_sample)
{
    return log_partition_given_parameters_density_orig(partition_sample, parameter_sample);
}

float log_clutter_given_parameters_density(const partition& part, const model::parameters& parameters) {
    return -logf(part.volume())*part.clutter().size();
}

float log_track_given_parameters_density(const boost::shared_ptr<const track>& track_p, const model::parameters& parameters)
{
    return track_p->log_posterior(parameters);
}

float log_parameters_prior_density(const model::parameters& parameters)
{
    // finite regions of support:
    if(mean_new_tracks_per_frame(parameters) <= 0.f)
        return -std::numeric_limits<float>::max();
    if(mean_false_observations_per_frame(parameters) <= 0.f)
        return -std::numeric_limits<float>::max();
    if(frame_to_frame_survival_probability(parameters) < 0.f)
        return -std::numeric_limits<float>::max();
    if(frame_to_frame_survival_probability(parameters) > 1.f)
        return -std::numeric_limits<float>::max();
    if(generate_observation_probability(parameters) < 0.f)
        return -std::numeric_limits<float>::max();
    if(generate_observation_probability(parameters) > 1.f)
        return -std::numeric_limits<float>::max();

    /*
    if (mean_false_observations_per_frame(parameters) != 0.1f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    if (mean_new_tracks_per_frame(parameters) != .5f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    if (generate_observation_probability(parameters) != .9f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    if (frame_to_frame_survival_probability(parameters) != .9f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    */
    // prior on R:
    // FIXME PRIOR calculation
    /*
    if ( observation_error_covariance(parameters) == Eigen::Matrix2f::Identity()* 0.09f ) {
        return 0.f;
    } else {
        return -std::numeric_limits<float>::max();
    }
    */
    return detail::log_inverse_wishart_pdf(observation_error_covariance(parameters), 2.f * Eigen::Matrix2f::Identity(), 5);
}

float log_likelihood(const partition& part_sample, const model::parameters& para_sample) {
    float log_pdf(0.f);

    // track PDFs
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks())
    {
        log_pdf += log_track_given_parameters_density(t_ptr, para_sample);
    }

    // clutter PDF
    log_pdf += log_clutter_given_parameters_density(part_sample, para_sample);
    return log_pdf;
}

float log_tracks_given_parameters_density(const partition& part, const model::parameters& parameters) {
    // track PDFs
    float log_pdf(0.f);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part.tracks())
    {
        float track_log_pdf = log_track_given_parameters_density(t_ptr, parameters);
        if (not std::isfinite(track_log_pdf)) {
            std::cerr << "track duration " << t_ptr->duration() << std::endl;
            std::cerr << "track size " << t_ptr->size() << std::endl;
        }
        log_pdf += track_log_pdf;
    }
    return log_pdf;
}

void dump_part_para(const partition& part, const model::parameters& para) {
    size_t ctr = 0;
    std::string fname = boost::str(boost::format("no_finite_log_pdf.%04d.json") % ctr);
    while (boost::filesystem::exists(fname)) {
        ctr++;
        if (ctr > 9999) {
            std::cerr << "nothing dumped :(" << std::endl;
            return;
        }
        fname = boost::str(boost::format("no_finite_log_pdf.%04d.json") % ctr);
    }
    std::ofstream fh(fname.c_str());
    fh << "{ \"parameters\": ";
    io::write_model_parameters_to_json_stream(fh, para);
    fh << ", \"partition\": " << std::endl;
    io::write_partition_to_json_stream(fh, part);
    fh << "}" << std::endl;
}

float log_partition_given_parameters_and_data_density(const partition& part, const model::parameters& parameters)
{
    float log_pdf(0.f);
    float temp(0.f);

    // clutter PDF
    temp = log_clutter_given_parameters_density(part, parameters);
    if (not std::isfinite(temp)) {
        std::cerr << " *** clutter log density not finite" << std::endl;
        std::cerr << "     " << part.volume() << std::endl;
        dump_part_para(part, parameters);
    }
    log_pdf += temp;

    temp = log_tracks_given_parameters_density(part, parameters);
    if (not std::isfinite(temp)) {
        std::cerr << " *** track log density not finite" << std::endl;
        std::cerr << temp << std::endl;
        std::cerr << model::observation_error_covariance(parameters) << std::endl;
        dump_part_para(part, parameters);
    }
    log_pdf += temp;

    //log_pdf = 0.f; // FIXME PRIOR test get rid of this line again

    // data-independent track PDF
    temp = log_partition_given_parameters_density(part, parameters);
    if (not std::isfinite(temp)) {
        std::cerr << " *** partition log density not finite" << std::endl;
        dump_part_para(part, parameters);
    }
    log_pdf += temp;

    // prior
    // the prior is only need to record the log pdf and not for the sampling itself.
    // log_pdf += log_parameters_prior_density(parameters);

    return log_pdf;
}

}

}

ext: cpp

max_stars_repo_name: fbi-octopus/biggles

max_issues_repo_name: fbi-octopus/biggles

max_forks_repo_name: fbi-octopus/biggles


text:
// Copyright (c) 2017, Xiaomi, Inc.  All rights reserved.
// This source code is licensed under the Apache License Version 2.0, which
// can be found in the LICENSE file in the root directory of this source tree.

#include <algorithm>
#include <memory>
#include <string>
#include <boost/asio.hpp>

#include <dsn/utility/string_conv.h>
#include <dsn/utility/rand.h>

#include <gtest/gtest.h>
#include <rrdb/rrdb.client.h>
#include <pegasus_utils.h>
#include "proxy_layer.h"
#include "redis_parser.h"

using namespace boost::asio;
using namespace ::pegasus::proxy;

class proxy_test_app : public ::dsn::service_app
{
public:
    explicit proxy_test_app(const dsn::service_app_info *info) : service_app(info) {}

    ::dsn::error_code start(const std::vector<std::string> &args) override
    {
        if (args.size() < 3) {
            return ::dsn::ERR_INVALID_PARAMETERS;
        }

        proxy_session::factory f = [](proxy_stub *p, dsn::message_ex *m) {
            return std::make_shared<redis_parser>(p, m);
        };
        _proxy = dsn::make_unique<proxy_stub>(f, args[1].c_str(), args[2].c_str());
        return ::dsn::ERR_OK;
    }
    ::dsn::error_code stop(bool) override { return ::dsn::ERR_OK; }

private:
    std::unique_ptr<pegasus::proxy::proxy_stub> _proxy;
};

class redis_test_parser : public redis_parser
{
public:
    redis_test_parser(proxy_stub *stub, dsn::message_ex *msg) : redis_parser(stub, msg)
    {
        _reserved_entry.reserve(20);
        for (int i = 0; i < 20; ++i) {
            _reserved_entry.emplace_back(new message_entry());
        }
        _entry_index = 0;
        _got_a_message = false;
    }

    void reset()
    {
        _got_a_message = false;
        _entry_index = 0;
    }

    void set_msg(int index, redis_request msg)
    {
        _reserved_entry[index]->request.sub_request_count = msg.sub_request_count;
        _reserved_entry[index]->request.sub_requests = std::move(msg.sub_requests);
    }

    static dsn::message_ex *create_message(const char *data)
    {
        return dsn::message_ex::create_received_request(
            RPC_CALL_RAW_MESSAGE, dsn::DSF_THRIFT_BINARY, (void *)data, strlen(data));
    }

    static dsn::message_ex *create_message(const char *data, int length)
    {
        return dsn::message_ex::create_received_request(
            RPC_CALL_RAW_MESSAGE, dsn::DSF_THRIFT_BINARY, (void *)data, length);
    }

    static dsn::message_ex *marshalling_array(const redis_request &request)
    {
        dsn::message_ex *msg = create_message("dummy");

        dsn::message_ex *resp = msg->create_response();
        ::dsn::rpc_write_stream stream(resp);

        stream.write_pod('*');
        std::string count_str = std::to_string(request.sub_request_count);
        stream.write(count_str.c_str(), count_str.length());
        stream.write_pod(CR);
        stream.write_pod(LF);
        for (const auto &sub_request : request.sub_requests) {
            sub_request.marshalling(stream);
        }

        msg->release_ref();
        return resp;
    }

protected:
    void handle_command(std::unique_ptr<message_entry> &&entry) override
    {
        redis_request &act_request = entry->request;
        redis_request &exp_request = _reserved_entry[_entry_index]->request;

        ASSERT_TRUE(act_request.sub_request_count > 0);
        ASSERT_EQ(act_request.sub_request_count, exp_request.sub_request_count);
        for (unsigned int i = 0; i < act_request.sub_request_count; ++i) {
            redis_bulk_string &bs1 = act_request.sub_requests[i];
            redis_bulk_string &bs2 = exp_request.sub_requests[i];
            ASSERT_EQ(bs1.length, bs2.length);
            if (bs1.length > 0) {
                ASSERT_EQ(bs1.data.length(), bs2.data.length());
                ASSERT_EQ(0, memcmp(bs1.data.data(), bs2.data.data(), bs2.data.length()));
            }
        }

        _got_a_message = true;
        ++_entry_index;
    }

private:
    friend class proxy_test;
    FRIEND_TEST(proxy_test, test_simple_cases);
    FRIEND_TEST(proxy_test, test_segmented_cases);
    FRIEND_TEST(proxy_test, test_georadius);
    FRIEND_TEST(proxy_test, test_georadiusbymember);
    FRIEND_TEST(proxy_test, test_geopos);
    FRIEND_TEST(proxy_test, test_error_message);
    FRIEND_TEST(proxy_test, test_nil_bulk_string);
    FRIEND_TEST(proxy_test, test_random_cases);
    FRIEND_TEST(proxy_test, test_parse_parameters);

    std::vector<std::unique_ptr<message_entry>> _reserved_entry;
    int _entry_index;
    bool _got_a_message;
};

class proxy_test : public ::testing::Test
{
public:
    proxy_test()
    {
        dsn::message_ex *msg = dsn::message_ex::create_received_request(
            RPC_CALL_RAW_MESSAGE, dsn::DSF_THRIFT_BINARY, nullptr, 0);
        msg->header->from_address = dsn::rpc_address("127.0.0.1", 123);
        _parser.reset(new redis_test_parser(nullptr, msg));
    }

    void SetUp() override { _parser->reset(); }

    void reset() { _parser->reset(); }
    void set_msg(int index, redis_parser::redis_request msg)
    {
        _parser->set_msg(index, std::move(msg));
    }
    bool parse(dsn::message_ex *msg) { return _parser->parse(msg); }
    bool got_message() { return _parser->_got_a_message; }
    int parsed_entry_count() { return _parser->_entry_index; }

private:
    std::shared_ptr<redis_test_parser> _parser;
};

TEST_F(proxy_test, test_simple_cases)
{
    set_msg(0, redis_test_parser::redis_request(3, {{"SET"}, {"foo"}, {"bar"}}));
    const char *request_data = "*3\r\n$3\r\nSET\r\n$3\r\nfoo\r\n$3\r\nbar\r\n";
    auto request = redis_test_parser::create_message(request_data);
    ASSERT_TRUE(parse(request));
    ASSERT_TRUE(got_message());
}

TEST_F(proxy_test, test_segmented_cases)
{
    set_msg(0, redis_test_parser::redis_request(3, {{"SET"}, {"foo"}, {"bar"}}));
    const char *request_data1 = "*3\r\n$3\r\nSET\r\n$3\r";
    const char *request_data2 = "\nfoo\r\n$3\r\nbar\r\n";
    auto request1 = redis_test_parser::create_message(request_data1);
    auto request2 = redis_test_parser::create_message(request_data2);
    ASSERT_TRUE(parse(request1));
    ASSERT_TRUE(parse(request2));
    ASSERT_TRUE(got_message());
}

TEST_F(proxy_test, test_georadius)
{
    set_msg(0,
            redis_test_parser::redis_request(
                6, {{"GEORADIUS"}, {""}, {"123.4"}, {"56.78"}, {"100"}, {"m"}}));
    const char *request_data = "*6\r\n$9\r\nGEORADIUS\r\n$0\r\n\r\n$5\r\n123.4\r\n$5\r\n56."
                               "78\r\n$3\r\n100\r\n$1\r\nm\r\n";
    auto request = redis_test_parser::create_message(request_data);
    ASSERT_TRUE(parse(request));
    ASSERT_TRUE(got_message());
}

TEST_F(proxy_test, test_georadiusbymember)
{
    set_msg(0,
            redis_test_parser::redis_request(
                5, {{"GEORADIUSBYMEMBER"}, {""}, {"member1"}, {"1000.5"}, {"km"}}));
    const char *request_data = "*5\r\n$17\r\nGEORADIUSBYMEMBER\r\n$0\r\n\r\n$"
                               "7\r\nmember1\r\n$6\r\n1000.5\r\n$2\r\nkm\r\n";
    auto request = redis_test_parser::create_message(request_data);
    ASSERT_TRUE(parse(request));
    ASSERT_TRUE(got_message());
}

TEST_F(proxy_test, test_geopos)
{
    set_msg(0,
            redis_test_parser::redis_request(
                5, {{"GEOPOS"}, {""}, {"member1"}, {"member2"}, {"member3"}}));
    const char *request_data = "*5\r\n$6\r\nGEOPOS\r\n$0\r\n\r\n$"
                               "7\r\nmember1\r\n$7\r\nmember2\r\n$7\r\nmember3\r\n";
    auto request = redis_test_parser::create_message(request_data);
    ASSERT_TRUE(parse(request));
    ASSERT_TRUE(got_message());
}

TEST_F(proxy_test, test_error_message)
{
    const char *bad_data[] = {"$1\r\n$1\r\nt\r\n",
                              "*1\r$5\r\ntest_\r\n",
                              "*hello\r\n$1\r\nt\r\n",
                              "*-23\r\n$1\r\nt\r\n",
                              "*1\r\n12\r\ntest_command\r\n",
                              "*1\r\n$12test_command\r\n",
                              "*1\r\n$12\rtest_command\r\n",
                              "*2\r\n$3\r\nget\r\n*6\r\nkeykey\r\n",
                              "*2\r\n$3\r\nget\r\n$6\rkeykey\r\n",
                              nullptr};

    for (unsigned int i = 0; bad_data[i]; ++i) {
        auto request = redis_test_parser::create_message(bad_data[i]);
        ASSERT_FALSE(parse(request));
        ASSERT_FALSE(got_message());
    }

    // after wrong message, parser should be reset
    reset(); // TODO should remove?
    set_msg(0, redis_test_parser::redis_request(3, {{"set"}, {"hello"}, {""}}));
    const char *good_data = "*3\r\n$3\r\nset\r\n$5\r\nhello\r\n$0\r\n\r\n";
    auto request = redis_test_parser::create_message(good_data);
    ASSERT_TRUE(parse(request));
    ASSERT_TRUE(got_message());
}

TEST_F(proxy_test, test_nil_bulk_string)
{
    set_msg(0, redis_test_parser::redis_request(1, {{redis_test_parser::redis_bulk_string()}}));
    const char *data = "*1\r\n$-1\r\n";
    ASSERT_TRUE(parse(redis_test_parser::create_message(data)));
    ASSERT_TRUE(got_message());
}

TEST_F(proxy_test, test_random_cases)
{
    int total_requests = 10;
    std::vector<dsn::message_ex *> fake_requests;
    int total_body_size = 0;

    // create several requests
    for (int i = 0; i < total_requests; ++i) {
        int sub_request_count = dsn::rand::next_u32(1, 20);
        std::vector<redis_test_parser::redis_bulk_string> sub_requests(sub_request_count);
        for (auto &sub_request : sub_requests) {
            sub_request.length = dsn::rand::next_u32(0, 8);
            if (sub_request.length == 0) {
                sub_request.length = -1;
            } else if (sub_request.length == 1) {
                sub_request.length = 0;
            } else {
                sub_request.length = dsn::rand::next_u32(1, 256);
                std::shared_ptr<char> raw_buf(new char[sub_request.length],
                                              std::default_delete<char[]>());
                memset(raw_buf.get(), 't', sub_request.length);
                sub_request.data.assign(std::move(raw_buf), 0, sub_request.length);
            }
        }

        redis_test_parser::redis_request request(sub_request_count, std::move(sub_requests));
        set_msg(i, request);
        dsn::message_ex *fake_response = redis_test_parser::marshalling_array(request);
        dsn::message_ex *fake_request = fake_response->copy(true, true);

        fake_response->add_ref();
        fake_response->release_ref();

        fake_requests.push_back(fake_request);
        total_body_size += fake_request->body_size();
    }

    // let's copy the messages
    std::shared_ptr<char> msg_buffer(new char[total_body_size + 10], std::default_delete<char[]>());
    char *msg_buffer_ptr = msg_buffer.get();

    for (dsn::message_ex *fake_request : fake_requests) {
        void *rw_ptr;
        size_t length;
        while (fake_request->read_next(&rw_ptr, &length)) {
            memcpy(msg_buffer_ptr, rw_ptr, length);
            msg_buffer_ptr += length;
            fake_request->read_commit(length);
        }
        fake_request->add_ref();
        fake_request->release_ref();
    }
    *msg_buffer_ptr = 0;

    ASSERT_EQ(msg_buffer_ptr - msg_buffer.get(), total_body_size);

    msg_buffer_ptr = msg_buffer.get();
    // first create a big message, test the pipeline
    {
        reset();
        dsn::message_ex *msg = redis_test_parser::create_message(msg_buffer_ptr, total_body_size);
        ASSERT_TRUE(parse(msg));
        ASSERT_EQ(parsed_entry_count(), total_requests);
    }

    // let's split the messages into different pieces
    {
        reset();
        size_t slice_count = dsn::rand::next_u32(total_requests, total_body_size);
        std::set<int> offsets;
        while (offsets.size() < slice_count - 1) {
            offsets.insert(dsn::rand::next_u32(1, total_body_size - 1));
        }
        offsets.insert(total_body_size);

        int last_offset = 0;
        for (int offset : offsets) {
            dsn::message_ex *msg = redis_test_parser::create_message(msg_buffer_ptr + last_offset,
                                                                     offset - last_offset);
            ASSERT_TRUE(parse(msg));
            last_offset = offset;
        }
        ASSERT_EQ(parsed_entry_count(), total_requests);
    }
}

TEST_F(proxy_test, test_parse_parameters)
{
    double radius_m = 0;
    std::string unit;
    pegasus::geo::geo_client::SortType sort_type = pegasus::geo::geo_client::SortType::random;
    int count = 0;
    bool WITHCOORD = false;
    bool WITHDIST = false;
    bool WITHVALUE = false;

    {
        radius_m = 0;
        sort_type = pegasus::geo::geo_client::SortType::random;
        count = 0;
        WITHCOORD = false;
        WITHDIST = false;
        WITHVALUE = false;
        std::vector<redis_test_parser::redis_bulk_string> opts({{"GEORADIUS"},
                                                                {""},
                                                                {"12.3"},
                                                                {"45.6"},
                                                                {"100"},
                                                                {"m"},
                                                                {"WITHCOORD"},
                                                                {"WITHDIST"},
                                                                {"WITHHASH"},
                                                                {"COUNT"},
                                                                {"-1"},
                                                                {"ASC"},
                                                                {"WITHVALUE"}});

        redis_test_parser::parse_geo_radius_parameters(
            opts, 4, radius_m, unit, sort_type, count, WITHCOORD, WITHDIST, WITHVALUE);

        ASSERT_DOUBLE_EQ(radius_m, 100);
        ASSERT_EQ(unit, "m");
        ASSERT_EQ(sort_type, pegasus::geo::geo_client::SortType::asc);
        ASSERT_EQ(count, -1);
        ASSERT_TRUE(WITHCOORD);
        ASSERT_TRUE(WITHDIST);
        ASSERT_TRUE(WITHVALUE);
    }

    {
        radius_m = 0;
        sort_type = pegasus::geo::geo_client::SortType::random;
        count = 0;
        WITHCOORD = false;
        WITHDIST = false;
        WITHVALUE = false;
        std::vector<redis_test_parser::redis_bulk_string> opts({{"GEORADIUS"},
                                                                {""},
                                                                {"12.3"},
                                                                {"45.6"},
                                                                {"100.23"},
                                                                {"km"},
                                                                {"COUNT"},
                                                                {"500"},
                                                                {"DESC"}});

        redis_test_parser::parse_geo_radius_parameters(
            opts, 4, radius_m, unit, sort_type, count, WITHCOORD, WITHDIST, WITHVALUE);

        ASSERT_DOUBLE_EQ(radius_m, 100230);
        ASSERT_EQ(unit, "km");
        ASSERT_EQ(sort_type, pegasus::geo::geo_client::SortType::desc);
        ASSERT_EQ(count, 500);
        ASSERT_FALSE(WITHCOORD);
        ASSERT_FALSE(WITHDIST);
        ASSERT_FALSE(WITHVALUE);
    }

    {
        radius_m = 0;
        sort_type = pegasus::geo::geo_client::SortType::random;
        count = 0;
        WITHCOORD = false;
        WITHDIST = false;
        WITHVALUE = false;
        std::vector<redis_test_parser::redis_bulk_string> opts({{"GEORADIUSBYMEMBER"},
                                                                {""},
                                                                {"somekey"},
                                                                {"100"},
                                                                {"m"},
                                                                {"WITHCOORD"},
                                                                {"WITHDIST"},
                                                                {"WITHHASH"},
                                                                {"COUNT"},
                                                                {"-1"},
                                                                {"ASC"},
                                                                {"WITHVALUE"}});

        redis_test_parser::parse_geo_radius_parameters(
            opts, 3, radius_m, unit, sort_type, count, WITHCOORD, WITHDIST, WITHVALUE);

        ASSERT_DOUBLE_EQ(radius_m, 100);
        ASSERT_EQ(unit, "m");
        ASSERT_EQ(sort_type, pegasus::geo::geo_client::SortType::asc);
        ASSERT_EQ(count, -1);
        ASSERT_TRUE(WITHCOORD);
        ASSERT_TRUE(WITHDIST);
        ASSERT_TRUE(WITHVALUE);
    }

    {
        radius_m = 0;
        sort_type = pegasus::geo::geo_client::SortType::random;
        count = 0;
        WITHCOORD = false;
        WITHDIST = false;
        WITHVALUE = false;
        std::vector<redis_test_parser::redis_bulk_string> opts({{"GEORADIUSBYMEMBER"},
                                                                {""},
                                                                {"somekey"},
                                                                {"100.23"},
                                                                {"km"},
                                                                {"COUNT"},
                                                                {"500"},
                                                                {"DESC"}});

        redis_test_parser::parse_geo_radius_parameters(
            opts, 3, radius_m, unit, sort_type, count, WITHCOORD, WITHDIST, WITHVALUE);

        ASSERT_DOUBLE_EQ(radius_m, 100230);
        ASSERT_EQ(unit, "km");
        ASSERT_EQ(sort_type, pegasus::geo::geo_client::SortType::desc);
        ASSERT_EQ(count, 500);
        ASSERT_FALSE(WITHCOORD);
        ASSERT_FALSE(WITHDIST);
        ASSERT_FALSE(WITHVALUE);
    }

    {
        int ttl_seconds = 0;
        std::vector<redis_test_parser::redis_bulk_string> opts(
            {{"SET"}, {"KK"}, {"vv"}, {"EX"}, {"123"}});
        redis_test_parser::parse_set_parameters(opts, ttl_seconds);
        ASSERT_EQ(ttl_seconds, 123);
    }
}

TEST(proxy, connection)
{
    ::dsn::rpc_address redis_address("127.0.0.1", 12345);
    ip::tcp::endpoint redis_endpoint(ip::address_v4(redis_address.ip()), redis_address.port());

    io_service ios;
    ip::tcp::socket client_socket(ios);
    client_socket.open(ip::tcp::v4());
    boost::system::error_code ec;
    ASSERT_TRUE(!client_socket.connect(redis_endpoint, ec));

    char got_reply[1024];
    // basic pipeline
    {
        const char *reqs = "*3\r\n$3\r\nSET\r\n$3\r\nfoo\r\n$4\r\nbar1\r\n"
                           "*3\r\n$3\r\nSET\r\n$3\r\nfoo\r\n$4\r\nbar2\r\n"
                           "*3\r\n$3\r\nSET\r\n$3\r\nfoo\r\n$4\r\nbar3\r\n";

        boost::asio::write(client_socket, boost::asio::buffer(reqs, strlen(reqs)));

        const char *resps = "+OK\r\n"
                            "+OK\r\n"
                            "+OK\r\n";
        size_t got_length =
            boost::asio::read(client_socket, boost::asio::buffer(got_reply, strlen(resps)));
        got_reply[got_length] = 0;
        ASSERT_STREQ(resps, got_reply);
    }

    // then let's get the value
    {
        const char *req = "*2\r\n$3\r\nGET\r\n$3\r\nfoo\r\n";
        boost::asio::write(client_socket, boost::asio::buffer(req, strlen(req)));

        const char *resp = "$4\r\nbar3\r\n";
        size_t got_length =
            boost::asio::read(client_socket, boost::asio::buffer(got_reply, strlen(resp)));
        got_reply[got_length] = 0;
        ASSERT_STREQ(resp, got_reply);
    }

    // then ttl test of set key
    {
        const char *req = "*4\r\n$5\r\nSETEX\r\n$3\r\nfo1\r\n$1\r\n2\r\n$3\r\nbar\r\n"
                          "*4\r\n$5\r\nSETEX\r\n$3\r\nfo2\r\n$4\r\n9999\r\n$3\r\nbar\r\n"
                          "*3\r\n$3\r\nSET\r\n$3\r\nfo3\r\n$3\r\nbar\r\n";
        boost::asio::write(client_socket, boost::asio::buffer(req, strlen(req)));

        const char *resps = "+OK\r\n"
                            "+OK\r\n"
                            "+OK\r\n";
        size_t got_length =
            boost::asio::read(client_socket, boost::asio::buffer(got_reply, strlen(resps)));
        got_reply[got_length] = 0;
        ASSERT_STREQ(resps, got_reply);
    }

    {
        const char *req = "*2\r\n$3\r\nTTL\r\n$3\r\nfo2\r\n"
                          "*2\r\n$3\r\nTTL\r\n$3\r\nfo3\r\n"
                          "*2\r\n$3\r\nTTL\r\n$3\r\nfo4\r\n";
        boost::asio::write(client_socket, boost::asio::buffer(req, strlen(req)));

        const char *resps1 = ":9999\r\n"
                             ":-1\r\n"
                             ":-2\r\n";
        const char *resps2 = ":9998\r\n"
                             ":-1\r\n"
                             ":-2\r\n";
        size_t got_length =
            boost::asio::read(client_socket, boost::asio::buffer(got_reply, strlen(resps1)));
        got_reply[got_length] = 0;
        ASSERT_EQ(got_length, strlen(resps1));
        ASSERT_TRUE(strncmp(got_reply, resps1, got_length) == 0 ||
                    strncmp(got_reply, resps2, got_length) == 0);
    }

    {
        std::this_thread::sleep_for(std::chrono::milliseconds(2100));
        const char *req = "*2\r\n$3\r\nGET\r\n$3\r\nfo1\r\n"
                          "*2\r\n$3\r\nGET\r\n$3\r\nfo2\r\n"
                          "*2\r\n$3\r\nGET\r\n$3\r\nfo3\r\n";
        boost::asio::write(client_socket, boost::asio::buffer(req, strlen(req)));

        const char *resps = "$-1\r\n"
                            "$3\r\nbar\r\n"
                            "$3\r\nbar\r\n";
        size_t got_length =
            boost::asio::read(client_socket, boost::asio::buffer(got_reply, strlen(resps)));
        got_reply[got_length] = 0;
        ASSERT_STREQ(resps, got_reply);
    }

    // let's send partitial message then close the socket
    {
        const char *req = "*3\r\n$3\r\nSET\r\n$3\r\nfoo\r\n$4\r\nbar1\r\n"
                          "*3\r\n$3\r\nSET\r\n$3\r\nfo";
        boost::asio::write(client_socket, boost::asio::buffer(req, strlen(req)));

        try {
            client_socket.shutdown(boost::asio::socket_base::shutdown_both);
            client_socket.close();
        } catch (...) {
            ddebug("exception in shutdown");
        }
    }
}

void dsn_init()
{
    dsn::service_app::register_factory<proxy_test_app>("proxy");
    dsn_run_config("config.ini", false);
}

GTEST_API_ int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    dsn_init();
    int ret = RUN_ALL_TESTS();
    dsn_exit(ret);
}


text:
#define BOOST_TEST_DYN_LINK
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/unit_test.hpp>

#include <luacxx/binding/function.hpp>
#include <luacxx/binding/class.hpp>
#include <luacxx/binding/engine.hpp>

#include <config.h>
#include "incomplete_data.hpp"

namespace
{
  luacxx::state_type global_state;

  bool fct1(int a, luacxx::state_type state, int b)
  {
    return (a == 5) && (b == 10) && state == global_state;
  }

  bool fct2(luacxx::state_type state)
  {
    return state == global_state;
  }
}

namespace luacxx
{
  template <> struct is_incomplete<opaque_data> : ::std::true_type
  {
  };
}

BOOST_AUTO_TEST_CASE( incomplete_01 )
{
  luacxx::engine engine;
  luacxx::make_function(engine, "fct1", fct1);
  luacxx::make_function(engine, "fct2", fct2);
  BOOST_CHECK(engine.bind());
  global_state = engine.get_state();

  std::pair<bool, std::string> ret = engine.do_file(LUACXX_TESTS_DIR "incomplete_01.lua");
  if(!ret.first)
  {
    BOOST_FAIL(ret.second);
  }
}

BOOST_AUTO_TEST_CASE( incomplete_02 )
{
  luacxx::engine engine;
  luacxx::make_class<opaque_data>(engine, "Data");
  luacxx::make_function(engine, "create_opaque_data", create_opaque_data);
  luacxx::make_function(engine, "delate_paque_data",  delate_paque_data);
  luacxx::make_function(engine, "inc_opaque_data",    inc_opaque_data);
  luacxx::make_function(engine, "dec_opaque_data",    dec_opaque_data);
  luacxx::make_function(engine, "get_opaque_data",    get_opaque_data);
  BOOST_CHECK(engine.bind());

  std::pair<bool, std::string> ret = engine.do_file(LUACXX_TESTS_DIR "incomplete_02.lua");
  if(!ret.first)
  {
    BOOST_FAIL(ret.second);
  }
}

max_stars_repo_name: EVaillant/luacxx

max_issues_repo_name: EVaillant/luacxx

max_forks_repo_name: EVaillant/luacxx


text:
#ifndef OSRM_EXTRACTOR_RESTRICTION_COMPRESSOR_HPP_
#define OSRM_EXTRACTOR_RESTRICTION_COMPRESSOR_HPP_

#include "extractor/restriction.hpp"
#include "util/typedefs.hpp"

#include <boost/unordered_map.hpp>
#include <vector>

namespace osrm
{
namespace extractor
{

struct NodeRestriction;
struct TurnRestriction;

// OSRM stores restrictions in the form node -> node -> node instead of way -> node -> way (or
// way->way->way) as it is done in OSM. These restrictions need to match the state of graph
// compression which we perform in the graph compressor that removes certain degree two nodes from
// the graph (all but the ones with penalties/barriers, as of the state of writing).
// Since this graph compression ins performed after creating the restrictions in the extraction
// phase, we need to update the involved nodes whenever one of the nodes is compressed.
//
//
// !!!! Will bind to the restrictions vector and modify it in-place !!!!
class RestrictionCompressor
{
  public:
    RestrictionCompressor(std::vector<TurnRestriction> &restrictions,
                          std::vector<ConditionalTurnRestriction> &conditional_turn_restrictions);

    // account for the compression of `from-via-to` into `from-to`
    void Compress(const NodeID from, const NodeID via, const NodeID to);

  private:
    // a turn restriction is given as `from star via node to end`. Edges ending at `head` being
    // contracted move the head pointer to their respective head. Edges starting at tail move the
    // tail values to their respective tails. Way turn restrictions are represented by two
    // node-restrictions, so we can focus on them alone
    boost::unordered_multimap<NodeID, NodeRestriction *> starts;
    boost::unordered_multimap<NodeID, NodeRestriction *> ends;
};

} // namespace extractor
} // namespace osrm

#endif // OSRM_EXTRACTOR_RESTRICTION_COMPRESSOR_HPP_

max_stars_repo_name: neilbu/osrm-backend

max_issues_repo_name: neilbu/osrm-backend

max_forks_repo_name: neilbu/osrm-backend


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

#include <boost/filesystem/convenience.hpp>  // TODO(jeff): use std::filesystem in C++17
#include <boost/optional.hpp>
#include <boost/program_options.hpp>

#include <eigen_conversions/eigen_msg.h>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <rosgraph_msgs/Clock.h>
#include <tf2_msgs/TFMessage.h>
#ifdef USE_FLOAT64
#include <std_msgs/Float64.h>
#else
#include <std_msgs/Float32.h>
#endif
#include <std_msgs/Header.h>
#include <std_msgs/String.h>

#include "a2d2_to_ros/json_utils.hpp"
#include "a2d2_to_ros/lib_a2d2_to_ros.hpp"
#include "a2d2_to_ros/log_build_options.hpp"
#include "a2d2_to_ros/logging.hpp"
#include "ros_cnpy/cnpy.h"

///
/// Program constants and defaults.
///

static constexpr auto EPS = 1e-8;
static constexpr auto _TF_FREQUENCEY = 10.0;
static constexpr auto _PROGRAM_OPTIONS_LINE_LENGTH = 120u;
static constexpr auto _INCLUDE_ORIGINAL = false;
static constexpr auto _INCLUDE_CONVERTED = true;
static constexpr auto _INCLUDE_CLOCK_TOPIC = false;
static constexpr auto _CLOCK_TOPIC = "/clock";
static constexpr auto _BUS_FRAME_NAME = "wheels";
static constexpr auto _OUTPUT_PATH = ".";
static constexpr auto _DATASET_NAMESPACE = "/a2d2";
static constexpr auto _ORIGINAL_VALUE_TOPIC = "original_value";
static constexpr auto _ORIGINAL_UNITS_TOPIC = "original_units";
static constexpr auto _VALUE_TOPIC = "value";
static constexpr auto _HEADER_TOPC = "header";
static constexpr auto _START_TIME = static_cast<uint64_t>(0);
static constexpr auto _MIN_TIME_OFFSET = 0.0;
static constexpr auto _VERBOSE = false;
static constexpr auto _DURATION = std::numeric_limits<double>::max();
static constexpr auto _SENSOR_CONFIG_FILENAME = "cams_lidars.json";

///
/// Executable specific stuff
///

#define VERIFY_BASIS_ORIGIN(basis, origin, sensors, frame)                  \
  {                                                                         \
    if (!a2d2::vector_is_valid(origin)) {                                   \
      X_FATAL("Origin for " << sensors << "::" << frame                     \
                            << " is not valid. Origin must be finite and "  \
                               "real valued. Cannot continue.");            \
      return EXIT_FAILURE;                                                  \
    }                                                                       \
    if (basis.isZero(0.0)) {                                                \
      X_FATAL(                                                              \
          "Basis for "                                                      \
          << sensors << "::" << frame                                       \
          << " cannot be constructed. Check that the X/Y axes are valid."); \
      return EXIT_FAILURE;                                                  \
    }                                                                       \
  }

namespace {
namespace a2d2 = a2d2_to_ros;
namespace po = boost::program_options;
}  // namespace

typedef std::set<a2d2::DataPair, a2d2::DataPairTimeComparator> DataPairSet;
typedef std::unordered_map<std::string, std::tuple<std::string, DataPairSet>>
    DataPairMap;

int main(int argc, char* argv[]) {
  X_INFO("<Bus Signal Converter>");
  BUILD_INFO;  // just write to log what build options were specified

  ///
  /// Set up command line arguments
  ///

  boost::optional<std::string> schema_path_opt;
  boost::optional<std::string> json_path_opt;
  boost::optional<std::string> sensor_config_path_opt;
  boost::optional<std::string> sensor_config_schema_path_opt;

  po::options_description desc(
      "Convert sequential bus signal data to rosbag for the A2D2 Sensor Fusion "
      "data set. In addition, write a transform bag file containing the "
      "vehicle box model and tf tree for the vehicle sensor configuration. See "
      "README.md for details.\nAvailable options are listed  below. Arguments "
      "without default values are required",
      _PROGRAM_OPTIONS_LINE_LENGTH);
  desc.add_options()("help,h", "Print help and exit.")(
      "sensor-config-json-path,c",
      po::value(&sensor_config_path_opt)->required(),
      "Path to the directory containing the JSON for vehicle/sensor config.")(
      "sensor-config-schema-path,s",
      po::value(&sensor_config_schema_path_opt)->required(),
      "Path to the JSON schema for the vehicle/sensor config.")(
      "bus-signal-json-path,j", po::value(&json_path_opt)->required(),
      "Path to the directory containing the JSON bus signal file.")(
      "bus-signal-schema-path,b", po::value(&schema_path_opt)->required(),
      "Path to the JSON schema for bus signal data.")(
      "start-time,a", po::value<uint64_t>()->default_value(_START_TIME),
      "Optional: Start on or after this time.")(
      "min-time-offset,m", po::value<double>()->default_value(_MIN_TIME_OFFSET),
      "Optional: Seconds to skip ahead in the data before starting the bag.")(
      "duration,d", po::value<double>()->default_value(_DURATION),
      "Optional: Seconds after min-time-offset to include in bag file.")(
      "output-path,o", po::value<std::string>()->default_value(_OUTPUT_PATH),
      "Optional: Path for the output bag file.")(
      "include-clock-topic,t",
      po::value<bool>()->default_value(_INCLUDE_CLOCK_TOPIC),
      "Optional: Write bus signal times to a /clock topic in the TF bag.")(
      "include-original-values,i",
      po::value<bool>()->default_value(_INCLUDE_ORIGINAL),
      "Optional: Include data set values in their original units.")(
      "include-converted-values,r",
      po::value<bool>()->default_value(_INCLUDE_CONVERTED),
      "Optional: Include data set values converted to ROS standard units.")(
      "verbose,v", po::value<bool>()->default_value(_VERBOSE),
      "Optional: Show name of each file after it is processed.");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);

  const auto help_requested = vm.count("help");
  if (help_requested) {
    std::cout << desc << "\n";
    return EXIT_SUCCESS;
  }

  try {
    po::notify(vm);
  } catch (boost::program_options::required_option& e) {
    std::cerr << "Ensure that all required options are specified: " << e.what()
              << "\n\n";
    std::cerr << desc << "\n";
    return EXIT_FAILURE;
  }

  ///
  /// Get commandline parameters
  ///

  const auto sensor_config_path =
      *sensor_config_path_opt + "/" + _SENSOR_CONFIG_FILENAME;
  const auto sensor_config_schema_path = *sensor_config_schema_path_opt;
  const auto schema_path = *schema_path_opt;
  const auto json_data_path = *json_path_opt;
  const auto output_path = vm["output-path"].as<std::string>();
  const auto include_original = vm["include-original-values"].as<bool>();
  const auto include_converted = vm["include-converted-values"].as<bool>();
  const auto include_clock_topic = vm["include-clock-topic"].as<bool>();
  const auto start_time = vm["start-time"].as<uint64_t>();
  const auto min_time_offset = vm["min-time-offset"].as<double>();
  const auto duration = vm["duration"].as<double>();
  const auto verbose = vm["verbose"].as<bool>();

  const auto valid_min_offset = (std::isfinite(min_time_offset) &&

                                 a2d2::strictly_non_negative(min_time_offset));
  const auto valid_duration =
      (std::isfinite(duration) && a2d2::strictly_non_negative(duration));
  if (!valid_min_offset || !valid_duration) {
    X_FATAL(
        "Time constraints {min-time-offset: "
        << min_time_offset << ", duration: " << duration
        << "} are not valid. They must be finite, real valued, and >= 0.0.");
    return EXIT_FAILURE;
  }

  ///
  /// Get the path for the bus signal JSON data
  /// There should be only one file in the directory, and it should be the data
  ///
  boost::filesystem::directory_iterator it{json_data_path};
  std::string json_path;
  auto iteration = 0;
  constexpr auto MAX_ITERATIONS = 100;
  while (it != boost::filesystem::directory_iterator{}) {
    const auto path = it->path().string();
    const auto extension = it->path().extension().string();
    if (extension == ".json") {
      json_path = path;
    }
    ++it;
    ++iteration;
    if (iteration >= MAX_ITERATIONS) {
      break;
    }
  }

  if (json_path.empty()) {
    X_FATAL(
        "Could not find bus signal data. Either no json file exists in "
        "location, or the directory contains too many files (examined: "
        << iteration << ", max allowed: " << MAX_ITERATIONS << ").");
    return EXIT_FAILURE;
  }
  X_INFO("Found json file: " << json_path
                             << ", assuming this is bus signal data.");

  const auto file_basename = boost::filesystem::basename(json_path);
  const auto topic_prefix =
      (std::string(_DATASET_NAMESPACE) + "/" + file_basename);

  ///
  /// Get the JSON schema for the data set
  ///

  auto d_schema_opt = a2d2::get_rapidjson_dom(schema_path);
  if (!d_schema_opt) {
    X_FATAL("Could not open '" << schema_path);
    return EXIT_FAILURE;
  }
  auto& d_schema = *d_schema_opt;
  rapidjson::SchemaDocument schema(d_schema);

  ///
  /// Get the JSON data set
  ///

  auto d_json_opt = a2d2::get_rapidjson_dom(json_path);
  if (!d_json_opt) {
    X_FATAL("Could not open '" << json_path);
    return EXIT_FAILURE;
  }
  auto& d_json = *d_json_opt;

  X_INFO("Loaded and parsed schema and data set successfully.");

  ///
  /// Validate the data set against the schema
  ///

  {
    rapidjson::SchemaValidator validator(schema);
    if (!d_json.Accept(validator)) {
      const auto err_string = a2d2::get_validator_error_string(validator);
      X_FATAL(err_string);
      return EXIT_FAILURE;
    }
    X_INFO("Validated: " << json_path);
  }

  ///
  /// Get the JSON for vehicle/sensor config
  ///

  auto d_sensor_config_opt = a2d2::get_rapidjson_dom(sensor_config_path);
  if (!d_sensor_config_opt) {
    X_FATAL("Could not open '" << sensor_config_path);
    return EXIT_FAILURE;
  }
  auto& sensor_config_d = *d_sensor_config_opt;

  ///
  /// Get the JSON schema for the config JSON
  ///

  auto d_sensor_config_schema_opt =
      a2d2::get_rapidjson_dom(sensor_config_schema_path);
  if (!d_sensor_config_schema_opt) {
    X_FATAL("Could not open '" << sensor_config_schema_path);
    return EXIT_FAILURE;
  }
  rapidjson::SchemaDocument config_schema(*d_sensor_config_schema_opt);

  ///
  /// Validate the sensor config against the schema
  ///

  {
    rapidjson::SchemaValidator validator(config_schema);
    if (!sensor_config_d.Accept(validator)) {
      const auto err_string = a2d2::get_validator_error_string(validator);
      X_FATAL(err_string);
      return EXIT_FAILURE;
    }
    X_INFO("Validated: " << sensor_config_path);
  }

  ///
  /// Build ego vehicle shape message
  ///

  const rapidjson::Value& ego_dims =
      sensor_config_d["vehicle"]["ego-dimensions"];
  const rapidjson::Value& x_dims = ego_dims["x-range"];
  const rapidjson::Value& y_dims = ego_dims["y-range"];
  const rapidjson::Value& z_dims = ego_dims["z-range"];

  constexpr auto MIN_IDX = static_cast<rapidjson::SizeType>(0);
  constexpr auto MAX_IDX = static_cast<rapidjson::SizeType>(1);
  const auto x_min = x_dims[MIN_IDX].GetDouble();
  const auto x_max = x_dims[MAX_IDX].GetDouble();
  const auto y_min = y_dims[MIN_IDX].GetDouble();
  const auto y_max = y_dims[MAX_IDX].GetDouble();
  const auto z_min = z_dims[MIN_IDX].GetDouble();
  const auto z_max = z_dims[MAX_IDX].GetDouble();

  const auto ego_bbox_valid =
      a2d2::verify_ego_bbox_params(x_min, x_max, y_min, y_max, z_min, z_max);
  if (!ego_bbox_valid) {
    X_FATAL(
        "Ego bounding box parameters are invalid. They must be finite, "
        "real-valued, and ordered: x: ["
        << x_min << ", " << x_max << "], y: [" << y_min << ", " << y_max
        << "], z: [" << z_min << ", " << z_max << "]");
    return EXIT_FAILURE;
  }

  const auto ego_shape_msg =
      a2d2::build_ego_shape_msg(x_min, x_max, y_min, y_max, z_min, z_max);

  ///
  /// Get sensor poses
  ///

  const auto sensors = a2d2::sensors::Frames::get_sensors();

  // each block will add its transform message to this container
  tf2_msgs::TFMessage msgtf;

  // For each sensor type...
  for (const auto& name :
       {a2d2::sensors::Names::CAMERAS, a2d2::sensors::Names::LIDARS}) {
    const auto is_camera = (name == a2d2::sensors::Names::CAMERAS);
    const auto is_lidar = (name == a2d2::sensors::Names::LIDARS);

    // For each sensor position...
    for (auto i = 0; i < sensors.size(); ++i) {
      const auto& frame = sensors[i];

      // No lidars at these positions
      const auto is_side_left = (a2d2::sensors::Frames::SIDE_LEFT_IDX == i);
      const auto is_side_right = (a2d2::sensors::Frames::SIDE_RIGHT_IDX == i);
      const auto is_rear_center = (a2d2::sensors::Frames::REAR_CENTER_IDX == i);
      if (is_lidar) {
        if (is_side_left || is_side_right || is_rear_center) {
          continue;
        }
      }

      // No cameras at these positions
      const auto is_rear_left = (a2d2::sensors::Frames::REAR_LEFT_IDX == i);
      const auto is_rear_right = (a2d2::sensors::Frames::REAR_RIGHT_IDX == i);
      if (is_camera) {
        if (is_rear_left || is_rear_right) {
          continue;
        }
      }

      // compute transform between sensor and vehicle
      const Eigen::Matrix3d basis =
          a2d2::json_axes_to_eigen_basis(sensor_config_d, name, frame, EPS);
      const Eigen::Vector3d origin =
          a2d2::json_origin_to_eigen_vector(sensor_config_d, name, frame);
      VERIFY_BASIS_ORIGIN(basis, origin, name, frame);

      const Eigen::Affine3d Tx = a2d2::Tx_global_sensor(basis, origin);

      {
        geometry_msgs::Transform Tx_msg;
        tf::transformEigenToMsg(Tx, Tx_msg);

        geometry_msgs::TransformStamped Tx_stamped_msg;
        Tx_stamped_msg.transform = Tx_msg;
        Tx_stamped_msg.header.frame_id = "chassis";
        Tx_stamped_msg.child_frame_id = a2d2::tf_frame_name(name, frame);
        msgtf.transforms.push_back(Tx_stamped_msg);
      }

      // Lidar data lives in camera frames, but it is motion corrected, so it
      // lives in 'wheels' not 'chassis'
      if (is_camera) {
        geometry_msgs::Transform Tx_msg;
        tf::transformEigenToMsg(Tx, Tx_msg);

        geometry_msgs::TransformStamped Tx_stamped_msg;
        Tx_stamped_msg.transform = Tx_msg;
        Tx_stamped_msg.header.frame_id = "wheels";
        Tx_stamped_msg.child_frame_id =
            a2d2::tf_motion_compensated_sensor_frame_name(name, frame);
        msgtf.transforms.push_back(Tx_stamped_msg);
      }

      {
        geometry_msgs::Transform Tx_msg;
        tf::transformEigenToMsg(Eigen::Affine3d::Identity(), Tx_msg);

        geometry_msgs::TransformStamped Tx_stamped_msg;
        Tx_stamped_msg.transform = Tx_msg;
        Tx_stamped_msg.header.frame_id = "wheels";
        Tx_stamped_msg.child_frame_id = "chassis";
        msgtf.transforms.push_back(Tx_stamped_msg);
      }
    }
  }

  ///
  /// Get the field names from schema, retrieve data from DOM, write to bag.
  /// This loop does not do error checking on the DOM because it should already
  /// have been validated by the schema.
  ///

  std::map<uint64_t, float> roll_angles;
  std::map<uint64_t, float> pitch_angles;

  std::set<ros::Time> stamps;
  rosbag::Bag bus_signal_bag;
  {
    const auto bag_name = output_path + "/" + file_basename + ".bag";
    bus_signal_bag.open(bag_name, rosbag::bagmode::Write);
  }
  const rapidjson::Value& r = d_schema["required"];
  for (rapidjson::SizeType idx = 0; idx < r.Size(); ++idx) {
    const auto name = std::string(r[idx].GetString());
    if (verbose) {
      X_INFO("Converting " << name << "...");
    }

    const rapidjson::Value& obj = d_json[name.c_str()].GetObject();
    const rapidjson::Value& values = obj["values"];
    const auto units = obj["unit"].IsNull() ? "null" : obj["unit"].GetString();

    // publish data for each of the values in this field
    auto no_units_yet = true;
    boost::optional<ros::Time> first_time;
    for (rapidjson::SizeType idx = 0; idx < values.Size(); ++idx) {
      constexpr auto TIMESTAMP_IDX = static_cast<rapidjson::SizeType>(0);
      constexpr auto VALUE_IDX = static_cast<rapidjson::SizeType>(1);
      const rapidjson::Value& t_v = values[idx];
      const auto time = t_v[TIMESTAMP_IDX].GetUint64();
      if (!a2d2::valid_ros_timestamp(time)) {
        X_FATAL("Timestamp "
                << time
                << " has unsupported magnitude: ROS does not support "
                   "timestamps on or after 4294967296000000 "
                   "(Sunday, February 7, 2106 6:28:16 AM GMT)\nCall "
                   "Zager and Evans for details.");
        return EXIT_FAILURE;
      }

      if (time < start_time) {
        continue;
      }

      const auto value = t_v[VALUE_IDX].GetDouble();
      const auto data = a2d2::DataPair::build(value, time, _BUS_FRAME_NAME);

      const auto& stamp = data.header.stamp;
      if (!first_time) {
        if (start_time != _START_TIME) {
          first_time = a2d2::a2d2_timestamp_to_ros_time(start_time);
        } else {
          first_time = stamp;
        }
      }

      const auto time_since_begin = (stamp - *first_time).toSec();
      if (time_since_begin < min_time_offset) {
        continue;
      }

      const auto recorded_duration = (time_since_begin - min_time_offset);
      if (recorded_duration > duration) {
        break;
      }

      if (name == "roll_angle") {
        if (roll_angles.find(time) != std::end(roll_angles)) {
          X_FATAL("Non unique values for roll angle time: "
                  << time << ". Cannot continue.");
          return EXIT_FAILURE;
        }
        roll_angles[time] = a2d2::to_ros_units(units, value);
      }

      if (name == "pitch_angle") {
        if (pitch_angles.find(time) != std::end(pitch_angles)) {
          X_FATAL("Non unique values for pitch angle time: "
                  << time << ". Cannot continue.");
          return EXIT_FAILURE;
        }
        pitch_angles[time] = a2d2::to_ros_units(units, value);
      }

      // TODO(jeff): typo _HEADER_TOPC
      bus_signal_bag.write(topic_prefix + "/" + name + "/" + _HEADER_TOPC,
                           stamp, data.header);
      if (include_original) {
        bus_signal_bag.write(
            topic_prefix + "/" + name + "/" + _ORIGINAL_VALUE_TOPIC, stamp,
            data.value);

        if (no_units_yet) {
          std_msgs::String units_msg;
          units_msg.data = units;

          bus_signal_bag.write(
              topic_prefix + "/" + name + "/" + _ORIGINAL_UNITS_TOPIC, stamp,
              units_msg);
          no_units_yet = false;
        }
      }

      if (include_converted) {
        const auto is_lat_lon =
            ((name == "longitude_degree") || (name == "latitude_degree"));
        const auto ros_value =
            (is_lat_lon ? data.value.data
                        : a2d2::to_ros_units(units, data.value.data));
        a2d2::DataPair::value_type ros_value_msg;
        ros_value_msg.data = ros_value;
        bus_signal_bag.write(topic_prefix + "/" + name + "/" + _VALUE_TOPIC,
                             stamp, ros_value_msg);
      }

      if (include_clock_topic) {
        stamps.insert(stamp);
      }
    }
  }

  ///
  /// Finish the bus signal bag
  ///

  bus_signal_bag.close();

  ///
  /// Write TF bag
  ///

  // TODO(jeff)
  if (roll_angles.size() != pitch_angles.size()) {
    X_FATAL("roll/pitch angle size mismatch: "
            << roll_angles.size() << " roll values, but " << pitch_angles.size()
            << " pitch values. Cannot continue.");
    return EXIT_FAILURE;
  }

  ///
  /// Write all tf messages to the bag
  ///

  X_INFO("Writing TF bag file...");

  rosbag::Bag tf_bag;
  {
    const auto bag_name = output_path + "/" + file_basename + "_tf.bag";
    tf_bag.open(bag_name, rosbag::bagmode::Write);
  }
  for (const auto& p : roll_angles) {
    const auto it_pitch = pitch_angles.find(p.first);
    if (it_pitch == std::end(pitch_angles)) {
      X_FATAL("Could not find timestamp "
              << p.first << " from roll data in pitch data. Cannot continue.");
      tf_bag.close();
      return EXIT_FAILURE;
    }
    const auto ros_time = a2d2::a2d2_timestamp_to_ros_time(p.first);
    const auto roll = p.second;
    const auto pitch = it_pitch->second;

    {
      tf2_msgs::TFMessage chassistf;

      Eigen::Matrix3d R;
      R = (Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) *
           Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitZ()) *
           Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()));

      const Eigen::Affine3d Tx = (Eigen::Translation3d::Identity() * R);
      {
        geometry_msgs::Transform Tx_msg;
        tf::transformEigenToMsg(Tx, Tx_msg);

        geometry_msgs::TransformStamped Tx_stamped_msg;
        Tx_stamped_msg.transform = Tx_msg;
        Tx_stamped_msg.header.stamp = ros_time;
        Tx_stamped_msg.header.frame_id = "wheels";
        Tx_stamped_msg.child_frame_id = "chassis";
        chassistf.transforms.push_back(Tx_stamped_msg);
      }

      tf_bag.write("/tf", ros_time, chassistf);
    }

    for (auto& msg : msgtf.transforms) {
      msg.header.stamp = ros_time;
    }
    tf_bag.write("/tf_static", ros_time, msgtf);
    tf_bag.write("/a2d2/ego_shape", ros_time, ego_shape_msg);
  }

  ///
  /// Write a clock message for every unique timestamp in the data set
  ///

  // TODO(jeff): add this to tf bag
  if (include_clock_topic) {
    X_INFO("Adding " << _CLOCK_TOPIC << " topic to TF bag file...");
  }
  for (const auto& stamp : stamps) {
    rosgraph_msgs::Clock clock_msg;
    clock_msg.clock = stamp;
    tf_bag.write(_CLOCK_TOPIC, stamp, clock_msg);
  }

  tf_bag.close();
  return EXIT_SUCCESS;
}


text:
/**
 Boost Logging library

 Author: John Torjo, www.torjo.com

 Copyright (C) 2007 John Torjo (see www.torjo.com for email)

 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)

 See http://www.boost.org for updates, documentation, and revision history.
 See http://www.torjo.com/log2/ for more details
*/

/**
@example using_tags.cpp

@copydoc using_tags

@page using_tags using_tags.cpp Example

This usage:
- You have one logger and one filter, which can be turned on or off
- You want to format the message before it's written
- The logger has several log destinations
    - The output goes to console, and a file called out.txt
    - Formatting - message will look like this: <tt>[file/line] [thread_id] [idx] [time] message [enter] </tt>

Optimizations:
- use a cache string (from optimize namespace), in order to make formatting the message faster

In this example, all output will be written to the console, debug window, and "out.txt" file.
The output can look like:

@code
logging\samples\scenarios\using_tags.cpp:94 [T7204] [1] 14:55 this is so cool 1
logging\samples\scenarios\using_tags.cpp:95 [T7204] [2] 14:55 this is so cool again 2
@endcode

*/

#include <boost/logging/format_fwd.hpp>

namespace bl = boost::logging;
typedef bl::tag::holder< bl::optimize::cache_string_one_str<>, bl::tag::file_line, bl::tag::thread_id, bl::tag::time> log_string;
BOOST_LOG_FORMAT_MSG( log_string )

#include <boost/logging/format_ts.hpp>
#include <boost/logging/format/formatter/tags.hpp>
#include <boost/logging/format/formatter/named_spacer.hpp>

using namespace boost::logging;

using namespace boost::logging::scenario::usage;
typedef use<
        //  the filter is always accurate (but slow)
        filter_::change::always_accurate,
        //  filter does not use levels
        filter_::level::no_levels,
        // the logger is initialized once, when only one thread is running
        logger_::change::set_once_when_one_thread,
        // the logger favors speed (on a dedicated thread)
        logger_::favor::speed> finder;

BOOST_DECLARE_LOG_FILTER(g_log_filter, finder::filter )
BOOST_DECLARE_LOG(g_l, finder::logger)

#define L_ BOOST_LOG_USE_LOG_IF_FILTER(g_l(), g_log_filter()->is_enabled() ) .set_tag( BOOST_LOG_TAG_FILELINE)

BOOST_DEFINE_LOG_FILTER(g_log_filter, finder::filter )
BOOST_DEFINE_LOG(g_l, finder::logger)

void using_tags_example() {
    //         add formatters and destinations
    //         That is, how the message is to be formatted and where should it be written to

    g_l()->writer().add_formatter( formatter::named_spacer( "%fileline% [T%thread_id%] [%idx%] %time%" )
        .add( "time", formatter::tag::time("$mm:$ss ") )                // time tag
        .add( "idx", formatter::idx() )
        .add( "thread_id", formatter::tag::thread_id() )                // thread_id tag
        .add( "fileline", formatter::tag::file_line() ) );              // file/line tag

    g_l()->writer().add_formatter( formatter::append_newline() );
    g_l()->writer().add_destination( destination::cout() );
    g_l()->writer().add_destination( destination::file("out.txt") );
    g_l()->mark_as_initialized();

    int i = 1;
    L_ << "this is so cool " << i++;
    L_ << "this is so cool again " << i++;
}

int main() {
    using_tags_example();
}

// End of file

max_stars_repo_name: gbucknell/fsc-sdk

max_issues_repo_name: gbucknell/fsc-sdk

max_forks_repo_name: gbucknell/fsc-sdk


text:
#define BOOST_TEST_MODULE PriceOptimiserTest

#include "PriceOptimiser.hpp"
#include <vector>

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/algorithm/string.hpp>

BOOST_AUTO_TEST_SUITE(PriceOptimiserSuite)

using namespace com::github::nimelo;
using namespace std;

BOOST_AUTO_TEST_CASE(TestScenario_01)
{
  // Arrange
  vector<size_t> drive_prices = { 3, 1 };
  vector<size_t> keyboard_prices = { 5, 2, 8 };
  size_t money = 10;
  signed long expected = 9;

  PriceOptimiser po(keyboard_prices, drive_prices);

  // Act
  auto actual = po.optimise(money);

  // Assert
  BOOST_CHECK_EQUAL(expected, actual);
}

BOOST_AUTO_TEST_CASE(TestScenario_02)
{
  // Arrange
  vector<size_t> drive_prices = { 4 };
  vector<size_t> keyboard_prices = { 5 };
  size_t money = 5;
  signed long expected = -1;

  PriceOptimiser po(keyboard_prices, drive_prices);

  // Act
  auto actual = po.optimise(money);

  // Assert
  BOOST_CHECK_EQUAL(expected, actual);
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: Nimelo/hacker-rank

max_issues_repo_name: Nimelo/hacker-rank

max_forks_repo_name: Nimelo/hacker-rank