<commit_before>/*
 * bacteria-core, core for cellular automaton
 * Copyright (C) 2016 Pavel Dolgov
 *
 * See the LICENSE file for terms of use.
 */

#include <boost/foreach.hpp>
#include <boost/test/unit_test.hpp>

#include "Model.hpp"

typedef void (Implementation::Model::*OneArgMethod) (
    const Abstract::Point& coordinates
);

typedef void (Implementation::Model::*TwoArgsMethod) (
    const Abstract::Point& coordinates,
    int change
);

typedef void (Implementation::Model::*MultiArgsMethod) (
    const Abstract::Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
);

template<typename Func>
void checkModelMethodForThrow(
    Implementation::Model* model,
    Func model_method,
    int arg1,
    int arg2
) {
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(arg1, arg2), Exception
    );
}

template<>
void checkModelMethodForThrow<OneArgMethod>(
    Implementation::Model* model,
    OneArgMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates), Exception
    );
}

template<>
void checkModelMethodForThrow<TwoArgsMethod>(
    Implementation::Model* model,
    TwoArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates, 0), Exception
    );
}

template<>
void checkModelMethodForThrow<MultiArgsMethod>(
    Implementation::Model* model,
    MultiArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(
            coordinates,
            DEFAULT_MASS,
            0,
            0,
            0
        ),
        Exception
    );
}

// "dead" test: attempt to do something with dead bacterium
template<typename Func>
static void deadTest(
    Implementation::Model* model,
    Func model_method,
    bool by_coordinates
) {
    if (by_coordinates) {
        Abstract::Point coordinates(0, 0);
        model->killByCoordinates(coordinates);
    } else {
        model->kill(0, 0);
    }
    checkModelMethodForThrow(
        model,
        model_method,
        0,
        0
    );
}

template<typename Func>
static void checkErrorHandling(
    Implementation::Model* model,
    Func model_method,
    bool by_coordinates,
    bool dead_test
) {
    // Range errors: test all combinations of
    // "wrong" (outside of correct range) arguments.
    // This solution works for coordinates and non-coordinates
    // methods of Model. (< 0, = 0, > MAX)
    int wrong_args[] = {-1, 0, MIN_WIDTH};
    BOOST_FOREACH (int arg1, wrong_args) {
        BOOST_FOREACH (int arg2, wrong_args) {
            if ((arg1 != 0) || (arg2 != 0)) {
                // (0, 0) is correct
                checkModelMethodForThrow(
                    model,
                    model_method,
                    arg1,
                    arg2
                );
            }
        }
    }
    if (dead_test) {
        // "dead" error
        // (attempt to do something with dead bacterium)
        deadTest(model, model_method, by_coordinates);
    }
}

#define CREATE_NEW \
    model->createNewByCoordinates( \
        coordinates, \
        DEFAULT_MASS, \
        0, \
        0, \
        0 \
    );

static Abstract::Point createInBaseCoordinates(
    Implementation::Model* model
) {
    Abstract::Point coordinates(0, 0);
    CREATE_NEW
    return coordinates;
}

static void createByCoordinates(
    Implementation::Model* model,
    Abstract::Point coordinates
) {
    CREATE_NEW
}

#undef CREATE_NEW

static Implementation::Model* createBaseModel(
    int bacteria = 0,
    int teams = 1
) {
    Implementation::Model* model =
        Abstract::makeModel<Implementation::Model>(
            MIN_WIDTH,
            MIN_HEIGHT,
            bacteria,
            teams
        );
    return model;
}

BOOST_AUTO_TEST_CASE (bacteria_number_test) {
    Implementation::Model* model = createBaseModel();
    int bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 0);
    createInBaseCoordinates(model);
    bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 1);
    // range errors
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(-1), Exception);
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(1), Exception);
    delete model;
}

BOOST_AUTO_TEST_CASE (get_mass_test) {
    Implementation::Model* model = createBaseModel(1, 1);
    int mass = model->getMass(0, 0);
    BOOST_REQUIRE(mass == DEFAULT_MASS);
    checkErrorHandling(model, &Implementation::Model::getMass);
    delete model;
}

BOOST_AUTO_TEST_CASE (height_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getHeight() == MIN_HEIGHT);
    delete model;
}

BOOST_AUTO_TEST_CASE (width_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getWidth() == MIN_WIDTH);
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->kill(0, 0);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    // error handling checks
    createInBaseCoordinates(model);
    // FIXME test doesn't work correctly without this function call.
    // The solution is to use set instead of vector in model.
    model->clearBeforeMove(0);
    checkErrorHandling(model, &Implementation::Model::kill);
    delete model;
}

BOOST_AUTO_TEST_CASE (create_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::BACTERIUM);
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->killByCoordinates(coordinates);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    delete model;
}
<commit_msg>Remove static function createByCoordinates()<commit_after>/*
 * bacteria-core, core for cellular automaton
 * Copyright (C) 2016 Pavel Dolgov
 *
 * See the LICENSE file for terms of use.
 */

#include <boost/foreach.hpp>
#include <boost/test/unit_test.hpp>

#include "Model.hpp"

typedef void (Implementation::Model::*OneArgMethod) (
    const Abstract::Point& coordinates
);

typedef void (Implementation::Model::*TwoArgsMethod) (
    const Abstract::Point& coordinates,
    int change
);

typedef void (Implementation::Model::*MultiArgsMethod) (
    const Abstract::Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
);

template<typename Func>
void checkModelMethodForThrow(
    Implementation::Model* model,
    Func model_method,
    int arg1,
    int arg2
) {
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(arg1, arg2), Exception
    );
}

template<>
void checkModelMethodForThrow<OneArgMethod>(
    Implementation::Model* model,
    OneArgMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates), Exception
    );
}

template<>
void checkModelMethodForThrow<TwoArgsMethod>(
    Implementation::Model* model,
    TwoArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates, 0), Exception
    );
}

template<>
void checkModelMethodForThrow<MultiArgsMethod>(
    Implementation::Model* model,
    MultiArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(
            coordinates,
            DEFAULT_MASS,
            0,
            0,
            0
        ),
        Exception
    );
}

// "dead" test: attempt to do something with dead bacterium
template<typename Func>
static void deadTest(
    Implementation::Model* model,
    Func model_method,
    bool by_coordinates
) {
    if (by_coordinates) {
        Abstract::Point coordinates(0, 0);
        model->killByCoordinates(coordinates);
    } else {
        model->kill(0, 0);
    }
    checkModelMethodForThrow(
        model,
        model_method,
        0,
        0
    );
}

template<typename Func>
static void checkErrorHandling(
    Implementation::Model* model,
    Func model_method,
    bool by_coordinates,
    bool dead_test
) {
    // Range errors: test all combinations of
    // "wrong" (outside of correct range) arguments.
    // This solution works for coordinates and non-coordinates
    // methods of Model. (< 0, = 0, > MAX)
    int wrong_args[] = {-1, 0, MIN_WIDTH};
    BOOST_FOREACH (int arg1, wrong_args) {
        BOOST_FOREACH (int arg2, wrong_args) {
            if ((arg1 != 0) || (arg2 != 0)) {
                // (0, 0) is correct
                checkModelMethodForThrow(
                    model,
                    model_method,
                    arg1,
                    arg2
                );
            }
        }
    }
    if (dead_test) {
        // "dead" error
        // (attempt to do something with dead bacterium)
        deadTest(model, model_method, by_coordinates);
    }
}

#define CREATE_NEW \
    model->createNewByCoordinates( \
        coordinates, \
        DEFAULT_MASS, \
        0, \
        0, \
        0 \
    );

static Abstract::Point createInBaseCoordinates(
    Implementation::Model* model
) {
    Abstract::Point coordinates(0, 0);
    CREATE_NEW
    return coordinates;
}

#undef CREATE_NEW

static Implementation::Model* createBaseModel(
    int bacteria = 0,
    int teams = 1
) {
    Implementation::Model* model =
        Abstract::makeModel<Implementation::Model>(
            MIN_WIDTH,
            MIN_HEIGHT,
            bacteria,
            teams
        );
    return model;
}

BOOST_AUTO_TEST_CASE (bacteria_number_test) {
    Implementation::Model* model = createBaseModel();
    int bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 0);
    createInBaseCoordinates(model);
    bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 1);
    // range errors
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(-1), Exception);
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(1), Exception);
    delete model;
}

BOOST_AUTO_TEST_CASE (get_mass_test) {
    Implementation::Model* model = createBaseModel(1, 1);
    int mass = model->getMass(0, 0);
    BOOST_REQUIRE(mass == DEFAULT_MASS);
    checkErrorHandling(model, &Implementation::Model::getMass);
    delete model;
}

BOOST_AUTO_TEST_CASE (height_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getHeight() == MIN_HEIGHT);
    delete model;
}

BOOST_AUTO_TEST_CASE (width_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getWidth() == MIN_WIDTH);
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->kill(0, 0);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    // error handling checks
    createInBaseCoordinates(model);
    // FIXME test doesn't work correctly without this function call.
    // The solution is to use set instead of vector in model.
    model->clearBeforeMove(0);
    checkErrorHandling(model, &Implementation::Model::kill);
    delete model;
}

BOOST_AUTO_TEST_CASE (create_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::BACTERIUM);
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->killByCoordinates(coordinates);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    delete model;
}
<|endoftext|>


<commit_before>#include "memcache_client_pool.h"

#include "vbucket_config.h"
#include "evpp/event_loop_thread_pool.h"
#include "likely.h"

namespace evmc {

#define GET_FILTER_KEY_POS(name, key)          \
    if(filter_len_ > 0) {                      \
        name = filter_len_;                    \
        if(key.size() < filter_len_) {         \
            name = key.size();                 \
        }                                      \
    } else {                                   \
        name = key.find(key_filter_[0]);       \
        if (name == std::string::npos) {       \
            name = key.size();                 \
        }                                      \
    }

#define SET_SERVERID(vbucket,command) \
    MultiModeVbucketConfig* vbconf = vbucket_config(); \
    uint16_t server_id = vbconf->SelectServerId(vbucket, BAD_SERVER_ID); \
    command->set_server_id(server_id); \

