<commit_before>
#ifndef __CLIENT_HPP__
#define __CLIENT_HPP__

#include <pthread.h>
#include "protocol.hpp"
#include "sqlite_protocol.hpp"
#include <stdint.h>
#include "assert.h"

#define update_salt 6830

using namespace std;

/* Information shared between clients */
struct shared_t {
public:
    shared_t(config_t *_config)
        : config(_config),
          qps_offset(0), latencies_offset(0),
          qps_fd(NULL), latencies_fd(NULL),
          last_qps(0), n_op(1), n_tick(1), n_ops_so_far(0)
        {
            pthread_mutex_init(&mutex, NULL);

            if(config->qps_file[0] != 0) {
                qps_fd = fopen(config->qps_file, "wa");
            }
            if(config->latency_file[0] != 0) {
                latencies_fd = fopen(config->latency_file, "wa");
            }

            value_buf = new char[config->values.max];
            memset((void *) value_buf, 'A', config->values.max);
        }

    ~shared_t() {
        if(qps_fd) {
            fwrite(qps, 1, qps_offset, qps_fd);
            fclose(qps_fd);
        }
        if(latencies_fd) {
            fwrite(latencies, 1, latencies_offset, latencies_fd);
            fclose(latencies_fd);
        }

        pthread_mutex_destroy(&mutex);

        delete value_buf;
    }

    void push_qps(int _qps, int tick) {
        if(!qps_fd && !latencies_fd)
            return;

        lock();

        // Collect qps info from all clients before attempting to print
        if(config->clients > 1) {
            int qps_count = 0, agg_qps = 0;
            map<int, pair<int, int> >::iterator op = qps_map.find(tick);
            if(op != qps_map.end()) {
                qps_count = op->second.first;
                agg_qps = op->second.second;
            }

            qps_count += 1;
            agg_qps += _qps;

            if(qps_count == config->clients) {
                _qps = agg_qps;
                qps_map.erase(op);
            } else {
                qps_map[tick] = pair<int, int>(qps_count, agg_qps);
                unlock();
                return;
            }
        }

        last_qps = _qps;

        if(!qps_fd) {
            unlock();
            return;
        }

        int _off = snprintf(qps + qps_offset, sizeof(qps) - qps_offset, "%d\t\t%d\n", n_tick, _qps);
        if(_off >= sizeof(qps) - qps_offset) {
            // Couldn't write everything, flush
            fwrite(qps, 1, qps_offset, qps_fd);

            // Write again
            qps_offset = 0;
            _off = snprintf(qps + qps_offset, sizeof(qps) - qps_offset, "%d\t\t%d\n", n_tick, _qps);
        }
        qps_offset += _off;

        n_tick++;

        unlock();
    }

    void push_latency(float latency) {
        lock();
        n_ops_so_far++;
        unlock();

        /*
        if(n_ops_so_far % 200000 == 0)
            printf("%ld\n", n_ops_so_far);
        */
        if(!latencies_fd || last_qps == 0)
            return;

        // We cannot possibly write every latency because that stalls
        // the client, so we want to scale that by the number of qps
        // (we'll sample latencies for roughly N random ops every
        // second).
        const int samples_per_second = 20;
        if(rand() % (last_qps / samples_per_second) != 0) {
            return;
        }

        lock();

        int _off = snprintf(latencies + latencies_offset, sizeof(latencies) - latencies_offset, "%ld\t\t%.2f\n", n_op, latency);
        if(_off >= sizeof(latencies) - latencies_offset) {
            // Couldn't write everything, flush
            fwrite(latencies, 1, latencies_offset, latencies_fd);

            // Write again
            latencies_offset = 0;
            _off = snprintf(latencies + latencies_offset, sizeof(latencies) - latencies_offset, "%ld\t\t%.2f\n", n_op, latency);
        }
        latencies_offset += _off;

        n_op++;

        unlock();
    }

private:
    config_t *config;
    map<int, pair<int, int> > qps_map;
    char qps[40960], latencies[40960];
    int qps_offset, latencies_offset;
    FILE *qps_fd, *latencies_fd;
    pthread_mutex_t mutex;
    int last_qps;

    long n_op;
    int n_tick;
    long n_ops_so_far;

public:
    // We have one value shared among all the threads.
    char *value_buf;

private:
    void lock() {
        pthread_mutex_lock(&mutex);
    }
    void unlock() {
        pthread_mutex_unlock(&mutex);
    }
};

/* Communication structure for main thread and clients */
struct client_data_t {
    config_t *config;
    server_t *server;
    shared_t *shared;
    protocol_t *proto;
    sqlite_protocol_t *sqlite;
    int id;
    int min_seed, max_seed;
};

/* The function that does the work */
void* run_client(void* data) {
    // Grab the config
    client_data_t *client_data = (client_data_t*)data;
    config_t *config = client_data->config;
    server_t *server = client_data->server;
    shared_t *shared = client_data->shared;
    protocol_t *proto = client_data->proto;
    sqlite_protocol_t *sqlite = client_data->sqlite;
    if(sqlite)
        sqlite->set_id(client_data->id);

    const size_t per_key_size = config->keys.calculate_max_length(config->clients - 1);

    // Perform the ops
    ticks_t last_time = get_ticks(), start_time = last_time, last_qps_time = last_time, now_time;
    int qps = 0, tick = 0;
    int total_queries = 0;
    int total_inserts = 0;
    int total_deletes = 0;
    bool keep_running = true;
    while(keep_running) {
        // Generate the command
        load_t::load_op_t cmd = config->load.toss((config->batch_factor.min + config->batch_factor.max) / 2.0f);

        payload_t op_keys[config->batch_factor.max];
        char key_space[per_key_size * config->batch_factor.max];

        for (int i = 0; i < config->batch_factor.max; i++)
            op_keys[i].first = key_space + (per_key_size * i);

        payload_t op_vals[config->batch_factor.max];

        for (int i = 0; i < config->batch_factor.max; i++)
            op_vals[i].first = shared->value_buf;

        char val_verifcation_buffer[MAX_VALUE_SIZE];
        char *old_val_buffer;

        int j, k, l; // because we can't declare in the loop
        int count;
        int keyn; //same deal
        uint64_t id_salt = client_data->id;
        id_salt += id_salt << 32;
        // TODO: If an workload contains contains no inserts and there are no keys available for a particular client (and the duration is specified in q/i), it'll just loop forever.
        try {
            switch(cmd) {
            case load_t::delete_op:
                if (client_data->min_seed == client_data->max_seed)
                    break;

                config->keys.toss(op_keys, client_data->min_seed ^ id_salt, client_data->id, config->clients - 1);

                // Delete it from the server
                proto->remove(op_keys->first, op_keys->second);
                if (sqlite && (client_data->min_seed % RELIABILITY) == 0)
                    sqlite->remove(op_keys->first, op_keys->second);

                client_data->min_seed++;

                qps++;
                total_queries++;
                total_deletes++;
                break;

            case load_t::update_op:
                // Find the key and generate the payload
                if (client_data->min_seed == client_data->max_seed)
                    break;

                keyn = random(client_data->min_seed, client_data->max_seed);

                config->keys.toss(op_keys, keyn ^ id_salt, client_data->id, config->clients - 1);
                op_vals[0].second = seeded_random(config->values.min, config->values.max, client_data->max_seed ^ id_salt ^ update_salt);

                // Send it to server
                proto->update(op_keys->first, op_keys->second, op_vals[0].first, op_vals[0].second);

                if (sqlite && (keyn % RELIABILITY) == 0)
                    sqlite->update(op_keys->first, op_keys->second, op_vals[0].first, op_vals[0].second);

                // Free the value
                qps++;
                total_queries++;
                break;

            case load_t::insert_op:
                // Generate the payload
                config->keys.toss(op_keys, client_data->max_seed ^ id_salt, client_data->id, config->clients - 1);
                op_vals[0].second = seeded_random(config->values.min, config->values.max, client_data->max_seed ^ id_salt);

                // Send it to server
                proto->insert(op_keys->first, op_keys->second, op_vals[0].first, op_vals[0].second);

                if (sqlite && (client_data->max_seed % RELIABILITY) == 0)
                    sqlite->insert(op_keys->first, op_keys->second, op_vals[0].first, op_vals[0].second);

                client_data->max_seed++;

                // Free the value and save the key
                qps++;
                total_queries++;
                total_inserts++;
                break;

            case load_t::read_op:
                // Find the key
                if(client_data->min_seed == client_data->max_seed)
                    break;
                j = random(config->batch_factor.min, config->batch_factor.max);
                j = std::min(j, client_data->max_seed - client_data->min_seed);
                l = random(client_data->min_seed, client_data->max_seed - 1);
                for (k = 0; k < j; k++) {
                    config->keys.toss(&op_keys[k], l ^ id_salt, client_data->id, config->clients - 1);
                    l++;
                    if(l >= client_data->max_seed)
                        l = client_data->min_seed;

                }
                // Read it from the server
                proto->read(&op_keys[0], j);

                qps += j;
                total_queries += j;
                break;
            case load_t::append_op:
                //TODO, this doesn't check if we'll be making the value too big. Gotta check for that.
                //Find the key
                if (client_data->min_seed == client_data->max_seed)
                    break;

                keyn = random(client_data->min_seed, client_data->max_seed);

                config->keys.toss(op_keys, keyn ^ id_salt, client_data->id, config->clients - 1);
                op_vals[0].second = seeded_random(config->values.min, config->values.max, client_data->max_seed ^ id_salt);

                proto->append(op_keys->first, op_keys->second, op_vals->first, op_vals->second);
                if (sqlite && (keyn % RELIABILITY) == 0)
                    sqlite->append(op_keys->first, op_keys->second, op_vals->first, op_vals->second);

                qps++;
                total_queries++;
                break;

            case load_t::prepend_op:
                //Find the key
                if (client_data->min_seed == client_data->max_seed)
                    break;

                keyn = random(client_data->min_seed, client_data->max_seed);

                config->keys.toss(op_keys, keyn ^ id_salt, client_data->id, config->clients - 1);
                op_vals[0].second = seeded_random(config->values.min, config->values.max, client_data->max_seed ^ id_salt);

                proto->prepend(op_keys->first, op_keys->second, op_vals->first, op_vals->second);
                if (sqlite && (keyn % RELIABILITY) == 0)
                    sqlite->prepend(op_keys->first, op_keys->second, op_vals->first, op_vals->second);

                qps++;
                total_queries++;
                break;
            case load_t::verify_op:
                /* this is a very expensive operation it will first do a very
                 * expensive operation on the SQLITE reference db and then it will
                 * do several queries on the db that's being stressed (and only add
                 * 1 total query), it does not make sense to use this as part of a
                 * benchmarking run */

                // we can't do anything without a reference
                if (!sqlite)
                    break;

                /* this is hacky but whatever */
                old_val_buffer = op_vals[0].first;
                op_vals[0].first = val_verifcation_buffer;
                op_vals[0].second = 0;

                sqlite->dump_start();
                while (sqlite->dump_next(op_keys, op_vals)) {
                    proto->read(op_keys, 1, op_vals);
                }
                sqlite->dump_end();

                op_vals[0].first = old_val_buffer;

                qps++;
                total_queries++;
                break;
            default:
                fprintf(stderr, "Uknown operation\n");
                exit(-1);
            };
        } catch (const protocol_error_t& e) {
            fprintf(stderr, "Protocol error: %s\n", e.c_str());
            exit(-1);
        }
        now_time = get_ticks();

        // Deal with individual op latency
        ticks_t latency = now_time - last_time;
        shared->push_latency(ticks_to_us(latency));
        last_time = now_time;

        // Deal with QPS
        if(ticks_to_secs(now_time - last_qps_time) >= 1.0f) {
            shared->push_qps(qps, tick);

            last_qps_time = now_time;
            qps = 0;
            tick++;
        }

        // See if we should keep running
        if (config->duration.duration != -1) {
            switch(config->duration.units) {
            case duration_t::queries_t:
                keep_running = total_queries < config->duration.duration / config->clients;
                break;
            case duration_t::seconds_t:
                keep_running = ticks_to_secs(now_time - start_time) < config->duration.duration;
                break;
            case duration_t::inserts_t:
                keep_running = total_inserts - total_deletes < config->duration.duration / config->clients;
                break;
            default:
                fprintf(stderr, "Unknown duration unit\n");
                exit(-1);
            }
        }
    }
}

