#pragma once

#include <Eigen/Dense>

#include <init_traj_planner.hpp>
#include <mission.hpp>
#include <param.hpp>
#include <timer.hpp>
#include <optimization.h>

// Set the timestep length
size_t N;
int qn;

double plan[32][500][5];

class Corridor {
public:
    SFC_t SFC; // safe flight corridors to avoid obstacles

    Corridor(std::shared_ptr<InitTrajPlanner> _initTrajPlanner_obj,
             std::shared_ptr<DynamicEDTOctomap> _distmap_obj,
             SwarmPlanning::Mission _mission,
             SwarmPlanning::Param _param)
            : initTrajPlanner_obj(std::move(_initTrajPlanner_obj)),
              distmap_obj(std::move(_distmap_obj)),
              mission(std::move(_mission)),
              param(std::move(_param))
    {
        initTraj = initTrajPlanner_obj.get()->initTraj;
        T = initTrajPlanner_obj.get()->T;
        makespan = T.back();
        M = T.size()-1; // the number of segments

        qn = mission.qn; // the number of agents

        int i,j,k;

        double dx;
        double dy;
        double dis;

        for (i=0;i<qn;i++){
            for(j=0;j<M;j++){

                dx=(initTraj[i][j+1].x()-initTraj[i][j].x())/5;
                dy=(initTraj[i][j+1].y()-initTraj[i][j].y())/5;
                dis= 25*(dx*dx+dy*dy);

                for (k=0;k<5;k++){
                    plan[i][5*j+k][0]=initTraj[i][j].x()+dx*k;
                    plan[i][5*j+k][1]=initTraj[i][j].y()+dy*k;

                    if (dis<0.1)
                    {
                      plan[i][5*j+k][3]=0;
                    }
                    if (dis<1.1){
                      plan[i][5*j+k][3]=MAXV*0.7;
                    }
                    else{
                      plan[i][5*j+k][3]=MAXV;
                    }
                }
            }
            plan[i][5*M][0]=initTraj[i][M].x();
            plan[i][5*M][1]=initTraj[i][M].y();
        }

        N = M*5+1;
    }

    bool update(bool log){
        return updateObsBox(log);
    }

private:
    std::shared_ptr<InitTrajPlanner> initTrajPlanner_obj;
    std::shared_ptr<DynamicEDTOctomap> distmap_obj;
    SwarmPlanning::Mission mission;
    SwarmPlanning::Param param;

    initTraj_t initTraj;
    std::vector<double> T;
    double makespan;
    int M;

    bool isObstacleInBox(const std::vector<double>& box, double margin){
        double x,y,z;
        int count1 = 0;
        for(double i = box[0]; i < box[3] + SP_EPSILON_FLOAT; i += param.box_xy_res) {
            int count2 = 0;
            for(double j = box[1]; j < box[4] + SP_EPSILON_FLOAT; j += param.box_xy_res) {
                int count3 = 0;
                for(double k = box[2]; k < box[5] + SP_EPSILON_FLOAT; k += param.box_z_res) {
                    x = i + SP_EPSILON_FLOAT;
                    if(count1 == 0)
                        x = box[0] - SP_EPSILON_FLOAT;
                    y = j + SP_EPSILON_FLOAT;
                    if(count2 == 0)
                        y = box[1] - SP_EPSILON_FLOAT;
                    z = k + SP_EPSILON_FLOAT;
                    if(count3 == 0)
                        z = box[2] - SP_EPSILON_FLOAT;

                    octomap::point3d cur_point(x, y, z);
                    float dist = distmap_obj.get()->getDistance(cur_point);

                    assert(dist>=0);
                    if(dist < margin - SP_EPSILON_FLOAT){
                        return true;
                    }
                    count3++;
                }
                count2++;
            }
            count1++;
        }

        return false;
    }

    bool isBoxInBoundary(const std::vector<double>& box){
        return box[0] > param.world_x_min - SP_EPSILON&&
               box[1] > param.world_y_min - SP_EPSILON&&
               box[3] < param.world_x_max + SP_EPSILON&&
               box[4] < param.world_y_max + SP_EPSILON;
    }

    bool isPointInBox(const octomap::point3d& point,
                      const std::vector<double>& box){
        return point.x() > box[0] - SP_EPSILON &&
               point.y() > box[1] - SP_EPSILON &&
               point.x() < box[3] + SP_EPSILON &&
               point.y() < box[4] + SP_EPSILON;
    }

    void expand_box(std::vector<double>& box, double margin) {
        std::vector<double> box_cand, box_update;
        std::vector<int> axis_cand{0, 1, 2, 3, 4, 5};

        int i = -1;
        int axis;
        while (!axis_cand.empty()) {
            box_cand = box;
            box_update = box;

            //check update_box only! update_box + current_box = cand_box
            while ( !isObstacleInBox(box_update, margin) && isBoxInBoundary(box_update) ) {
                i++;
                if(i >= axis_cand.size()){
                    i = 0;
                }
                axis = axis_cand[i];

                //update current box
                box = box_cand;
                box_update = box_cand;

                //expand cand_box and get updated part of box(update_box)
                if (axis < 3) {
                    box_update[axis+3] = box_cand[axis];
                    if (axis == 2){
                        box_cand[axis] = box_cand[axis] - param.box_z_res;
                    }
                    else {
                        box_cand[axis] = box_cand[axis] - param.box_xy_res;
                    }
                    box_update[axis] = box_cand[axis];
                }
                else{
                    box_update[axis-3] = box_cand[axis];
                    if (axis == 5){
                        box_cand[axis] = box_cand[axis] + param.box_z_res;
                    }
                    else{
                        box_cand[axis] = box_cand[axis] + param.box_xy_res;
                    }
                    box_update[axis] = box_cand[axis];
                }
            }
            axis_cand.erase(axis_cand.begin() + i);
            if(i > 0) {
                i--;
            }
            else{
                i = axis_cand.size()-1;
            }
        }
    }