    MemcacheClientPool::~MemcacheClientPool() {
        if (loop_pool_.IsRunning()) {
            Stop(true);
        }
    }

    void MemcacheClientPool::Stop(bool wait_thread_exit) {
        loop_pool_.Stop(wait_thread_exit);
        MemcacheClientBase::Stop();
    }

    bool MemcacheClientPool::Start() {

        bool ok = loop_pool_.Start(true);

        if (UNLIKELY(!ok)) {
            LOG_ERROR << "loop pool start failed";
            return false;
        }

        if (!MemcacheClientBase::Start(true)) {
            loop_pool_.Stop(true);
            LOG_ERROR << "vbucket init failed";
            return false;
        }
        auto server_list = vbucket_config()->server_list();

        // 须先构造memc_client_map_数组，再各个元素取地址，否则地址不稳定，可能崩溃
        for (uint32_t i = 0; i < loop_pool_.thread_num(); ++i) {
            memc_client_map_.emplace_back(MemcClientMap());
            evpp::EventLoop* loop = loop_pool_.GetNextLoopWithHash(i);

            for (size_t svr = 0; svr < server_list.size(); ++svr) {
                auto& client_map = memc_client_map_.back();
                BuilderMemClient(loop, server_list[svr], client_map, timeout_ms_);
            }
        }

        for (uint32_t i = 0; i < loop_pool_.thread_num(); ++i) {
            evpp::EventLoop* loop = loop_pool_.GetNextLoopWithHash(i);
            loop->set_context(evpp::Any(&memc_client_map_[i]));
        }

        return ok;
    }

    void MemcacheClientPool::Set(evpp::EventLoop* caller_loop, const std::string& key, const std::string& value, uint32_t flags,
                                 uint32_t expire, SetCallback callback) {

        std::size_t pos = 0;
        GET_FILTER_KEY_POS(pos, key)
        const uint16_t vbucket = vbucket_config()->GetVbucketByKey(key.c_str(), pos);
        CommandPtr command = std::make_shared<SetCommand>(caller_loop, vbucket, key, value, flags, expire, callback);
        SET_SERVERID(vbucket, command)
        LaunchCommand(command);
    }

    void MemcacheClientPool::Remove(evpp::EventLoop* caller_loop, const std::string& key, RemoveCallback callback) {
        std::size_t pos = 0;
        GET_FILTER_KEY_POS(pos, key)
        const uint16_t vbucket = vbucket_config()->GetVbucketByKey(key.c_str(), pos);
        CommandPtr command = std::make_shared<RemoveCommand>(caller_loop, vbucket, key, callback);
        SET_SERVERID(vbucket, command)
        LaunchCommand(command);
    }

    void MemcacheClientPool::Get(evpp::EventLoop* caller_loop, const std::string& key, GetCallback callback) {
        std::size_t pos = 0;
        GET_FILTER_KEY_POS(pos, key)
        const uint16_t vbucket = vbucket_config()->GetVbucketByKey(key.c_str(), pos);
        CommandPtr command = std::make_shared<GetCommand>(caller_loop, vbucket, key, callback);
        SET_SERVERID(vbucket, command)
        LaunchCommand(command);
    }

    void MemcacheClientPool::PrefixGet(evpp::EventLoop* caller_loop, const std::string& key, PrefixGetCallback callback) {
        std::size_t pos = 0;
        GET_FILTER_KEY_POS(pos, key)
        const uint16_t vbucket = vbucket_config()->GetVbucketByKey(key.c_str(), pos);
        CommandPtr command = std::make_shared<PrefixGetCommand>(caller_loop, vbucket, key, callback);
        SET_SERVERID(vbucket, command)
        LaunchCommand(command);
    }

    void MemcacheClientPool::RunBackGround(const std::function<void(void)>& fun) {
        auto loop = loop_pool_.GetNextLoopWithHash(rand());
        loop->RunInLoop(fun);
    }

    void MemcacheClientPool::MultiGet(evpp::EventLoop* caller_loop, std::vector<std::string>& keys, MultiGetCallback& callback) {
        if (UNLIKELY(keys.size() <= 0)) {
            MultiGetResult result;
            caller_loop->RunInLoop(std::bind(callback, std::move(result)));
            return;
        }
        const std::size_t size = keys.size();
        std::map<uint16_t, IdInfo> serverid_keys;

        MultiModeVbucketConfig* vbconf = vbucket_config();
        uint16_t vbucket = 0;
        uint16_t server_id = 0;
        MultiKeyGetHandlerPtr handler = std::make_shared<MultiKeyHandler<MultiGetResult, MultiGetCallback> > (callback);
        auto& result = handler->get_result();
        std::size_t pos = 0;
        std::vector<int> serverid_table(vbconf->server_list().size(), -1);

        for (size_t i = 0; i < size; ++i) {
            auto& key = keys[i];
            GET_FILTER_KEY_POS(pos, key)
            vbucket = vbconf->GetVbucketByKey(key.c_str(), pos);
            server_id = vbconf->SelectServerFirstId(vbucket);
            if (serverid_table[server_id] == -1) {
                serverid_table[server_id] = vbconf->SelectServerId(vbucket, BAD_SERVER_ID);
            }
            server_id = serverid_table[server_id];

            auto& item = serverid_keys[server_id];
            item.keys.emplace_back(key);
            item.vbuckets.emplace_back(vbucket);
            result.emplace(std::move(key), std::move(GetResult()));
        }
        handler->set_serverid_keys(serverid_keys);

        auto& serverid_keys_d = handler->get_serverid_keys();
        for (auto& it : serverid_keys_d) {
            server_id = it.first;
            CommandPtr command = std::make_shared<MultiGetCommand>(caller_loop, server_id, handler);
            command->set_server_id(server_id);
            LaunchCommand(command);
        }
    }

    void MemcacheClientPool::PrefixMultiGet(evpp::EventLoop* caller_loop, std::vector<std::string>& keys, PrefixMultiGetCallback callback) {
        if (UNLIKELY(keys.size() <= 0)) {
            PrefixMultiGetResult result;
            caller_loop->RunInLoop(std::bind(callback, std::move(result)));
            return;
        }
        MultiModeVbucketConfig* vbconf = vbucket_config();
        const std::size_t size = keys.size();
        std::map<uint16_t, IdInfo> serverid_keys;
        std::vector<int> serverid_table(vbconf->server_list().size(), -1);

        uint16_t vbucket = 0;
        uint16_t server_id = 0;
        PrefixMultiKeyGetHandlerPtr handler = std::make_shared<MultiKeyHandler<PrefixMultiGetResult, PrefixMultiGetCallback> >(callback);
        auto& result = handler->get_result();
        std::size_t pos = 0;

        for (size_t i = 0; i < size; ++i) {
            auto& key = keys[i];
            GET_FILTER_KEY_POS(pos, key)
            vbucket = vbconf->GetVbucketByKey(key.c_str(), pos);
            server_id = vbconf->SelectServerFirstId(vbucket);
            if (serverid_table[server_id] == -1) {
                serverid_table[server_id] = vbconf->SelectServerId(vbucket, BAD_SERVER_ID);
            }
            server_id = serverid_table[server_id];
            auto& item = serverid_keys[server_id];
            item.keys.emplace_back(key);
            item.vbuckets.emplace_back(vbucket);
            result.emplace(std::move(key), std::make_shared<PrefixGetResult>());
        }
        handler->set_serverid_keys(serverid_keys);

        auto& serverid_keys_d = handler->get_serverid_keys();
        for (auto& it : serverid_keys_d) {
            server_id = it.first;
            CommandPtr command = std::make_shared<PrefixMultiGetCommand>(caller_loop, server_id, handler);
            command->set_server_id(server_id);
            LaunchCommand(command);
        }
    }

    void MemcacheClientPool::LaunchCommand(CommandPtr& command) {
        //const int thread = next_thread_++;
        auto loop = loop_pool_.GetNextLoopWithHash(rand());
        loop->RunInLoop(
            std::bind(&MemcacheClientPool::DoLaunchCommand, this, loop, command));
    }

    void MemcacheClientPool::DoLaunchCommand(evpp::EventLoop* loop, CommandPtr command) {
        MultiModeVbucketConfig* vbconf = vbucket_config();

        uint16_t server_id = command->server_id();
        if (UNLIKELY(!command->ShouldRetry())) { //重试 需要重新算serverid
            uint16_t vbucket = command->vbucket_id();
            server_id = vbconf->SelectServerId(vbucket, command->server_id());
            if (UNLIKELY(server_id == BAD_SERVER_ID)) {
                LOG_ERROR << "bad server id";
                command->OnError(ERR_CODE_DISCONNECT);
                return;
            }
        }

        std::string server_addr = vbconf->GetServerAddrById(server_id);
        MemcClientMap* client_map = GetMemcClientMap(loop);

        if (UNLIKELY(client_map == nullptr)) {
            command->OnError(ERR_CODE_DISCONNECT);
            LOG_INFO << "DoLaunchCommand thread pool empty";
            return;
        }

        auto it = client_map->find(server_addr);

        if (UNLIKELY(it == client_map->end())) {
            BuilderMemClient(loop, server_addr, *client_map, timeout_ms_);
            auto client = client_map->find(server_addr);
            client->second->PushWaitingCommand(command);
            return;
        }

        if (LIKELY(it->second->conn() && it->second->conn()->IsConnected())) {
            it->second->PushRunningCommand(command);
            command->Launch(it->second);
            return;
        }

        if (!it->second->conn() || it->second->conn()->status() == evpp::TCPConn::kConnecting) {
            it->second->PushWaitingCommand(command);
        } else {
            if (command->ShouldRetry()) {
                LOG_INFO << "OnClientConnection disconnect retry";
                command->set_id(0);
                command->set_server_id(command->server_id());
                LaunchCommand(command);
            } else {
                command->OnError(ERR_CODE_DISCONNECT);
            }
        }
    }
    }