#endif // __CLIENT_HPP__

<commit_msg>Fix the stress clients behavior for high latency situations<commit_after>
#ifndef __CLIENT_HPP__
#define __CLIENT_HPP__

#include <pthread.h>
#include "protocol.hpp"
#include "sqlite_protocol.hpp"
#include <stdint.h>
#include "assert.h"

#define update_salt 6830

using namespace std;

/* Information shared between clients */
struct shared_t {
public:
    shared_t(config_t *_config)
        : config(_config),
          qps_offset(0), latencies_offset(0),
          qps_fd(NULL), latencies_fd(NULL),
          last_qps(0), n_op(1), n_tick(1), n_ops_so_far(0)
        {
            pthread_mutex_init(&mutex, NULL);

            if(config->qps_file[0] != 0) {
                qps_fd = fopen(config->qps_file, "wa");
            }
            if(config->latency_file[0] != 0) {
                latencies_fd = fopen(config->latency_file, "wa");
            }

            value_buf = new char[config->values.max];
            memset((void *) value_buf, 'A', config->values.max);
        }

    ~shared_t() {
        if(qps_fd) {
            fwrite(qps, 1, qps_offset, qps_fd);
            fclose(qps_fd);
        }
        if(latencies_fd) {
            fwrite(latencies, 1, latencies_offset, latencies_fd);
            fclose(latencies_fd);
        }

        pthread_mutex_destroy(&mutex);

        delete value_buf;
    }

    void push_qps(int _qps, int tick) {
        if(!qps_fd && !latencies_fd)
            return;

        lock();

        // Collect qps info from all clients before attempting to print
        if(config->clients > 1) {
            int qps_count = 0, agg_qps = 0;
            map<int, pair<int, int> >::iterator op = qps_map.find(tick);
            if(op != qps_map.end()) {
                qps_count = op->second.first;
                agg_qps = op->second.second;
            }

            qps_count += 1;
            agg_qps += _qps;

            if(qps_count == config->clients) {
                _qps = agg_qps;
                qps_map.erase(op);
            } else {
                qps_map[tick] = pair<int, int>(qps_count, agg_qps);
                unlock();
                return;
            }
        }

        last_qps = _qps;

        if(!qps_fd) {
            unlock();
            return;
        }

        int _off = snprintf(qps + qps_offset, sizeof(qps) - qps_offset, "%d\t\t%d\n", n_tick, _qps);
        if(_off >= sizeof(qps) - qps_offset) {
            // Couldn't write everything, flush
            fwrite(qps, 1, qps_offset, qps_fd);

            // Write again
            qps_offset = 0;
            _off = snprintf(qps + qps_offset, sizeof(qps) - qps_offset, "%d\t\t%d\n", n_tick, _qps);
        }
        qps_offset += _off;

        n_tick++;

        unlock();
    }

    void push_latency(float latency) {
        lock();
        n_ops_so_far++;
        unlock();

        /*
        if(n_ops_so_far % 200000 == 0)
            printf("%ld\n", n_ops_so_far);
        */
        if(!latencies_fd || last_qps == 0)
            return;

        // We cannot possibly write every latency because that stalls
        // the client, so we want to scale that by the number of qps
        // (we'll sample latencies for roughly N random ops every
        // second).
        const int samples_per_second = 20;
        if(rand() % (last_qps / samples_per_second) != 0) {
            return;
        }

        lock();

        int _off = snprintf(latencies + latencies_offset, sizeof(latencies) - latencies_offset, "%ld\t\t%.2f\n", n_op, latency);
        if(_off >= sizeof(latencies) - latencies_offset) {
            // Couldn't write everything, flush
            fwrite(latencies, 1, latencies_offset, latencies_fd);

            // Write again
            latencies_offset = 0;
            _off = snprintf(latencies + latencies_offset, sizeof(latencies) - latencies_offset, "%ld\t\t%.2f\n", n_op, latency);
        }
        latencies_offset += _off;

        n_op++;

        unlock();
    }

private:
    config_t *config;
    map<int, pair<int, int> > qps_map;
    char qps[40960], latencies[40960];
    int qps_offset, latencies_offset;
    FILE *qps_fd, *latencies_fd;
    pthread_mutex_t mutex;
    int last_qps;

    long n_op;
    int n_tick;
    long n_ops_so_far;

public:
    // We have one value shared among all the threads.
    char *value_buf;

private:
    void lock() {
        pthread_mutex_lock(&mutex);
    }
    void unlock() {
        pthread_mutex_unlock(&mutex);
    }
};

/* Communication structure for main thread and clients */
struct client_data_t {
    config_t *config;
    server_t *server;
    shared_t *shared;
    protocol_t *proto;
    sqlite_protocol_t *sqlite;
    int id;
    int min_seed, max_seed;
};

/* The function that does the work */
void* run_client(void* data) {
    // Grab the config
    client_data_t *client_data = (client_data_t*)data;
    config_t *config = client_data->config;
    server_t *server = client_data->server;
    shared_t *shared = client_data->shared;
    protocol_t *proto = client_data->proto;
    sqlite_protocol_t *sqlite = client_data->sqlite;
    if(sqlite)
        sqlite->set_id(client_data->id);

    const size_t per_key_size = config->keys.calculate_max_length(config->clients - 1);

    // Perform the ops
    ticks_t last_time = get_ticks(), start_time = last_time, last_qps_time = last_time, now_time;
    int qps = 0, tick = 0;
    int total_queries = 0;
    int total_inserts = 0;
    int total_deletes = 0;
    bool keep_running = true;
    while(keep_running) {
        // Generate the command
        load_t::load_op_t cmd = config->load.toss((config->batch_factor.min + config->batch_factor.max) / 2.0f);

        payload_t op_keys[config->batch_factor.max];
        char key_space[per_key_size * config->batch_factor.max];

        for (int i = 0; i < config->batch_factor.max; i++)
            op_keys[i].first = key_space + (per_key_size * i);

        payload_t op_vals[config->batch_factor.max];

        for (int i = 0; i < config->batch_factor.max; i++)
            op_vals[i].first = shared->value_buf;

        char val_verifcation_buffer[MAX_VALUE_SIZE];
        char *old_val_buffer;

        int j, k, l; // because we can't declare in the loop
        int count;
        int keyn; //same deal
        uint64_t id_salt = client_data->id;
        id_salt += id_salt << 32;
        // TODO: If an workload contains contains no inserts and there are no keys available for a particular client (and the duration is specified in q/i), it'll just loop forever.
        try {
            switch(cmd) {
            case load_t::delete_op:
                if (client_data->min_seed == client_data->max_seed)
                    break;

                config->keys.toss(op_keys, client_data->min_seed ^ id_salt, client_data->id, config->clients - 1);

                // Delete it from the server
                proto->remove(op_keys->first, op_keys->second);
                if (sqlite && (client_data->min_seed % RELIABILITY) == 0)
                    sqlite->remove(op_keys->first, op_keys->second);

                client_data->min_seed++;

                qps++;
                total_queries++;
                total_deletes++;
                break;

            case load_t::update_op:
                // Find the key and generate the payload
                if (client_data->min_seed == client_data->max_seed)
                    break;

                keyn = random(client_data->min_seed, client_data->max_seed);

                config->keys.toss(op_keys, keyn ^ id_salt, client_data->id, config->clients - 1);
                op_vals[0].second = seeded_random(config->values.min, config->values.max, client_data->max_seed ^ id_salt ^ update_salt);

                // Send it to server
                proto->update(op_keys->first, op_keys->second, op_vals[0].first, op_vals[0].second);

                if (sqlite && (keyn % RELIABILITY) == 0)
                    sqlite->update(op_keys->first, op_keys->second, op_vals[0].first, op_vals[0].second);

                // Free the value
                qps++;
                total_queries++;
                break;

            case load_t::insert_op:
                // Generate the payload
                config->keys.toss(op_keys, client_data->max_seed ^ id_salt, client_data->id, config->clients - 1);
                op_vals[0].second = seeded_random(config->values.min, config->values.max, client_data->max_seed ^ id_salt);

                // Send it to server
                proto->insert(op_keys->first, op_keys->second, op_vals[0].first, op_vals[0].second);

                if (sqlite && (client_data->max_seed % RELIABILITY) == 0)
                    sqlite->insert(op_keys->first, op_keys->second, op_vals[0].first, op_vals[0].second);

                client_data->max_seed++;

                // Free the value and save the key
                qps++;
                total_queries++;
                total_inserts++;
                break;

            case load_t::read_op:
                // Find the key
                if(client_data->min_seed == client_data->max_seed)
                    break;
                j = random(config->batch_factor.min, config->batch_factor.max);
                j = std::min(j, client_data->max_seed - client_data->min_seed);
                l = random(client_data->min_seed, client_data->max_seed - 1);
                for (k = 0; k < j; k++) {
                    config->keys.toss(&op_keys[k], l ^ id_salt, client_data->id, config->clients - 1);
                    l++;
                    if(l >= client_data->max_seed)
                        l = client_data->min_seed;

                }
                // Read it from the server
                proto->read(&op_keys[0], j);

                qps += j;
                total_queries += j;
                break;
            case load_t::append_op:
                //TODO, this doesn't check if we'll be making the value too big. Gotta check for that.
                //Find the key
                if (client_data->min_seed == client_data->max_seed)
                    break;

                keyn = random(client_data->min_seed, client_data->max_seed);

                config->keys.toss(op_keys, keyn ^ id_salt, client_data->id, config->clients - 1);
                op_vals[0].second = seeded_random(config->values.min, config->values.max, client_data->max_seed ^ id_salt);

                proto->append(op_keys->first, op_keys->second, op_vals->first, op_vals->second);
                if (sqlite && (keyn % RELIABILITY) == 0)
                    sqlite->append(op_keys->first, op_keys->second, op_vals->first, op_vals->second);

                qps++;
                total_queries++;
                break;

            case load_t::prepend_op:
                //Find the key
                if (client_data->min_seed == client_data->max_seed)
                    break;

                keyn = random(client_data->min_seed, client_data->max_seed);

                config->keys.toss(op_keys, keyn ^ id_salt, client_data->id, config->clients - 1);
                op_vals[0].second = seeded_random(config->values.min, config->values.max, client_data->max_seed ^ id_salt);

                proto->prepend(op_keys->first, op_keys->second, op_vals->first, op_vals->second);
                if (sqlite && (keyn % RELIABILITY) == 0)
                    sqlite->prepend(op_keys->first, op_keys->second, op_vals->first, op_vals->second);

                qps++;
                total_queries++;
                break;
            case load_t::verify_op:
                /* this is a very expensive operation it will first do a very
                 * expensive operation on the SQLITE reference db and then it will
                 * do several queries on the db that's being stressed (and only add
                 * 1 total query), it does not make sense to use this as part of a
                 * benchmarking run */

                // we can't do anything without a reference
                if (!sqlite)
                    break;

                /* this is hacky but whatever */
                old_val_buffer = op_vals[0].first;
                op_vals[0].first = val_verifcation_buffer;
                op_vals[0].second = 0;

                sqlite->dump_start();
                while (sqlite->dump_next(op_keys, op_vals)) {
                    proto->read(op_keys, 1, op_vals);
                }
                sqlite->dump_end();

                op_vals[0].first = old_val_buffer;

                qps++;
                total_queries++;
                break;
            default:
                fprintf(stderr, "Uknown operation\n");
                exit(-1);
            };
        } catch (const protocol_error_t& e) {
            fprintf(stderr, "Protocol error: %s\n", e.c_str());
            exit(-1);
        }
        now_time = get_ticks();

        // Deal with individual op latency
        ticks_t latency = now_time - last_time;
        shared->push_latency(ticks_to_us(latency));
        last_time = now_time;

        // Deal with QPS
        if (ticks_to_secs(now_time - last_qps_time) >= 1.0f) {
            // First deal with missed QPS
            int qps_to_write = ticks_to_secs(now_time - last_qps_time);
            while (qps_to_write > 1) {
                shared->push_qps(0, tick);

                //last_qps_time = now_time;
                last_qps_time = last_qps_time + secs_to_ticks(1.0f);
                tick++;
                --qps_to_write;
            }

            shared->push_qps(qps, tick);

            //last_qps_time = now_time;
            last_qps_time = last_qps_time + secs_to_ticks(1.0f);
            qps = 0;
            tick++;
        }

        // See if we should keep running
        if (config->duration.duration != -1) {
            switch(config->duration.units) {
            case duration_t::queries_t:
                keep_running = total_queries < config->duration.duration / config->clients;
                break;
            case duration_t::seconds_t:
                //keep_running = ticks_to_secs(now_time - start_time) < config->duration.duration;
                keep_running = tick < config->duration.duration;
                break;
            case duration_t::inserts_t:
                keep_running = total_inserts - total_deletes < config->duration.duration / config->clients;
                break;
            default:
                fprintf(stderr, "Unknown duration unit\n");
                exit(-1);
            }
        }
    }
}

