//
// Created by joshua on 8/16/17.
//
#include <boost/format.hpp>
#include <boost/math/constants/constants.hpp>
#include <mps/planner/ompl/state/SimEnvState.h>
#include <mps/planner/util/Logging.h>
#include <mps/planner/util/Math.h>
#include <mps/planner/util/Serialize.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <sim_env/utils/EigenUtils.h>
#include <sstream>
#include <string>

#define CARTESIAN_GRADIENT_DELTA 0.01f
#define RADIANS_GRADIENT_DELTA 0.1f

namespace mps {
namespace planner {
    namespace ompl {
        namespace state {
            namespace internal {
                /**
                     * Create name for a state space for the specified dofs.
                     * @param dofs - degrees of freedom the state space is for
                     * @param type_name - type description of state space
                     * @return a name for the state space
                     */
                std::string createSpaceName(const std::vector<int>& dofs, const std::string& type_name)
                {
                    std::stringstream ss;
                    ss << "DOFs[";
                    for (auto& dof : dofs) {
                        ss << dof << ",";
                    }
                    ss << "]-" << type_name;
                    return ss.str();
                }
            }
        }
    }
}
}

typedef boost::format bf;
namespace mps_logging = mps::planner::util::logging;
using namespace mps::planner::ompl::state;
////////////////////////////////////////////////////////////////////////////////////////////
/////////////////// SimEnvObjectConfigurationSpace::StateType aka SimEnvConfiguration //////
////////////////////////////////////////////////////////////////////////////////////////////
SimEnvObjectConfigurationSpace::StateType::StateType(const std::vector<internal::SubstateTypeDescription>& desc,
    unsigned int dimension)
    : _state_descriptions(desc)
    , _dimension(dimension)
{
}

SimEnvObjectConfigurationSpace::StateType::~StateType()
{
}

Eigen::VectorXf SimEnvObjectConfigurationSpace::StateType::getConfiguration() const
{
    Eigen::VectorXf config;
    getConfiguration(config);
    return config;
}

void SimEnvObjectConfigurationSpace::StateType::getConfiguration(Eigen::VectorXf& config) const
{
    config.resize(_dimension);
    unsigned int dof_idx = 0;
    for (size_t space_idx = 0; space_idx < _state_descriptions.size(); ++space_idx) {
        switch (_state_descriptions.at(space_idx).type) {
        case internal::SubspaceType::RealVector: {
            auto* real_vector_state = static_cast<::ompl::base::RealVectorStateSpace::StateType*>(components[space_idx]);
            for (unsigned int i = 0; i < _state_descriptions.at(space_idx).dim; ++i) {
                config[dof_idx] = (float)real_vector_state->values[i];
                dof_idx++;
            }
            break;
        }
        case internal::SubspaceType::SO3: {
            auto* so3_state = static_cast<::ompl::base::SO3StateSpace::StateType*>(components[space_idx]);
            Eigen::Vector3f euler;
            Eigen::Quaternionf quat((const float&)so3_state->w, (const float&)so3_state->x,
                (const float&)so3_state->y, (const float&)so3_state->z);
            sim_env::utils::eigen::quaternionToEuler(quat, euler);
            config[dof_idx++] = euler[0];
            config[dof_idx++] = euler[1];
            config[dof_idx++] = euler[2];
            break;
        }
        case internal::SubspaceType ::SO2: {
            auto* so2_state = static_cast<::ompl::base::SO2StateSpace::StateType*>(components[space_idx]);
            config[dof_idx++] = (float)so2_state->value;
            break;
        }
        }
    }
}

void SimEnvObjectConfigurationSpace::StateType::setConfiguration(const Eigen::VectorXf& config)
{
    unsigned int dof_idx = 0;
    for (size_t space_idx = 0; space_idx < _state_descriptions.size(); ++space_idx) {
        switch (_state_descriptions.at(space_idx).type) {
        case internal::SubspaceType::RealVector: {
            auto* real_vector_state = static_cast<::ompl::base::RealVectorStateSpace::StateType*>(components[space_idx]);
            for (unsigned int i = 0; i < _state_descriptions.at(space_idx).dim; ++i) {
                real_vector_state->values[i] = (double)config[dof_idx++];
            }
            break;
        }
        case internal::SubspaceType::SO3: {
            auto* so3_state = static_cast<::ompl::base::SO3StateSpace::StateType*>(components[space_idx]);
            Eigen::Quaternionf quat;
            sim_env::utils::eigen::eulerToQuaternion(config[dof_idx],
                config[dof_idx + 1],
                config[dof_idx + 2],
                quat);
            dof_idx += 3;
            so3_state->w = quat.w();
            so3_state->x = quat.x();
            so3_state->y = quat.y();
            so3_state->z = quat.z();
            break;
        }
        case internal::SubspaceType ::SO2: {
            auto* so2_state = static_cast<::ompl::base::SO2StateSpace::StateType*>(components[space_idx]);
            so2_state->value = (double)config[dof_idx++];
            util::math::normalize_orientation(so2_state->value);
            break;
        }
        }
    }
}

void SimEnvObjectConfigurationSpace::StateType::serializeInNumbers(std::ostream& ostream) const
{
    ostream << getConfiguration().transpose().format(eigen_format);
}

void SimEnvObjectConfigurationSpace::StateType::deserializeFromNumbers(std::istream& istream)
{
    auto config = getConfiguration();
    for (unsigned int i = 0; i < _dimension; ++i) {
        std::string next_number;
        std::getline(istream, next_number, ',');
        config[i] = std::stof(next_number);
    }
    setConfiguration(config);
}

unsigned int SimEnvObjectConfigurationSpace::StateType::getNumNumbers() const
{
    return _dimension;
}

std::string SimEnvObjectConfigurationSpace::StateType::toString() const
{
    std::stringstream ss;
    serializeInNumbers(ss);
    return ss.str();
}

////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////// SimEnvObjectConfigurationSpace ////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
SimEnvObjectConfigurationSpace::SimEnvObjectConfigurationSpace(sim_env::ObjectConstPtr object,
    const Eigen::MatrixX2f& position_limits,
    double pos_weight,
    double orientation_weight,
    double joint_weight)
    : _object(object)
    , _active_dofs(object->getActiveDOFs())
    , _position_weight(pos_weight)
    , _orientation_weight(orientation_weight)
    , _joint_weight(joint_weight)
    , _log_prefix("[mps::planner::ompl::state::SimEnvObjectConfigurationSpace::")
{
    static const std::string log_prefix(_log_prefix + "SimEnvObjectConfigurationSpace]");
    assert(_active_dofs.size() > 0);
    _gradient_deltas.resize(_active_dofs.size());
    setName("sim_env::Object(" + object->getName() + ")-ConfigurationSpace");
    _limits = position_limits;
    if (_limits.rows() == 0) {
        _limits = object->getDOFPositionLimits();
    }
    addPoseSubspace(object);
    addJointsSubspace(object);
    lock();
    mps_logging::logDebug(bf("Created configurations space for object %s") % object->getName(),
        log_prefix);
    mps_logging::logDebug(bf("The dimension of this space is %i") % getDimension(),
        log_prefix);
    mps_logging::logDebug(bf("The bounds are\n %d") % _limits,
        log_prefix);
}

SimEnvObjectConfigurationSpace::~SimEnvObjectConfigurationSpace() = default;

::ompl::base::State* SimEnvObjectConfigurationSpace::allocState() const
{
    SimEnvObjectConfiguration* configuration = new SimEnvObjectConfiguration(_state_descriptions,
        _active_dofs.size());
    allocStateComponents(configuration);
    return configuration;
}

void SimEnvObjectConfigurationSpace::freeState(::ompl::base::State* state) const
{
    SimEnvObjectConfiguration* configuration = static_cast<SimEnvObjectConfiguration*>(state);
    for (unsigned int i = 0; i < componentCount_; ++i) {
        components_[i]->freeState(configuration->components[i]);
    }
    delete[] configuration->components;
    delete configuration;
}

void SimEnvObjectConfigurationSpace::computeDirection(StateType const* state_1,
    StateType const* state_2,
    Eigen::VectorXf& dir) const
{
    Eigen::VectorXf a = state_1->getConfiguration();
    Eigen::VectorXf b = state_2->getConfiguration();
    computeDirection(a, b, dir);
}

void SimEnvObjectConfigurationSpace::computeDirection(const Eigen::VectorXf& config_1,
    const Eigen::VectorXf& config_2,
    Eigen::VectorXf& dir) const
{
    static const std::string log_prefix("[mps::planner::ompl::state::SimEnvObjectConfigurationSpace::computeDirection]");
    mps_logging::logDebug(boost::format("Computing direction from configuration %1% to configuration %2%") % config_1.transpose() % config_2.transpose(), log_prefix);
    assert(config_1.size() == config_2.size());
    dir.resize(config_1.size());
    unsigned int c_idx = 0;
    for (const auto& space_desc : _state_descriptions) {
        switch (space_desc.type) {
        case internal::SubspaceType::RealVector: {
            dir.segment(c_idx, space_desc.dim) = config_2.segment(c_idx, space_desc.dim)
                - config_1.segment(c_idx, space_desc.dim);
            break;
        }
        case internal::SubspaceType::SO2: {
            dir[c_idx] = mps::planner::util::math::shortest_direction_so2(config_1[c_idx], config_2[c_idx]);
            break;
        }
        case internal::SubspaceType::SO3: {
            for (unsigned int i = 0; i < space_desc.dim; ++i) {
                dir[c_idx + i] = mps::planner::util::math::shortest_direction_so2(
                    config_1[c_idx + i], config_2[c_idx + i]);
            }
            break;
        }
        }
        c_idx += space_desc.dim;
    }
    mps_logging::logDebug(boost::format("Computed direction is %1%") % dir.transpose(), log_prefix);
}

Eigen::VectorXf SimEnvObjectConfigurationSpace::getGradientDeltas() const
{
    return _gradient_deltas;
}

void SimEnvObjectConfigurationSpace::addPoseSubspace(sim_env::ObjectConstPtr object)
{
    if (object->isStatic()) { // no pose subspace to add
        return;
    }
    // if the object is not static we need to distinguish two different cases
    // either we are in a 2d world (getNumBaseDOFs == 3) or in a 3d world (getNumBaseDOFs == 6)
    if (object->getNumBaseDOFs() == 3) { // we are in a 2d world
        addPoseSubspace2D(object);
    } else {
        assert(object->getNumBaseDOFs() == 6); // we are in a 3d world
        addPoseSubspace3D(object);
    }
}

void SimEnvObjectConfigurationSpace::addPoseSubspace2D(sim_env::ObjectConstPtr object)
{
    // let's first create a helper vector for which idx_helper[i] = _active_dofs[i] if i < _active_dofs.size(),
    // else idx_helper[i] = -1 for i = 0 .. 2
    Eigen::VectorXi idx_helper(3);
    for (unsigned int i = 0; i < idx_helper.size(); ++i) {
        idx_helper[i] = _active_dofs.size() > i ? _active_dofs[i] : -1;
    }
    if (idx_helper[0] == 0 and idx_helper[1] == 1) {
        // we have both x and y, so let's add a 2d real vector space
        auto pos_state_space = std::make_shared<::ompl::base::RealVectorStateSpace>(2);
        pos_state_space->setBounds(makeBounds({ 0, 1 }));
        pos_state_space->setName(internal::createSpaceName({ 0, 1 }, "ConfigurationSpace"));
        addSubspace(pos_state_space, _position_weight);
        _gradient_deltas[0] = CARTESIAN_GRADIENT_DELTA;
        _gradient_deltas[1] = CARTESIAN_GRADIENT_DELTA;
        internal::SubstateTypeDescription desc(internal::SubspaceType::RealVector, 2);
        _state_descriptions.push_back(desc);
        addSingleBasePoseSpace2D(idx_helper[2], 2); // and maybe also SO
    } else {
        // else we can treat each potential base dof independently
        addSingleBasePoseSpace2D(idx_helper[0], 0); // might be position or orientation
        addSingleBasePoseSpace2D(idx_helper[1], 1); // might be orientation
    }
}