<commit_msg>fix format shortcomming 	modified:   memcache_client_pool.cc<commit_after>#include "memcache_client_pool.h"

#include "vbucket_config.h"
#include "evpp/event_loop_thread_pool.h"
#include "likely.h"

namespace evmc {

#define GET_FILTER_KEY_POS(name, key)          \
    if (filter_len_ > 0) {                     \
        name = filter_len_;                    \
        if (key.size() < filter_len_) {        \
            name = key.size();                 \
        }                                      \
    } else {                                   \
        name = key.find(key_filter_[0]);       \
        if (name == std::string::npos) {       \
            name = key.size();                 \
        }                                      \
    }

#define SET_SERVERID(vbucket,command) \
    MultiModeVbucketConfig* vbconf = vbucket_config(); \
    uint16_t server_id = vbconf->SelectServerId(vbucket, BAD_SERVER_ID); \
    command->set_server_id(server_id); \

    MemcacheClientPool::~MemcacheClientPool() {
        if (loop_pool_.IsRunning()) {
            Stop(true);
        }
    }

    void MemcacheClientPool::Stop(bool wait_thread_exit) {
        loop_pool_.Stop(wait_thread_exit);
        MemcacheClientBase::Stop();
    }

    bool MemcacheClientPool::Start() {

        bool ok = loop_pool_.Start(true);

        if (UNLIKELY(!ok)) {
            LOG_ERROR << "loop pool start failed";
            return false;
        }

        if (!MemcacheClientBase::Start(true)) {
            loop_pool_.Stop(true);
            LOG_ERROR << "vbucket init failed";
            return false;
        }
        auto server_list = vbucket_config()->server_list();

        // 须先构造memc_client_map_数组，再各个元素取地址，否则地址不稳定，可能崩溃
        for (uint32_t i = 0; i < loop_pool_.thread_num(); ++i) {
            memc_client_map_.emplace_back(MemcClientMap());
            evpp::EventLoop* loop = loop_pool_.GetNextLoopWithHash(i);

            for (size_t svr = 0; svr < server_list.size(); ++svr) {
                auto& client_map = memc_client_map_.back();
                BuilderMemClient(loop, server_list[svr], client_map, timeout_ms_);
            }
        }

        for (uint32_t i = 0; i < loop_pool_.thread_num(); ++i) {
            evpp::EventLoop* loop = loop_pool_.GetNextLoopWithHash(i);
            loop->set_context(evpp::Any(&memc_client_map_[i]));
        }

        return ok;
    }

    void MemcacheClientPool::Set(evpp::EventLoop* caller_loop, const std::string& key, const std::string& value, uint32_t flags,
                                 uint32_t expire, SetCallback callback) {

        std::size_t pos = 0;
        GET_FILTER_KEY_POS(pos, key)
        const uint16_t vbucket = vbucket_config()->GetVbucketByKey(key.c_str(), pos);
        CommandPtr command = std::make_shared<SetCommand>(caller_loop, vbucket, key, value, flags, expire, callback);
        SET_SERVERID(vbucket, command)
        LaunchCommand(command);
    }

    void MemcacheClientPool::Remove(evpp::EventLoop* caller_loop, const std::string& key, RemoveCallback callback) {
        std::size_t pos = 0;
        GET_FILTER_KEY_POS(pos, key)
        const uint16_t vbucket = vbucket_config()->GetVbucketByKey(key.c_str(), pos);
        CommandPtr command = std::make_shared<RemoveCommand>(caller_loop, vbucket, key, callback);
        SET_SERVERID(vbucket, command)
        LaunchCommand(command);
    }

    void MemcacheClientPool::Get(evpp::EventLoop* caller_loop, const std::string& key, GetCallback callback) {
        std::size_t pos = 0;
        GET_FILTER_KEY_POS(pos, key)
        const uint16_t vbucket = vbucket_config()->GetVbucketByKey(key.c_str(), pos);
        CommandPtr command = std::make_shared<GetCommand>(caller_loop, vbucket, key, callback);
        SET_SERVERID(vbucket, command)
        LaunchCommand(command);
    }

    void MemcacheClientPool::PrefixGet(evpp::EventLoop* caller_loop, const std::string& key, PrefixGetCallback callback) {
        std::size_t pos = 0;
        GET_FILTER_KEY_POS(pos, key)
        const uint16_t vbucket = vbucket_config()->GetVbucketByKey(key.c_str(), pos);
        CommandPtr command = std::make_shared<PrefixGetCommand>(caller_loop, vbucket, key, callback);
        SET_SERVERID(vbucket, command)
        LaunchCommand(command);
    }

    void MemcacheClientPool::RunBackGround(const std::function<void(void)>& fun) {
        auto loop = loop_pool_.GetNextLoopWithHash(rand());
        loop->RunInLoop(fun);
    }

    void MemcacheClientPool::MultiGet(evpp::EventLoop* caller_loop, std::vector<std::string>& keys, MultiGetCallback& callback) {
        if (UNLIKELY(keys.size() <= 0)) {
            MultiGetResult result;
            caller_loop->RunInLoop(std::bind(callback, std::move(result)));
            return;
        }
        const std::size_t size = keys.size();
        std::map<uint16_t, IdInfo> serverid_keys;

        MultiModeVbucketConfig* vbconf = vbucket_config();
        uint16_t vbucket = 0;
        uint16_t server_id = 0;
        MultiKeyGetHandlerPtr handler = std::make_shared<MultiKeyHandler<MultiGetResult, MultiGetCallback> > (callback);
        auto& result = handler->get_result();
        std::size_t pos = 0;
        std::vector<int> serverid_table(vbconf->server_list().size(), -1);

        for (size_t i = 0; i < size; ++i) {
            auto& key = keys[i];
            GET_FILTER_KEY_POS(pos, key)
            vbucket = vbconf->GetVbucketByKey(key.c_str(), pos);
            server_id = vbconf->SelectServerFirstId(vbucket);
            if (serverid_table[server_id] == -1) {
                serverid_table[server_id] = vbconf->SelectServerId(vbucket, BAD_SERVER_ID);
            }
            server_id = serverid_table[server_id];

            auto& item = serverid_keys[server_id];
            item.keys.emplace_back(key);
            item.vbuckets.emplace_back(vbucket);
            result.emplace(std::move(key), std::move(GetResult()));
        }
        handler->set_serverid_keys(serverid_keys);

        auto& serverid_keys_d = handler->get_serverid_keys();
        for (auto& it : serverid_keys_d) {
            server_id = it.first;
            CommandPtr command = std::make_shared<MultiGetCommand>(caller_loop, server_id, handler);
            command->set_server_id(server_id);
            LaunchCommand(command);
        }
    }

    void MemcacheClientPool::PrefixMultiGet(evpp::EventLoop* caller_loop, std::vector<std::string>& keys, PrefixMultiGetCallback callback) {
        if (UNLIKELY(keys.size() <= 0)) {
            PrefixMultiGetResult result;
            caller_loop->RunInLoop(std::bind(callback, std::move(result)));
            return;
        }
        MultiModeVbucketConfig* vbconf = vbucket_config();
        const std::size_t size = keys.size();
        std::map<uint16_t, IdInfo> serverid_keys;
        std::vector<int> serverid_table(vbconf->server_list().size(), -1);

        uint16_t vbucket = 0;
        uint16_t server_id = 0;
        PrefixMultiKeyGetHandlerPtr handler = std::make_shared<MultiKeyHandler<PrefixMultiGetResult, PrefixMultiGetCallback> >(callback);
        auto& result = handler->get_result();
        std::size_t pos = 0;

        for (size_t i = 0; i < size; ++i) {
            auto& key = keys[i];
            GET_FILTER_KEY_POS(pos, key)
            vbucket = vbconf->GetVbucketByKey(key.c_str(), pos);
            server_id = vbconf->SelectServerFirstId(vbucket);
            if (serverid_table[server_id] == -1) {
                serverid_table[server_id] = vbconf->SelectServerId(vbucket, BAD_SERVER_ID);
            }
            server_id = serverid_table[server_id];
            auto& item = serverid_keys[server_id];
            item.keys.emplace_back(key);
            item.vbuckets.emplace_back(vbucket);
            result.emplace(std::move(key), std::make_shared<PrefixGetResult>());
        }
        handler->set_serverid_keys(serverid_keys);

        auto& serverid_keys_d = handler->get_serverid_keys();
        for (auto& it : serverid_keys_d) {
            server_id = it.first;
            CommandPtr command = std::make_shared<PrefixMultiGetCommand>(caller_loop, server_id, handler);
            command->set_server_id(server_id);
            LaunchCommand(command);
        }
    }

    void MemcacheClientPool::LaunchCommand(CommandPtr& command) {
        //const int thread = next_thread_++;
        auto loop = loop_pool_.GetNextLoopWithHash(rand());
        loop->RunInLoop(
            std::bind(&MemcacheClientPool::DoLaunchCommand, this, loop, command));
    }