#endif // __CLIENT_HPP__

<|endoftext|>


<commit_before>/**
 * @file
 */
#pragma once

#include "bi/io/indentable_ostream.hpp"

namespace bi {
/**
 * C++ code generator.
 *
 * @ingroup compiler_io
 */
class CppBaseGenerator: public indentable_ostream {
public:
  CppBaseGenerator(std::ostream& base, const int level = 0,
      const bool header = false);

  using indentable_ostream::visit;

  virtual void visit(const Name* o);

  virtual void visit(const List<Expression>* o);
  virtual void visit(const Literal<bool>* o);
  virtual void visit(const Literal<int64_t>* o);
  virtual void visit(const Literal<double>* o);
  virtual void visit(const Literal<const char*>* o);
  virtual void visit(const Parentheses* o);
  virtual void visit(const Brackets* o);
  virtual void visit(const Call* o);
  virtual void visit(const BinaryCall* o);
  virtual void visit(const UnaryCall* o);
  virtual void visit(const Slice* o);
  virtual void visit(const Query* o);
  virtual void visit(const Get* o);
  virtual void visit(const LambdaFunction* o);
  virtual void visit(const Span* o);
  virtual void visit(const Index* o);
  virtual void visit(const Range* o);
  virtual void visit(const Member* o);
  virtual void visit(const Super* o);
  virtual void visit(const This* o);
  virtual void visit(const Nil* o);
  virtual void visit(const Parameter* o);
  virtual void visit(const MemberParameter* o);
  virtual void visit(const Identifier<Parameter>* o);
  virtual void visit(const Identifier<MemberParameter>* o);
  virtual void visit(const Identifier<GlobalVariable>* o);
  virtual void visit(const Identifier<LocalVariable>* o);
  virtual void visit(const Identifier<MemberVariable>* o);
  virtual void visit(const OverloadedIdentifier<Function>* o);
  virtual void visit(const OverloadedIdentifier<Fiber>* o);
  virtual void visit(const OverloadedIdentifier<MemberFunction>* o);
  virtual void visit(const OverloadedIdentifier<MemberFiber>* o);
  virtual void visit(const OverloadedIdentifier<BinaryOperator>* o);
  virtual void visit(const OverloadedIdentifier<UnaryOperator>* o);

  virtual void visit(const File* o);
  virtual void visit(const GlobalVariable* o);
  virtual void visit(const LocalVariable* o);
  virtual void visit(const MemberVariable* o);
  virtual void visit(const List<Statement>* o);
  virtual void visit(const Function* o);
  virtual void visit(const Fiber* o);
  virtual void visit(const MemberFunction* o);
  virtual void visit(const MemberFiber* o);
  virtual void visit(const Program* o);
  virtual void visit(const BinaryOperator* o);
  virtual void visit(const UnaryOperator* o);
  virtual void visit(const AssignmentOperator* o);
  virtual void visit(const ConversionOperator* o);
  virtual void visit(const Basic* o);
  virtual void visit(const Class* o);
  virtual void visit(const Alias* o);
  virtual void visit(const Assignment* o);
  virtual void visit(const ExpressionStatement* o);
  virtual void visit(const If* o);
  virtual void visit(const For* o);
  virtual void visit(const While* o);
  virtual void visit(const Assert* o);
  virtual void visit(const Return* o);
  virtual void visit(const Yield* o);
  virtual void visit(const Raw* o);

  virtual void visit(const ListType* o);
  virtual void visit(const EmptyType* o);
  virtual void visit(const ArrayType* o);
  virtual void visit(const ParenthesesType* o);
  virtual void visit(const FunctionType* o);
  virtual void visit(const FiberType* o);
  virtual void visit(const OptionalType* o);
  virtual void visit(const ClassType* o);
  virtual void visit(const BasicType* o);
  virtual void visit(const AliasType* o);

protected:
  /**
   * Generate the initialization of a variable, including the call to the
   * constructor and/or assignment of the initial value.
   */
  template<class T>
  void genInit(const T* o);

  /**
   * Output header instead of source?
   */
  bool header;
};
}

template<class T>
void bi::CppBaseGenerator::genInit(const T* o) {
  if (o->type->isArray()) {
    ArrayType* type = dynamic_cast<ArrayType*>(o->type);
    assert(type);
    middle('(');
    if (!o->value->isEmpty()) {
      middle(o->value << ", ");
    }
    middle("bi::make_frame(" << type->brackets << ')');
    if (!o->parens->isEmpty()) {
      middle(", " << o->parens->strip());
    }
    middle(')');
  } else if (o->type->isClass()) {
    ClassType* type = dynamic_cast<ClassType*>(o->type);
    assert(type);
    if (!o->parens->isEmpty()) {
      middle(" = bi::make_object<bi::" << type->name << '>' << o->parens);
    } else if (o->value->isEmpty()) {
      middle(" = bi::make_object<bi::" << type->name << ">()");
    }
  } else if (!o->value->isEmpty()) {
    middle(" = " << o->value);
  }
}
<commit_msg>Fixed initialisation of objects when initial value given.<commit_after>/**
 * @file
 */
#pragma once

#include "bi/io/indentable_ostream.hpp"

namespace bi {
/**
 * C++ code generator.
 *
 * @ingroup compiler_io
 */
class CppBaseGenerator: public indentable_ostream {
public:
  CppBaseGenerator(std::ostream& base, const int level = 0,
      const bool header = false);

  using indentable_ostream::visit;

  virtual void visit(const Name* o);

  virtual void visit(const List<Expression>* o);
  virtual void visit(const Literal<bool>* o);
  virtual void visit(const Literal<int64_t>* o);
  virtual void visit(const Literal<double>* o);
  virtual void visit(const Literal<const char*>* o);
  virtual void visit(const Parentheses* o);
  virtual void visit(const Brackets* o);
  virtual void visit(const Call* o);
  virtual void visit(const BinaryCall* o);
  virtual void visit(const UnaryCall* o);
  virtual void visit(const Slice* o);
  virtual void visit(const Query* o);
  virtual void visit(const Get* o);
  virtual void visit(const LambdaFunction* o);
  virtual void visit(const Span* o);
  virtual void visit(const Index* o);
  virtual void visit(const Range* o);
  virtual void visit(const Member* o);
  virtual void visit(const Super* o);
  virtual void visit(const This* o);
  virtual void visit(const Nil* o);
  virtual void visit(const Parameter* o);
  virtual void visit(const MemberParameter* o);
  virtual void visit(const Identifier<Parameter>* o);
  virtual void visit(const Identifier<MemberParameter>* o);
  virtual void visit(const Identifier<GlobalVariable>* o);
  virtual void visit(const Identifier<LocalVariable>* o);
  virtual void visit(const Identifier<MemberVariable>* o);
  virtual void visit(const OverloadedIdentifier<Function>* o);
  virtual void visit(const OverloadedIdentifier<Fiber>* o);
  virtual void visit(const OverloadedIdentifier<MemberFunction>* o);
  virtual void visit(const OverloadedIdentifier<MemberFiber>* o);
  virtual void visit(const OverloadedIdentifier<BinaryOperator>* o);
  virtual void visit(const OverloadedIdentifier<UnaryOperator>* o);

  virtual void visit(const File* o);
  virtual void visit(const GlobalVariable* o);
  virtual void visit(const LocalVariable* o);
  virtual void visit(const MemberVariable* o);
  virtual void visit(const List<Statement>* o);
  virtual void visit(const Function* o);
  virtual void visit(const Fiber* o);
  virtual void visit(const MemberFunction* o);
  virtual void visit(const MemberFiber* o);
  virtual void visit(const Program* o);
  virtual void visit(const BinaryOperator* o);
  virtual void visit(const UnaryOperator* o);
  virtual void visit(const AssignmentOperator* o);
  virtual void visit(const ConversionOperator* o);
  virtual void visit(const Basic* o);
  virtual void visit(const Class* o);
  virtual void visit(const Alias* o);
  virtual void visit(const Assignment* o);
  virtual void visit(const ExpressionStatement* o);
  virtual void visit(const If* o);
  virtual void visit(const For* o);
  virtual void visit(const While* o);
  virtual void visit(const Assert* o);
  virtual void visit(const Return* o);
  virtual void visit(const Yield* o);
  virtual void visit(const Raw* o);

  virtual void visit(const ListType* o);
  virtual void visit(const EmptyType* o);
  virtual void visit(const ArrayType* o);
  virtual void visit(const ParenthesesType* o);
  virtual void visit(const FunctionType* o);
  virtual void visit(const FiberType* o);
  virtual void visit(const OptionalType* o);
  virtual void visit(const ClassType* o);
  virtual void visit(const BasicType* o);
  virtual void visit(const AliasType* o);

protected:
  /**
   * Generate the initialization of a variable, including the call to the
   * constructor and/or assignment of the initial value.
   */
  template<class T>
  void genInit(const T* o);

  /**
   * Output header instead of source?
   */
  bool header;
};
}

template<class T>
void bi::CppBaseGenerator::genInit(const T* o) {
  if (o->type->isArray()) {
    ArrayType* type = dynamic_cast<ArrayType*>(o->type);
    assert(type);
    middle('(');
    if (!o->value->isEmpty()) {
      middle(o->value << ", ");
    }
    middle("bi::make_frame(" << type->brackets << ')');
    if (!o->parens->isEmpty()) {
      middle(", " << o->parens->strip());
    }
    middle(')');
  } else if (o->type->isClass()) {
    ClassType* type = dynamic_cast<ClassType*>(o->type);
    assert(type);
    if (!o->parens->isEmpty()) {
      middle(" = bi::make_object<bi::" << type->name << '>' << o->parens);
    } else if (o->value->isEmpty()) {
      middle(" = bi::make_object<bi::" << type->name << ">()");
    }
    if (!o->value->isEmpty()) {
      middle(" = " << o->value);
    }
  } else if (!o->value->isEmpty()) {
    middle(" = " << o->value);
  }
}
<|endoftext|>


<commit_before>#include "L6470Stepper.h"

L6470Stepper::L6470Stepper(int pinCS, int nDaisyChain, int* motorSteps)
: SPIDaisyChain(pinCS, nDaisyChain)
{
	_spiData = new L6470SpiData(nDaisyChain);
	_motor = new L6470MotorData[nDaisyChain];
	_readData = new uint32_t[nDaisyChain];

	for (size_t i = 0; i < nDaisyChain; i++) {
		_readData[i] = 0;
	}

	// pin settings
	_pinBUSY = 0xFF;	// NOT USE, default
	_pinSTEP = 0xFF;	// NOT USE, default
	useBusy = false;
	useStep = false;

	for (int i=0; i<nDaisyChain; i++) {
		if (motorSteps == NULL)	_motor[i].setMotorStep(DEFAULT_MOTOR_STEP);
		else					_motor[i].setMotorStep(motorSteps[i]);
	}

	resetDriver();
}

L6470Stepper::~L6470Stepper()
{
	delete[] _motor;
	delete[] _spiData;
	delete[] _readData;
}

void L6470Stepper::setup(registerStruct* regParamList, int size, int id)
{
	// set motor driver as L6480
	if (regParamList == NULL)	regParamList = (registerStruct*)L6470DefaultRegParamList;
	_writableRegNum = WRITABLE_REG_NUM_L6470;
	for (int i=0; i<getDaisyChainNum(); i++) {
		_motor[i].setup(L6470_REGISTER);
	}

	// initialize register
	if (id < 0) {
		for (int i=0; i<getDaisyChainNum(); i++) {
			initRegister(regParamList, _writableRegNum, i);
		}
	} else {
		if (size == 0)	size = _writableRegNum;
		initRegister(regParamList, size, id);
	}
}

void	L6470Stepper::angle(int degree)
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		setAngle(degree, i);
	}
	transferAction();
}

