/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_MODULE MultiBodyTest
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

// RBDyn
#include "RBDyn/Body.h"
#include "RBDyn/FK.h"
#include "RBDyn/Joint.h"
#include "RBDyn/MultiBody.h"
#include "RBDyn/MultiBodyConfig.h"
#include "RBDyn/MultiBodyGraph.h"

// arm
#include "XYZSarm.h"

BOOST_AUTO_TEST_CASE(MultiBodyGraphTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;

  MultiBodyGraph mbg1;

  // test addBody
  Body b1(RBInertiad(), "b1");
  Body b2(RBInertiad(), "b2");
  Body b3(RBInertiad(), "b3");
  Body b4(RBInertiad(), "b4");
  BOOST_CHECK_NO_THROW(mbg1.addBody(b1));
  BOOST_CHECK_NO_THROW(mbg1.addBody(b2));
  BOOST_CHECK_NO_THROW(mbg1.addBody(b3));
  BOOST_CHECK_NO_THROW(mbg1.addBody(b4));

  // name already exists
  BOOST_CHECK_THROW(mbg1.addBody(Body(RBInertiad(), "b3")), std::domain_error);

  // must be 4 nodes
  BOOST_CHECK_EQUAL(mbg1.nrNodes(), 4);

  // test nodeByName
  std::shared_ptr<MultiBodyGraph::Node> node1, node2;
  BOOST_CHECK_NO_THROW(node1 = mbg1.nodeByName("b1"));
  BOOST_CHECK_NO_THROW(node2 = mbg1.nodeByName("b2"));
  BOOST_CHECK_EQUAL(node1->body, b1);
  BOOST_CHECK_EQUAL(node2->body, b2);

  // test addJoint
  Joint j1(Joint::RevX, true, "j1");
  Joint j2(Joint::RevX, true, "j2");
  Joint j3(Joint::RevX, true, "j3");
  BOOST_CHECK_NO_THROW(mbg1.addJoint(j1));
  BOOST_CHECK_NO_THROW(mbg1.addJoint(j2));
  BOOST_CHECK_NO_THROW(mbg1.addJoint(j3));

  // name already exists
  BOOST_CHECK_THROW(mbg1.addJoint(Joint(Joint::RevX, true, "j3")), std::domain_error);

  // must be 3 joints
  BOOST_CHECK_EQUAL(mbg1.nrJoints(), 3);

  // test jointByName
  std::shared_ptr<Joint> joint1, joint2, joint3;
  BOOST_CHECK_NO_THROW(joint1 = mbg1.jointByName("j1"));
  BOOST_CHECK_NO_THROW(joint2 = mbg1.jointByName("j2"));
  BOOST_CHECK_NO_THROW(joint3 = mbg1.jointByName("j3"));

  BOOST_CHECK_EQUAL(*joint1, j1);
  BOOST_CHECK_EQUAL(*joint2, j2);
  BOOST_CHECK_EQUAL(*joint3, j3);

  // check non-existant name
  BOOST_CHECK_THROW(mbg1.jointByName("j10"), std::out_of_range);

  // test linkBody
  //        b2
  //   j1  /   j2      j3
  //       b1 ---- b3 ---- b4

  BOOST_CHECK_NO_THROW(mbg1.linkBodies("b1", PTransformd::Identity(), "b2", PTransformd::Identity(), "j1"));
  BOOST_CHECK_NO_THROW(mbg1.linkBodies("b1", PTransformd::Identity(), "b3", PTransformd::Identity(), "j2"));
  BOOST_CHECK_NO_THROW(mbg1.linkBodies("b3", PTransformd::Identity(), "b4", PTransformd::Identity(), "j3"));

  // check non-existant body 1
  BOOST_CHECK_THROW(mbg1.linkBodies("b10", PTransformd::Identity(), "b4", PTransformd::Identity(), "j3"),
                    std::out_of_range);
  // check non-existant body 2
  BOOST_CHECK_THROW(mbg1.linkBodies("b3", PTransformd::Identity(), "b10", PTransformd::Identity(), "j3"),
                    std::out_of_range);
  // check non-existant joint
  BOOST_CHECK_THROW(mbg1.linkBodies("b3", PTransformd::Identity(), "b4", PTransformd::Identity(), "j10"),
                    std::out_of_range);
}

void checkMultiBodyNames(const rbd::MultiBody & mb,
                         const std::vector<std::string> & bodies,
                         const std::vector<std::string> & joints)
{
  for(const rbd::Body & b : mb.bodies())
  {
    BOOST_CHECK(std::find(bodies.begin(), bodies.end(), b.name()) != bodies.end());
  }
  for(const rbd::Joint & j : mb.joints())
  {
    BOOST_CHECK(std::find(joints.begin(), joints.end(), j.name()) != joints.end());
  }
}

BOOST_AUTO_TEST_CASE(MultiBodyGraphRmTest)
{
  rbd::MultiBody mb;
  rbd::MultiBodyConfig mbc;
  rbd::MultiBodyGraph mbgBack;
  std::tie(mb, mbc, mbgBack) = makeXYZSarm();

  // test copy constructor
  rbd::MultiBodyGraph mbg(mbgBack);
  BOOST_CHECK_EQUAL(mbg.nrJoints(), mbgBack.nrJoints());
  BOOST_CHECK_EQUAL(mbg.nrNodes(), mbgBack.nrNodes());

  BOOST_CHECK_EQUAL(mbg.nrJoints(), 4);
  BOOST_CHECK_EQUAL(mbg.nrNodes(), 5);
  BOOST_CHECK_EQUAL(mb.nrJoints(), 5);
  BOOST_CHECK_EQUAL(mb.nrBodies(), 5);

  mbg.removeJoint("b0", "j3");

  mb = mbg.makeMultiBody("b0", true);
  BOOST_CHECK_EQUAL(mbg.nrJoints(), 3);
  BOOST_CHECK_EQUAL(mbg.nrNodes(), 4);
  BOOST_CHECK_EQUAL(mb.nrJoints(), 4);
  BOOST_CHECK_EQUAL(mb.nrBodies(), 4);
  checkMultiBodyNames(mb, {"b0", "b1", "b2", "b3"}, {"Root", "j0", "j1", "j2"});

  // test operator=
  mbg = mbgBack;
  BOOST_CHECK_EQUAL(mbg.nrJoints(), mbgBack.nrJoints());
  BOOST_CHECK_EQUAL(mbg.nrNodes(), mbgBack.nrNodes());

  mbg.removeJoint("b0", "j0");
  mb = mbg.makeMultiBody("b0", true);
  BOOST_CHECK_EQUAL(mbg.nrJoints(), 0);
  BOOST_CHECK_EQUAL(mbg.nrNodes(), 1);
  BOOST_CHECK_EQUAL(mb.nrJoints(), 1);
  BOOST_CHECK_EQUAL(mb.nrBodies(), 1);

  BOOST_CHECK_EQUAL(mb.body(0).name(), "b0");
  BOOST_CHECK_EQUAL(mb.joint(0).name(), "Root");

  mbg = mbgBack;
  BOOST_CHECK_EQUAL(mbg.nrJoints(), mbgBack.nrJoints());
  BOOST_CHECK_EQUAL(mbg.nrNodes(), mbgBack.nrNodes());

  mbg.removeJoints("b0", std::vector<std::string>({"j3", "j2"}));
  mb = mbg.makeMultiBody("b0", true);
  BOOST_CHECK_EQUAL(mbg.nrJoints(), 2);
  BOOST_CHECK_EQUAL(mbg.nrNodes(), 3);
  BOOST_CHECK_EQUAL(mb.nrJoints(), 3);
  BOOST_CHECK_EQUAL(mb.nrBodies(), 3);
  checkMultiBodyNames(mb, {"b0", "b1", "b2"}, {"Root", "j0", "j1"});

  mbg = mbgBack;
  BOOST_CHECK_EQUAL(mbg.nrJoints(), mbgBack.nrJoints());
  BOOST_CHECK_EQUAL(mbg.nrNodes(), mbgBack.nrNodes());

  mbg.removeJoint("b0", "j1");
  mb = mbg.makeMultiBody("b0", true);
  BOOST_CHECK_EQUAL(mbg.nrJoints(), 2);
  BOOST_CHECK_EQUAL(mbg.nrNodes(), 3);
  BOOST_CHECK_EQUAL(mb.nrJoints(), 3);
  BOOST_CHECK_EQUAL(mb.nrBodies(), 3);
  checkMultiBodyNames(mb, {"b0", "b1", "b4"}, {"Root", "j0", "j3"});

  mbg = mbgBack;
  BOOST_CHECK_EQUAL(mbg.nrJoints(), mbgBack.nrJoints());
  BOOST_CHECK_EQUAL(mbg.nrNodes(), mbgBack.nrNodes());

  mbg.removeJoints("b0", std::vector<std::string>({"j1", "j2"}));
  mb = mbg.makeMultiBody("b0", true);
  BOOST_CHECK_EQUAL(mbg.nrJoints(), 2);
  BOOST_CHECK_EQUAL(mbg.nrNodes(), 3);
  BOOST_CHECK_EQUAL(mb.nrJoints(), 3);
  BOOST_CHECK_EQUAL(mb.nrBodies(), 3);
  checkMultiBodyNames(mb, {"b0", "b1", "b4"}, {"Root", "j0", "j3"});
}

void checkMultiBodyEq(const rbd::MultiBody & mb,
                      std::vector<rbd::Body> bodies,
                      std::vector<rbd::Joint> joints,
                      std::vector<int> pred,
                      std::vector<int> succ,
                      std::vector<int> parent,
                      std::vector<sva::PTransformd> Xt)
{
  // bodies
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.bodies().begin(), mb.bodies().end(), bodies.begin(), bodies.end());
  // joints
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.joints().begin(), mb.joints().end(), joints.begin(), joints.end());
  // pred
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.predecessors().begin(), mb.predecessors().end(), pred.begin(), pred.end());
  // succ
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.successors().begin(), mb.successors().end(), succ.begin(), succ.end());
  // parent
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.parents().begin(), mb.parents().end(), parent.begin(), parent.end());

  // Xt
  BOOST_CHECK_EQUAL_COLLECTIONS(mb.transforms().begin(), mb.transforms().end(), Xt.begin(), Xt.end());

  // nrBodies
  BOOST_CHECK_EQUAL(mb.nrBodies(), bodies.size());
  // nrJoints
  BOOST_CHECK_EQUAL(mb.nrJoints(), bodies.size());

  int params = 0, dof = 0;
  for(int i = 0; i < static_cast<int>(joints.size()); ++i)
  {
    BOOST_CHECK_EQUAL(mb.jointPosInParam(i), params);
    BOOST_CHECK_EQUAL(mb.jointsPosInParam()[i], params);
    BOOST_CHECK_EQUAL(mb.sJointPosInParam(i), params);

    BOOST_CHECK_EQUAL(mb.jointPosInDof(i), dof);
    BOOST_CHECK_EQUAL(mb.jointsPosInDof()[i], dof);
    BOOST_CHECK_EQUAL(mb.sJointPosInDof(i), dof);

    params += joints[i].params();
    dof += joints[i].dof();
  }

  BOOST_CHECK_EQUAL(params, mb.nrParams());
  BOOST_CHECK_EQUAL(dof, mb.nrDof());

  BOOST_CHECK_THROW(mb.sJointPosInParam(mb.nrJoints()), std::out_of_range);
  BOOST_CHECK_THROW(mb.sJointPosInDof(mb.nrJoints()), std::out_of_range);
}