void SimEnvObjectConfigurationSpace::addPoseSubspace3D(sim_env::ObjectConstPtr object)
{
    // let's first create a helper vector for which idx_helper[i] = _active_dofs[i] if i < _active_dofs.size(),
    // else idx_helper[i] = -1 for i = 0 .. 5
    Eigen::VectorXi idx_helper(6);
    for (unsigned int i = 0; i < idx_helper.size(); ++i) {
        idx_helper[i] = _active_dofs.size() > i ? _active_dofs[i] : -1;
    }
    unsigned int rdio = 0; // rotation dof index offset

    if (idx_helper[0] == 0 and idx_helper[1] == 1 and idx_helper[2] == 2) {
        // all position dimensions are part of the configspace
        auto pos_state_space = std::make_shared<::ompl::base::RealVectorStateSpace>(3);
        pos_state_space->setName(internal::createSpaceName({ 0, 1, 2 }, "ConfigurationSpace"));
        pos_state_space->setBounds(makeBounds({ 0, 1, 2 }));
        addSubspace(pos_state_space, _position_weight);
        _gradient_deltas[0] = CARTESIAN_GRADIENT_DELTA;
        _gradient_deltas[1] = CARTESIAN_GRADIENT_DELTA;
        _gradient_deltas[2] = CARTESIAN_GRADIENT_DELTA;
        internal::SubstateTypeDescription desc(internal::SubspaceType::RealVector, 3);
        _state_descriptions.push_back(desc);
        rdio = 3;
    } else if ((idx_helper[0] == 0 and idx_helper[1] == 1) or (idx_helper[0] == 0 and idx_helper[1] == 2) or (idx_helper[0] == 1 and idx_helper[1] == 2)) {
        // only 2 position dimensions are part of the configspace
        auto pos_state_space = std::make_shared<::ompl::base::RealVectorStateSpace>(2);
        pos_state_space->setName(internal::createSpaceName({ idx_helper[0], idx_helper[1] }, "ConfigurationSpace"));
        pos_state_space->setBounds(makeBounds({ 0, 1 }));
        _gradient_deltas[0] = CARTESIAN_GRADIENT_DELTA;
        _gradient_deltas[1] = CARTESIAN_GRADIENT_DELTA;
        addSubspace(pos_state_space, _position_weight);
        internal::SubstateTypeDescription desc(internal::SubspaceType::RealVector, 2);
        _state_descriptions.push_back(desc);
        rdio = 2;
    } else {
        // at most one dimension is part of the config space (active_dofs should be sorted)
        addSingleBasePoseSpace3D(idx_helper[0], 0);
        rdio = (unsigned int)(idx_helper[0] == 0 || idx_helper[0] == 1 || idx_helper[0] == 2);
    }
    // now do a similiar thing for the orientation
    if (idx_helper[rdio] == 3 and idx_helper[rdio + 1] == 4 and idx_helper[rdio + 2] == 5) {
        // we have all rotational degrees of freedom
        ::ompl::base::StateSpacePtr orientation_state_space = std::make_shared<::ompl::base::SO3StateSpace>();
        orientation_state_space->setName(internal::createSpaceName({ 3, 4, 5 }, "ConfigurationSpace"));
        _gradient_deltas[rdio] = RADIANS_GRADIENT_DELTA;
        _gradient_deltas[rdio + 1] = RADIANS_GRADIENT_DELTA;
        _gradient_deltas[rdio + 2] = RADIANS_GRADIENT_DELTA;
        addSubspace(orientation_state_space, _orientation_weight);
        internal::SubstateTypeDescription desc(internal::SubspaceType::SO3, 3);
        _state_descriptions.push_back(desc);
    } else {
        // we have at most 2 rotational dofs
        // TODO in case we still have two rotational degrees of freedom, adding them as separate spaces
        // TODO might have undesirable consequences for the distance function
        addSingleBasePoseSpace3D(idx_helper[rdio], rdio);
        addSingleBasePoseSpace3D(idx_helper[rdio + 1], rdio + 1);
    }
}

void SimEnvObjectConfigurationSpace::addJointsSubspace(sim_env::ObjectConstPtr object)
{
    std::vector<int> joint_idx;
    std::vector<unsigned int> indices;
    for (unsigned int i = 0; i < _active_dofs.size(); ++i) {
        int dof = _active_dofs[i];
        if (dof >= object->getNumBaseDOFs()) {
            joint_idx.push_back(dof);
            indices.push_back(i);
            _gradient_deltas[i] = RADIANS_GRADIENT_DELTA;
        }
    }
    if (joint_idx.empty())
        return;
    auto space = std::make_shared<::ompl::base::RealVectorStateSpace>(joint_idx.size());
    space->setName(internal::createSpaceName(joint_idx, "ConfigurationSpace"));
    space->setBounds(makeBounds(indices));
    addSubspace(space, _joint_weight);
    _state_descriptions.emplace_back(internal::SubstateTypeDescription(internal::SubspaceType::RealVector,
        (unsigned int)joint_idx.size()));
}

void SimEnvObjectConfigurationSpace::addSingleBasePoseSpace2D(int dof, unsigned int idx)
{
    ::ompl::base::StateSpacePtr space;
    internal::SubstateTypeDescription type_desc;
    type_desc.dim = 1;
    double weight = 1.0;
    if (dof == 0 or dof == 1) { // check whether its a base position
        assert(_active_dofs[idx] == dof);
        auto real_space = std::make_shared<::ompl::base::RealVectorStateSpace>(1);
        real_space->setBounds(makeBounds({ idx }));
        space = real_space;
        type_desc.type = internal::SubspaceType::RealVector;
        weight = _position_weight;
        _gradient_deltas[idx] = CARTESIAN_GRADIENT_DELTA;
    } else if (dof == 2) { // or base orientation
        assert(_active_dofs[idx] == dof);
        space = std::make_shared<::ompl::base::SO2StateSpace>();
        weight = _orientation_weight;
        type_desc.type = internal::SubspaceType::SO2;
        _gradient_deltas[idx] = RADIANS_GRADIENT_DELTA;
    } else {
        return; // or some other odf, then we don't need to add it
    }
    space->setName(internal::createSpaceName({ dof }, "ConfigurationSpace"));
    addSubspace(space, weight);
    _state_descriptions.push_back(type_desc);
}

void SimEnvObjectConfigurationSpace::addSingleBasePoseSpace3D(int dof, unsigned int idx)
{
    ::ompl::base::StateSpacePtr space;
    double weight = 1.0;
    internal::SubstateTypeDescription type_desc;
    type_desc.dim = 1;
    if (dof == 0 or dof == 1 or dof == 2) { // check whether it's a base position
        assert(_active_dofs[idx] == dof);
        auto real_space = std::make_shared<::ompl::base::RealVectorStateSpace>(1);
        real_space->setBounds(makeBounds({ idx }));
        space = real_space;
        weight = _position_weight;
        type_desc.type = internal::SubspaceType::RealVector;
        _gradient_deltas[idx] = CARTESIAN_GRADIENT_DELTA;
    } else if (dof == 3 or dof == 4 or dof == 5) { // else it's a base orientation
        assert(_active_dofs[idx] == dof);
        space = std::make_shared<::ompl::base::SO2StateSpace>();
        weight = _orientation_weight;
        type_desc.type = internal::SubspaceType::SO2;
        _gradient_deltas[idx] = RADIANS_GRADIENT_DELTA;
    } else {
        return; // do not proceed further
    }
    space->setName(internal::createSpaceName({ dof }, "ConfigurationSpace"));
    addSubspace(space, weight);
    _state_descriptions.push_back(type_desc);
}

sim_env::LoggerConstPtr SimEnvObjectConfigurationSpace::getLogger() const
{
    auto object = _object.lock();
    if (!object) {
        throw std::logic_error("[mps::planner::ompl::state::SimEnvObjectConfigurationSpace::getLogger]"
                               "Could not acquire access to sim_env::Object. This state space should not exist anymore.");
    }
    return object->getConstWorld()->getConstLogger();
}

::ompl::base::RealVectorBounds SimEnvObjectConfigurationSpace::makeBounds(const std::vector<unsigned int>& indices)
{
    unsigned int dim = (unsigned int)indices.size();
    ::ompl::base::RealVectorBounds bounds(dim);
    for (unsigned int i = 0; i < dim; ++i) {
        bounds.setLow(i, _limits(indices[i], 0));
        bounds.setHigh(i, _limits(indices[i], 1));
    }
    return bounds;
}

////////////////////////////////////////////////////////////////////////////////////////
/////////////////// SimEnvObjectVelocity aka SimEnvObjectVelocitySpace::StateType //////
////////////////////////////////////////////////////////////////////////////////////////
SimEnvObjectVelocitySpace::StateType::StateType(const std::vector<internal::SubstateTypeDescription>& desc,
    unsigned int dimension)
    : _state_descriptions(desc)
    , _dimension(dimension)
{
}

SimEnvObjectVelocitySpace::StateType::~StateType()
{
}

Eigen::VectorXf SimEnvObjectVelocitySpace::StateType::getVelocity() const
{
    Eigen::VectorXf vel;
    getVelocity(vel);
    return vel;
}

void SimEnvObjectVelocitySpace::StateType::getVelocity(Eigen::VectorXf& vel) const
{
    vel.resize(_dimension);
    unsigned int dof_idx = 0;
    for (size_t space_idx = 0; space_idx < _state_descriptions.size(); ++space_idx) {
        assert(_state_descriptions.at(space_idx).type == internal::SubspaceType::RealVector);
        ::ompl::base::RealVectorStateSpace::StateType* real_vector_state = static_cast<::ompl::base::RealVectorStateSpace::StateType*>(components[space_idx]);
        for (unsigned int i = 0; i < _state_descriptions.at(space_idx).dim; ++i) {
            vel[dof_idx] = (float)real_vector_state->values[i];
            dof_idx++;
        }
    }
}

void SimEnvObjectVelocitySpace::StateType::setVelocity(const Eigen::VectorXf& vel)
{
    unsigned int dof_idx = 0;
    for (size_t space_idx = 0; space_idx < _state_descriptions.size(); ++space_idx) {
        assert(_state_descriptions.at(space_idx).type == internal::SubspaceType::RealVector);
        ::ompl::base::RealVectorStateSpace::StateType* real_vector_state = static_cast<::ompl::base::RealVectorStateSpace::StateType*>(components[space_idx]);
        for (unsigned int i = 0; i < _state_descriptions.at(space_idx).dim; ++i) {
            real_vector_state->values[i] = vel[dof_idx];
            dof_idx++;
        }
    }
    assert(dof_idx == _dimension - 1);
}

void SimEnvObjectVelocitySpace::StateType::serializeInNumbers(std::ostream& ostream) const
{
    ostream << getVelocity().transpose().format(eigen_format);
}

void SimEnvObjectVelocitySpace::StateType::deserializeFromNumbers(std::istream& istream)
{
    auto vel = getVelocity();
    for (unsigned int i = 0; i < _dimension; ++i) {
        std::string next_number;
        std::getline(istream, next_number, ',');
        vel[i] = std::stof(next_number);
    }
    setVelocity(vel);
}

unsigned int SimEnvObjectVelocitySpace::StateType::getNumNumbers() const
{
    return _dimension;
}

std::string SimEnvObjectVelocitySpace::StateType::toString() const
{
    std::stringstream ss;
    serializeInNumbers(ss);
    return ss.str();
}

////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////// SimEnvObjectVelocitySpace ////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
SimEnvObjectVelocitySpace::SimEnvObjectVelocitySpace(sim_env::ObjectConstPtr object,
    const Eigen::MatrixX2f& velocity_limits,
    double pos_weight,
    double orientation_weight,
    double joint_weight)
    : _object(object)
    , _active_dofs(object->getActiveDOFs())
    , _position_vel_weight(pos_weight)
    , _orientation_vel_weight(orientation_weight)
    , _joint_vel_weight(joint_weight)
    , _log_prefix("[mps::planner::ompl::state::SimEnvObjectVelocitySpace::")
{
    static const std::string log_prefix(_log_prefix + "SimEnvObjectVelocitySpace]");
    assert(_active_dofs.size() > 0);
    setName("sim_env::Object(" + object->getName() + ")-VelocitySpace");
    if (velocity_limits.rows() == 0) {
        _limits = object->getDOFVelocityLimits();
    } else {
        _limits = velocity_limits;
    }
    addPoseVelocitySubspace(object);
    addJointsVelocitySubspace(object);
    lock();
    mps_logging::logDebug(bf("Created velocity space for object %s") % object->getName(),
        log_prefix);
    mps_logging::logDebug(bf("The dimension of this space is %i") % getDimension(),
        log_prefix);
    mps_logging::logDebug(bf("The bounds are\n %d") % _limits,
        log_prefix);
}