    void MemcacheClientPool::DoLaunchCommand(evpp::EventLoop* loop, CommandPtr command) {
        MultiModeVbucketConfig* vbconf = vbucket_config();

        uint16_t server_id = command->server_id();
        if (UNLIKELY(!command->ShouldRetry())) { //重试 需要重新算serverid
            uint16_t vbucket = command->vbucket_id();
            server_id = vbconf->SelectServerId(vbucket, command->server_id());
            if (UNLIKELY(server_id == BAD_SERVER_ID)) {
                LOG_ERROR << "bad server id";
                command->OnError(ERR_CODE_DISCONNECT);
                return;
            }
        }

        std::string server_addr = vbconf->GetServerAddrById(server_id);
        MemcClientMap* client_map = GetMemcClientMap(loop);

        if (UNLIKELY(client_map == nullptr)) {
            command->OnError(ERR_CODE_DISCONNECT);
            LOG_INFO << "DoLaunchCommand thread pool empty";
            return;
        }

        auto it = client_map->find(server_addr);

        if (UNLIKELY(it == client_map->end())) {
            BuilderMemClient(loop, server_addr, *client_map, timeout_ms_);
            auto client = client_map->find(server_addr);
            client->second->PushWaitingCommand(command);
            return;
        }

        if (LIKELY(it->second->conn() && it->second->conn()->IsConnected())) {
            it->second->PushRunningCommand(command);
            command->Launch(it->second);
            return;
        }

        if (!it->second->conn() || it->second->conn()->status() == evpp::TCPConn::kConnecting) {
            it->second->PushWaitingCommand(command);
        } else {
            if (command->ShouldRetry()) {
                LOG_INFO << "OnClientConnection disconnect retry";
                command->set_id(0);
                command->set_server_id(command->server_id());
                LaunchCommand(command);
            } else {
                command->OnError(ERR_CODE_DISCONNECT);
            }
        }
    }
    }

<|endoftext|>


<commit_before>/*
 * bacteria-core, core for cellular automaton
 * Copyright (C) 2016 Pavel Dolgov
 *
 * See the LICENSE file for terms of use.
 */

#include <boost/foreach.hpp>
#include <boost/test/unit_test.hpp>

#include "Model.hpp"

typedef void (Implementation::Model::*OneArgMethod) (
    const Abstract::Point& coordinates
);

typedef void (Implementation::Model::*TwoArgsMethod) (
    const Abstract::Point& coordinates,
    int change
);

typedef void (Implementation::Model::*MultiArgsMethod) (
    const Abstract::Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
);

template<typename Func>
void checkModelMethodForThrow(
    Implementation::Model* model,
    Func model_method,
    int arg1,
    int arg2
) {
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(arg1, arg2), Exception
    );
}

template<>
void checkModelMethodForThrow<OneArgMethod>(
    Implementation::Model* model,
    OneArgMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates), Exception
    );
}

template<>
void checkModelMethodForThrow<TwoArgsMethod>(
    Implementation::Model* model,
    TwoArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates, 0), Exception
    );
}

template<>
void checkModelMethodForThrow<MultiArgsMethod>(
    Implementation::Model* model,
    MultiArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(
            coordinates,
            DEFAULT_MASS,
            0,
            0,
            0
        ),
        Exception
    );
}

// "dead" test: attempt to do something with dead bacterium
template<typename Func>
static void deadTest(
    Implementation::Model* model,
    Func model_method,
    bool by_coordinates
) {
    if (by_coordinates) {
        Abstract::Point coordinates(0, 0);
        model->killByCoordinates(coordinates);
    } else {
        model->kill(0, 0);
    }
    checkModelMethodForThrow(
        model,
        model_method,
        0,
        0
    );
}

template<typename Func>
static void checkErrorHandling(
    Implementation::Model* model,
    Func model_method,
    bool by_coordinates,
    bool dead_test
) {
    // Range errors: test all combinations of
    // "wrong" (outside of correct range) arguments.
    // This solution works for coordinates and non-coordinates
    // methods of Model. (< 0, = 0, > MAX)
    int wrong_args[] = {-1, 0, MIN_WIDTH};
    BOOST_FOREACH (int arg1, wrong_args) {
        BOOST_FOREACH (int arg2, wrong_args) {
            if ((arg1 != 0) || (arg2 != 0)) {
                // (0, 0) is correct
                checkModelMethodForThrow(
                    model,
                    model_method,
                    arg1,
                    arg2
                );
            }
        }
    }
    if (dead_test) {
        // "dead" error
        // (attempt to do something with dead bacterium)
        deadTest(model, model_method, by_coordinates);
    }
}

#define CREATE_NEW \
    model->createNewByCoordinates( \
        coordinates, \
        DEFAULT_MASS, \
        0, \
        0, \
        0 \
    );

static Abstract::Point createInBaseCoordinates(
    Implementation::Model* model
) {
    Abstract::Point coordinates(0, 0);
    CREATE_NEW
    return coordinates;
}

#undef CREATE_NEW

static Implementation::Model* createBaseModel(
    int bacteria = 0,
    int teams = 1
) {
    Implementation::Model* model =
        Abstract::makeModel<Implementation::Model>(
            MIN_WIDTH,
            MIN_HEIGHT,
            bacteria,
            teams
        );
    return model;
}

BOOST_AUTO_TEST_CASE (bacteria_number_test) {
    Implementation::Model* model = createBaseModel();
    int bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 0);
    createInBaseCoordinates(model);
    bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 1);
    // range errors
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(-1), Exception);
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(1), Exception);
    delete model;
}

BOOST_AUTO_TEST_CASE (get_mass_test) {
    Implementation::Model* model = createBaseModel(1, 1);
    int mass = model->getMass(0, 0);
    BOOST_REQUIRE(mass == DEFAULT_MASS);
    checkErrorHandling(model, &Implementation::Model::getMass);
    delete model;
}

BOOST_AUTO_TEST_CASE (height_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getHeight() == MIN_HEIGHT);
    delete model;
}

BOOST_AUTO_TEST_CASE (width_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getWidth() == MIN_WIDTH);
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->kill(0, 0);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    // error handling checks
    createInBaseCoordinates(model);
    // FIXME test doesn't work correctly without this function call.
    // The solution is to use set instead of vector in model.
    model->clearBeforeMove(0);
    checkErrorHandling(model, &Implementation::Model::kill);
    delete model;
}

BOOST_AUTO_TEST_CASE (create_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::BACTERIUM);
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->killByCoordinates(coordinates);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    delete model;
}
<commit_msg>Model tests: remove macro CREATE_NEW<commit_after>/*
 * bacteria-core, core for cellular automaton
 * Copyright (C) 2016 Pavel Dolgov
 *
 * See the LICENSE file for terms of use.
 */

#include <boost/foreach.hpp>
#include <boost/test/unit_test.hpp>

#include "Model.hpp"

typedef void (Implementation::Model::*OneArgMethod) (
    const Abstract::Point& coordinates
);

typedef void (Implementation::Model::*TwoArgsMethod) (
    const Abstract::Point& coordinates,
    int change
);

typedef void (Implementation::Model::*MultiArgsMethod) (
    const Abstract::Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
);

template<typename Func>
void checkModelMethodForThrow(
    Implementation::Model* model,
    Func model_method,
    int arg1,
    int arg2
) {
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(arg1, arg2), Exception
    );
}

template<>
void checkModelMethodForThrow<OneArgMethod>(
    Implementation::Model* model,
    OneArgMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates), Exception
    );
}

template<>
void checkModelMethodForThrow<TwoArgsMethod>(
    Implementation::Model* model,
    TwoArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates, 0), Exception
    );
}

template<>
void checkModelMethodForThrow<MultiArgsMethod>(
    Implementation::Model* model,
    MultiArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(
            coordinates,
            DEFAULT_MASS,
            0,
            0,
            0
        ),
        Exception
    );
}

// "dead" test: attempt to do something with dead bacterium
template<typename Func>
static void deadTest(
    Implementation::Model* model,
    Func model_method,
    bool by_coordinates
) {
    if (by_coordinates) {
        Abstract::Point coordinates(0, 0);
        model->killByCoordinates(coordinates);
    } else {
        model->kill(0, 0);
    }
    checkModelMethodForThrow(
        model,
        model_method,
        0,
        0
    );
}

template<typename Func>
static void checkErrorHandling(
    Implementation::Model* model,
    Func model_method,
    bool by_coordinates,
    bool dead_test
) {
    // Range errors: test all combinations of
    // "wrong" (outside of correct range) arguments.
    // This solution works for coordinates and non-coordinates
    // methods of Model. (< 0, = 0, > MAX)
    int wrong_args[] = {-1, 0, MIN_WIDTH};
    BOOST_FOREACH (int arg1, wrong_args) {
        BOOST_FOREACH (int arg2, wrong_args) {
            if ((arg1 != 0) || (arg2 != 0)) {
                // (0, 0) is correct
                checkModelMethodForThrow(
                    model,
                    model_method,
                    arg1,
                    arg2
                );
            }
        }
    }
    if (dead_test) {
        // "dead" error
        // (attempt to do something with dead bacterium)
        deadTest(model, model_method, by_coordinates);
    }
}

static Abstract::Point createInBaseCoordinates(
    Implementation::Model* model
) {
    Abstract::Point coordinates(0, 0);
    model->createNewByCoordinates(
        coordinates,
        DEFAULT_MASS,
        0,
        0,
        0
    );
    return coordinates;
}

static Implementation::Model* createBaseModel(
    int bacteria = 0,
    int teams = 1
) {
    Implementation::Model* model =
        Abstract::makeModel<Implementation::Model>(
            MIN_WIDTH,
            MIN_HEIGHT,
            bacteria,
            teams
        );
    return model;
}

BOOST_AUTO_TEST_CASE (bacteria_number_test) {
    Implementation::Model* model = createBaseModel();
    int bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 0);
    createInBaseCoordinates(model);
    bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 1);
    // range errors
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(-1), Exception);
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(1), Exception);
    delete model;
}

BOOST_AUTO_TEST_CASE (get_mass_test) {
    Implementation::Model* model = createBaseModel(1, 1);
    int mass = model->getMass(0, 0);
    BOOST_REQUIRE(mass == DEFAULT_MASS);
    checkErrorHandling(model, &Implementation::Model::getMass);
    delete model;
}