BOOST_AUTO_TEST_CASE(MultiBodyTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  //              b2
  //          j1  /  j2      j3
  // root --j0-- b1 ---- b6 ---- b5

  RBInertiad r(1., Vector3d::Random(), Matrix3d::Random());
  std::vector<Body> bodies = {Body(r, "b1"), Body(r, "b2"), Body(r, "b3"), Body(r, "b4")};

  std::vector<Joint> joints = {Joint(Joint::RevX, true, "j0"), Joint(Joint::RevX, true, "j1"),
                               Joint(Joint::RevX, true, "j2"), Joint(Joint::RevX, true, "j3")};

  std::vector<int> pred = {-1, 0, 0, 2};
  std::vector<int> succ = {0, 1, 2, 3};
  std::vector<int> parent = {-1, 0, 0, 2};
  Vector3d tmp = Vector3d::Random();

  PTransformd I = PTransformd::Identity();
  std::vector<PTransformd> Xt = {I, I, PTransformd(tmp), I};

  MultiBody mb(bodies, joints, pred, succ, parent, Xt);

  // check MultiBody equality
  checkMultiBodyEq(mb, bodies, joints, pred, succ, parent, Xt);

  // NameToIndex
  // bodyIndexByName
  BOOST_CHECK_EQUAL(mb.bodyIndexByName("b1"), 0);
  BOOST_CHECK_EQUAL(mb.bodyIndexByName("b2"), 1);
  BOOST_CHECK_EQUAL(mb.bodyIndexByName("b3"), 2);
  BOOST_CHECK_EQUAL(mb.bodyIndexByName("b4"), 3);

  // jointIndexByName
  BOOST_CHECK_EQUAL(mb.jointIndexByName("j0"), 0);
  BOOST_CHECK_EQUAL(mb.jointIndexByName("j1"), 1);
  BOOST_CHECK_EQUAL(mb.jointIndexByName("j2"), 2);
  BOOST_CHECK_EQUAL(mb.jointIndexByName("j3"), 3);

  // safe accessors
  // body
  BOOST_CHECK_NO_THROW(mb.sBody(0));
  BOOST_CHECK_THROW(mb.sBody(10), std::out_of_range);

  // joint
  BOOST_CHECK_NO_THROW(mb.sJoint(0));
  BOOST_CHECK_THROW(mb.sJoint(10), std::out_of_range);

  // pred
  BOOST_CHECK_NO_THROW(mb.sPredecessor(0));
  BOOST_CHECK_THROW(mb.sPredecessor(10), std::out_of_range);

  // succ
  BOOST_CHECK_NO_THROW(mb.sSuccessor(0));
  BOOST_CHECK_THROW(mb.sSuccessor(10), std::out_of_range);

  // parent
  BOOST_CHECK_NO_THROW(mb.sParent(0));
  BOOST_CHECK_THROW(mb.sParent(10), std::out_of_range);

  // transformFrom
  BOOST_CHECK_NO_THROW(mb.sTransform(0));
  BOOST_CHECK_THROW(mb.sTransform(10), std::out_of_range);

  // bodyIndexById
  BOOST_CHECK_NO_THROW(mb.sBodyIndexByName("b1"));
  BOOST_CHECK_THROW(mb.sBodyIndexByName("b10"), std::out_of_range);

  // jointIndexById
  BOOST_CHECK_NO_THROW(mb.sJointIndexByName("j0"));
  BOOST_CHECK_THROW(mb.sJointIndexByName("j10"), std::out_of_range);

  // Setter test

  // transform setter
  sva::PTransformd newTrans(Vector3d(1., 1., 1));
  BOOST_CHECK_NO_THROW(mb.sTransform(1, newTrans));
  BOOST_CHECK_THROW(mb.sTransform(10, newTrans), std::out_of_range);
  BOOST_CHECK_EQUAL(mb.transform(1), newTrans);
  mb.transform(0, newTrans);
  BOOST_CHECK_EQUAL(mb.transform(0), newTrans);

  // transforms setter
  std::vector<PTransformd> newXt = {newTrans, newTrans, I, tmp};
  BOOST_CHECK_NO_THROW(mb.sTransforms(newXt));
  BOOST_CHECK_THROW(mb.sTransforms({newTrans, newTrans, I}), std::runtime_error);
  checkMultiBodyEq(mb, bodies, joints, pred, succ, parent, newXt);

  // body setter
  int b3Index = mb.bodyIndexByName("b3");
  Body oldBody(mb.body(b3Index));
  RBInertiad newR(123., Vector3d::Random(), Matrix3d::Random());
  Body newBody(newR, "b3");
  BOOST_CHECK_NO_THROW(mb.sBody(b3Index, newBody));
  BOOST_CHECK_THROW(mb.sBody(10, newBody), std::out_of_range);
  BOOST_CHECK_EQUAL(mb.body(b3Index), newBody);
  mb.body(b3Index, oldBody);
  BOOST_CHECK_EQUAL(mb.body(b3Index), oldBody);

  // bodies setter
  std::vector<Body> newBodies = {Body(newR, "b1"), Body(newR, "b2"), Body(newR, "b3"), Body(r, "b4")};
  BOOST_CHECK_NO_THROW(mb.sBodies(newBodies));
  BOOST_CHECK_THROW(mb.sBodies({Body(newR, "b1"), Body(newR, "b2")}), std::runtime_error);
  checkMultiBodyEq(mb, bodies, joints, pred, succ, parent, newXt);
}

BOOST_AUTO_TEST_CASE(MakeMultiBodyTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace constants = boost::math::constants;

  MultiBodyGraph mbg1;

  RBInertiad rbi(1., Vector3d::Zero(), Matrix3d::Identity());
  Body b1(rbi, "b1");
  Body b2(rbi, "b2");
  Body b3(rbi, "b3");
  Body b4(rbi, "b4");
  mbg1.addBody(b1);
  mbg1.addBody(b2);
  mbg1.addBody(b3);
  mbg1.addBody(b4);

  Joint j1(Joint::RevX, true, "j1");
  Joint j2(Joint::RevX, true, "j2");
  Joint j3(Joint::RevX, true, "j3");
  mbg1.addJoint(j1);
  mbg1.addJoint(j2);
  mbg1.addJoint(j3);

  //        b2
  //   j1  /   j2      j3
  //       b1 ---- b3 ---- b4
  mbg1.linkBodies("b1", PTransformd::Identity(), "b2", PTransformd::Identity(), "j1");
  mbg1.linkBodies("b1", PTransformd::Identity(), "b3", PTransformd::Identity(), "j2");
  mbg1.linkBodies("b3", PTransformd::Identity(), "b4", PTransformd::Identity(), "j3");

  //                b2
  //           j1  /  j2      j3
  // root -fixed- b1 ---- b3 ---- b4

  BOOST_CHECK_NO_THROW(mbg1.makeMultiBody("b1", true));
  BOOST_CHECK_THROW(mbg1.makeMultiBody("b10", true), std::out_of_range);

  // test the successorJoints and predecessorJoint function
  auto testSucc = [](const std::map<std::string, std::vector<std::string>> & s1,
                     const std::map<std::string, std::vector<std::string>> & s2) {
    BOOST_CHECK_EQUAL(s1.size(), s2.size());
    auto it1 = s1.begin();
    auto it2 = s2.begin();
    for(; it1 != s1.end(); ++it1, ++it2)
    {
      BOOST_CHECK_EQUAL(it1->first, it2->first);
      BOOST_CHECK_EQUAL_COLLECTIONS(it1->second.begin(), it1->second.end(), it2->second.begin(), it2->second.end());
    }
  };
  auto testPred = [](const std::map<std::string, std::string> & s1, const std::map<std::string, std::string> & s2) {
    BOOST_CHECK_EQUAL(s1.size(), s2.size());
    auto it1 = s1.begin();
    auto it2 = s2.begin();
    for(; it1 != s1.end(); ++it1, ++it2)
    {
      BOOST_CHECK_EQUAL(it1->first, it2->first);
      BOOST_CHECK_EQUAL(it1->second, it2->second);
    }
  };
  std::map<std::string, std::vector<std::string>> succRes1{
      {"b1", {"j1", "j2"}}, {"b2", {}}, {"b3", {"j3"}}, {"b4", {}}};
  std::map<std::string, std::string> predRes1{{"b1", "Root"}, {"b2", "j1"}, {"b3", "j2"}, {"b4", "j3"}};

  testSucc(mbg1.successorJoints("b1"), succRes1);
  testPred(mbg1.predecessorJoint("b1"), predRes1);

  MultiBody mb1 = mbg1.makeMultiBody("b1", true);

  std::vector<Body> bodies = {b1, b2, b3, b4};

  std::vector<Joint> joints = {Joint(Joint::Fixed, true, "Root"), j1, j2, j3};

  std::vector<int> pred = {-1, 0, 0, 2};
  std::vector<int> succ = {0, 1, 2, 3};
  std::vector<int> parent = {-1, 0, 0, 2};

  PTransformd I = PTransformd::Identity();
  std::vector<PTransformd> Xt = {I, I, I, I};

  // check MultiBody equality
  checkMultiBodyEq(mb1, bodies, joints, pred, succ, parent, Xt);

  // check bodyIndexById
  BOOST_CHECK_EQUAL(mb1.bodyIndexByName("b1"), 0);
  BOOST_CHECK_EQUAL(mb1.bodyIndexByName("b2"), 1);
  BOOST_CHECK_EQUAL(mb1.bodyIndexByName("b3"), 2);
  BOOST_CHECK_EQUAL(mb1.bodyIndexByName("b4"), 3);

  // check jointIndexById
  BOOST_CHECK_EQUAL(mb1.jointIndexByName("Root"), 0);
  BOOST_CHECK_EQUAL(mb1.jointIndexByName("j1"), 1);
  BOOST_CHECK_EQUAL(mb1.jointIndexByName("j2"), 2);
  BOOST_CHECK_EQUAL(mb1.jointIndexByName("j3"), 3);

  //                b2
  //          j1  /  j2      j3
  // root -free- b1 ---- b3 ---- b4

  MultiBody mb2 = mbg1.makeMultiBody("b1", false);
  joints = {Joint(Joint::Free, true, "Root"), j1, j2, j3};

  // check MultiBody equality
  checkMultiBodyEq(mb2, bodies, joints, pred, succ, parent, Xt);

  // test the successorJoints and predecessorJoint function
  std::map<std::string, std::vector<std::string>> succRes2{
      {"b1", {"j1", "j2"}}, {"b2", {}}, {"b3", {"j3"}}, {"b4", {}}};
  std::map<std::string, std::string> predRes2{{"b1", "Root"}, {"b2", "j1"}, {"b3", "j2"}, {"b4", "j3"}};

  testSucc(mbg1.successorJoints("b1"), succRes2);
  testPred(mbg1.predecessorJoint("b1"), predRes2);

  //                   j1      j2      j3
  // root -fixed-  b2 ---- b1 ---- b3 ---- b4

  MultiBody mb3 = mbg1.makeMultiBody("b2", true);

  // test the successorJoints and predecessorJoint function
  std::map<std::string, std::vector<std::string>> succRes3{{"b2", {"j1"}}, {"b1", {"j2"}}, {"b3", {"j3"}}, {"b4", {}}};
  std::map<std::string, std::string> predRes3{{"b2", "Root"}, {"b1", "j1"}, {"b3", "j2"}, {"b4", "j3"}};

  testSucc(mbg1.successorJoints("b2"), succRes3);
  testPred(mbg1.predecessorJoint("b2"), predRes3);

  bodies = {b2, b1, b3, b4};

  j1.forward(false);
  joints = {Joint(Joint::Fixed, true, "Root"), j1, j2, j3};

  pred = {-1, 0, 1, 2};
  succ = {0, 1, 2, 3};
  parent = {-1, 0, 1, 2};

  Xt = {I, I, I, I};

  // check joint j1 direction
  BOOST_CHECK_EQUAL(mb3.joint(1).forward(), false);

  // check MultiBody equality
  checkMultiBodyEq(mb3, bodies, joints, pred, succ, parent, Xt);

  // check bodyIndexByName
  BOOST_CHECK_EQUAL(mb3.bodyIndexByName("b1"), 1);
  BOOST_CHECK_EQUAL(mb3.bodyIndexByName("b2"), 0);
  BOOST_CHECK_EQUAL(mb3.bodyIndexByName("b3"), 2);
  BOOST_CHECK_EQUAL(mb3.bodyIndexByName("b4"), 3);

  // check jointIndexByName
  BOOST_CHECK_EQUAL(mb3.jointIndexByName("Root"), 0);
  BOOST_CHECK_EQUAL(mb3.jointIndexByName("j1"), 1);
  BOOST_CHECK_EQUAL(mb3.jointIndexByName("j2"), 2);
  BOOST_CHECK_EQUAL(mb3.jointIndexByName("j3"), 3);

  // check transform                         j2
  //                       j1           tx(1)-tz(-1) --- B3
  // root -fixed- B1 -tx(1)-ty(-1)- B2 /     j3
  //                                   \ rx(90)-I ---- B4

  MultiBodyGraph mbg2;

  mbg2.addBody(b1);
  mbg2.addBody(b2);
  mbg2.addBody(b3);
  mbg2.addBody(b4);

  mbg2.addJoint(j1);
  mbg2.addJoint(j2);
  mbg2.addJoint(j3);

  mbg2.linkBodies("b1", PTransformd(Vector3d(Vector3d::UnitX())), "b2", PTransformd(Vector3d(-Vector3d::UnitY())),
                  "j1");
  mbg2.linkBodies("b2", PTransformd(Vector3d(Vector3d::UnitX())), "b3", PTransformd(Vector3d(-Vector3d::UnitZ())),
                  "j2");
  mbg2.linkBodies("b2", PTransformd(Matrix3d(sva::RotX(constants::pi<double>() / 2.))), "b4", PTransformd::Identity(),
                  "j3");

  // add () around the Vector3d because Clang think that
  // a function declaration
  sva::PTransformd root((Vector3d(Vector3d::Random())));
  MultiBody mb4 = mbg2.makeMultiBody("b1", true, root);

  // test the successorJoints and predecessorJoint function
  std::map<std::string, std::vector<std::string>> succRes4{
      {"b1", {"j1"}}, {"b2", {"j2", "j3"}}, {"b3", {}}, {"b4", {}}};
  std::map<std::string, std::string> predRes4{{"b1", "Root"}, {"b2", "j1"}, {"b3", "j2"}, {"b4", "j3"}};

  testSucc(mbg2.successorJoints("b1"), succRes4);
  testPred(mbg2.predecessorJoint("b1"), predRes4);

  bodies = {b1, b2, b3, b4};

  joints = {Joint(Joint::Fixed, true, "Root"), j1, j2, j3};

  pred = {-1, 0, 1, 1};
  succ = {0, 1, 2, 3};
  parent = {-1, 0, 1, 1};

  Xt = {root, PTransformd(Vector3d(1., 0., 0.)), PTransformd(Vector3d(1., 1., 0.)),
        PTransformd(RotX(constants::pi<double>() / 2.), Vector3d(0., 1., 0.))};

  // check MultiBody equality
  checkMultiBodyEq(mb4, bodies, joints, pred, succ, parent, Xt);

  // check body com
  std::vector<Vector3d> bCom = {Vector3d::Zero(), Vector3d(0., 1., 0.), Vector3d(0., 0., 1.), Vector3d(0., 0., 0.)};

  for(int i = 0; i < mb4.nrBodies(); ++i)
  {
    BOOST_CHECK_EQUAL(mb4.body(i).inertia().momentum(), bCom[i]);
  }
}

