/*
 * Copyright 2016,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
#include <pinocchio/fwd.hpp>
#include <sot/core/debug.hh>

#include <sot/dynamic-pinocchio/dynamic-pinocchio.h>

#include <boost/version.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>

#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/spatial/motion.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/centroidal.hpp>
#include <pinocchio/multibody/model.hpp>

#include <dynamic-graph/all-commands.h>

#include "../src/dynamic-command.h"

using namespace dynamicgraph::sot;
using namespace dynamicgraph;

const std::string dg::sot::DynamicPinocchio::CLASS_NAME = "DynamicPinocchio";

DynamicPinocchio::DynamicPinocchio(const std::string& name)
    : Entity(name),
      m_model(NULL),
      m_data(NULL)

      ,
      jointPositionSIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::position"),
      freeFlyerPositionSIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::ffposition"),
      jointVelocitySIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::velocity"),
      freeFlyerVelocitySIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::ffvelocity"),
      jointAccelerationSIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::acceleration"),
      freeFlyerAccelerationSIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::ffacceleration")

      ,
      pinocchioPosSINTERN(boost::bind(&DynamicPinocchio::getPinocchioPos, this, _1, _2),
                          jointPositionSIN << freeFlyerPositionSIN,
                          "sotDynamicPinocchio(" + name + ")::intern(dummy)::pinocchioPos"),
      pinocchioVelSINTERN(boost::bind(&DynamicPinocchio::getPinocchioVel, this, _1, _2),
                          jointVelocitySIN << freeFlyerVelocitySIN,
                          "sotDynamicPinocchio(" + name + ")::intern(dummy)::pinocchioVel"),
      pinocchioAccSINTERN(boost::bind(&DynamicPinocchio::getPinocchioAcc, this, _1, _2),
                          jointAccelerationSIN << freeFlyerAccelerationSIN,
                          "sotDynamicPinocchio(" + name + ")::intern(dummy)::pinocchioAcc")

      ,
      newtonEulerSINTERN(boost::bind(&DynamicPinocchio::computeNewtonEuler, this, _1, _2),
                         pinocchioPosSINTERN << pinocchioVelSINTERN << pinocchioAccSINTERN,
                         "sotDynamicPinocchio(" + name + ")::intern(dummy)::newtoneuler"),
      jacobiansSINTERN(boost::bind(&DynamicPinocchio::computeJacobians, this, _1, _2), pinocchioPosSINTERN,
                       "sotDynamicPinocchio(" + name + ")::intern(dummy)::computeJacobians"),
      forwardKinematicsSINTERN(boost::bind(&DynamicPinocchio::computeForwardKinematics, this, _1, _2),
                               pinocchioPosSINTERN << pinocchioVelSINTERN << pinocchioAccSINTERN,
                               "sotDynamicPinocchio(" + name + ")::intern(dummy)::computeForwardKinematics"),
      ccrbaSINTERN(boost::bind(&DynamicPinocchio::computeCcrba, this, _1, _2),
                   pinocchioPosSINTERN << pinocchioVelSINTERN,
                   "sotDynamicPinocchio(" + name + ")::intern(dummy)::computeCcrba"),
      zmpSOUT(boost::bind(&DynamicPinocchio::computeZmp, this, _1, _2), newtonEulerSINTERN,
              "sotDynamicPinocchio(" + name + ")::output(vector)::zmp"),
      JcomSOUT(boost::bind(&DynamicPinocchio::computeJcom, this, _1, _2), pinocchioPosSINTERN,
               "sotDynamicPinocchio(" + name + ")::output(matrix)::Jcom"),
      comSOUT(boost::bind(&DynamicPinocchio::computeCom, this, _1, _2), forwardKinematicsSINTERN,
              "sotDynamicPinocchio(" + name + ")::output(vector)::com"),
      inertiaSOUT(boost::bind(&DynamicPinocchio::computeInertia, this, _1, _2), pinocchioPosSINTERN,
                  "sotDynamicPinocchio(" + name + ")::output(matrix)::inertia"),
      footHeightSOUT(boost::bind(&DynamicPinocchio::computeFootHeight, this, _1, _2), pinocchioPosSINTERN,
                     "sotDynamicPinocchio(" + name + ")::output(double)::footHeight"),
      upperJlSOUT(boost::bind(&DynamicPinocchio::getUpperPositionLimits, this, _1, _2), sotNOSIGNAL,
                  "sotDynamicPinocchio(" + name + ")::output(vector)::upperJl")

      ,
      lowerJlSOUT(boost::bind(&DynamicPinocchio::getLowerPositionLimits, this, _1, _2), sotNOSIGNAL,
                  "sotDynamicPinocchio(" + name + ")::output(vector)::lowerJl")

      ,
      upperVlSOUT(boost::bind(&DynamicPinocchio::getUpperVelocityLimits, this, _1, _2), sotNOSIGNAL,
                  "sotDynamicPinocchio(" + name + ")::output(vector)::upperVl")

      ,
      upperTlSOUT(boost::bind(&DynamicPinocchio::getMaxEffortLimits, this, _1, _2), sotNOSIGNAL,
                  "sotDynamicPinocchio(" + name + ")::output(vector)::upperTl")

      ,
      inertiaRotorSOUT("sotDynamicPinocchio(" + name + ")::output(matrix)::inertiaRotor"),
      gearRatioSOUT("sotDynamicPinocchio(" + name + ")::output(matrix)::gearRatio"),
      inertiaRealSOUT(boost::bind(&DynamicPinocchio::computeInertiaReal, this, _1, _2),
                      inertiaSOUT << gearRatioSOUT << inertiaRotorSOUT,
                      "sotDynamicPinocchio(" + name + ")::output(matrix)::inertiaReal"),
      MomentaSOUT(boost::bind(&DynamicPinocchio::computeMomenta, this, _1, _2), ccrbaSINTERN,
                  "sotDynamicPinocchio(" + name + ")::output(vector)::momenta"),
      AngularMomentumSOUT(boost::bind(&DynamicPinocchio::computeAngularMomentum, this, _1, _2), ccrbaSINTERN,
                          "sotDynamicPinocchio(" + name + ")::output(vector)::angularmomentum"),
      dynamicDriftSOUT(boost::bind(&DynamicPinocchio::computeTorqueDrift, this, _1, _2), newtonEulerSINTERN,
                       "sotDynamicPinocchio(" + name + ")::output(vector)::dynamicDrift") {
  sotDEBUGIN(5);

  // TODO-------------------------------------------

  // if( build ) buildModel();
  // firstSINTERN.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
  // DEBUG: Why =0? should be function. firstSINTERN.setConstant(0);
  // endTODO--------------------------------------------

  signalRegistration(jointPositionSIN);
  signalRegistration(freeFlyerPositionSIN);
  signalRegistration(jointVelocitySIN);
  signalRegistration(freeFlyerVelocitySIN);
  signalRegistration(jointAccelerationSIN);
  signalRegistration(freeFlyerAccelerationSIN);
  signalRegistration(zmpSOUT);
  signalRegistration(comSOUT);
  signalRegistration(JcomSOUT);
  signalRegistration(footHeightSOUT);
  signalRegistration(upperJlSOUT);
  signalRegistration(lowerJlSOUT);
  signalRegistration(upperVlSOUT);
  signalRegistration(upperTlSOUT);
  signalRegistration(inertiaSOUT);
  signalRegistration(inertiaRealSOUT);
  signalRegistration(inertiaRotorSOUT);
  signalRegistration(gearRatioSOUT);
  signalRegistration(MomentaSOUT);
  signalRegistration(AngularMomentumSOUT);
  signalRegistration(dynamicDriftSOUT);

  //
  // Commands
  //
  std::string docstring;
  // setFiles

  docstring =
      "\n"
      "    Display the current robot configuration.\n"
      "\n"
      "      Input:\n"
      "        - none \n"
      "\n";
  addCommand("displayModel", new command::DisplayModel(*this, docstring));
  docstring =
      "    \n"
      "    Get the dimension of the robot configuration.\n"
      "    \n"
      "      Return:\n"
      "        an unsigned int: the dimension.\n"
      "    \n";
  addCommand("getDimension", new command::GetDimension(*this, docstring));

  {
    using namespace ::dg::command;
    // CreateOpPoint
    // TODO add operational joints
    docstring =
        "    \n"
        "    Create an operational point attached to a robot joint local frame.\n"
        "    \n"
        "      Input: \n"
        "        - a string: name of the operational point,\n"
        "        - a string: name the joint, or among (gaze, left-ankle, right ankle\n"
        "          , left-wrist, right-wrist, waist, chest).\n"
        "\n";
    addCommand("createOpPoint", makeCommandVoid2(*this, &DynamicPinocchio::cmd_createOpPointSignals, docstring));

    docstring = docCommandVoid2("Create a jacobian (world frame) signal only for one joint.", "string (signal name)",
                                "string (joint name)");
    addCommand("createJacobian", makeCommandVoid2(*this, &DynamicPinocchio::cmd_createJacobianWorldSignal, docstring));

    docstring = docCommandVoid2("Create a jacobian (endeff frame) signal only for one joint.", "string (signal name)",
                                "string (joint name)");
    addCommand("createJacobianEndEff",
               makeCommandVoid2(*this, &DynamicPinocchio::cmd_createJacobianEndEffectorSignal, docstring));

    docstring = docCommandVoid2(
        "Create a jacobian (endeff frame) signal only for one joint. "
        "The returned jacobian is placed at the joint position, but oriented with the world axis.",
        "string (signal name)", "string (joint name)");
    addCommand("createJacobianEndEffWorld",
               makeCommandVoid2(*this, &DynamicPinocchio::cmd_createJacobianEndEffectorWorldSignal, docstring));

    docstring = docCommandVoid2("Create a position (matrix homo) signal only for one joint.", "string (signal name)",
                                "string (joint name)");
    addCommand("createPosition", makeCommandVoid2(*this, &DynamicPinocchio::cmd_createPositionSignal, docstring));

    docstring = docCommandVoid2("Create a velocity (vector) signal only for one joint.", "string (signal name)",
                                "string (joint name)");
    addCommand("createVelocity", makeCommandVoid2(*this, &DynamicPinocchio::cmd_createVelocitySignal, docstring));

    docstring = docCommandVoid2("Create an acceleration (vector) signal only for one joint.", "string (signal name)",
                                "string (joint name)");
    addCommand("createAcceleration",
               makeCommandVoid2(*this, &DynamicPinocchio::cmd_createAccelerationSignal, docstring));
    docstring =
        "\n"
        "  Return robot joint names.\n\n";
    addCommand("getJointNames", new command::GetJointNames(*this, docstring));
  }

  sphericalJoints.clear();

  sotDEBUG(10) << "Dynamic class_name address" << &CLASS_NAME << std::endl;
  sotDEBUGOUT(5);
}

DynamicPinocchio::~DynamicPinocchio(void) {
  sotDEBUGIN(15);
  // TODO currently, m_model and m_data are pointers owned by the Python interpreter
  // so we should not delete them.
  // I (Joseph Mirabel) think it would be wiser to make them belong to this class but
  // I do not know the impact it has.
  // if (0!=m_data ) { delete m_data ; m_data =NULL; }
  // if (0!=m_model) { delete m_model; m_model=NULL; }

  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin(); iter != genericSignalRefs.end();
       ++iter) {
    SignalBase<int>* sigPtr = *iter;
    delete sigPtr;
  }
  sotDEBUGOUT(15);
}

void DynamicPinocchio::setModel(pinocchio::Model* modelPtr) {
  this->m_model = modelPtr;

  if (this->m_model->nq > m_model->nv) {
    if (pinocchio::nv(this->m_model->joints[1]) == 6) sphericalJoints.push_back(3);  // FreeFlyer Orientation

    for (int i = 1; i < this->m_model->njoints; i++)     // 0: universe
      if (pinocchio::nq(this->m_model->joints[i]) == 4)  // Spherical Joint Only
        sphericalJoints.push_back(pinocchio::idx_v(this->m_model->joints[i]));
  }
}

void DynamicPinocchio::setData(pinocchio::Data* dataPtr) { this->m_data = dataPtr; }

/*--------------------------------GETTERS-------------------------------------------*/