BOOST_AUTO_TEST_CASE (height_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getHeight() == MIN_HEIGHT);
    delete model;
}

BOOST_AUTO_TEST_CASE (width_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getWidth() == MIN_WIDTH);
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->kill(0, 0);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    // error handling checks
    createInBaseCoordinates(model);
    // FIXME test doesn't work correctly without this function call.
    // The solution is to use set instead of vector in model.
    model->clearBeforeMove(0);
    checkErrorHandling(model, &Implementation::Model::kill);
    delete model;
}

BOOST_AUTO_TEST_CASE (create_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::BACTERIUM);
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->killByCoordinates(coordinates);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    delete model;
}
<|endoftext|>


<commit_before>// Spinlock.
// (C) 2013 Cybozu.

#ifndef CYBOZU_SPINLOCK_HPP
#define CYBOZU_SPINLOCK_HPP

#include <atomic>
#if defined(__i386__) || defined(__x86_64__)
# include <x86intrin.h>
#endif

namespace cybozu {

// A simple spinlock.
class spinlock {
public:
    spinlock(): m_flag(ATOMIC_FLAG_INIT) {}
    void lock() {
        while( m_flag.test_and_set(std::memory_order_acquire) )
            pause();
    }
    void unlock() {
        m_flag.clear(std::memory_order_release);
    }

private:
    std::atomic_flag m_flag;
    void pause() {
#if defined(__i386__) || defined(__x86_64__)
        _mm_pause();
#endif
    }
};

} // namespace cybozu

#endif // CYBOZU_SPINLOCK_HPP
<commit_msg>Use asm instead of _mm_pause for non-SSE CPUs.<commit_after>// Spinlock.
// (C) 2013 Cybozu.

#ifndef CYBOZU_SPINLOCK_HPP
#define CYBOZU_SPINLOCK_HPP

#include <atomic>
#if defined(__i386__) || defined(__x86_64__)
# include <x86intrin.h>
#endif

namespace cybozu {

// A simple spinlock.
class spinlock {
public:
    spinlock(): m_flag(ATOMIC_FLAG_INIT) {}
    void lock() {
        while( m_flag.test_and_set(std::memory_order_acquire) )
            pause();
    }
    void unlock() {
        m_flag.clear(std::memory_order_release);
    }

private:
    std::atomic_flag m_flag;
    void pause() {
#if defined(__i386__) || defined(__x86_64__)
# if defined(__SSE__)
        _mm_pause();
# else
        __asm__ __volatile__ ("rep; nop");
# endif
#endif
    }
};

} // namespace cybozu

#endif // CYBOZU_SPINLOCK_HPP
<|endoftext|>


<commit_before>#include <asiocurl/scope.hpp>

#include <asiocurl/optional.hpp>
#include <utility>
#include <catch.hpp>

namespace {

	class functor {

		private:

			bool * ptr_;
			bool i_;

		public:

			functor () noexcept : ptr_(nullptr), i_(false) {	}
			functor (bool & b) noexcept : ptr_(&b), i_(false) {	}

			explicit operator bool () const noexcept {

				//	Determines whether this object has been
				//	invoked directly
				return i_;

			}

			void operator () () noexcept {

				i_=true;
				if (ptr_) *ptr_=true;

			}

	};

}

template <typename T>
static void fail (asiocurl::scope_exit<T> e) noexcept {

	try {

		asiocurl::scope_exit<T> i(std::move(e));

		throw 0;

	} catch (...) {	}

}

SCENARIO("scope_exit objects invoke their stored functor when they go out of scope unless they are moved or released","[asiocurl][scope_exit][scope]") {

	using asiocurl::in_place;
	using asiocurl::nullopt;
	using asiocurl::optional;
	using asiocurl::scope_exit;

	bool i=false;
	functor f(i);

	GIVEN("A scope_exit object which is not templated on a reference type") {

		optional<scope_exit<functor>> e(in_place,f);

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It is released") {

			e->release();

			AND_WHEN("It goes out of scope") {

				e=nullopt;

				THEN("The stored functor is not invoked") {

					CHECK(!i);

				}

			}

		}

		WHEN("It is moved") {

			optional<scope_exit<functor>> mt(in_place,std::move(*e));

			AND_WHEN("The moved from scope_exit object goes out of scope") {

				e=nullopt;

				THEN("The stored functor is not invoked") {

					CHECK(!i);

				}

				AND_WHEN("The moved to scope_exit object goes out of scope") {

					mt=nullopt;

					THEN("The stored functor is invoked") {

						CHECK(i);
						CHECK(!f);

					}

				}

			}

		}

	}

	GIVEN("A scope_exit object which is templated on a reference type") {

		optional<scope_exit<functor &>> e(in_place,f);

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The functor (not a copy thereof) is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The functor is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

		WHEN("It is released") {

			e->release();

			AND_WHEN("It goes out of scope") {

				e=nullopt;

				THEN("The functor is not invoked") {

					CHECK(!i);

				}

			}

		}

		WHEN("It is moved") {

			optional<scope_exit<functor &>> mt(in_place,std::move(*e));

			AND_WHEN("The moved from scope_exit object goes out of scope") {

				e=nullopt;

				THEN("The functor is not invoked") {

					CHECK(!i);

				}

				AND_WHEN("The moved to scope_exit object goes out of scope") {

					mt=nullopt;

					THEN("The functor (not a copy thereof) is invoked") {

						CHECK(i);
						CHECK(f);

					}

				}

			}

		}

	}

}

SCENARIO("scope_exit objects obtained from the make_scope_exit factory invoke their stored functor when they go out of scope","[asiocurl][scope_exit][scope]") {

	using asiocurl::make_scope_exit;
	using asiocurl::make_scope_fail;
	using asiocurl::make_scope_success;
	using asiocurl::nullopt;
	using asiocurl::scope_exit;
	using asiocurl::optional;

	bool i=false;
	functor f(i);

	GIVEN("A scope_exit object obtained from the make_scope_exit factory called with an rvalue") {

		optional<scope_exit<functor>> e(make_scope_exit(std::move(f)));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_exit factory called with a const lvalue reference") {

		const auto & r=f;
		optional<scope_exit<functor>> e(make_scope_exit(r));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_exit factory called with a mutable lvalue reference to a functor") {

		optional<scope_exit<functor &>> e(make_scope_exit(f));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The functor (not a copy thereof) is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The functor (not a copy thereof) is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_fail factory called with an rvalue") {

		optional<scope_exit<functor>> e(make_scope_fail(std::move(f)));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is not invoked") {

				CHECK(!i);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_fail factory called with a const lvalue reference") {

		const auto & r=f;
		optional<scope_exit<functor>> e(make_scope_fail(r));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is not invoked") {

				CHECK(!i);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_fail factory called with a mutable lvalue reference to a functor") {

		optional<scope_exit<functor &>> e(make_scope_fail(f));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The functor is not invoked") {

				CHECK(!i);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The functor (not a copy thereof) is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_success factory called with an rvalue") {

		optional<scope_exit<functor>> e(make_scope_success(std::move(f)));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is not invoked") {

				CHECK(!i);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_success factory called with a const lvalue reference") {

		const auto & r=f;
		optional<scope_exit<functor>> e(make_scope_success(r));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is not invoked") {

				CHECK(!i);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_success factory called with a mutable lvalue reference to a functor") {

		optional<scope_exit<functor &>> e(make_scope_success(f));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The functor (not a copy thereof) is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The functor is not invoked") {

				CHECK(!i);

			}

		}

	}

}
<commit_msg>asiocurl::scope_exit Tests - Satisfy -Wunused-member-function on Clang<commit_after>#include <asiocurl/scope.hpp>

#include <asiocurl/optional.hpp>
#include <utility>
#include <catch.hpp>

namespace {

	class functor {

		private:

			bool * ptr_;
			bool i_;

		public:

			functor () = delete;
			functor (bool & b) noexcept : ptr_(&b), i_(false) {	}

			explicit operator bool () const noexcept {

				//	Determines whether this object has been
				//	invoked directly
				return i_;

			}

			void operator () () noexcept {

				i_=true;
				if (ptr_) *ptr_=true;

			}

	};

}

template <typename T>
static void fail (asiocurl::scope_exit<T> e) noexcept {

	try {

		asiocurl::scope_exit<T> i(std::move(e));

		throw 0;

	} catch (...) {	}

}

SCENARIO("scope_exit objects invoke their stored functor when they go out of scope unless they are moved or released","[asiocurl][scope_exit][scope]") {

	using asiocurl::in_place;
	using asiocurl::nullopt;
	using asiocurl::optional;
	using asiocurl::scope_exit;

	bool i=false;
	functor f(i);

	GIVEN("A scope_exit object which is not templated on a reference type") {

		optional<scope_exit<functor>> e(in_place,f);

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It is released") {

			e->release();

			AND_WHEN("It goes out of scope") {

				e=nullopt;

				THEN("The stored functor is not invoked") {

					CHECK(!i);

				}

			}

		}

		WHEN("It is moved") {

			optional<scope_exit<functor>> mt(in_place,std::move(*e));

			AND_WHEN("The moved from scope_exit object goes out of scope") {

				e=nullopt;

				THEN("The stored functor is not invoked") {

					CHECK(!i);

				}

				AND_WHEN("The moved to scope_exit object goes out of scope") {

					mt=nullopt;

					THEN("The stored functor is invoked") {

						CHECK(i);
						CHECK(!f);

					}

				}

			}

		}

	}

	GIVEN("A scope_exit object which is templated on a reference type") {

		optional<scope_exit<functor &>> e(in_place,f);

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The functor (not a copy thereof) is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The functor is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

		WHEN("It is released") {

			e->release();

			AND_WHEN("It goes out of scope") {

				e=nullopt;

				THEN("The functor is not invoked") {

					CHECK(!i);

				}

			}

		}

		WHEN("It is moved") {

			optional<scope_exit<functor &>> mt(in_place,std::move(*e));

			AND_WHEN("The moved from scope_exit object goes out of scope") {

				e=nullopt;

				THEN("The functor is not invoked") {

					CHECK(!i);

				}

				AND_WHEN("The moved to scope_exit object goes out of scope") {

					mt=nullopt;

					THEN("The functor (not a copy thereof) is invoked") {

						CHECK(i);
						CHECK(f);

					}

				}

			}

		}

	}

}

