/* Copyright (c) AIRBUS and its affiliates.
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */
#ifndef SKDECIDE_RIW_HH
#define SKDECIDE_RIW_HH

#include <functional>
#include <memory>
#include <unordered_set>
#include <unordered_map>
#include <vector>
#include <queue>
#include <list>
#include <chrono>
#include <random>

#include <boost/container_hash/hash.hpp>
#include <boost/range/irange.hpp>

#include "spdlog/spdlog.h"
#include "spdlog/sinks/stdout_color_sinks.h"

#include "utils/associative_container_deducer.hh"
#include "utils/string_converter.hh"
#include "utils/execution.hh"

namespace skdecide {

/** Use default hasher provided with domain's states */
template <typename Tdomain, typename Tfeature_vector>
struct DomainStateHash {
    typedef typename Tdomain::State Key;

    template <typename Tnode>
    static const Key& get_key(const Tnode& n) {
        return n.state;
    }

    struct Hash {
        std::size_t operator()(const Key& k) const {
            return typename Tdomain::State::Hash()(k);
        }
    };

    struct Equal {
        bool operator()(const Key& k1, const Key& k2) const {
            return typename Tdomain::State::Equal()(k1, k2);
        }
    };
};

/** Use state binary feature vector to hash states */
template <typename Tdomain, typename Tfeature_vector>
struct StateFeatureHash {
    typedef Tfeature_vector Key;

    template <typename Tnode>
    static const Key& get_key(const Tnode& n) {
        return *n.features;
    }

    struct Hash {
        std::size_t operator()(const Key& k) const {
            std::size_t seed = 0;
            for (std::size_t i = 0 ; i < k.size() ; i++) {
                boost::hash_combine(seed, k[i]);
            }
            return seed;
        }
    };

    struct Equal {
        bool operator()(const Key& k1, const Key& k2) const {
            std::size_t size = k1.size();
            if (size != k2.size()) {
                return false;
            }
            for (std::size_t i = 0 ; i < size ; i++) {
                if (!(k1[i] == k2[i])) {
                    return false;
                }
            }
            return true;
        }
    };
};

/** Use Environment domain knowledge for rollouts */
template <typename Tdomain>
struct EnvironmentRollout {
    std::list<typename Tdomain::Event> _action_prefix;

    void init_rollout(Tdomain& domain, const std::size_t* thread_id) {
        domain.reset(thread_id);
        std::for_each(_action_prefix.begin(), _action_prefix.end(),
                      [&domain, &thread_id](const typename Tdomain::Event& a){domain.step(a, thread_id);});
    }

    typename Tdomain::TransitionOutcome progress(Tdomain& domain,
                                                 const typename Tdomain::State& state,
                                                 const typename Tdomain::Event& action,
                                                 const std::size_t* thread_id) {
        return domain.step(action, thread_id);
    }

    void advance(Tdomain& domain,
                 const typename Tdomain::State& state,
                 const typename Tdomain::Event& action,
                 bool record_action,
                 const std::size_t* thread_id) {
        if (record_action) {
            _action_prefix.push_back(action);
        } else {
            domain.step(action, thread_id);
        }
    }

    std::list<typename Tdomain::Event> action_prefix() const {
        return _action_prefix;
    }
};

/** Use Simulation domain knowledge for rollouts */
template <typename Tdomain>
struct SimulationRollout {
    void init_rollout([[maybe_unused]] Tdomain& domain, [[maybe_unused]] const std::size_t* thread_id) {}

    typename Tdomain::TransitionOutcome progress(Tdomain& domain,
                                                 const typename Tdomain::State& state,
                                                 const typename Tdomain::Event& action,
                                                 const std::size_t* thread_id) {
        return domain.sample(state, action, thread_id);
    }

    void advance([[maybe_unused]] Tdomain& domain,
                 [[maybe_unused]] const typename Tdomain::State& state,
                 [[maybe_unused]] const typename Tdomain::Event& action,
                 [[maybe_unused]] bool record_action,
                 [[maybe_unused]] const std::size_t* thread_id) {}

    std::list<typename Tdomain::Event> action_prefix() const {
        return std::list<typename Tdomain::Event>();
    }
};

template <typename Tdomain,
          typename Tfeature_vector,
          template <typename...> class Thashing_policy = DomainStateHash,
          template <typename...> class Trollout_policy = EnvironmentRollout,
          typename Texecution_policy = SequentialExecution>
class RIWSolver {
public :
    typedef Tdomain Domain;
    typedef typename Domain::State State;
    typedef typename Domain::Event Action;
    typedef Tfeature_vector FeatureVector;
    typedef Thashing_policy<Domain, FeatureVector> HashingPolicy;
    typedef Trollout_policy<Domain> RolloutPolicy;
    typedef Texecution_policy ExecutionPolicy;

    RIWSolver(Domain& domain,
              const std::function<std::unique_ptr<FeatureVector> (Domain& d, const State& s, const std::size_t* thread_id)>& state_features,
              std::size_t time_budget = 3600000,
              std::size_t rollout_budget = 100000,
              std::size_t max_depth = 1000,
              double exploration = 0.25,
              double discount = 1.0,
              bool online_node_garbage = false,
              bool debug_logs = false)
    : _domain(domain), _state_features(state_features),
      _time_budget(time_budget), _rollout_budget(rollout_budget),
      _max_depth(max_depth), _exploration(exploration),
      _discount(discount), _online_node_garbage(online_node_garbage),
      _min_reward(std::numeric_limits<double>::max()),
      _nb_rollouts(0), _debug_logs(debug_logs) {
        if (debug_logs) {
            spdlog::set_level(spdlog::level::debug);
        } else {
            spdlog::set_level(spdlog::level::info);
        }

        std::random_device rd;
        _gen = std::make_unique<std::mt19937>(rd());
    }

    // clears the solver (clears the search graph, thus preventing from reusing
    // previous search results)
    void clear() {
        _graph.clear();
        _min_reward = std::numeric_limits<double>::max();
    }

    // solves from state s
    void solve(const State& s) {
        try {
            spdlog::info("Running " + ExecutionPolicy::print_type() + " RIW solver from state " + s.print());
            auto start_time = std::chrono::high_resolution_clock::now();
            _nb_rollouts = 0;
            std::size_t nb_of_binary_features = _state_features(_domain, s, nullptr)->size();

            TupleVector feature_tuples;
            bool found_solution = false;

            for (atomic_size_t w = 1 ; w <= nb_of_binary_features ; w++) {
                if(WidthSolver(*this, _domain, _state_features,
                               _time_budget, _rollout_budget,
                               _max_depth, _exploration, _discount,
                               _min_reward, w, _graph, _rollout_policy,
                               _execution_policy, *_gen, _gen_mutex, _time_mutex,
                               _debug_logs).solve(s, start_time, _nb_rollouts, feature_tuples)) {
                    found_solution = true;
                    break;
                }
            }

            auto end_time = std::chrono::high_resolution_clock::now();
            auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - start_time).count();
            auto exploration_statistics = get_exploration_statistics();
            std::string solution_str(found_solution?("finished to solve"):("could not find a solution"));
            spdlog::info("RIW " + solution_str + " from state " + s.print() +
                         " in " + StringConverter::from((double) duration / (double) 1e9) + " seconds with " +
                         StringConverter::from(_nb_rollouts) + " rollouts and pruned " +
                         StringConverter::from(exploration_statistics.second) + " states among " +
                         StringConverter::from(exploration_statistics.first) + " visited states.");
        } catch (const std::exception& e) {
            spdlog::error("RIW failed solving from state " + s.print() + ". Reason: " + e.what());
            throw;
        }
    }

    bool is_solution_defined_for(const State& s) const {
        auto si = _graph.find(Node(s, _domain, _state_features, nullptr));
        if ((si == _graph.end()) || (si->best_action == nullptr)) {// || (si->solved == false)) {
            return false;
        } else {
            return true;
        }
    }

    Action get_best_action(const State& s) {
        auto si = _graph.find(Node(s, _domain, _state_features, nullptr));
        if ((si == _graph.end()) || (si->best_action == nullptr)) {
            spdlog::error("SKDECIDE exception: no best action found in state " + s.print());
            throw std::runtime_error("SKDECIDE exception: no best action found in state " + s.print());
        }
        _rollout_policy.advance(_domain, s, *(si->best_action), true, nullptr);
        Action best_action = *(si->best_action);
        std::unordered_set<Node*> root_subgraph;
        compute_reachable_subgraph(const_cast<Node&>(*si), root_subgraph); // we won't change the real key (Node::state) so we are safe
        Node* next_node = nullptr;
        for (auto& child : si->children) {
            if (&std::get<0>(child) == si->best_action) {
                next_node = std::get<2>(child);
                break;
            }
        }
        if (next_node == nullptr) {
            spdlog::error("SKDECIDE exception: best action's next node from state " + s.print() + " not found in the graph");
            throw std::runtime_error("SKDECIDE exception: best action's next node from state " + s.print() + " not found in the graph");
        }
        if (_debug_logs) { spdlog::debug("Expected outcome of best action " + si->best_action->print() +
                                         ": " + next_node->state.print()); }
        std::unordered_set<Node*> child_subgraph;
        if (_online_node_garbage) {
            compute_reachable_subgraph(*next_node, child_subgraph);
        }
        update_graph(root_subgraph, child_subgraph);
        return best_action;
    }

    double get_best_value(const State& s) const {
        auto si = _graph.find(Node(s, _domain, _state_features, nullptr));
        if (si == _graph.end()) {
            spdlog::error("SKDECIDE exception: no best action found in state " + s.print());
            throw std::runtime_error("SKDECIDE exception: no best action found in state " + s.print());
        }
        return si->value;
    }