dg::Vector& DynamicPinocchio::getLowerPositionLimits(dg::Vector& res, const int&) const {
  sotDEBUGIN(15);
  assert(m_model);

  res.resize(m_model->nv);
  if (!sphericalJoints.empty()) {

    int fillingIndex = 0;  // SoTValue
    int origIndex = 0;     // PinocchioValue
    for (std::vector<int>::const_iterator it = sphericalJoints.begin(); it < sphericalJoints.end(); it++) {
      if (*it - fillingIndex > 0) {
        res.segment(fillingIndex, *it - fillingIndex) =
            m_model->lowerPositionLimit.segment(origIndex, *it - fillingIndex);

        // Don't Change this order
        origIndex += *it - fillingIndex;
        fillingIndex += *it - fillingIndex;
      }
      // Found a Spherical Joint.
      // Assuming that spherical joint limits are unset
      // Version C++11
      //res(fillingIndex) = std::numeric_limits<double>::lowest();
      //res(fillingIndex + 1) = std::numeric_limits<double>::lowest();
      //res(fillingIndex + 2) = std::numeric_limits<double>::lowest();
      // For now use C++98
      res(fillingIndex) = -std::numeric_limits<double>::max();
      res(fillingIndex + 1) = -std::numeric_limits<double>::max();
      res(fillingIndex + 2) = -std::numeric_limits<double>::max();

      fillingIndex += 3;
      origIndex += 4;
    }

    assert(m_model->nv - fillingIndex == m_model->nq - origIndex);
    if (m_model->nv > fillingIndex)
      res.segment(fillingIndex, m_model->nv - fillingIndex) =
          m_model->lowerPositionLimit.segment(origIndex, m_model->nv - fillingIndex);
  } else {
    res = m_model->lowerPositionLimit;
  }
  sotDEBUG(15) << "lowerLimit (" << res << ")=" << std::endl;
  sotDEBUGOUT(15);
  return res;
}

dg::Vector& DynamicPinocchio::getUpperPositionLimits(dg::Vector& res, const int&) const {
  sotDEBUGIN(15);
  assert(m_model);

  res.resize(m_model->nv);
  if (!sphericalJoints.empty()) {
    int fillingIndex = 0;  // SoTValue
    int origIndex = 0;     // PinocchioValue
    for (std::vector<int>::const_iterator it = sphericalJoints.begin(); it < sphericalJoints.end(); it++) {
      if (*it - fillingIndex > 0) {
        res.segment(fillingIndex, *it - fillingIndex) =
            m_model->upperPositionLimit.segment(origIndex, *it - fillingIndex);

        // Don't Change this order
        origIndex += *it - fillingIndex;
        fillingIndex += *it - fillingIndex;
      }
      // Found a Spherical Joint.
      // Assuming that spherical joint limits are unset
      res(fillingIndex) = std::numeric_limits<double>::max();
      res(fillingIndex + 1) = std::numeric_limits<double>::max();
      res(fillingIndex + 2) = std::numeric_limits<double>::max();
      fillingIndex += 3;
      origIndex += 4;
    }
    assert(m_model->nv - fillingIndex == m_model->nq - origIndex);
    if (m_model->nv > fillingIndex)
      res.segment(fillingIndex, m_model->nv - fillingIndex) =
          m_model->upperPositionLimit.segment(origIndex, m_model->nv - fillingIndex);
  } else {
    res = m_model->upperPositionLimit;
  }
  sotDEBUG(15) << "upperLimit (" << res << ")=" << std::endl;
  sotDEBUGOUT(15);
  return res;
}

dg::Vector& DynamicPinocchio::getUpperVelocityLimits(dg::Vector& res, const int&) const {
  sotDEBUGIN(15);
  assert(m_model);

  res.resize(m_model->nv);
  res = m_model->velocityLimit;

  sotDEBUG(15) << "upperVelocityLimit (" << res << ")=" << std::endl;
  sotDEBUGOUT(15);
  return res;
}

dg::Vector& DynamicPinocchio::getMaxEffortLimits(dg::Vector& res, const int&) const {
  sotDEBUGIN(15);
  assert(m_model);

  res.resize(m_model->nv);
  res = m_model->effortLimit;

  sotDEBUGOUT(15);
  return res;
}

/* ---------------- INTERNAL ------------------------------------------------ */
dg::Vector& DynamicPinocchio::getPinocchioPos(dg::Vector& q, const int& time) {
  sotDEBUGIN(15);
  dg::Vector qJoints = jointPositionSIN.access(time);
  if (!sphericalJoints.empty()) {
    if (freeFlyerPositionSIN) {
      dg::Vector qFF = freeFlyerPositionSIN.access(time);
      qJoints.head<6>() = qFF;          // Overwrite qJoints ff with ffposition value
      assert(sphericalJoints[0] == 3);  // FreeFlyer should ideally be present.
    }
    q.resize(qJoints.size() + sphericalJoints.size());
    int fillingIndex = 0;
    int origIndex = 0;
    for (std::vector<int>::const_iterator it = sphericalJoints.begin(); it < sphericalJoints.end(); it++) {
      if (*it - origIndex > 0) {
        q.segment(fillingIndex, *it - origIndex) = qJoints.segment(origIndex, *it - origIndex);
        fillingIndex += *it - origIndex;
        origIndex += *it - origIndex;
      }
      assert(*it == origIndex);
      Eigen::Quaternion<double> temp = Eigen::AngleAxisd(qJoints(origIndex + 2), Eigen::Vector3d::UnitZ()) *
                                       Eigen::AngleAxisd(qJoints(origIndex + 1), Eigen::Vector3d::UnitY()) *
                                       Eigen::AngleAxisd(qJoints(origIndex), Eigen::Vector3d::UnitX());
      q(fillingIndex) = temp.x();
      q(fillingIndex + 1) = temp.y();
      q(fillingIndex + 2) = temp.z();
      q(fillingIndex + 3) = temp.w();
      fillingIndex += 4;
      origIndex += 3;
    }
    if (qJoints.size() > origIndex)
      q.segment(fillingIndex, qJoints.size() - origIndex) = qJoints.tail(qJoints.size() - origIndex);
  } else {
    q.resize(qJoints.size());
    q = qJoints;
  }

  sotDEBUG(15) << "Position out" << q << std::endl;
  sotDEBUGOUT(15);
  return q;
}

dg::Vector& DynamicPinocchio::getPinocchioVel(dg::Vector& v, const int& time) {
  const Eigen::VectorXd vJoints = jointVelocitySIN.access(time);
  if (freeFlyerVelocitySIN) {
    const Eigen::VectorXd vFF = freeFlyerVelocitySIN.access(time);
    if (v.size() != vJoints.size() + vFF.size()) v.resize(vJoints.size() + vFF.size());
    v << vFF, vJoints;
    return v;
  } else {
    v = vJoints;
    return v;
  }
}

dg::Vector& DynamicPinocchio::getPinocchioAcc(dg::Vector& a, const int& time) {
  const Eigen::VectorXd aJoints = jointAccelerationSIN.access(time);
  if (freeFlyerAccelerationSIN) {
    const Eigen::VectorXd aFF = freeFlyerAccelerationSIN.access(time);
    if (a.size() != aJoints.size() + aFF.size()) a.resize(aJoints.size() + aFF.size());
    a << aFF, aJoints;
    return a;
  } else {
    a = aJoints;
    return a;
  }
}

/* --- SIGNAL ACTIVATION ---------------------------------------------------- */
dg::SignalTimeDependent<dg::Matrix, int>& DynamicPinocchio::createJacobianSignal(const std::string& signame,
                                                                                const std::string& jointName) {
  sotDEBUGIN(15);
  assert(m_model);
  dg::SignalTimeDependent<dg::Matrix, int>* sig;
  if (m_model->existFrame(jointName)) {
    long int frameId = m_model->getFrameId(jointName);
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
        boost::bind(&DynamicPinocchio::computeGenericJacobian, this, true, frameId, _1, _2), jacobiansSINTERN,
        "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
  } else if (m_model->existJointName(jointName)) {
    long int jointId = m_model->getJointId(jointName);
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
        boost::bind(&DynamicPinocchio::computeGenericJacobian, this, false, jointId, _1, _2), jacobiansSINTERN,
        "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
  } else
    SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC, "Robot has no joint corresponding to " + jointName);

  genericSignalRefs.push_back(sig);
  signalRegistration(*sig);
  sotDEBUGOUT(15);
  return *sig;
}