void	L6470Stepper::setAngle(int degree, int id)
{
    if(id >= getDaisyChainNum())
        return;
    static int pre_degree = 0;
    _motor[id].setAction(GOTO_DIR, degree, ((degree >= pre_degree) ? 1 : 0));
    pre_degree = degree;
}

void	L6470Stepper::step(int steps)
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		setStep(steps, i);
	}
	transferAction();
}

void	L6470Stepper::setStep(int steps, int id)
{
	if(id >= getDaisyChainNum())
		return;
	if (steps < 0) {
		_motor[id].setAction(MOVE, abs(steps), CCW);
	} else {
		_motor[id].setAction(MOVE, abs(steps), CW);
	}
}

void	L6470Stepper::rotate(motorDirection dir, int rps)
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		setRotate(dir, i, rps);
	}
	transferAction();
}

void	L6470Stepper::setRotate(motorDirection dir, int id, int rps)
{
	if(id >= getDaisyChainNum())
		return;
	if (rps == 0) {
		rps = _motor[id].getCurRps();
	}
	_motor[id].setAction(RUN, abs(rps), dir);
}

void 	L6470Stepper::setStepClockMode(_motorDirection dir, int id)
{
	if(id >= getDaisyChainNum())
        return;
    _motor[id].setAction(STEPCLOCK, 0, dir);
	transferAction();
}

long L6470Stepper::angleToStep(float angle, int motor_id)
{
	long steps = (uint32_t)((double)_motor[motor_id].getMotorStep() * (double)pow(2, _motor[motor_id].getMicroStep()) * (double)angle / 360.0);
	return steps;
}

float L6470Stepper::stepToAngle(long step, int motor_id)
{
	unsigned long all_steps = (unsigned long)((double)_motor[motor_id].getMotorStep() * (double)pow(2, _motor[motor_id].getMicroStep()));
	float angle = 360. * step / all_steps;
	return angle;
}

void L6470Stepper::stepClock(float abs_angle, float ms, int motor_id)
{
	float curr_angle = getCurrAngle();
	float next_angle = abs_angle - curr_angle;
	_motorDirection dir = (next_angle - curr_angle < 0) ? CCW : CW;

	// long curr_step = getCurrStep();
	// long next_step = angleToStep(abs_angle);
	// long diff_step = next_step - curr_step;
	// _motorDirection direction = (diff_step < 0) ? CCW : CW;
	// diff_step = abs(diff_step);

	stepClock(angleToStep(next_angle - curr_angle), dir, ms, motor_id);
}

void L6470Stepper::stepClock(long step, _motorDirection dir, float ms, int motor_id)
{
	// noTone(_pinSTEP);
	// delay(100);
	setStepClockMode(dir, motor_id);
	// delay(100);
	unsigned int hz = 0;
	if (ms <= 0) {
		hz = 0xFFFF; // numeric_limits<int>::max() 16bit
	} else {
		hz = (unsigned int)getHzFrom(step, ms);
	}
	tone(_pinSTEP, hz);
}

long L6470Stepper::getHzFrom(long steps, float ms)
{
	if (ms <= 0) {
		return 0;
	}
	return (long)(1. / ((double)ms / 1000. / (double)steps));
}

void L6470Stepper::setRps(int rps)
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		setRps(rps, i);
	}
	transferRegister();
}

void L6470Stepper::setRps(int rps, int id)
{
	if(id >= getDaisyChainNum())
		return;
	_motor[id].setRegister(REG_MAX_SPEED, rps);
}

long L6470Stepper::readRegister(int reg, int motor_id)
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		requestRegister(reg, i);
	}
	transferRegister();

	long ret = 0;
	if (reg == REG_ABS_POS) {
		long sign = ((_readData[motor_id] & 0x200000) >> 21);
		long val = (_readData[motor_id] & 0x1FFFFF);
		if (sign == 0) {
			ret = (long)val;
		} else {
			ret = (long)(-1) * (long)(0x200000 - val);
		}
		// Serial.print("sign : "); Serial.println(sign, HEX);
		// Serial.print("value: "); Serial.println(val, HEX);
	} else {
		ret = (long)_readData[motor_id];
	}
	return ret;
}

void L6470Stepper::requestRegister(int reg, int id)
{
	if(id >= getDaisyChainNum())
		return;
	_motor[id].requestRegister(reg);
}

long L6470Stepper::getCurrStep(int motor_id)
{
	long step = readRegister(REG_ABS_POS, motor_id);
	return step;
}

float 	L6470Stepper::getCurrAngle(int motor_id)
{
	long step = getCurrStep();
	return stepToAngle(step);
}

void L6470Stepper::execute()
{
	bool needToSetReg = false;
	bool needToSetCmd = false;

	// check if spi action is needed
	for (int i=0; i<getDaisyChainNum(); i++) {
		needToSetReg |= _motor[i].isRegChanged();
		needToSetCmd |= _motor[i].isCmdChanged();
	}

	if (needToSetReg) {
		transferRegister();
	}

	if (needToSetCmd) {
		transferAction();
	}
}

void L6470Stepper::transferAction()
{
	_spiData->updateAsAppCmd(_motor);

	transferDaisyChain(_spiData->getCmd());
	for (int i=SPI_VAL_MAXSIZE-_spiData->getSize(); i<SPI_VAL_MAXSIZE; i++) {
		transferDaisyChain(_spiData->getVal(i));
	}
}

void L6470Stepper::transferRegister()
{
	// send data order / 0: MSB n: LSB
	// read data order / 0: LSB n: MSB

	_readUnion readUnion[getDaisyChainNum()];
	_spiData->updateAsRegCmd(_motor);

	for (size_t i = 0; i < getDaisyChainNum(); i++) {
		for (size_t j = 0; j < 4; j++) {
			readUnion[i].bval[j] = 0;
		}
		// readUnion[i].ival = 0;
	}

	// Serial.println("Send Command: ");
	transferDaisyChain(_spiData->getCmd());
	int startByte = SPI_VAL_MAXSIZE-_spiData->getSize();
	for (int i=0; i<SPI_VAL_MAXSIZE; i++) {
		if (i < startByte) {
			for (size_t j = 0; j < getDaisyChainNum(); j++) {
				readUnion[j].bval[SPI_VAL_MAXSIZE-1-i] = 0;
			}
		} else {
			// Serial.println("Send Data and Read Register: ");
			byte* readBytes;
			readBytes = (byte*)transferDaisyChain(_spiData->getVal(i));	// get pointer, read data comes from MSB
			for (int j=0; j<getDaisyChainNum(); j++) {
				// Serial.println("Got Data: ");
				readUnion[j].bval[SPI_VAL_MAXSIZE-1-i] = readBytes[j];
				// Serial.println(readBytes[j], HEX);
				// Serial.println(readUnion[j].bval[SPI_VAL_MAXSIZE-1-i], HEX);
			}
		}
	}

	// copy data to array
	// Serial.println("final data: ");
	for (int i=0; i<getDaisyChainNum(); i++) {
		_readData[i] = readUnion[i].ival;
		// for (size_t j = 0; j < 4; j++) {
		// 	Serial.print(readUnion[i].bval[j], HEX); Serial.print(" ");
		// }
		// Serial.print("int: "); Serial.println(readUnion[i].ival, HEX);
		// Serial.print("int: "); Serial.println(_readData[i], HEX);
	}
}

void	L6470Stepper::clearMotorActions()
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		clearMotorAction(i);
	}
}

void	L6470Stepper::clearMotorAction(int id)
{
	_motor[id].setAction(NOP);
	_motor[id].setRegister(REG_NOP);
}

void L6470Stepper::resetDriver()
{
	for (int i=0; i<getDaisyChainNum(); i++)
	{
		_motor[i].setAction(NOP);
	}
	transferAction();

	for (int i=0; i<getDaisyChainNum(); i++)
	{
		_motor[i].setAction(RESETDEVICE);
	}
	transferAction();
}

void L6470Stepper::initRegister(registerStruct* regParamList, int size, int id)
{
	for (int i=0; i<size; i++) {
		for (int j=0; j<getDaisyChainNum(); j++) {
			if (j == id)	_motor[j].setRegister(regParamList[i].reg, regParamList[i].val);
			else			_motor[j].setRegister(REG_NOP, 0);
		}
		transferRegister();
	}
}

// optional
void	L6470Stepper::setBusyEnable(int pinBUSY)
{
	_pinBUSY = pinBUSY;
	useBusy = true;
}

void	L6470Stepper::setStepEnable(int pinSTEP)
{
	_pinSTEP = pinSTEP;
	useStep = true;
}

boolean	L6470Stepper::isBusy()
{
	// todo: faster one
	// todo: status register has the mirror
	return !digitalRead(_pinBUSY);	// BUSY: LOW, FREE: HIGH
}

L6480Stepper::L6480Stepper(int pinCS, int nDaisyChain, int* motorSteps)
:L6470Stepper(pinCS, nDaisyChain, motorSteps)
{
}

L6480Stepper::~L6480Stepper()
{

}

void L6480Stepper::setup(registerStruct* regParamList, int size, int id)
{
	// set motor driver as L6480
	if (regParamList == NULL)	regParamList = (registerStruct*)L6480DefaultRegParamList;
	_writableRegNum = WRITABLE_REG_NUM_L6480;
	for (int i=0; i<getDaisyChainNum(); i++) {
		_motor[i].setup(L6480_REGISTER);
	}

	// initialize register
	if (id < 0) {
		for (int i=0; i<getDaisyChainNum(); i++) {
			initRegister(regParamList, _writableRegNum, i);
		}
	} else {
		if (size == 0)	size = _writableRegNum;
		initRegister(regParamList, size, id);
	}
}
<commit_msg>fix step-mode<commit_after>#include "L6470Stepper.h"

L6470Stepper::L6470Stepper(int pinCS, int nDaisyChain, int* motorSteps)
: SPIDaisyChain(pinCS, nDaisyChain)
{
	_spiData = new L6470SpiData(nDaisyChain);
	_motor = new L6470MotorData[nDaisyChain];
	_readData = new uint32_t[nDaisyChain];

	for (size_t i = 0; i < nDaisyChain; i++) {
		_readData[i] = 0;
	}

	// pin settings
	_pinBUSY = 0xFF;	// NOT USE, default
	_pinSTEP = 0xFF;	// NOT USE, default
	useBusy = false;
	useStep = false;

	for (int i=0; i<nDaisyChain; i++) {
		if (motorSteps == NULL)	_motor[i].setMotorStep(DEFAULT_MOTOR_STEP);
		else					_motor[i].setMotorStep(motorSteps[i]);
	}

	resetDriver();
}

L6470Stepper::~L6470Stepper()
{
	delete[] _motor;
	delete[] _spiData;
	delete[] _readData;
}

void L6470Stepper::setup(registerStruct* regParamList, int size, int id)
{
	// set motor driver as L6480
	if (regParamList == NULL)	regParamList = (registerStruct*)L6470DefaultRegParamList;
	_writableRegNum = WRITABLE_REG_NUM_L6470;
	for (int i=0; i<getDaisyChainNum(); i++) {
		_motor[i].setup(L6470_REGISTER);
	}

	// initialize register
	if (id < 0) {
		for (int i=0; i<getDaisyChainNum(); i++) {
			initRegister(regParamList, _writableRegNum, i);
		}
	} else {
		if (size == 0)	size = _writableRegNum;
		initRegister(regParamList, size, id);
	}
}

void	L6470Stepper::angle(int degree)
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		setAngle(degree, i);
	}
	transferAction();
}

void	L6470Stepper::setAngle(int degree, int id)
{
    if(id >= getDaisyChainNum())
        return;
    static int pre_degree = 0;
    _motor[id].setAction(GOTO_DIR, degree, ((degree >= pre_degree) ? 1 : 0));
    pre_degree = degree;
}

void	L6470Stepper::step(int steps)
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		setStep(steps, i);
	}
	transferAction();
}

void	L6470Stepper::setStep(int steps, int id)
{
	if(id >= getDaisyChainNum())
		return;
	if (steps < 0) {
		_motor[id].setAction(MOVE, abs(steps), CCW);
	} else {
		_motor[id].setAction(MOVE, abs(steps), CW);
	}
}

void	L6470Stepper::rotate(motorDirection dir, int rps)
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		setRotate(dir, i, rps);
	}
	transferAction();
}