    std::size_t get_nb_of_explored_states() const {
        return _graph.size();
    }

    std::size_t get_nb_of_pruned_states() const {
        std::size_t cnt = 0;
        for (const auto&  n : _graph)  {
            if (n.pruned) {
                cnt++;
            }
        }
        return cnt;
    }

    std::pair<std::size_t, std::size_t> get_exploration_statistics() const {
        std::size_t pruned = 0;
        std::size_t explored = 0;
        for (const auto&  n : _graph)  {
            explored++;
            if (n.pruned) {
                pruned++;
            }
        }
        return std::make_pair(explored, pruned);
    }

    std::size_t get_nb_rollouts() const {
        return _nb_rollouts;
    }

    std::list<Action> action_prefix() const {
        return _rollout_policy.action_prefix();
    }

    typename MapTypeDeducer<State, std::pair<Action, double>>::Map policy() {
        typename MapTypeDeducer<State, std::pair<Action, double>>::Map p;
        for (auto& n : _graph) {
            if (n.best_action != nullptr) {
                p.insert(std::make_pair(n.state, std::make_pair(*n.best_action, (double) n.value)));
            }
        }
        return p;
    }

private :

    typedef typename ExecutionPolicy::template atomic<std::size_t> atomic_size_t;
    typedef typename ExecutionPolicy::template atomic<double> atomic_double;
    typedef typename ExecutionPolicy::template atomic<bool> atomic_bool;

    Domain& _domain;
    std::function<std::unique_ptr<FeatureVector> (Domain&, const State& s, const std::size_t* thread_id)> _state_features;
    atomic_size_t _time_budget;
    atomic_size_t _rollout_budget;
    atomic_size_t _max_depth;
    atomic_double _exploration;
    atomic_double _discount;
    bool _online_node_garbage;
    atomic_double _min_reward;
    atomic_size_t _nb_rollouts;
    RolloutPolicy _rollout_policy;
    ExecutionPolicy _execution_policy;
    std::unique_ptr<std::mt19937> _gen;
    typename ExecutionPolicy::Mutex _gen_mutex;
    typename ExecutionPolicy::Mutex _time_mutex;
    atomic_bool _debug_logs;

    struct Node {
        State state;
        std::unique_ptr<FeatureVector> features;
        std::vector<std::tuple<Action, double, Node*>> children;
        std::unordered_set<Node*> parents;
        atomic_double value;
        atomic_size_t depth;
        atomic_size_t novelty;
        Action* best_action;
        atomic_bool terminal; // true if seen terminal from the simulator's perspective
        atomic_bool pruned; // true if pruned from novelty measure perspective
        atomic_bool solved; // from this node: true if all reached states are either max_depth, or terminal or pruned
        mutable typename ExecutionPolicy::Mutex mutex;

        Node(const State& s, Domain& d,
             const std::function<std::unique_ptr<FeatureVector> (Domain&, const State&, const std::size_t*)>& state_features,
             const std::size_t* thread_id)
            : state(s),
              value(-std::numeric_limits<double>::max()),
              depth(std::numeric_limits<std::size_t>::max()),
              novelty(std::numeric_limits<std::size_t>::max()),
              best_action(nullptr),
              terminal(false),
              pruned(false),
              solved(false) {
            features = state_features(d, s, thread_id);
        }

        Node(const Node& n)
            : state(n.state), features(std::move(const_cast<std::unique_ptr<FeatureVector>&>(n.features))),
              children(n.children), parents(n.parents), value((double) n.value), depth((std::size_t) n.depth),
              novelty((std::size_t) n.novelty), best_action(n.best_action), terminal((bool) n.terminal),
              pruned((bool) n.pruned), solved((bool) n.solved) {}

        struct Key {
            const typename HashingPolicy::Key& operator()(const Node& n) const {
                return HashingPolicy::get_key(n);
            }
        };
    };

    typedef typename SetTypeDeducer<Node, HashingPolicy>::Set Graph;
    Graph _graph;

    typedef std::vector<std::pair<std::size_t, typename FeatureVector::value_type>> TupleType; // pair of var id and var value
    typedef std::vector<std::unordered_map<TupleType, std::size_t, boost::hash<TupleType>>> TupleVector; // mapped to min reached depth

    class WidthSolver { // known as IW(i), i.e. the fixed-width solver sequentially run by IW
    public :
        typedef Tdomain Domain;
        typedef typename Domain::State State;
        typedef typename Domain::Event Action;
        typedef Tfeature_vector FeatureVector;
        typedef Thashing_policy<Domain, FeatureVector> HashingPolicy;
        typedef Trollout_policy<Domain> RolloutPolicy;
        typedef Texecution_policy ExecutionPolicy;

        WidthSolver(RIWSolver& parent_solver, Domain& domain,
                    const std::function<std::unique_ptr<FeatureVector> (Domain& d, const State& s, const std::size_t* thread_id)>& state_features,
                    const atomic_size_t& time_budget,
                    const atomic_size_t& rollout_budget,
                    const atomic_size_t& max_depth,
                    const atomic_double& exploration,
                    const atomic_double& discount,
                    atomic_double& min_reward,
                    const atomic_size_t& width,
                    Graph& graph,
                    RolloutPolicy& rollout_policy,
                    ExecutionPolicy& execution_policy,
                    std::mt19937& gen,
                    typename ExecutionPolicy::Mutex& gen_mutex,
                    typename ExecutionPolicy::Mutex& time_mutex,
                    const atomic_bool& debug_logs)
            : _parent_solver(parent_solver), _domain(domain), _state_features(state_features),
              _time_budget(time_budget), _rollout_budget(rollout_budget),
              _max_depth(max_depth), _exploration(exploration),
              _discount(discount), _min_reward(min_reward), _min_reward_changed(false),
              _width(width), _graph(graph), _rollout_policy(rollout_policy),
              _execution_policy(execution_policy), _gen(gen), _gen_mutex(gen_mutex),
              _time_mutex(time_mutex), _debug_logs(debug_logs) {}