dg::SignalTimeDependent<dg::Matrix, int>& DynamicPinocchio::createEndeffJacobianSignal(const std::string& signame,
                                                                                const std::string& jointName,
                                                                                const bool isLocal) {
  sotDEBUGIN(15);
  assert(m_model);
  dg::SignalTimeDependent<dg::Matrix, int>* sig;

  if (m_model->existFrame(jointName)) {
    long int frameId = m_model->getFrameId(jointName);
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
        boost::bind(&DynamicPinocchio::computeGenericEndeffJacobian, this, true, isLocal, frameId, _1, _2),
        jacobiansSINTERN << forwardKinematicsSINTERN, "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
  } else if (m_model->existJointName(jointName)) {
    long int jointId = m_model->getJointId(jointName);
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
        boost::bind(&DynamicPinocchio::computeGenericEndeffJacobian, this, false, isLocal, jointId, _1, _2),
        jacobiansSINTERN << forwardKinematicsSINTERN, "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
  } else
    SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC, "Robot has no joint corresponding to " + jointName);
  genericSignalRefs.push_back(sig);
  signalRegistration(*sig);
  sotDEBUGOUT(15);
  return *sig;
}

dg::SignalTimeDependent<MatrixHomogeneous, int>& DynamicPinocchio::createPositionSignal(const std::string& signame,
                                                                                const std::string& jointName) {
  sotDEBUGIN(15);
  assert(m_model);
  dg::SignalTimeDependent<MatrixHomogeneous, int>* sig;
  if (m_model->existFrame(jointName)) {
    long int frameId = m_model->getFrameId(jointName);
    sig = new dg::SignalTimeDependent<MatrixHomogeneous, int>(
        boost::bind(&DynamicPinocchio::computeGenericPosition, this, true, frameId, _1, _2), forwardKinematicsSINTERN,
        "sotDynamicPinocchio(" + name + ")::output(matrixHomo)::" + signame);
  } else if (m_model->existJointName(jointName)) {
    long int jointId = m_model->getJointId(jointName);
    sig = new dg::SignalTimeDependent<MatrixHomogeneous, int>(
        boost::bind(&DynamicPinocchio::computeGenericPosition, this, false, jointId, _1, _2), forwardKinematicsSINTERN,
        "sotDynamicPinocchio(" + name + ")::output(matrixHomo)::" + signame);
  } else
    SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC, "Robot has no joint corresponding to " + jointName);

  genericSignalRefs.push_back(sig);
  signalRegistration(*sig);
  sotDEBUGOUT(15);
  return *sig;
}

SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::createVelocitySignal(const std::string& signame,
                                                                             const std::string& jointName) {
  sotDEBUGIN(15);
  assert(m_model);
  long int jointId = m_model->getJointId(jointName);

  SignalTimeDependent<dg::Vector, int>* sig = new SignalTimeDependent<dg::Vector, int>(
      boost::bind(&DynamicPinocchio::computeGenericVelocity, this, jointId, _1, _2), forwardKinematicsSINTERN,
      "sotDynamicPinocchio(" + name + ")::output(dg::Vector)::" + signame);
  genericSignalRefs.push_back(sig);
  signalRegistration(*sig);

  sotDEBUGOUT(15);
  return *sig;
}

dg::SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::createAccelerationSignal(const std::string& signame,
                                                                                const std::string& jointName) {
  sotDEBUGIN(15);
  assert(m_model);
  long int jointId = m_model->getJointId(jointName);
  dg::SignalTimeDependent<dg::Vector, int>* sig = new dg::SignalTimeDependent<dg::Vector, int>(
      boost::bind(&DynamicPinocchio::computeGenericAcceleration, this, jointId, _1, _2), forwardKinematicsSINTERN,
      "sotDynamicPinocchio(" + name + ")::output(dg::Vector)::" + signame);

  genericSignalRefs.push_back(sig);
  signalRegistration(*sig);

  sotDEBUGOUT(15);
  return *sig;
}

void DynamicPinocchio::destroyJacobianSignal(const std::string& signame) {
  sotDEBUGIN(15);

  bool deletable = false;
  dg::SignalTimeDependent<dg::Matrix, int>* sig = &jacobiansSOUT(signame);
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin(); iter != genericSignalRefs.end();
       ++iter) {
    if ((*iter) == sig) {
      genericSignalRefs.erase(iter);
      deletable = true;
      break;
    }
  }

  if (!deletable) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::CANT_DESTROY_SIGNAL, "Cannot destroy signal",
                               " (while trying to remove generic jac. signal <%s>).", signame.c_str());
  }
  signalDeregistration(signame);
  delete sig;
}

void DynamicPinocchio::destroyPositionSignal(const std::string& signame) {
  sotDEBUGIN(15);
  bool deletable = false;
  dg::SignalTimeDependent<MatrixHomogeneous, int>* sig = &positionsSOUT(signame);
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin(); iter != genericSignalRefs.end();
       ++iter) {
    if ((*iter) == sig) {
      genericSignalRefs.erase(iter);
      deletable = true;
      break;
    }
  }

  if (!deletable) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::CANT_DESTROY_SIGNAL, "Cannot destroy signal",
                               " (while trying to remove generic pos. signal <%s>).", signame.c_str());
  }

  signalDeregistration(signame);

  delete sig;
}

void DynamicPinocchio::destroyVelocitySignal(const std::string& signame) {
  sotDEBUGIN(15);
  bool deletable = false;
  SignalTimeDependent<dg::Vector, int>* sig = &velocitiesSOUT(signame);
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin(); iter != genericSignalRefs.end();
       ++iter) {
    if ((*iter) == sig) {
      genericSignalRefs.erase(iter);
      deletable = true;
      break;
    }
  }

  if (!deletable) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::CANT_DESTROY_SIGNAL, "Cannot destroy signal",
                               " (while trying to remove generic pos. signal <%s>).", signame.c_str());
  }

  signalDeregistration(signame);

  delete sig;
}

void DynamicPinocchio::destroyAccelerationSignal(const std::string& signame) {
  sotDEBUGIN(15);
  bool deletable = false;
  dg::SignalTimeDependent<dg::Vector, int>* sig = &accelerationsSOUT(signame);
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin(); iter != genericSignalRefs.end();
       ++iter) {
    if ((*iter) == sig) {
      genericSignalRefs.erase(iter);
      deletable = true;
      break;
    }
  }

  if (!deletable) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::CANT_DESTROY_SIGNAL, getName() + ":cannot destroy signal",
                               " (while trying to remove generic acc "
                               "signal <%s>).",
                               signame.c_str());
  }

  signalDeregistration(signame);

  delete sig;
}

/* --------------------- COMPUTE ------------------------------------------------- */

dg::Vector& DynamicPinocchio::computeZmp(dg::Vector& res, const int& time) {
  // TODO: To be verified
  sotDEBUGIN(25);
  assert(m_data);
  if (res.size() != 3) res.resize(3);
  newtonEulerSINTERN(time);

  const pinocchio::Force& ftau = m_data->oMi[1].act(m_data->f[1]);
  const pinocchio::Force::Vector3& tau = ftau.angular();
  const pinocchio::Force::Vector3& f = ftau.linear();
  res(0) = -tau[1] / f[2];
  res(1) = tau[0] / f[2];
  res(2) = 0;

  sotDEBUGOUT(25);

  return res;
}

// In world coordinates

// Updates the jacobian matrix in m_data
int& DynamicPinocchio::computeJacobians(int& dummy, const int& time) {
  sotDEBUGIN(25);
  forwardKinematicsSINTERN(time);
  pinocchio::computeJointJacobians(*m_model, *m_data);
  sotDEBUG(25) << "Jacobians updated" << std::endl;
  sotDEBUGOUT(25);
  return dummy;
}
int& DynamicPinocchio::computeForwardKinematics(int& dummy, const int& time) {
  sotDEBUGIN(25);
  assert(m_model);
  assert(m_data);
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
  const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
  const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
  pinocchio::forwardKinematics(*m_model, *m_data, q, v, a);
  sotDEBUG(25) << "Kinematics updated" << std::endl;
  sotDEBUGOUT(25);
  return dummy;
}

int& DynamicPinocchio::computeCcrba(int& dummy, const int& time) {
  sotDEBUGIN(25);
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
  const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
  pinocchio::ccrba(*m_model, *m_data, q, v);
  sotDEBUG(25) << "Inertia and Momentum updated" << std::endl;
  sotDEBUGOUT(25);
  return dummy;
}

dg::Matrix& DynamicPinocchio::computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res,
                                                     const int& time) {
  sotDEBUGIN(25);
  assert(m_model);
  assert(m_data);
  if (res.rows() != 6 || res.cols() != m_model->nv) res = Matrix::Zero(6, m_model->nv);
  jacobiansSINTERN(time);

  pinocchio::JointIndex id =
      isFrame ? m_model->frames[(pinocchio::JointIndex)jointId].parent : (pinocchio::JointIndex)jointId;

  // Computes Jacobian in world coordinates.
  pinocchio::getJointJacobian(*m_model, *m_data, id, pinocchio::WORLD, res);
  sotDEBUGOUT(25);
  return res;
}

dg::Matrix& DynamicPinocchio::computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int id,
                                                           dg::Matrix& res, const int& time) {
  sotDEBUGIN(25);
  assert(m_model);
  assert(m_data);
  if (res.rows() != 6 || res.cols() != m_model->nv) res = Matrix::Zero(6, m_model->nv);

  jacobiansSINTERN(time);

  pinocchio::FrameIndex fid;
  pinocchio::JointIndex jid;
  bool changeFrame = !isLocal;
  pinocchio::SE3 M;

  // Computes Jacobian in end-eff coordinates.
  if (isFrame) {
    changeFrame = true;
    fid = (pinocchio::FrameIndex)id;
    const pinocchio::Frame& frame = m_model->frames[fid];
    jid = frame.parent;

    M = frame.placement.inverse();
    if (!isLocal)  // Express the jacobian is world coordinate system.
      M.rotation() = m_data->oMf[fid].rotation() * M.rotation();
  } else {
    jid = (pinocchio::JointIndex)id;
    if (!isLocal) {  // Express the jacobian is world coordinate system.
      M.rotation() = m_data->oMi[jid].rotation();
      M.translation().setZero();
    }
  }
  pinocchio::getJointJacobian(*m_model, *m_data, jid, pinocchio::LOCAL, res);

  if (changeFrame) pinocchio::motionSet::se3Action(M, res, res);

  sotDEBUGOUT(25);
  return res;
}