void	L6470Stepper::setRotate(motorDirection dir, int id, int rps)
{
	if(id >= getDaisyChainNum())
		return;
	if (rps == 0) {
		rps = _motor[id].getCurRps();
	}
	_motor[id].setAction(RUN, abs(rps), dir);
}

void 	L6470Stepper::setStepClockMode(_motorDirection dir, int id)
{
	if(id >= getDaisyChainNum())
        return;
    _motor[id].setAction(STEPCLOCK, 0, dir);
	transferAction();
}

long L6470Stepper::angleToStep(float angle, int motor_id)
{
	long steps = (uint32_t)((double)_motor[motor_id].getMotorStep() * (double)pow(2, _motor[motor_id].getMicroStep()) * (double)angle / 360.0);
	return steps;
}

float L6470Stepper::stepToAngle(long step, int motor_id)
{
	unsigned long all_steps = (unsigned long)((double)_motor[motor_id].getMotorStep() * (double)pow(2, _motor[motor_id].getMicroStep()));
	float angle = 360. * step / all_steps;
	return angle;
}

void L6470Stepper::stepClock(float abs_angle, float ms, int motor_id)
{
	long curr_step = getCurrStep();
	long diff_step = angleToStep(abs_angle) - curr_step;
	_motorDirection dir = (diff_step < 0) ? CCW : CW;

	stepClock(abs(diff_step), dir, ms, motor_id);
}

void L6470Stepper::stepClock(long step, _motorDirection dir, float ms, int motor_id)
{
	setStepClockMode(dir, motor_id);
	unsigned int hz = 0;
	if (ms <= 0) {
		hz = 0xFFFF; // numeric_limits<int>::max() 16bit
		tone(_pinSTEP, hz);
	} else {
		hz = (unsigned int)getHzFrom(step, ms);
		if (hz > 65535) {
			Serial.print("Too High Frequency !! : "); Serial.println(hz);
			hz = 65535;
		} else if (hz < 31) {
			Serial.print("Too Low Frequency !! : "); Serial.println(hz);
			hz = 31;
		}
		// Serial.print("frequency is : "); Serial.println(hz);
		tone(_pinSTEP, hz, ms);
	}
}

long L6470Stepper::getHzFrom(long steps, float ms)
{
	if (ms <= 0) {
		return 0;
	}
	return (long)(1. / ((double)ms / 1000. / (double)steps));
}

void L6470Stepper::setRps(int rps)
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		setRps(rps, i);
	}
	transferRegister();
}

void L6470Stepper::setRps(int rps, int id)
{
	if(id >= getDaisyChainNum())
		return;
	_motor[id].setRegister(REG_MAX_SPEED, rps);
}

long L6470Stepper::readRegister(int reg, int motor_id)
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		requestRegister(reg, i);
	}
	transferRegister();

	long ret = 0;
	if (reg == REG_ABS_POS) {
		long sign = ((_readData[motor_id] & 0x200000) >> 21);
		long val = (_readData[motor_id] & 0x1FFFFF);
		if (sign == 0) {
			ret = (long)val;
		} else {
			ret = (long)(-1) * (long)(0x200000 - val);
		}
		// Serial.print("sign : "); Serial.println(sign, HEX);
		// Serial.print("value: "); Serial.println(val, HEX);
	} else {
		ret = (long)_readData[motor_id];
	}
	return ret;
}

void L6470Stepper::requestRegister(int reg, int id)
{
	if(id >= getDaisyChainNum())
		return;
	_motor[id].requestRegister(reg);
}

long L6470Stepper::getCurrStep(int motor_id)
{
	long step = readRegister(REG_ABS_POS, motor_id);
	return step;
}

float 	L6470Stepper::getCurrAngle(int motor_id)
{
	long step = getCurrStep();
	return stepToAngle(step);
}

void L6470Stepper::execute()
{
	bool needToSetReg = false;
	bool needToSetCmd = false;

	// check if spi action is needed
	for (int i=0; i<getDaisyChainNum(); i++) {
		needToSetReg |= _motor[i].isRegChanged();
		needToSetCmd |= _motor[i].isCmdChanged();
	}

	if (needToSetReg) {
		transferRegister();
	}

	if (needToSetCmd) {
		transferAction();
	}
}

void L6470Stepper::transferAction()
{
	_spiData->updateAsAppCmd(_motor);

	transferDaisyChain(_spiData->getCmd());
	for (int i=SPI_VAL_MAXSIZE-_spiData->getSize(); i<SPI_VAL_MAXSIZE; i++) {
		transferDaisyChain(_spiData->getVal(i));
	}
}

void L6470Stepper::transferRegister()
{
	// send data order / 0: MSB n: LSB
	// read data order / 0: LSB n: MSB

	_readUnion readUnion[getDaisyChainNum()];
	_spiData->updateAsRegCmd(_motor);

	for (size_t i = 0; i < getDaisyChainNum(); i++) {
		for (size_t j = 0; j < 4; j++) {
			readUnion[i].bval[j] = 0;
		}
		// readUnion[i].ival = 0;
	}

	// Serial.println("Send Command: ");
	transferDaisyChain(_spiData->getCmd());
	int startByte = SPI_VAL_MAXSIZE-_spiData->getSize();
	for (int i=0; i<SPI_VAL_MAXSIZE; i++) {
		if (i < startByte) {
			for (size_t j = 0; j < getDaisyChainNum(); j++) {
				readUnion[j].bval[SPI_VAL_MAXSIZE-1-i] = 0;
			}
		} else {
			// Serial.println("Send Data and Read Register: ");
			byte* readBytes;
			readBytes = (byte*)transferDaisyChain(_spiData->getVal(i));	// get pointer, read data comes from MSB
			for (int j=0; j<getDaisyChainNum(); j++) {
				// Serial.println("Got Data: ");
				readUnion[j].bval[SPI_VAL_MAXSIZE-1-i] = readBytes[j];
				// Serial.println(readBytes[j], HEX);
				// Serial.println(readUnion[j].bval[SPI_VAL_MAXSIZE-1-i], HEX);
			}
		}
	}

	// copy data to array
	// Serial.println("final data: ");
	for (int i=0; i<getDaisyChainNum(); i++) {
		_readData[i] = readUnion[i].ival;
		// for (size_t j = 0; j < 4; j++) {
		// 	Serial.print(readUnion[i].bval[j], HEX); Serial.print(" ");
		// }
		// Serial.print("int: "); Serial.println(readUnion[i].ival, HEX);
		// Serial.print("int: "); Serial.println(_readData[i], HEX);
	}
}

void	L6470Stepper::clearMotorActions()
{
	for (int i=0; i<getDaisyChainNum(); i++) {
		clearMotorAction(i);
	}
}

void	L6470Stepper::clearMotorAction(int id)
{
	_motor[id].setAction(NOP);
	_motor[id].setRegister(REG_NOP);
}

void L6470Stepper::resetDriver()
{
	for (int i=0; i<getDaisyChainNum(); i++)
	{
		_motor[i].setAction(NOP);
	}
	transferAction();

	for (int i=0; i<getDaisyChainNum(); i++)
	{
		_motor[i].setAction(RESETDEVICE);
	}
	transferAction();
}

void L6470Stepper::initRegister(registerStruct* regParamList, int size, int id)
{
	for (int i=0; i<size; i++) {
		for (int j=0; j<getDaisyChainNum(); j++) {
			if (j == id)	_motor[j].setRegister(regParamList[i].reg, regParamList[i].val);
			else			_motor[j].setRegister(REG_NOP, 0);
		}
		transferRegister();
	}
}

// optional
void	L6470Stepper::setBusyEnable(int pinBUSY)
{
	_pinBUSY = pinBUSY;
	useBusy = true;
}

void	L6470Stepper::setStepEnable(int pinSTEP)
{
	_pinSTEP = pinSTEP;
	useStep = true;
}

boolean	L6470Stepper::isBusy()
{
	// todo: faster one
	// todo: status register has the mirror
	return !digitalRead(_pinBUSY);	// BUSY: LOW, FREE: HIGH
}

L6480Stepper::L6480Stepper(int pinCS, int nDaisyChain, int* motorSteps)
:L6470Stepper(pinCS, nDaisyChain, motorSteps)
{
}

L6480Stepper::~L6480Stepper()
{

}

void L6480Stepper::setup(registerStruct* regParamList, int size, int id)
{
	// set motor driver as L6480
	if (regParamList == NULL)	regParamList = (registerStruct*)L6480DefaultRegParamList;
	_writableRegNum = WRITABLE_REG_NUM_L6480;
	for (int i=0; i<getDaisyChainNum(); i++) {
		_motor[i].setup(L6480_REGISTER);
	}

	// initialize register
	if (id < 0) {
		for (int i=0; i<getDaisyChainNum(); i++) {
			initRegister(regParamList, _writableRegNum, i);
		}
	} else {
		if (size == 0)	size = _writableRegNum;
		initRegister(regParamList, size, id);
	}
}
<|endoftext|>


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

//ROS libraries
#include <tf/transform_datatypes.h>

//ROS messages
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>

//Package include
#include <usbSerial.h>

using namespace std;

//aBridge functions
void cmdHandler(const geometry_msgs::Twist::ConstPtr& message);
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void serialActivityTimer(const ros::TimerEvent& e);
void publishRosTopics();
void parseData(string data);

//Globals
geometry_msgs::QuaternionStamped fingerAngle;
geometry_msgs::QuaternionStamped wristAngle;
sensor_msgs::Imu imu;
nav_msgs::Odometry odom;
sensor_msgs::Range sonarLeft;
sensor_msgs::Range sonarCenter;
sensor_msgs::Range sonarRight;
USBSerial usb;
const int baud = 115200;
char dataCmd[] = "d\n";
char moveCmd[16];
char host[128];
char delimiter = ',';
vector<string> dataSet;
float linearSpeed = 0.;
float turnSpeed = 0.;
const float deltaTime = 0.1;

//Publishers
ros::Publisher fingerAnglePublish;
ros::Publisher wristAnglePublish;
ros::Publisher imuPublish;
ros::Publisher odomPublish;
ros::Publisher sonarLeftPublish;
ros::Publisher sonarCenterPublish;
ros::Publisher sonarRightPublish;

//Subscribers
ros::Subscriber velocitySubscriber;
ros::Subscriber fingerAngleSubscriber;
ros::Subscriber wristAngleSubscriber;

//Timers
ros::Timer publishTimer;

int main(int argc, char **argv) {

    gethostname(host, sizeof (host));
    string hostname(host);
    string publishedName;
    ros::init(argc, argv, (hostname + "_ABRIDGE"));

    ros::NodeHandle param("~");
    string devicePath;
    param.param("device", devicePath, string("/dev/ttyUSB0"));
    usb.openUSBPort(devicePath, baud);

    sleep(5);

    ros::NodeHandle aNH;

    if (argc >= 2) {
        publishedName = argv[1];
        cout << "Welcome to the world of tomorrow " << publishedName << "!  ABridge module started." << endl;
    } else {
        publishedName = hostname;
        cout << "No Name Selected. Default is: " << publishedName << endl;
    }

    fingerAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/fingerAngle/prev_cmd"), 10);
    wristAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/wristAngle/prev_cmd"), 10);
    imuPublish = aNH.advertise<sensor_msgs::Imu>((publishedName + "/imu"), 10);
    odomPublish = aNH.advertise<nav_msgs::Odometry>((publishedName + "/odom"), 10);
    sonarLeftPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarLeft"), 10);
    sonarCenterPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarCenter"), 10);
    sonarRightPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarRight"), 10);

    velocitySubscriber = aNH.subscribe((publishedName + "/velocity"), 10, cmdHandler);
    fingerAngleSubscriber = aNH.subscribe((publishedName + "/fingerAngle/cmd"), 1, fingerAngleHandler);
    wristAngleSubscriber = aNH.subscribe((publishedName + "/wristAngle/cmd"), 1, wristAngleHandler);

    publishTimer = aNH.createTimer(ros::Duration(deltaTime), serialActivityTimer);

    imu.header.frame_id = publishedName+"/base_link";

    odom.header.frame_id = publishedName+"/odom";
    odom.child_frame_id = publishedName+"/base_link";

    ros::spin();

    return EXIT_SUCCESS;
}