SimEnvObjectVelocitySpace::~SimEnvObjectVelocitySpace() = default;

::ompl::base::State* SimEnvObjectVelocitySpace::allocState() const
{
    auto* velocity = new SimEnvObjectVelocity(_state_descriptions, _active_dofs.size());
    allocStateComponents(velocity);
    return velocity;
}

void SimEnvObjectVelocitySpace::freeState(::ompl::base::State* state) const
{
    auto* velocity = static_cast<SimEnvObjectVelocity*>(state);
    for (unsigned int i = 0; i < componentCount_; ++i) {
        components_[i]->freeState(velocity->components[i]);
    }
    delete[] velocity->components;
    delete velocity;
}

void SimEnvObjectVelocitySpace::computeDirection(const StateType* state_a,
    const StateType* state_b,
    Eigen::VectorXf& dir) const
{
    static const std::string log_prefix("[mps::planner::ompl::state::SimEnvObjectVelocitySpace::computeDirection]");
    mps_logging::logDebug("Computing direction from state " + state_a->toString() + " to state " + state_b->toString(), log_prefix);
    Eigen::VectorXf vel_a;
    state_a->getVelocity(vel_a);
    Eigen::VectorXf vel_b;
    state_b->getVelocity(vel_b);
    dir = vel_b - vel_a;
    mps_logging::logDebug(boost::format("Computed direction is %1%") % dir.transpose(), log_prefix);
}

void SimEnvObjectVelocitySpace::addPoseVelocitySubspace(sim_env::ObjectConstPtr object)
{
    if (object->isStatic()) { // no pose velocity subspace to add
        return;
    }
    std::vector<int> position_dofs;
    std::vector<unsigned int> position_dof_indices;
    std::vector<int> orientation_dofs;
    std::vector<unsigned int> orientation_dof_indices;
    // 2d world
    int max_num_position_dofs = 2;
    int max_num_orientation_dofs = 1;
    if (object->getNumBaseDOFs() == 6) { // 3d world
        max_num_position_dofs = 3;
        max_num_orientation_dofs = 3;
    }
    // collect indices
    for (unsigned int i = 0; i < _active_dofs.size(); ++i) {
        if (_active_dofs[i] < max_num_position_dofs) {
            position_dofs.push_back(_active_dofs[i]);
            position_dof_indices.push_back(i);
        }
        if (max_num_position_dofs < _active_dofs[i] and _active_dofs[i] < max_num_position_dofs + max_num_orientation_dofs) {
            orientation_dofs.push_back(_active_dofs[i]);
            orientation_dof_indices.push_back(i);
        }
    }
    if (!position_dofs.empty()) {
        // TODO we shouldn't use RealVector spaces here (they are box shaped).
        // TODO Should use a ball shaped space
        auto velocity_space = std::make_shared<::ompl::base::RealVectorStateSpace>(position_dofs.size());
        velocity_space->setName(internal::createSpaceName(position_dofs, "VelocitySpace"));
        velocity_space->setBounds(makeBounds(position_dof_indices));
        addSubspace(velocity_space, _position_vel_weight);
        _state_descriptions.emplace_back(internal::SubstateTypeDescription(internal::SubspaceType::RealVector,
            (unsigned int)position_dofs.size()));
    }
    if (!orientation_dofs.empty()) {
        // TODO we shouldn't use RealVector spaces here (they are box shaped).
        // TODO Should use a ball shaped space
        auto velocity_space = std::make_shared<::ompl::base::RealVectorStateSpace>(orientation_dofs.size());
        velocity_space->setName(internal::createSpaceName(orientation_dofs, "VelocitySpace"));
        velocity_space->setBounds(makeBounds(orientation_dof_indices));
        addSubspace(velocity_space, _orientation_vel_weight);
        _state_descriptions.emplace_back(internal::SubstateTypeDescription(internal::SubspaceType::RealVector,
            (unsigned int)orientation_dofs.size()));
    }
}

void SimEnvObjectVelocitySpace::addJointsVelocitySubspace(sim_env::ObjectConstPtr object)
{
    std::vector<int> joint_idx;
    std::vector<unsigned int> indices;
    for (unsigned int i = 0; i < _active_dofs.size(); ++i) {
        int dof = _active_dofs[i];
        if (dof >= object->getNumBaseDOFs()) {
            joint_idx.push_back(dof);
            indices.push_back(i);
        }
    }
    if (joint_idx.empty())
        return;
    auto space = std::make_shared<::ompl::base::RealVectorStateSpace>(joint_idx.size());
    space->setName(internal::createSpaceName(joint_idx, "VelocitySpace"));
    space->setBounds(makeBounds(indices));
    addSubspace(space, _joint_vel_weight);
    _state_descriptions.emplace_back(internal::SubstateTypeDescription(internal::SubspaceType::RealVector,
        (unsigned int)joint_idx.size()));
}

sim_env::LoggerConstPtr SimEnvObjectVelocitySpace::getLogger() const
{
    auto object = _object.lock();
    if (!object) {
        throw std::logic_error("[mps::planner::ompl::state::SimEnvObjectVelocitySpace::getLogger]"
                               "Could not acquire access to sim_env::Object. This state space should not exist anymore.");
    }
    return object->getConstWorld()->getConstLogger();
}

::ompl::base::RealVectorBounds SimEnvObjectVelocitySpace::makeBounds(const std::vector<unsigned int>& indices)
{
    unsigned int dim = (unsigned int)indices.size();
    ::ompl::base::RealVectorBounds bounds(dim);
    for (unsigned int i = 0; i < dim; ++i) {
        bounds.setLow(i, _limits(indices[i], 0));
        bounds.setHigh(i, _limits(indices[i], 1));
    }
    return bounds;
}

////////////////////////////////////////////////////////////////////////////////////////
/////////////////// SimEnvObjectState aka SimEnvObjectState::StateType /////////////////
////////////////////////////////////////////////////////////////////////////////////////
SimEnvObjectStateSpace::StateType::StateType(int num_dofs, bool configuration_only)
    : _configuration_only(configuration_only)
    , _num_dofs(num_dofs)
{
}

SimEnvObjectStateSpace::StateType::~StateType()
{
}

Eigen::VectorXf SimEnvObjectStateSpace::StateType::getConfiguration() const
{
    Eigen::VectorXf config;
    getConfiguration(config);
    return config;
}

void SimEnvObjectStateSpace::StateType::getConfiguration(Eigen::VectorXf& config) const
{
    auto* config_component = components[0]->as<SimEnvObjectConfiguration>();
    config_component->getConfiguration(config);
}

SimEnvObjectConfiguration* SimEnvObjectStateSpace::StateType::getConfigurationState() const
{
    return components[0]->as<SimEnvObjectConfiguration>();
}

void SimEnvObjectStateSpace::StateType::setConfiguration(const Eigen::VectorXf& config)
{
    auto* config_component = components[0]->as<SimEnvObjectConfiguration>();
    config_component->setConfiguration(config);
}

bool SimEnvObjectStateSpace::StateType::hasVelocity() const
{
    return not _configuration_only;
}

Eigen::VectorXf SimEnvObjectStateSpace::StateType::getVelocity() const
{
    Eigen::VectorXf vel;
    getVelocity(vel);
    return vel;
}

void SimEnvObjectStateSpace::StateType::getVelocity(Eigen::VectorXf& vel) const
{
    if (not hasVelocity()) {
        vel.resize(_num_dofs);
        vel.setZero();
        return;
    }
    auto* vel_component = components[1]->as<SimEnvObjectVelocity>();
    vel_component->getVelocity(vel);
}

SimEnvObjectVelocity* SimEnvObjectStateSpace::StateType::getVelocityState() const
{
    if (hasVelocity()) {
        return components[1]->as<SimEnvObjectVelocity>();
    }
    return nullptr;
}

void SimEnvObjectStateSpace::StateType::setVelocity(const Eigen::VectorXf& vel)
{
    if (not hasVelocity()) {
        return;
    }
    auto* vel_component = components[1]->as<SimEnvObjectVelocity>();
    vel_component->setVelocity(vel);
}

void SimEnvObjectStateSpace::StateType::serializeInNumbers(std::ostream& ostream) const
{
    auto* serializable_component = dynamic_cast<util::serialize::RealValueSerializable*>(components[0]);
    serializable_component->serializeInNumbers(ostream);
    if (hasVelocity()) {
        ostream << ", ";
        serializable_component = dynamic_cast<util::serialize::RealValueSerializable*>(components[1]);
        serializable_component->serializeInNumbers(ostream);
    }
}

void SimEnvObjectStateSpace::StateType::deserializeFromNumbers(std::istream& istream)
{
    auto* serializable_component = dynamic_cast<util::serialize::RealValueSerializable*>(components[0]);
    serializable_component->deserializeFromNumbers(istream);
    if (hasVelocity()) {
        serializable_component = dynamic_cast<util::serialize::RealValueSerializable*>(components[1]);
        serializable_component->deserializeFromNumbers(istream);
    }
}

unsigned int SimEnvObjectStateSpace::StateType::getNumNumbers() const
{
    auto* serializable_component = dynamic_cast<util::serialize::RealValueSerializable*>(components[0]);
    unsigned int num_params = serializable_component->getNumNumbers();
    if (hasVelocity()) {
        serializable_component = dynamic_cast<util::serialize::RealValueSerializable*>(components[1]);
        num_params += serializable_component->getNumNumbers();
    }
    return num_params;
}

void SimEnvObjectStateSpace::StateType::print(std::ostream& ostream) const
{
    // TODO do we want to print more here?
    serializeInNumbers(ostream);
}

std::string SimEnvObjectStateSpace::StateType::toString() const
{
    std::stringstream ss;
    print(ss);
    return ss.str();
}

////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////// SimEnvObjectStateSpace ////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
SimEnvObjectStateSpace::SimEnvObjectStateSpace(sim_env::ObjectConstPtr object,
    bool position_only,
    const Eigen::MatrixX2f& position_limits,
    const Eigen::MatrixX2f& velocity_limits,
    const DistanceWeights& weights)
    : _object(object)
    , _position_only(position_only)
    , _active_dofs(object->getActiveDOFs())
{
    assert(_active_dofs.size() > 0);
    setName("sim_env::Object(" + object->getName() + ")-StateSpace");
    ::ompl::base::StateSpacePtr configuration_space = std::make_shared<SimEnvObjectConfigurationSpace>(object,
        position_limits,
        weights.position_weight,
        weights.orientation_weight,
        weights.joint_weight);
    addSubspace(configuration_space, weights.configuration_weight);
    if (not _position_only) {
        ::ompl::base::StateSpacePtr velocity_space = std::make_shared<SimEnvObjectVelocitySpace>(object,
            velocity_limits,
            weights.position_weight,
            weights.orientation_weight,
            weights.joint_weight);
        addSubspace(velocity_space, weights.velocity_weight);
    }
    lock();
}

SimEnvObjectStateSpace::~SimEnvObjectStateSpace()
{
}

SimEnvObjectConfigurationSpacePtr SimEnvObjectStateSpace::getConfigurationSpace()
{
    return std::dynamic_pointer_cast<SimEnvObjectConfigurationSpace>(getSubspace(0));
}

SimEnvObjectVelocitySpacePtr SimEnvObjectStateSpace::getVelocitySpace()
{
    if (not _position_only) {
        return std::dynamic_pointer_cast<SimEnvObjectVelocitySpace>(getSubspace(1));
    }
    return nullptr;
}

SimEnvObjectConfigurationSpaceConstPtr SimEnvObjectStateSpace::getConfigurationSpaceConst() const
{
    return std::dynamic_pointer_cast<const SimEnvObjectConfigurationSpace>(getSubspace(0));
}