MatrixHomogeneous& DynamicPinocchio::computeGenericPosition(const bool isFrame, const int id, MatrixHomogeneous& res,
                                                            const int& time) {
  sotDEBUGIN(25);
  forwardKinematicsSINTERN(time);
  if (isFrame) {
    const pinocchio::Frame& frame = m_model->frames[id];
    res.matrix() = (m_data->oMi[frame.parent] * frame.placement).toHomogeneousMatrix();
  } else {
    res.matrix() = m_data->oMi[id].toHomogeneousMatrix();
  }
  sotDEBUG(25) << "For " << (isFrame ? m_model->frames[id].name : m_model->names[id]) << " with id: " << id
               << " position is " << res << std::endl;
  sotDEBUGOUT(25);
  return res;
}

dg::Vector& DynamicPinocchio::computeGenericVelocity(const int jointId, dg::Vector& res, const int& time) {
  sotDEBUGIN(25);
  forwardKinematicsSINTERN(time);
  res.resize(6);
  const pinocchio::Motion& aRV = m_data->v[jointId];
  res << aRV.linear(), aRV.angular();
  sotDEBUGOUT(25);
  return res;
}

dg::Vector& DynamicPinocchio::computeGenericAcceleration(const int jointId, dg::Vector& res, const int& time) {
  sotDEBUGIN(25);
  forwardKinematicsSINTERN(time);
  res.resize(6);
  const pinocchio::Motion& aRA = m_data->a[jointId];
  res << aRA.linear(), aRA.angular();
  sotDEBUGOUT(25);
  return res;
}

int& DynamicPinocchio::computeNewtonEuler(int& dummy, const int& time) {
  sotDEBUGIN(15);
  assert(m_model);
  assert(m_data);
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
  const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
  const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
  pinocchio::rnea(*m_model, *m_data, q, v, a);

  sotDEBUG(1) << "pos = " << q << std::endl;
  sotDEBUG(1) << "vel = " << v << std::endl;
  sotDEBUG(1) << "acc = " << a << std::endl;

  sotDEBUGOUT(15);
  return dummy;
}

dg::Matrix& DynamicPinocchio::computeJcom(dg::Matrix& Jcom, const int& time) {
  sotDEBUGIN(25);
  forwardKinematicsSINTERN(time);
  Jcom = pinocchio::jacobianCenterOfMass(*m_model, *m_data, false);
  sotDEBUGOUT(25);
  return Jcom;
}

dg::Vector& DynamicPinocchio::computeCom(dg::Vector& com, const int& time) {
  sotDEBUGIN(25);
  if (JcomSOUT.needUpdate(time)) {
    forwardKinematicsSINTERN(time);
    pinocchio::centerOfMass(*m_model, *m_data, false);
  }
  com = m_data->com[0];
  sotDEBUGOUT(25);
  return com;
}

dg::Matrix& DynamicPinocchio::computeInertia(dg::Matrix& res, const int& time) {
  sotDEBUGIN(25);
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
  res = pinocchio::crba(*m_model, *m_data, q);
  res.triangularView<Eigen::StrictlyLower>() = res.transpose().triangularView<Eigen::StrictlyLower>();
  sotDEBUGOUT(25);
  return res;
}

dg::Matrix& DynamicPinocchio::computeInertiaReal(dg::Matrix& res, const int& time) {
  sotDEBUGIN(25);

  const dg::Matrix& A = inertiaSOUT(time);
  const dg::Vector& gearRatio = gearRatioSOUT(time);
  const dg::Vector& inertiaRotor = inertiaRotorSOUT(time);

  res = A;
  for (int i = 0; i < gearRatio.size(); ++i) res(i, i) += (gearRatio(i) * gearRatio(i) * inertiaRotor(i));

  sotDEBUGOUT(25);
  return res;
}

double& DynamicPinocchio::computeFootHeight(double& res, const int&) {
  // Ankle position in local foot frame
  // TODO: Confirm that it is in the foot frame
  sotDEBUGIN(25);
  if (!m_model->existJointName("r_sole_joint")) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC, "Robot has no joint corresponding to rigthFoot");
  }
  long int jointId = m_model->getJointId("r_sole_joint");
  Eigen::Vector3d anklePosInLocalRefFrame = m_data->liMi[jointId].translation();
  // TODO: positive or negative? Current output:negative
  res = anklePosInLocalRefFrame(2);
  sotDEBUGOUT(25);
  return res;
}

dg::Vector& DynamicPinocchio::computeTorqueDrift(dg::Vector& tauDrift, const int& time) {
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
  tauDrift = m_data->tau;
  sotDEBUGOUT(25);
  return tauDrift;
}

dg::Vector& DynamicPinocchio::computeMomenta(dg::Vector& Momenta, const int& time) {
  sotDEBUGIN(25);
  ccrbaSINTERN(time);
  if (Momenta.size() != 6) Momenta.resize(6);

  Momenta = m_data->hg.toVector_impl();

  sotDEBUGOUT(25) << "Momenta :" << Momenta;
  return Momenta;
}

dg::Vector& DynamicPinocchio::computeAngularMomentum(dg::Vector& Momenta, const int& time) {
  sotDEBUGIN(25);
  ccrbaSINTERN(time);

  if (Momenta.size() != 3) Momenta.resize(3);
  Momenta = m_data->hg.angular_impl();

  sotDEBUGOUT(25) << "AngularMomenta :" << Momenta;
  return Momenta;
}

/* ------------------------ SIGNAL CASTING--------------------------------------- */

dg::SignalTimeDependent<dg::Matrix, int>& DynamicPinocchio::jacobiansSOUT(const std::string& name) {
  SignalBase<int>& sigabs = Entity::getSignal(name);
  try {
    dg::SignalTimeDependent<dg::Matrix, int>& res = dynamic_cast<dg::SignalTimeDependent<dg::Matrix, int>&>(sigabs);
    return res;
  } catch (std::bad_cast e) {
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
                              " (while getting signal <%s> of type matrix.", name.c_str());
  }
}
dg::SignalTimeDependent<MatrixHomogeneous, int>& DynamicPinocchio::positionsSOUT(const std::string& name) {
  SignalBase<int>& sigabs = Entity::getSignal(name);
  try {
    dg::SignalTimeDependent<MatrixHomogeneous, int>& res =
        dynamic_cast<dg::SignalTimeDependent<MatrixHomogeneous, int>&>(sigabs);
    return res;
  } catch (std::bad_cast e) {
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
                              " (while getting signal <%s> of type matrixHomo.", name.c_str());
  }
}

dg::SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::velocitiesSOUT(const std::string& name) {
  SignalBase<int>& sigabs = Entity::getSignal(name);
  try {
    dg::SignalTimeDependent<dg::Vector, int>& res = dynamic_cast<dg::SignalTimeDependent<dg::Vector, int>&>(sigabs);
    return res;
  } catch (std::bad_cast e) {
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
                              " (while getting signal <%s> of type Vector.", name.c_str());
  }
}

dg::SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::accelerationsSOUT(const std::string& name) {
  SignalBase<int>& sigabs = Entity::getSignal(name);
  try {
    dg::SignalTimeDependent<dg::Vector, int>& res = dynamic_cast<dg::SignalTimeDependent<dg::Vector, int>&>(sigabs);
    return res;
  } catch (std::bad_cast e) {
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
                              " (while getting signal <%s> of type Vector.", name.c_str());
  }
}

/*-------------------------------------------------------------------------*/

/*-------------------------------------------------------------------------*/

/* --- PARAMS --------------------------------------------------------------- */

// jointName is either a fixed-joint (pinocchio operational frame) or a
// movable joint (pinocchio joint-variant).
void DynamicPinocchio::cmd_createOpPointSignals(const std::string& opPointName, const std::string& jointName) {
  createEndeffJacobianSignal(std::string("J") + opPointName, jointName, true);
  createPositionSignal(opPointName, jointName);
}
void DynamicPinocchio::cmd_createJacobianWorldSignal(const std::string& signalName, const std::string& jointName) {
  createJacobianSignal(signalName, jointName);
}
void DynamicPinocchio::cmd_createJacobianEndEffectorSignal(const std::string& signalName,
                                                           const std::string& jointName) {
  createEndeffJacobianSignal(signalName, jointName, true);
}

void DynamicPinocchio::cmd_createJacobianEndEffectorWorldSignal(const std::string& signalName,
                                                                const std::string& jointName) {
  createEndeffJacobianSignal(signalName, jointName, false);
}

void DynamicPinocchio::cmd_createPositionSignal(const std::string& signalName, const std::string& jointName) {
  createPositionSignal(signalName, jointName);
}
void DynamicPinocchio::cmd_createVelocitySignal(const std::string& signalName, const std::string& jointName) {
  createVelocitySignal(signalName, jointName);
}
void DynamicPinocchio::cmd_createAccelerationSignal(const std::string& signalName, const std::string& jointName) {
  createAccelerationSignal(signalName, jointName);
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ATAN2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ATAN2_HPP_INCLUDED
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/arch/common/detail/scalar/f_invtrig.hpp>
#include <boost/simd/arch/common/detail/scalar/d_invtrig.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/function/copysign.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_positive.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/signnz.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( atan2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      #ifndef BOOST_SIMD_NO_NANS
      if (is_nan(a0) || is_nan(a1)) return Nan<A0>();
      #endif

      #ifndef BOOST_SIMD_NO_INFINITIES
      if (is_inf(a0) && is_inf(a1))
      {
        a0 = copysign(One<A0>(), a0);
        a1 = copysign(One<A0>(), a1);
      }
      #endif
      A0 q = bs::abs(a0/a1);
      A0 z = detail::invtrig_base<A0,tag::radian_tag, tag::not_simd_type>::kernel_atan(q, rec(q));
      A0 sgn = signnz(a0);
      z = (is_positive(a1)? z: Pi<A0>()-z)*sgn;
    return is_eqz(a0) ? if_else_zero(is_negative(a1), Pi<A0>()*sgn) : z;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( atan2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &,  A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return std::atan2(a0, a1);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( atan2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::fast_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      A0 q = bs::abs(a0/a1);
      A0 z = detail::invtrig_base<A0,tag::radian_tag, tag::not_simd_type>::kernel_atan(q, bs::rec(q));
      return (is_positive(a1)? z: Pi<A0>()-z)*signnz(a0);
    }
  };
} } }

#endif


#include "sev/acp_ros_conversions/conversions.h"

#include <array>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <limits>
#include <sstream>
#include <utility>

#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/TwistStamped.h"
#include "minkindr_conversions/kindr_msg.h"
#include "std_msgs/Header.h"

#include "sev/acp/types.h"

#include <Eigen/Core>