        // solves from state s
        // return true iff no state has been pruned or time or rollout budgets are consumed
        bool solve(const State& s,
                   const std::chrono::time_point<std::chrono::high_resolution_clock>& start_time,
                   atomic_size_t& nb_rollouts,
                   TupleVector& feature_tuples) {
            try {
                spdlog::info("Running " + ExecutionPolicy::print_type() + " RIW(" + StringConverter::from(_width) + ") solver from state " + s.print());
                auto local_start_time = std::chrono::high_resolution_clock::now();

                // Clear the solved bits
                // /!\ 'solved' bit set to 1 in RIW even if no solution found with previous width so we need to clear all the bits
                std::for_each(_graph.begin(), _graph.end(), [](const Node& n){
                    // we don't change the real key (Node::state) so we are safe
                    const_cast<Node&>(n).solved = false;
                    const_cast<Node&>(n).pruned = false;
                });

                // Create the root node containing the given state s
                auto si = _graph.emplace(Node(s, _domain, _state_features, nullptr));
                Node& root_node = const_cast<Node&>(*(si.first)); // we won't change the real key (Node::state) so we are safe
                root_node.depth = 0;
                atomic_bool states_pruned(false);
                atomic_bool reached_end_of_trajectory_once(false);

                // Vector of sets of state feature tuples generated so far, for each w <= _width
                if (feature_tuples.size() < _width) {
                    feature_tuples.push_back(typename TupleVector::value_type());
                }
                novelty(feature_tuples, root_node, true); // initialize feature_tuples with the root node's bits

                boost::integer_range<std::size_t> parallel_rollouts(0, _domain.get_parallel_capacity());

                std::for_each(ExecutionPolicy::policy, parallel_rollouts.begin(), parallel_rollouts.end(),
                              [this, &start_time, &root_node, &feature_tuples, &nb_rollouts, &states_pruned,
                               &reached_end_of_trajectory_once] (const std::size_t& thread_id) {
                    // Start rollouts
                    while (!root_node.solved && elapsed_time(start_time) < _time_budget && nb_rollouts < _rollout_budget) {
                        rollout(root_node, feature_tuples, nb_rollouts,
                                states_pruned, reached_end_of_trajectory_once,
                                start_time, &thread_id);
                    }
                });

                if (_debug_logs) spdlog::debug("time budget: " + StringConverter::from(elapsed_time(start_time)) +
                                               " ms, rollout budget: " + StringConverter::from(nb_rollouts) +
                                               ", states pruned: " + StringConverter::from(states_pruned));

                if (static_cast<std::size_t>(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start_time).count()) < _time_budget &&
                    nb_rollouts < _rollout_budget &&
                    !reached_end_of_trajectory_once &&
                    states_pruned) {
                    spdlog::info("RIW(" + StringConverter::from(_width) + ") could not find a solution from state " + s.print());
                    return false;
                } else {
                    auto end_time = std::chrono::high_resolution_clock::now();
                    auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - local_start_time).count();
                    spdlog::info("RIW(" + StringConverter::from(_width) + ") finished to solve from state " + s.print() + " in " + StringConverter::from((double) duration / (double) 1e9) + " seconds.");
                    return true;
                }
            } catch (const std::exception& e) {
                spdlog::error("RIW(" + StringConverter::from(_width) + ") failed solving from state " + s.print() + ". Reason: " + e.what());
                throw;
            }
        }

    private :
        typedef typename ExecutionPolicy::template atomic<std::size_t> atomic_size_t;
        typedef typename ExecutionPolicy::template atomic<double> atomic_double;
        typedef typename ExecutionPolicy::template atomic<bool> atomic_bool;

        RIWSolver& _parent_solver;
        Domain& _domain;
        const std::function<std::unique_ptr<FeatureVector> (Domain& d, const State& s, const std::size_t* thread_id)>& _state_features;
        const atomic_size_t& _time_budget;
        const atomic_size_t& _rollout_budget;
        const atomic_size_t& _max_depth;
        const atomic_double& _exploration;
        const atomic_double& _discount;
        atomic_double& _min_reward;
        atomic_bool _min_reward_changed;
        const atomic_size_t& _width;
        Graph& _graph;
        RolloutPolicy& _rollout_policy;
        ExecutionPolicy& _execution_policy;
        std::mt19937& _gen;
        typename ExecutionPolicy::Mutex& _gen_mutex;
        typename ExecutionPolicy::Mutex& _time_mutex;
        const atomic_bool& _debug_logs;

        void rollout(Node& root_node, TupleVector& feature_tuples, atomic_size_t& nb_rollouts,
                     atomic_bool& states_pruned, atomic_bool& reached_end_of_trajectory_once,
                     const std::chrono::time_point<std::chrono::high_resolution_clock>& start_time,
                     const std::size_t* thread_id) {
            // Start new rollout
            nb_rollouts += 1;
            Node* current_node = &root_node;

            if (_debug_logs) spdlog::debug("New rollout" + ExecutionPolicy::print_thread() +
                                           " from state: " + current_node->state.print() +
                                           ", depth=" + StringConverter::from(current_node->depth) +
                                           ", value=" + StringConverter::from(current_node->value) +
                                           ExecutionPolicy::print_thread());
            _rollout_policy.init_rollout(_domain, thread_id);
            bool break_loop = false;

            while (!(current_node->solved) && !break_loop) {

                std::vector<std::size_t> unsolved_children;
                std::vector<double> probabilities;

                _execution_policy.protect([this, &current_node, &unsolved_children, &probabilities, &thread_id](){
                    if (_debug_logs) spdlog::debug("Current state" + ExecutionPolicy::print_thread() + ": " +
                                                   current_node->state.print() +
                                                   ", depth=" + StringConverter::from(current_node->depth) +
                                                   ", value=" + StringConverter::from(current_node->value) +
                                                   ExecutionPolicy::print_thread());

                    if (current_node->children.empty()) {
                        // Generate applicable actions
                        auto applicable_actions = _domain.get_applicable_actions(current_node->state, thread_id).get_elements();
                        std::for_each(applicable_actions.begin(), applicable_actions.end(), [&current_node](auto a){
                            current_node->children.push_back(std::make_tuple(a, 0, nullptr));
                        });
                    }

                    // Sample unsolved child
                    for (std::size_t i = 0 ; i < current_node->children.size() ; i++) {
                        Node* n = std::get<2>(current_node->children[i]);
                        if (!n) {
                            unsolved_children.push_back(i);
                            probabilities.push_back(_exploration);
                        } else if (!(n->solved)) {
                            unsolved_children.push_back(i);
                            probabilities.push_back((1.0 - _exploration) / ((double) n->novelty));
                        }
                    }
                }, current_node->mutex);

                // In parallel execution mode, child nodes can have been solved since we have checked
                // for this current node's solve bit
                if (unsolved_children.empty()) {
                    if(std::is_same<ExecutionPolicy, SequentialExecution>::value) {
                        throw std::runtime_error("In sequential mode, nodes labelled as unsolved must have unsolved children.");
                    }
                    current_node->solved = true;
                    break;
                }

                std::size_t pick = 0;
                _execution_policy.protect([this, &pick, &unsolved_children, &probabilities](){
                    pick = unsolved_children[std::discrete_distribution<>(probabilities.begin(), probabilities.end())(_gen)];
                }, _gen_mutex);
                bool new_node = false;

                if (fill_child_node(current_node, pick, new_node, thread_id)) { // terminal state
                    if (_debug_logs) {
                        _execution_policy.protect([&current_node](){
                            spdlog::debug("Found" + ExecutionPolicy::print_thread() +
                                          " a terminal state: " + current_node->state.print() +
                                          ", depth=" + StringConverter::from(current_node->depth) +
                                          ", value=" + StringConverter::from(current_node->value) +
                                          ExecutionPolicy::print_thread());
                        }, current_node->mutex);
                    }
                    update_node(*current_node, true);
                    reached_end_of_trajectory_once = true;
                    break_loop = true;
                } else if (!novelty(feature_tuples, *current_node, new_node)) { // no new tuple or not reached with lower depth => terminal node
                    if (_debug_logs) {
                        _execution_policy.protect([&current_node](){
                            spdlog::debug("Pruning" + ExecutionPolicy::print_thread() +
                                          " state: " + current_node->state.print() +
                                          ", depth=" + StringConverter::from(current_node->depth) +
                                          ", value=" + StringConverter::from(current_node->value) +
                                          ExecutionPolicy::print_thread());
                        }, current_node->mutex);
                    }
                    states_pruned = true;
                    current_node->pruned = true;
                    // /!\ current_node can become solved with some unsolved children in case it was
                    // already visited and novel but now some of its features are reached with lower depth
                    update_node(*current_node, true);
                    break_loop = true;
                } else if (current_node->depth >= _max_depth) {
                    if (_debug_logs) {
                        _execution_policy.protect([&current_node](){
                            spdlog::debug("Max depth reached" + ExecutionPolicy::print_thread() +
                                          "in state: " + current_node->state.print() +
                                          ", depth=" + StringConverter::from(current_node->depth) +
                                          ", value=" + StringConverter::from(current_node->value) +
                                          ExecutionPolicy::print_thread());
                        }, current_node->mutex);
                    }
                    update_node(*current_node, true);
                    reached_end_of_trajectory_once = true;
                    break_loop = true;
                } else if (elapsed_time(start_time) >= _time_budget) {
                    if (_debug_logs) {
                        _execution_policy.protect([&current_node](){
                            spdlog::debug("Time budget consumed" + ExecutionPolicy::print_thread() +
                                          " in state: " + current_node->state.print() +
                                          ", depth=" + StringConverter::from(current_node->depth) +
                                          ", value=" + StringConverter::from(current_node->value) +
                                          ExecutionPolicy::print_thread());
                        }, current_node->mutex);
                    }
                    // next test: unexpanded node considered as a temporary (i.e. not solved) terminal node
                    // don't backup expanded node at this point otherwise the fscore initialization in update_node is wrong!
                    bool current_node_no_children = false;
                    _execution_policy.protect([&current_node_no_children, &current_node](){
                        current_node_no_children = current_node->children.empty();
                    }, current_node->mutex);
                    if (current_node_no_children) {
                        update_node(*current_node, false);
                    }
                    break_loop = true;
                }
            }
        }

        // Input: feature tuple vector, node for which to compute novelty depth, boolean indicating whether this node is new or not
        // Returns true if at least one tuple is new or is reached with lower depth
        bool novelty(TupleVector& feature_tuples, Node& n, bool nn) const {
            // feature_tuples is a set of state variable combinations of size _width
            std::size_t nov = n.features->size() + 1;
            const FeatureVector& state_features = *n.features;
            bool novel_depth = false;

            for (std::size_t k = 1 ; k <= std::min((std::size_t) _width, (std::size_t) state_features.size()) ; k++) {
                // we must recompute combinations from previous width values just in case
                // this state would be visited for the first time across width iterations
                generate_tuples(k, state_features.size(),
                                [this, &state_features, &feature_tuples, &k, &novel_depth, &n, &nn, &nov](TupleType& cv){
                    for (auto& e : cv) {
                        e.second = state_features[e.first];
                    }
                    _execution_policy.protect([&feature_tuples, &cv, &k, &novel_depth, &n, &nn, &nov]()->void {
                        auto it = feature_tuples[k-1].insert(std::make_pair(cv, (std::size_t) n.depth));
                        novel_depth = novel_depth || it.second || (nn && (it.first->second > n.depth)) || (!nn && (it.first->second == n.depth));
                        it.first->second = std::min(it.first->second, (std::size_t) n.depth);
                        if(it.second) {
                            nov = std::min(nov, k);
                        }
                    });
                });
            }
            n.novelty = nov;
            if (_debug_logs) spdlog::debug("Novelty: " + StringConverter::from(nov));
            if (_debug_logs) spdlog::debug("Novelty depth check: " + StringConverter::from(novel_depth));
            return novel_depth;
        }

        // Generates all combinations of size k from [0 ... (n-1)]
        void generate_tuples(const std::size_t& k,
                             const std::size_t& n,
                             const std::function<void (TupleType&)>& f) const {
            TupleType cv(k); // one combination (the first one)
            for (std::size_t i = 0 ; i < k ; i++) {
                cv[i].first = i;
            }
            f(cv);
            bool more_combinations = true;
            while (more_combinations) {
                more_combinations = false;
                // find the rightmost element that has not yet reached its highest possible value
                for (std::size_t i = k; i > 0; i--) {
                    if (cv[i-1].first < n - k + i - 1) {
                        // once finding this element, we increment it by 1,
                        // and assign the lowest valid value to all subsequent elements
                        cv[i-1].first++;
                        for (std::size_t j = i; j < k; j++) {
                            cv[j].first = cv[j-1].first + 1;
                        }
                        f(cv);
                        more_combinations = true;
                        break;
                    }
                }
            }
        }

        // Get the state reachable by calling the simulator from given node by applying given action number
        // Sets given node to the next one and returns whether the next one is terminal or not
        bool fill_child_node(Node*& node, std::size_t action_number, bool& new_node, const std::size_t* thread_id) {
            Node* node_child = nullptr;

            _execution_policy.protect([this, &node, &node_child, &action_number](){
                if (_debug_logs) spdlog::debug("Applying " + ExecutionPolicy::print_thread() +
                                               " action: " + std::get<0>(node->children[action_number]).print() +
                                               ExecutionPolicy::print_thread());
                node_child = std::get<2>(node->children[action_number]);
            }, node->mutex);

            if (!node_child) { // first visit
                // Sampled child has not been visited so far, so generate it
                typename Domain::TransitionOutcome outcome;
                _execution_policy.protect([this, &node, &outcome, &action_number, &thread_id](){
                    outcome = _rollout_policy.progress(_domain, node->state, std::get<0>(node->children[action_number]), thread_id);
                }, node->mutex);

                _execution_policy.protect([this, &node_child, &thread_id, &new_node, &outcome](){
                    auto i = _graph.emplace(Node(outcome.state(), _domain, _state_features, thread_id));
                    new_node = i.second;
                    node_child = &const_cast<Node&>(*(i.first)); // we won't change the real key (StateNode::state) so we are safe
                });
                Node& next_node = *node_child;
                _execution_policy.protect([&node, &action_number, &outcome, &node_child](){
                    std::get<1>(node->children[action_number]) = outcome.reward();
                    std::get<2>(node->children[action_number]) = node_child;
                }, node->mutex);
                if (outcome.reward() < _min_reward) {
                    _min_reward = outcome.reward();
                    _min_reward_changed = true;
                }

                _execution_policy.protect([this, &node, &next_node, &new_node, &outcome](){
                    next_node.parents.insert(node);
                    if (new_node) {
                        if (_debug_logs) spdlog::debug("Exploring" + ExecutionPolicy::print_thread() +
                                                       " new outcome: " + next_node.state.print() +
                                                       ", depth=" + StringConverter::from(next_node.depth) +
                                                       ", value=" + StringConverter::from(next_node.value) +
                                                       ExecutionPolicy::print_thread());
                        next_node.depth = node->depth + 1;
                        node = &next_node;
                        node->terminal = outcome.terminal();
                        if (node->terminal && _min_reward > 0.0) {
                            _min_reward = 0.0;
                            _min_reward_changed = true;
                        }
                    } else { // outcome already explored
                        if (_debug_logs) spdlog::debug("Exploring" + ExecutionPolicy::print_thread() +
                                                       " known outcome: " + next_node.state.print() +
                                                       ", depth=" + StringConverter::from(next_node.depth) +
                                                       ", value=" + StringConverter::from(next_node.value) +
                                                       ExecutionPolicy::print_thread());
                        next_node.depth = std::min((std::size_t) next_node.depth, (std::size_t) node->depth + 1);
                        node = &next_node;
                    }
                }, next_node.mutex);
            } else { // second visit, unsolved child
                new_node = false;
                // call the simulator to be coherent with the new current node /!\ Assumes deterministic environment!
                _execution_policy.protect([this, &node, &action_number, &thread_id](){
                    _rollout_policy.advance(_domain, node->state, std::get<0>(node->children[action_number]), false, thread_id);
                }, node->mutex);
                Node& next_node = *node_child;
                next_node.depth = std::min((std::size_t) next_node.depth, (std::size_t) node->depth + 1);
                node = node_child;
                if (_debug_logs) {
                    _execution_policy.protect([&node](){
                        spdlog::debug("Exploring" + ExecutionPolicy::print_thread() +
                                      " known outcome: " + node->state.print() +
                                      ", depth=" + StringConverter::from(node->depth) +
                                      ", value=" + StringConverter::from(node->value) +
                                      ExecutionPolicy::print_thread());
                    }, next_node.mutex);
                }
            }
            return (node->terminal) || (node->solved); // consider solved node as terminal to stop current rollout
        }

        void update_node(Node& node, bool solved) {
            node.solved = solved;
            node.value = 0;
            std::unordered_set<Node*> frontier;
            if (_min_reward_changed) {
                // need for backtracking all leaf nodes in the graph
                _execution_policy.protect([this, &frontier](){
                    for (auto& n : _graph) {
                        _execution_policy.protect([&n, &frontier](){
                            if (n.children.empty()) {
                                frontier.insert(&const_cast<Node&>(n)); // we won't change the real key (Node::state) so we are safe
                            }
                        }, n.mutex);
                    }
                });
                _min_reward_changed = false;
            } else {
                frontier.insert(&node);
            }
            _parent_solver.backup_values(frontier);
        }

        std::size_t elapsed_time(const std::chrono::time_point<std::chrono::high_resolution_clock>& start_time) {
            std::size_t milliseconds_duration;
            _execution_policy.protect([&milliseconds_duration, &start_time](){
                milliseconds_duration = static_cast<std::size_t>(
                    std::chrono::duration_cast<std::chrono::milliseconds>(
                        std::chrono::high_resolution_clock::now() - start_time
                    ).count()
                );
            }, _time_mutex);
            return milliseconds_duration;
        }
    }; // WidthSolver class

    void compute_reachable_subgraph(Node& node, std::unordered_set<Node*>& subgraph) {
        std::unordered_set<Node*> frontier;
        frontier.insert(&node);
        subgraph.insert(&node);
        while(!frontier.empty()) {
            std::unordered_set<Node*> new_frontier;
            for (auto& n : frontier) {
                if (n) {
                    for (auto& child : n->children) {
                        if (subgraph.find(std::get<2>(child)) == subgraph.end()) {
                            new_frontier.insert(std::get<2>(child));
                            subgraph.insert(std::get<2>(child));
                        }
                    }
                }
            }
            frontier = new_frontier;
        }
    }

    // Prune the nodes that are no more reachable from the root's chosen child and reduce the depth of the
    // nodes reachable from the child node by 1
    void update_graph(std::unordered_set<Node*>& root_subgraph, std::unordered_set<Node*>& child_subgraph) {
        std::unordered_set<Node*> removed_subgraph;
        // First pass: look for nodes in root_subgraph but not child_subgraph and remove
        // those nodes from their children's parents
        // Don't actually remove those nodes in the first pass otherwise some children to remove
        // won't exist anymore when looking for their parents
        std::unordered_set<Node*> frontier;
        for (auto& n : root_subgraph) {
            if (n) {
                if (_online_node_garbage && (child_subgraph.find(n) == child_subgraph.end())) {
                    for (auto& child : n->children) {
                        if (std::get<2>(child)) {
                            std::get<2>(child)->parents.erase(n);
                        }
                    }
                    removed_subgraph.insert(n);
                } else {
                    n->depth -= 1;
                    // if (n->solved) {
                    if (n->children.empty()) {
                        frontier.insert(n);
                    }
                }
            }
        }
        // Second pass: actually remove nodes in root_subgraph but not in child_subgraph
        for (auto& n : removed_subgraph) {
            _graph.erase(Node(n->state, _domain, _state_features, nullptr));
        }
        // Third pass: recompute fscores
        backup_values(frontier);
    }

    // Backup values from tip solved nodes to their parents in graph
    void backup_values(std::unordered_set<Node*>& frontier) {
        std::size_t depth = 0; // used to prevent infinite loop in case of cycles
        for (auto& n : frontier) {
            _execution_policy.protect([this, &n](){
                if (n->pruned) {
                    n->value = 0;
                    for (std::size_t d = 0 ; d < (_max_depth - n->depth) ; d++) {
                        n->value = _min_reward + (_discount * (n->value));
                    }
                }
            }, n->mutex);
        }

        while (!frontier.empty() && depth <= _max_depth) {
            depth += 1;
            std::unordered_set<Node*> new_frontier;
            for (auto& n : frontier) {
                update_frontier(new_frontier, n, &_execution_policy);
            }
            frontier = new_frontier;
        }
    }

    template <typename TTexecution_policy,
              std::enable_if_t<std::is_same<TTexecution_policy, SequentialExecution>::value, int> = 0>
    void update_frontier(std::unordered_set<Node*>& new_frontier, Node* n, [[maybe_unused]] TTexecution_policy* execution_policy) {
        for (auto& p : n->parents) {
            p->solved = true;
            p->value = -std::numeric_limits<double>::max();
            p->best_action = nullptr;
            for (auto& nn : p->children) {
                p->solved = p->solved && std::get<2>(nn) && std::get<2>(nn)->solved;
                if (std::get<2>(nn)) {
                    double tentative_value = std::get<1>(nn) + (_discount * std::get<2>(nn)->value);
                    if (p->value < tentative_value) {
                        p->value = tentative_value;
                        p->best_action = &std::get<0>(nn);
                    }
                }
            }
            new_frontier.insert(p);
        }
    }

    template <typename TTexecution_policy,
              std::enable_if_t<std::is_same<TTexecution_policy, ParallelExecution>::value, int> = 0>
    void update_frontier(std::unordered_set<Node*>& new_frontier, Node* n, [[maybe_unused]] TTexecution_policy* execution_policy) {
        std::list<Node*> parents;
        _execution_policy.protect([&n, &parents](){
            std::copy(n->parents.begin(), n->parents.end(), std::back_inserter(parents));
        }, n->mutex);
        for (auto& p : parents) {
            p->solved = true;
            p->value = -std::numeric_limits<double>::max();
            p->best_action = nullptr;
            _execution_policy.protect([this, &p](){
                for (auto& nn : p->children) {
                    p->solved = p->solved && std::get<2>(nn) && std::get<2>(nn)->solved;
                    if (std::get<2>(nn)) {
                        double tentative_value = std::get<1>(nn) + (_discount * std::get<2>(nn)->value);
                        if (p->value < tentative_value) {
                            p->value = tentative_value;
                            p->best_action = &std::get<0>(nn);
                        }
                    }
                }
            }, p->mutex);
            new_frontier.insert(p);
        }
    }
}; // RIWSolver class

} // namespace skdecide