SCENARIO("scope_exit objects obtained from the make_scope_exit factory invoke their stored functor when they go out of scope","[asiocurl][scope_exit][scope]") {

	using asiocurl::make_scope_exit;
	using asiocurl::make_scope_fail;
	using asiocurl::make_scope_success;
	using asiocurl::nullopt;
	using asiocurl::scope_exit;
	using asiocurl::optional;

	bool i=false;
	functor f(i);

	GIVEN("A scope_exit object obtained from the make_scope_exit factory called with an rvalue") {

		optional<scope_exit<functor>> e(make_scope_exit(std::move(f)));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_exit factory called with a const lvalue reference") {

		const auto & r=f;
		optional<scope_exit<functor>> e(make_scope_exit(r));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_exit factory called with a mutable lvalue reference to a functor") {

		optional<scope_exit<functor &>> e(make_scope_exit(f));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The functor (not a copy thereof) is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The functor (not a copy thereof) is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_fail factory called with an rvalue") {

		optional<scope_exit<functor>> e(make_scope_fail(std::move(f)));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is not invoked") {

				CHECK(!i);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_fail factory called with a const lvalue reference") {

		const auto & r=f;
		optional<scope_exit<functor>> e(make_scope_fail(r));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is not invoked") {

				CHECK(!i);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_fail factory called with a mutable lvalue reference to a functor") {

		optional<scope_exit<functor &>> e(make_scope_fail(f));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The functor is not invoked") {

				CHECK(!i);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The functor (not a copy thereof) is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_success factory called with an rvalue") {

		optional<scope_exit<functor>> e(make_scope_success(std::move(f)));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is not invoked") {

				CHECK(!i);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_success factory called with a const lvalue reference") {

		const auto & r=f;
		optional<scope_exit<functor>> e(make_scope_success(r));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The stored functor is invoked") {

				CHECK(i);
				CHECK(!f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The stored functor is not invoked") {

				CHECK(!i);

			}

		}

	}

	GIVEN("A scope_exit object obtained from the make_scope_success factory called with a mutable lvalue reference to a functor") {

		optional<scope_exit<functor &>> e(make_scope_success(f));

		WHEN("It goes out of scope") {

			e=nullopt;

			THEN("The functor (not a copy thereof) is invoked") {

				CHECK(i);
				CHECK(f);

			}

		}

		WHEN("It goes out of scope due to an exception") {

			fail(std::move(*e));

			THEN("The functor is not invoked") {

				CHECK(!i);

			}

		}

	}

}
<|endoftext|>