    bool updateObsBox(bool log){
        double x_next, y_next, z_next, dx, dy, dz;

        int count=0;
        SFC.resize(mission.qn);
        for (size_t qi = 0; qi < mission.qn; ++qi) {
            std::vector<double> box_prev{0,0,0,0,0,0};

            double height=param.height;
            for (int i = 0; i < N-1; i++) {

                double x = plan[qi][i][0];
                double y = plan[qi][i][1];
                double z = height;

                std::vector<double> box;

                x_next = plan[qi][i+1][0];
                y_next = plan[qi][i+1][1];
                z_next = height;

                if(isPointInBox(octomap::point3d(x_next, y_next, z_next), box_prev)){
                    continue;
                }

                // Initialize box

                box.emplace_back(round(std::min(x,x_next) / param.box_xy_res) * param.box_xy_res);
                box.emplace_back(round(std::min(y,y_next) / param.box_xy_res) * param.box_xy_res);
                box.emplace_back(round(std::min(z,z_next) / param.box_z_res) * param.box_z_res);
                box.emplace_back(round(std::max(x,x_next) / param.box_xy_res) * param.box_xy_res);
                box.emplace_back(round(std::max(y,y_next) / param.box_xy_res) * param.box_xy_res);
                box.emplace_back(round(std::max(z,z_next) / param.box_z_res) * param.box_z_res);
                box.emplace_back(round(std::max(z,z_next) / param.box_z_res) * param.box_z_res + param.box_z_res);

                //if (isObstacleInBox(box, mission.quad_size[qi])) {
                if (isObstacleInBox(box, mission.quad_size[qi]+0.1)) {
                    box.clear();
                    box.emplace_back(round(x_next / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(y_next / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(z_next / param.box_z_res) * param.box_z_res);
                    box.emplace_back(round(x_next / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(y_next / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(z_next / param.box_z_res) * param.box_z_res);

                }
                expand_box(box, mission.quad_size[qi]+0.1);

                if (box[3]==box[0] || box[4]==box[1]){
                    box.emplace_back(round(x_next / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(y_next / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(z_next / param.box_z_res) * param.box_z_res);
                    box.emplace_back(round(x_next / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(y_next / param.box_xy_res) * param.box_xy_res);
                    box.emplace_back(round(z_next / param.box_z_res) * param.box_z_res);
                    expand_box(box, mission.quad_size[qi]+0.05);
                    count++;
                }

                SFC[qi].emplace_back(std::make_pair(box, i+1));

                box_prev = box;
            }

        }

        if(param.log)
            std::cout<<"Safe Corridor construction error: "<<count<<std::endl;

        return true;
    }

};


#ifndef SEMPR_COMPONENT_AFFINETRANSFORM_HPP_
#define SEMPR_COMPONENT_AFFINETRANSFORM_HPP_

#include <Eigen/Eigen>
#include "../Component.hpp"

namespace sempr {

/**
    The AffineTransform class is a simple wrapper for an Eigen::Affine3d
    instance. The magic comes and falls with builtins that allow us to work
    with these in the rules, and apply them to geometries etc.
*/
class AffineTransform : public Component {
    SEMPR_COMPONENT

    Eigen::Affine3d transform_;
public:
    using Ptr = std::shared_ptr<AffineTransform>;

    /// create identity
    AffineTransform();
    /// copy given transform
    AffineTransform(const Eigen::Affine3d& transform);

    /**
        Return the internally stored Eigen::Affine3d (by value)
    */
    Eigen::Affine3d transform() const;

    /**
        Sets a new transform
    */
    void setTransform(const Eigen::Affine3d& transform);

    /**
        Serialization with cereal
    */
    template <class Archive>
    void save(Archive& ar) const
    {
        double data[4][4];
        for (int col = 0; col < 4; col++)
        {
            for (int row = 0; row < 4; row++)
            {
                data[col][row] = transform_.matrix()(col, row);
            }
        }

        ar( cereal::make_nvp<Archive>("base", cereal::base_class<Component>(this)),
            cereal::make_nvp<Archive>("matrix", data) );
    }

    template <class Archive>
    void load(Archive& ar)
    {
        double data[4][4];
        ar( cereal::make_nvp<Archive>("base", cereal::base_class<Component>(this)),
            cereal::make_nvp<Archive>("matrix", data) );

        for (int col = 0; col < 4; col++)
        {
            for (int row = 0; row < 4; row++)
            {
                transform_.matrix()(col, row) = data[col][row];
            }
        }
    }
};

template <>
struct ComponentName<AffineTransform> {
    static constexpr const char* value = "Transform";
};

}

CEREAL_REGISTER_TYPE(sempr::AffineTransform)

// register a to_string function to display AffineTransform in the rete network
#include <rete-core/Util.hpp>
namespace rete { namespace util {

template <> std::string to_string(const sempr::AffineTransform& c);
template <> std::string to_string(const sempr::AffineTransform::Ptr& c);

}}

#endif /* include guard: SEMPR_COMPONENT_AFFINETRANSFORM_HPP_ */


#include <indigox/algorithm/access.hpp>
#include <indigox/algorithm/graph/connectivity.hpp>
#include <indigox/classes/atom.hpp>
#include <indigox/classes/molecule.hpp>
#include <indigox/graph/condensed.hpp>
#include <indigox/graph/molecular.hpp>
#include <indigox/utils/common.hpp>
#include <indigox/utils/triple.hpp>

#include <boost/dynamic_bitset.hpp>
#include <boost/graph/connected_components.hpp>

#include <EASTL/bitset.h>
#include <EASTL/vector_map.h>
#include <memory>
#include <vector>

namespace indigox::algorithm {

  using namespace indigox::graph;

  // ===========================================================================
  // == Connected components implementation ====================================
  // ===========================================================================

  template <class V, class E, class S, class D, class VP, class EP,
            class Container>
  int64_t ConnectedComponents(BaseGraph<V, E, S, D, VP, EP> &G,
                              Container &bits) {
    static_assert(!D::is_directed, "Connectivity requires an undirected graph");

    using GraphType = BaseGraph<V, E, S, D, VP, EP>;
    using BaseType = typename GraphType::graph_type;
    using VertIdxMap = eastl::vector_map<typename GraphType::VertType, int>;
    using namespace boost;

    BaseType &g = access::GetGraph(G);
    VertIdxMap data;
    associative_property_map<VertIdxMap> indexMap(data);
    typename GraphType::VertIter begin, end;
    std::tie(begin, end) = boost::vertices(g);
    for (int i = 0; begin != end; ++begin, ++i) put(indexMap, *begin, i);
    size_t num = connected_components(g, get(&VP::component, g),
                                      vertex_index_map(indexMap));
    bits.clear();
    bits.assign(num, typename GraphType::VertContain());
    for (auto &v : access::GetVertexMap(G).right)
      bits[g[v.first].component].emplace_back(v.second);
    return bits.size();
  }

  template int64_t
  ConnectedComponents(CondensedMolecularGraph::graph_type &,
                      CondensedMolecularGraph::ComponentContain &);
  template int64_t ConnectedComponents(MolecularGraph::graph_type &,
                                       MolecularGraph::ComponentContain &);

  // =======================================================================
  // == Connected subgraphs implementation =================================
  // =======================================================================

  template <class GraphType> struct ConnectedSubgraphs<GraphType>::Impl {

    using vert_contain = typename GraphType::VertContain;
    using BitSet = boost::dynamic_bitset<>;
    using StackItem = stdx::triple<BitSet>;

    GraphType graph;
    size_t min_subgraph_size;
    size_t max_subgraph_size;
    vert_contain vertices;
    eastl::vector_map<size_t, BitSet> neighbours;
    std::vector<StackItem> stack;

    Impl(GraphType &G, size_t min, size_t max)
        : graph(G), min_subgraph_size(min), max_subgraph_size(max),
          vertices(G.GetVertices()) {}
    ~Impl() { }

    void BuildNeighboursBitsets() {
      for (size_t i = 0; i < vertices.size(); ++i) {
        auto &v_nbrs = graph.GetNeighbours(vertices[i]);
        BitSet nbrs(vertices.size());
        nbrs.reset();
        for (auto &v : v_nbrs) {
          auto pos = std::find(vertices.begin(), vertices.end(), v);
          nbrs.set(std::distance(vertices.begin(), pos));
        }
        neighbours.emplace(i, nbrs);
      }
    }

    void Initialise() {
      BuildNeighboursBitsets();
      BitSet bag(vertices.size());
      bag.reset();
      for (size_t i = 0; i < vertices.size(); ++i) bag.set(i);
      BitSet initial(vertices.size());
      initial.reset();
      BitSet nbrs(bag);
      nbrs.reset();
      size_t pos = initial.find_first();
      while (pos < neighbours.size()) {
        auto test = neighbours.find(pos);
        if (test == neighbours.end()) break;
        nbrs |= test->second;
        pos = initial.find_next(pos);
      }
      stack.emplace_back(bag, initial, nbrs);
    }

    bool NextSubgraph(GraphType &subgraph) {
      while (stack.size()) {
        StackItem item = stack.back();
        stack.pop_back();

        BitSet cur_bag = item.first;
        BitSet cur_subg = item.second;
        BitSet cur_nbrs = item.third;

        BitSet possible;
        if (cur_subg.none())
          possible = cur_bag;
        else
          possible = cur_bag & cur_nbrs;

        if (possible.none() && cur_subg.any() &&
            cur_subg.count() <= max_subgraph_size &&
            cur_subg.count() >= min_subgraph_size) {
          std::vector<typename GraphType::VertexType> verts;
          verts.reserve(cur_subg.count());
          size_t pos = cur_subg.find_first();
          while (pos < vertices.size()) {
            verts.emplace_back(vertices[pos]);
            pos = cur_subg.find_next(pos);
          }
          GraphType subg = graph.Subgraph(verts);
          subgraph = subg;
          return true;
        } else if (possible.any()) {
          BitSet v = possible;
          v.reset();
          v.set(possible.find_first());
          if (cur_subg.count() <= max_subgraph_size) {
            BitSet bag_minus_v = cur_bag;
            bag_minus_v.reset(v.find_first());
            stack.emplace_back(bag_minus_v, cur_subg, cur_nbrs);
            stack.emplace_back(bag_minus_v, cur_subg | v,
                               cur_nbrs | neighbours.at(v.find_first()));
          }
        }
      }
      return false;
    }
  };

  template <class GraphType>
  ConnectedSubgraphs<GraphType>::ConnectedSubgraphs(GraphType &G, size_t min,
                                                    size_t max)
      : implementation(std::make_unique<Impl>(G, min, max)) {
    implementation->Initialise();
  }

  template <class GraphType>
  ConnectedSubgraphs<GraphType>::~ConnectedSubgraphs() = default;

  template <class GraphType>
  bool ConnectedSubgraphs<GraphType>::operator()(GraphType &subgraph) {
    return implementation->NextSubgraph(subgraph);
  }

  template class ConnectedSubgraphs<graph::MolecularGraph>;
  template class ConnectedSubgraphs<graph::CondensedMolecularGraph>;

  // =======================================================================
  // == Optimal charge groups implementation ===============================
  // =======================================================================
  struct ChargeGroupOptimiser {
    using BitSet = boost::dynamic_bitset<>;
    using Score = double;
    using Division = std::vector<BitSet>;
    using ScoredDivisions = std::map<Division, Score>;
    using OptimalDivision = std::map<BitSet, Division>;

    graph::MolecularGraph full_graph, leafless;
    int32_t size_limit;

    std::vector<graph::MGVertex> non_leaf_vertices, leaf_vertices;
    std::vector<int32_t> weights;

    std::map<BitSet::size_type, Score> position_scores;
    std::map<BitSet::size_type, BitSet> neighbours;

    ScoredDivisions seen_division_scores;
    OptimalDivision optimal_seen_divisions;

    ChargeGroupOptimiser(const Molecule &mol, int32_t limit);

    double PenaltyScore(BitSet group);

    void Initalise();

    std::vector<std::vector<graph::MGVertex>> Optimise();

    Division &Divisions(BitSet bag);
    void ConnectedSubgraphs(BitSet bag, BitSet current, BitSet nbrs,
                            std::vector<BitSet> &subgraphs);
  };

  std::vector<std::vector<Atom>> OptimalChargeGroups(const Molecule &mol,
                                                     int32_t limit) {
    ChargeGroupOptimiser optimiser(mol, limit);
    optimiser.Initalise();
    std::vector<std::vector<graph::MGVertex>> v_cgs = optimiser.Optimise();
    std::vector<std::vector<Atom>> charge_groups;
    for (std::vector<graph::MGVertex> grp : v_cgs) {
      charge_groups.emplace_back(std::vector<Atom>());
      for (graph::MGVertex v : grp) {
        charge_groups.back().emplace_back(v.GetAtom());
      }
    }
    return charge_groups;
  }

  ChargeGroupOptimiser::ChargeGroupOptimiser(const Molecule &mol, int32_t limit)
      : full_graph(mol.GetGraph()), size_limit(limit) {}

  double ChargeGroupOptimiser::PenaltyScore(BitSet group) {
    double score = 0.;
    BitSet::size_type pos = group.find_first();
    while (pos < group.size()) {
      score += position_scores[pos];
      pos = group.find_next(pos);
    }
    return abs(score);
  }

  void ChargeGroupOptimiser::Initalise() {
    // Vertices identify and sort
    for (graph::MGVertex v : full_graph.GetVertices()) {
      if (full_graph.Degree(v) == 1) {
        leaf_vertices.push_back(v);
      } else {
        non_leaf_vertices.push_back(v);
      }
    }
    leafless = full_graph.Subgraph(non_leaf_vertices);

    BitSet::size_type v_pos = 0;
    for (graph::MGVertex v : non_leaf_vertices) {
      // Neighbours generation

      BitSet nbrs(non_leaf_vertices.size());
      for (graph::MGVertex n : leafless.GetNeighbours(v)) {
        BitSet::size_type n_pos = std::distance(
            non_leaf_vertices.begin(),
            std::find(non_leaf_vertices.begin(), non_leaf_vertices.end(), n));
        nbrs.set(n_pos);
      }
      neighbours.emplace(v_pos, nbrs);

      // Scores per vertex pre calculation
      int32_t weight = 1;
      Atom atm = v.GetAtom();
      double score = atm.GetFormalCharge() - atm.GetPartialCharge();
      for (graph::MGVertex u : full_graph.GetNeighbours(v)) {
        if (std::find(non_leaf_vertices.begin(), non_leaf_vertices.end(), u) ==
            non_leaf_vertices.end()) {
          atm = u.GetAtom();
          score += atm.GetFormalCharge() - atm.GetPartialCharge();
          ++weight;
        }
      }
      position_scores.emplace(v_pos, score);
      weights.emplace_back(weight);
      ++v_pos;

      // Terminal case seen score
      seen_division_scores.emplace(Division(), 0.0);
      optimal_seen_divisions.emplace(BitSet(non_leaf_vertices.size()),
                                     Division());
    }
  }

  std::vector<std::vector<graph::MGVertex>> ChargeGroupOptimiser::Optimise() {
    BitSet bag(non_leaf_vertices.size());
    bag.set();
    Division optimal_division = Divisions(bag);

    std::vector<std::vector<graph::MGVertex>> charge_groups;

    for (BitSet div : optimal_division) {
      charge_groups.emplace_back(std::vector<graph::MGVertex>());
      charge_groups.back().reserve(div.count());
      BitSet::size_type pos = div.find_first();
      while (pos < div.size()) {
        charge_groups.back().emplace_back(non_leaf_vertices[pos]);
        for (graph::MGVertex nbr :
             full_graph.GetNeighbours(non_leaf_vertices[pos])) {
          if (full_graph.Degree(nbr) == 1) {
            charge_groups.back().emplace_back(nbr);
          }
        }
        pos = div.find_next(pos);
      }
    }

    return charge_groups;
  }

  ChargeGroupOptimiser::Division &ChargeGroupOptimiser::Divisions(BitSet bag) {
    auto seen_pos = optimal_seen_divisions.find(bag);
    if (seen_pos != optimal_seen_divisions.end()) { return seen_pos->second; }

    BitSet::size_type v = bag.find_first();
    std::vector<BitSet> subgraphs;
    BitSet bag_minus_v(bag);
    bag_minus_v.reset(v);
    BitSet current(bag.size()), nbrs(neighbours.at(v));
    current.set(v);
    ConnectedSubgraphs(bag_minus_v, current, nbrs, subgraphs);

    for (BitSet g : subgraphs) {
      BitSet bag_minus_g = bag - g;
      Division new_div = Divisions(bag_minus_g);
      Score new_score = seen_division_scores.at(new_div);
      Score cur_score = PenaltyScore(g);

      auto seen_position = optimal_seen_divisions.find(bag);
      Division optimal_div(new_div);
      optimal_div.emplace_back(g);
      Score optimal_score = cur_score + new_score;
      if (seen_position == optimal_seen_divisions.end()) {
        seen_division_scores.emplace(optimal_div, optimal_score);
        optimal_seen_divisions.emplace(bag, optimal_div);
      } else if ((seen_division_scores.at(seen_position->second) >
                  (optimal_score))) {
        seen_position->second = optimal_div;
        seen_division_scores[optimal_div] = optimal_score;
      }

      if (-1e-10 < optimal_score && 1e-10 > optimal_score) break;
    }
    return optimal_seen_divisions.at(bag);
  }

  void ChargeGroupOptimiser::ConnectedSubgraphs(
      BitSet bag, BitSet current, BitSet nbrs, std::vector<BitSet> &subgraphs) {
    subgraphs.clear();
    using StackItem = stdx::triple<BitSet>;
    std::vector<StackItem> stack;
    stack.emplace_back(bag, current, nbrs);
    while (!stack.empty()) {
      StackItem item = stack.back();
      stack.pop_back();

      BitSet possibles = item.first;
      if (item.second.any()) possibles = item.first & item.third;
      if (possibles.none() && item.second.any()) {
        subgraphs.emplace_back(item.second);
      } else if (possibles.any()) {
        BitSet::size_type v = possibles.find_first();
        int32_t v_weight = 0;
        BitSet::size_type pos = item.second.find_first();
        while (pos < item.second.size()) {
          v_weight += weights[pos];
          pos = item.second.find_next(pos);
        }
        if (v_weight < size_limit) {
          BitSet bag_minus_v(item.first);
          bag_minus_v.reset(v);
          BitSet g_and_v(item.second);
          g_and_v.set(v);
          BitSet new_nbrs = item.third | neighbours.at(v);
          stack.emplace_back(bag_minus_v, item.second, item.third);
          stack.emplace_back(bag_minus_v, g_and_v, new_nbrs);
        } else {
          subgraphs.emplace_back(item.second);
        }
      }
    }
    std::sort(subgraphs.begin(), subgraphs.end(),
              [](BitSet &a, BitSet &b) { return a.count() < b.count(); });
  }

} // namespace indigox::algorithm


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef GREENUTILS_HPP
#define GREENUTILS_HPP

#include "Config.hpp"

#include <Eigen/Core>

#include "DerivativeTypes.hpp"
#include "utils/Stencils.hpp"

namespace GreenUtils {
/*! Returns value of the directional derivative of the function passed for the pair of points p1, p2:
 *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_2}\f$
 *  Notice that this method returns the directional derivative with respect
 *  to the probe point.
 *  \tparam DerivativeTraits evaluation strategy for the function and its derivatives
 *  \param[in] functor   function-object handle to the evaluation of the differentiable function
 *  \param[in] normal_p2 the normal vector to p2
 *  \param[in]        p1 first point
 *  \param[in]        p2 second point
 */
template <typename DerivativeTraits>
double derivativeProbe(const pcm::function<DerivativeTraits(DerivativeTraits *, DerivativeTraits *)> & functor,
                       const Eigen::Vector3d & normal_p2, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2)
{
    DerivativeTraits t1[3], t2[3], der;
    t1[0] = p1(0); t1[1] = p1(1); t1[2] = p1(2);
    t2[0] = p2(0); t2[1] = p2(1); t2[2] = p2(2);
    t2[0][1] = normal_p2(0); t2[1][1] = normal_p2(1); t2[2][1] = normal_p2(2);
    der = functor(t1, t2);
    return der[1];
}

/*! Returns value of the directional derivative of the function passed for the pair of points p1, p2:
 *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_1}\f$
 *  Notice that this method returns the directional derivative with respect
 *  to the source point.
 *  \tparam DerivativeTraits evaluation strategy for the function and its derivatives
 *  \param[in] functor   function-object handle to the evaluation of the differentiable function
 *  \param[in] normal_p1 the normal vector to p1
 *  \param[in]        p1 first point
 *  \param[in]        p2 second point
 */
template <typename DerivativeTraits>
double derivativeSource(const pcm::function<DerivativeTraits(DerivativeTraits *, DerivativeTraits *)> & functor,
                        const Eigen::Vector3d & normal_p1, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2)
{
    DerivativeTraits t1[3], t2[3], der;
    t1[0] = p1(0); t1[1] = p1(1); t1[2] = p1(2);
    t1[0][1] = normal_p1(0); t1[1][1] = normal_p1(1); t1[2][1] = normal_p1(2);
    t2[0] = p2(0); t2[1] = p2(1); t2[2] = p2(2);
    der = functor(t1, t2);
    return der[1];
}

/*! Returns value of the directional derivative of the function passed for the pair of points p1, p2:
 *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_2}\f$
 *  Notice that this method returns the directional derivative with respect
 *  to the probe point.
 *  \param[in] functor   function-object handle to the evaluation of the differentiable function
 *  \param[in] normal_p2 the normal vector to p2
 *  \param[in]        p1 first point
 *  \param[in]        p2 second point
 */
double derivativeProbe(const DifferentiableFunction & functor, const Eigen::Vector3d & normal_p2, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2)
{
    return threePointStencil(pcm::bind(functor, pcm::_1, _2), p2, p1, normal_p2);
}

/*! Returns value of the directional derivative of the function passed for the pair of points p1, p2:
 *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_1}\f$
 *  Notice that this method returns the directional derivative with respect
 *  to the source point.
 *  \param[in] functor   function-object handle to the evaluation of the differentiable function
 *  \param[in] normal_p1 the normal vector to p1
 *  \param[in]        p1 first point
 *  \param[in]        p2 second point
 */
double derivativeSource(const DifferentiableFunction & functor, const Eigen::Vector3d & normal_p1, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2)
{
        return threePointStencil(pcm::bind(functor,_1, _2), p1, p2, normal_p1);
}

/*! Returns full gradient of the function passed for the pair of points p1, p2:
 *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
 *  Notice that this method returns the gradient with respect to the source point.
 *  \tparam DerivativeTraits evaluation strategy for the function and its derivatives
 *  \param[in] functor   function-object handle to the evaluation of the differentiable function
 *  \param[in] p1 first point
 *  \param[in] p2 second point
 */
template <typename DerivativeTraits>
Eigen::Vector3d gradientSource(const pcm::function<DerivativeTraits(DerivativeTraits *, DerivativeTraits *)> & functor,
                               const Eigen::Vector3d & p1, const Eigen::Vector3d & p2)
{
    return (Eigen::Vector3d() << derivativeSource(functor, Eigen::Vector3d::UnitX(), p1, p2),
                                 derivativeSource(functor, Eigen::Vector3d::UnitY(), p1, p2),
                                 derivativeSource(functor, Eigen::Vector3d::UnitZ(), p1, p2)).finished();
}

/*! Returns full gradient of the function passed for the pair of points p1, p2:
 *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
 *  Notice that this method returns the gradient with respect to the probe point.
 *  \tparam DerivativeTraits evaluation strategy for the function and its derivatives
 *  \param[in] functor   function-object handle to the evaluation of the differentiable function
 *  \param[in] p1 first point
 *  \param[in] p2 second point
 */
template <typename DerivativeTraits>
Eigen::Vector3d gradientProbe(const pcm::function<DerivativeTraits(DerivativeTraits *, DerivativeTraits *)> & functor,
                              const Eigen::Vector3d & p1, const Eigen::Vector3d & p2)
{
    return (Eigen::Vector3d() << derivativeProbe(functor, Eigen::Vector3d::UnitX(), p1, p2),
                                 derivativeProbe(functor, Eigen::Vector3d::UnitY(), p1, p2),
                                 derivativeProbe(functor, Eigen::Vector3d::UnitZ(), p1, p2)).finished();
}

/*! Returns full gradient of the function passed for the pair of points p1, p2:
 *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
 *  Notice that this method returns the gradient with respect to the source point.
 *  \param[in] functor   function-object handle to the evaluation of the differentiable function
 *  \param[in] p1 first point
 *  \param[in] p2 second point
 */
Eigen::Vector3d gradientSource(const DifferentiableFunction & functor, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2)
{
    return (Eigen::Vector3d() << derivativeSource(functor, Eigen::Vector3d::UnitX(), p1, p2),
                                 derivativeSource(functor, Eigen::Vector3d::UnitY(), p1, p2),
                                 derivativeSource(functor, Eigen::Vector3d::UnitZ(), p1, p2)).finished();
}

/*! Returns full gradient of the function passed for the pair of points p1, p2:
 *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
 *  Notice that this method returns the gradient with respect to the probe point.
 *  \param[in] functor   function-object handle to the evaluation of the differentiable function
 *  \param[in] p1 first point
 *  \param[in] p2 second point
 */
Eigen::Vector3d gradientProbe(const DifferentiableFunction & functor, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2)
{
    return (Eigen::Vector3d() << derivativeProbe(functor, Eigen::Vector3d::UnitX(), p1, p2),
                                 derivativeProbe(functor, Eigen::Vector3d::UnitY(), p1, p2),
                                 derivativeProbe(functor, Eigen::Vector3d::UnitZ(), p1, p2)).finished();
}
} // namespace GreenUtils

#endif // GREENUTILS_HPP


/// @author Andrew Hundt <athundt@gmail.com>
#ifndef _INVERSE_KINEMATICS_VREP_PLUGIN_
#define _INVERSE_KINEMATICS_VREP_PLUGIN_

/// @todo remove IGNORE_ROTATION or make it a runtime configurable parameter
// #define IGNORE_ROTATION

#include <string>
#include <tuple>
#include <boost/format.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

// SpaceVecAlg
// https://github.com/jrl-umi3218/SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

// RBDyn
// https://github.com/jrl-umi3218/RBDyn
#include <RBDyn/Body.h>
#include <RBDyn/Joint.h>
#include <RBDyn/MultiBody.h>
#include <RBDyn/MultiBodyConfig.h>
#include <RBDyn/MultiBodyGraph.h>
#include <RBDyn/EulerIntegration.h>
#include <RBDyn/FK.h>
#include <RBDyn/IK.h>
#include <RBDyn/FV.h>
#include <RBDyn/ID.h>

// Tasks
// https://github.com/jrl-umi3218/Tasks
#include <Tasks/Tasks.h>
#include <Tasks/Bounds.h>
#include <Tasks/QPConstr.h>
#include <Tasks/QPContactConstr.h>
#include <Tasks/QPMotionConstr.h>
#include <Tasks/QPSolver.h>
#include <Tasks/QPTasks.h>

// grl
#include "grl/vrep/Vrep.hpp"
#include "grl/vrep/VrepRobotArmDriver.hpp"
#include "grl/vrep/VrepRobotArmJacobian.hpp"
#include "grl/vrep/Eigen.hpp"
#include "grl/vrep/SpaceVecAlg.hpp"

#include <boost/lexical_cast.hpp>
#include <boost/range/algorithm/copy.hpp>

#include <spdlog/spdlog.h>

namespace grl {
namespace vrep {

/// Sets the VREP simulation joint positions from the RBDyn configuration
///
/// @param vrepJointNames the name of each joint, order must match vrepJointHandles
/// @param vrepJointHandles the V-REP simulation object handle for each joint
/// @param rbdJointNames the names of all the simArmMultiBody joints, which typically has more joints than the VREP list.
/// @param simArmMultiBody defines the structure of the robot arm for the RBDyn library, see RBDyn documentation
/// @param simArmConfig defines the current position of the robot arm for the RBDyn library, see RBDyn documentation
/// @param debug if empty string, no effect, if any other string a trace of the joint angles will be printed to std::cout
void SetVRepArmFromRBDyn(
    const std::vector<std::string>& vrepJointNames,
    const std::vector<int>& vrepJointHandles,
    const rbd::MultiBody& simArmMultiBody,
    const rbd::MultiBodyConfig& simArmConfig,
    std::string debug = "")
{
    std::string str;
    for (std::size_t i = 0; i < vrepJointHandles.size(); ++ i)
    {
        // modify parameters as follows https://github.com/jrl-umi3218/Tasks/issues/10#issuecomment-257466822
        std::string jointName = vrepJointNames[i];
        std::size_t jointIdx = simArmMultiBody.jointIndexByName(jointName);
        float futureAngle = simArmConfig.q[jointIdx][0];
        simSetJointPosition(vrepJointHandles[i],futureAngle);
        /// @todo TODO(ahundt) add torque information
        // simSetJointTargetVelocity(jointHandles_[i],jointAngles_dt[i]/simulationTimeStep);
        // simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]);
        // simSetJointTargetPosition(jointHandles_[i],futureAngle);

        if(!debug.empty())
        {
             str+=boost::lexical_cast<std::string>(futureAngle);
             if (i<vrepJointHandles.size()-1) str+=", ";
        }
    }

    if(!debug.empty())
    {
        spdlog::get("console")->info(debug + " jointAngles: " + str);
    }
}

/// Sets the VREP simulation joint positions from the RBDyn configuration
///
/// @param vrepJointNames the name of each joint, order must match vrepJointHandles
/// @param vrepJointHandles the V-REP simulation object handle for each joint
/// @param rbdJointNames the names of all the simArmMultiBody joints, which typically has more joints than the VREP list.
/// @param simArmMultiBody defines the structure of the robot arm for the RBDyn library, see RBDyn documentation
/// @param simArmConfig defines the current position of the robot arm for the RBDyn library, see RBDyn documentation
/// @param debug if empty string, no effect, if any other string a trace of the joint angles will be printed to std::cout
void SetRBDynArmFromVrep(
    const std::vector<std::string>& vrepJointNames,
    const std::vector<int>& vrepJointHandles,
    const rbd::MultiBody& simArmMultiBody,
    rbd::MultiBodyConfig& simArmConfig,
    std::string debug = "")
{
    std::string str;
    float futureAngle;
    for (std::size_t i = 0; i < vrepJointHandles.size(); ++ i)
    {
        // modify parameters as follows https://github.com/jrl-umi3218/Tasks/issues/10#issuecomment-257466822
        std::string jointName = vrepJointNames[i];
        std::size_t jointIdx = simArmMultiBody.jointIndexByName(jointName);
        simGetJointPosition(vrepJointHandles[i],&futureAngle);
        /// @todo TODO(ahundt) warn/error when size!=0 or explicitly handle those cases
        if(simArmConfig.q[jointIdx].size()>0) simArmConfig.q[jointIdx][0] = futureAngle;

        /// @todo TODO(ahundt) add torque information
        // simSetJointTargetVelocity(jointHandles_[i],jointAngles_dt[i]/simulationTimeStep);
        // simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]);
        // simSetJointTargetPosition(jointHandles_[i],futureAngle);

        if(!debug.empty())
        {
             str+=boost::lexical_cast<std::string>(futureAngle);
             if (i<vrepJointHandles.size()-1) str+=", ";
        }
    }

    if(!debug.empty())
    {
        spdlog::get("console")->info(debug + " jointAngles: " + str);
    }
}

/// This object handles taking data from a V-REP based arm simulation
/// using it to configure an arm constrained optimization algorithm,
/// runs the algorithm, then updates the simulation accordingly.
///
/// The types of problems this can solve include reaching a goal position,
/// applying the desired levels of force to a system, and avoiding obstacles
/// at each time step.
///
/// @see the Tasks libraryhttps://github.com/jrl-umi3218/Tasks for a full set of possible capabilities.
///
/// @todo verify Robotiiwa is the correct base, and RobotMillTipTarget is the right target, because if the transform doesn't match the one in the jacobian the algorithm will break
//template<typename DesiredKinematics = DesiredKinematicsObject>
class InverseKinematicsVrepPlugin
{
public:

    //typedef grl::InverseKinematicsController parent_type;

    //using parent_type::currentKinematicsStateP_;
    //using parent_type::parent_type::InitializeKinematicsState;

    enum  VrepVFControllerParamsIndex {
        DesiredKinematicsObjectName,
        IKGroupName
    };

    typedef std::tuple<
             std::string // DesiredKinematicsObjectName
            ,std::string // IKGroupName
        > Params;

    static Params defaultParams()
    {
       return std::make_tuple("RobotMillTipTarget","IK_Group1_iiwa");
    }

    /// @todo need to call parent constructor:
        /*! Constructor
    */
    InverseKinematicsVrepPlugin()
    {
    }

    /// @todo TODO(ahundt) accept params for both simulated and measured arms
    void construct(Params params = defaultParams()){
        // get kinematics group name
        // get number of joints
        logger_ = spdlog::get("console");

        // Get the arm that will be used to generate simulated results to command robot
        // the "base" of this ik is Robotiiwa
        VrepRobotArmDriverSimulatedP_ = std::make_shared<vrep::VrepRobotArmDriver>();
        VrepRobotArmDriverSimulatedP_->construct();
        // Get the "Measured" arm that will be set based off of real arm sensor data, from sources like KukaVrepPlugin or ROS
        // in example simulation this ends in #0
        VrepRobotArmDriverMeasuredP_ = std::make_shared<vrep::VrepRobotArmDriver>(vrep::VrepRobotArmDriver::measuredArmParams());
        VrepRobotArmDriverMeasuredP_->construct();
        measuredJointHandles_ = VrepRobotArmDriverMeasuredP_->getJointHandles();
        measuredJointNames_ = VrepRobotArmDriverMeasuredP_->getJointNames();

        ikGroupHandle_ = simGetIkGroupHandle(std::get<IKGroupName>(params).c_str());
        simSetExplicitHandling(ikGroupHandle_,1); // enable explicit handling for the ik

        auto armDriverSimulatedParams = VrepRobotArmDriverSimulatedP_->getParams();
        // the base frame of the arm
        ikGroupBaseName_ = (std::get<VrepRobotArmDriver::RobotTargetBaseName>(armDriverSimulatedParams));
        // the tip of the arm
        ikGroupTipName_ = (std::get<VrepRobotArmDriver::RobotTipName>(armDriverSimulatedParams));
        // the target, or where the tip of the arm should go
        ikGroupTargetName_ = (std::get<VrepRobotArmDriver::RobotTargetName>(armDriverSimulatedParams));
        robotFlangeTipName_ = (std::get<VrepRobotArmDriver::RobotFlangeTipName>(armDriverSimulatedParams));

        ikGroupBaseHandle_ = grl::vrep::getHandle(ikGroupBaseName_);
        ikGroupTipHandle_ = grl::vrep::getHandle(ikGroupTipName_);
        ikGroupTargetHandle_ = grl::vrep::getHandle(ikGroupTargetName_);
        /// for why this is named as it is see: https://github.com/jrl-umi3218/Tasks/blob/master/tests/arms.h#L34
        sva::PTransform<double> X_base = getObjectPTransform(ikGroupBaseHandle_);
        //X_base = sva::PTransformd(X_base.rotation().inverse(), X_base.translation());
        // start by assuming the base is fixed
        bool isFixed = true;
        bool isForwardJoint = true;

        { // Initalize the RBDyn arm representation

            jointHandles_ = VrepRobotArmDriverSimulatedP_->getJointHandles();
            jointNames_ = VrepRobotArmDriverSimulatedP_->getJointNames();

            linkNames_ =VrepRobotArmDriverSimulatedP_->getLinkNames();
            linkHandles_ =VrepRobotArmDriverSimulatedP_->getLinkHandles();
            linkRespondableNames_ = VrepRobotArmDriverSimulatedP_->getLinkRespondableNames();
            linkRespondableHandles_ = VrepRobotArmDriverSimulatedP_->getLinkRespondableHandles();

            // save the initial joint angles for later
            // and set the arm to the 0 position for initialization of the
            // optimizer
            std::vector<float> initialJointAngles;
            for (int handle : jointHandles_) {
               float angle;
               simGetJointPosition(handle,&angle);
               simSetJointPosition(handle,0);
               initialJointAngles.push_back(angle);
            }

            rbd_bodyNames_.push_back(ikGroupBaseName_);
            // note: bodyNames are 1 longer than link names, and start with the base!
            boost::copy(linkNames_, std::back_inserter(rbd_bodyNames_));
            boost::copy(jointNames_, std::back_inserter(rbd_jointNames_));
            /// @todo TODO(ahundt) replace hardcoded joint strings with a vrep tree traversal object that generates an RBDyn MultiBodyGraph
            jointNames_.push_back("LBR_iiwa_14_R820_link8");
            jointNames_.push_back("cutter_joint");
            rbd_jointNames_.push_back("LBR_iiwa_14_R820_link8");
            rbd_jointNames_.push_back(robotFlangeTipName_);
            rbd_jointNames_.push_back("cutter_joint");
            rbd_jointNames_.push_back(ikGroupTipName_);

            rbd_bodyNames_.push_back("cutter_joint");
            rbd_bodyNames_.push_back(ikGroupTipName_);
            getHandles(rbd_bodyNames_,std::back_inserter(bodyHandles_));
            getHandles(rbd_jointNames_,std::back_inserter(rbd_jointHandles_));

            // Note that V-REP specifies full transforms to place objects that rotate joints around the Z axis

            // based on: https://github.com/jrl-umi3218/Tasks/blob/master/tests/arms.h#L34
            // and https://github.com/jrl-umi3218/RBDyn/issues/18#issuecomment-257214536
            for(std::size_t i = 0; i < rbd_jointNames_.size(); i++)
            {

                // Note that V-REP specifies full transforms to place objects that rotate joints around the Z axis
                std::string thisJointName = rbd_jointNames_[i];
                rbd::Joint::Type jointType = rbd::Joint::Fixed;
                /// @todo TODO(ahundt) fix hard coded Revolute vs fixed joint https://github.com/ahundt/grl/issues/114
                if(simGetObjectType(rbd_jointHandles_[i])==sim_object_joint_type)
                {
                    jointType = rbd::Joint::Rev;
                }
                rbd::Joint j_i(jointType, Eigen::Vector3d::UnitZ(), isForwardJoint, thisJointName);
                rbd_mbg_.addJoint(j_i);
            }

            // Note that V-REP specifies full transforms to place objects that rotate joints around the Z axis
            /// @todo TODO(ahundt) last "joint" RobotMillTip is really fixed...
            //rbd::Joint j_i(rbd::Joint::Fixed, Eigen::Vector3d::UnitZ(), isForwardJoint, ikGroupTipName_);
            //rbd::Joint j_i(rbd::Joint::Fixed, Eigen::Vector3d::UnitZ(), isForwardJoint, ikGroupTipName_);
            //rbd_mbg_.addJoint(j_i);

            for(std::size_t i = 0; i < rbd_bodyNames_.size(); i++)
            {
                /// @todo TODO(ahundt) extract real inertia and center of mass from V-REP with http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetShapeMassAndInertia.htm

                double mass = 1.;
                auto I = Eigen::Matrix3d::Identity();
                auto h = Eigen::Vector3d::Zero();
                /// @todo TODO(ahundt) consider the origin of the inertia! https://github.com/jrl-umi3218/Tasks/issues/10#issuecomment-257198604
                sva::RBInertiad rbi_i(mass, h, I);
                /// @todo TODO(ahundt) add real support for links, particularly the respondable aspects, see LBR_iiwa_14_R820_joint1_resp in RoboneSimulation.ttt
                //  @todo TODO(ahundt) REMEMBER: The first joint is NOT respondable!

                // remember, bodyNames[0] is the ikGroupBaseName, so entity order is
                // bodyNames[i], joint[i], bodyNames[i+1]
                std::string bodyName(rbd_bodyNames_[i]);
                rbd::Body b_i(rbi_i,bodyName);

                rbd_mbg_.addBody(b_i);
            }

            std::string dummyName0(("Dummy"+ boost::lexical_cast<std::string>(0+10)));
            int currentDummy0 = simGetObjectHandle(dummyName0.c_str());
            Eigen::Affine3d eto0 = getObjectTransform(rbd_jointHandles_[0],-1);
            logger_->info("{} \n{}", dummyName0 , eto0.matrix());
            setObjectTransform(currentDummy0,-1,eto0);

            std::vector<int> frameHandles;
            frameHandles.push_back(simGetObjectHandle(ikGroupBaseName_.c_str()));
            boost::copy(rbd_jointHandles_,std::back_inserter(frameHandles));
            for(std::size_t i = 0; i < rbd_jointHandles_.size()-1; i++)
            {
                // note: Tasks takes transforms in the successor (child link) frame, so it is the inverse of v-rep
                //       thus we are getting the current joint in the frame of the next joint
                sva::PTransformd to(getObjectPTransform(frameHandles[i+1],frameHandles[i]));
                // this should be the identity matrix because we set the joints to 0!
                //sva::PTransformd from(getJointPTransform(rbd_jointHandles_[i+1]));
                sva::PTransformd from(sva::PTransformd::Identity());

                // Link the PREVIOUSLY created body to the currently created body with the PREVIOUSLY created joint
                // remember, bodyNames[0] is the ikGroupBaseName, so entity order is
                // bodyNames[i], joint[i], bodyNames[i+1]
                std::string prevBody = rbd_bodyNames_[i];
                std::string curJoint = rbd_jointNames_[i];
                std::string nextBody = rbd_bodyNames_[i+1];

                rbd_mbg_.linkBodies(prevBody, to, nextBody, from, curJoint);

            }

            // note: Tasks takes transforms in the successor (child link) frame, so it is the inverse of v-rep
            rbd_mbs_.push_back(rbd_mbg_.makeMultiBody(ikGroupBaseName_,isFixed,X_base));
            rbd_mbcs_.push_back(rbd::MultiBodyConfig(rbd_mbs_[0]));
            rbd_mbcs_[0].zero(rbd_mbs_[0]);

            // we only have one robot for the moment so the index of it is 0
            const std::size_t simulatedRobotIndex = 0;
            auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex];
            auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex];

            // update the simulated arm position
            for (std::size_t i = 0; i < jointHandles_.size(); ++i) {
               simSetJointPosition(jointHandles_[i],initialJointAngles[i]);
            }
            SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig);

            rbd::forwardKinematics(simArmMultiBody, simArmConfig);
            rbd::forwardVelocity(simArmMultiBody, simArmConfig);

            // set the preferred position to the initial position
            // https://github.com/jrl-umi3218/Tasks/blob/15aff94e3e03f6a161a87799ca2cf262b756bd0c/src/QPTasks.h#L426
            rbd_preferred_mbcs_.push_back(simArmConfig);

            debugFrames();

        }

        /// @todo verify object lifetimes

        // add virtual fixtures? need names and number of rows

        /// @todo read objective rows from vrep

        /// @todo set vrep explicit handling of IK here, plus unset in destructor of this object

    }

    /// Set dummy frames named Dummy0 to Dummy19 to the current state of the
    /// RBDyn and V-REP representations of joints for debugging.
    void debugFrames(bool print = false)
    {
        int prevDummy = -1;
        // we only have one robot for the moment so the index of it is 0
        const std::size_t simulatedRobotIndex = 0;
        auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex];
        auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex];

        // Debug output
       for (std::size_t i=0 ; i < jointHandles_.size() ; i++)
       {
          Eigen::Affine3d eto;
          std::string dummyName2(("Dummy"+ boost::lexical_cast<std::string>(i+11)));
          int currentDummy2 = simGetObjectHandle(dummyName2.c_str());
          eto = getObjectTransform(jointHandles_[i],-1);
          setObjectTransform(currentDummy2,-1,eto);
          if(print) logger_->info("{} V-REP World\n{}",dummyName2 , eto.matrix());

          if(i>0)
          {
            Eigen::Affine3d NextJointinPrevFrame(getObjectTransform(jointHandles_[i],jointHandles_[i-1]));
            if(print) logger_->info("{} V-REP JointInPrevFrame\n{}",dummyName2 , NextJointinPrevFrame.matrix());
          }

          bool dummy_world_frame = true;
          if( dummy_world_frame )
          {
              // visualize each joint position
              sva::PTransform<double>     plinkWorld = simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(linkNames_[i])];
              Eigen::Affine3d linkWorld = PTranformToEigenAffine(plinkWorld);
              std::string dummyName(("Dummy0"+ boost::lexical_cast<std::string>(i+1)));
              int currentDummy = simGetObjectHandle(dummyName.c_str());
              if(print) logger_->info("{} RBDyn World\n{}",dummyName, linkWorld.matrix());
              setObjectTransform(currentDummy,-1,linkWorld);
              prevDummy=currentDummy;
              sva::PTransform<double>     plinkToSon = simArmConfig.parentToSon[simArmMultiBody.bodyIndexByName(linkNames_[i])];
              Eigen::Affine3d linkToSon = PTranformToEigenAffine(plinkToSon);
              if(print) logger_->info("{} RBDyn ParentLinkToSon\n{}",dummyName, linkToSon.matrix());

          }
          else
          {
              // visualize each joint position
              sva::PTransform<double>     plinkWorld = simArmConfig.parentToSon[simArmMultiBody.bodyIndexByName(linkNames_[i])];
              Eigen::Affine3d linkWorld = PTranformToEigenAffine(plinkWorld);
              std::string dummyName(("Dummy0"+ boost::lexical_cast<std::string>(i+1)));
              int currentDummy = simGetObjectHandle(dummyName.c_str());
              if(print) logger_->info("{} RBDyn\n{}",dummyName , linkWorld.matrix());
              setObjectTransform(currentDummy,prevDummy,linkWorld);
              prevDummy=currentDummy;

          }
       }

       // Frame "Dummy" with no numbers goes at the tip!
       Eigen::Affine3d tipTf = PTranformToEigenAffine(simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(ikGroupTipName_)]);
       setObjectTransform(simGetObjectHandle("Dummy"),-1,tipTf);
    }

    void testPose(){

       /// simulation tick time step in float seconds from vrep API call
       float simulationTimeStep = simGetSimulationTimeStep();

        // we only have one robot for the moment so the index of it is 0
        const std::size_t simulatedRobotIndex = 0;
        auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex];
        auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex];

        std::vector<float> vrepJointAngles;
        double angle = 0.7;
        for(auto i : jointHandles_)
        {
            angle *= -1;
            simSetJointPosition(i,angle);
            vrepJointAngles.push_back(angle);
        }

       /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command?

        ////////////////////////////////////////////////////
        // Set joints to current arm position in simulation
        SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig);
        rbd::forwardKinematics(simArmMultiBody, simArmConfig);
        rbd::forwardVelocity(simArmMultiBody, simArmConfig);