#endif // SKDECIDE_RIW_HH


/* RTQL8, Copyright (c) 2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Sehoon Ha <sha9@gatech.edu>
 * Georgia Tech Graphics Lab
 */

 #define _USE_MATH_DEFINES

#include "pydart_api.h"
#include <iostream>
#include <string>
#include <vector>
#include <map>
#include <math.h>
using std::cout;
using std::cerr;
using std::endl;

// Boost headers
#include <boost/algorithm/string.hpp>

// Dart headers
#include "dart/dart.h"
// #include "dart/renderer/LoadOpengl.h"
// #include "dart/renderer/RenderInterface.h"
// #include "dart/renderer/OpenGLRenderInterface.h"
// #include "dart/simulation/World.h"
// #include "dart/dynamics/Skeleton.h"
// #include "dart/dynamics/BodyNode.h"
// #include "dart/dynamics/Shape.h"
// #include "dart/dynamics/MeshShape.h"
// #include "dart/dynamics/Joint.h"
// #include "dart/dynamics/DegreeOfFreedom.h"
// #include "dart/constraint/ConstraintSolver.h"
// #include "dart/collision/CollisionDetector.h"
// #include "dart/collision/dart/DARTCollisionDetector.h"
// #include "dart/collision/fcl/FCLCollisionDetector.h"
// #include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h"
// #include "dart/collision/bullet/BulletCollisionDetector.h"
// #include "dart/constraint/ContactConstraint.h"
// // #include "dart/utils/Paths.h"
// #include "dart/math/Geometry.h"
// #include "dart/utils/SkelParser.h"
// #include "dart/utils/sdf/SoftSdfParser.h"
// #include "dart/utils/urdf/DartLoader.h"
// #include "dart/utils/VskParser.h"
// #include "dart/utils/FileInfoWorld.h"