namespace sev_conversions {

using Transformation = kindr::minimal::QuatTransformation;
using Position3D = kindr::minimal::Position;

// Conversion from rotation matrix to roll, pitch, yaw angles.
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
RotationMatrixToRollPitchYaw(const Eigen::MatrixBase<Derived>& rot) {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 3);
  Eigen::Matrix<typename Derived::Scalar, 3, 1> rpy;
  rpy(1, 0) =
      atan2(-rot(2, 0), sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0)));
  if (std::abs(cos(rpy(1, 0))) >
      static_cast<typename Derived::Scalar>(1.0e-12)) {
    rpy(2, 0) = atan2(rot(1, 0) / cos(rpy(1, 0)), rot(0, 0) / cos(rpy(1, 0)));
    rpy(0, 0) = atan2(rot(2, 1) / cos(rpy(1, 0)), rot(2, 2) / cos(rpy(1, 0)));
  } else if (sin(rpy(1, 0)) > static_cast<typename Derived::Scalar>(0)) {
    rpy(2, 0) = static_cast<typename Derived::Scalar>(0);
    rpy(0, 0) = atan2(rot(0, 1), rot(1, 1));
  } else {
    rpy(2, 0) = static_cast<typename Derived::Scalar>(0);
    rpy(0, 0) = -atan2(rot(0, 1), rot(1, 1));
  }
  return rpy;
}

// Conversion from roll, pitch, yaw to rotation matrix.
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
RollPitchYawToRotationMatrix(const Eigen::MatrixBase<Derived>& roll_pitch_yaw) {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 1);
  Eigen::Matrix<typename Derived::Scalar, 3, 3> rotation_matrix_x;
  typename Derived::Scalar zero = static_cast<typename Derived::Scalar>(0.);
  typename Derived::Scalar one = static_cast<typename Derived::Scalar>(1.);
  rotation_matrix_x << one, zero, zero, zero, cos(roll_pitch_yaw(0)),
      -sin(roll_pitch_yaw(0)), zero, sin(roll_pitch_yaw(0)),
      cos(roll_pitch_yaw(0));

  Eigen::Matrix<typename Derived::Scalar, 3, 3> rotation_matrix_y;
  rotation_matrix_y << cos(roll_pitch_yaw(1)), zero, sin(roll_pitch_yaw(1)),
      zero, one, zero, -sin(roll_pitch_yaw(1)), 0, cos(roll_pitch_yaw(1));

  Eigen::Matrix<typename Derived::Scalar, 3, 3> rotation_matrix_z;
  rotation_matrix_z << cos(roll_pitch_yaw(2)), -sin(roll_pitch_yaw(2)), zero,
      sin(roll_pitch_yaw(2)), cos(roll_pitch_yaw(2)), zero, zero, zero, one;

  return rotation_matrix_z * rotation_matrix_y * rotation_matrix_x;
}

using TimePoint = std::chrono::time_point<std::chrono::system_clock>;
using Duration = std::chrono::nanoseconds;
constexpr double kNanosecondsToSeconds = 1e-9;
constexpr double kSecondsToNanoSeconds = 1e9;

inline TimePoint toTimePoint(const int64_t timestamp_ns) {
  // Making sure that conversion between TimePoint and int64_t comes at no
  // runtime cost.
  static_assert(std::is_same<TimePoint::rep, int64_t>::value, "");
  static_assert(
      std::is_same<
          std::chrono::system_clock::period, std::ratio<1, 1000000000>>::value,
      "");
  return TimePoint{std::chrono::nanoseconds{timestamp_ns}};
}

inline TimePoint toTimePoint(const ros::Time& ros_time) {
  const int64_t timestamp_ns = ros_time.toNSec();
  return toTimePoint(timestamp_ns);
}

inline int64_t toNs(const TimePoint timestamp) {
  return timestamp.time_since_epoch().count();
}

inline ros::Time toRosTime(const TimePoint& time_point) {
  const int64_t timestamp_ns = toNs(time_point);
  const int32_t ros_timestamp_sec = timestamp_ns * kNanosecondsToSeconds;
  const int32_t ros_timestamp_nsec =
      timestamp_ns % static_cast<int64_t>(kSecondsToNanoSeconds);
  return ros::Time(ros_timestamp_sec, ros_timestamp_nsec);
}

inline ros::Time convertNanosecondsInt64ToRosTime(
    const int64_t timestamp_nanoseconds) {
  return toRosTime(toTimePoint(timestamp_nanoseconds));
}

inline int64_t convertRosTimeToNanosecondsInt64(
    const ros::Time& ros_timestamp) {
  return toNs(toTimePoint(ros_timestamp));
}
}  // namespace sev_conversions