BOOST_AUTO_TEST_CASE(MultiBodyConfigFunction)
{
  using namespace std;
  using namespace Eigen;
  using namespace rbd;

  std::vector<std::vector<double>> v1 = {{}, {1., 2., 3.}, {4.}, {5., 6.}, {7.}};
  std::vector<std::vector<double>> v2 = {{}, {0., 0., 0.}, {0.}, {0., 0.}, {0.}};
  VectorXd e(7);

  BOOST_CHECK_NO_THROW(sParamToVector(v1, e));
  BOOST_CHECK_NO_THROW(sVectorToParam(e, v2));

  for(std::size_t i = 0; i < v1.size(); ++i)
  {
    BOOST_CHECK_EQUAL_COLLECTIONS(v1[i].begin(), v1[i].end(), v2[i].begin(), v2[i].end());
  }

  e.resize(4);
  BOOST_CHECK_THROW(sParamToVector(v1, e), out_of_range);
  BOOST_CHECK_THROW(sVectorToParam(e, v2), out_of_range);
}

BOOST_AUTO_TEST_CASE(MultiBodyConfigFunction2)
{
  using namespace std;
  using namespace Eigen;
  using namespace rbd;
  using namespace sva;

  MultiBodyGraph mbg;

  double mass = 1.;
  Matrix3d I = Matrix3d::Identity();
  Vector3d h = Vector3d::Zero();

  RBInertiad rbi(mass, h, I);

  Body b0(rbi, "b0");
  Body b1(rbi, "b1");
  Body b2(rbi, "b2");
  Body b3(rbi, "b3");

  mbg.addBody(b0);
  mbg.addBody(b1);
  mbg.addBody(b2);
  mbg.addBody(b3);

  Joint j0(Joint::Spherical, true, "j0");
  Joint j1(Joint::Spherical, true, "j1");
  Joint j2(Joint::RevX, true, "j2");

  mbg.addJoint(j0);
  mbg.addJoint(j1);
  mbg.addJoint(j2);

  mbg.linkBodies("b0", PTransformd::Identity(), "b1", PTransformd::Identity(), "j0");
  mbg.linkBodies("b1", PTransformd::Identity(), "b2", PTransformd::Identity(), "j1");
  mbg.linkBodies("b2", PTransformd::Identity(), "b3", PTransformd::Identity(), "j2");

  MultiBody mb = mbg.makeMultiBody("b0", true);

  MultiBodyConfig mbc(mb);

  // test paramToVector
  vector<vector<double>> confP = {{}, {1., 0., 0., 0.}, {1., 0., 0., 0.}, {2.}};
  vector<vector<double>> confD = {{}, {1., 2., 3.}, {3., 4., 5.}, {7.}};
  VectorXd eTestP(9);
  VectorXd eTestD(7);
  VectorXd e;

  paramToVector(confP, eTestP);
  paramToVector(confD, eTestD);

  BOOST_CHECK_NO_THROW(e = sParamToVector(mb, confP));
  BOOST_CHECK_THROW(sParamToVector(mb, confD), std::out_of_range);

  BOOST_CHECK_EQUAL(e, eTestP);

  // test dofToVector
  BOOST_CHECK_NO_THROW(e = sDofToVector(mb, confD));
  BOOST_CHECK_THROW(sDofToVector(mb, confP), std::out_of_range);

  BOOST_CHECK_EQUAL(e, eTestD);

  // test vectorToParam
  vector<vector<double>> conf;
  BOOST_CHECK_NO_THROW(conf = sVectorToParam(mb, eTestP));
  BOOST_CHECK_THROW(sVectorToParam(mb, eTestD), std::out_of_range);

  BOOST_CHECK_EQUAL(conf.size(), confP.size());
  for(std::size_t i = 0; i < conf.size(); ++i)
  {
    BOOST_CHECK_EQUAL_COLLECTIONS(conf[i].begin(), conf[i].end(), confP[i].begin(), confP[i].end());
  }

  // test vectorToDof
  BOOST_CHECK_NO_THROW(conf = sVectorToDof(mb, eTestD));
  BOOST_CHECK_THROW(sVectorToDof(mb, eTestP), std::out_of_range);

  BOOST_CHECK_EQUAL(conf.size(), confD.size());
  for(std::size_t i = 0; i < conf.size(); ++i)
  {
    BOOST_CHECK_EQUAL_COLLECTIONS(conf[i].begin(), conf[i].end(), confD[i].begin(), confD[i].end());
  }

  // test zero configuration
  mb = mbg.makeMultiBody("b0", false);

  mbc = MultiBodyConfig(mb);
  mbc.zero(mb);
  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    std::vector<double> zp = mb.joint(i).zeroParam();
    std::vector<double> zd = mb.joint(i).zeroDof();

    BOOST_CHECK_EQUAL_COLLECTIONS(mbc.q[i].begin(), mbc.q[i].end(), zp.begin(), zp.end());
    BOOST_CHECK_EQUAL_COLLECTIONS(mbc.alpha[i].begin(), mbc.alpha[i].end(), zd.begin(), zd.end());
    BOOST_CHECK_EQUAL_COLLECTIONS(mbc.alphaD[i].begin(), mbc.alphaD[i].end(), zd.begin(), zd.end());
    BOOST_CHECK_EQUAL_COLLECTIONS(mbc.jointTorque[i].begin(), mbc.jointTorque[i].end(), zd.begin(), zd.end());
  }

  for(int i = 0; i < mb.nrBodies(); ++i)
  {
    BOOST_CHECK_EQUAL(mbc.force[i].vector(), Vector6d::Zero());
  }
}

void checkConfig(const rbd::MultiBodyConfig & mbc1, const rbd::MultiBodyConfig & mbc2)
{
  for(std::size_t i = 0; i < mbc1.q.size(); ++i)
  {
    BOOST_CHECK_EQUAL_COLLECTIONS(mbc1.q[i].begin(), mbc1.q[i].end(), mbc2.q[i].begin(), mbc2.q[i].end());

    BOOST_CHECK_EQUAL_COLLECTIONS(mbc1.alpha[i].begin(), mbc1.alpha[i].end(), mbc2.alpha[i].begin(),
                                  mbc2.alpha[i].end());

    BOOST_CHECK_EQUAL_COLLECTIONS(mbc1.alphaD[i].begin(), mbc1.alphaD[i].end(), mbc2.alphaD[i].begin(),
                                  mbc2.alphaD[i].end());
  }

  BOOST_CHECK_EQUAL_COLLECTIONS(mbc1.force.begin(), mbc1.force.end(), mbc2.force.begin(), mbc2.force.end());
}

BOOST_AUTO_TEST_CASE(ConfigConverterTest)
{
  using namespace std;
  using namespace Eigen;
  using namespace rbd;
  using namespace sva;

  MultiBodyGraph mbg;

  double mass = 1.;
  Matrix3d I = Matrix3d::Identity();
  Vector3d h = Vector3d::Zero();

  RBInertiad rbi(mass, h, I);

  Body b0(rbi, "b0");
  Body b1(rbi, "b1");
  Body b2(rbi, "b2");
  Body b3(rbi, "b3");

  mbg.addBody(b0);
  mbg.addBody(b1);
  mbg.addBody(b2);
  mbg.addBody(b3);

  Joint j0(Joint::Spherical, true, "j0");
  Joint j1(Joint::RevY, true, "j1");
  Joint j2(Joint::RevX, true, "j2");

  mbg.addJoint(j0);
  mbg.addJoint(j1);
  mbg.addJoint(j2);

  mbg.linkBodies("b0", PTransformd::Identity(), "b1", PTransformd::Identity(), "j0");
  mbg.linkBodies("b1", PTransformd::Identity(), "b2", PTransformd::Identity(), "j1");
  mbg.linkBodies("b2", PTransformd::Identity(), "b3", PTransformd::Identity(), "j2");

  MultiBody mb1 = mbg.makeMultiBody("b0", true);
  MultiBody mb2 = mbg.makeMultiBody("b3", true);
  MultiBody mb3 = mbg.makeMultiBody("b1", true);

  ConfigConverter * mb1tomb2 = ConfigConverter::sConstructor(mb1, mb2);
  ConfigConverter * mb1tomb3 = ConfigConverter::sConstructor(mb1, mb3);

  ConfigConverter * mb2tomb1 = ConfigConverter::sConstructor(mb2, mb1);
  ConfigConverter * mb3tomb1 = ConfigConverter::sConstructor(mb3, mb1);

  MultiBodyConfig mbc1(mb1);
  MultiBodyConfig mbc1Tmp(mb1);
  MultiBodyConfig mbc2(mb2);
  MultiBodyConfig mbc3(mb3);

  mbc1.q = {{}, {1., 0., 0., 0.}, {1.}, {2.}};
  mbc1.alpha = {{}, {1., 0., 0., 0.}, {1.2}, {2.2}};
  mbc1.alphaD = {{}, {0., 3., 2.}, {1.4}, {2.3}};
  mbc1.force = {ForceVecd(Vector6d::Random()), ForceVecd(Vector6d::Random()), ForceVecd(Vector6d::Random()),
                ForceVecd(Vector6d::Random())};

  mb1tomb2->sConvert(mbc1, mbc2);
  mb2tomb1->sConvert(mbc2, mbc1Tmp);
  checkConfig(mbc1, mbc1Tmp);

  mbc1.q = {{}, {1., 0., 0., 0.}, {5.}, {6.}};
  mb1tomb3->sConvert(mbc1, mbc3);
  mb3tomb1->sConvert(mbc3, mbc1Tmp);
  checkConfig(mbc1, mbc1Tmp);

  // sConvertJoint
  mbc1.q = {{}, {1., 0., 0., 0.}, {5.}, {3.}};
  mb1tomb2->sConvertJoint(mbc1.q, mbc2.q);
  mb2tomb1->sConvertJoint(mbc2.q, mbc1Tmp.q);
  checkConfig(mbc1, mbc1Tmp);

  mbc1.q = {{}, {1., 0., 0., 0.}, {2.}, {3.}};
  mb1tomb3->sConvertJoint(mbc1.q, mbc3.q);
  mb3tomb1->sConvertJoint(mbc3.q, mbc1Tmp.q);
  checkConfig(mbc1, mbc1Tmp);

  // sConvertJoint return version
  mbc1.q = {{}, {1., 0., 0., 0.}, {5.}, {3.}};
  mbc2.q = mb1tomb2->convertJoint(mbc1.q);
  mbc1Tmp.q = mb2tomb1->convertJoint(mbc2.q);
  checkConfig(mbc1, mbc1Tmp);

  mbc1.q = {{}, {1., 0., 0., 0.}, {2.}, {3.}};
  mbc3.q = mb1tomb3->convertJoint(mbc1.q);
  mbc1Tmp.q = mb3tomb1->convertJoint(mbc3.q);
  checkConfig(mbc1, mbc1Tmp);

  delete mb1tomb2;
  delete mb1tomb3;
  delete mb2tomb1;
  delete mb3tomb1;
}