void cmdHandler(const geometry_msgs::Twist::ConstPtr& message) {
    // remove artificial factor that was multiplied for simulation. this scales it back down to -1.0 to +1.0
  linearSpeed = (message->linear.x); // / 1.5;
  turnSpeed = (message->angular.z); // / 8;

    if (linearSpeed != 0.) {
        sprintf(moveCmd, "m,%d\n", (int) (linearSpeed * 255));
        usb.sendData(moveCmd);
    } else if (turnSpeed != 0.) {
        sprintf(moveCmd, "t,%d\n", (int) (turnSpeed * 255));
        usb.sendData(moveCmd);
    } else {
        sprintf(moveCmd, "s\n");
        usb.sendData(moveCmd);
    }

    memset(&moveCmd, '\0', sizeof (moveCmd));
}

// The finger and wrist handlers receive gripper angle commands in floating point
// radians, write them to a string and send that to the arduino
// for processing.
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
  char cmd[16]={'\0'};

  // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'f' indicates this is a finger command to the arduino
    sprintf(cmd, "f,0\n");
  } else {
    sprintf(cmd, "f,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
    char cmd[16]={'\0'};

    // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'w' indicates this is a wrist command to the arduino
    sprintf(cmd, "w,0\n");
  } else {
    sprintf(cmd, "w,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void serialActivityTimer(const ros::TimerEvent& e) {
    usb.sendData(dataCmd);
    parseData(usb.readData());
    publishRosTopics();
}

void publishRosTopics() {
    fingerAnglePublish.publish(fingerAngle);
    wristAnglePublish.publish(wristAngle);
    imuPublish.publish(imu);
    odomPublish.publish(odom);
    sonarLeftPublish.publish(sonarLeft);
    sonarCenterPublish.publish(sonarCenter);
    sonarRightPublish.publish(sonarRight);
}

void parseData(string str) {
    istringstream oss(str);
    string word;
    while (getline(oss, word, delimiter)) {
        dataSet.push_back(word);
    }
    if (dataSet.size() == 18) {
        imu.header.stamp = ros::Time::now();
        imu.linear_acceleration.x = atof(dataSet.at(0).c_str());
        imu.linear_acceleration.y = atof(dataSet.at(1).c_str());
        imu.linear_acceleration.z = atof(dataSet.at(2).c_str());
        imu.angular_velocity.x = atof(dataSet.at(3).c_str());
        imu.angular_velocity.y = atof(dataSet.at(4).c_str());
        imu.angular_velocity.z = atof(dataSet.at(5).c_str());
        imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(6).c_str()), atof(dataSet.at(7).c_str()), atof(dataSet.at(8).c_str()));

        odom.header.stamp = ros::Time::now();
        odom.pose.pose.position.x += atof(dataSet.at(9).c_str()) / 100.0;
        odom.pose.pose.position.y += atof(dataSet.at(10).c_str()) / 100.0;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(atof(dataSet.at(11).c_str()));
        odom.twist.twist.linear.x = atof(dataSet.at(12).c_str()) / 100.0;
        odom.twist.twist.linear.y = atof(dataSet.at(13).c_str()) / 100.0;
        odom.twist.twist.angular.z = atof(dataSet.at(14).c_str());

        sonarLeft.range = atof(dataSet.at(15).c_str()) / 100.0;
        sonarCenter.range = atof(dataSet.at(16).c_str()) / 100.0;
        sonarRight.range = atof(dataSet.at(17).c_str()) / 100.0;
    }

    dataSet.clear();
}
<commit_msg>Enhance abridge's parsing function to handle multiple delimited newlines<commit_after>#include <ros/ros.h>

//ROS libraries
#include <tf/transform_datatypes.h>

//ROS messages
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>

//Package include
#include <usbSerial.h>

using namespace std;

//aBridge functions
void cmdHandler(const geometry_msgs::Twist::ConstPtr& message);
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void serialActivityTimer(const ros::TimerEvent& e);
void publishRosTopics();
void parseData(string data);

//Globals
geometry_msgs::QuaternionStamped fingerAngle;
geometry_msgs::QuaternionStamped wristAngle;
sensor_msgs::Imu imu;
nav_msgs::Odometry odom;
sensor_msgs::Range sonarLeft;
sensor_msgs::Range sonarCenter;
sensor_msgs::Range sonarRight;
USBSerial usb;
const int baud = 115200;
char dataCmd[] = "d\n";
char moveCmd[16];
char host[128];
char delimiter = ',';
vector<string> dataSet;
float linearSpeed = 0.;
float turnSpeed = 0.;
const float deltaTime = 0.1;

//Publishers
ros::Publisher fingerAnglePublish;
ros::Publisher wristAnglePublish;
ros::Publisher imuPublish;
ros::Publisher odomPublish;
ros::Publisher sonarLeftPublish;
ros::Publisher sonarCenterPublish;
ros::Publisher sonarRightPublish;

//Subscribers
ros::Subscriber velocitySubscriber;
ros::Subscriber fingerAngleSubscriber;
ros::Subscriber wristAngleSubscriber;

//Timers
ros::Timer publishTimer;

int main(int argc, char **argv) {

    gethostname(host, sizeof (host));
    string hostname(host);
    string publishedName;
    ros::init(argc, argv, (hostname + "_ABRIDGE"));

    ros::NodeHandle param("~");
    string devicePath;
    param.param("device", devicePath, string("/dev/ttyUSB0"));
    usb.openUSBPort(devicePath, baud);

    sleep(5);

    ros::NodeHandle aNH;

    if (argc >= 2) {
        publishedName = argv[1];
        cout << "Welcome to the world of tomorrow " << publishedName << "!  ABridge module started." << endl;
    } else {
        publishedName = hostname;
        cout << "No Name Selected. Default is: " << publishedName << endl;
    }

    fingerAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/fingerAngle/prev_cmd"), 10);
    wristAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/wristAngle/prev_cmd"), 10);
    imuPublish = aNH.advertise<sensor_msgs::Imu>((publishedName + "/imu"), 10);
    odomPublish = aNH.advertise<nav_msgs::Odometry>((publishedName + "/odom"), 10);
    sonarLeftPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarLeft"), 10);
    sonarCenterPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarCenter"), 10);
    sonarRightPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarRight"), 10);

    velocitySubscriber = aNH.subscribe((publishedName + "/velocity"), 10, cmdHandler);
    fingerAngleSubscriber = aNH.subscribe((publishedName + "/fingerAngle/cmd"), 1, fingerAngleHandler);
    wristAngleSubscriber = aNH.subscribe((publishedName + "/wristAngle/cmd"), 1, wristAngleHandler);

    publishTimer = aNH.createTimer(ros::Duration(deltaTime), serialActivityTimer);

    imu.header.frame_id = publishedName+"/base_link";

    odom.header.frame_id = publishedName+"/odom";
    odom.child_frame_id = publishedName+"/base_link";

    ros::spin();

    return EXIT_SUCCESS;
}

void cmdHandler(const geometry_msgs::Twist::ConstPtr& message) {
    // remove artificial factor that was multiplied for simulation. this scales it back down to -1.0 to +1.0
  linearSpeed = (message->linear.x); // / 1.5;
  turnSpeed = (message->angular.z); // / 8;

    if (linearSpeed != 0.) {
        sprintf(moveCmd, "m,%d\n", (int) (linearSpeed * 255));
        usb.sendData(moveCmd);
    } else if (turnSpeed != 0.) {
        sprintf(moveCmd, "t,%d\n", (int) (turnSpeed * 255));
        usb.sendData(moveCmd);
    } else {
        sprintf(moveCmd, "s\n");
        usb.sendData(moveCmd);
    }

    memset(&moveCmd, '\0', sizeof (moveCmd));
}

// The finger and wrist handlers receive gripper angle commands in floating point
// radians, write them to a string and send that to the arduino
// for processing.
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
  char cmd[16]={'\0'};

  // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'f' indicates this is a finger command to the arduino
    sprintf(cmd, "f,0\n");
  } else {
    sprintf(cmd, "f,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
    char cmd[16]={'\0'};

    // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'w' indicates this is a wrist command to the arduino
    sprintf(cmd, "w,0\n");
  } else {
    sprintf(cmd, "w,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void serialActivityTimer(const ros::TimerEvent& e) {
    usb.sendData(dataCmd);
    parseData(usb.readData());
    publishRosTopics();
}

void publishRosTopics() {
    fingerAnglePublish.publish(fingerAngle);
    wristAnglePublish.publish(wristAngle);
    imuPublish.publish(imu);
    odomPublish.publish(odom);
    sonarLeftPublish.publish(sonarLeft);
    sonarCenterPublish.publish(sonarCenter);
    sonarRightPublish.publish(sonarRight);
}

void parseData(string str) {
    istringstream oss(str);
    string sentence;

    while (getline(oss, sentence, '\n')) {
		istringstream wss(sentence);
		string word;

		vector<string> dataSet;
		while (getline(wss, word, ',')) {
			dataSet.push_back(word);
		}

		if (dataSet.size() >= 3 && dataSet.at(1) == "1") {

			if (dataSet.at(0) == "GRF") {
				fingerAngle.header.stamp = ros::Time::now();
				fingerAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
			}
			else if (dataSet.at(0) == "GRW") {
				wristAngle.header.stamp = ros::Time::now();
				wristAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
			}
			else if (dataSet.at(0) == "IMU") {
				imu.header.stamp = ros::Time::now();
				imu.linear_acceleration.x = atof(dataSet.at(2).c_str());
				imu.linear_acceleration.y = atof(dataSet.at(3).c_str());
				imu.linear_acceleration.z = atof(dataSet.at(4).c_str());
				imu.angular_velocity.x = atof(dataSet.at(5).c_str());
				imu.angular_velocity.y = atof(dataSet.at(6).c_str());
				imu.angular_velocity.z = atof(dataSet.at(7).c_str());
				imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(8).c_str()), atof(dataSet.at(9).c_str()), atof(dataSet.at(10).c_str()));
			}
			else if (dataSet.at(0) == "ODOM") {
				odom.header.stamp = ros::Time::now();
				odom.pose.pose.position.x += atof(dataSet.at(2).c_str()) / 100.0;
				odom.pose.pose.position.y += atof(dataSet.at(3).c_str()) / 100.0;
				odom.pose.pose.position.z = 0.0;
				odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(atof(dataSet.at(4).c_str()));
				odom.twist.twist.linear.x = atof(dataSet.at(5).c_str()) / 100.0;
				odom.twist.twist.linear.y = atof(dataSet.at(6).c_str()) / 100.0;
				odom.twist.twist.angular.z = atof(dataSet.at(7).c_str());
			}
			else if (dataSet.at(0) == "USL") {
				sonarLeft.header.stamp = ros::Time::now();
				sonarLeft.range = atof(dataSet.at(2).c_str()) / 100.0;
			}
			else if (dataSet.at(0) == "USC") {
				sonarCenter.header.stamp = ros::Time::now();
				sonarCenter.range = atof(dataSet.at(2).c_str()) / 100.0;
			}
			else if (dataSet.at(0) == "USR") {
				sonarRight.header.stamp = ros::Time::now();
				sonarRight.range = atof(dataSet.at(2).c_str()) / 100.0;
			}

		}
	}
}
<|endoftext|>


<commit_before>class Solution {
public:
    vector<int> intersect(vector<int>& nums1, vector<int>& nums2) {
        std::unordered_map<int, int> m1, m2;
        for (const auto &num : nums1) {
            auto iter = m1.find(num);
            if (iter == m1.end()) {
                m1.emplace(num, 1);
            } else {
                ++iter->second;
            }
        }
        for (const auto &num : nums2) {
            auto iter = m2.find(num);
            if (iter == m2.end()) {
                m2.emplace(num, 1);
            } else {
                ++iter->second;
            }
        }
        std::vector<int> ret;
        for (auto iter = m1.begin(); iter != m1.end(); ++iter) {
            auto iter2 = m2.find(iter->first);
            if (iter2 == m2.end())
                continue;
            int temp = 0;
            temp = std::min(iter->second, iter2->second);
            while (temp > 0) {
                ret.emplace_back(iter->first);
                --temp;
            }
        }
        return ret;
    }
};
<commit_msg>Intersection of Two Arrays II<commit_after>class Solution {
public:
    vector<int> intersect(vector<int>& nums1, vector<int>& nums2) {
        std::unordered_map<int, int> m1, m2;
        for (const auto &num : nums1) {
            auto iter = m1.find(num);
            if (iter == m1.end()) {
                m1.emplace(num, 1);
            } else {
                ++iter->second;
            }
        }
        for (const auto &num : nums2) {
            auto iter = m2.find(num);
            if (iter == m2.end()) {
                m2.emplace(num, 1);
            } else {
                ++iter->second;
            }
        }
        std::vector<int> ret;
        for (auto iter = m1.begin(); iter != m1.end(); ++iter) {
            auto iter2 = m2.find(iter->first);
            if (iter2 == m2.end())
                continue;
            int temp = 0;
            temp = std::min(iter->second, iter2->second);
            while (temp > 0) {
                ret.emplace_back(iter->first);
                --temp;
            }
        }
        return ret;
    }
};