namespace sev::acp::udp_ros_bridge {
constexpr double M_TO_MM = 1000;
#ifndef M_PI
#error "M_PI is not defined (issue with _USE_MATH_DEFINES?)"
#endif
constexpr double RAD_TO_CDEG = 100 * 180 / M_PI;

std_msgs::Header convertToRosStamped(sev::acp::MessageHeader const& mh) {
  std_msgs::Header retval;
  retval.seq = mh.seq;
  retval.stamp =
      sev_conversions::convertNanosecondsInt64ToRosTime(mh.timestamp);
  return retval;
}

sev::acp::MessageHeader convertToAcp(const std_msgs::Header& header) {
  sev::acp::MessageHeader mh;
  mh.seq = header.seq;
  mh.timestamp =
      sev_conversions::convertRosTimeToNanosecondsInt64(header.stamp);
  return mh;
}

auto getRotationMatrix(const geometry_msgs::Pose& ros_pose) {
  // Protection against invalid data.
  const auto compute_sq_norm = [](const auto& o) {
    return o.x * o.x + o.y * o.y + o.z * o.z + o.w * o.w;
  };
  const auto norm_sq = compute_sq_norm(ros_pose.orientation);

  // Copied from minkindr.
  // The decision boundary should be at the (binary represented) same number,
  // therefore using abs() would lead to slightly different decision than in
  // kindr's CHECK.
  if ((norm_sq > (1. - 1.e-4)) && (norm_sq < (1. + 1.e-4))) {
    sev_conversions::Transformation T_M_I;
    tf::poseMsgToKindr(ros_pose, &T_M_I);
    return T_M_I.getRotation().getRotationMatrix();
  } else {
    std::stringstream ss;
    ss << "Encountered non-normalized orientation during conversion:\n";
    ss << ros_pose.orientation;
    throw ConversionError(ss.str());
  }
}

geometry_msgs::Pose poseFromMatrix(
    const std::array<double, 3>& position_array,
    const sev_conversions::Transformation::RotationMatrix& rotation_matrix) {
  // TODO(pseyfert): Simplify until poseKindrToMsg.
  if (sev_conversions::Transformation::Rotation::isValidRotationMatrix(
          rotation_matrix)) {
    sev_conversions::Position3D position(&position_array[0]);
    const auto quat_trafo = sev_conversions::Transformation(
        sev_conversions::Transformation::Rotation{std::move(rotation_matrix)},
        std::move(position));
    geometry_msgs::Pose ros_pose;
    tf::poseKindrToMsg(std::move(quat_trafo), &ros_pose);
    return ros_pose;
  } else {
    std::stringstream ss;
    ss << "Encountered invalid rotation during conversion:\n";
    ss << rotation_matrix;
    throw ConversionError(ss.str());
  }
}

geometry_msgs::Pose poseFromMatrix(
    const std::array<double, 3>& position_array, const sev::acp::Rot3& R_m_r) {
  // TODO(pseyfert): Simplify until poseKindrToMsg.
  sev_conversions::Transformation::RotationMatrix rotation_matrix;
  for (std::size_t i = 0; i < 9; ++i) {
    rotation_matrix.data()[i] = R_m_r.mat[i];
  }
  return poseFromMatrix(position_array, rotation_matrix);
}

std::tuple<sev::acp::Pose> convertToAcp(
    const geometry_msgs::PoseStamped& ros_message) {
  sev::acp::Pose out_pose;
  const auto& ros_pose = ros_message.pose;
  out_pose.position[0] = ros_pose.position.x;
  out_pose.position[1] = ros_pose.position.y;
  out_pose.position[2] = ros_pose.position.z;

  for (std::size_t i = 0; i < 3; ++i) {
    out_pose.velocity[i] = std::numeric_limits<double>::quiet_NaN();
  }

  const auto rotation_matrix = getRotationMatrix(ros_pose);

  for (std::size_t i = 0; i < 9; ++i) {
    out_pose.R_m_r.mat[i] = rotation_matrix.data()[i];
  }
  out_pose.yaw =
      sev_conversions::RotationMatrixToRollPitchYaw(rotation_matrix)[2];

  out_pose.header = convertToAcp(ros_message.header);
  return std::make_tuple(std::move(out_pose));
}

std::tuple<geometry_msgs::PoseStamped> convertToRosStamped(
    const sev::acp::Pose& acp_pose) {
  geometry_msgs::PoseStamped ros_pose;

  std::array<double, 3> position;
  for (std::size_t i = 0; i < position.size(); i++) {
    position[i] = acp_pose.position[i];
  }
  ros_pose.pose = poseFromMatrix(position, acp_pose.R_m_r);
  // TODO(pseyfert): acp_pose.velocity is ignored.

  ros_pose.header = convertToRosStamped(acp_pose.header);
  return std::make_tuple(std::move(ros_pose));
}

std::tuple<sev::acp::WheelOdometryIntegrated, sev::acp::WheelOdometryInt>
convertToAcp(const geometry_msgs::TwistStamped& ros_message) {
  const auto& linear = ros_message.twist.linear;
  const auto& angular = ros_message.twist.angular;
  const auto header = convertToAcp(ros_message.header);

  const auto woi_int = [&]() {
    sev::acp::WheelOdometryInt woi;
    const auto buffer =
        std::array<int, 6>{linear.x * M_TO_MM,      linear.y * M_TO_MM,
                           linear.z * M_TO_MM,      angular.x * RAD_TO_CDEG,
                           angular.y * RAD_TO_CDEG, angular.z * RAD_TO_CDEG};
    // Can't use std::copy because woi.twist is packed and STL algorithms don't
    // handle packed structs.
    for (std::size_t i = 0; i < buffer.size(); ++i) {
      woi.twist[i] = buffer[i];
    }
    woi.header = header;
    return woi;
  }();
  const auto woi_integrated = [&]() {
    sev::acp::WheelOdometryIntegrated woi;
    const auto buffer = std::array{linear.x,  linear.y,  linear.z,
                                   angular.x, angular.y, angular.z};
    // Can't use std::copy because woi.twist is packed and STL algorithms don't
    // handle packed structs.
    for (std::size_t i = 0; i < buffer.size(); ++i) {
      woi.twist[i] = buffer[i];
    }
    woi.header = header;
    return woi;
  }();
  return {std::move(woi_integrated), std::move(woi_int)};
}

std::tuple<geometry_msgs::TwistStamped> convertToRosStamped(
    const sev::acp::WheelOdometryIntegrated& woi) {
  geometry_msgs::TwistStamped ros_message;

  std::size_t i = 0;
  ros_message.twist.linear.x = woi.twist[i++];
  ros_message.twist.linear.y = woi.twist[i++];
  ros_message.twist.linear.z = woi.twist[i++];
  ros_message.twist.angular.x = woi.twist[i++];
  ros_message.twist.angular.y = woi.twist[i++];
  ros_message.twist.angular.z = woi.twist[i++];

  ros_message.header = convertToRosStamped(woi.header);
  return std::make_tuple(std::move(ros_message));
}

std::tuple<sev::acp::OperationState> convertToAcp(
    const state_machine_msgs::State& status) {
  sev::acp::OperationState acp;
  acp.task = status.state_int;
  acp.stage = status.substate_int;
  // TODO(pseyfert): Fill with meaningful data.
  for (std::size_t i = 0; i < 16; ++i) {
    acp.task_uuid[i] = 0;
  }
  acp.optimization_progress = -1;
  acp.header = convertToAcp(status.header);
  return std::make_tuple(std::move(acp));
}

std::tuple<state_machine_msgs::State> convertToRosStamped(
    const sev::acp::OperationState& os) {
  state_machine_msgs::State state;
  state.state_int = os.task;
  state.substate_int = os.stage;
  // TODO(pseyfert): Fill with meaningful data.
  // state.state =
  // state.substate =
  state.header = convertToRosStamped(os.header);
  return std::make_tuple(std::move(state));
}

// sev::acp::Notifications
std::tuple<sev::acp::Notifications> convertToAcp(
    const state_machine_msgs::Status& status) {
  sev::acp::Notifications acp;
  acp.module_id = status.module;
  acp.status_code = status.status_code;
  acp.severity = status.severity;
  // TODO(pseyfert): Fill with meaningful data.
  acp.logId = -1;
  acp.logEntryId = -1;
  acp.header = convertToAcp(status.header);
  return std::make_tuple(std::move(acp));
}

std::tuple<state_machine_msgs::Status> convertToRosStamped(
    const sev::acp::Notifications& notifications) {
  state_machine_msgs::Status status;
  status.severity = notifications.severity;
  status.module = notifications.module_id;
  status.status_code = notifications.status_code;
  // TODO(pseyfert): Fill with meaningful data.
  // status.status_text =
  status.header = convertToRosStamped(notifications.header);
  return std::make_tuple(std::move(status));
}

std::tuple<atlas_msgs::PositioningUpdate> convertToRosStamped(
    const sev::acp::PoseInt& acp_pose) {
  atlas_msgs::PositioningUpdate retval;
  retval.header = convertToRosStamped(acp_pose.header);
  std::array<double, 3> position;
  for (std::size_t i = 0; i < 3; ++i) {
    position[i] = acp_pose.position_mm[i] / M_TO_MM;
  }
  Eigen::Matrix<double, 3, 1> angles{
      acp_pose.roll_cdeg / RAD_TO_CDEG,
      acp_pose.pitch_cdeg / RAD_TO_CDEG,
      acp_pose.yaw_cdeg / RAD_TO_CDEG,
  };
  const auto rotation_matrix =
      sev_conversions::RollPitchYawToRotationMatrix(angles);
  retval.pose = poseFromMatrix(position, rotation_matrix);
  retval.quality.level = acp_pose.quality;
  retval.is_relocalization_event = acp_pose.relocalization;
  return std::make_tuple(std::move(retval));
}

std::tuple<sev::acp::PoseFloat, sev::acp::PoseInt> convertToAcp(
    const atlas_msgs::PositioningUpdate& ros_msg) {
  const auto rotation_matrix = getRotationMatrix(ros_msg.pose);
  const auto angles =
      sev_conversions::RotationMatrixToRollPitchYaw(rotation_matrix);
  const auto header = convertToAcp(ros_msg.header);

  const auto pose_int = [&]() {
    sev::acp::PoseInt retval;

    retval.position_mm[0] = ros_msg.pose.position.x * M_TO_MM;
    retval.position_mm[1] = ros_msg.pose.position.y * M_TO_MM;
    retval.position_mm[2] = ros_msg.pose.position.z * M_TO_MM;

    for (std::size_t i = 0; i < 3; ++i) {
      // TODO(pseyfert): Fill with meaningful data.
      retval.velocity_mmps[i] = 0;
    }
    retval.quality = ros_msg.quality.level;

    retval.roll_cdeg = angles[0] * RAD_TO_CDEG;
    retval.pitch_cdeg = angles[1] * RAD_TO_CDEG;
    retval.yaw_cdeg = angles[2] * RAD_TO_CDEG;

    retval.relocalization = ros_msg.is_relocalization_event ? 1 : 0;

    retval.header = header;
    return retval;
  }();

  const auto pose_float = [&]() {
    sev::acp::PoseFloat retval;

    retval.position[0] = ros_msg.pose.position.x;
    retval.position[1] = ros_msg.pose.position.y;
    retval.position[2] = ros_msg.pose.position.z;

    for (std::size_t i = 0; i < 3; ++i) {
      retval.velocity[i] = std::numeric_limits<double>::quiet_NaN();
    }
    retval.quality = ros_msg.quality.level;

    for (std::size_t i = 0; i < 9; ++i) {
      retval.rot.mat[i] = rotation_matrix.data()[i];
    }
    retval.roll = angles[0];
    retval.pitch = angles[1];
    retval.yaw = angles[2];

    retval.relocalization = ros_msg.is_relocalization_event ? 1 : 0;

    retval.header = header;
    return retval;
  }();

  return {std::move(pose_float), std::move(pose_int)};
}

std::tuple<geometry_msgs::PoseStamped, atlas_msgs::PositioningUpdate>
convertToRosStamped(const sev::acp::PoseFloat& acp_pose) {
  atlas_msgs::PositioningUpdate positioning_update;
  geometry_msgs::PoseStamped ros_pose;

  std::array<double, 3> position;
  for (std::size_t i = 0; i < position.size(); i++) {
    position[i] = acp_pose.position[i];
  }
  positioning_update.pose = poseFromMatrix(position, acp_pose.rot);
  ros_pose.pose = poseFromMatrix(position, acp_pose.rot);

  positioning_update.header = convertToRosStamped(acp_pose.header);
  ros_pose.header = convertToRosStamped(acp_pose.header);
  positioning_update.quality.level = acp_pose.quality;
  positioning_update.is_relocalization_event = acp_pose.relocalization;
  return std::make_tuple(std::move(ros_pose), std::move(positioning_update));
}

std::tuple<geometry_msgs::TwistStamped> convertToRosStamped(
    const sev::acp::WheelOdometryInt& woi) {
  geometry_msgs::TwistStamped ros_message;

  std::size_t i = 0;
  ros_message.twist.linear.x = woi.twist[i++] / M_TO_MM;
  ros_message.twist.linear.y = woi.twist[i++] / M_TO_MM;
  ros_message.twist.linear.z = woi.twist[i++] / M_TO_MM;
  ros_message.twist.angular.x = woi.twist[i++] / RAD_TO_CDEG;
  ros_message.twist.angular.y = woi.twist[i++] / RAD_TO_CDEG;
  ros_message.twist.angular.z = woi.twist[i++] / RAD_TO_CDEG;

  ros_message.header = convertToRosStamped(woi.header);
  return std::make_tuple(std::move(ros_message));
}
}  // namespace sev::acp::udp_ros_bridge


#include <iostream>                  // for std::cout
#include <utility>                   // for std::pair
#include <algorithm>                 // for std::for_each
#include <boost/graph/graph_traits.hpp> // for creation of descriptors vertex and edges.
#include <boost/graph/adjacency_list.hpp> //for usage of adjacency list
#include <boost/graph/graphml.hpp>
#include <boost/graph/connected_components.hpp>
#include <ilcplex/ilocplex.h>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/graph/copy.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/program_options.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/exception/all.hpp>
#include <exception>
#include <vector>
ILOSTLBEGIN //initialization make vs work properly

using namespace boost;
namespace po = boost::program_options;
//basic definitions
typedef IloArray<IloNumVarArray> IloVarMatrix;

typedef dynamic_bitset<> db;
typedef std::map<int, std::size_t> rank_t; // => order on Element
typedef std::map<int, int> parent_t;

template <typename EdgeColorMap, typename ValidColorsMap>
struct valid_edge_color {
	valid_edge_color() { }
	valid_edge_color(EdgeColorMap color, ValidColorsMap v_colors) : m_color(color), v_map(v_colors) { }
	template <typename Edge>
	bool operator()(const Edge& e) const {
		return v_map.test(get(m_color, e));
	}
	EdgeColorMap m_color;
	ValidColorsMap v_map;
};

template<class Graph, class Mask>
void print_filtered_graph(Graph &g, Mask valid) { //pay atention to the position of the bits and the colors positions in array
	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef typename boost::dynamic_bitset<> db;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;

	valid_edge_color<EdgeColorMap, Mask> filter(get(edge_color, g), valid);
	fg tg(g, filter);
	print_edges(edges(tg).first, edges(tg).second, tg);
}
//template function to print edges.
template<class EdgeIter, class Graph>
void print_edges(EdgeIter first, EdgeIter last, const Graph& G) {
	typedef typename property_map<Graph, edge_color_t>::const_type ColorMap;
	ColorMap colors = get(edge_color, G);
	//make color type generic
	//typedef typename property_traits<ColorMap>::value_type ColorType;
	//ColorType edge_color;
	for (auto it = first; it != last; ++it) {
		std::cout << "Edge: " << "(" << source(*it, G) << "," << target(*it, G) << ") " << " Color: " << colors[*it] << "\n";
		std::cout << "Edge: " << "(" << target(*it, G) << "," << source(*it, G) << ") " << " Color: " << colors[*it] << "\n";
	}
	std::cout << " Number of vertex: " << num_vertices(G) << std::endl;
	std::cout << " Number of edges: " << num_edges(G) << std::endl;
	std::vector<int> components(num_vertices(G));
	int num = connected_components(G, &components[0]);
	std::vector<int>::size_type i;
	std::cout << "Total number of components: " << num << std::endl;
	for (i = 0; i != components.size(); ++i)
		std::cout << "Vertex " << i << " is in component " << components[i] << std::endl;
	std::cout << std::endl;
}
template<class Graph, class Mask>
int get_components(Graph &g, Mask &m, vector<int> &components) {
	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef typename boost::dynamic_bitset<> db;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;
	valid_edge_color<EdgeColorMap, Mask> filter(get(edge_color, g), m);
	fg tg(g, filter);
	int num = connected_components(tg, &components[0]);
	return num;
}