BOOST_AUTO_TEST_CASE(MultiBodyBaseTransformTest)
{
  using namespace Eigen;

  // we construct 3 multibody from body id 0, 3 and 4
  // we try to find the base of each body with all the multibody
  rbd::MultiBody mb0, mb3, mb4;
  rbd::MultiBodyConfig mbc0, mbc3, mbc4;
  rbd::MultiBodyGraph mbg;
  sva::PTransformd mb3Surface(Quaterniond(Vector4d::Random()).normalized(), Vector3d::Random());
  sva::PTransformd mb4Surface(Quaterniond(Vector4d::Random()).normalized(), Vector3d::Random());
  std::tie(mb0, mbc0, mbg) = makeXYZSarm();
  // the multibody graph is construct with the old body linking api
  // so even the multibody from body 0 must have transform to each bodies base
  auto mb0ToBase = mbg.bodiesBaseTransform("b0");

  VectorXd q(VectorXd::Random(mb0.nrParams()));
  // we must normalize the configuration of the spherical joint
  q.segment(mb0.jointPosInParam(mb0.jointIndexByName("j3")), 4).normalize();
  rbd::vectorToParam(q, mbc0.q);
  rbd::forwardKinematics(mb0, mbc0);

  // since the multibody is construct with the old body linking api
  // we must add an offset to joint origin of mb3 that correspond mb0 transform
  // to body base
  mb3 = mbg.makeMultiBody("b3", true, mb3Surface * mb0ToBase["b3"] * mbc0.bodyPosW[mb0.bodyIndexByName("b3")],
                          mb3Surface);
  rbd::ConfigConverter mb0ToMb3(mb0, mb3);
  mbc3 = rbd::MultiBodyConfig(mb3);
  mb0ToMb3.convert(mbc0, mbc3);
  rbd::forwardKinematics(mb3, mbc3);
  auto mb3ToBase = mbg.bodiesBaseTransform("b3", mb3Surface);

  mb4 = mbg.makeMultiBody("b4", true, mb4Surface * mb0ToBase["b4"] * mbc0.bodyPosW[mb0.bodyIndexByName("b4")],
                          mb4Surface);
  mbc4 = rbd::MultiBodyConfig(mb4);
  rbd::ConfigConverter mb0ToMb4(mb0, mb4);
  mb0ToMb4.convert(mbc0, mbc4);
  rbd::forwardKinematics(mb4, mbc4);
  auto mb4ToBase = mbg.bodiesBaseTransform("b4", mb4Surface);

  for(int bi = 0; bi < mb0.nrBodies(); ++bi)
  {
    std::string name = mb0.body(bi).name();
    int mb3Index = mb3.bodyIndexByName(name);
    int mb4Index = mb4.bodyIndexByName(name);
    BOOST_CHECK_SMALL(
        ((mb0ToBase[name] * mbc0.bodyPosW[bi]).matrix() - (mb3ToBase[name] * mbc3.bodyPosW[mb3Index]).matrix()).norm(),
        1e-8);
    BOOST_CHECK_SMALL(
        ((mb0ToBase[name] * mbc0.bodyPosW[bi]).matrix() - (mb4ToBase[name] * mbc4.bodyPosW[mb4Index]).matrix()).norm(),
        1e-8);
  }
}


/*------------------------------------------------------------------------------
Copyright © 2015 by Nicola Bombieri

H-BF is provided under the terms of The MIT License (MIT):

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
------------------------------------------------------------------------------*/
/**
 * @author Federico Busato
 * Univerity of Verona, Dept. of Computer Science
 * federico.busato@univr.it
 */
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include "XLib.hpp"
#include "Host/GraphSSSP.hpp"

using namespace timer;

void GraphSSSP::BoostDijkstra(const int source) {
	using namespace boost;

	typedef adjacency_list < listS, vecS, directedS, no_property, property<edge_weight_t, node_t> > graph_t;
	typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
	typedef std::pair<node_t, node_t> Edge;

	Edge* edge_array = new Edge[E];
	int k = 0;
	for (int i = 0; i < V; i++) {
		for (int j = OutNodes[i]; j < OutNodes[i + 1]; j++)
			edge_array[k++] = Edge(i, OutEdges[j]);
	}

	graph_t g(edge_array, &edge_array[E], Weights, V);

	std::vector<vertex_descriptor> p(num_vertices(g));
	std::vector<int> d(num_vertices(g));
	vertex_descriptor s = vertex(source, g);

	Timer<HOST> TM;
    TM.start();

	dijkstra_shortest_paths(g, s,
            predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g))).
            distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g))));

	TM.getTime("Boost Dijkstra");

    BellmanFord_Queue_init();
    BellmanFord_Queue(source);

	graph_traits < graph_t >::vertex_iterator vi, vend;
	int i = 0;
	for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) {
		if (d[*vi] != Distance[i++])
			__ERROR("distance error");
	}
    BellmanFord_Queue_end();
}


//
//  GPUTSDFVolume.hpp
//  A GPU Based TSDF
//
//  Created by Dave on 11/03/2016.
//  Copyright © 2016 Sindesso. All rights reserved.
//

#ifndef TSDFVolume_hpp
#define TSDFVolume_hpp

#include "Camera.hpp"

#include <Eigen/Core>
#include "vector_types.h"

#include <cstdint>
#include <iostream>
#include <string>

class TSDFVolume {
public:
    struct DeformationNode {
        float3  translation;
        float3  rotation;
    };

    struct Float3 {
        float x;
        float y;
        float z;
        inline Float3( const float3& rhs ) {
            x = rhs.x;
            y = rhs.y;
            z = rhs.z;
        }
        inline Float3( float fx=0.0f, float fy=0.0f, float fz=0.0f ) {
            x = fx;
            y = fy;
            z = fz;
        }
        inline operator float3() const {return float3{x, y, z}; }

        inline Float3 operator -( const Float3& rhs ) const {
            return float3{x - rhs.x, y - rhs.y, z - rhs.z};
        }
        inline Float3 operator +( const Float3& rhs ) const {
            return float3{x + rhs.x, y + rhs.y, z + rhs.z};
        }
        inline Float3 operator /( const float scalar ) const {
            return float3{x / scalar, y / scalar, z / scalar};
        }
        inline Float3 operator *( const Float3& rhs ) const {
            return float3{x * rhs.x, y * rhs.y, z * rhs.z};
        }
        inline float norm( ) const {
            return std::sqrt( x*x + y*y + z*z );
        }

    };

    struct Int3 {
        int16_t x;
        int16_t y;
        int16_t z;
    };

    struct  UInt3 {
        unsigned int x;
        unsigned int y;
        unsigned int z;

        inline UInt3( const dim3& rhs ) : x{rhs.x}, y{rhs.y}, z{rhs.z} {};
        inline UInt3( uint32_t x, uint32_t y, uint32_t z ) : x{x}, y{y}, z{z} {};
        inline operator dim3() const {return dim3{x, y, z}; }
    };

    ~TSDFVolume();

    /**
    * Make a TSDFVolume with the given dimensions (voxels) and physcial dimensions
    * @param size The number of voxels in each X,Y and Z dimension
    * @param physical_size The size ( in mm ) of the space contained in the volume
    */
    TSDFVolume( const UInt3& size = UInt3{64, 64, 64},
                const Float3& physical_size = Float3 { 3000.0f, 3000.0f, 3000.0f} );

    /**
     * Make a TSDFVolume with the given dimensins and physical dimensions
     * @param volume_x X dimension in voxels
     * @param volume_y Y dimension in voxels
     * @param volume_z Z dimension in voxels
     * @param psize_x Physical size in X dimension in mm
     * @param psize_y Physical size in Y dimension in mm
     * @param psize_z Physical size in Z dimension in mm
     */
    TSDFVolume( uint16_t volume_x, uint16_t volume_y, uint16_t volume_z, float psize_x, float psize_y, float psize_z );

    /**
     * Load a TSDFVolume from the specified file. The volume must previously have been saved
     */
    TSDFVolume( const std::string& file_name );

    /**
     * Set the size of the volume. This will delete any existing values and resize the volume, clearing it when done.
     * Volume offset is maintained
     * @param volume_x X dimension in voxels
     * @param volume_y Y dimension in voxels
     * @param volume_z Z dimension in voxels
     * @param psize_x Physical size in X dimension in mm
     * @param psize_y Physical size in Y dimension in mm
     * @param psize_z Physical size in Z dimension in mm
     */
    void set_size( uint16_t volume_x, uint16_t volume_y, uint16_t volume_z, float psize_x, float psize_y, float psize_z);
    void set_truncation_distance(float d);
    /**
     * @return the size of this space in voxels.
     */
    inline UInt3 size( ) const { return (UInt3) m_size; }

    /**
     * @return the dimensions of each voxel in mm
     */
    inline Float3 voxel_size( ) const { return (Float3) m_voxel_size; }

    /**
     * @return the physical size of the volume in world coords (mm)
     */
    inline Float3 physical_size( ) const { return (Float3) m_physical_size; }

    /**
     * @return the truncation distance (mm)
     */
    inline float truncation_distance( ) const { return m_truncation_distance; }

    /**
     * Offset the TSDF volume in space by the given offset. By default, the bottom, left, front corner of
     * voxel (0,0,0) is at world coordinate (0,0,0). This moves that point to the new world coordinate by a
     * @param ox X offset in mm
     * @param oy Y offset in mm
     * @param oz Z offset in mm
     */
    inline void offset( float ox, float oy, float oz ) {
        m_offset.x = ox;
        m_offset.y = oy;
        m_offset.z = oz;
    }

    /**
     * @return the offset f the TSDF volume in space
     */
    inline Float3 offset( ) const { return (Float3) m_offset; }

    /**
     * Clear the voxel and weight data
     */
    void clear( );

#pragma mark - Data access

 /**
     *
     */
    inline size_t index( int x, int y, int z ) const {
        return x + (y * m_size.x) + (z * m_size.x * m_size.y);
    };

    /**
     * @return pointer to translation data
     */
    inline DeformationNode *  deformation() const {
        return m_deformation_nodes;
    }

    /**
     * Set the deformation data for this space
     * @param data Data in host memory space; Assumed to be vx*vy*vz DeformationNode
     */
    void set_deformation( DeformationNode *deformation);

    /**
     * Return pointer to distance data
     * @return Pointer to distance data
     */
    inline const float * distance_data() const {
        return m_distances;
    }

    /**
     * Set the distance data for the TSDF in one call
     * @param distance_data Pointer to enough floats to populate the TSFD
     */
    void set_distance_data( const float * distance_data );

    /**
     * Return pointer to weight data
     * @return Pointer to weight data
     */
    inline const float * weight_data() const {
        return m_weights;
    }

    /**
     * Set the weight data for the TSDF in one call
     * @param weight_data Pointer to enough floats to populate the TSFD
     */
    void set_weight_data( const float * weight_data );

    /**
     * @return the global rotation of the TSDF deformation
     * as a vector of 3 Euler angles (X then Y then Z - also Tait-Bryan angles)
     */
    inline float3 global_rotation( ) const {
        return m_global_rotation;
    }

    /**
     * @return the global translation of the TSDF deformation
     */
    inline float3 global_translation( ) const {
        return m_global_translation;
    }

#pragma mark - Deform a set of points
    /**
     * Apply the volume's deformation field t the given set of points, modifying them in place
     */
    void deform_mesh( const int num_points, float3 * points ) const;

#pragma mark - Integrate new depth data
    /**
     * Integrate a range map into the TSDF
     * @param depth_map Pointer to width*height depth values where 0 is an invalid depth and positive values are expressed in mm
     * @param width The horiontal dimension of the depth_map
     * @param height The height of the depth_map
     * @param camera The camera from which the depth_map was taken
     */
    void integrate( const uint16_t * depth_map, uint32_t width, uint32_t height, const Camera & camera );

#pragma mark - Import/Export

    /**
     * Save the TSDF to file
     * @param The filename
     * @return true if the file saved OK otherwise false.
     */
    bool save_to_file( const std::string & file_name) const;

    /**
     * Load the given TSDF file
     * @param The filename
     * @return true if the file saved OK otherwise false.
     */
    bool load_from_file( const std::string & file_name);

#pragma mark - Rendering
    void raycast( uint16_t width, uint16_t height, const Camera& camera, Eigen::Matrix<float, 3, Eigen::Dynamic>& vertices, Eigen::Matrix<float, 3, Eigen::Dynamic>& normals ) const ;

private:
    /**
     * Deallocate storage for this TSDF
     */
    void deallocate( );

    // Size of voxel grid
    dim3                    m_size;

    // Physical size of space represented in mm
    float3                  m_physical_size;

    // Offset of physical grid in world coordinates
    float3                  m_offset;

    // Size of a voxel
    float3 m_voxel_size;

    //  Truncation distance
    float m_truncation_distance;

    // Max weight for a voxel
    float m_max_weight;

    // Per grid point data
    float                   *m_distances;

    // Colour data, RGB as 3xuchar
    uchar3                  *m_colours;

    //  Confidence weight for distance and colour
    float                   *m_weights;

    // Deformation field
    DeformationNode         *m_deformation_nodes;

    // Global translation
    float3                  m_global_translation;

    // Global rotation
    float3                  m_global_rotation;
};
#endif /* TSDFVolume_hpp */