class Solution {
public:
    vector<int> intersect(vector<int>& nums1, vector<int>& nums2) {
        std::unordered_map<int, int> m1, m2;
        for (const auto& num : nums1) {
            m1[num] += 1;
        }
        for (const auto& num : nums2) {
            m2[num] += 1;
        }
        std::vector<int> ret;
        for (const auto& [key, value] : m1) {
            if (m2[key] > 0) {
                int count = std::min(value, m2[key]);
                while (count > 0) {
                    ret.emplace_back(std::move(key));
                    --count;
                }
            }
        }
        return ret;
    }
};
<|endoftext|>


<commit_before>/*
 Copyright (c) 2019 Anon authors, see AUTHORS file.

 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 <epc.h>
#include <netdb.h>
#include "dns_lookup.h"

endpoint_cluster::endpoint_cluster(const char *host, int port,
                                   bool do_tls,
                                   const tls_context *tls_ctx,
                                   int max_conn_per_ep,
                                   int lookup_frequency_in_seconds)
    : host_(host),
      port_(port),
      do_tls_(do_tls),
      tls_ctx_(tls_ctx),
      max_conn_per_ep_(max_conn_per_ep),
      lookup_frequency_in_seconds_(lookup_frequency_in_seconds),
      last_lookup_time_((struct timespec){}),
      round_robin_index_(0),
      looking_up_endpoints_(false),
      max_io_block_time_(k_default_io_block_time),
      retries_enabled_(true)
{
}

void endpoint_cluster::update_endpoints()
{
  auto addrs = dns_lookup::get_addrinfo(host_.c_str(), port_);

  fiber_lock l(mtx_);
  if (addrs.first != 0 || addrs.second.size() == 0)
  {
    // if there are no current endpoints that are already known
    // for this host name, then this error object will be thrown
    // in the fiber has initiated this call to update_endpoints.
    // In that case, the fact that it is an instance of fiber_io_error
    // will cause the exponential backoff logic to kick in and
    // re-attempt the operation - optimistically hoping that whatever
    // is causing the error will resolve itself
    lookup_err_ = std::unique_ptr<fiber_io_error>(new fiber_io_error(Log::fmt(
        [&](std::ostream &msg) { msg << "dns lookup failed for: " << host_ << ", error: " << (addrs.first < 0 ? gai_strerror(addrs.first) : error_string(addrs.first)); })));
  }
  else
  {
    // build a map which is the union of all currently
    // known endpoints plus the ones that were just
    // returned in the call to get_adddrinfo.
    auto now = cur_time();
    std::map<sockaddr_in6, std::shared_ptr<endpoint>> endpoints;
    for (auto &ep : endpoints_)
      endpoints.insert(std::make_pair(ep->addr_, ep));
    for (auto &addr : addrs.second)
    {
      auto it = endpoints.find(addr);
      if (it == endpoints.end())
        endpoints.insert(std::make_pair(addr, std::make_shared<endpoint>(addr)));
      else
        it->second->last_lookup_time_ = now;
    }

    // we use a simple policy rule that we are willing to
    // keep attempting to use any ip address that was returned
    // by getaddrinfo up to 10 times whatever the
    // dns lookup_frequency_in_seconds_ is.  If any cached
    // open sockets become unusable they will be deleted.
    // If new connection attempts fail to one of these
    // "slighly old" endpoints we will delete that endpoint
    // from our list.
    auto oldest = now - lookup_frequency_in_seconds_ * 10;
    for (auto it = endpoints.begin(); it != endpoints.end();)
    {
      if (it->second->last_lookup_time_ < oldest)
      {
#ifdef ANON_LOG_DNS_LOOKUP
        anon_log("aging out endpoint " << it->second->addr_ << ", because it was " << to_seconds(now - it->second->last_lookup_time_) << " seconds old");
#endif
        it = endpoints.erase(it);
      }
      else
        ++it;
    }
    endpoints_.resize(endpoints.size());
    int indx = 0;
#ifdef ANON_LOG_DNS_LOOKUP
    anon_log("host: " << host_);
#endif
    for (auto &p : endpoints)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      auto age = to_seconds(now - p.second->last_lookup_time_);
      if (age < 0.0)
        age = 0.0;
      anon_log(" using " << p.second->addr_ << ", lookup age: " << age << " seconds");
#endif
      endpoints_[indx++] = p.second;
    }
  }

  last_lookup_time_ = cur_time();
  looking_up_endpoints_ = false;
  cond_.notify_all();
}

// This is called either by erase_if_empty ep has only one open
// socket (erase_if_empty is called if there is a problem with
// that socket) - or if we get a connection error trying to
// open a new socket for this endpoint.  We delete the endpoint
// from our list to force dns to run again next time we try
// to get a functional pipe
void endpoint_cluster::erase(const std::shared_ptr<endpoint> &ep)
{
  fiber_lock l(mtx_);
  auto it = endpoints_.begin();
  while (it != endpoints_.end())
  {
    if (*it == ep)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("endpoint_cluster::erase emptying");
#endif
      endpoints_.erase(it);
      return;
    }
    it++;
  }
}

// This is called when there is some error that has occurred
// on a socket related to this endpoint.  If it turns out that
// that socket was the only one in existence for this endpoint
// then remove the matching entry in the endpoints_ vector.
// This causes that endpoint to get deleted (when the other
// shared pointers to it are destructed) and means that the
// endpoint_cluster itself will not attempt to use this endpoint
// again without first going back through dns resolution.
// This cleans up cases where the ip address itself is no
// longer good and dns lookup is correctly no longer reporting
// that ip address.  We have a policy of continuing to attempt
// to use that ip address for some period after dns lookup has
// returned it.
void endpoint_cluster::erase_if_empty(const std::shared_ptr<endpoint> &ep)
{
  {
    fiber_lock l(ep->mtx_);
    if (ep->socks_.size() != 0 || ep->outstanding_requests_ != 1)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("endpoint_cluster::erase_if_empty, not emptying, ep->socks_.size(): " << ep->socks_.size() << ", ep->outstanding_requests_: " << ep->outstanding_requests_);
#endif
      return;
    }
  }

  erase(ep);
}

void endpoint_cluster::delete_cached_endpoints()
{
  fiber_lock l(mtx_);
  endpoints_.resize(0);
}

namespace
{

class cleanup
{
public:
  cleanup(const std::weak_ptr<endpoint_cluster::endpoint> &wep,
          const std::shared_ptr<endpoint_cluster::endpoint::sock> &sock,
          const std::weak_ptr<endpoint_cluster>& wcp)
      : wep(wep),
        sock(sock),
        cache(false),
        exception_thrown(true)
#ifdef ANON_LOG_DNS_LOOKUP
        ,addr(wep.lock()->addr_)
#endif
  {
  }

  ~cleanup()
  {
    auto ep = wep.lock();
    if (ep)
    {
      fiber_lock l(ep->mtx_);
      --ep->outstanding_requests_;
      if (exception_thrown) {
        auto cp = wcp.lock();
        if (cp)
          cp->erase(ep);
      }
      if (cache)
      {
        sock->idle_start_time = cur_time();
        ep->socks_.push(sock);
      }
      ep->error_ |= exception_thrown;
      ep->cond_.notify_all();
    }
    else
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("epc, appears that endpoint for " << addr << " was deleted prior to callback returning");
#endif
    }
  }

  std::weak_ptr<endpoint_cluster::endpoint> wep;
  std::shared_ptr<endpoint_cluster::endpoint::sock> sock;
  std::weak_ptr<endpoint_cluster> wcp;
  bool cache;
  bool exception_thrown;

#ifdef ANON_LOG_DNS_LOOKUP
  const struct sockaddr_in6 addr;
#endif
};

class eraser
{
public:
  eraser(endpoint_cluster *epc,
         const std::shared_ptr<endpoint_cluster::endpoint> &ep)
      : epc(epc),
        ep(ep),
        success(false)
  {
  }

  ~eraser()
  {
    if (!success)
      epc->erase(ep);
  }

  endpoint_cluster *epc;
  std::shared_ptr<endpoint_cluster::endpoint> ep;
  bool success;
};

} // namespace

void endpoint_cluster::do_with_connected_pipe(const std::function<bool(const pipe_t *pipe)> &f)
{
  // if there are currently no available endpoints, or if it has been too long since we have
  // last looked up endpoints, then restart the lookup process.  If there are no endpoints
  // then wait until the lookup is complete, otherwise continue to use the endpoints we
  // already know about and let the lookup complete asynchronously.  If there was an error
  // attempting the lookup throw that error if there are no current endpoints, otherwise
  // ignore the error
  std::shared_ptr<endpoint> ep;
  std::weak_ptr<endpoint> wep;
  {
    fiber_lock l(mtx_);
    if (endpoints_.size() == 0 || to_seconds(cur_time() - last_lookup_time_) > lookup_frequency_in_seconds_)
    {
      if (!looking_up_endpoints_)
      {
        looking_up_endpoints_ = true;
        lookup_err_.reset();
        std::weak_ptr<endpoint_cluster> wp = shared_from_this();
        auto stack_size = 16 * 1024;
        fiber::run_in_fiber(
            [wp] {
              auto ths = wp.lock();
              if (ths)
              {
                ths->update_endpoints();
              }
            },
            stack_size, "epc, update_endpoints");
      }
      while (endpoints_.size() == 0)
      {
        cond_.wait(l);
        if (lookup_err_)
          throw *lookup_err_;
      }
    }
    ep = endpoints_[round_robin_index_++ % endpoints_.size()];
    wep = ep;
  }

  std::shared_ptr<endpoint::sock> sock;
  {
    fiber_lock l(ep->mtx_);

    while (ep->outstanding_requests_ >= max_conn_per_ep_)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("waiting to use endpoint because it has " << ep->outstanding_requests_ << " current connections, maximum allowed: " << max_conn_per_ep_);
#endif
      ep->cond_.wait(l);
    }
    if (ep->error_)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("endpoint for " << ep->addr_ << " had previous error (and has been removed from endpoint list) - trying again");
#endif
      l.unlock();
      ep.reset();
      do_with_connected_pipe(f);
      return;
    }
    ++ep->outstanding_requests_;

    while (!sock && ep->socks_.size() != 0)
    {
      auto s = ep->socks_.front();
      ep->socks_.pop();
      if (cur_time() < s->idle_start_time + k_max_idle_time)
        sock = s;
#ifdef ANON_LOG_DNS_LOOKUP
      else
        anon_log("releasing socket (fd=" << s->pipe_->get_fd() << ") because it has been idle for " << cur_time() - s->idle_start_time << " seconds");
#endif
    }

    if (sock)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("epc reused connection (fd=" << sock->pipe_->get_fd() << ", idle_time=" << cur_time() - sock->idle_start_time << ") to " << ep->addr_);
#endif
    }
    else
    {
      l.unlock();
      eraser era(this, ep);
      auto conn = tcp_client::connect((struct sockaddr *)&ep->addr_, ep->addr_.sin6_family == AF_INET6 ? sizeof(struct sockaddr_in6) : sizeof(struct sockaddr_in));
      era.success = true;
      if (conn.first != 0)
      {
        erase(ep);
        anon_throw(fiber_io_error, "tcp connect failed for " << ep->addr_ << ", error: " << error_string(conn.first));
      }
      conn.second->limit_io_block_time(max_io_block_time_);

      std::unique_ptr<pipe_t> pipe;
      if (do_tls_)
        pipe = std::unique_ptr<pipe_t>(new tls_pipe(std::move(conn.second),
                                                    true, // client (not server)
                                                    true, // verify_peer
                                                    true, // doSNI
                                                    host_.c_str(),
                                                    *tls_ctx_));
      else
        pipe = std::unique_ptr<pipe_t>(conn.second.release());
      sock = std::shared_ptr<endpoint::sock>(new endpoint::sock(std::move(pipe)));
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("epc established new connection (fd=" << sock->pipe_->get_fd() << ") to " << ep->addr_);
#endif
    }
  }

  // we let go of the endpoint itself and only hold
  // the weak pointer to it across the call.  This
  // lets it timeout and get deleted more smoothly.
  cleanup cu(wep, sock, shared_from_this());
  ep.reset();
  cu.cache = f(sock->pipe_.get());
  cu.exception_thrown = false;
}
<commit_msg>logging tweak<commit_after>/*
 Copyright (c) 2019 Anon authors, see AUTHORS file.

 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 <epc.h>
#include <netdb.h>
#include "dns_lookup.h"

endpoint_cluster::endpoint_cluster(const char *host, int port,
                                   bool do_tls,
                                   const tls_context *tls_ctx,
                                   int max_conn_per_ep,
                                   int lookup_frequency_in_seconds)
    : host_(host),
      port_(port),
      do_tls_(do_tls),
      tls_ctx_(tls_ctx),
      max_conn_per_ep_(max_conn_per_ep),
      lookup_frequency_in_seconds_(lookup_frequency_in_seconds),
      last_lookup_time_((struct timespec){}),
      round_robin_index_(0),
      looking_up_endpoints_(false),
      max_io_block_time_(k_default_io_block_time),
      retries_enabled_(true)
{
}

void endpoint_cluster::update_endpoints()
{
  auto addrs = dns_lookup::get_addrinfo(host_.c_str(), port_);

  fiber_lock l(mtx_);
  if (addrs.first != 0 || addrs.second.size() == 0)
  {
    // if there are no current endpoints that are already known
    // for this host name, then this error object will be thrown
    // in the fiber has initiated this call to update_endpoints.
    // In that case, the fact that it is an instance of fiber_io_error
    // will cause the exponential backoff logic to kick in and
    // re-attempt the operation - optimistically hoping that whatever
    // is causing the error will resolve itself
    lookup_err_ = std::unique_ptr<fiber_io_error>(new fiber_io_error(Log::fmt(
        [&](std::ostream &msg) { msg << "dns lookup failed for: " << host_ << ", error: " << (addrs.first < 0 ? gai_strerror(addrs.first) : error_string(addrs.first)); })));
  }
  else
  {
    // build a map which is the union of all currently
    // known endpoints plus the ones that were just
    // returned in the call to get_adddrinfo.
    auto now = cur_time();
    std::map<sockaddr_in6, std::shared_ptr<endpoint>> endpoints;
    for (auto &ep : endpoints_)
      endpoints.insert(std::make_pair(ep->addr_, ep));
    for (auto &addr : addrs.second)
    {
      auto it = endpoints.find(addr);
      if (it == endpoints.end())
        endpoints.insert(std::make_pair(addr, std::make_shared<endpoint>(addr)));
      else
        it->second->last_lookup_time_ = now;
    }

    // we use a simple policy rule that we are willing to
    // keep attempting to use any ip address that was returned
    // by getaddrinfo up to 10 times whatever the
    // dns lookup_frequency_in_seconds_ is.  If any cached
    // open sockets become unusable they will be deleted.
    // If new connection attempts fail to one of these
    // "slighly old" endpoints we will delete that endpoint
    // from our list.
    auto oldest = now - lookup_frequency_in_seconds_ * 10;
    for (auto it = endpoints.begin(); it != endpoints.end();)
    {
      if (it->second->last_lookup_time_ < oldest)
      {
#ifdef ANON_LOG_DNS_LOOKUP
        anon_log("aging out endpoint " << it->second->addr_ << ", because it was " << to_seconds(now - it->second->last_lookup_time_) << " seconds old");
#endif
        it = endpoints.erase(it);
      }
      else
        ++it;
    }
    endpoints_.resize(endpoints.size());
    int indx = 0;
#ifdef ANON_LOG_DNS_LOOKUP
    anon_log("host: " << host_);
#endif
    for (auto &p : endpoints)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      auto age = to_seconds(now - p.second->last_lookup_time_);
      if (age < 0.0)
        age = 0.0;
      anon_log(" using " << p.second->addr_ << ", lookup age: " << age << " seconds");
#endif
      endpoints_[indx++] = p.second;
    }
  }

  last_lookup_time_ = cur_time();
  looking_up_endpoints_ = false;
  cond_.notify_all();
}

// This is called either by erase_if_empty ep has only one open
// socket (erase_if_empty is called if there is a problem with
// that socket) - or if we get a connection error trying to
// open a new socket for this endpoint.  We delete the endpoint
// from our list to force dns to run again next time we try
// to get a functional pipe
void endpoint_cluster::erase(const std::shared_ptr<endpoint> &ep)
{
  fiber_lock l(mtx_);
  auto it = endpoints_.begin();
  while (it != endpoints_.end())
  {
    if (*it == ep)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("endpoint_cluster::erase emptying");
#endif
      endpoints_.erase(it);
      return;
    }
    it++;
  }
}

// This is called when there is some error that has occurred
// on a socket related to this endpoint.  If it turns out that
// that socket was the only one in existence for this endpoint
// then remove the matching entry in the endpoints_ vector.
// This causes that endpoint to get deleted (when the other
// shared pointers to it are destructed) and means that the
// endpoint_cluster itself will not attempt to use this endpoint
// again without first going back through dns resolution.
// This cleans up cases where the ip address itself is no
// longer good and dns lookup is correctly no longer reporting
// that ip address.  We have a policy of continuing to attempt
// to use that ip address for some period after dns lookup has
// returned it.
void endpoint_cluster::erase_if_empty(const std::shared_ptr<endpoint> &ep)
{
  {
    fiber_lock l(ep->mtx_);
    if (ep->socks_.size() != 0 || ep->outstanding_requests_ != 1)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("endpoint_cluster::erase_if_empty, not emptying, ep->socks_.size(): " << ep->socks_.size() << ", ep->outstanding_requests_: " << ep->outstanding_requests_);
#endif
      return;
    }
  }

  erase(ep);
}

void endpoint_cluster::delete_cached_endpoints()
{
  fiber_lock l(mtx_);
  endpoints_.resize(0);
}

namespace
{

class cleanup
{
public:
  cleanup(const std::weak_ptr<endpoint_cluster::endpoint> &wep,
          const std::shared_ptr<endpoint_cluster::endpoint::sock> &sock,
          const std::weak_ptr<endpoint_cluster>& wcp)
      : wep(wep),
        sock(sock),
        cache(false),
        exception_thrown(true)
#ifdef ANON_LOG_DNS_LOOKUP
        ,addr(wep.lock()->addr_)
#endif
  {
  }

  ~cleanup()
  {
    auto ep = wep.lock();
    if (ep)
    {
      fiber_lock l(ep->mtx_);
      --ep->outstanding_requests_;
      if (exception_thrown) {
        auto cp = wcp.lock();
        if (cp)
          cp->erase(ep);
      }
      if (cache)
      {
        sock->idle_start_time = cur_time();
        ep->socks_.push(sock);
      }
      ep->error_ |= exception_thrown;
      ep->cond_.notify_all();
    }
    else
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("epc, appears that endpoint for " << addr << " was deleted prior to callback returning");
#endif
    }
  }

  std::weak_ptr<endpoint_cluster::endpoint> wep;
  std::shared_ptr<endpoint_cluster::endpoint::sock> sock;
  std::weak_ptr<endpoint_cluster> wcp;
  bool cache;
  bool exception_thrown;

#ifdef ANON_LOG_DNS_LOOKUP
  const struct sockaddr_in6 addr;
#endif
};

class eraser
{
public:
  eraser(endpoint_cluster *epc,
         const std::shared_ptr<endpoint_cluster::endpoint> &ep)
      : epc(epc),
        ep(ep),
        success(false)
  {
  }

  ~eraser()
  {
    if (!success)
      epc->erase(ep);
  }

  endpoint_cluster *epc;
  std::shared_ptr<endpoint_cluster::endpoint> ep;
  bool success;
};

} // namespace

void endpoint_cluster::do_with_connected_pipe(const std::function<bool(const pipe_t *pipe)> &f)
{
  // if there are currently no available endpoints, or if it has been too long since we have
  // last looked up endpoints, then restart the lookup process.  If there are no endpoints
  // then wait until the lookup is complete, otherwise continue to use the endpoints we
  // already know about and let the lookup complete asynchronously.  If there was an error
  // attempting the lookup throw that error if there are no current endpoints, otherwise
  // ignore the error
  std::shared_ptr<endpoint> ep;
  std::weak_ptr<endpoint> wep;
  {
    fiber_lock l(mtx_);
    if (endpoints_.size() == 0 || to_seconds(cur_time() - last_lookup_time_) > lookup_frequency_in_seconds_)
    {
      if (!looking_up_endpoints_)
      {
        looking_up_endpoints_ = true;
        lookup_err_.reset();
        std::weak_ptr<endpoint_cluster> wp = shared_from_this();
        auto stack_size = 16 * 1024;
        fiber::run_in_fiber(
            [wp] {
              auto ths = wp.lock();
              if (ths)
              {
                ths->update_endpoints();
              }
            },
            stack_size, "epc, update_endpoints");
      }
      while (endpoints_.size() == 0)
      {
        cond_.wait(l);
        if (lookup_err_)
          throw *lookup_err_;
      }
    }
    ep = endpoints_[round_robin_index_++ % endpoints_.size()];
    wep = ep;
  }

  std::shared_ptr<endpoint::sock> sock;
  {
    fiber_lock l(ep->mtx_);

    while (ep->outstanding_requests_ >= max_conn_per_ep_)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("waiting to use endpoint because it has " << ep->outstanding_requests_ << " current connections, maximum allowed: " << max_conn_per_ep_);
#endif
      ep->cond_.wait(l);
    }
    if (ep->error_)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("endpoint for " << ep->addr_ << " had previous error (and has been removed from endpoint list) - trying again");
#endif
      l.unlock();
      ep.reset();
      do_with_connected_pipe(f);
      return;
    }
    ++ep->outstanding_requests_;

    while (!sock && ep->socks_.size() != 0)
    {
      auto s = ep->socks_.front();
      ep->socks_.pop();
      if (cur_time() < s->idle_start_time + k_max_idle_time)
        sock = s;
#ifdef ANON_LOG_DNS_LOOKUP
      else
        anon_log("releasing socket (fd=" << s->pipe_->get_fd() << ", from " << ep->addr_ << ") because it has been idle for " << cur_time() - s->idle_start_time << " seconds");
#endif
    }

    if (sock)
    {
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("epc reused connection (fd=" << sock->pipe_->get_fd() << ", idle_time=" << cur_time() - sock->idle_start_time << ") to " << ep->addr_);
#endif
    }
    else
    {
      l.unlock();
      eraser era(this, ep);
      auto conn = tcp_client::connect((struct sockaddr *)&ep->addr_, ep->addr_.sin6_family == AF_INET6 ? sizeof(struct sockaddr_in6) : sizeof(struct sockaddr_in));
      era.success = true;
      if (conn.first != 0)
      {
        erase(ep);
        anon_throw(fiber_io_error, "tcp connect failed for " << ep->addr_ << ", error: " << error_string(conn.first));
      }
      conn.second->limit_io_block_time(max_io_block_time_);

      std::unique_ptr<pipe_t> pipe;
      if (do_tls_)
        pipe = std::unique_ptr<pipe_t>(new tls_pipe(std::move(conn.second),
                                                    true, // client (not server)
                                                    true, // verify_peer
                                                    true, // doSNI
                                                    host_.c_str(),
                                                    *tls_ctx_));
      else
        pipe = std::unique_ptr<pipe_t>(conn.second.release());
      sock = std::shared_ptr<endpoint::sock>(new endpoint::sock(std::move(pipe)));
#ifdef ANON_LOG_DNS_LOOKUP
      anon_log("epc established new connection (fd=" << sock->pipe_->get_fd() << ") to " << ep->addr_);
#endif
    }
  }

  // we let go of the endpoint itself and only hold
  // the weak pointer to it across the call.  This
  // lets it timeout and get deleted more smoothly.
  cleanup cu(wep, sock, shared_from_this());
  ep.reset();
  cu.cache = f(sock->pipe_.get());
  cu.exception_thrown = false;
}
<|endoftext|>