//MVCA modified always has k colors
template <class Graph>
int kLSFMVCA(Graph &g, int k_sup, int n_labels) {
	std::vector<int> components(num_vertices(g));
	db temp(n_labels);
	int f_colors = n_labels - num_vertices(g) + 1;
	int num_c = get_components(g, temp, components);
	int num_c_best = num_c;
	while (temp.count() < k_sup) {
		int best_label = 0;
		for (int i = 0; i < f_colors; ++i) {
			if (!temp.test(i)) {
				temp.set(i);
				int nc = get_components(g, temp, components);
				if (nc <= num_c_best) {
					num_c_best = nc;
					best_label = i;
				}
				temp.flip(i);
			}
		}
		temp.set(best_label);
	}
	num_c_best = get_components(g, temp, components);
	//print_filtered_graph(g,temp);
	return  num_c_best;//just to be right
}

template <class Graph>
db MkLSFMVCA(Graph &g, int k_sup, int n_labels) {
	std::vector<int> components(num_vertices(g));
	int f_colors = size - num_vertices(g) + 1;
	db temp(n_labels);
	int num_c = get_components(g, temp, components);
	int num_c_best = num_c;
	while (temp.count() < k_sup) {
		int best_label = 0;
		for (int i = 0; i < f_colors; ++i) {
			if (!temp.test(i)) {
				temp.set(i);
				int nc = get_components(g, temp, components);
				if (nc <= num_c_best) {
					num_c_best = nc;
					best_label = i;
				}
				temp.flip(i);
			}
		}
		temp.set(best_label);
	}
	//num_c_best = get_components(g, temp, components);
	//print_filtered_graph(g,temp);
	return  temp;//just to be right
}

int root(int current, std::vector<int> &parent) {
	while (parent[current] != current) {
		current = parent[current];
	}
	return current;
}

template<class Graph>
int max_reduce(Graph &g, int n_curr, int n_colors, std::vector<int> &comp, int label) {
	std::vector<int> parent(n_curr), level(n_curr);
	volatile int comp_a, comp_b; //so i could debug dont know why.
	int result;
	for (int i = 0; i < n_curr; ++i) {
		parent[i] = i;
		level[i] = 0;
	}
	result = 0;

	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;
	typedef typename fg::edge_iterator eit;
	eit it, end;
	db mask(n_colors);
	mask.set(label);
	valid_edge_color<EdgeColorMap, db> filter(get(edge_color, g), mask);
	fg G(g, filter);
	std::tie(it, end) = boost::edges(G);

	while (it != end) {
		comp_a = comp[source(*it, G)];
		comp_b = comp[target(*it, G)];
		if (comp_a != comp_b) {
			volatile int root_a, root_b;
			root_a = root(comp_a, parent);
			root_b = root(comp_b, parent);
			if (root(comp_a, parent) != root(comp_b, parent)) {
				if (level[root(comp_a, parent)] > level[root(comp_b, parent)]) parent[root(comp_b, parent)] = root(comp_a, parent);
				else {
					if (level[root(comp_a, parent)] == level[root(comp_b, parent)]) {
						level[root(comp_b, parent)]++;
					}
					parent[root(comp_a, parent)] = root(comp_b, parent);
				}
				result++;
			}
		}
		++it;
	}
	return result;
}

typedef typename adjacency_list<vecS, vecS, undirectedS, no_property, property<edge_color_t, int>> graph_t;

template<class Graph>
property_map<graph_t, edge_color_t>::type get_colors(Graph &g) {
	typedef typename property_map<Graph, edge_color_t>::type ColorMap;
	ColorMap colors = get(edge_color, g);
	//make color type generic
	return colors;
}

template<class Graph,class Model,class Mask>
void generateCuts(Graph g, Model &mod,Mask temp,int n_colors,IloBoolVarArray& z) {
	std::vector<int> components(num_vertices(g));
	auto colors = get_colors(g);
	graph_traits<graph_t>::edge_iterator it, end;
	//std::cout << " user cutting" << std::endl;
	int no = get_components(g, temp, components);
	int num_c_best = no;
	int best_label = -1;
	boost::random::mt19937 gen(std::time(0));
	boost::random::uniform_int_distribution<> dist(3, 10);
	int i = dist(gen);
	while (num_c_best >= i) {//peharps temp.count() < k_sup
														   /*boost::random::uniform_int_distribution<> gen(0, size-1);
														   int i = gen(rng);													   if (!temp.test(i))temp.set(i);
														   */
		int diff;
		int best_diff = num_vertices(g);
		no = num_c_best;
		best_label = -1;
		for (int i = 0; i < n_colors; ++i) {
			if (!temp.test(i)) {
				temp.set(i);
				num_c_best = get_components(g, temp, components);
				diff = no - num_c_best;
				if (diff < best_diff) {
					best_diff = diff;
					best_label = i;
					temp.flip(i);
					break;
				}
				temp.flip(i);
			}
		}
		if (best_label >= 0)temp.set(best_label);
		num_c_best = get_components(g, temp, components);
	}
	if (best_label >= 0)temp.flip(best_label);
	num_c_best = get_components(g, temp, components);
	if (num_c_best > 1) {
		//std::cout << "add user cut" << std::endl;
		//db temp1(size);
		std::tie(it, end) = edges(g);
		IloExpr expr(mod.getEnv());
		vector<db> masks(num_c_best);
		for (int i = 0; i < num_c_best; ++i) masks[i].resize(n_colors);
		while (it != end) {
			if (components[source(*it, g)] != components[target(*it, g)]) {
				masks[components[source(*it, g)]].set(colors[*it]);
				masks[components[target(*it, g)]].set(colors[*it]);
			}
			++it;
		}
		for (int i = 0; i < num_c_best; ++i) {
			for (int j = 0; j < n_colors; ++j) if (masks[i].test(j))expr += z[j];
			mod.add(expr >= 1);
			expr.clear();
		}
		expr.end();
	}

}

// preprocessing functions
template<class Graph>
void treefy(Graph& g, int n_colors) {
	Graph result(num_vertices(g));
	typedef boost::graph_traits<Graph>::edge_descriptor edge_t;
	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;
	typedef typename fg::edge_iterator eit;
	eit it, end;
	for (int l = 0; l < n_colors; ++l) {
		db mask(n_colors);
		mask.set(l);
		std::vector<int> components(num_vertices(g));// components graph
		int n_curr = get_components(g, mask, components);
		std::vector<int> my_mapping(n_curr, -1);
		for (int u = 0; u < num_vertices(g); ++u) {
			if (my_mapping[components[u]] == -1)my_mapping[components[u]] = u;
			else add_edge(my_mapping[components[u]], u, property<edge_color_t, int>(l), result);
		}
	}
	g.clear();
	copy_graph(result, g);
}
template<class Graph>
void completefy(Graph& g, int n_colors) {
	Graph result(num_vertices(g));
	typedef boost::graph_traits<Graph>::edge_descriptor edge_t;
	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;
	typedef typename fg::edge_iterator eit;
	eit it, end;
	for (int l = 0; l < n_colors; ++l) {
		db mask(n_colors);
		mask.set(l);
		std::vector<int> components(num_vertices(g));// components graph
		int n_curr = get_components(g, mask, components);
		std::vector<int> my_mapping(n_curr, -1);
		for (int u = 0; u < num_vertices(g); ++u) {
			for (int v = u + 1; v < num_vertices(g); ++v) {
				if (components[u] == components[v]) {
					add_edge(u, v, property<edge_color_t, int>(l), result);
				}
			}
		}
	}
	g.clear();
	copy_graph(result, g);
}
// preprocessing functions
template<class Graph>
void MCR(Graph& g, int n_colors) {
	Graph result(num_vertices(g));
	typedef boost::graph_traits<Graph>::edge_descriptor edge_t;
	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;
	typedef typename fg::edge_iterator eit;
	eit it, end;
	for (int l = 0; l < n_colors; ++l) {
		db mask(n_colors);
		mask.set(l);
		valid_edge_color<EdgeColorMap, db> filter(get(edge_color, g), mask);
		fg H(g, filter);
		typedef typename property_map<fg, vertex_index_t>::type IndexMap;
		IndexMap index = get(vertex_index, H);
		//disjoint_sets ds(num_vertices(g))

		rank_t rank_map;
		parent_t parent_map;
		boost::associative_property_map<rank_t>   rank_pmap(rank_map);
		boost::associative_property_map<parent_t> parent_pmap(parent_map);
		boost::disjoint_sets<
			associative_property_map<rank_t>,
			associative_property_map<parent_t> > ds(
				rank_pmap,
				parent_pmap);
		//std::vector<Element> elements;
		//elements.push_back(Element(...));
		//rank_t rank_map;
		//parent_t parent_map;

		//boost::associative_property_map<rank_t>   rank_pmap(rank_map);
		//boost::associative_property_map<parent_t> parent_pmap(parent_map);

		for (int i = 0; i < num_vertices(g); ++i) {
			ds.make_set(i);
		}
		std::tie(it, end) = edges(H);
		while (it != end) {
			int u = index[source(*it, H)];
			int v = index[target(*it, H)];
			if (ds.find_set(u) != ds.find_set(v)) {
				add_edge(u, v, property<edge_color_t, int>(l), result);
				ds.union_set(u, v);
			}
			else {
				std::cout << "MCR removed edge:" << " (" << u << "," << v << ") " << " Color: " << l << std::endl;
			}
			++it;
		}
	}
	g.clear();
	copy_graph(result, g);
}