SimEnvObjectVelocitySpaceConstPtr SimEnvObjectStateSpace::getVelocitySpaceConst() const
{
    if (not _position_only) {
        return std::dynamic_pointer_cast<const SimEnvObjectVelocitySpace>(getSubspace(1));
    }
    return nullptr;
}

void SimEnvObjectStateSpace::computeDirection(const StateType* state_a,
    const StateType* state_b,
    Eigen::VectorXf& dir) const
{
    static const std::string log_prefix("[mps::planner::ompl::state::SimEnvObjectStateSpace::computeDirection]");
    mps_logging::logDebug("Computing direction from state " + state_a->toString() + " to state " + state_b->toString(), log_prefix);
    dir.resize(getDimension());
    auto configuration_space = getConfigurationSpaceConst();
    Eigen::VectorXf sub_dir;
    configuration_space->computeDirection(state_a->getConfigurationState(), state_b->getConfigurationState(), sub_dir);
    dir.head(sub_dir.size()) = sub_dir;
    if (not _position_only) {
        auto velocity_space = getVelocitySpaceConst();
        velocity_space->computeDirection(state_a->getVelocityState(), state_b->getVelocityState(), sub_dir);
        dir.tail(sub_dir.size()) = sub_dir;
    }
    mps_logging::logDebug(boost::format("Computed direction is %1%") % dir.transpose(), log_prefix);
}

void SimEnvObjectStateSpace::shiftState(StateType* state, const Eigen::VectorXf& dir) const
{
    Eigen::VectorXf config;
    state->getConfiguration(config);
    config += dir.segment(0, config.size());
    state->setConfiguration(config);
    if (not _position_only) {
        Eigen::VectorXf vel;
        state->getVelocity(vel);
        vel += dir.tail(config.size());
        state->setVelocity(vel);
    }
}

::ompl::base::State* SimEnvObjectStateSpace::allocState() const
{
    SimEnvObjectState* object_state = new SimEnvObjectState(_active_dofs.size(), _position_only);
    allocStateComponents(object_state);
    return object_state;
}

void SimEnvObjectStateSpace::freeState(::ompl::base::State* state) const
{
    SimEnvObjectState* object_state = static_cast<SimEnvObjectState*>(state);
    for (unsigned int i = 0; i < componentCount_; ++i) {
        components_[i]->freeState(object_state->components[i]);
    }
    delete[] object_state->components;
    delete object_state;
}

sim_env::ObjectConstPtr SimEnvObjectStateSpace::getObject() const
{
    return _object.lock();
}

bool SimEnvObjectStateSpace::hasVelocities() const
{
    return !_position_only;
}

////////////////////////////////////////////////////////////////////////////////
//////////// SimEnvWorldState aka SimEnvWorldStateSpace::StateType//////////////
////////////////////////////////////////////////////////////////////////////////
SimEnvWorldStateSpace::StateType::StateType(unsigned int num_objects)
    : _num_objects(num_objects)
{
}

SimEnvWorldStateSpace::StateType::~StateType()
{
}

unsigned int SimEnvWorldStateSpace::StateType::getNumObjects() const
{
    return _num_objects;
}

SimEnvObjectState* SimEnvWorldStateSpace::StateType::getObjectState(unsigned int i) const
{
    if (i < _num_objects) {
        return static_cast<SimEnvObjectState*>(components[i]);
    }
    return nullptr;
}

void SimEnvWorldStateSpace::StateType::serializeInNumbers(std::ostream& ostream) const
{
    for (unsigned int i = 0; i < getNumObjects(); i++) {
        auto* state = getObjectState(i);
        state->serializeInNumbers(ostream);
        if (i < getNumObjects() - 1) {
            ostream << ", ";
        }
    }
}

void SimEnvWorldStateSpace::StateType::deserializeFromNumbers(std::istream& istream)
{
    for (unsigned int i = 0; i < getNumObjects(); i++) {
        auto* state = getObjectState(i);
        state->deserializeFromNumbers(istream);
    }
}

unsigned int SimEnvWorldStateSpace::StateType::getNumNumbers() const
{
    unsigned int num_params = 0;
    for (unsigned int i = 0; i < getNumObjects(); i++) {
        auto* state = getObjectState(i);
        num_params += state->getNumNumbers();
    }
    return num_params;
}

void SimEnvWorldStateSpace::StateType::print(std::ostream& ostream) const
{
    for (unsigned int i = 0; i < getNumObjects(); i++) {
        auto* state = getObjectState(i);
        state->print(ostream);
        if (i < getNumObjects() - 1) {
            ostream << ", ";
        }
    }
}