namespace pydart {

////////////////////////////////////////////////////////////
// class Manager
class Manager {
public:
    static void init();
    static void destroy();
    static Manager* getInstance() { return g_manager; }
    static dart::renderer::RenderInterface* getRI() {
        return g_ri;

    }

    static dart::simulation::WorldPtr world(int index = 0);
    static dart::dynamics::SkeletonPtr skeleton(int index);
    static dart::dynamics::SkeletonPtr skeleton(int wid, int skid);
    static int createWorld(double timestep);
    static int createWorldFromSkel(const char* const path);
    static void destroyWorld(int id);

protected:
    static Manager* g_manager;
    static dart::renderer::RenderInterface* g_ri;

    // std::vector<dart::simulation::WorldPtr> worlds;
    int next_id;
    std::map<int, dart::simulation::WorldPtr> worlds;

};

Manager* Manager::g_manager = NULL;
dart::renderer::RenderInterface* Manager::g_ri = NULL;

void Manager::init() {
    g_manager = new Manager();
    g_ri = new dart::renderer::OpenGLRenderInterface();
    //   g_ri->initialize();
    g_manager->next_id = 0;
    cout << " [pydart_api] Initialize pydart manager OK" << endl;
}

void Manager::destroy() {
    if (g_manager) {
        delete g_manager;
        g_manager = NULL;
    }
    if (g_ri) {
        delete g_ri;
        g_ri = NULL;
    }
    cout << " [pydart_api] Destroy pydart manager OK" << endl;
}

dart::simulation::WorldPtr Manager::world(int index) {
    Manager* manager = getInstance();
    return manager->worlds[index];
}

dart::dynamics::SkeletonPtr Manager::skeleton(int index) {
    return world()->getSkeleton(index);
}

dart::dynamics::SkeletonPtr Manager::skeleton(int wid, int skid) {
    return world(wid)->getSkeleton(skid);
}

int Manager::createWorld(double timestep) {
    Manager* manager = getInstance();

    dart::simulation::WorldPtr w(new dart::simulation::World());
    w->setTimeStep(timestep);
    w->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));
    // int id = manager->worlds.size();
    // manager->worlds.push_back(w);
    int id = manager->next_id++;
    manager->worlds[id] = w;
    return id;
}

int Manager::createWorldFromSkel(const char* const path) {
    Manager* manager = getInstance();

    dart::simulation::WorldPtr w(dart::utils::SkelParser::readWorld(path));
    // w->setTimeStep(timestep);
    // w->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));
    int id = manager->next_id++;
    manager->worlds[id] = w;
    // int id = manager->worlds.size();
    // manager->worlds.push_back(w);
    return id;
}

void Manager::destroyWorld(int id) {
    Manager* manager = getInstance();
    dart::simulation::WorldPtr w = manager->worlds[id];
    manager->worlds.erase(id);
    cout << " [pydart_api] worlds.size = " << manager->worlds.size() << endl;
    // w.reset();
    cout << " [pydart_api] Destroy world OK: " << id << endl;
}

// class Manager
////////////////////////////////////////////////////////////

} // namespace pydart

using namespace pydart;

////////////////////////////////////////////////////////////////////////////////
// Init Functions
void init() {
    if (Manager::getInstance()) {
        Manager::destroy();
    }
    Manager::init();
}

void destroy() {
    Manager::destroy();
}

////////////////////////////////////////////////////////////////////////////////
// Manipulation Functions
int createWorld(double timestep) {
    return Manager::createWorld(timestep);
}

int createWorldFromSkel(const char* const path) {
    int wid = Manager::createWorldFromSkel(path);
    std::cout << " [pydart_api] # Skeletons in " << path << " = " << numSkeletons(wid) << std::endl;
    return wid;
}

void destroyWorld(int wid) {
    Manager::destroyWorld(wid);
}

int saveWorldToFile(int wid, const char* const path) {
    using namespace dart::simulation;
    WorldPtr world = Manager::world(wid);
    dart::utils::FileInfoWorld* file = new dart::utils::FileInfoWorld();
    bool result = file->saveFile(path, world->getRecording());
    return (int)result;
}

int addSkeleton(int wid, const char* const path, double frictionCoeff) {
    using namespace dart::simulation;
    using namespace dart::dynamics;
    std::string strpath(path);
    std::string ext = strpath.substr(strpath.length() - 4);
    boost::algorithm::to_lower(ext);

    SkeletonPtr skel = NULL;
    if (ext == ".sdf") {
        cout << " [pydart_api] parse as SDF (ext: " << ext << ")" << endl;
        skel = dart::utils::SoftSdfParser::readSkeleton(path);
    } else if (ext == "urdf") {
        cout << " [pydart_api] parse as URDF (ext: " << ext << ")" << endl;
        dart::utils::DartLoader urdfLoader;
        skel = urdfLoader.parseSkeleton(path);
    } else if (ext == ".vsk") {
        cout << " [pydart_api] parse as VSK (ext: " << ext << ")" << endl;
	cout << "Not implemented yet" << endl;
	std::cin.get();
        //skel = dart::utils::VskParser::readSkeleton(path);
    } else {
        cout << " [pydart_api] bad extension (ext: " << ext << ")" << endl;
        return -1;
    }
    // SkeletonPtr skel = urdfLoader.parseSkeleton(path);

    cout << " [pydart_api] skel [" << path << "] : friction = " << frictionCoeff << endl;
    for (size_t i = 0; i < skel->getNumBodyNodes(); ++i) {
        dart::dynamics::BodyNode* bn = skel->getBodyNode(i);
        bn->setFrictionCoeff(frictionCoeff);
    }

    WorldPtr world = Manager::world(wid);
    int id = world->getNumSkeletons();
    world->addSkeleton(skel);

    // Debug
    dart::constraint::ConstraintSolver* solver = world->getConstraintSolver();
    dart::collision::CollisionDetector* detector = solver->getCollisionDetector();
    dart::collision::CollisionDetector* detector2 = new dart::collision::FCLMeshCollisionDetector();
    solver->setCollisionDetector(detector2);
    detector = detector2;

    if (dynamic_cast<dart::collision::DARTCollisionDetector*>(detector)) {
        std::cout << " [pydart_api] DARTCollisionDetector!" << std::endl;
    } else if (dynamic_cast<dart::collision::FCLCollisionDetector*>(detector)) {
        std::cout << " [pydart_api] FCLCollisionDetector!" << std::endl;
    } else if (dynamic_cast<dart::collision::FCLMeshCollisionDetector*>(detector)) {
        std::cout << " [pydart_api] FCLMeshCollisionDetector!" << std::endl;
    } else if (dynamic_cast<dart::collision::BulletCollisionDetector*>(detector)) {
        std::cout << " [pydart_api] BulletCollisionDetector!" << std::endl;
    } else {
        std::cout << " [pydart_api] Unknown CollisionDetector... (maybe bullet)" << std::endl;
    }

    dart::constraint::ContactConstraint::setErrorReductionParameter(0);
    std::cout << " [pydart_api] Zero ERP!!!" << std::endl;
    std::cout << " [pydart_api] ERP = " << dart::constraint::ContactConstraint::getErrorReductionParameter() << std::endl;

    return id;
}

int numSkeletons(int wid) {
    using namespace dart::simulation;
    WorldPtr world = Manager::world(wid);
    return world->getNumSkeletons();
}