/**
   @file algo.hpp
   @author Kevin Hu
   @date 2009.11.24
 */
#ifndef ALGO_HPP
#define ALGO_HPP

//#ifdef __cplusplus
//extern "C"
//{
//#endif
#include "hlmalloc.h"
//#ifdef __cplusplus
//}
//#endif

#include "types.h"
#include <string>
#include <vector>
#include <ostream>
//#include "vector_string.hpp"
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <fstream>
#include <sys/types.h>
#include <math.h>
//#include <iconv.h>

enum StrCompMode { SM_SENSITIVE, SM_IGNORE };

/**
   @class is_alphabet
   @brief it's used to judge if it's english charactor
 */
template<
class CHAR_T = char
>
class is_alphabet
{
public:
    static bool value(CHAR_T c)
    {
        return (c>='a' && c<='z') ||(c>='A' && c<= 'Z');
    }

    static bool is_upper(CHAR_T c)
    {
        return c>='A' && c<= 'Z';
    }

    static bool is_lower(CHAR_T c)
    {
        return c>='a' && c<='z';
    }

    static CHAR_T to_lower(CHAR_T c)
    {
        if (is_upper(c))
            return c - ('A'-'a');

        return c;
    }

    static CHAR_T to_upper(CHAR_T c)
    {
        if (is_lower(c))
            return c + ('A'-'a');
        return c;
    }

    static bool equal_ignore_case(CHAR_T c1, CHAR_T c2)
    {
        // normalize to lowercase first
        if(c1 >= 'A' && c1 <= 'Z')
            c1 += 'a' - 'A';

        if(c2 >= 'A' && c2 <= 'Z')
            c2 += 'a' - 'A';

        return c1 == c2;
    }

    static bool is_space(CHAR_T c)
    {
        return c == ' ';
    }

    static bool is_new_line(CHAR_T c)
    {
        return (c == '\n');
    }

}
;

/**
   @class is_numeric
   @brief it's used for numeric operation
 */
template<
class CHAR_T = char
>
class is_numeric
{
public:
    static bool value(CHAR_T c)
    {
        return c>='0' && c<='9';
    }

    static bool is_positive(CHAR_T c)
    {
        return c=='+';
    }

    static bool is_negative(CHAR_T c)
    {
        return c=='-';
    }

    static CHAR_T  numeric_value(CHAR_T c)
    {
        if (!value(c))
            return 0;

        return c- '0';
    }

    static bool is_dot(CHAR_T c)
    {
        return c == '.';
    }

    static CHAR_T dot()
    {
        return '.';
    }

}
;

NS_IZENELIB_UTIL_BEGIN

/**
 *@class Algorithm
 * It is an algorithm set about string. Temlate parameter StringT indicates the string type.
 *IS_NUMERIC indicates a class about deciding if it is a numeric. IS_ALPHABET is a class to
 *judge about charactors in alphabet.
 *@brief An string algorithm set.
 **/
template <
    class StringT = std::string,
    class IS_NUMERIC = is_numeric<typename StringT::value_type>,
    class IS_ALPHABET = is_alphabet<typename StringT::value_type>
    >
class Algorithm
{
    typedef typename StringT::value_type CharT;
    typedef typename StringT::size_t size_t;

public:
    //*******************KMP**********************
    static void get_nextval(const StringT& s, uint64_t* next, StrCompMode caseChk=SM_SENSITIVE)
    {
        uint64_t j = 0, k = -1;

        next[0] = -1;

        while ( j < s.length()-1 )
        {
            if (k == (uint64_t)-1
                    || (caseChk==SM_SENSITIVE && s[j]== s[k])
                    || (caseChk==SM_IGNORE && IS_ALPHABET::equal_ignore_case(s[j], s[k])) )
            {
                if (k != (uint64_t)-1 )
                    IASSERT(k<s.length());

                ++j;
                ++k;

                IASSERT(k<s.length());
                if ((caseChk==SM_SENSITIVE && s[j]!= s[k])
                        || (caseChk==SM_IGNORE && !IS_ALPHABET::equal_ignore_case(s[j], s[k])) )
                    next[j] = k;
                else
                {
                    IASSERT(k<=s.length());
                    next[j] = next[k];
                }
            }// if
            else
            {
                IASSERT(k<=s.length());
                k = next[k];
            }
        }// while
    }// get_nextval

    /*!
     *KMP, an string maching algorithm. caseChk could be SM_SENSITIVE or SM_IGNORE.
     *Return the index of the first instance of pattern started from 0.
     **/
    static uint64_t KMP(const StringT& text,const StringT& pattern, StrCompMode caseChk=SM_SENSITIVE) //const 表示函数内部不会改变这个参数的值。
    {

        if( text.length()==0 || pattern.length()==0 )//
            return -1;//空指针或空串，返回-1。

        uint64_t len= pattern.length();

        uint64_t *next= (uint64_t*)HLmemory::hlmalloc((len+1)*sizeof (uint64_t));

        get_nextval(pattern,next, caseChk);//求Pattern的next函数值

        uint64_t index=0,i=0,j=0;

        while(i < text.length() && j < pattern.length())
        {
            if((caseChk==SM_SENSITIVE && text[i]== pattern[j])
                    || (caseChk==SM_IGNORE && IS_ALPHABET::equal_ignore_case(text[i], pattern[j])) )
            {
                ++i;// 继续比较后继字符
                ++j;

            }
            else
            {
                index += j-next[j];

                if(next[j]!=(uint64_t)-1)

                    j=next[j];// 模式串向右移动
                else
                {
                    j=0;
                    ++i;
                }
            }
        }//while

        HLmemory::hlfree(next);

        if(j == pattern.length())
        {
            IASSERT(index<text.length());
            return index;// 匹配成功
        }
        else
            return -1;

    }

    //*******************rKMP**********************
    static void rget_nextval(const StringT& s, uint64_t next[], StrCompMode caseChk=SM_SENSITIVE)
    {
        uint64_t j = s.length()-1, k = s.length();

        next[s.length()] = s.length();

        while ( j >0 )
        {
            if (k == s.length()
                    || caseChk==SM_SENSITIVE && s[j]== s[k]
                    || caseChk==SM_IGNORE && IS_ALPHABET::equal_ignore_case(s[j], s[k]) )
            {
                --j;
                --k;

                if (caseChk==SM_SENSITIVE && s[j]!= s[k]
                        ||caseChk==SM_IGNORE && !IS_ALPHABET::equal_ignore_case(s[j], s[k]) )
                    next[j+1] = k;
                else
                    next[j+1] = next[k+1];

            }// if
            else
                k = next[k+1];

        }// while

    }// get_nextval

    /**
     *KMP, an string maching algorithm. caseChk could be SM_SENSITIVE or SM_IGNORE.
     * It searches from back.
     *Return the index of the first instance of pattern started from 0.
     **/
    static uint64_t rKMP(const StringT& text,const StringT& pattern, StrCompMode caseChk=SM_SENSITIVE)
    {

        if( text.length()==0 || pattern.length()==0 )//
            return -1;//

        uint64_t len= pattern.length();

        uint64_t *next= (uint64_t*)HLmemory::hlmalloc((len+1)*sizeof (uint64_t));

        rget_nextval(pattern,next, caseChk);//

        uint64_t index=text.length()-1,i=text.length()-1,j=pattern.length()-1;

        while(i !=(uint64_t)-1  && j !=(uint64_t)-1 )
        {
            if(caseChk==SM_SENSITIVE && text[i]== pattern[j]
                    || caseChk==SM_IGNORE && IS_ALPHABET::equal_ignore_case(text[i], pattern[j]) )
            {
                --i;
                --j;
            }
            else
            {
                index -= next[j+1] - j;

                if(next[j+1]!= pattern.length())
                    j=next[j+1];
                else
                {
                    j=pattern.length()-1;
                    --i;
                }
            }

        }//while

        HLmemory::hlfree(next);

        if(j == (uint64_t)-1)
            return index;// 匹配成功
        else
            return -1;
    }

    //*******************Data type**********************

    static int to_integer(const StringT& str)
    {
        int r = 0;
        int k=1;

        for (typename StringT::const_reverse_iterator i= str.rbegin(); i!=str.rend(); i++, k *= 10)
        {
            if (!IS_NUMERIC::value(*i))
            {
                assert(IS_NUMERIC::is_positive( *i) || IS_NUMERIC::is_negative( *i));

                if (IS_NUMERIC::is_negative( *i))
                    return -1*r;
                return r;
            }

            r += IS_NUMERIC::numeric_value(*i)*k;
        }

        return r;
    }

    static long to_long(const StringT& str)
    {
        long r=0;
        long k =1;

        for (typename StringT::const_reverse_iterator i= str.rbegin(); i!=str.rend(); i++, k *= 10)
        {
            if (!IS_NUMERIC::value(*i))
            {
                assert(IS_NUMERIC::is_positive( *i) || IS_NUMERIC::is_negative( *i));
                if (IS_NUMERIC::is_negative( *i))
                    return -1*r;
                return r;
            }

            r += IS_NUMERIC::numeric_value(*i)*k;
        }

        return r;
    }

    static unsigned long to_ulong(const StringT& str)
    {
        unsigned  long r=0;
        unsigned long k =1;

        for (typename StringT::const_reverse_iterator i= str.rbegin(); i!=str.rend(); i++, k *= 10)
        {
            r += IS_NUMERIC::numeric_value(*i)*k;
        }

        return r;
    }

    static float to_float(const StringT& str)
    {
        float r = 0;
        float k = 1;

        size_t dot = str.find(IS_NUMERIC::dot());
        if (dot == StringT::npos)
            dot = str.length();

        size_t i=dot-1;
        for (; i!=(size_t)-1 && IS_NUMERIC::value(str[i]); i--, k *=10)
            r += IS_NUMERIC::numeric_value(str[i])*k;

        float o = 1;
        if (i!=(size_t)-1 && IS_NUMERIC::is_negative(str[i]))
            o = -1;

        k = 10.;
        for (i = dot+1; i<str.length() && str[i]>='0'&& str[i]<='9'; i++, k *=10)
            r += IS_NUMERIC::numeric_value(str[i])/k;

        r *= o;

        return r;
    }

    static void to_lower(StringT& str)
    {
        for (typename StringT::iterator i=str.begin(); i!=str.end(); i++)
        {
            *i = IS_ALPHABET::to_lower(*i);
        }
    }

    /**
     * @brief Change all the lower case to upper caes in the _text.
     *
      Change all the lower case to upper caes in the _text.
      It forces const char* to char*. Do more research on std::string
     *
     */
    static void to_upper(StringT& str)
    {
        for (typename StringT::iterator i=str.begin(); i!=str.end(); i++)
        {
            *i = IS_ALPHABET::to_upper(*i);
        }
    }

    /**
     * @brief Change all the upper case to lower case in the _text.
     *
      Change all the upper case to lower case in the _text.
      It forces const char* to char*. Do more research on std::string
     *
     */
    static StringT to_lower(const StringT& str)
    {
        StringT s = str;
        for (typename StringT::iterator i=s.begin(); i!=s.end(); i++)
        {
            *i = IS_ALPHABET::to_lower(*i);
        }
        return s;
    }

    static StringT to_upper(const StringT& str)
    {
        StringT s = str;
        for (typename StringT::iterator i=s.begin(); i!=s.end(); i++)
        {
            *i = IS_ALPHABET::to_upper(*i);
        }
        return s;
    }

    static bool equal_ignore_case (const StringT& str1, const StringT& str2)
    {
        if (str1.length()!=str2.length())
            return false;

        typename StringT::const_iterator j=str2.begin();

        for (typename StringT::const_iterator i=str1.begin(); i!=str1.end()&&j!=str2.end(); i++, j++)
        {
            if (!IS_ALPHABET::equal_ignore_case(*i, *j))
                return false;
        }

        return true;
    }

    /**
     *@brief Compare two string after cutting head and tail spaces out.
     **/
    static bool equal_ignore_case_space(const StringT& str1, const StringT& str2)
    {
        typename StringT::const_iterator j=str2.begin();
        typename StringT::const_iterator i=str1.begin();

        for (; i!=str1.end()&&j!=str2.end(); i++,j++)
        {
            while (i!=str1.end() && IS_ALPHABET::is_space(*i))
                i++;

            while (j!=str2.end() && IS_ALPHABET::is_space(*j))
                j++;

            if (i==str1.end() || j==str2.end())\
                break;

            if (!IS_ALPHABET::equal_ignore_case(*i, *j))
                return false;
        }

        while (i!=str1.end() && IS_ALPHABET::is_space(*i))
            i++;

        if (i!= str1.end())
            return false;

        while (j!=str2.end() && IS_ALPHABET::is_space(*j))
            j++;
        if (j!= str2.end())
            return false;

        return true;
    }