        debugFrames();
    }

    /// Configures updateKinematics with the goal the kinematics should aim for
    enum class GoalPosE { realGoalPosition, debugGoalPosition };
    /// Configures updateKinematics the algorithm the kinematics should use for solving
    enum class AlgToUseE { ik, multiIterQP, singleIterQP };
    /// Configures if the target pose objective should stick to the constant initalized
    /// version, or if it should update every iteration in tasks::qp::PostureTask
    /// https://github.com/jrl-umi3218/Tasks/blob/15aff94e3e03f6a161a87799ca2cf262b756bd0c/src/QPTasks.h#L426
    enum class PostureTaskStrategyE { constant, updateToCurrent };
    /// Determines where the current arm state is copied from
    /// simulatedArm is the current simulation, measuredArm
    /// is based off of data measured from the real physical arm
    enum class ArmStateSource { simulatedArm, measuredArm };

    /// Runs inverse kinematics or constrained optimization at every simulation time step
    /// @param runOnce Set runOnce = true to only update kinematics once for debugging purposes. runOnce = false runs this function at every time step.
    /// @param solveForPosition defines where the arm is aiming for, the moving goal pose object or a stationary debug position to verify the algorithm
    /// @param postureStrategy helps determine the position/orientation at which the elbow is held
    /// @param syncSimulatedKinematicsState data used to set the simulated arm position at the START of the kinematics update, use measuredArm with the real arm, simulatedArm when running simulation only.
    /// @param syncMeasuredKinematicsState set measured arm position at END of the kinematics update. Use simulatedArm when running simulation, measuredArm when running with a real physical arm from which you can get measurements.
    /// @param scaleSimulationTimeStep scale the amount of time the Tasks library thinks is passing. WARNING: DANGEROUS HACK, MAY WORKS AROUND SPEED LIMITS, MIGHT WANT TO LEAVE THIS AT 1.0
    void updateKinematics(
        const bool runOnce = false,
        const GoalPosE solveForPosition = GoalPosE::realGoalPosition,
        const AlgToUseE alg = AlgToUseE::multiIterQP,
        const PostureTaskStrategyE postureStrategy = PostureTaskStrategyE::constant,
        const ArmStateSource syncSimulatedKinematicsState = ArmStateSource::measuredArm,
        const ArmStateSource syncMeasuredKinematicsState = ArmStateSource::measuredArm,
        int numSolverIterations = 10,
        double scaleSimulationTimeStep = 10
    ){
        if(runOnce && ranOnce_) return;
        ranOnce_ = true;

        jointHandles_ = VrepRobotArmDriverSimulatedP_->getJointHandles();

        ///////////////////////////////////////////////////////////
        // Copy Joint Interval, the range of motion for each joint

        /// @todo TODO(ahundt) change source of current state based on if physical arm is running
        if(syncSimulatedKinematicsState == ArmStateSource::simulatedArm)
        {
            VrepRobotArmDriverSimulatedP_->getState(currentArmState_);
        }
        else
        {
            VrepRobotArmDriverMeasuredP_->getState(currentArmState_);
        }

        // lower limits
        auto & llim = std::get<vrep::VrepRobotArmDriver::JointLowerPositionLimit>(currentArmState_);
        std::vector<double> llimits(llim.begin(),llim.end());

        // upper limits
        auto & ulim = std::get<vrep::VrepRobotArmDriver::JointUpperPositionLimit>(currentArmState_);
        std::vector<double> ulimits(ulim.begin(),ulim.end());

        // current position
        auto & currentJointPos = std::get<vrep::VrepRobotArmDriver::JointPosition>(currentArmState_);
        std::vector<double> currentJointPosVec(currentJointPos.begin(),currentJointPos.end());

        /// @todo TODO(ahundt) also copy acceleration velocity, torque, etc

        const auto& handleParams = VrepRobotArmDriverSimulatedP_->getVrepHandleParams();
        Eigen::Affine3d currentEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  currentEigenT = currentEndEffectorPose.translation();

        Eigen::Affine3d desiredEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  desiredEigenT = desiredEndEffectorPose.translation();

        //////////////////////
        /// @todo TODO(ahundt) move code below here back into separate independent setup and solve functions, move some steps like limits to construct()

       /// simulation tick time step in float seconds from vrep API call
       float simulationTimeStep = simGetSimulationTimeStep();

        // we only have one robot for the moment so the index of it is 0
        const std::size_t simulatedRobotIndex = 0;
        auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex];
        auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex];

       /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command?

        ////////////////////////////////////////////////////
        // Set joints to current arm position in simulation
        if(syncSimulatedKinematicsState == ArmStateSource::simulatedArm)
        {
            SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig);
        }
        else
        {
            // note that the regular joint names are used but the measured handles are used
            // this means the names will match correctly for accessing the multibody, while
            // measured angles are what will actually be transferred.
            /// @todo TODO(ahundt) using it like this is a bit fragile, and a reasonable change to
            ///                    SetRBDynArmFromVrep could cause breakage, so improve the API design.
            SetRBDynArmFromVrep(jointNames_,measuredJointHandles_,simArmMultiBody,simArmConfig);
        }
        rbd::forwardKinematics(simArmMultiBody, simArmConfig);
        rbd::forwardVelocity(simArmMultiBody, simArmConfig);

        // save the current MultiBodyConfig for comparison after running algorithms
        rbd_prev_mbcs_ = rbd_mbcs_;
        // set the preferred position to the current position
        // https://github.com/jrl-umi3218/Tasks/blob/15aff94e3e03f6a161a87799ca2cf262b756bd0c/src/QPTasks.h#L426
        if(postureStrategy == PostureTaskStrategyE::updateToCurrent) rbd_preferred_mbcs_ = rbd_mbcs_;

        /// @todo TODO(ahundt) make solver object a member variable if possible, initialize in constructor
        tasks::qp::QPSolver solver;

        ////////////////////////////////////////////////////
        // Set position goal of the arm
        sva::PTransformd targetWorldTransform;

        if( solveForPosition == GoalPosE::realGoalPosition )
        {
            // go to the real target position
            targetWorldTransform = getObjectPTransform(ikGroupTargetHandle_);
            logger_->info("target translation (vrep format):\n{}", targetWorldTransform.translation());
        }
        else
        {
            // go to a debugging target position
            targetWorldTransform = simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(ikGroupTipName_)];
            targetWorldTransform.translation().z() -= 0.0001;
            logger_->info("target translation (rbdyn format):\n", targetWorldTransform.translation());
        }
        tasks::qp::PositionTask posTask(rbd_mbs_, simulatedRobotIndex, ikGroupTipName_,targetWorldTransform.translation());
        tasks::qp::SetPointTask posTaskSp(rbd_mbs_, simulatedRobotIndex, &posTask, 50., 10.);
        tasks::qp::OrientationTask oriTask(rbd_mbs_,simulatedRobotIndex, ikGroupTipName_,targetWorldTransform.rotation());
        tasks::qp::SetPointTask oriTaskSp(rbd_mbs_, simulatedRobotIndex, &oriTask, 50., 1.);
        tasks::qp::PostureTask postureTask(rbd_mbs_,simulatedRobotIndex,rbd_preferred_mbcs_[simulatedRobotIndex].q,1,0.01);

        double inf = std::numeric_limits<double>::infinity();

        ////////////////////////////////////
        // Set joint limits

        // joint limit objects, initialize to current q so entries
        // will be reasonable, such as empty entries for fixed joints
        std::vector<std::vector<double> > lBound = simArmConfig.q;
        std::vector<std::vector<double> > uBound = simArmConfig.q;
        std::vector<std::vector<double> > lVelBound = simArmConfig.alpha;
        std::vector<std::vector<double> > uVelBound = simArmConfig.alpha;

        // for all joints
        for (std::size_t i=0 ; i < rbd_jointHandles_.size()-1; i++)
        {
            /// limits must be organized as described in https://github.com/jrl-umi3218/Tasks/issues/10#issuecomment-257793242
            std::string jointName = rbd_jointNames_[i];
            std::size_t jointIdx = simArmMultiBody.jointIndexByName(jointName);

            /// @todo TODO(ahundt) ulimits aren't the same size as jointHandles_, need velocity limits too
            // set joint position limits
            if(boost::iequals(jointName,"cutter_joint"))
            { /// @todo TODO(ahundt) hardcoded mill tip joint limits, remove these, load from simulation
                lBound[jointIdx][0] = -inf;
                uBound[jointIdx][0] = inf;
                lVelBound[jointIdx][0] = -inf;
                uVelBound[jointIdx][0] = inf;
            }
            else if(i<llimits.size() && lBound[jointIdx].size()==1)
            {
                lBound[jointIdx][0] = llimits[i];
                uBound[jointIdx][0] = ulimits[i];
            }

            // set joint velocity limits
            if(i<llimits.size() && lVelBound[jointIdx].size()==1)
            {
                /// @todo TODO(ahundt) replace hardcoded velocity limits with real and useful limits specific to each joint loaded from V-REP or another file.
                // 0.99 radians per second is the limit of the slowest joint (joint 1) on the KUKA iiwa R820
                lVelBound[jointIdx][0] = -0.99;
                uVelBound[jointIdx][0] = 0.99;
            }
        }

        tasks::qp::DamperJointLimitsConstr dampJointConstr(rbd_mbs_, simulatedRobotIndex, {lBound, uBound},{lVelBound, uVelBound}, 0.125, 0.025, 1., simulationTimeStep);

        // Test add*Constraint
        dampJointConstr.addToSolver(solver);
        BOOST_VERIFY(solver.nrBoundConstraints() == 1);
        BOOST_VERIFY(solver.nrConstraints() == 1);

        solver.nrVars(rbd_mbs_, {}, {});
        solver.updateConstrSize();

        solver.addTask(&posTaskSp);
        BOOST_VERIFY(solver.nrTasks() == 1);
        solver.addTask(&oriTaskSp);
        solver.addTask(&postureTask);

        ////////////////////////////////////
        // Run constrained optimization
        if(alg == AlgToUseE::ik)
        {
            // use basic inverse kinematics to solve for the position
            //rbd::InverseKinematics ik(simArmMultiBody,simArmMultiBody.jointIndexByName(jointNames_[6]));
            rbd::InverseKinematics ik(simArmMultiBody,simArmMultiBody.bodyIndexByName(ikGroupTipName_));
            ik.sInverseKinematics(simArmMultiBody,simArmConfig,targetWorldTransform);
            // update the simulated arm position
        }
        else if( alg == AlgToUseE::multiIterQP)
        {
            // multiple iteration version of solving
            // Test JointLimitsConstr
            /// @todo TODO(ahundt) was this commented correctly?
            //simArmConfig = mbcInit;
            double timeStepDividedIntoIterations = scaleSimulationTimeStep*simulationTimeStep/numSolverIterations;
            // This actually runs every time step, so only one iteration here, unless we want to subdivide
            // a v-rep time step into smaller rbdyn time steps.
            for(int i = 0; i < numSolverIterations; ++i)
            {
                //BOOST_REQUIRE(solver.solve(rbd_mbs_, rbd_mbcs_));
                /// @todo TODO(ahundt) remove BOOST_VERIFY(), try/catch solver failure, print an error, send a v-rep message, and shut down solver or enter a hand guiding mode
                BOOST_VERIFY(solver.solve(rbd_mbs_, rbd_mbcs_));
                // This should be handled by the simulator or physical robot, "forward simulation of dynamics"
                rbd::sEulerIntegration(simArmMultiBody, simArmConfig, timeStepDividedIntoIterations);
                //rbd::sEulerIntegration(simArmMultiBody, simArmConfig, simulationTimeStep);

                rbd::sForwardKinematics(simArmMultiBody, simArmConfig);
                rbd::sForwardVelocity(simArmMultiBody, simArmConfig);
                //BOOST_REQUIRE_GT(simArmConfig.q[1][simulatedRobotIndex], -cst::pi<double>()/4. - 0.01);
            }
        }
        else
        {
            // single iteration version of solving
            BOOST_VERIFY(solver.solve(rbd_mbs_, rbd_mbcs_));
            rbd::sEulerIntegration(simArmMultiBody, simArmConfig, simulationTimeStep);
            rbd::sForwardKinematics(simArmMultiBody, simArmConfig);
            rbd::sForwardVelocity(simArmMultiBody, simArmConfig);
            // update the simulated arm position
        }

        SetVRepArmFromRBDyn(jointNames_,jointHandles_,simArmMultiBody,simArmConfig);

        if(syncMeasuredKinematicsState == ArmStateSource::simulatedArm)
        {
            // Running simulation only, so update the "measured" arm to match the simulated arm

            // note that the regular joint names are used but the measured handles are used
            // this means the names will match correctly for accessing the multibody, while
            // measured angles are what will actually be transferred.
            /// @todo TODO(ahundt) using it like this is a bit fragile, and a reasonable change to
            ///                    SetRBDynArmFromVrep could cause breakage, so improve the API design.
            SetVRepArmFromRBDyn(jointNames_,measuredJointHandles_,simArmMultiBody,simArmConfig);
        }

       debugFrames();
    } // end updateKinematics()

    /// may not need this it is in the base class
    /// blocking call, call in separate thread, just allocates memory
    void run_one(){
       // If true, run real inverse kinematics algorith, if false go to a test pose.
       const bool ik = true;
       if(ik) updateKinematics();
       else   testPose();
    }

    /// may not need this it is in the base class
    /// this will have output
    /// blocking call, call in separate thread, just allocates memory
    /// this runs the actual optimization algorithm
    void solve(){

    }

    std::shared_ptr<spdlog::logger> logger_;

    rbd::MultiBodyGraph                 rbd_mbg_;
    std::vector<rbd::MultiBody>         rbd_mbs_;
    std::vector<rbd::MultiBodyConfig>   rbd_mbcs_;
    /// rbd_prev_mbcs_ is for debugging
    std::vector<rbd::MultiBodyConfig>   rbd_prev_mbcs_;
    /// preferred posture to resolve ambiguities
    std::vector<rbd::MultiBodyConfig>   rbd_preferred_mbcs_;
    std::vector<rbd::Body>              rbd_bodies_;
    std::vector<sva::RBInertia<double>> rbd_inertias_;
    std::vector<rbd::Joint>             rbd_joints_;
    std::vector<std::string>            rbd_bodyNames_;
    std::vector<int>                    bodyHandles_;
    /// rbd "joints" include fixed joints that bridge
    /// various v-rep objects like the world origin and the ik group base.
    /// This is organized a bit differently from the v-rep joint names
    /// because we need additional fixed "joints" so we have transforms
    /// that match the V-REP scene
    std::vector<std::string>            rbd_jointNames_;
    /// rbd "joints" include fixed joints that bridge
    /// various v-rep objects like the world origin and the ik group base.
    /// This is organized a bit differently from the v-rep joint names
    /// because we need additional fixed "joints" so we have transforms
    /// that match the V-REP scene
    std::vector<int>                    rbd_jointHandles_;

    //tasks::qp::QPSolver qp_solver_;

    std::vector<int> jointHandles_; ///< @todo move this item back into VrepRobotArmDriver
    std::vector<int> linkHandles_;
    std::vector<int> linkRespondableHandles_;
    std::vector<std::string> jointNames_;
    std::vector<std::string> linkNames_;
    std::vector<std::string> linkRespondableNames_;

    std::vector<int> measuredJointHandles_; ///< @todo move this item back into VrepRobotArmDriver
    std::vector<std::string> measuredJointNames_;

    int ikGroupHandle_;
    int ikGroupBaseHandle_;
    int ikGroupTipHandle_;
    int ikGroupTargetHandle_;

    std::string ikGroupBaseName_;
    std::string ikGroupTipName_;
    std::string ikGroupTargetName_;
    std::string robotFlangeTipName_; // not part of the V-REP ik group

    std::shared_ptr<vrep::VrepRobotArmDriver> VrepRobotArmDriverSimulatedP_;
    std::shared_ptr<vrep::VrepRobotArmDriver> VrepRobotArmDriverMeasuredP_;
    vrep::VrepRobotArmDriver::State currentArmState_;

    bool ranOnce_ = false;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace vrep
} // namespace grl

#endif