void setSkeletonJointDamping(int wid, int skid, double damping) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);

    for (size_t i = 1; i < skel->getNumBodyNodes(); ++i) {
        dart::dynamics::Joint* joint = skel->getJoint(i);
        if (joint->getNumDofs() > 0) {
            for (size_t j = 0; j < joint->getNumDofs(); ++j) {
                joint->setDampingCoefficient(j, damping);
            }
        }
    }

}
////////////////////////////////////////////////////////////////////////////////
// Simulation Functions
void resetWorld(int wid) {
    using namespace dart::simulation;
    using namespace dart::dynamics;
    WorldPtr world = Manager::world(wid);
    world->reset();
    // Debug
    // std::cout << "reinitialize collision detector" << std::endl;
    // dart::constraint::ConstraintSolver* solver = world->getConstraintSolver();
    // dart::collision::CollisionDetector* detector = solver->getCollisionDetector();
    // std::cout << "reinitialize collision detector... creating new.." << std::endl;
    // dart::collision::CollisionDetector* detector2 = new dart::collision::FCLMeshCollisionDetector();
    // dart::collision::CollisionDetector* detector2 = new dart::collision::BulletCollisionDetector();
    // dart::collision::CollisionDetector* detector2 = new dart::collision::FCLCollisionDetector();
    // solver->setCollisionDetector(detector2);
    // std::cout << " [pydart_api] reinitialize collision detector... deleting old.." << std::endl;
    // delete detector;
    // std::cout << " [pydart_api] OK" << std::endl;
    for (int skid = 0; skid < numSkeletons(wid); skid++) {
        SkeletonPtr skel = Manager::skeleton(wid, skid);

        skel->resetCommands();
        skel->resetPositions();
        skel->resetVelocities();
        skel->resetAccelerations();
        skel->resetGeneralizedForces();
        skel->clearExternalForces();
        skel->clearConstraintImpulses();
    }
    // std::cout << "reinitialize collision detector... done" << std::endl;
}

void stepWorld(int wid) {
    using namespace dart::simulation;
    WorldPtr world = Manager::world(wid);
    world->step();
    // world->bake();
}

void render(int wid) {
    using namespace dart::simulation;
    dart::renderer::RenderInterface* ri = Manager::getRI();
    WorldPtr world = Manager::world(wid);

    for (size_t i = 0; i < world->getNumSkeletons(); i++) {
        world->getSkeleton(i)->draw(ri);
    }
}

void renderSkeleton(int wid, int skid) {
    using namespace dart::dynamics;
    dart::renderer::RenderInterface* ri = Manager::getRI();
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    skel->drawMarkers(ri);
}

void renderSkeletonWithColor(int wid, int skid, double r, double g, double b, double a) {
    using namespace dart::dynamics;
    dart::renderer::RenderInterface* ri = Manager::getRI();
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    skel->draw(ri, Eigen::Vector4d(r, g, b, a), false);
}

void renderSkeletonMarkers(int wid, int skid) {
    using namespace dart::dynamics;
    dart::renderer::RenderInterface* ri = Manager::getRI();
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    skel->drawMarkers(ri);
}

////////////////////////////////////////////////////////////////////////////////
// World Functions
double getWorldTime(int wid) {
    using namespace dart::simulation;
    WorldPtr world = Manager::world(wid);
    return world->getTime();
}

double getWorldTimeStep(int wid) {
    using namespace dart::simulation;
    WorldPtr world = Manager::world(wid);
    return world->getTimeStep();
}

void setWorldTimeStep(int wid, double _timeStep) {
    using namespace dart::simulation;
    WorldPtr world = Manager::world(wid);
    world->setTimeStep(_timeStep);
}

int getWorldSimFrames(int wid) {
    using namespace dart::simulation;
    WorldPtr world = Manager::world(wid);
    return world->getSimFrames();
}

void setWorldSimFrame(int wid, int playFrame) {
    using namespace dart::simulation;
    WorldPtr world = Manager::world(wid);
    if (playFrame >= world->getRecording()->getNumFrames()) {
        return;
    }

    size_t nSkels = world->getNumSkeletons();
    for (size_t i = 0; i < nSkels; i++) {
        // size_t start = world->getIndex(i);
        // size_t size = world->getSkeleton(i)->getNumDofs();
        world->getSkeleton(i)->setPositions(world->getRecording()->getConfig(playFrame, i));
        world->getSkeleton(i)->computeForwardKinematics(true, true, false);
    }
}

int getWorldNumContacts(int wid) {
    dart::simulation::WorldPtr world = Manager::world(wid);
    dart::collision::CollisionDetector* cd =
        world->getConstraintSolver()->getCollisionDetector();
    return cd->getNumContacts();
}

void getWorldContacts(int wid, double* outv, int len) {
    dart::simulation::WorldPtr world = Manager::world(wid);
    dart::collision::CollisionDetector* cd =
        world->getConstraintSolver()->getCollisionDetector();
    size_t n = cd->getNumContacts();
    if (7 * n != (size_t)len) {
        cerr << "getWorldContacts: 7n is needed for the output vector. n = " << n << ", len =  " << len << endl;
        return;
    }

    // ( v.x, v.y, v.z, p.x, p.y, p.z, id )

    int ptr = 0;
    for (size_t i = 0; i < n; i++) {
        Eigen::Vector3d v = cd->getContact(i).point;
        Eigen::Vector3d f = cd->getContact(i).force;
        for (int j = 0; j < 3; j++) {
            outv[ptr++] = v(j);
        }
        for (int j = 0; j < 3; j++) {
            outv[ptr++] = f(j);
        }
        outv[ptr++] = i;

    }
}

void setWorldCollisionPair(int wid, int skid1, int bid1, int skid2, int bid2, int bEnable) {
    using namespace dart::simulation;
    using namespace dart::dynamics;
    // Get collision detector
    WorldPtr world = Manager::world(wid);
    dart::constraint::ConstraintSolver* solver = world->getConstraintSolver();
    dart::collision::CollisionDetector* detector = solver->getCollisionDetector();

    // Get body1 and body2
    SkeletonPtr skel1 = Manager::skeleton(wid, skid1);
    BodyNode* body1 = skel1->getBodyNode(bid1);
    SkeletonPtr skel2 = Manager::skeleton(wid, skid2);
    BodyNode* body2 = skel2->getBodyNode(bid2);
    // cout << " [pydart_api] set collision pair "
    //      << "(skel " << skid1 << " body " <<  bid1 << ") and "
    //      << "(skel " << skid2 << " body " <<  bid2 << ")"
    //      << " as " << bEnable << endl;
    bool enable = (bEnable != 0);
    if (enable) {
        detector->enablePair(body1, body2);
    } else {
        detector->disablePair(body1, body2);
    }
}

////////////////////////////////////////////////////////////////////////////////
// Skeleton Attribute Functions
const char* getSkeletonName(int wid, int skid) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    return skel->getName().c_str();
}

double getSkeletonMass(int wid, int skid) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    return skel->getMass();
}

int getSkeletonNumDofs(int wid, int skid) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    return skel->getNumDofs();
}

int getSkeletonNumBodies(int wid, int skid) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    return skel->getNumBodyNodes();
}

int getSkeletonNumJoints(int wid, int skid) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    return skel->getNumJoints();
}

const char* getSkeletonBodyName(int wid, int skid, int bodyid) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    return skel->getBodyNode(bodyid)->getName().c_str();
}

const char* getSkeletonDofName(int wid, int skid, int dofid) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    // return skel->getGenCoordInfo(dofid).joint->getName().c_str();
    return skel->getDof(dofid)->getName().c_str();
}

const char* getSkeletonJointName(int wid, int skid, int jointid) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    return skel->getJoint(jointid)->getName().c_str();
}

int getSkeletonMobile(int wid, int skid) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    if (skel->isMobile()) {
        return 1;
    } else {
        return 0;
    }
}

void setSkeletonMobile(int wid, int skid, int mobile) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    skel->setMobile(mobile != 0);
}

void setSkeletonSelfCollision(int wid, int skid, int bSelfCollision, int bAdjacentBodies) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    if (bSelfCollision != 0) {
        bool _enableAdjecentBodies = (bAdjacentBodies != 0);
        cout << " [pydart_api] enable skeleton self collision: skid = " << skid << endl;
        cout << " [pydart_api] _enableAdjecentBodies = " << _enableAdjecentBodies << endl;
        skel->enableSelfCollision(_enableAdjecentBodies);
    } else {
        cout << " [pydart_api] disable skeleton self collision: skid = " << skid << endl;
        skel->disableSelfCollision();
    }
}

////////////////////////////////////////////////////////////////////////////////
// Skeleton Pose Functions
void getSkeletonPositions(int wid, int skid, double* outpose, int ndofs) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);

    Eigen::VectorXd q = skel->getPositions();
    for (int i = 0; i < q.size(); i++) {
        outpose[i] = q(i);
    }
}

void getSkeletonVelocities(int wid, int skid, double* outpose, int ndofs) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);

    Eigen::VectorXd qdot = skel->getVelocities();
    for (int i = 0; i < qdot.size(); i++) {
        outpose[i] = qdot(i);
    }
}

void getSkeletonMassMatrix(int wid, int skid, double* array2, int nrows, int ncols) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    const Eigen::MatrixXd& M = skel->getMassMatrix();

    int ptr = 0;
    for (int i = 0; i < M.rows(); i++) {
        for (int j = 0; j < M.cols(); j++) {
            array2[ptr++] = M(i, j);
        }
    }
}

void getSkeletonCoriolisAndGravityForces(int wid, int skid, double* outpose, int ndofs) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    const Eigen::VectorXd& C = skel->getCoriolisAndGravityForces();
    for (int i = 0; i < C.size(); i++) {
        outpose[i] = C(i);
    }
}

void getSkeletonConstraintForces(int wid, int skid, double* outpose, int ndofs) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    const Eigen::VectorXd& F = skel->getConstraintForces();
    for (int i = 0; i < F.size(); i++) {
        outpose[i] = F(i);
    }
}