    static size_t find(const StringT& str1, const StringT& str2, size_t pos1 = 0, StrCompMode caseChk=SM_IGNORE)
    {
        if (str2.length()>str1.length()-pos1)
            return -1;

        if (SM_SENSITIVE == caseChk)
            return str1.find(str2, pos1);
        else
        {
            size_t i = KMP(str1.substr(pos1), str2, caseChk);
            if (i == (size_t)-1)
                return -1;
            return pos1 + i;
        }
    }

    static size_t rfind(const StringT& str1, const StringT& str2, size_t pos1 = StringT::npos, StrCompMode caseChk=SM_IGNORE)
    {
        if (pos1 == StringT::npos &&  str2.length()>str1.length())
            return -1;
        if (pos1 != StringT::npos && str2.length()>pos1+1)
            return -1;

        if (SM_SENSITIVE == caseChk)
            return str1.rfind(str2, pos1);
        else
        {
            return rKMP(str1.substr(0, (pos1 == StringT::npos? pos1: pos1+1)), str2, caseChk);
        }
    }

    /**
        @brief : Returns the number of keywords found.
                      returnIndices contains the offsets for the keywords.
              If there is no such keyword, it sets the offset to -1.
     *
     */
    static size_t multi_find(const StringT& str1, const StringT& str2, std::vector<size_t>& indices,
                             size_t pos1 = 0, StrCompMode caseChk=SM_IGNORE)
    {
        indices.clear();

        size_t i = pos1;

        while (i!= (size_t)-1)
        {
            i = find(str1, str2, i, caseChk);
            if (i!= (size_t)-1)
            {
                indices.push_back(i);
                i += str2.length();
            }

        }

        return indices.size();
    }

    /**
     * @brief Returns true if the string starts with the given key in the
                        position pos.
     */
    static bool start_with(const StringT& str1, const StringT& str2, size_t pos1 = 0, StrCompMode caseChk=SM_IGNORE)
    {
        if (str2.length()>str1.length()-pos1)
            return false;

        size_t i = pos1;
        size_t j = 0;

        if (SM_SENSITIVE == caseChk)
        {
            for (; j<str2.length(); i++, j++)
            {
                if (str1[i] != str2[j])
                    return false;
            }
        }
        else
        {
            for (; j<str2.length(); i++, j++)
            {
                if (!IS_ALPHABET::equal_ignore_case(str1[i], str2[j]))
                    return false;
            }
        }

        return true;
    }

    static bool end_with(const StringT& str1, const StringT& str2, size_t pos1 = StringT::npos, StrCompMode caseChk=SM_IGNORE)
    {
        if (pos1 == StringT::npos)
            pos1 = str1.length()-1;

        if (str2.length()>pos1+1)
            return false;

        size_t i = pos1;
        size_t j = str2.length()-1;

        if (SM_SENSITIVE == caseChk)
        {
            for (; j!=(size_t)-1; i--, j--)
            {
                if (str1[i] != str2[j])
                    return false;
            }
        }
        else
        {
            for (; j !=(size_t) -1; i--, j--)
            {
                if (!IS_ALPHABET::equal_ignore_case(str1[i], str2[j]))
                    return false;
            }
        }

        return true;
    }

    /**
    *
       @brief Returns the number of occurrences for a given pattern in the
                      string.
    *
    */
    static size_t num_occurrence(const StringT& str1, const StringT& str2,
                                 size_t pos1 = 0, StrCompMode caseChk=SM_IGNORE)
    {
        size_t r =0;
        size_t i = pos1;

        while (i!= (size_t)-1)
        {
            i = find(str1, str2, i, caseChk);
            if (i!= (size_t)-1)
            {
                i += str2.length();
                r ++;
            }

        }

        return r;
    }

    /**
     * @brief  Returns the counter array (reference argument) each array
                   element representing the number of occurrences of the
    	 keyword in the patterns array.
     */
    static void frequency_counter(const StringT& str, const std::vector<StringT>& patterns, std::vector<size_t>& freq,
                                  StrCompMode caseChk=SM_IGNORE)
    {
        freq.clear();
        freq.reserve(patterns.size());

        for (typename std::vector<StringT>::const_iterator i=patterns.begin(); i!= patterns.end(); i++)
        {
            freq.push_back(num_occurrence(str, *i, 0, caseChk));
        }
    }

    /**
    * @brief Substitute to character for from.
    * @return the times of ocurrence
    */
    static size_t  substitute_char(CharT from, CharT to, StringT& str, size_t pos = 0, StrCompMode caseChk=SM_IGNORE)
    {
        size_t r = 0;
        for (size_t i=pos; i<str.length(); i++)
            if (caseChk==SM_IGNORE && IS_ALPHABET::equal_ignore_case(str[i], from)
                    ||caseChk==SM_SENSITIVE && str[i]==from)
            {
                r++;
                str[i] = to;
            }
        return r;
    }

    /**
       @return substituted string.
     */
    static StringT  substitute_char(CharT from, CharT to, const StringT& str, size_t pos = 0, StrCompMode caseChk=SM_IGNORE)
    {
        StringT s = str;
        size_t r = 0;
        for (size_t i=pos; i<str.length(); i++)
            if (caseChk==SM_IGNORE && IS_ALPHABET::equal_ignore_case(str[i], from)
                    ||caseChk==SM_SENSITIVE && str[i]==from)
            {
                r++;
                s[i] = to;
            }
        return r;
    }

    /**
       * @brief Substitute the to string for from.
       *
         Substitute the to string for from. Beware that it will
                      substitute as many times as necessary.
    		Returns the resulting string.

       *
       */
    static size_t  multi_substitute_string(const StringT& from, const StringT& to,
                                           StringT& str, size_t pos = 0,
                                           StrCompMode caseChk=SM_IGNORE)
    {
        std::vector<size_t> v;
        multi_find(str, from, v, pos, caseChk);

        StringT s;

        size_t last = 0;
        for (typename std::vector<size_t>::iterator i=v.begin(); i!=v.end(); i++)
        {
            assert(last<str.length());
            //std::cout<<str.substr(last, *i-last)<<last<<" "<<*i-last<<"\n";
            StringT ssss = str.substr(last, *i-last);
            //std::cout<<"jjjjjjjjjjjjj\n";
            s += ssss;
            s += to;
            last = *i+from.length();
        }

        s += str.substr(last);
        str = s;

        return v.size();
    }

    static StringT  multi_substitute_string(const StringT& from, const StringT& to,
                                            const StringT& str, size_t pos = 0,
                                            StrCompMode caseChk=SM_IGNORE)
    {
        std::vector<size_t> v;
        multi_find(str, from, v, pos, caseChk);
        StringT s;

        size_t last = 0;
        for (typename std::vector<size_t>::iterator i=v.begin(); i!=v.end(); i++)
        {
            assert(last<str.length());
            s += str.substr(last, *i-last);
            s += to;
            last = *i+from.length();
        }

        s += str.substr(last);
        return s;
    }

    /**
    * @brief Substitute the to string for from.
    *
    Substitute the to string for from. Beware that it will
                   substitute only one time!
    	Returns the resulting string. Aborts if there is no
    	  'from' string.

    *
    */
    static StringT  substitute_string(const StringT& from, const StringT& to,
                                      const StringT& str, size_t pos = 0,
                                      StrCompMode caseChk=SM_IGNORE)
    {
        StringT s = str;
        substitute_string(from, to, s, pos, caseChk);
        return s;

    }

    static void substitute_string(const StringT& from, const StringT& to,
                                  StringT& str, size_t pos = 0,
                                  StrCompMode caseChk=SM_IGNORE)
    {
        size_t i = find(str, from, pos, caseChk);
        StringT s;
        if (i == (size_t)-1)
            return;

        s += str.substr(0, i);
        s += to;
        i += from.length();
        if (i<str.length())
            s += str.substr(i);

        str = s;
    }

    static size_t remove_newlines(StringT& str)
    {
        size_t r = 0;
        for (size_t i=0; i<str.length(); i++)
            if (IS_ALPHABET::is_new_line(str[i]))
            {
                r++;
                str[i] = ' ';
            }

        return r;
    }

//   static StringT remove_newlines(const StringT& str)
//   {
//     StringT r = str;
//     remove_newlines(r);
//     return r;
//   }

    /**
     * @brief Replace all the occurrences of the given characters with
                     spaces.
     *
      Replace all the occurrences of the given characters with
                     spaces.
     *
     */
    static void remove_duplicate_chars(StringT& str, CharT c)
    {
        StringT s;
        s.reserve(str.length());

        typename StringT::iterator i;
        int count = 0;

        for (i = str.begin(); i < str.end(); i++)
        {
            if (*i == c)
            {
                if (count > 0);
                else s += c;
                count++;
            }
            else
            {
                s += *i;
                count = 0;
            }
        }

        //std::cout<<s.getReferCnt()<<std::endl;
        str = s;
        //std::cout<<s.getReferCnt()<<std::endl;
    }

//   static StringT remove_duplicate_chars(const StringT& str, CharT c)
//   {
//     StringT s = str;
//     remove_duplicate_chars(s, c);
//     return s;

//   }

    static void remove_multi_whitespace(StringT& str)
    {
        remove_duplicate_chars(str, ' ');
    }

//   static StringT remove_multi_whitespace(const StringT& str)
//   {
//     return remove_duplicate_chars(str, ' ');
//   }

    /**
     *   @brief : Returns an array of strings, including delimiters, each
     *                   containing tokens separated by the delimiter argument.
     *
     */
    static void make_tokens_with_delimiter(const StringT& str, const StringT& delimiter, std::vector<StringT>& tokens)
    {
        tokens.clear();
        if (str.length()<=delimiter.length())
            return;

        std::vector<size_t> indices;
        multi_find(str, delimiter, indices, 0, SM_SENSITIVE);
        tokens.reserve(indices.size()+1);
        indices.push_back(-1);

        size_t i = indices[0];
        if (i!=0)
            tokens.push_back(str.substr(0, i));

        for (i=0; i<indices.size()-1; i++)
        {
            if (indices[i]+delimiter.length()>=indices[i+1])
                continue;
            tokens.push_back(str.substr(indices[i]+delimiter.length(),
                                        indices[i+1]==(size_t)-1? indices[i+1]: indices[i+1]-(indices[i]+delimiter.length())));
        }
    }

    /**
       @brief cut the content of string with the range
     */
    static void cut_range(StringT& str, size_t start, size_t end)
    {
        str = str.substr(start, end - start + 1);
    }

    //   static StringT cut_range(const StringT& str, size_t start, size_t end)
//   {
//     return str.substr(start, end - start + 1);
//   }

    /**
     * @brief string retrieval method.
     *
     string retrieval method. Returns the string from
                      start to right before the delimiter character.
      Does not affect the receiver itself.
     *
     */
    static void cut_delimiter(StringT& str, size_t start, CharT delimiter)
    {
        assert(start < str.length());

        size_t i=start;

        while(str[i]!= delimiter)
        {
            i++;
            if (start + i >= str.length())
            {
                str.clear();
                return;
            }
        }

        if (i==start)
        {
            str.clear();
            return ;
        }

        return cut_range(str, start, i-1);
    }

    //   static StringT cut_delimiter(const StringT& str, size_t start, CharT delimiter)
//   {
//     StringT s =str;
//     cut_delimiter(s, start, delimiter);
//     return s;
//   }

    /**
     * @brief Cut between the specified two patterns.
     *
       Cut between the specified two patterns.
                     The return values starts with the first pattern and ends
                with the second one.

     */
    static void cut_between_words(StringT& str, const StringT& p1,
                                  const StringT& p2, StrCompMode caseChk=SM_SENSITIVE )
    {
        size_t i = find(str, p1, 0, caseChk);
        if (i==(size_t)-1)
        {
            str.clear();
            return;
        }

        size_t j = rfind(str, p2, StringT::npos, caseChk);
        if (j==(size_t)-1)
        {
            str.clear();
            return ;
        }

        if (i+p1.length()>=j)
        {
            str.clear();
            return ;
        }

        str = str.substr(i+p1.length(), j-i-p1.length()-1);
    }
    //   static StringT cut_between_words(const StringT& str, const StringT& p1,
//                                    const StringT& p2, StrCompMode caseChk=SM_SENSITIVE)
//   {
//     StringT s = str;
//     cut_between_words(s, p1, p2, caseChk);
//     return s;
//   }

    /**
     *@brief cut the word after a tag.
     */
    static void cut_word_after(StringT& str, const StringT& tag, StrCompMode caseChk=SM_IGNORE)
    {
        size_t index = find(str, tag,0, caseChk);
        if (index!=(size_t)-1)
        {
            size_t i = index + tag.length();
            for (; i < str.length() && IS_ALPHABET::is_space(str[i]); i++);

            if (i == str.length())
            {
                str.clear();
                return ;
            }

            size_t start = i;
            for (; i < str.length() && !IS_ALPHABET::is_space(str[i]); i++);

            cut_range(str, start, i - 1);
            return;
        }

        str.clear();
    }
//   static StringT cut_word_after(const StringT& str, const StringT& tag, StrCompMode caseChk=SM_IGNORE)
//   {
//     StringT s = str;
//     cut_word_after(s, tag, caseChk);
//     return s;
//   }