template<class Graph> // dont work with multigraph
void buildFlowModel(IloModel mod, IloBoolVarArray Z, IloVarMatrix F, const int k, const Graph &g,int opt,int lower_bound) {
	IloEnv env = mod.getEnv();
	int n_colors = Z.getSize();
	typedef typename property_map<Graph, edge_color_t>::const_type ColorMap;
	typedef typename graph_traits<Graph>::edge_descriptor edge_desc;
	ColorMap colors = get(edge_color, g);
	/*std::vector<Graph> monocromatic_graphs(n_colors, Graph(num_vertices(g)));
	//creating new colored graphs mantain diferent colors
	auto[it_edges, last_edge] = edges(g);
	ColorMap colors = get(edge_color, g);
	while (it_edges != last_edge) {
		add_edge(source(*it_edges, g), target(*it_edges, g), property<edge_color_t, int>(colors[*it_edges]), monocromatic_graphs[colors[*it_edges]]);
		it_edges++;
	}*/

	//modelling objective function
	IloExpr exp(env);
	int n_vertices = num_vertices(g);
	int s = n_vertices - 1;//super source
	int f_colors = n_colors - n_vertices + 1;
	for (int i = f_colors; i < n_colors; ++i) {
		exp += Z[i];
	}
	mod.add(IloMinimize(env, exp));
	//mod.add(exp >= lower_bound);
	//mod.add(exp>=40);
	//mod.add(exp == 1);
	//exp.end();
	//modelling f_{ij} temporario ajeitar para deixar mais compactor depois um para cada aresta

	for (int i = 0; i < n_vertices; ++i) {
		F[i] = IloNumVarArray(env, n_vertices, 0, n_vertices-1, ILOFLOAT);
	}
	for (int i = 0; i < n_vertices; ++i) {
		for (int j = 0; j < n_vertices; ++j) {
			F[i][j].setName(("f_" + std::to_string(i) + "_" + std::to_string(j)).c_str());
		}
	}
	//setting names to labels variables.
	for (int i = 0; i<n_colors; ++i) {
		Z[i].setName(("z" + std::to_string(i)).c_str());
	}
	// first constraint
	typedef typename graph_traits<Graph>::vertex_iterator vertex_it;
	typedef typename graph_traits<Graph>::in_edge_iterator in_edge_it;
	IloExpr lhs(env);
	IloExpr lhs1(env);
	vertex_it vit, vend;
	std::tie(vit, vend) = vertices(g);
	in_edge_it eit, eend;
	for (auto it = vit; it != vend && *it<s; ++it) {
		std::tie(eit, eend) = in_edges(*it, g);
		for (auto tit = eit; tit != eend; ++tit) {
			lhs += F[source(*tit, g)][target(*tit, g)];
			//new constraint 1
			//if (source(*tit, g) != s) {
				lhs -= F[target(*tit, g)][source(*tit, g)];
				lhs1 += F[target(*tit, g)][source(*tit, g)];
			//}
		}
		mod.add(lhs == 0);
		mod.add(lhs1 >= 1);
		lhs.clear();
		lhs1.clear();
	}
	lhs.end();
	lhs1.end();

	edge_desc my_edge;
	bool result = false;
	//IloExpr expression1(env);
	for (int i = 0; i < n_vertices-1; ++i) {
		for (int j = 0; j < n_vertices-1; ++j) {
			std::tie(my_edge, result) = edge(i, j, g);
			if (result) {
				mod.add(F[i][j]<= (n_vertices - 1) * Z[colors[my_edge]]);
			}
		}

	}
	//expression1.end();
	//third big-mconstraint
	for (int i = f_colors; i < n_colors; ++i) {
		mod.add(F[s][i - f_colors] == IloInt(1));
		mod.add(F[i - f_colors][s] <= IloInt(n_vertices-1)*Z[i]);
		mod.add(F[i - f_colors][s] >= Z[i]);
		//mod.add(F[s][i - f_colors] <= IloInt(n_vertices - 1)*Z[i]);
	}
	exp.end();

	//constraint every vertex has a colored edge incident
	auto [first_vertex, last_vertex] = vertices(g);
	while (first_vertex != last_vertex) {
		db used_colors(n_colors);
		auto[first_edge, last_edge] = in_edges(*first_vertex,g);
		IloExpr expInEdges(env);
		while (first_edge != last_edge) {
			volatile int idx = colors[*first_edge];
			if (!used_colors.test_set(idx,1)) {
				expInEdges += Z[colors[*first_edge]];
			}
			first_edge++;
		}
		mod.add(expInEdges >= 1);
		expInEdges.end();
		first_vertex++;
	}

	IloExpr texp(env);
	for (int i = 0; i < f_colors; ++i) {
		texp += Z[i];
	}
	mod.add(texp == k);
	texp.end();

}

template<class Graph>
void solveModel(int n_vertices, int n_colors, int k, Graph &g) {

	//starting cplex code part
	IloEnv   env; //environment
	try {
		IloModel model(env);
		IloBoolVarArray Z(env, n_colors);
		IloNumArray pri(env, n_colors);
		IloVarMatrix    F(env, n_vertices); //each edge has at least a edge to the supersource
		int opt = kLSFMVCA(g, k, n_colors) - 1;
		buildFlowModel(model, Z, F, k, g, opt, 1);
		IloCplex cplex(model);
		int f_colors = n_colors - num_vertices(g) + 1;
		cplex.exportModel("kSLF_fluxo.lp"); // good to see if the model is correct
											//cross your fingers
		{//set priorities number edges by color.
			auto it = boost::edges(g).first;
			auto end = boost::edges(g).second;
			auto colormap = get(edge_color, g);
			while (it != end) {
				pri[colormap[*it]]++;
				++it;
			}
		}

		//trying to disable automatic cuts
		//cplex.setParam(IloCplex::Param::MIP::Limits::CutPasses,-1);
		cplex.setParam(IloCplex::Param::MIP::Tolerances::UpperCutoff, opt);
		cplex.setParam(IloCplex::Param::Threads, 4);
		//cplex.setParam(IloCplex::Param::Parallel, -1);
		cplex.setParam(IloCplex::Param::Emphasis::MIP, 1);
		cplex.setParam(IloCplex::Param::Benders::Strategy, 3);
		cplex.setParam(IloCplex::TiLim, 7300);
		cplex.setPriorities(Z, pri);
		cplex.solve();
		//cplex.exportModel("kSLF_fluxo_after_presolve.lp");
		cplex.out() << "solution status = " << cplex.getStatus() << endl;

		cplex.out() << endl;
		cplex.out() << "Number of components   = " << cplex.getObjValue() << endl;
		db temp(n_colors);
		cplex.out() << "color(s) solution:";
		for (int i = 0; i < f_colors; i++) {
			if (std::abs(cplex.getValue(Z[i]) - 1.0f) <= 1e-3)cplex.out() << " " << i;
		}
		cplex.out() << endl;
		cplex.out() << "root(s) solution:";
		//int f_colors = Z.getSize() - n_vertices + 1;
		for (int i = f_colors; i < n_colors; i++) {
			if (std::abs(cplex.getValue(Z[i]) - 1.0f) <= 1e-3)cplex.out() << " " << i - f_colors;
		}
		cplex.out() << endl;
	}
	catch (IloException& e) {
		cerr << "Concert exception caught: " << e << endl;
	}
	catch (...) {
		cerr << "Unknown exception caught" << endl;
	}
	//memory cleaning
	env.end();
}

int main(int argc, const char *argv[])
{
	typedef adjacency_list<vecS, vecS, undirectedS, no_property, property<edge_color_t, int>> Graph;
	typedef std::pair<int, int> Edge;
	typedef boost::graph_traits<Graph>::vertex_descriptor vertex_t;
	Graph::edge_iterator it, end;
	Graph g;
	int n_vertices, n_colors;
	//command-line processor

	try {
		std::ifstream ifn;
		po::options_description desc{ "Options" };
		desc.add_options()("help,h", "produce help message")
			("input-file,i", po::value< string >(), "input file")
			("include-path,I", po::value< string >(), "include path")
			("setup-file", po::value< string >(), "setup file");
		po::positional_options_description p;
		p.add("input-file", -1);

		po::variables_map vm;
		po::store(po::command_line_parser(argc, argv).
			options(desc).positional(p).run(), vm);
		po::notify(vm);

		if (vm.count("help")) {
			cout << desc << "\n";
			return 1;
		}
		else if (vm.count("input-file"))
		{
			std::cout << "Input files are: " << vm["input-file"].as<string>() << "\n";
			if (vm.count("include-path"))ifn.open((vm["include-path"].as<string>() + vm["input-file"].as<string>()).c_str(), ifstream::in);
			else ifn.open(vm["input-file"].as<string>().c_str(), ifstream::in);
			if (!ifn.is_open()) {
				std::cout << "error opening file" << std::endl;
				exit(EXIT_FAILURE);
			}
			dynamic_properties dp;
			dp.property("color", get(edge_color, g));
			read_graphml(ifn, g, dp);

			vector<string> vecI;
			split(vecI, vm["input-file"].as<string>(), is_any_of("-."), token_compress_off);
			if (vecI.size() == 6) {
				std::cout << vecI[0] << std::endl;
				n_vertices = stoi(vecI[0]);
				std::cout << vecI[2] << std::endl;
				n_colors = stoi(vecI[2]);
				std::cout << vecI[3] << std::endl;
				int k = stoi(vecI[3]);
				//add edges to super source vertex. remember!!!
				vertex_t u = add_vertex(g);
				n_vertices++;
				for (int i = 0; i < n_vertices - 1; ++i) boost::add_edge(u, i, property<edge_color_t, int>(n_colors++), g);
				//std::tie(it, end) = boost::edges(g);
				//print_edges(it, end, g);
				MCR(g, n_colors);
				//treefy(g, n_colors);
				//completefy(g, n_colors);
				//auto colors = get(edge_color, g);
				//dynamic_properties dp;
				//dp.property("Color", colors);
				//std::ofstream tmp("out.graphml");
				//write_graphml(tmp, g, dp, true);
				solveModel(n_vertices, n_colors, k, g);
			}
			else {
				std::cout << "file wrong name format." << std::endl;
			}

		}
		else if (vm.count("setup-file")) {
			std::cout << "Not implemented yet" << std::endl;
		}
		else {
			std::cout << "see options(-h)." << std::endl;
		}

	}
	catch (const po::error &ex) {
		std::cout << ex.what();
		exit(EXIT_FAILURE);
	}
	catch (boost::exception &ex) {
		std::cout << boost::diagnostic_information(ex) << std::endl;
	}
	catch (std::exception &ex) {
		std::cout << ex.what();
		exit(EXIT_FAILURE);
	}

	return 0;
}