void setSkeletonPositions(int wid, int skid, double* inpose, int ndofs) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);

    Eigen::VectorXd q(ndofs);
    for (int i = 0; i < q.size(); i++) {
        q(i) = inpose[i];
    }
    skel->setPositions(q);
    // skel->computeForwardKinematics(true, true, false);
}

void setSkeletonVelocities(int wid, int skid, double* inpose, int ndofs) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);

    Eigen::VectorXd q(ndofs);
    for (int i = 0; i < q.size(); i++) {
        q(i) = inpose[i];
    }
    skel->setVelocities(q);
    // skel->computeForwardKinematics(true, true, false);
}

void setSkeletonForces(int wid, int skid, double* intorque, int ndofs) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);

    Eigen::VectorXd tau(ndofs);
    for (int i = 0; i < tau.size(); i++) {
        tau(i) = intorque[i];
    }
    skel->setForces(tau);
}

////////////////////////////////////////////////////////////////////////////////
// Skeleton Limit Functions
void getSkeletonPositionLowerLimit(int wid, int skid, double* outpose, int ndofs) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    for (int i = 0; i < ndofs; i++) {
        outpose[i] = skel->getPositionLowerLimit(i);
    }
}

void getSkeletonPositionUpperLimit(int wid, int skid, double* outpose, int ndofs) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    for (int i = 0; i < ndofs; i++) {
        outpose[i] = skel->getPositionUpperLimit(i);
    }
}

void getSkeletonForceLowerLimit(int wid, int skid, double* outpose, int ndofs) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    for (int i = 0; i < ndofs; i++) {
        outpose[i] = skel->getForceLowerLimit(i);
    }
}

void getSkeletonForceUpperLimit(int wid, int skid, double* outpose, int ndofs) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    for (int i = 0; i < ndofs; i++) {
        outpose[i] = skel->getForceUpperLimit(i);
    }
}

////////////////////////////////////////////////////////////////////////////////
// Skeleton Momentum Functions
void getSkeletonWorldCOM(int wid, int skid, double outv3[3]) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    // Eigen::Vector3d C = skel->getWorldCOM();
    Eigen::Vector3d C = skel->getCOM();
    for (int i = 0; i < C.size(); i++) {
        outv3[i] = C(i);
    }
}

void getSkeletonWorldCOMVelocity(int wid, int skid, double outv3[3]) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    // Eigen::Vector3d CV = skel->getWorldCOMVelocity();
    Eigen::Vector3d CV = skel->getCOMLinearVelocity();
    for (int i = 0; i < CV.size(); i++) {
        outv3[i] = CV(i);
    }
}

////////////////////////////////////////////////////////////////////////////////
// BodyNode Functions
double getBodyNodeMass(int wid, int skid, int bid) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    return bn->getMass();
}

void getBodyNodeInertia(int wid, int skid, int bid, double outv33[3][3]) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
    bn->getMomentOfInertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz);
    outv33[0][0] = Ixx;    outv33[1][1] = Iyy;    outv33[2][2] = Izz;
    outv33[0][1] = Ixy;    outv33[1][0] = Ixy;
    outv33[0][2] = Ixz;    outv33[2][0] = Ixz;
    outv33[1][2] = Iyz;    outv33[2][1] = Iyz;
}

void getBodyNodeLocalCOM(int wid, int skid, int bid, double outv3[3]) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    const Eigen::Vector3d& x = bn->getLocalCOM();
    for (int i = 0; i < x.size(); i++) {
        outv3[i] = x(i);
    }
}

void getBodyNodeShapeBoundingBoxDim(int wid, int skid, int bid, double outv3[3]) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    // cout << "bodynode " << bid << " is fetched" << endl;
    // Assume single shape for now
    const int num_shapes = bn->getNumCollisionShapes();
    if (num_shapes == 0) {
        for (int i = 0; i < 3; i++) {
            outv3[i] = 0.0;
        }
    } else {
        dart::dynamics::ShapePtr shape = bn->getCollisionShape(0);
        Eigen::Vector3d scale(1, 1, 1);
        dart::dynamics::MeshShape* mshape = dynamic_cast<dart::dynamics::MeshShape*>(shape.get());
        if (mshape) {
            scale = mshape->getScale();
        }
        // cout << "Scale:" << scale << endl;
        const Eigen::Vector3d& x = shape->getBoundingBoxDim();
        for (int i = 0; i < x.size(); i++) {
            outv3[i] = x(i) * scale(i);
        }
    }
}

void getBodyNodeWorldCOM(int wid, int skid, int bid, double outv3[3]) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    // const Eigen::Vector3d& x = bn->getWorldCOM();
    const Eigen::Vector3d& x = bn->getCOM();
    for (int i = 0; i < x.size(); i++) {
        outv3[i] = x(i);
    }
}

void getBodyNodeWorldCOMVelocity(int wid, int skid, int bid, double outv3[3]) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    // const Eigen::Vector3d& x = bn->getWorldCOMVelocity();
    const Eigen::Vector3d& x = bn->getCOMLinearVelocity();
    for (int i = 0; i < x.size(); i++) {
        outv3[i] = x(i);
    }
}

void getBodyNodeWorldCOMSpatialVelocity(int wid, int skid, int bid, double outv6[6]) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    const Eigen::Vector6d& x = bn->getCOMSpatialVelocity();
    for (int i = 0; i < x.size(); i++) {
        outv6[i] = x(i);
    }
}

void getBodyNodeWorldCOMSpatialAcceleration(int wid, int skid, int bid, double outv6[6]) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    const Eigen::Vector6d& x = bn->getCOMSpatialAcceleration();
    // cout << "C = " << bn->getCOM().transpose() << endl;
    // cout << "x = " << x.transpose() << endl;
    for (int i = 0; i < x.size(); i++) {
        outv6[i] = x(i);
    }
}

void getBodyNodeLocalCOMSpatialVelocity(int wid, int skid, int bid, double outv6[6]) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    const dart::dynamics::Frame* world = dart::dynamics::Frame::World();
    const Eigen::Vector6d& x = bn->getCOMSpatialVelocity(world, bn);
    for (int i = 0; i < x.size(); i++) {
        outv6[i] = x(i);
    }
}

void getBodyNodeLocalCOMSpatialAcceleration(int wid, int skid, int bid, double outv6[6]) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    const dart::dynamics::Frame* world = dart::dynamics::Frame::World();
    const Eigen::Vector6d& x = bn->getCOMSpatialAcceleration(world, bn);
    for (int i = 0; i < x.size(); i++) {
        outv6[i] = x(i);
    }
}

int getBodyNodeNumContacts(int wid, int skid, int bid) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    // dart::dynamics::BodyNode* bn = skel->getBodyNode(bid);
    dart::dynamics::BodyNodePtr bn = skel->getBodyNode(bid);

    dart::simulation::WorldPtr world = Manager::world(wid);
    dart::collision::CollisionDetector* cd =
        world->getConstraintSolver()->getCollisionDetector();
    size_t n = cd->getNumContacts();
    int cnt = 0;
    for (size_t i = 0; i < n; i++) {
        dart::collision::Contact& c = cd->getContact(i);
        if (c.bodyNode1.lock() == bn || c.bodyNode2.lock() == bn) {
            cnt++;
        }
    }
    return cnt;
}

void getBodyNodeContacts(int wid, int skid, int bid, double* outv, int len) {
    dart::dynamics::SkeletonPtr skel = Manager::skeleton(wid, skid);
    dart::dynamics::BodyNodePtr bn = skel->getBodyNode(bid);

    dart::simulation::WorldPtr world = Manager::world(wid);
    dart::collision::CollisionDetector* cd =
        world->getConstraintSolver()->getCollisionDetector();
    size_t n = cd->getNumContacts();

    int m = 0;
    for (size_t i = 0; i < n; i++) {
        dart::collision::Contact& c = cd->getContact(i);
        if (c.bodyNode1.lock() != bn && c.bodyNode2.lock() != bn) {
            continue;
        }
        m++;
    }

    if (7 * m != len) {
        cerr << "getBodyNodeContacts: 7m is needed for the output vector. m = " << m
             << ", n = " << n << ", len =  " << len << endl;
        return;
    }

    int ptr = 0;
    for (size_t i = 0; i < n; i++) {
        dart::collision::Contact& c = cd->getContact(i);
        if (c.bodyNode1.lock() != bn && c.bodyNode2.lock() != bn) {
            continue;
        }
        Eigen::Vector3d v = cd->getContact(i).point;
        Eigen::Vector3d f = cd->getContact(i).force;
        for (int j = 0; j < 3; j++) {
            outv[ptr++] = v(j);
        }
        for (int j = 0; j < 3; j++) {
            outv[ptr++] = f(j);
        }
        outv[ptr++] = i;

    }
}

void getBodyNodeTransformation(int wid, int skid, int bid, double outv44[4][4]) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    BodyNode* body = skel->getBodyNode(bid);
    if (!body) {
        cerr << "cannot find the body : " << bid << endl;
    }
    const Eigen::Isometry3d& T = body->getTransform();
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            outv44[i][j] = T(i, j);
        }
    }
}