    /**
     *@brief cut a line after a tag.
     */
    static void cut_line_after(StringT& str, const StringT& tag, StrCompMode caseChk=SM_IGNORE)
    {
        size_t index = find(str, tag, 0, caseChk);
        if (index!=(size_t)-1)
        {
            size_t i = index + tag.length();
            if (i == str.length())
            {
                str.clear();
                return;
            }

            size_t start = i;
            for (; i < str.length() && !IS_ALPHABET::is_new_line(str[i]); i++);

            cut_range(str, start, i - 1);
            return;
        }

        str.clear();
    }
    // static StringT cut_line_after(const StringT& str, const StringT& tag, StrCompMode caseChk=SM_IGNORE)
//   {
//     StringT s = str;
//     cut_line_after(s, tag, caseChk);
//     return s;
//   }

    /**
    *
     @brief : Returns n DynamicArray of StringTs of which elements contain
                 the words between the marks.
           It doesn't contain the marks itself.
    *
    */
    static void take_between_marks(const StringT& str, CharT beg, CharT end,
                                   std::vector<StringT>& arrayOfstrings)
    {
        arrayOfstrings.clear();

        if (beg == end)
            return;

        size_t index = -1;
        size_t level = 0;
        size_t step = 0;

        for (typename StringT::const_iterator i = str.begin();
                i != str.end(); i++)
        {
            if (*i == beg)
            {
                index++;
                level++;
            }
            else if (*i == end)
            {
                if (level > 0)
                {
                    level--;
                    if (level == 0)
                        step = 0;
                    else
                        step++;
                }
            }
            else if (level > 0)
            {
                CharT temp = *i;
                size_t finalIndex = index - step;
                while (finalIndex >=arrayOfstrings.size())
                {
                    arrayOfstrings.push_back(StringT());
                }

                arrayOfstrings[finalIndex] += temp;
            }
            else;
        }
    }

    /**
       *
         @brief : Returns n DynamicArray of strings of which elements contain
                     the words between the marks.
    	       It doesn't contain the marks itself.
       *
       */
    static void take_between_mark(const StringT& str, CharT mark,
                                  std::vector<StringT>& arrayOfstrings)
    {
        arrayOfstrings.clear();
        if (str.length()==0)
            return;

        size_t index = 0;
        size_t prev = 0;

        index = str.find(mark, index);
        while(index == prev)
        {
            prev = index+1;
            index = str.find(mark, prev);
        }

        while (index != (size_t)-1)
        {
            StringT s = str;
            cut_range(s, prev, index - 1);
            arrayOfstrings.push_back(s);
            prev = index + 1;
            index = str.find(mark, index+1);
        }

        StringT s = str;
        cut_range(s, prev, str.length()-1);
        arrayOfstrings.push_back(s);
    }

    /**
      @brief Returns the rear part that begins with the tag, including
                      the tag depending on the option.
    */
    static void reverse_cut_rear_with(StringT& str, CharT tag,
                                      bool includeTag = false, StrCompMode caseChk = SM_IGNORE)
    {
        size_t i;
        for (i = str.length()-1; i!=(size_t)-1; i--)
        {
            if (caseChk == SM_SENSITIVE && str[i]==tag
                    || caseChk == SM_IGNORE && IS_ALPHABET::equal_ignore_case(str[i],tag))
                break;
        }

        if (i!=(size_t)-1)
        {
            if (includeTag)
                cut_range(str, i, str.length()-1);
            else if (i < str.length()-1)
                cut_range(str, i+1, str.length()-1);

            return;

        }

        str.clear();
    }
    // static StringT reverse_cut_rear_with(const StringT& str, CharT tag,
//                                        bool includeTag = false, StrCompMode caseChk = SM_IGNORE)
//   {
//     StringT s = str;
//     reverse_cut_rear_with(s, tag, includeTag, caseChk);
//     return s;
//   }

    /**
       @brief cut the content that starts with the tag.
     */
    static void reverse_cut_front_with(StringT& str, CharT tag,
                                       bool includeTag = false, StrCompMode caseChk = SM_IGNORE)
    {
        size_t i;
        for (i = str.length()-1; i!=(size_t)-1; i--)
            if (caseChk == SM_SENSITIVE && str[i]==tag
                    || caseChk == SM_IGNORE && IS_ALPHABET::equal_ignore_case(str[i],tag))
                break;

        if (i!=(size_t)-1)
        {
            if (includeTag)
                cut_range(str, 0, i);
            else if (i!=0)
                cut_range(str, 0, i-1);
            return;
        }

        str.clear();
    }
    //   static StringT reverse_cut_front_with(const StringT& str, CharT tag,
//                                        bool includeTag = false, StrCompMode caseChk = SM_IGNORE)
//   {
//     StringT s = str;
//     reverse_cut_front_with(s, tag, includeTag, caseChk);
//     return s;
//   }

    /**
     @brief Cut trailing spaces in the head.
     */
    static void compact_head(StringT& str)
    {
        size_t i=0;
        for (; i<str.length(); i++)
            if (!IS_ALPHABET::is_space(str[i]))
                break;

        str = str.substr(i);
    }
    // static StringT compact_head(const StringT& str)
//   {
//     StringT s = str;

//     compact_head(s);
//     return s;
//   }

    /**
     @brief Cut trailing spaces in the tail.
     */
    static void compact_tail(StringT& str)
    {
        size_t i=str.length()-1;
        for (; i!=(size_t)-1; i--)
            if (!IS_ALPHABET::is_space(str[i]))
                break;

        str = str.substr(0, i+1);
    }
    // static StringT compact_tail(const StringT& str)
//   {
//     StringT s = str;
//     compact_tail(s);
//     return s;
//   }

    /**
       @brief remove all the spaces at head or tail
     */
    static void compact(StringT& str)
    {
        compact_head(str);
        compact_tail(str);
    }
    // static StringT compact(const StringT& str)
//   {
//     StringT s;
//     s = compact_head(str);
//     compact_tail(s);
//     return s;
//   }

    /**
       @brief remove all the spaces
     */
    static StringT trim(const StringT& str)
    {
        StringT s;
        s.reserve(str.length()*3/4);

        size_t i=0;
        size_t last = i;
        for (; i<str.length(); i++)
        {
            if (!IS_ALPHABET::is_space(str[i]))
                continue;

            if (last<i )
            {
                s += str.substr(last, i-last);
            }

            last = i+1;
        }

        if (last < str.length())
            s += str.substr(last);

        return s;
    }

    static StringT trimToOneSpace(const StringT& str)
    {
        StringT s;
        s.reserve(str.length()*3/4);

        size_t i=0;
        size_t last = i;
        for (; i<str.length(); i++)
        {
            if (!IS_ALPHABET::is_space(str[i]))
                continue;

            if (last<i )
            {
                s += str.substr(last, i-last+1);
            }

            last = i+1;
        }

        if (last < str.length())
            s += str.substr(last);

        return s;
    }

    static StringT padForAlphaNum(const StringT& str)
    {
        StringT s;
        if (str.empty()) return s;
        s.reserve(str.length());

        if (IS_ALPHABET::value(str[0]) || IS_NUMERIC::value(str[0]))
            s.push_back(' ');
        s.push_back(str[0]);

        //size_t last = 0;
        for (size_t current = 1; current < str.length(); ++current)
        {
            if (str[current] == ' ')
            {
                if (str[current - 1] != ' ')
                    s.push_back(' ');
            }
            else if (IS_ALPHABET::value(str[current]))
            {
                if (str[current - 1] != ' ' && (!IS_ALPHABET::value(str[current - 1]) || IS_NUMERIC::value(str[current - 1]) ))
                    s.push_back(' ');
                s.push_back(str[current]);
            }
            else if (IS_NUMERIC::value(str[current]))
            {
                if (str[current - 1] != ' ' && (!IS_NUMERIC::value(str[current - 1]) || IS_ALPHABET::value(str[current - 1]) ))
                    s.push_back(' ');
                s.push_back(str[current]);
            }
            else
            {
                if (IS_ALPHABET::value(str[current - 1]) || IS_NUMERIC::value(str[current - 1]))
                    s.push_back(' ');
                s.push_back(str[current]);
            }
        }

        if (IS_ALPHABET::value(str[str.length() - 1]) || IS_NUMERIC::value(str[str.length() - 1]))
            s.push_back(' ');

        return s;
    }

    static StringT trimHead(const StringT& str)
    {
        StringT s;
        s.reserve(str.length()*3/4);

        size_t i=0;
        // size_t last = i;
        for (; i<str.length(); i++)
        {
            if (!IS_ALPHABET::is_space(str[i]))
                break;
        }
        s += str.substr(i);

        return s;
    }

    static StringT trimTail(const StringT& str)
    {
        StringT s;
        s.reserve(str.length()*3/4);

        size_t i=str.length();
        // size_t last = i;
        for (; i>0; i--)
        {
            if (!IS_ALPHABET::is_space(str[i]))
                break;
        }
        s += str.substr(0,i);

        return s;
    }

    static StringT trim1(const StringT& str)
    {
        return  trimHead(trimToOneSpace(str));
    }

    static StringT trim2(const StringT& str)
    {
        return    trimTail(trim1(str));
    }
    // static void trim(StringT& str)
//   {
//     const StringT s =str;
//     str = trim(s);
//   }

    static void read_from_file(const char* filename, std::vector<StringT>& lines, CharT line='\n')
    {
        std::ifstream file (filename, std::ios::in|std::ios::binary|std::ios::ate);
        // get length of file:
        file.seekg (0, std::ios::end);
        std::size_t length = file.tellg();
        file.seekg (0, std::ios::beg);

        // allocate memory:
        char* buffer = (char*)HLmemory::hlmalloc(length);

        // read data as a block:
        file.read (buffer,length);
        file.close();

        StringT s((CharT* )buffer, length/sizeof(CharT));
        HLmemory::hlfree(buffer);

        make_tokens_with_delimiter(s, line, lines);
    }

    static void read_from_file(const char* encode_t, const char* filename, std::vector<StringT>& lines, CharT line='\n')
    {
        std::ifstream file (filename, std::ios::in|std::ios::binary|std::ios::ate);
        // get length of file:
        file.seekg (0, std::ios::end);
        std::size_t length = file.tellg();
        file.seekg (0, std::ios::beg);

        // allocate memory:
        char* buffer = (char*)HLmemory::hlmalloc(length);

        // read data as a block:
        file.read (buffer,length);
        file.close();

        StringT s;
        read_from_encode(encode_t, buffer, length, s);

        HLmemory::hlfree(buffer);

        make_tokens_with_delimiter(s, line, lines);
    }

    static void write_to_file(const char* filename, const std::vector<StringT>& lines)
    {
        std::ofstream of(filename, std::ios::out|std::ios::binary);

        for (size_t i=0; i<lines.size(); i++)
        {
            for (typename StringT::const_iterator j = lines[i].begin(); j!=lines[i].end(); j++)
            {
                CharT ch = *j;
                of.write((char*)&ch, sizeof(CharT));
            }
            CharT ch = '\n';
            of.write((char*)&ch, sizeof(CharT));
        }

        of.close();
    }

    /**
    * @brief Read all the names of the files in the specified directory.
    *
       Read all the names of the files in the specified directory.
                    If there are any errors such as no such directory, it
              just returns leaving the reference argument unchanged.
            It discards '.' and '..' files.

    *
    */
    static void read_filenames(const StringT& dir, std::vector<StringT>& files)
    {
//     DIR *dirp;
//     struct dirent *direntp;
//     dirp = opendir(dir.c_str());
//     if (dirp == NULL) {
//       std::cout << "Warning: Algorithm::read_filenames : invalid directory: "
//            << dir << std::endl;
//       return;
//     }
//     while ( (direntp = readdir( dirp )) != NULL )
//       if (!((strcmp(direntp->d_name, ".") == 0
// 	     || strcmp(direntp->d_name, "..") == 0)))
// 	files.push_back(direntp->d_name);
//     (void)closedir( dirp );

    }

    /**
       @brief it generates random strings
       @param size the length of string
     */
    static StringT generate_random_string(size_t size)
    {
        StringT s;
        s.reserve(size);
        CharT span = 'z'-'a';

        for (size_t i=0; i<size; i++)
            s += (CharT)(::rand()%span);

        return s;
    }