<commit_before>/* mbed Microcontroller Library
 * Copyright (c) 2016 Realtek Semiconductor Corp.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#if defined(CONFIG_MBED_ENABLED)

#include <stdio.h>
#include "mbed_assert.h"
#include "mbed_events.h"

#include "rtw_emac.h"
#include "EMACMemoryManager.h"

#include "rtos.h"
#include "lwip/pbuf.h"
#include "netif/etharp.h"

#include "lwip_intf.h"
#include "wifi_constants.h"
#include "wifi_conf.h"

#define RTW_EMAC_MTU_SIZE  (1500U)

RTW_EMAC::RTW_EMAC()
{
    set_callback_func((emac_callback)(&RTW_EMAC::wlan_emac_recv), this);
}

uint32_t RTW_EMAC::get_mtu_size() const
{
    return RTW_EMAC_MTU_SIZE;
}

uint32_t RTW_EMAC::get_align_preference() const
{
    return true;
}

void RTW_EMAC::get_ifname(char *name, uint8_t size) const
{
    MBED_ASSERT(name != NULL);
    strncpy(name, "r0", size);
}

uint8_t RTW_EMAC::get_hwaddr_size() const
{
    return ETH_HWADDR_LEN;
}

bool RTW_EMAC::get_hwaddr(uint8_t *addr) const
{
    char mac[20];
    int val[6];
    int i;

    if (RTW_SUCCESS == wifi_get_mac_address(mac)) {
        if (sscanf(mac, "%x:%x:%x:%x:%x:%x",
            &val[0], &val[1], &val[2], &val[3], &val[4], &val[5]) != 6) {
            printf("Get HW address failed\r\n");
        }
        for (i = 0; i < 6; i++) {
            addr[i] = (unsigned char) val[i];
        }
    } else {
        printf("Get HW address failed\r\n");
    }
    return true;
}

void RTW_EMAC::set_hwaddr(const uint8_t *addr)
{
}

bool RTW_EMAC::link_out(emac_mem_buf_t *buf)
{
    struct eth_drv_sg *sg_list;
    int sg_len = 0;
    int tot_len;
    emac_mem_buf_t *p;
    bool ret = true;
    if (!rltk_wlan_running(0)) {
        memory_manager->free(buf);
        return false;
    }

    sg_list = (struct eth_drv_sg *)malloc(sizeof(struct eth_drv_sg)*MAX_ETH_DRV_SG);
    if (sg_list == 0) {
        memory_manager->free(buf);
        return false;
    }

    p = buf;
    tot_len = memory_manager->get_total_len(p);
    for (; p != NULL && sg_len < MAX_ETH_DRV_SG; p = memory_manager->get_next(p)) {
        sg_list[sg_len].buf = (unsigned int)(static_cast<uint8_t *>(memory_manager->get_ptr(p)));
        sg_list[sg_len].len = memory_manager->get_len(p);
        sg_len++;
    }
    if (sg_len) {
        if (rltk_wlan_send(0, sg_list, sg_len, tot_len) != 0) {
            ret = false;
        }
    }

    memory_manager->free(buf);
    free(sg_list);
    return ret;
}

bool RTW_EMAC::power_up()
{
    wifi_on(RTW_MODE_STA);
    wait_ms(1000);
    wlan_emac_link_change(true);
    return true;
}

void RTW_EMAC::power_down()
{
    wifi_off();
}

void RTW_EMAC::set_link_input_cb(emac_link_input_cb_t input_cb)
{
    emac_link_input_cb = input_cb;
}

void RTW_EMAC::set_link_state_cb(emac_link_state_change_cb_t state_cb)
{
    emac_link_state_cb = state_cb;
}

void RTW_EMAC::add_multicast_group(const uint8_t *addr)
{
}

void RTW_EMAC::remove_multicast_group(const uint8_t *addr)
{
}

void RTW_EMAC::set_all_multicast(bool all)
{
}

void RTW_EMAC::set_memory_manager(EMACMemoryManager &mem_mngr)
{
    memory_manager = &mem_mngr;
}

void RTW_EMAC::wlan_emac_recv(void *param, struct netif *netif, uint32_t len)
{
    struct eth_drv_sg sg_list[MAX_ETH_DRV_SG] = {0};
    emac_mem_buf_t *buf;
    RTW_EMAC *enet = static_cast<RTW_EMAC *>(param);
    emac_mem_buf_t *p;
    int sg_len = 0;
    if (!rltk_wlan_running(0)) {
        return;
    }

    if (len > MAX_ETH_MSG || len < 0) {
        len = MAX_ETH_MSG;
    }

    buf = enet->memory_manager->alloc_heap(len, 0);
    if (buf == NULL) {
        return;
    }

    enet->memory_manager->set_len(buf, len);
    p = buf;
    for (; p != NULL && sg_len < MAX_ETH_DRV_SG; p = enet->memory_manager->get_next(p)) {
        sg_list[sg_len].buf = (unsigned int)(static_cast<uint8_t *>(enet->memory_manager->get_ptr(p)));
        sg_list[sg_len].len = enet->memory_manager->get_len(p);
        sg_len++;
    }

    rltk_wlan_recv(0, sg_list, sg_len);
    if (enet->emac_link_input_cb) {
        enet->emac_link_input_cb(buf);
    }
}

void mbed_default_mac_address(char *mac) {
    unsigned char RTK_mac_addr[3] = {0x00, 0xE0, 0x4C}; // default Realtek mac address
    mac[0] = RTK_mac_addr[0];
    mac[1] = RTK_mac_addr[1];
    mac[2] = RTK_mac_addr[2];
    mac[3] = 0x87;
    mac[4] = 0x00;
    mac[5] = 0x01;
    return;
}

void mbed_mac_address(char *mac)
{
    char hwaddr[20];
    int val[6];
    int i;
    if (RTW_SUCCESS == wifi_get_mac_address(hwaddr)) {
        if (sscanf(hwaddr, "%x:%x:%x:%x:%x:%x",
            &val[0], &val[1], &val[2], &val[3], &val[4], &val[5]) != 6) {
            printf("Get HW address failed\r\n");
        }
        for (i = 0; i < 6; i++) {
            mac[i] = (unsigned char) val[i];
        }
    } else {
        printf("Get HW address failed\r\n");
        mbed_default_mac_address(mac);
    }
}

void RTW_EMAC::wlan_emac_link_change(bool up)
{
    if (emac_link_state_cb) {
        emac_link_state_cb(up);
    }
}

RTW_EMAC &RTW_EMAC::get_instance() {
    static RTW_EMAC rtw_emac;
    return rtw_emac;
}
// Weak so a module can override
MBED_WEAK EMAC &EMAC::get_default_instance() {
    return RTW_EMAC::get_instance();
}
#endif
<commit_msg>Fixed Realtek test build<commit_after>/* mbed Microcontroller Library
 * Copyright (c) 2016 Realtek Semiconductor Corp.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#if defined(CONFIG_MBED_ENABLED)

#include <stdio.h>
#include "mbed_assert.h"
#include "mbed_events.h"
#include "mbed_wait_api.h"

#include "rtw_emac.h"
#include "EMACMemoryManager.h"

#include "rtos.h"
#include "lwip/pbuf.h"
#include "netif/etharp.h"

#include "lwip_intf.h"
#include "wifi_constants.h"
#include "wifi_conf.h"

#define RTW_EMAC_MTU_SIZE  (1500U)

RTW_EMAC::RTW_EMAC()
{
    set_callback_func((emac_callback)(&RTW_EMAC::wlan_emac_recv), this);
}

uint32_t RTW_EMAC::get_mtu_size() const
{
    return RTW_EMAC_MTU_SIZE;
}

uint32_t RTW_EMAC::get_align_preference() const
{
    return true;
}

void RTW_EMAC::get_ifname(char *name, uint8_t size) const
{
    MBED_ASSERT(name != NULL);
    strncpy(name, "r0", size);
}

uint8_t RTW_EMAC::get_hwaddr_size() const
{
    return ETH_HWADDR_LEN;
}

bool RTW_EMAC::get_hwaddr(uint8_t *addr) const
{
    char mac[20];
    int val[6];
    int i;

    if (RTW_SUCCESS == wifi_get_mac_address(mac)) {
        if (sscanf(mac, "%x:%x:%x:%x:%x:%x",
            &val[0], &val[1], &val[2], &val[3], &val[4], &val[5]) != 6) {
            printf("Get HW address failed\r\n");
        }
        for (i = 0; i < 6; i++) {
            addr[i] = (unsigned char) val[i];
        }
    } else {
        printf("Get HW address failed\r\n");
    }
    return true;
}

void RTW_EMAC::set_hwaddr(const uint8_t *addr)
{
}

bool RTW_EMAC::link_out(emac_mem_buf_t *buf)
{
    struct eth_drv_sg *sg_list;
    int sg_len = 0;
    int tot_len;
    emac_mem_buf_t *p;
    bool ret = true;
    if (!rltk_wlan_running(0)) {
        memory_manager->free(buf);
        return false;
    }

    sg_list = (struct eth_drv_sg *)malloc(sizeof(struct eth_drv_sg)*MAX_ETH_DRV_SG);
    if (sg_list == 0) {
        memory_manager->free(buf);
        return false;
    }

    p = buf;
    tot_len = memory_manager->get_total_len(p);
    for (; p != NULL && sg_len < MAX_ETH_DRV_SG; p = memory_manager->get_next(p)) {
        sg_list[sg_len].buf = (unsigned int)(static_cast<uint8_t *>(memory_manager->get_ptr(p)));
        sg_list[sg_len].len = memory_manager->get_len(p);
        sg_len++;
    }
    if (sg_len) {
        if (rltk_wlan_send(0, sg_list, sg_len, tot_len) != 0) {
            ret = false;
        }
    }

    memory_manager->free(buf);
    free(sg_list);
    return ret;
}

bool RTW_EMAC::power_up()
{
    wifi_on(RTW_MODE_STA);
    wait_ms(1000);
    wlan_emac_link_change(true);
    return true;
}

void RTW_EMAC::power_down()
{
    wifi_off();
}

void RTW_EMAC::set_link_input_cb(emac_link_input_cb_t input_cb)
{
    emac_link_input_cb = input_cb;
}

void RTW_EMAC::set_link_state_cb(emac_link_state_change_cb_t state_cb)
{
    emac_link_state_cb = state_cb;
}

void RTW_EMAC::add_multicast_group(const uint8_t *addr)
{
}

void RTW_EMAC::remove_multicast_group(const uint8_t *addr)
{
}

void RTW_EMAC::set_all_multicast(bool all)
{
}

void RTW_EMAC::set_memory_manager(EMACMemoryManager &mem_mngr)
{
    memory_manager = &mem_mngr;
}

void RTW_EMAC::wlan_emac_recv(void *param, struct netif *netif, uint32_t len)
{
    struct eth_drv_sg sg_list[MAX_ETH_DRV_SG] = {0};
    emac_mem_buf_t *buf;
    RTW_EMAC *enet = static_cast<RTW_EMAC *>(param);
    emac_mem_buf_t *p;
    int sg_len = 0;
    if (!rltk_wlan_running(0)) {
        return;
    }

    if (len > MAX_ETH_MSG || len < 0) {
        len = MAX_ETH_MSG;
    }

    buf = enet->memory_manager->alloc_heap(len, 0);
    if (buf == NULL) {
        return;
    }

    enet->memory_manager->set_len(buf, len);
    p = buf;
    for (; p != NULL && sg_len < MAX_ETH_DRV_SG; p = enet->memory_manager->get_next(p)) {
        sg_list[sg_len].buf = (unsigned int)(static_cast<uint8_t *>(enet->memory_manager->get_ptr(p)));
        sg_list[sg_len].len = enet->memory_manager->get_len(p);
        sg_len++;
    }

    rltk_wlan_recv(0, sg_list, sg_len);
    if (enet->emac_link_input_cb) {
        enet->emac_link_input_cb(buf);
    }
}

void mbed_default_mac_address(char *mac) {
    unsigned char RTK_mac_addr[3] = {0x00, 0xE0, 0x4C}; // default Realtek mac address
    mac[0] = RTK_mac_addr[0];
    mac[1] = RTK_mac_addr[1];
    mac[2] = RTK_mac_addr[2];
    mac[3] = 0x87;
    mac[4] = 0x00;
    mac[5] = 0x01;
    return;
}

void mbed_mac_address(char *mac)
{
    char hwaddr[20];
    int val[6];
    int i;
    if (RTW_SUCCESS == wifi_get_mac_address(hwaddr)) {
        if (sscanf(hwaddr, "%x:%x:%x:%x:%x:%x",
            &val[0], &val[1], &val[2], &val[3], &val[4], &val[5]) != 6) {
            printf("Get HW address failed\r\n");
        }
        for (i = 0; i < 6; i++) {
            mac[i] = (unsigned char) val[i];
        }
    } else {
        printf("Get HW address failed\r\n");
        mbed_default_mac_address(mac);
    }
}

void RTW_EMAC::wlan_emac_link_change(bool up)
{
    if (emac_link_state_cb) {
        emac_link_state_cb(up);
    }
}

RTW_EMAC &RTW_EMAC::get_instance() {
    static RTW_EMAC rtw_emac;
    return rtw_emac;
}
// Weak so a module can override
MBED_WEAK EMAC &EMAC::get_default_instance() {
    return RTW_EMAC::get_instance();
}
#endif
<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *
 */
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
#include <pcl/cloud_iterator.h>

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             Matrix4 &transformation_matrix) const
{
  size_t nr_points = cloud_src.points.size ();
  if (cloud_tgt.points.size () != nr_points)
  {
    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ());
    return;
  }

  ConstCloudIterator<PointSource> source_it (cloud_src);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const std::vector<int> &indices_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             Matrix4 &transformation_matrix) const
{
  size_t nr_points = indices_src.size ();
  if (cloud_tgt.points.size () != nr_points)
  {
    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ());
    return;
  }

  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const std::vector<int> &indices_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             const std::vector<int> &indices_tgt,
                             Matrix4 &transformation_matrix) const
{
  size_t nr_points = indices_src.size ();
  if (indices_tgt.size () != nr_points)
  {
    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
    return;
  }

  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             const pcl::Correspondences &correspondences,
                             Matrix4 &transformation_matrix) const
{
  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
                               const double & tx,    const double & ty,   const double & tz,
                               Matrix4 &transformation_matrix) const
{

  // Construct the transformation matrix from rotation and translation
  transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  transformation_matrix (0, 0) =  static_cast<Scalar> (cos (gamma) * cos (beta));
  transformation_matrix (0, 1) = -static_cast<Scalar> (sin (gamma) * cos (alpha) + cos (gamma) * sin (beta) * sin (alpha));
  transformation_matrix (0, 2) =  static_cast<Scalar> (sin (gamma) * sin (alpha) + cos (gamma) * sin (beta) * cos (alpha));
  transformation_matrix (1, 0) =  static_cast<Scalar> (sin (gamma) * cos (beta));
  transformation_matrix (1, 1) =  static_cast<Scalar> (cos (gamma) * cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
  transformation_matrix (1, 2) = -static_cast<Scalar> (cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * cos (alpha));
  transformation_matrix (2, 0) = -static_cast<Scalar> (sin (beta));
  transformation_matrix (2, 1) =  static_cast<Scalar> (cos (beta) * sin (alpha));
  transformation_matrix (2, 2) =  static_cast<Scalar> (cos (beta) * cos (alpha));

  transformation_matrix (0, 3) = static_cast<Scalar> (tx);
  transformation_matrix (1, 3) = static_cast<Scalar> (ty);
  transformation_matrix (2, 3) = static_cast<Scalar> (tz);
  transformation_matrix (3, 3) = static_cast<Scalar> (1);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
{
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  typedef Eigen::Matrix<double, 6, 6> Matrix6d;

  Matrix6d ATA;
  Vector6d ATb;
  ATA.setZero ();
  ATb.setZero ();

  // Approximate as a linear least squares problem
  while (source_it.isValid () && target_it.isValid ())
  {
    if (!pcl_isfinite (source_it->x) ||
        !pcl_isfinite (source_it->y) ||
        !pcl_isfinite (source_it->z) ||
        !pcl_isfinite (source_it->normal_x) ||
        !pcl_isfinite (source_it->normal_y) ||
        !pcl_isfinite (source_it->normal_z) ||
        !pcl_isfinite (target_it->x) ||
        !pcl_isfinite (target_it->y) ||
        !pcl_isfinite (target_it->z) ||
        !pcl_isfinite (target_it->normal_x) ||
        !pcl_isfinite (target_it->normal_y) ||
        !pcl_isfinite (target_it->normal_z))
    {
      ++target_it;
      ++source_it;
      continue;
    }

    const float & sx = source_it->x;
    const float & sy = source_it->y;
    const float & sz = source_it->z;
    const float & dx = target_it->x;
    const float & dy = target_it->y;
    const float & dz = target_it->z;
    const float & nx = target_it->normal[0];
    const float & ny = target_it->normal[1];
    const float & nz = target_it->normal[2];

    double a = nz*sy - ny*sz;
    double b = nx*sz - nz*sx;
    double c = ny*sx - nx*sy;

    //    0  1  2  3  4  5
    //    6  7  8  9 10 11
    //   12 13 14 15 16 17
    //   18 19 20 21 22 23
    //   24 25 26 27 28 29
    //   30 31 32 33 34 35

    ATA.coeffRef (0) += a * a;
    ATA.coeffRef (1) += a * b;
    ATA.coeffRef (2) += a * c;
    ATA.coeffRef (3) += a * nx;
    ATA.coeffRef (4) += a * ny;
    ATA.coeffRef (5) += a * nz;
    ATA.coeffRef (7) += b * b;
    ATA.coeffRef (8) += b * c;
    ATA.coeffRef (9) += b * nx;
    ATA.coeffRef (10) += b * ny;
    ATA.coeffRef (11) += b * nz;
    ATA.coeffRef (14) += c * c;
    ATA.coeffRef (15) += c * nx;
    ATA.coeffRef (16) += c * ny;
    ATA.coeffRef (17) += c * nz;
    ATA.coeffRef (21) += nx * nx;
    ATA.coeffRef (22) += nx * ny;
    ATA.coeffRef (23) += nx * nz;
    ATA.coeffRef (28) += ny * ny;
    ATA.coeffRef (29) += ny * nz;
    ATA.coeffRef (35) += nz * nz;

    double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
    ATb.coeffRef (0) += a * d;
    ATb.coeffRef (1) += b * d;
    ATb.coeffRef (2) += c * d;
    ATb.coeffRef (3) += nx * d;
    ATb.coeffRef (4) += ny * d;
    ATb.coeffRef (5) += nz * d;

    ++target_it;
    ++source_it;
  }
  ATA.coeffRef (6) = ATA.coeff (1);
  ATA.coeffRef (12) = ATA.coeff (2);
  ATA.coeffRef (13) = ATA.coeff (8);
  ATA.coeffRef (18) = ATA.coeff (3);
  ATA.coeffRef (19) = ATA.coeff (9);
  ATA.coeffRef (20) = ATA.coeff (15);
  ATA.coeffRef (24) = ATA.coeff (4);
  ATA.coeffRef (25) = ATA.coeff (10);
  ATA.coeffRef (26) = ATA.coeff (16);
  ATA.coeffRef (27) = ATA.coeff (22);
  ATA.coeffRef (30) = ATA.coeff (5);
  ATA.coeffRef (31) = ATA.coeff (11);
  ATA.coeffRef (32) = ATA.coeff (17);
  ATA.coeffRef (33) = ATA.coeff (23);
  ATA.coeffRef (34) = ATA.coeff (29);

  // Solve A*x = b
  Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);

  // Construct the transformation matrix from x
  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */
<commit_msg>fixed bug, thanks Martin<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *
 */
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
#include <pcl/cloud_iterator.h>

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             Matrix4 &transformation_matrix) const
{
  size_t nr_points = cloud_src.points.size ();
  if (cloud_tgt.points.size () != nr_points)
  {
    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ());
    return;
  }

  ConstCloudIterator<PointSource> source_it (cloud_src);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const std::vector<int> &indices_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             Matrix4 &transformation_matrix) const
{
  size_t nr_points = indices_src.size ();
  if (cloud_tgt.points.size () != nr_points)
  {
    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ());
    return;
  }

  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const std::vector<int> &indices_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             const std::vector<int> &indices_tgt,
                             Matrix4 &transformation_matrix) const
{
  size_t nr_points = indices_src.size ();
  if (indices_tgt.size () != nr_points)
  {
    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
    return;
  }

  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             const pcl::Correspondences &correspondences,
                             Matrix4 &transformation_matrix) const
{
  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
                               const double & tx,    const double & ty,   const double & tz,
                               Matrix4 &transformation_matrix) const
{

  // Construct the transformation matrix from rotation and translation
  transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  transformation_matrix (0, 0) = static_cast<Scalar> ( cos (gamma) * cos (beta));
  transformation_matrix (0, 1) = static_cast<Scalar> (-sin (gamma) * cos (alpha) + cos (gamma) * sin (beta) * sin (alpha));
  transformation_matrix (0, 2) = static_cast<Scalar> ( sin (gamma) * sin (alpha) + cos (gamma) * sin (beta) * cos (alpha));
  transformation_matrix (1, 0) = static_cast<Scalar> ( sin (gamma) * cos (beta));
  transformation_matrix (1, 1) = static_cast<Scalar> ( cos (gamma) * cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
  transformation_matrix (1, 2) = static_cast<Scalar> (-cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * cos (alpha));
  transformation_matrix (2, 0) = static_cast<Scalar> (-sin (beta));
  transformation_matrix (2, 1) = static_cast<Scalar> ( cos (beta) * sin (alpha));
  transformation_matrix (2, 2) = static_cast<Scalar> ( cos (beta) * cos (alpha));

  transformation_matrix (0, 3) = static_cast<Scalar> (tx);
  transformation_matrix (1, 3) = static_cast<Scalar> (ty);
  transformation_matrix (2, 3) = static_cast<Scalar> (tz);
  transformation_matrix (3, 3) = static_cast<Scalar> (1);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
{
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  typedef Eigen::Matrix<double, 6, 6> Matrix6d;

  Matrix6d ATA;
  Vector6d ATb;
  ATA.setZero ();
  ATb.setZero ();

  // Approximate as a linear least squares problem
  while (source_it.isValid () && target_it.isValid ())
  {
    if (!pcl_isfinite (source_it->x) ||
        !pcl_isfinite (source_it->y) ||
        !pcl_isfinite (source_it->z) ||
        !pcl_isfinite (source_it->normal_x) ||
        !pcl_isfinite (source_it->normal_y) ||
        !pcl_isfinite (source_it->normal_z) ||
        !pcl_isfinite (target_it->x) ||
        !pcl_isfinite (target_it->y) ||
        !pcl_isfinite (target_it->z) ||
        !pcl_isfinite (target_it->normal_x) ||
        !pcl_isfinite (target_it->normal_y) ||
        !pcl_isfinite (target_it->normal_z))
    {
      ++target_it;
      ++source_it;
      continue;
    }

    const float & sx = source_it->x;
    const float & sy = source_it->y;
    const float & sz = source_it->z;
    const float & dx = target_it->x;
    const float & dy = target_it->y;
    const float & dz = target_it->z;
    const float & nx = target_it->normal[0];
    const float & ny = target_it->normal[1];
    const float & nz = target_it->normal[2];

    double a = nz*sy - ny*sz;
    double b = nx*sz - nz*sx;
    double c = ny*sx - nx*sy;

    //    0  1  2  3  4  5
    //    6  7  8  9 10 11
    //   12 13 14 15 16 17
    //   18 19 20 21 22 23
    //   24 25 26 27 28 29
    //   30 31 32 33 34 35

    ATA.coeffRef (0) += a * a;
    ATA.coeffRef (1) += a * b;
    ATA.coeffRef (2) += a * c;
    ATA.coeffRef (3) += a * nx;
    ATA.coeffRef (4) += a * ny;
    ATA.coeffRef (5) += a * nz;
    ATA.coeffRef (7) += b * b;
    ATA.coeffRef (8) += b * c;
    ATA.coeffRef (9) += b * nx;
    ATA.coeffRef (10) += b * ny;
    ATA.coeffRef (11) += b * nz;
    ATA.coeffRef (14) += c * c;
    ATA.coeffRef (15) += c * nx;
    ATA.coeffRef (16) += c * ny;
    ATA.coeffRef (17) += c * nz;
    ATA.coeffRef (21) += nx * nx;
    ATA.coeffRef (22) += nx * ny;
    ATA.coeffRef (23) += nx * nz;
    ATA.coeffRef (28) += ny * ny;
    ATA.coeffRef (29) += ny * nz;
    ATA.coeffRef (35) += nz * nz;

    double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
    ATb.coeffRef (0) += a * d;
    ATb.coeffRef (1) += b * d;
    ATb.coeffRef (2) += c * d;
    ATb.coeffRef (3) += nx * d;
    ATb.coeffRef (4) += ny * d;
    ATb.coeffRef (5) += nz * d;

    ++target_it;
    ++source_it;
  }
  ATA.coeffRef (6) = ATA.coeff (1);
  ATA.coeffRef (12) = ATA.coeff (2);
  ATA.coeffRef (13) = ATA.coeff (8);
  ATA.coeffRef (18) = ATA.coeff (3);
  ATA.coeffRef (19) = ATA.coeff (9);
  ATA.coeffRef (20) = ATA.coeff (15);
  ATA.coeffRef (24) = ATA.coeff (4);
  ATA.coeffRef (25) = ATA.coeff (10);
  ATA.coeffRef (26) = ATA.coeff (16);
  ATA.coeffRef (27) = ATA.coeff (22);
  ATA.coeffRef (30) = ATA.coeff (5);
  ATA.coeffRef (31) = ATA.coeff (11);
  ATA.coeffRef (32) = ATA.coeff (17);
  ATA.coeffRef (33) = ATA.coeff (23);
  ATA.coeffRef (34) = ATA.coeff (29);

  // Solve A*x = b
  Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);

  // Construct the transformation matrix from x
  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */
<|endoftext|>