std::string SimEnvWorldStateSpace::StateType::toString() const
{
    std::stringstream ss;
    print(ss);
    return ss.str();
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////// SimEnvWorldStateSpace ///////////////////////////
////////////////////////////////////////////////////////////////////////////////
SimEnvWorldStateSpace::SimEnvWorldStateSpace(sim_env::WorldConstPtr world,
    const PlanningSceneBounds& bounds,
    bool position_only,
    const WeightMap& weights)
    : _world(world)
    , _position_only(position_only)
{
    std::vector<sim_env::ObjectConstPtr> objects;
    world->getObjects(objects, false);
    for (auto& object : objects) {
        if (object->getNumActiveDOFs() > 0) {
            // get limits for this object
            Eigen::ArrayX2f position_limits;
            Eigen::ArrayX2f velocity_limits;
            constructLimits(object, bounds, position_limits, velocity_limits);
            // get distance weights if we have some, else use default
            SimEnvObjectStateSpace::DistanceWeights obj_weight;
            auto weight_iter = weights.find(object->getName());
            if (weight_iter != weights.end()) {
                obj_weight = weight_iter->second;
            }
            SimEnvObjectStateSpacePtr state_space = std::make_shared<SimEnvObjectStateSpace>(object,
                position_only,
                position_limits,
                velocity_limits,
                obj_weight);
            _state_space_map[object->getName()] = state_space;
            addSubspace(state_space, 1.0);
            _object_names.push_back(object->getName());
        }
    }
}

SimEnvWorldStateSpace::~SimEnvWorldStateSpace()
{
}

::ompl::base::State* SimEnvWorldStateSpace::allocState() const
{
    SimEnvWorldState* world_state = new SimEnvWorldState(_object_names.size());
    allocStateComponents(world_state);
    return world_state;
}

void SimEnvWorldStateSpace::freeState(::ompl::base::State* state) const
{
    SimEnvWorldState* world_state = static_cast<SimEnvWorldState*>(state);
    for (unsigned int i = 0; i < componentCount_; ++i) {
        components_[i]->freeState(world_state->components[i]);
    }
    delete[] world_state->components;
    delete world_state;
}

double SimEnvWorldStateSpace::distance(const ::ompl::base::State* state_1, const ::ompl::base::State* state_2) const
{
    if (_distance_measure) {
        return _distance_measure->distance(state_1, state_2);
    }
    return ::ompl::base::CompoundStateSpace::distance(state_1, state_2);
}

void SimEnvWorldStateSpace::copySubState(::ompl::base::State* state_1,
    const ::ompl::base::State* state_2,
    unsigned int obj_id) const
{
    auto* world_state_1 = static_cast<SimEnvWorldState*>(state_1);
    auto* world_state_2 = static_cast<const SimEnvWorldState*>(state_2);
    auto object_state_1 = world_state_1->getObjectState(obj_id);
    auto object_state_2 = world_state_2->getObjectState(obj_id);
    auto subspace = getSubspace(obj_id);
    subspace->copyState(object_state_1, object_state_2);
}

sim_env::ObjectConstPtr SimEnvWorldStateSpace::getObject(unsigned int i) const
{
    if (i >= _object_names.size()) {
        return nullptr;
    }
    sim_env::WorldConstPtr world = _world.lock();
    if (!world) {
        throw std::logic_error("[mps::planner::ompl::state::SimEnvWorldStateSpace::getObject]"
                               " The world of this state space does no longer exist.");
    }
    return world->getObjectConst(_object_names[i], false);
}

std::string SimEnvWorldStateSpace::getObjectName(unsigned int i) const
{
    if (i >= _object_names.size()) {
        return "";
    }
    return _object_names[i];
}

int SimEnvWorldStateSpace::getObjectIndex(const std::string& obj_name) const
{
    for (unsigned int i = 0; i < _object_names.size(); ++i) {
        if (_object_names.at(i) == obj_name)
            return i;
    }
    return -1;
}

unsigned int SimEnvWorldStateSpace::getNumObjects() const
{
    return (unsigned int)_object_names.size();
}

SimEnvObjectStateSpacePtr SimEnvWorldStateSpace::getObjectStateSpace(const std::string& name)
{
    int obj_idx = getObjectIndex(name);
    if (obj_idx < 0) {
        return nullptr;
    }
    return getObjectStateSpace((unsigned int)obj_idx);
}

SimEnvObjectStateSpacePtr SimEnvWorldStateSpace::getObjectStateSpace(unsigned int obj_idx)
{
    if (obj_idx > getNumObjects()) {
        return nullptr;
    }
    return std::dynamic_pointer_cast<SimEnvObjectStateSpace>(getSubspace(obj_idx));
}

SimEnvObjectStateSpaceConstPtr SimEnvWorldStateSpace::getObjectStateSpaceConst(unsigned int obj_idx) const
{
    if (obj_idx > getNumObjects()) {
        return nullptr;
    }
    return std::dynamic_pointer_cast<const SimEnvObjectStateSpace>(getSubspace(obj_idx));
}

void SimEnvWorldStateSpace::computeDirection(StateType const* state_a, StateType const* state_b, Eigen::VectorXf& dir) const
{
    static const std::string log_prefix("[mps::planner::ompl::state::SimEnvWorldStateSpace::computeDirection]");
    mps_logging::logDebug("Computing direction from state " + state_a->toString() + " to state " + state_b->toString(), log_prefix);
    dir.resize(getDimension());
    assert(getNumObjects() == getSubspaceCount());
    unsigned int dim_offset = 0;
    for (unsigned int sub_idx = 0; sub_idx < getSubspaceCount(); ++sub_idx) {
        auto sub_space = std::dynamic_pointer_cast<SimEnvObjectStateSpace>(getSubspace(sub_idx));
        Eigen::VectorXf obj_dir;
        sub_space->computeDirection(state_a->getObjectState(sub_idx), state_b->getObjectState(sub_idx), obj_dir);
        dir.segment(dim_offset, obj_dir.size()) = obj_dir;
        dim_offset += obj_dir.size();
    }
    mps_logging::logDebug(boost::format("Computed direction is %1%") % dir.transpose(), log_prefix);
}

void SimEnvWorldStateSpace::shiftState(StateType* state, const Eigen::VectorXf& dir) const
{
    static const std::string log_prefix("[mps::planner::ompl::state::SimEnvWorldStateSpace::shiftState]");
    mps_logging::logDebug(boost::format("Shifting state " + state->toString() + " by %1%") % dir.transpose(), log_prefix);
    unsigned int dim_offset = 0;
    assert(dir.size() == getDimension());
    for (unsigned int obj_id = 0; obj_id < getNumObjects(); ++obj_id) {
        auto obj_state = state->getObjectState(obj_id);
        auto obj_state_space = getObjectStateSpaceConst(obj_id);
        Eigen::VectorXf component_dir(dir.segment(dim_offset, obj_state_space->getDimension()));
        obj_state_space->shiftState(obj_state, component_dir);
        dim_offset += obj_state_space->getDimension();
    }
    mps_logging::logDebug("Resulting state is " + state->toString(), log_prefix);
}

void SimEnvWorldStateSpace::setToState(sim_env::WorldPtr world, const StateType* state) const
{
    std::lock_guard<std::recursive_mutex> world_lock(world->getMutex());
    for (unsigned int obj_id = 0; obj_id < state->getNumObjects(); ++obj_id) {
        auto* obj_state = state->getObjectState(obj_id);
        std::string obj_name = getObjectName(obj_id);
        sim_env::ObjectPtr obj = world->getObject(obj_name, false);
        obj->setDOFPositions(obj_state->getConfiguration());
        obj->setDOFVelocities(obj_state->getVelocity());
    }
}

void SimEnvWorldStateSpace::extractState(sim_env::WorldConstPtr world, StateType* state) const
{
    std::lock_guard<std::recursive_mutex> world_lock(world->getMutex());
    for (unsigned int i = 0; i < getNumObjects(); ++i) {
        sim_env::ObjectConstPtr object = world->getObjectConst(_object_names[i], false);
        SimEnvObjectState* obj_state = state->getObjectState(i);
        obj_state->setConfiguration(object->getDOFPositions());
        obj_state->setVelocity(object->getDOFVelocities());
    }
}

void SimEnvWorldStateSpace::setDistanceMeasure(StateDistanceMeasureConstPtr measure)
{
    _distance_measure = measure;
}

StateDistanceMeasureConstPtr SimEnvWorldStateSpace::getDistanceMeasure()
{
    return _distance_measure;
}

bool SimEnvWorldStateSpace::hasVelocities() const
{
    return !_position_only;
}

void SimEnvWorldStateSpace::constructLimits(sim_env::ObjectConstPtr object, const PlanningSceneBounds& bounds,
    Eigen::ArrayX2f& position_limits, Eigen::ArrayX2f& velocity_limits) const
{
    Eigen::VectorXi active_dofs = object->getActiveDOFs();
    position_limits = object->getDOFPositionLimits();
    velocity_limits = object->getDOFVelocityLimits();
    // if we have a static object, there is nothing more to do
    if (object->isStatic()) {
        return;
    }
    // else we might need to change the limits of dofs related to the object's pose
    // run over potential pose dofs
    for (unsigned int i = 0; i < std::min((unsigned int)active_dofs.size(), object->getNumBaseDOFs()); ++i) {
        if (active_dofs[i] < object->getNumBaseDOFs()) {
            if (object->getNumBaseDOFs() == 3) {
                // we are in a 2d world
                if (active_dofs[i] == 0) {
                    // x
                    position_limits(i, 0) = bounds.x_limits[0];
                    position_limits(i, 1) = bounds.x_limits[1];
                    velocity_limits(i, 0) = -bounds.max_velocity;
                    velocity_limits(i, 1) = bounds.max_velocity;
                } else if (active_dofs[i] == 1) {
                    // y
                    position_limits(i, 0) = bounds.y_limits[0];
                    position_limits(i, 1) = bounds.y_limits[1];
                    velocity_limits(i, 0) = -bounds.max_velocity;
                    velocity_limits(i, 1) = bounds.max_velocity;
                } else {
                    // theta
                    velocity_limits(i, 0) = -bounds.max_rotation_vel;
                    velocity_limits(i, 1) = bounds.max_rotation_vel;
                }
            } else {
                // we are in a 3d world
                assert(object->getNumBaseDOFs() == 6);
                if (active_dofs[i] == 0) {
                    // x
                    position_limits(i, 0) = bounds.x_limits[0];
                    position_limits(i, 1) = bounds.x_limits[1];
                    velocity_limits(i, 0) = -bounds.max_velocity;
                    velocity_limits(i, 1) = bounds.max_velocity;
                } else if (active_dofs[i] == 1) {
                    // y
                    position_limits(i, 0) = bounds.y_limits[0];
                    position_limits(i, 1) = bounds.y_limits[1];
                    velocity_limits(i, 0) = -bounds.max_velocity;
                    velocity_limits(i, 1) = bounds.max_velocity;
                } else if (active_dofs[i] == 2) {
                    // z
                    position_limits(i, 0) = bounds.z_limits[0];
                    position_limits(i, 1) = bounds.z_limits[1];
                    velocity_limits(i, 0) = -bounds.max_velocity;
                    velocity_limits(i, 1) = bounds.max_velocity;
                } else {
                    // theta
                    velocity_limits(i, 0) = -bounds.max_rotation_vel;
                    velocity_limits(i, 1) = bounds.max_rotation_vel;
                }
            }
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////// SimEnvValidityChecker ///////////////////////////
////////////////////////////////////////////////////////////////////////////////
SimEnvValidityChecker::CollisionPolicy::CollisionPolicy()
    : _b_static_col_allowed(true)
{
}

SimEnvValidityChecker::CollisionPolicy::CollisionPolicy(const CollisionPolicy& other) = default;

SimEnvValidityChecker::CollisionPolicy::~CollisionPolicy() = default;

SimEnvValidityChecker::CollisionPolicy& SimEnvValidityChecker::CollisionPolicy::operator=(const CollisionPolicy& other)
{
    if (this != &other) {
        _b_static_col_allowed = other._b_static_col_allowed;
        _forbidden_collisions = other._forbidden_collisions;
        _static_collisions = other._static_collisions;
    }
    return *this;
}

void SimEnvValidityChecker::CollisionPolicy::setStaticCollisions(bool allowed)
{
    _b_static_col_allowed = allowed;
}

void SimEnvValidityChecker::CollisionPolicy::setStaticCollisions(const std::string& obj, bool allowed)
{
    _static_collisions[obj] = allowed;
}

void SimEnvValidityChecker::CollisionPolicy::setStaticCollisions(sim_env::ObjectConstPtr obj, bool allowed)
{
    setStaticCollisions(obj->getName(), allowed);
}

bool SimEnvValidityChecker::CollisionPolicy::staticCollisionsAllowed() const
{
    return _b_static_col_allowed;
}

bool SimEnvValidityChecker::CollisionPolicy::staticCollisionsAllowed(const std::string& obj) const
{
    auto iter = _static_collisions.find(obj);
    if (iter != _static_collisions.end()) {
        return iter->second;
    }
    return staticCollisionsAllowed();
}

bool SimEnvValidityChecker::CollisionPolicy::staticCollisionsAllowed(sim_env::ObjectConstPtr obj) const
{
    return staticCollisionsAllowed(obj->getName());
}

void SimEnvValidityChecker::CollisionPolicy::setCollision(const std::string& obj1,
    const std::string& obj2,
    bool allowed)
{
    auto pair = std::make_pair(obj1, obj2);
    auto iter = _forbidden_collisions.find(pair);
    if (iter != _forbidden_collisions.end()) { // if we have (obj1, obj2) already stored
        iter->second = allowed;
    } else {
        _forbidden_collisions[pair] = allowed; // else store it
    }
}

void SimEnvValidityChecker::CollisionPolicy::setCollision(sim_env::ObjectConstPtr obj1,
    sim_env::ObjectConstPtr obj2,
    bool allowed)
{
    setCollision(obj1->getName(), obj2->getName(), allowed);
}

bool SimEnvValidityChecker::CollisionPolicy::collisionAllowed(const std::string& obj1, const std::string& obj2) const
{
    auto pair = std::make_pair(obj1, obj2);
    auto iter = _forbidden_collisions.find(pair);
    if (iter != _forbidden_collisions.end()) {
        return iter->second;
    }
    return true; // if we don't know about it, it is allowed
}

bool SimEnvValidityChecker::CollisionPolicy::collisionAllowed(sim_env::ObjectConstPtr obj1, sim_env::ObjectConstPtr obj2) const
{
    static const std::string log_prefix("[mps::planner::ompl::state::SimEnvValidityChecker::CollisionPolicy::collisionAllowed]");
    if (obj1->isStatic() and obj2->isStatic()) {
        mps_logging::logWarn("Request made whether two static objects are allowed to contact each other.",
            log_prefix);
    }
    bool collision_permitted = false;
    if (obj1->isStatic()) {
        collision_permitted = staticCollisionsAllowed(obj2->getName());
    } else if (obj2->isStatic()) {
        collision_permitted = staticCollisionsAllowed(obj1->getName());
    } else {
        collision_permitted = collisionAllowed(obj1->getName(), obj2->getName());
    }
    mps_logging::logDebug(boost::format("Collision between %1% and %2% is permitted: %3%")
            % obj1->getName() % obj2->getName() % collision_permitted,
        log_prefix);
    return collision_permitted;
}

std::size_t SimEnvValidityChecker::CollisionPolicy::PairHash::operator()(
    const std::pair<std::string, std::string>& key) const
{
    auto hash_fn = std::hash<std::string>();
    return key.first == key.second ? hash_fn(key.first) : hash_fn(key.first) ^ hash_fn(key.second);
}

void SimEnvValidityChecker::CollisionPolicy::getForbiddenCollisionPairs(
    std::vector<std::pair<std::string, std::string>>& pairs)
{
    for (auto& iter : _forbidden_collisions) {
        if (iter.second) {
            pairs.push_back(iter.first);
        }
    }
}

void SimEnvValidityChecker::CollisionPolicy::getForbiddenStaticCollisions(std::vector<std::string>& black_list)
{
    for (auto& iter : _static_collisions) {
        if (iter.second) {
            black_list.push_back(iter.first);
        }
    }
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////// SimEnvValidityChecker /////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SimEnvValidityChecker::SimEnvValidityChecker(::ompl::base::SpaceInformationPtr si,
    sim_env::WorldPtr world)
    : StateValidityChecker(si)
    , _world(world)
{
    _world_space = std::dynamic_pointer_cast<const SimEnvWorldStateSpace>(si->getStateSpace());
    _world_state = dynamic_cast<mps::planner::ompl::state::SimEnvWorldState*>(_world_space->allocState());
    specs_.clearanceComputationType = ::ompl::base::StateValidityCheckerSpecs::ClearanceComputationType::NONE;
    assert(_world);
    assert(_world_space);
}

SimEnvValidityChecker::~SimEnvValidityChecker()
{
    _world_space->freeState(_world_state);
}

bool SimEnvValidityChecker::isValid(const ::ompl::base::State* state) const
{
    auto* world_state = state->as<SimEnvWorldState>();
    // first check bounds
    bool bounds_valid = _world_space->satisfiesBounds(world_state);
    if (!bounds_valid) {
        mps_logging::logDebug("State bounds violated. Rejecting state.",
            "[mps::planner::ompl::state::SimEnvValidityChecker::isValid]");
        return false;
    }
    // now check rest
    // save the state the world is in
    _world->saveState();
    // now set it to represent the given SimEnvWorldState
    _world_space->setToState(_world, world_state);
    // check whether this state is valid in terms of collisions
    bool valid = isWorldStateValid(false);
    // restore whatever state the world was in before
    _world->restoreState();
    if (valid) {
        mps_logging::logDebug("The state " + world_state->toString() + " is valid.", "[SimEnvValidityChecker::isValid]");
    }
    return valid;
}

bool SimEnvValidityChecker::isWorldStateValid(bool check_bounds) const
{
    std::lock_guard<std::recursive_mutex> world_lock(_world->getMutex());
    if (check_bounds) {
        _world_space->extractState(_world, _world_state);
        // TODO do we need to check physical feasibility here, too?
        // first check whether the current state is within bounds
        bool bounds_valid = _world_space->satisfiesBounds(_world_state);
        if (!bounds_valid) {
            mps_logging::logDebug("State bounds violated. Rejecting state.",
                "[mps::planner::ompl::state::SimEnvValidityChecker::isValidIntermediate]");
            return false;
        }
    }
    // first check whether this state is physically feasible
    if (not _world->isPhysicallyFeasible()) {
        mps_logging::logDebug("Rejecting state because it is physically infeasible.",
            "[mps::planner::ompl::state::SimEnvValidityChecker::isValid]");
        return false;
    }
    // check for collisions
    std::vector<sim_env::ObjectPtr> objects;
    _world->getObjects(objects, false);
    for (auto& object : objects) {
        std::vector<sim_env::Contact> contacts;
        _world->checkCollision(object, contacts);
        for (auto& contact : contacts) {
            bool contact_ok = checkContact(contact);
            if (!contact_ok) {
                mps_logging::logDebug("Rejecting state due to violation of contact constraints.",
                    "[mps::planner::ompl::state::SimEnvValidityChecker::isValid]");
                return false;
            }
        }
    }
    return true;
}

bool SimEnvValidityChecker::isValidIntermediate(const ::ompl::base::State* state) const
{
    auto* world_state = state->as<SimEnvWorldState>();
    std::lock_guard<std::recursive_mutex> world_lock(_world->getMutex());
    _world->saveState();
    _world_space->setToState(_world, world_state);
    bool valid = isValidIntermediate();
    _world->restoreState();
    return valid;
}

bool SimEnvValidityChecker::isValidIntermediate() const
{
    _world_space->extractState(_world, _world_state);
    // TODO do we need to check physical feasibility here, too?
    // first check whether the current state is within bounds
    bool bounds_valid = _world_space->satisfiesBounds(_world_state);
    if (!bounds_valid) {
        mps_logging::logDebug("State bounds violated. Rejecting state.",
            "[mps::planner::ompl::state::SimEnvValidityChecker::isValidIntermediate]");
        return false;
    }
    // next check, whether the state is valid in terms of collisions
    std::lock_guard<std::recursive_mutex> world_lock(_world->getMutex());
    // check for collisions
    std::vector<sim_env::ObjectPtr> objects;
    _world->getObjects(objects, false);
    for (const auto& object : objects) {
        std::vector<sim_env::Contact> contacts;
        _world->checkCollision(object, contacts);
        for (const auto& contact : contacts) {
            bool contact_ok = checkContact(contact);
            if (!contact_ok) {
                mps_logging::logDebug("Rejecting state due to violation of contact constraints.",
                    "[mps::planner::ompl::state::SimEnvValidityChecker::isValidIntermediate]");
                return false;
            }
        }
    }
    return true;
}

bool SimEnvValidityChecker::isValidIntermediate(std::vector<sim_env::Contact>& contacts) const
{
    _world_space->extractState(_world, _world_state);
    // TODO do we need to check physical feasibility here, too?
    // first check whether the current state is within bounds
    bool bounds_valid = _world_space->satisfiesBounds(_world_state);
    if (!bounds_valid) {
        mps_logging::logDebug("State bounds violated. Rejecting state.",
            "[mps::planner::ompl::state::SimEnvValidityChecker::isValidIntermediate]");
        return false;
    }
    for (auto contact : contacts) {
        bool contact_ok = checkContact(contact);
        if (!contact_ok) {
            mps_logging::logDebug("Rejecting state due to violation of contact constraints.",
                "[mps::planner::ompl::state::SimEnvValidityChecker::isValidIntermediate]");
            return false;
        }
    }
    return true;
}

bool SimEnvValidityChecker::checkContact(const sim_env::Contact& contact) const
{
    sim_env::ObjectPtr obj1 = contact.object_a.lock();
    sim_env::ObjectPtr obj2 = contact.object_b.lock();
    if (!obj1 or !obj2) {
        throw std::logic_error("[mps::planner::ompl::state::SimEnvValidityChecker::checkContact]"
                               "Could not lock weak pointer to objects stored in contact.");
    }
    return collision_policy.collisionAllowed(obj1, obj2);
}

bool SimEnvValidityChecker::checkContact(sim_env::LinkPtr a, sim_env::LinkPtr b) const
{
    auto obj_a = a->getObject();
    auto obj_b = b->getObject();
    return collision_policy.collisionAllowed(obj_a, obj_b);
}


// Receives wheel speed data from vesc and yaw data from IMU
// Publishes odom and tf if requested

// Param predict_slip when set predicts wheel slip based on accel/brake
// assumes linear relation between accel and slip ratio
// requires params slip_gain and slip_offset

// x covariance increase with speed
// y covariance increase with ay
// yaw covariance increase with yaw rate

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <vesc_msgs/VescStateStamped.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_broadcaster.h>
#include <cmath>
#include <tf/transform_datatypes.h>
#include <boost/shared_ptr.hpp>

class FusedOdomPublisher
{
    public:
        FusedOdomPublisher(ros::NodeHandle nh, ros::NodeHandle pnh);

    private:
        double x_, y_, yaw_;
        double slip_tix_gain_;
        ros::Time last_imu_time_;

        // ROS params
        double speed_gain_, speed_offset_;
        double slip_gain_speed_, slip_offset_speed_;
        double slip_gain_accel_, slip_offset_accel_;
        double slip_gain_brake_, slip_offset_brake_;
        double geometric_tix_gain_;
        bool publish_tf_;
        bool predict_slip_;

        // ROS messages
        sensor_msgs::Imu::ConstPtr last_imu_;
        vesc_msgs::VescStateStamped::ConstPtr last_state_;
        nav_msgs::Odometry odom_;
        geometry_msgs::TransformStamped tf_;

        // ROS services
        ros::Subscriber vesc_state_sub_;
        ros::Subscriber imu_sub_;
        ros::Publisher odom_pub_;
        boost::shared_ptr<tf::TransformBroadcaster> tf_pub_;

        // ROS callbacks
        void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state);
        void imuCallback(const sensor_msgs::Imu::ConstPtr& servo);
};

template <typename T>
inline bool getRequiredParam(const ros::NodeHandle& n, std::string name, T& value);

FusedOdomPublisher::FusedOdomPublisher(ros::NodeHandle nh, ros::NodeHandle pnh) :
    publish_tf_(true), predict_slip_(false), x_(0.0), y_(0.0), yaw_(0.0)
{
    if (!getRequiredParam(nh, "vesc_interface/speed_gain", speed_gain_))
        return;
    if (!getRequiredParam(nh, "vesc_interface/speed_offset", speed_offset_))
        return;
    if (!getRequiredParam(nh, "vesc_interface/tix_gain", geometric_tix_gain_))
        return;
    pnh.param("publish_tf", publish_tf_, publish_tf_);
    pnh.param("predict_slip", predict_slip_, predict_slip_);
    if (predict_slip_)
    {
        if (!getRequiredParam(pnh, "slip_gain_accel", slip_gain_accel_))
            return;
        if (!getRequiredParam(pnh, "slip_offset_accel", slip_offset_accel_))
            return;
        if (!getRequiredParam(pnh, "slip_gain_brake", slip_gain_brake_))
            return;
        if (!getRequiredParam(pnh, "slip_offset_brake", slip_offset_brake_))
            return;
        if (!getRequiredParam(pnh, "slip_gain_speed", slip_gain_speed_))
            return;
        if (!getRequiredParam(pnh, "slip_offset_speed", slip_offset_speed_))
            return;
    }
    else
        slip_tix_gain_ = geometric_tix_gain_;

    // create odom publisher
    odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom_fused", 10);

    // create tf broadcaster
    if (publish_tf_) {
        tf_pub_.reset(new tf::TransformBroadcaster);
    }

    // subscribe to vesc state and servo command
    vesc_state_sub_ = nh.subscribe("sensors/core", 10, &FusedOdomPublisher::vescStateCallback, this);
    imu_sub_ = nh.subscribe("imu", 10, &FusedOdomPublisher::imuCallback, this);

    // set static values in odom_
    odom_.header.frame_id = "odom";
    odom_.child_frame_id = "base_link";
    odom_.pose.pose.position.z = 0.0;
    odom_.pose.pose.orientation.x = 0.0;
    odom_.pose.pose.orientation.y = 0.0;
    odom_.twist.twist.linear.y = 0.0;

    // set static values in tf_
    if (publish_tf_)
    {
        tf_.header.frame_id = "odom";
        tf_.child_frame_id = "base_link";
        tf_.transform.translation.z = 0.0;
    }

    last_imu_time_ = ros::Time::now();
}

void FusedOdomPublisher::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state)
{

    // use current state as last state if this is our first time here
    if (!last_state_)
        last_state_ = state;

    // wait for first imu msg
    if (!last_imu_)
        return;

    // check for imu timeout
    if ( (ros::Time::now()-last_imu_time_).toSec() > 1.0 ) {
        ROS_ERROR("IMU sensor timed out. Not publishing fused_odom.");
        return;
    }

    // get vesc state
    double wheel_speed = ( state->state.speed - speed_offset_ ) / speed_gain_;
    double current_disp = state->state.displacement;
    static double last_disp = current_disp;

    // calculate accel dependent tix gain
    double slip_ratio;
    if (predict_slip_) {
        double accel = last_imu_->linear_acceleration.x;
        accel *= (wheel_speed>0 ? 1 : -1);                       // account for when car moving backwards
        if (accel > 0)                                           // accelerating
            slip_ratio = slip_gain_accel_ * fmax(0,accel-slip_offset_accel_) + slip_gain_speed_ * fmax(0,wheel_speed-slip_offset_speed_);
        else                                                     // braking
            slip_ratio = slip_gain_brake_ * fmax(0,-1.0*accel-slip_offset_brake_) + slip_gain_speed_ * fmax(0,wheel_speed-slip_offset_speed_);
        slip_tix_gain_ = geometric_tix_gain_ * (1 + slip_ratio);
    }

    // calc elapsed time
    ros::Duration dt = state->header.stamp - last_state_->header.stamp;

    // propigate odometry
    double ds = (current_disp - last_disp) / slip_tix_gain_;
    yaw_ = tf::getYaw(last_imu_->orientation);
    x_ += ds * cos(yaw_);
    y_ += ds * sin(yaw_);

    // calculate covariance
    odom_.pose.covariance[0] = 0.1 + 5.0 * fabs(wheel_speed);
    odom_.twist.covariance[0] = 0.1 + 5.0 * fabs(wheel_speed);
    odom_.pose.covariance[6*1+1] = 0.5 + 0.5 * fmax(0,fabs(last_imu_->linear_acceleration.y)-0.2);
    odom_.twist.covariance[6*1+1] = 0.5 + 0.5 * fmax(0,fabs(last_imu_->linear_acceleration.y)-0.2);
    odom_.pose.covariance[6*5+5] = 0.1 * 0.05 * fmax(0,fabs(last_imu_->angular_velocity.z)-0.1);
    odom_.twist.covariance[6*5+5] = 0.1 * 0.1 * fmax(0,fabs(last_imu_->angular_velocity.z)-0.1);

    // save state for next time
    last_state_ = state;
    last_disp = current_disp;

    // publish odometry message
    odom_.header.stamp = state->header.stamp;
    odom_.pose.pose.position.x = x_;
    odom_.pose.pose.position.y = y_;
    odom_.pose.pose.orientation.z = sin(yaw_/2.0);
    odom_.pose.pose.orientation.w = cos(yaw_/2.0);

    // Velocity (in the coordinate frame given by the child_frame_id) accounting for wheel slip
    if (predict_slip_) {
        odom_.twist.twist.linear.x = wheel_speed / (1 + slip_ratio);
    } else odom_.twist.twist.linear.x = wheel_speed;
    odom_.twist.twist.angular.z = last_imu_->angular_velocity.z;

    if (publish_tf_) {
        tf_.header.stamp = ros::Time::now();
        tf_.transform.translation.x = x_;
        tf_.transform.translation.y = y_;
        tf_.transform.rotation = odom_.pose.pose.orientation;
        if (ros::ok()) {
            tf_pub_->sendTransform(tf_);
        }
    }

    if (ros::ok()) {
        odom_pub_.publish(odom_);
    }
}

void FusedOdomPublisher::imuCallback(const sensor_msgs::Imu::ConstPtr& imu)
{
    last_imu_time_ = imu->header.stamp;
    last_imu_ = imu;
}

template <typename T>
inline bool getRequiredParam(const ros::NodeHandle& n, std::string name, T& value)
{
  if (n.getParam(name, value))
    return true;

  ROS_FATAL("OdomPublisher: Parameter %s is required.", name.c_str());
  return false;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "fused_odom_publisher");
  ros::NodeHandle nh;
  ros::NodeHandle pnh("~");

  FusedOdomPublisher fused_odom_publisher(nh, pnh);

  ros::spin();

  return 0;
}


/*!
 * @author Hisashi Ikari
 */
#include <boost/python.hpp>
#include <cnoid/Body>
#include <cnoid/BodyState>
#include <cnoid/BodyLoader>
#include <cnoid/MultiValueSeq>
#include <cnoid/BodyMotion>
#include <cnoid/EigenTypes>
#include <cnoid/Link>
#include <cnoid/LinkTraverse>
#include <cnoid/Device>

#include "../ColladaBodyLoader.h"
#include "../VRMLBodyLoader.h"

// We recommend the use of minieigen.
using namespace boost::python;
using namespace cnoid;

namespace cnoid
{
/*
 * @brief This class is a wrapper to the Link.
 *        This wrapper will receive the arguments and self on python(def a(self)).
 *        It is completely independent to Link for that.
 */
class PyLinkVisitor : public def_visitor<PyLinkVisitor>
{
    friend class def_visitor_access;
public:
    template <class T>
    void visit(T& self) const {
        // Visitor find to match signature on c++.
        self
            // For example, this interface is to match link.p(eigen.Vector3(0,0,0)).
            // And link assignment to self. Vector3 assignment in value.
            .def("p", &PyLinkVisitor::setTranslation)
            .def("R", &PyLinkVisitor::setRotation)
            .def("tb", &PyLinkVisitor::setTb)
            .def("Rs", &PyLinkVisitor::setRs)
            .def("q", &PyLinkVisitor::setQ)
            .def("dq", &PyLinkVisitor::setDq)
            .def("ddq", &PyLinkVisitor::setDdq)
            .def("u", &PyLinkVisitor::setU)
            .def("v", &PyLinkVisitor::setV)
            .def("w", &PyLinkVisitor::setW)
            .def("dv", &PyLinkVisitor::setDv)
            .def("dw", &PyLinkVisitor::setDw)
            .def("setCenterOfMassGlobal", &PyLinkVisitor::setCenterOfMassGlobal)

            // Please decide the correct name.
            .def("setPosition", &PyLinkVisitor::setPosition, return_value_policy<return_by_value>())
            .def("setTb", &PyLinkVisitor::setTb, return_value_policy<return_by_value>())

            // Convert Eigen::Block<Concrete> to concrete manually.
            // Because the miniEigen can not convert this(Part, block).
            // @note We can use method of same name, if signatures are different.
            .def("position", &PyLinkVisitor::position, return_value_policy<return_by_value>())
            .def("translation", &PyLinkVisitor::translation, return_value_policy<return_by_value>())
            .def("rotation", &PyLinkVisitor::rotation, return_value_policy<return_by_value>())
            .def("offsetTranslation", &PyLinkVisitor::offsetTranslation, return_value_policy<return_by_value>())
            .def("offsetRotation", &PyLinkVisitor::offsetRotation, return_value_policy<return_by_value>())
            ;
    }

    // For example, this interface is to match link.p(eigen.Vector3(0,0,0)).
    // And link assignment to self. Vector3 assignment in value.
    static void setTranslation(Link& self, const Vector3& value) {
        self.translation() = Eigen::Vector3d(value[0], value[1], value[2]);
    }
    static void setPosition(Link& self, const Eigen::MatrixXd& position) {
        Eigen::Matrix<double, 3, 4> result;
        result << position(0, 0), position(0, 1), position(0, 2), position(0, 3),
            position(1, 0), position(1, 1), position(1, 2), position(1, 3),
            position(2, 0), position(2, 1), position(2, 2), position(2, 3);
        self.T() = result;
    }
    static void setTb(Link& self, const Eigen::MatrixXd& position) {
        Eigen::Matrix<double, 3, 4> result;
        result << position(0, 0), position(0, 1), position(0, 2), position(0, 3),
            position(1, 0), position(1, 1), position(1, 2), position(1, 3),
            position(2, 0), position(2, 1), position(2, 2), position(2, 3);
        self.Tb() = result;
    }
    static Eigen::MatrixXd position(Link& self) {
        Eigen::Matrix<double, 3, 4> result;
        result << self.T()(0, 0), self.T()(0, 1), self.T()(0, 2), self.T()(0, 3),
            self.T()(1, 0), self.T()(1, 1), self.T()(1, 2), self.T()(1, 3),
            self.T()(2, 0), self.T()(2, 1), self.T()(2, 2), self.T()(2, 3);
        return result;
    }
    static void setRotation(Link& self, const Matrix3& value) { self.T().linear() = value.cast<double>(); }
    static void setRs(Link& self, const Matrix3& value) { self.Rs() = value.cast<double>(); }
    static void setQ(Link& self, const double& value) { self.q() = value; }
    static void setDq(Link& self, const double& value) { self.dq() = value; }
    static void setDdq(Link& self, const double& value) { self.ddq() = value; }
    static void setU(Link& self, const double& value) { self.u() = value; }

    static void setV(Link& self, const Vector3& value) { self.v() = value; }
    static void setW(Link& self, const Vector3& value) { self.w() = value; }
    static void setDv(Link& self, const Vector3& value) { self.dv() = value; }
    static void setDw(Link& self, const Vector3& value) { self.dw() = value; }
    static void setCenterOfMassGlobal(Link& self, const Vector3& value) { self.wc() = value; }

    static Vector3 translation(Link& self) { return Vector3(self.translation()); }
    static Matrix3 rotation(Link& self) { return Matrix3(self.rotation()); }
    static Vector3 offsetTranslation(Link& self) { return Vector3(self.offsetTranslation()); }
    static Matrix3 offsetRotation(Link& self) { return Matrix3(self.offsetRotation()); }

};

/*
 * @brief This class is a wrapper to the Body.
 *        This wrapper will receive the arguments and self on python(def a(self)).
 *        It is completely independent to Body for that.
 */
class PyBodyVisitor : public def_visitor<PyBodyVisitor>
{
    friend class def_visitor_access;
public:
    template <class T>
    void visit(T& self) const {
        // Visitor find to match signature on c++.
        self
            // For example, this interface is to match link.p(eigen.Vector3(0,0,0)).
            // And link assignment to self. Vector3 assignment in value.
            .def("defaultPosition", &PyBodyVisitor::defaultPosition)
            ;
    }
    static Eigen::MatrixXd defaultPosition(Body& self) {
        Eigen::Matrix<double, 3, 4> result;
        result << self.defaultPosition()(0, 0), self.defaultPosition()(0, 1),
            self.defaultPosition()(0, 2), self.defaultPosition()(0, 3),
            self.defaultPosition()(1, 0), self.defaultPosition()(1, 1),
            self.defaultPosition()(1, 2), self.defaultPosition()(1, 3),
            self.defaultPosition()(2, 0), self.defaultPosition()(2, 1),
            self.defaultPosition()(2, 2), self.defaultPosition()(2, 3);
        return result;
    }
};

/*!
 * @brief Definition of concrete method of pure virtual for BodyLoader.
 */
struct BodyLoaderWrapper : BodyLoader, boost::python::wrapper<BodyLoader>
{
public:
    const char* format() { return this->get_override("format")(); }
    void setVerbose(bool on) { this->get_override("setVerbose")(on); }
    void enableShapeLoading(bool on) { this->get_override("enableShapeLoading")(on); }
    void setDefaultDivisionNumber(int n) { this->get_override("setDefaultDivisionNumber")(n); }
    void setDefaultCreaseAngle(double theta) { this->get_override("setDefaultCreaseAngle")(theta); }
};

};

namespace cnoid
{
/*!
 * @brief Reference(pointer) types are explicitly declare a function pointer.
 *        Reference types share a reference in the original(in C++).
 *        But, it does not share the type in destination(in python).
 *        (Reason-1. Primitive types can not be a reference type.)
 *        (Reason-2. Minieigen create a new object "always" on convertion.)
 *        Therefore, we must use the setter if you want to use the python.
 *        We will define the settings of the following variables of setter in c++.
 */
BOOST_PYTHON_MODULE(Body)
{
    /*!
     * @brief Definition of access the link-traverse.
     */
    class_<LinkTraverse, boost::noncopyable>("LinkTraverse", init<>())
        .def("find", &LinkTraverse::find)
        .def("numLinks", &LinkTraverse::numLinks, return_value_policy<return_by_value>())
        .def("empty", &LinkTraverse::empty, return_value_policy<return_by_value>())
        .def("size", &LinkTraverse::size, return_value_policy<return_by_value>())
        .def("rootLink", &LinkTraverse::rootLink, return_internal_reference<>())
        .def("link", &LinkTraverse::link, return_internal_reference<>())
        .def("isDownward", &LinkTraverse::isDownward, return_value_policy<return_by_value>())
        .def("calcForwardKinematics", &LinkTraverse::calcForwardKinematics);

    /*!
     * @brief Definition of access the function pointer of link.
     */
    const Vector3& (Link::*jointAxis)() const = &Link::jointAxis;
    Position& (Link::*position)() = & Link::position;

    Position& (Link::*Tb)() = &Link::Tb;
    Matrix3& (Link::*Rs)() = &Link::Rs;

    double& (Link::*q)() = &Link::q;
    double& (Link::*dq)() = &Link::dq;
    double& (Link::*ddq)() = &Link::ddq;
    double& (Link::*u)() = &Link::u;

    Vector3& (Link::*v)() = &Link::v;
    Vector3& (Link::*w)() = &Link::w;
    Vector3& (Link::*dv)() = &Link::dv;
    Vector3& (Link::*dw)() = &Link::dw;
    Vector6& (Link::*F_ext)() = &Link::F_ext;

    /*!
     * @brief Definition of access the link.
     */
    class_<Link, boost::noncopyable>("Link", no_init) // Prohibition of constructor, reference object only.
        .def(PyLinkVisitor())
        .def("index", &Link::index, return_value_policy<return_by_value>())
        .def("isValid", &Link::isValid, return_value_policy<return_by_value>())
        .def("parent", &Link::parent, return_internal_reference<>())
        .def("sibling", &Link::sibling, return_internal_reference<>())
        .def("child", &Link::child, return_internal_reference<>())
        .def("isRoot", &Link::isRoot, return_value_policy<return_by_value>())

        //.def("position", position, return_value_policy<return_by_value>())
        .def("Tb", Tb, return_value_policy<return_by_value>())
        .def("Rs", Rs, return_value_policy<return_by_value>())

        .def("jointId", &Link::jointId, return_value_policy<return_by_value>())
        .def("jointType", &Link::jointType, return_value_policy<return_by_value>())
        .def("isFixedJoint", &Link::isFixedJoint, return_value_policy<return_by_value>())
        .def("isFreeJoint", &Link::isFreeJoint, return_value_policy<return_by_value>())
        .def("isRotationalJoint", &Link::isRotationalJoint, return_value_policy<return_by_value>())
        .def("isSlideJoint", &Link::isSlideJoint, return_value_policy<return_by_value>())
        .def("jointAxis", &Link::jointAxis, return_internal_reference<>())

        .def("q", q, return_value_policy<return_by_value>())
        .def("dq", dq, return_value_policy<return_by_value>())
        .def("ddq", ddq, return_value_policy<return_by_value>())
        .def("u", u, return_value_policy<return_by_value>())
        .def("q_upper", &Link::q_upper, return_value_policy<return_by_value>())
        .def("q_lower", &Link::q_lower, return_value_policy<return_by_value>())
        .def("dq_upper", &Link::dq_upper, return_value_policy<return_by_value>())
        .def("dq_lower", &Link::dq_lower, return_value_policy<return_by_value>())
        .def("v", v, return_internal_reference<>())
        .def("w", w, return_internal_reference<>())
        .def("dv", dv, return_internal_reference<>())
        .def("dw", dw, return_internal_reference<>())

        .def("centerOfMass", &Link::centerOfMass, return_value_policy<return_by_value>())
        .def("centerOfMassGlobal", &Link::centerOfMassGlobal, return_value_policy<return_by_value>())
        .def("mass", &Link::mass, return_value_policy<return_by_value>())
        .def("I", &Link::I, return_value_policy<return_by_value>())
        .def("Jm2", &Link::Jm2, return_value_policy<return_by_value>())
        .def("F_ext", F_ext, return_internal_reference<>())

        .def("name", &Link::name, return_value_policy<return_by_value>())
        .def("setIndex", &Link::setIndex)
        .def("prependChild", &Link::prependChild)
        .def("appendChild", &Link::appendChild)
        .def("removeChild", &Link::removeChild, return_value_policy<return_by_value>())

        .def("setName", &Link::setName)
        .def("setOffsetPosition", &Link::setOffsetPosition)
        .def("setJointType", &Link::setJointType)
        .def("setJointId", &Link::setJointId)
        .def("setJointAxis", &Link::setJointAxis)
        .def("setJointRange", &Link::setJointRange)
        .def("setJointVelocityRange", &Link::setJointVelocityRange)

        .def("setMass", &Link::setMass)
        .def("setInertia", &Link::setInertia)
        .def("setCenterOfMass", &Link::setCenterOfMass)
        .def("setEquivalentRotorInertia", &Link::setEquivalentRotorInertia)

        .def("attitude", &Link::attitude, return_value_policy<return_by_value>())
        .def("setAttitude", &Link::setAttitude)
        .def("calcRfromAttitude", &Link::calcRfromAttitude, return_value_policy<return_by_value>());

    // (*1)... This result is the same as return_by_value by minieigen.
    //         We will return to the python pointer of Vector3(Eigen3 on C++).
    //         But, minieigen converts with a new python_object it.
    // (*2)... It is not available to primitive types.
    //         You can be found in the help function in python on this. e.g. help(variable)->output is c++ signature.

    /*!
     * @brief Definition for getting of variable link on the Device.
     */
    const Link* (Device::*link)() const = &Device::link;
    class_<Device, boost::noncopyable>("Device", no_init)
        .def("setIndex", &Device::setIndex)
        .def("setId", &Device::setId)
        .def("setName", &Device::setName)
        .def("setLink", &Device::setLink)
        .def("clearState", &Device::clearState)
        .def("hasStateOnly", &Device::hasStateOnly, return_value_policy<return_by_value>())
        .def("index", &Device::index, return_value_policy<return_by_value>())
        .def("id", &Device::id, return_value_policy<return_by_value>())
        .def("name", &Device::name, return_value_policy<return_by_value>())
        .def("link", link, return_internal_reference<>())
        .def("cycle", &Device::cycle, return_value_policy<return_by_value>())
        .def("setCycle", &Device::setCycle)
        .def("notifyStateChange", &Device::notifyStateChange);

    /*!
     * @brief Definition for getting of variable link on the body.
     */
    Link* (Body::*link1)(int) const = &Body::link;
    Link* (Body::*link2)(const std::string&) const = &Body::link;
    const Body::ExtraJoint& (Body::*extraJoint)(int index) const = &Body::extraJoint;
    const Device* (Body::*device)(int index) const = &Body::device;
    bool (Body::*installCustomizer)() = &Body::installCustomizer;

    /*!
     * @brief Definition of access the link.
     */
    {
        scope outer = class_<Body, boost::noncopyable>("Body", no_init) // Prohibition of constructor, reference object only.
            .def(PyBodyVisitor())
            .def("createLink", &Body::createLink, return_internal_reference<>(), (args("link") = 0))
            .def("name", &Body::name, return_value_policy<return_by_value>())
            .def("setName", &Body::setName)
            .def("modelName", &Body::modelName, return_value_policy<return_by_value>())
            .def("setModelName", &Body::setModelName)
            .def("setRootLink", &Body::setRootLink)
            .def("updateLinkTree", &Body::updateLinkTree)
            .def("initializeState", &Body::initializeState)
            .def("numJoints", &Body::numJoints, return_value_policy<return_by_value>())
            .def("numVirtualJoints", &Body::numVirtualJoints, return_value_policy<return_by_value>())
            .def("numLinks", &Body::numLinks, return_value_policy<return_by_value>())
            .def("joint", &Body::joint, return_internal_reference<>())        // (*1)
            .def("link", link1, return_internal_reference<>())                // (*1)
            .def("link", link2, return_internal_reference<>())          // (*1)
            .def("rootLink", &Body::rootLink, return_internal_reference<>())  // (*1)
            .def("numDevices", &Body::numDevices, return_value_policy<return_by_value>())
            .def("device", device, return_internal_reference<>())
            .def("addDevice", &Body::addDevice)
            .def("initializeDeviceStates", &Body::initializeDeviceStates)
            .def("clearDevices", &Body::clearDevices)
            .def("isStaticModel", &Body::isStaticModel, return_value_policy<return_by_value>())
            .def("isFixedRootModel", &Body::isFixedRootModel, return_value_policy<return_by_value>())
            .def("resetDefaultPosition", &Body::resetDefaultPosition, return_value_policy<return_by_value>())
            .def("mass", &Body::mass, return_value_policy<return_by_value>())
            .def("calcCenterOfMass", &Body::calcCenterOfMass, return_value_policy<return_by_value>())
            .def("centerOfMass", &Body::centerOfMass, return_value_policy<return_by_value>())
            .def("calcTotalMomentum", &Body::calcTotalMomentum)
            .def("calcForwardKinematics", &Body::calcForwardKinematics)
            .def("clearExternalForces", &Body::clearExternalForces)
            .def("numExtraJoints", &Body::numExtraJoints, return_value_policy<return_by_value>())
            // I can not the define inner class interface on boost::python now...
            //.def("extraJoint", extraJoint, return_value_policy<return_by_value>())
            .def("addExtraJoint", &Body::addExtraJoint)
            .def("clearExtraJoints", &Body::clearExtraJoints)
            .def("installCustomizer", installCustomizer, return_value_policy<return_by_value>())
            .def("hasVirtualJointForces", &Body::hasVirtualJointForces, return_value_policy<return_by_value>())
            .def("setVirtualJointForces", &Body::setVirtualJointForces)
            .def("addCustomizerDirectory", &Body::addCustomizerDirectory).staticmethod("addCustomizerDirectory");

        // (*1)... Reference type is returned this. Because these classes we defined.

        /*!
         * @brief Definition for getting of variable extra-joint on the body.
         * @reference http://goo.gl/Wg29s4
         */
        /*
          class_<Body::ExtraJoint, boost::noncopyable>("ExtraJoint", init<>())
          .def("type", &Body::ExtraJoint::getType, return_value_policy<return_by_value>())
          .def("axis", &Body::ExtraJoint::getAxis, return_value_policy<return_by_value>())
          .def("link", &Body::ExtraJoint::getLink, return_value_policy<return_by_value>())
          .def("point", &Body::ExtraJoint::getPoint, return_value_policy<return_by_value>());
        */
    }

    /*!
     * @brief Definition of access the body.
     */
    void (BodyState::*setRootLinkPosition)(const Position& T) = &BodyState::setRootLinkPosition;
    bool (BodyState::*getRootLinkPosition)(Position& out_T) const = &BodyState::getRootLinkPosition;

    class_<BodyState, boost::noncopyable>("BodyState", no_init)
        .def("storePositions", &BodyState::storePositions)
        .def("restorePositions", &BodyState::restorePositions, return_value_policy<return_by_value>())
        .def("setRootLinkPosition", setRootLinkPosition)
        .def("getRootLinkPosition", getRootLinkPosition, return_value_policy<return_by_value>())
        .def("setZMP", &BodyState::setZMP)
        .def("getZMP", &BodyState::getZMP, return_value_policy<return_by_value>());

    /*!
     * @brief Definition of access the body motion.
     */
    void (BodyMotion::*setDimension)(int numFrames, int numJoints, int numLinks, bool clearNewArea) = &BodyMotion::setDimension;
    const MultiValueSeqPtr& (BodyMotion::*jointPosSeq)() const = &BodyMotion::jointPosSeq;
    const MultiSE3SeqPtr& (BodyMotion::*linkPosSeq)() const = &BodyMotion::linkPosSeq;

    class_<BodyMotion, boost::noncopyable>("BodyMotion", no_init)
        .def("setDimension", setDimension, (args("numFrames"), args("numJoints"), args("numLinks"), args("clearNewArea") = false))
        .def("setNumParts", &BodyMotion::setNumParts, (args("numParts"), args("clearNewElements") = false))
        .def("numParts", &BodyMotion::getNumParts, return_value_policy<return_by_value>())
        .def("numJoints", &BodyMotion::numJoints, return_value_policy<return_by_value>())
        .def("numLinks", &BodyMotion::numLinks, return_value_policy<return_by_value>())
        .def("frameRate", &BodyMotion::frameRate, return_value_policy<return_by_value>())
        .def("setFrameRate", &BodyMotion::setFrameRate)
        .def("numFrames", &BodyMotion::numFrames, return_value_policy<return_by_value>())
        .def("setNumFrames", &BodyMotion::setNumFrames)
        .def("jointPosSeq", jointPosSeq, return_value_policy<return_by_value>())
        .def("linkPosSeq", linkPosSeq, return_value_policy<return_by_value>())
        .def("loadStandardYAMLformat", &BodyMotion::loadStandardYAMLformat)
        .def("saveAsStandardYAMLformat", &BodyMotion::saveAsStandardYAMLformat)
        .def("setExtraSeq", &BodyMotion::setExtraSeq)
        .def("clearExtraSeq", &BodyMotion::clearExtraSeq);

    // (*1)... primitive types to copy the value. We can not refer raw-pointer of this value.

    /*!
     * @brief Definition of access the body loader.
     */
    class_<BodyLoaderWrapper, boost::noncopyable>("BodyLoader", no_init)
        .def("format", pure_virtual(&BodyLoaderWrapper::format), return_value_policy<return_by_value>())
        .def("setVerbose", pure_virtual(&BodyLoaderWrapper::setVerbose))
        .def("setShapeLoadingEnabled", pure_virtual(&BodyLoaderWrapper::setShapeLoadingEnabled))
        .def("setDefaultDivisionNumber", pure_virtual(&BodyLoaderWrapper::setDefaultDivisionNumber))
        .def("setDefaultCreaseAngle", pure_virtual(&BodyLoaderWrapper::setDefaultCreaseAngle));

    /*!
     * @brief Definition of access the child body loader.
     */
    class_<ColladaBodyLoader, bases<BodyLoader> , boost::noncopyable>("ColladaBodyLoader", init<>())
        .def("format", &ColladaBodyLoader::format)
        .def("setVerbose", &ColladaBodyLoader::setVerbose)
        .def("setShapeLoadingEnabled", &ColladaBodyLoader::setShapeLoadingEnabled)
        .def("setDefaultDivisionNumber", &ColladaBodyLoader::setDefaultDivisionNumber)
        .def("setDefaultCreaseAngle", &ColladaBodyLoader::setDefaultCreaseAngle);

    /*!
     * @brief Provides following enum value of the Body.
     */
    enum_ <Body::ExtraJointType>("ExtraJointType")
        .value("EJ_PISTON", Body::EJ_PISTON)
        .value("EJ_BALL", Body::EJ_BALL);

    /*!
     * @brief Provides following enum value of the Link.
     */
    enum_ <Link::JointType>("JointType")
        .value("ROTATIONAL_JOINT", Link::ROTATIONAL_JOINT)
        .value("SLIDE_JOINT", Link::SLIDE_JOINT)
        .value("FREE_JOINT", Link::FREE_JOINT)
        .value("FIXED_JOINT", Link::FIXED_JOINT)
        .value("CRAWLER_JOINT", Link::CRAWLER_JOINT);

    /*!
     * @brief Provides following enum value of the body motion.
     */
    enum_ <BodyState::DataType>("BodyStateDataType")
        .value("JOINT_POSITIONS", BodyState::JOINT_POSITIONS)
        .value("LINK_POSITIONS", BodyState::LINK_POSITIONS)
        .value("JOINT_FORCE_OR_TORQUE", BodyState::JOINT_FORCE_OR_TORQUE)
        .value("ZMP", BodyState::ZMP);

}

}; // end of namespace


// Boost.Polygon library polygon_90_data_test.cpp file

//          Copyright Andrii Sydorchuk 2015.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#define BOOST_TEST_MODULE POLYGON_90_DATA_TEST
#include <vector>

#include <boost/mpl/list.hpp>
#include <boost/test/test_case_template.hpp>

#include "boost/polygon/polygon.hpp"
using namespace boost::polygon;

#include <iostream>

typedef boost::mpl::list<int> test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE(polygon_90_data_test, T, test_types) {
    typedef polygon_90_data<int> polygon_type;
    typedef polygon_traits_90<polygon_type>::point_type point_type;
    typedef polygon_type::iterator_type iterator_type;

    std::vector<point_type> data;
    data.push_back(point_type(0, 0)); // 1
    data.push_back(point_type(10, 0)); // 2
    data.push_back(point_type(10, 10)); // 3
    data.push_back(point_type(0, 10)); // 4

    polygon_type polygon;
    polygon.set(data.begin(), data.end());

    std::cout << "Interesting: " << std::endl;
    for (polygon_type::compact_iterator_type it = polygon.begin_compact(); it != polygon.end_compact(); ++it) {
        std::cout << *it << " ";
    }
    std::cout << std::endl;

    iterator_type it = polygon.begin();
    for (int i = 0; i < 2; i++) {
        it++;
    }

    iterator_type it_3rd = it;
    it++;
    BOOST_CHECK(it != it_3rd);
}