    /**
     * Text in input is transformed from encode_type into UCS-2 and stored in output.
     *Generally, this step is ahead of text processing for various encode type text.
     **/
//   static bool read_from_encode(const char* encode_type, const char* input, size_t len, StringT& output)
//   {
//     output.clear();

//     iconv_t ic = iconv_open("ucs-2", encode_type);
//     if (ic == (iconv_t)(-1))
//       return false;

//     size_t ilen = 0;
//     size_t olen = 0;
//     ilen = len;
//     olen = ilen * 4;

//     char *ibuf = const_cast<char *>(input);

//     char *obuf_org = (char*)HLmemory::hlmalloc(olen);
//     char *obuf = obuf_org;
//     std::fill(obuf, obuf + olen, 0);

//     size_t olen_org = olen;
//     iconv(ic, 0, &ilen, 0, &olen);  // reset iconv state

//     while (ilen != 0) {
//       if (iconv(ic, &ibuf, &ilen, &obuf, &olen)
//           == (size_t) -1) {
//         ibuf++;
//         ilen++;
//       }
//       //std::cout<<"uu"<<ilen<<std::endl;
//     }

//     output.assign((CharT*)obuf_org, (olen_org - olen)/sizeof(CharT));

//     HLmemory::hlfree(obuf_org);

//     iconv_close(ic);

//     return true;
//   }

    /**
     *Text in input of UCS-2 can be transformed into encode_type and restored into output.
     *There a new memory block will be applyed for outputing. And the size of the block will be of len.
     *So, the output should be released by calling release at the later section.
     *This step is mainly called after text have been processed.
     **/
//   static bool write_to_encode(const char* encode_type, char** output, size_t& len, const StringT& input)
//   {
//     iconv_t ic = iconv_open(encode_type, "ucs-2");

//     if (ic == (iconv_t)(-1))
//       return false;

//     size_t ilen = 0;
//     size_t olen = 0;
//     ilen = input.size();
//     olen = ilen * 4+1;
//     char *ibuf = (char*)HLmemory::hlmalloc(ilen);
//     char *ibuf_org = ibuf;

//     input.copy((CharT*)ibuf, input.length());

//     *output = (char*)HLmemory::hlmalloc(olen);
//     char *obuf_org = *output;
//     char *obuf = obuf_org;
//     std::fill(obuf, obuf + olen, 0);

//     size_t olen_org = olen;
//     iconv(ic, 0, &ilen, 0, &olen);  // reset iconv state
//     while (ilen != 0) {
//       if (iconv(ic, &ibuf, &ilen, &obuf, &olen)
//           == (size_t) -1) {
//         //std::cout<<"jj"<<ilen<<std::endl;
//         ibuf++;
//         ilen++;
//         //len =  olen_org - olen;
//         //HLmemory::hlfree(ibuf_org);
//         //iconv_close(ic);

//         //return false;
//       }
//       //std::cout<<"yy"<<ilen<<std::endl;
//     }

//     len =  olen_org - olen;

//     HLmemory::hlfree(ibuf_org);

//     iconv_close(ic);
//     return true;
//   }

//   static void release(char* p)
//   {
//     HLmemory::hlfree(p);
//   }

//   /**
//    *Output str in encode_t into os.
//    **/
//   static void display(const StringT& str, const char* encode_t="UTF-8", std::ostream& os=std::cout)
//   {
//     char* buf=NULL;
//     size_t len = 0;

//     if (!write_to_encode(encode_t, &buf, len, str))
//       return;
//     //buf[len-1] = '\0';

//     os<<buf<<std::endl;
//     release(buf);
//   }

}
;

NS_IZENELIB_UTIL_END
#endif


#include <iostream>
#include <unordered_map>
#include <elemental.hpp>
#include <boost/mpi.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <skylark.hpp>

namespace bmpi =  boost::mpi;
namespace bpo = boost::program_options;
namespace skybase = skylark::base;
namespace skysketch =  skylark::sketch;
namespace skynla = skylark::nla;
namespace skyalg = skylark::algorithms;
namespace skyml = skylark::ml;
namespace skyutil = skylark::utility;

struct simple_unweighted_graph_t {

    simple_unweighted_graph_t(const std::string &gf);

    ~simple_unweighted_graph_t() {
        delete[] _out;
    }

    int num_vertices() const { return _num_vertices; }
    int num_edges() const { return _num_edges; }
    int degree(int vertex) const { return _nodepairs.at(vertex).first; }
    const int *adjanct(int vertex) const { return _nodepairs.at(vertex).second; }

private:
    typedef std::pair<int, int *> nodepair_t;

    std::unordered_map<int, nodepair_t> _nodepairs;
    int *_out;
    int _num_vertices;
    int _num_edges;
};

simple_unweighted_graph_t::simple_unweighted_graph_t(const std::string &gf) {

    std::ifstream in(gf);
    std::string line, token;

    _num_edges = 0;
    while(true) {
        getline(in, line);
        if (in.eof())
            break;
        if (line[0] == '#')
            continue;

        std::istringstream tokenstream(line);
        tokenstream >> token;
        int i = atoi(token.c_str());
        tokenstream >> token;
        int j = atoi(token.c_str());

        if (i == j)
            continue;

        _nodepairs[i].first++;
        _nodepairs[j].first++;
        _num_edges += 2;
    }

    _num_vertices = _nodepairs.size();

    std::cout << "Finished first pass. Vertices = " << _num_vertices
              << " Edges = " << _num_edges << std::endl;
    _out = new int[_num_edges];

    // Set pointers and zero degrees.
    int count = 0;
    for(auto it = _nodepairs.begin(); it != _nodepairs.end(); it++) {
        int nodeid = it->first;
        int deg = it->second.first;
        _nodepairs[nodeid] = nodepair_t(0, _out + count);
        count += deg;
    }

    // Second pass
    in.clear();
    in.seekg(0, std::ios::beg);
    while(!in.eof()) {
        getline(in, line);
        if (line[0] == '#')
            continue;

        std::istringstream tokenstream(line);
        tokenstream >> token;
        int i = atoi(token.c_str());
        tokenstream >> token;
        int j = atoi(token.c_str());

        if (i == j)
            continue;

        nodepair_t &npi = _nodepairs[i];
        npi.second[npi.first] = j;
        npi.first++;

        nodepair_t &npj = _nodepairs[j];
        npj.second[npj.first] = i;
        npj.first++;
    }

    std::cout << "Finished reading... ";
    in.close();
}

int main(int argc, char** argv) {

    elem::Initialize(argc, argv);

    boost::mpi::timer timer;

    // Parse options
    double gamma, alpha, epsilon;
    bool recursive, interactive;
    std::string graphfile, indexfile;
    std::vector<std::string> seedss;
    std::vector<int> seeds;
    bpo::options_description
        desc("Options:");
    desc.add_options()
        ("help,h", "produce a help message")
        ("graphfile,g",
            bpo::value<std::string>(&graphfile),
            "File holding the graph. REQUIRED.")
        ("indexfile,d",
            bpo::value<std::string>(&indexfile)->default_value(""),
            "Index files mapping node-ids to strings. OPTIONAL.")
        ("interactive,i", "Whether to run in interactive mode.")
        ("seed,s",
            bpo::value<std::vector<std::string> >(&seedss),
            "Seed node. Use multiple times for multiple seeds. REQUIRED. ")
        ("recursive,r",
            bpo::value<bool>(&recursive)->default_value(true),
            "Whether to try to recursively improve clusters "
            "(use cluster found as a seed)" )
        ("gamma",
            bpo::value<double>(&gamma)->default_value(5.0),
            "Time to derive the diffusion. As gamma->inf we get closer to ppr.")
        ("alpha",
            bpo::value<double>(&alpha)->default_value(0.85),
            "PPR component parameter. alpha=1 will result in pure heat-kernel.")
        ("epsilon",
            bpo::value<double>(&epsilon)->default_value(0.001),
            "Accuracy parameter for convergence.");

    bpo::variables_map vm;
    try {
        bpo::store(bpo::command_line_parser(argc, argv)
            .options(desc).run(), vm);

        if (vm.count("help")) {
            std::cout << desc;
            return 0;
        }

        interactive = vm.count("interactive");

        if (!vm.count("graphfile")) {
            std::cout << "Input graph-file is required." << std::endl;
            return -1;
        }

        if (!interactive && !vm.count("seed")) {
            std::cout << "A seed is required in non-interactive mode."
                      << std::endl;
            return -1;
        }

        bpo::notify(vm);
    } catch(bpo::error& e) {
        std::cerr << e.what() << std::endl;
        std::cerr << desc << std::endl;
        return -1;
    }

    std::cout << "Reading the adjacency matrix... " << std::endl;
    std::cout.flush();
    timer.restart();
    simple_unweighted_graph_t G(graphfile);
    std::cout <<"took " << boost::format("%.2e") % timer.elapsed() << " sec\n";

    bool use_index = !indexfile.empty();
    std::unordered_map<int, std::string> id_to_name_map;
    std::unordered_map<std::string, int> name_to_id_map;
    if (use_index) {
        std::cout << "Reading index files... ";
        std::cout.flush();
        timer.restart();

        std::ifstream in(indexfile);
        std::string line, token;

        while(true) {
            getline(in, line);
            if (in.eof())
                break;

            if (line[0] == '#')
                continue;

            std::istringstream tokenstream(line);
            tokenstream >> token;
            std::string name = token;
            tokenstream >> token;
            int node = atoi(token.c_str());

            id_to_name_map[node] = name;
            name_to_id_map[name] = node;
        }

        in.close();

        std::cout <<"took " << boost::format("%.2e") % timer.elapsed() << " sec\n";
    }

    do {
        if (interactive) {
            std::cout << "Please input seeds: ";
            std::string line;
            std::getline(std::cin, line);
            if (line.empty())
                break;

            seeds.clear();
            std::stringstream strs(line);
            if (use_index) {
                std::string seed;
                int c = 0;
                while (strs >> seed) {
                    seeds.push_back(name_to_id_map[seed]);
                    c++;
                    if (c == 200)
                        exit(-1);
                }
            } else {
                int seed;
                int c = 0;
                while(strs >> seed) {
                    seeds.push_back(seed);
                    c++;
                    if (c == 200)
                        exit(-1);
                }
            }
        } else {
            for(auto it = seedss.begin(); it != seedss.end(); it++)
                if (use_index)
                    seeds.push_back(name_to_id_map[*it]);
                else
                    seeds.push_back(atoi(it->c_str()));
        }

        timer.restart();
        std::vector<int> cluster;
        double cond = skyml::FindLocalCluster(G, seeds, cluster,
            alpha, gamma, epsilon, recursive);
        std::cout <<"Analysis complete! Took "
                  << boost::format("%.2e") % timer.elapsed() << " sec\n";
        std::cout << "Cluster found:" << std::endl;
        for (auto it = cluster.begin(); it != cluster.end(); it++)
            if (use_index)
                std::cout << id_to_name_map[*it] << std::endl;
            else
                std::cout << *it << " ";
        if (!use_index)
            std::cout << std::endl;
        std::cout << "Conductivity = " << cond << std::endl;
    } while (interactive);

    return 0;
}


#ifndef STAN_MATH_REV_MAT_FUN_QUAD_FORM_SYM_HPP
#define STAN_MATH_REV_MAT_FUN_QUAD_FORM_SYM_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/typedefs.hpp>
#include <stan/math/rev/mat/fun/typedefs.hpp>
#include <stan/math/prim/mat/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/quad_form.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/rev/mat/fun/quad_form.hpp>

namespace stan {
  namespace math {

    template <typename TA, int RA, int CA, typename TB, int RB, int CB>
    inline typename
    boost::enable_if_c< boost::is_same<TA, var>::value ||
    boost::is_same<TB, var>::value,
                        Eigen::Matrix<var, CB, CB> >::type
      quad_form_sym(const Eigen::Matrix<TA, RA, CA>& A,
                    const Eigen::Matrix<TB, RB, CB>& B) {
      check_square("quad_form", "A", A);
      check_symmetric("quad_form_sym", "A", A);
      check_multiplicable("quad_form_sym",
                          "A", A,
                          "B", B);

      quad_form_vari<TA, RA, CA, TB, RB, CB> *baseVari
        = new quad_form_vari<TA, RA, CA, TB, RB, CB>(A, B, true);

      return baseVari->impl_->C_;
    }
    template <typename TA, int RA, int CA, typename TB, int RB>
    inline typename
    boost::enable_if_c< boost::is_same<TA, var>::value ||
    boost::is_same<TB, var>::value,
                        var >::type
      quad_form_sym(const Eigen::Matrix<TA, RA, CA>& A,
                    const Eigen::Matrix<TB, RB, 1>& B) {
      check_square("quad_form", "A", A);
      check_symmetric("quad_form_sym", "A", A);
      check_multiplicable("quad_form_sym",
                          "A", A,
                          "B", B);

      quad_form_vari<TA, RA, CA, TB, RB, 1> *baseVari
        = new quad_form_vari<TA, RA, CA, TB, RB, 1>(A, B, true);

      return baseVari->impl_->C_(0, 0);
    }

  }
}
#endif