void getBodyNodeWorldLinearJacobian(int wid, int skid, int bid, double inv3[3], double* array2, int nrows, int ncols) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    BodyNode* body = skel->getBodyNode(bid);
    if (!body) {
        cerr << "cannot find the body : " << bid << endl;
    }
    Eigen::Vector3d offset(inv3[0], inv3[1], inv3[2]);

    int N = skel->getNumDofs();
    // Eigen::MatrixXd J = body->getWorldLinearJacobian(offset);
    dart::math::LinearJacobian J = body->getLinearJacobian(offset);
    Eigen::MatrixXd JF = Eigen::MatrixXd::Zero(3, N);

    for (int i = 0; i < J.cols(); i++) {
        int j = body->getDependentGenCoordIndex(i);
        JF.col(j) = J.col(i);
    }

    int ptr = 0;
    for (int i = 0; i < JF.rows(); i++) {
        for (int j = 0; j < JF.cols(); j++) {
            array2[ptr++] = JF(i, j);
        }
    }
}

void addBodyNodeExtForce(int wid, int skid, int bid, double inv3[3]) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    BodyNode* body = skel->getBodyNode(bid);
    if (!body) {
        cerr << "cannot find the body : " << bid << endl;
    }
    Eigen::Vector3d f(inv3[0], inv3[1], inv3[2]);
    body->addExtForce(f);
}

void addBodyNodeExtForceAt(int wid, int skid, int bid, double inv3[3], double inv3_2[3]) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    BodyNode* body = skel->getBodyNode(bid);
    if (!body) {
        cerr << "cannot find the body : " << bid << endl;
    }
    Eigen::Vector3d f(inv3[0], inv3[1], inv3[2]);
    Eigen::Vector3d offset(inv3_2[0], inv3_2[1], inv3_2[2]);
    body->addExtForce(f, offset);
}

////////////////////////////////////////////////////////////////////////////////
// Joint Functions
void getJointTransform(int wid, int skid, int jid, double outv44[4][4]) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    Joint* joint = skel->getJoint(jid);
    if (!joint) {
        cerr << "cannot find the joint : " << jid << endl;
    }
    BodyNode* body = joint->getChildBodyNode();
    const Eigen::Isometry3d& T_j = joint->getTransformFromChildBodyNode();
    const Eigen::Isometry3d& T_b = body->getTransform();
    const Eigen::Isometry3d& T = T_b*T_j;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            outv44[i][j] = T(i, j);
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
// Marker Functions
int getBodyNodeNumMarkers(int wid, int skid, int bid) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    BodyNode* body = skel->getBodyNode(bid);
    return body->getNumMarkers();
}

void getMarkerPosition(int wid, int skid, int bid, int mid, double outv3[3]) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    BodyNode* body = skel->getBodyNode(bid);
    Marker* marker = body->getMarker(mid);
    if (!marker) {
        cerr << "cannot find the marker : " << mid << endl;
    }
    const Eigen::Vector3d& x = marker->getWorldPosition();
    for (int i = 0; i < x.size(); i++) {
        outv3[i] = x(i);
    }
}

////////////////////////////////////////////////////////////////////////////////
// C3D Functions
int readC3D(const char* const path, double* outv, int len) {
    dart::utils::FileInfoC3D file;
    file.loadFile(path);
    int nm = file.getNumMarkers();
    int nf = file.getNumFrames();
    if (len == 0) {
        cout << " [pydart_api] # Markers = " << nm << endl;
        cout << " [pydart_api] # Frames = " << nf << endl;
        return 3 * nf * nm + 2; // return buffer size
    } else if (len == 3 * nf * nm + 2) {
        const int DIM = 3;
        int ptr = 0;
        outv[ptr++] = nf;
        outv[ptr++] = nm;
        for (int i = 0; i < nf; i++) {
            for (int j = 0; j < nm; j++) {
                Eigen::Vector3d m = file.getDataAt(i, j);
                for (int k = 0; k < DIM; k++) {
                    outv[ptr++] = m[k];
                    // cout << ptr << " / " << i << " " << j << " " << k << " " << len << endl;
                }
            }
        }
        cout << " [pydart_api] load C3D [" << path << "] OK" << endl;
        return 0;
    } else {
        cerr << "invalid buffer size: " << len << endl;
        return -1;
    }
}

////////////////////////////////////////////////////////////////////////////////
// Auxiliary Functions

double C_D(double theta_rad) {
    return 2.0 * (1.0-fabs(cos(theta_rad))) + 0.005;
}

double C_L(double theta_rad) {
    double theta = theta_rad * 180.0 / M_PI;
    double xp[4] = {-85.0, -10.0, 30.0, 85.0};
    double fp[4] = {-0.2, -0.4, 2.0, 0.8};
    int lo, hi;
    double t;

    if (theta <= xp[0] || theta >= xp[3]) {
        return 0.0;
    } else if (theta >= xp[2]) {
        lo = 2; hi = 3;
    } else if (theta >= xp[1]) {
        lo = 1; hi = 2;
    } else {
        lo = 0; hi = 1;
    }

    t = (theta-xp[lo]) / (xp[hi]-xp[lo]);
    return (1.0-t)*fp[lo]+t*fp[hi];
}

Eigen::Vector3d computeAerodynamics(const Eigen::Vector3d& velocity, const Eigen::Vector3d& normal, const double area) {
    const double rho = 1000.0;
    const double eps = 1.0e-5;

    // Eigen::Vector3d& v_norm = v.normalized();

    // Zero Velocities
    double v_norm = velocity.norm();
    if (v_norm < eps)
        return Eigen::Vector3d::Zero();

    Eigen::Vector3d v = -velocity;
    Eigen::Vector3d d_normal = -normal.normalized();
    Eigen::Vector3d d_drag = v.normalized();

    // Check Reverse Direction
    if (d_normal.dot(d_drag) < 0.0)
        return Eigen::Vector3d::Zero();

    Eigen::Vector3d l = Eigen::Vector3d::Zero();

    // When normal and drag direction coincide
    if ((d_normal-d_drag).norm() < eps) {
        if (d_normal[0] > 0.5 || d_normal[1] > 0.5) {
            l = Eigen::Vector3d(d_normal[1],-d_normal[0],0.0);
        } else {
            l = Eigen::Vector3d(0.0,-d_normal[2],d_normal[1]);
        }
    } else {
        l = d_drag.cross(d_normal);
    }

    l.normalize();
    Eigen::Vector3d d_lift = l.cross(d_drag).normalized();

    Eigen::Vector3d v_n = d_drag.dot(d_normal) * v;
    Eigen::Vector3d v_t = v - v_n;

    double theta = atan2(v_n.dot(d_normal),v_t.norm());

    Eigen::Vector3d f_drag = 0.5 * rho * area * C_D(theta) * v_norm * v_norm * d_drag;
    Eigen::Vector3d f_lift = 0.5 * rho * area * C_L(theta) * v_norm * v_norm * d_lift;
    Eigen::Vector3d f_total = f_drag + f_lift;

    return f_total;
}

void addAerodynamicForce(int wid, int skid, int bid) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    BodyNode* body = skel->getBodyNode(bid);
    if (!body) {
        cerr << "cannot find the body : " << bid << endl;
    }
    double d[3];
    getBodyNodeShapeBoundingBoxDim(wid, skid, bid, d);
    Eigen::Vector3d v = body->getCOMLinearVelocity();
    const Eigen::Isometry3d& T = body->getTransform();

    for (size_t i = 0; i < 3; i++) {
        Eigen::Vector3d n1 = Eigen::Vector3d(T(0,i),T(1,i),T(2,i));
        Eigen::Vector3d n2 = -n1;
        Eigen::Vector3d offset1 = Eigen::Vector3d::Zero();
        Eigen::Vector3d offset2 = Eigen::Vector3d::Zero();
        offset1[i] = 0.5*d[i];
        offset2[i] = -0.5*d[i];
        double area = d[(i+1)%3]*d[(i+2)%3];

        Eigen::Vector3d f1 = computeAerodynamics(v,n1,area);
        Eigen::Vector3d f2 = computeAerodynamics(v,n2,area);

        body->addExtForce(f1, offset1);//, offset1, false, false);
        body->addExtForce(f2, offset2);//, offset2, false, false);
    }
}

void addAerodynamicForce(int wid, int skid) {
    using namespace dart::dynamics;
    SkeletonPtr skel = Manager::skeleton(wid, skid);
    for (size_t i = 0; i < skel->getNumBodyNodes(); i++) {
        addAerodynamicForce(wid, skid, i);
    }
}

void addAerodynamicForce(int wid) {
    using namespace dart::dynamics;
    for (int skid = 0; skid < numSkeletons(wid); skid++) {
        addAerodynamicForce(wid, skid);
    }
}


#include <iostream>
#include <complex>
#include <fstream>

#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/tokenizer.hpp>
#include <boost/token_functions.hpp>

#include "simulationexecutor.h"
#include "TridiagonalMatrix.h"

using namespace boost;
using namespace boost::program_options;

int main(int argc, char** argv) {
    options_description desc(
        "Crank Nicolson solver for one dimensional waves.\n"
        "This program solves the timedependent Schroedinger equation\n"
        "for one dimensional waves.\n"
        "The only parameter which is nessesary is the simulation file.\n"
        "This simulation file contains the data and\na path to a script to run a simulation.\n"
    );
    desc.add_options()
            ("help,h", "Show this help text")
            ("files,f", value<std::vector<std::string>>(), "Simulation files");

    variables_map vm;
    try {
        store(command_line_parser(argc, argv).options(desc).run(), vm);
        notify(vm);
    } catch (std::exception &e) {
        std::cout << e.what() << std::endl;
        std::cout << desc << std::endl;
    }

    if (vm.count("help")) {
        desc.print(std::cout);
    }

    if (vm.count("files")) {
        std::vector<std::string> files = vm["files"].as<std::vector<std::string>>();
        for (auto file : files) {
            std::cout << "process file: " << file << std::endl;
            SimulationExecutor(file.c_str()); //only call constructor
        }
    }

    return 0;
}