/*
 * Copyright 2015-2019 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "libwaypoint_follower/pure_pursuit.h"
#include "libwaypoint_follower/libwaypoint_follower.h"

#include <limits>
#include <utility>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>

void PurePursuit::setUseLerp(bool ul) { use_lerp_ = ul; }

void PurePursuit::setLookaheadDistance(double ld) { lookahead_distance_ = ld; }

void PurePursuit::setClosestThreshold(double clst_thr_dist, double clst_thr_ang)
{
  clst_thr_dist_ = clst_thr_dist;
  clst_thr_ang_ = clst_thr_ang;
}

geometry_msgs::Point PurePursuit::getLocationOfNextWaypoint() { return loc_next_wp_; }

geometry_msgs::Point PurePursuit::getLocationOfNextTarget() { return loc_next_tgt_; }

bool PurePursuit::isRequirementsSatisfied()
{
  return (curr_wps_ptr_  && curr_pose_ptr_) ? true : false;
}

std::pair<bool, double> PurePursuit::run()
{
  std::pair<bool, double> error = std::make_pair(false, std::numeric_limits<double>::quiet_NaN());
  if (!isRequirementsSatisfied())
    return error;

  auto clst_pair = findClosestIdxWithDistAngThr(*curr_wps_ptr_, *curr_pose_ptr_, clst_thr_dist_, clst_thr_ang_);

  if (!clst_pair.first)
  {
    ROS_WARN("cannot find, curr_bool: %d, clst_idx: %d", clst_pair.first, clst_pair.second);
    return error;
  }

  int32_t next_wp_idx = findNextPointIdx(clst_pair.second);
  if (next_wp_idx == -1)
  {
    ROS_WARN("lost next waypoint");
    return error;
  }

  loc_next_wp_ = curr_wps_ptr_->at(next_wp_idx).position;

  geometry_msgs::Point next_tgt_pos;
  // if use_lerp_ is false or next waypoint is first
  if (!use_lerp_ || next_wp_idx == 0)
  {
    next_tgt_pos = curr_wps_ptr_->at(next_wp_idx).position;
  }
  else
  {
    // linear interpolation
    std::pair<bool, geometry_msgs::Point> lerp_pair = lerpNextTarget(next_wp_idx);

    if (!lerp_pair.first)
    {
      ROS_WARN("lost target! ");
      return error;
    }

    next_tgt_pos = lerp_pair.second;
  }
  loc_next_tgt_ = next_tgt_pos;

  double kappa = calcCurvature(next_tgt_pos, *curr_pose_ptr_);

  return std::make_pair(true, kappa);
}

// linear interpolation of next target
std::pair<bool, geometry_msgs::Point> PurePursuit::lerpNextTarget(int32_t next_wp_idx)
{
  std::pair<bool, geometry_msgs::Point> error = std::make_pair(false, geometry_msgs::Point());
  constexpr double ERROR2 = 1e-5;  // 0.00001
  const geometry_msgs::Point &vec_end = curr_wps_ptr_->at(next_wp_idx).position;
  const geometry_msgs::Point &vec_start = curr_wps_ptr_->at(next_wp_idx - 1).position;
  const geometry_msgs::Pose &curr_pose = *curr_pose_ptr_;

  Eigen::Vector3d vec_a((vec_end.x - vec_start.x),
                        (vec_end.y - vec_start.y),
                        (vec_end.z - vec_start.z));

  if (vec_a.norm() < ERROR2)
    return error;

  double lateral_error = calcLateralError2D(
      vec_start, vec_end, curr_pose.position);

  if (std::fabs(lateral_error) > lookahead_distance_)
    return error;

  /* calculate the position of the foot of a perpendicular line */
  Eigen::Vector2d uva2d(vec_a.x(), vec_a.y());
  uva2d.normalize();
  Eigen::Rotation2Dd rot = (lateral_error > 0) ? Eigen::Rotation2Dd(-M_PI / 2.0) : Eigen::Rotation2Dd(M_PI / 2.0);
  Eigen::Vector2d uva2d_rot = rot * uva2d;

  geometry_msgs::Point h;
  h.x = curr_pose.position.x + fabs(lateral_error) * uva2d_rot.x();
  h.y = curr_pose.position.y + fabs(lateral_error) * uva2d_rot.y();
  h.z = curr_pose.position.z;

  // if there is a intersection
  if (fabs(fabs(lateral_error) - lookahead_distance_) < ERROR2)
  {
    return std::make_pair(true, h);
  }
  else
  {
    // if there are two intersection
    // get intersection in front of vehicle
    double s = sqrt(pow(lookahead_distance_, 2) - pow(lateral_error, 2));
    geometry_msgs::Point res;
    res.x = h.x + s * uva2d.x();
    res.y = h.y + s * uva2d.y();
    res.z = curr_pose.position.z;
    return std::make_pair(true, res);
  }
}

int32_t PurePursuit::findNextPointIdx(int32_t search_start_idx)
{
  // if waypoints are not given, do nothing.
  if (curr_wps_ptr_->size() < 3 || search_start_idx == -1)
    return -1;

  // look for the next waypoint.
  for (int32_t i = search_start_idx; i < static_cast<int32_t>(curr_wps_ptr_->size()); i++)
  {
    // if search waypoint is the last
    if (i == static_cast<int32_t>(curr_wps_ptr_->size() - 1))
    {
      return i;
    }

    // if waypoint is  not on the front
    if (isDirectionForward(*curr_wps_ptr_))
    {
      if (transformToRelativeCoordinate2D(curr_wps_ptr_->at(i).position, *curr_pose_ptr_).x < 0)
        continue;
    }
    else
    {
      if (transformToRelativeCoordinate2D(curr_wps_ptr_->at(i).position, *curr_pose_ptr_).x > 0)
        continue;
    }

    const geometry_msgs::Point &curr_motion_point = curr_wps_ptr_->at(i).position;
    const geometry_msgs::Point &curr_pose_point = curr_pose_ptr_->position;
    // if there exists an effective waypoint
    const double ds = calcDistSquared2D(curr_motion_point, curr_pose_point);
    if (ds > std::pow(lookahead_distance_, 2))
      return i;
  }

  // if this program reaches here , it means we lost the waypoint!
  ROS_ERROR("lost waypoint!");
  return -1;
}

void PurePursuit::setCurrentPose(const geometry_msgs::Pose &msg)
{
  curr_pose_ptr_ = std::make_shared<geometry_msgs::Pose>();
  *curr_pose_ptr_ = msg;
}

void PurePursuit::setWaypoints(const std::vector<geometry_msgs::Pose> &msg)
{
  curr_wps_ptr_ = std::make_shared<std::vector<geometry_msgs::Pose>>();
  *curr_wps_ptr_ = msg;
}


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file steiner_network_test.cpp
 * @brief
 * @author Piotr Wygocki, Piotr Godlewski
 * @version 1.0
 * @date 2013-06-24
 */

#include "test_utils/logger.hpp"

#include "paal/iterative_rounding/iterative_rounding.hpp"
#include "paal/iterative_rounding/steiner_network/steiner_network.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/test/unit_test.hpp>

#include <vector>

using namespace paal;
using namespace paal::ir;

template <typename VertexList, typename EdgeProp>
using Graph = boost::adjacency_list<boost::vecS, VertexList, boost::undirectedS,
                                    boost::property<boost::vertex_index_t, int>,
                                    EdgeProp>;

using EdgeProp = boost::property<boost::edge_index_t, std::size_t,
                                 boost::property<boost::edge_weight_t, int>>;
using VectorGraph = Graph<boost::vecS, EdgeProp>;

using Edge = boost::graph_traits<VectorGraph>::edge_descriptor;
using ResultNetwork = std::vector<Edge>;

int restrictions(int i, int j) { return 2; }

void print_result(const ResultNetwork & result_network) {
    LOGLN("Edges in steiner network");
    ON_LOG(for(auto const e : result_network) {
        LOGLN("Edge " << e);
    })
}

BOOST_AUTO_TEST_SUITE(steiner_network)
BOOST_AUTO_TEST_CASE(steiner_network_test) {
    // sample problem
    LOGLN("Sample problem:");
    VectorGraph g(3);
    ResultNetwork result_network;
    bool b;
    b = add_edge(0, 1, EdgeProp(0, 1), g).second;
    b &= add_edge(0, 1, EdgeProp(1, 1), g).second;
    b &= add_edge(1, 2, EdgeProp(2, 1), g).second;
    b &= add_edge(1, 2, EdgeProp(3, 1), g).second;
    b &= add_edge(2, 0, EdgeProp(4, 7), g).second;
    assert(b);

    //solve it
    steiner_network_iterative_rounding(
        g, restrictions, std::back_inserter(result_network));

    print_result(result_network);
    BOOST_CHECK_EQUAL(result_network.size(), 4u);
}

BOOST_AUTO_TEST_CASE(steiner_network_list) {
    // boost::listS instead of boost::vecS for vertex storage
    using ListGraph =
        Graph<boost::listS, boost::property<boost::edge_weight_t, int>>;
    using EdgeT = boost::graph_traits<ListGraph>::edge_descriptor;

    std::vector<std::pair<int, int>> edges = { { 0, 1 }, { 0, 1 }, { 1, 2 },
                                               { 1, 2 }, { 2, 0 } };
    std::vector<int> costs{ 1, 1, 1, 1, 7 };
    ListGraph g(edges.begin(), edges.end(), costs.begin(), 3);

    auto index = get(boost::vertex_index, g);
    int idx = 0;
    for (auto v : boost::make_iterator_range(vertices(g))) {
        put(index, v, idx);
        ++idx;
    }

    std::vector<EdgeT> result_network;
    steiner_network_iterative_rounding(
        g, restrictions, std::back_inserter(result_network));
    BOOST_CHECK_EQUAL(result_network.size(), 4u);
}

BOOST_AUTO_TEST_CASE(steiner_network_test_properties) {
    // sample problem
    LOGLN("Sample problem:");
    VectorGraph g(3);

    bool b;
    b = add_edge(0, 1, 0, g).second;
    b &= add_edge(0, 1, 1, g).second;
    b &= add_edge(1, 2, 2, g).second;
    b &= add_edge(1, 2, 3, g).second;
    b &= add_edge(2, 0, 4, g).second;
    assert(b);

    auto edge_id = get(boost::edge_index, g);
    std::vector<double> costs = {1, 1, 1, 1, 7};
    auto cost = boost::make_iterator_property_map(costs.begin(), edge_id);

    // solve it
    {
        ResultNetwork result_network;
        steiner_network_iterative_rounding(g, restrictions,
            boost::weight_map(cost), std::back_inserter(result_network));

        print_result(result_network);
    }
    {
        ResultNetwork result_network;
        auto steiner_network(make_steiner_network(g, restrictions,
                    boost::weight_map(cost),
                    std::back_inserter(result_network)));
        solve_iterative_rounding(steiner_network, steiner_network_ir_components<>());

        print_result(result_network);
    }
}

BOOST_AUTO_TEST_CASE(steiner_network_invalid_test) {
    // invalid problem (restrictions cannot be satisfied)
    LOGLN("Invalid problem (restrictions cannot be satisfied):");
    VectorGraph g(3);
    ResultNetwork result_network;
    bool b;
    b = add_edge(0, 1, EdgeProp(0, 1), g).second;
    b &= add_edge(1, 2, EdgeProp(1, 1), g).second;
    assert(b);

    //solve it
    auto steiner_network(make_steiner_network(g, restrictions,
                                    std::back_inserter(result_network)));
    auto invalid = steiner_network.check_input_validity();

    BOOST_CHECK(invalid);
    LOGLN(*invalid);
}
BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Eigen>
#include <sophus/se3.hpp>

#include <pangolin/pangolin.h>
#include <pangolin/glcuda.h>
#include <pangolin/glvbo.h>

#include <SceneGraph/SceneGraph.h>

#include <kangaroo/kangaroo.h>
#include <kangaroo/BoundedVolume.h>
#include <kangaroo/MarchingCubes.h>
#include <kangaroo/extra/ImageSelect.h>
#include <kangaroo/extra/BaseDisplayCuda.h>
#include <kangaroo/extra/DisplayUtils.h>
#include <kangaroo/extra/Handler3dGpuDepth.h>
#include <kangaroo/extra/SavePPM.h>
#include <kangaroo/extra/SaveMeshlab.h>

#ifdef HAVE_CVARS
#include <kangaroo/extra/CVarHelpers.h>
#endif // HAVE_CVARS

#include <opencv2/opencv.hpp>
#include <unistd.h>

#include <HAL/Utils/GetPot>
#include <HAL/Utils/TicToc.h>
#include <HAL/Camera/CameraDevice.h>
#include <PbMsgs/DensePose.pb.h>
#include <calibu/Calibu.h>
#include <CVars/CVar.h>

#include <Node/Node.h>
#include <PbMsgs/Matrix.h>
#include <string>

using namespace pangolin;

bool Capture( node::node* n, const std::string topic, float* depth_buffer_, Sophus::SE3d &pose )
{
  pb::DensePoseMsg dpm;
  if (n->receive( topic, dpm)) {
    fprintf(stderr, "Message received loud and clear!\n");
  } else {
    fprintf(stderr, "No message received on: %s\n", topic.c_str());
  }
  fflush(stderr);
  if (dpm.has_image()) {
    const int image_size_ = dpm.image().image(0).width()*dpm.image().image(0).height()*sizeof(float);
    memcpy(depth_buffer_, dpm.image().image(0).data().c_str(), image_size_);
  }
  if (dpm.has_pose()) {
    Eigen::VectorXd p;
    pb::ReadVector( dpm.pose().pose(), &p);
  }
}

int main( int argc, char* argv[] )
{
  node::node server;
  server.set_verbosity(9);
  server.init("MKServer");

  //  std::vector< node::node > nodes;
  // Initialise window
  View& container = SetupPangoGLWithCuda(1024, 768);
  SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();

  GetPot clArgs(argc, argv);
  const int num_nodes = atoi(clArgs.follow("0", "-nodes").c_str());

//  std::vector< const std::string > topics;
//  for(int ii = 0; ii < num_nodes; ii++) {
//    std::stringstream temp;
//    temp << "Node" << ii << "/DepthCam";
//    const std::string topic = temp.str();
//    topics.push_back( topic );
//  }

  const int imageWidth = 640; //camera.Width();
  const int imageHeight = 480; //camera.Height();

  ///----- Load camera model.
  calibu::CameraRig rig;
  //  if (!camera.GetDeviceProperty(hal::DeviceDirectory).empty()) {
  //    std::cout<<"Loaded camera: "<<camera.GetDeviceProperty(hal::DeviceDirectory) + '/' + clArgs.follow("cameras.xml", "-cmod")<<std::endl;
  //    rig = calibu::ReadXmlRig(camera.GetDeviceProperty(hal::DeviceDirectory) + '/' + clArgs.follow("cameras.xml", "-cmod"));
  //  }
  //  else {
  rig = calibu::ReadXmlRig(clArgs.follow("cameras.xml", "-cmod"));
  //  }

  // Force vision convention.
  rig = calibu::ToCoordinateConvention(rig, calibu::RdfVision);

  Eigen::Matrix3f KL;
  KL = rig.cameras[0].camera.K().cast<float>();

  ///----- Init aux variables and dense tracker.

  const bool use_colour = false;

  const int w = imageWidth;
  const int h = imageHeight;

  const int MaxLevels = 3;
  const int its[] = {1,0,2,3};

  const roo::ImageIntrinsics K(KL(0, 0),KL(1, 1), KL(0, 2), KL(1, 2) );

  const double knear = 0.01;
  const double kfar = 1.0;

  const float volrad = 0.250;
  const int volres = 256;

  roo::BoundingBox reset_bb(make_float3(-volrad,-volrad,knear), make_float3(volrad,volrad,knear+2*volrad));
  //    roo::BoundingBox reset_bb(make_float3(-volrad,-volrad,-volrad), make_float3(volrad,volrad,volrad));

#ifdef HAVE_CVARS
  CVarUtils::AttachCVar<roo::BoundingBox>("BoundingBox", &reset_bb);
#endif // HAVE_CVARS

  // Camera (rgb) to depth
  roo::Image<float, roo::TargetDevice, roo::Manage> dKinectMeters(w,h);
  roo::Pyramid<float, MaxLevels, roo::TargetDevice, roo::Manage> kin_d(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> kin_v(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> kin_n(w,h);
  roo::Image<float4, roo::TargetDevice, roo::Manage>  dDebug(w,h);

  roo::Pyramid<float, MaxLevels, roo::TargetDevice, roo::Manage> ray_i(w,h);
  roo::Pyramid<float, MaxLevels, roo::TargetDevice, roo::Manage> ray_d(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> ray_n(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> ray_v(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> ray_c(w,h);
  roo::BoundedVolume<roo::SDF_t, roo::TargetDevice, roo::Manage> vol(volres,volres,volres,reset_bb);
  roo::BoundedVolume<float, roo::TargetDevice, roo::Manage> colorVol(volres,volres,volres,reset_bb);

  std::vector<std::unique_ptr<KinectKeyframe> > keyframes;
  roo::Mat<roo::ImageKeyframe<uchar3>,10> kfs;

  SceneGraph::GLSceneGraph glgraph;
  SceneGraph::GLAxis glcamera(0.1);
  SceneGraph::GLAxisAlignedBox glboxfrustum;
  SceneGraph::GLAxisAlignedBox glboxvol;

  glboxvol.SetBounds(roo::ToEigen(vol.bbox.Min()), roo::ToEigen(vol.bbox.Max()) );
  glgraph.AddChild(&glcamera);
  glgraph.AddChild(&glboxvol);
  glgraph.AddChild(&glboxfrustum);

  pangolin::OpenGlRenderState s_cam(
        ProjectionMatrixRDF_TopLeft(w,h,K.fu,K.fv,K.u0,K.v0,0.1,1000),
        ModelViewLookAtRDF(0,0,-2,0,0,0,0,-1,0)
        );

  Var<bool> reset("ui.reset", true, false);
  Var<bool> run("ui.run", true, true);

  Var<bool> viewonly("ui.view only", true, true);
  Var<bool> fuse("ui.fuse", false, true);
  Var<float> max_rmse("ui.Max RMSE",0.10,0,0.5);
  Var<float> rmse("ui.RMSE",0);

  Var<int> show_level("ui.Show Level", 0, 0, MaxLevels-1);

  // TODO: This needs to be a function of the inverse depth
  Var<int>   biwin("ui.size",3, 1, 20);      // Bilateral filter
  Var<float> bigs("ui.gs",1.5, 1E-3, 5);     // Bilateral filter
  Var<float> icp_c("ui.icp c",0.1, 1E-3, 1);
  Var<float> bigr("ui.gr",0.1, 1E-6, 0.2);   // Bilateral filter
  roo::Image<unsigned char, roo::TargetDevice, roo::Manage> dScratch(w*sizeof(roo::LeastSquaresSystem<float,12>),h);
  Var<float> trunc_dist_factor("ui.trunc vol factor",2, 1, 4);

  Var<float> max_w("ui.max w", 1000, 1E-2, 1E3);
  Var<float> mincostheta("ui.min cos theta", 0.1, 0, 1);
  Var<float> rgb_fl("ui.RGB focal length", 535.7,400,600);

  ActivateDrawPyramid<float,MaxLevels> adrayimg(ray_i, GL_LUMINANCE32F_ARB, true, true);
  ActivateDrawPyramid<float4,MaxLevels> adraycolor(ray_c, GL_RGBA32F, true, true);
  ActivateDrawPyramid<float4,MaxLevels> adraynorm(ray_n, GL_RGBA32F, true, true);
  //    ActivateDrawPyramid<float,MaxLevels> addepth( kin_d, GL_LUMINANCE32F_ARB, false, true);
  ActivateDrawPyramid<float4,MaxLevels> adnormals( kin_n, GL_RGBA32F_ARB, false, true);
  ActivateDrawImage<float4> addebug( dDebug, GL_RGBA32F_ARB, false, true);

  Handler3DDepth<float,roo::TargetDevice> rayhandler(ray_d[0], s_cam, AxisNone);
  SetupContainer(container, 4, (float)w/h);
  container[0].SetDrawFunction(std::ref(adrayimg))
      .SetHandler(&rayhandler);
  container[1].SetDrawFunction(SceneGraph::ActivateDrawFunctor(glgraph, s_cam))
      .SetHandler( new Handler3D(s_cam, AxisNone) );
  container[2].SetDrawFunction(std::ref(use_colour?adraycolor:adraynorm))
      .SetHandler(&rayhandler);
  container[3].SetDrawFunction(std::ref(adnormals));

  float* depthBuffer = (float *) malloc(imageWidth*imageHeight*sizeof(float));
  SceneGraph::ImageView depthView;
  SceneGraph::ImageView currentView;
  container.AddDisplay( depthView );
  container.AddDisplay( currentView );

  Sophus::SE3d T_wl;

  bool bStep = false;
  pangolin::RegisterKeyPressCallback(' ', [&viewonly]() { viewonly=!viewonly;} );
  pangolin::RegisterKeyPressCallback('l', [&vol,&viewonly]() {LoadPXM("save.vol", vol); viewonly = true;} );
  //    pangolin::RegisterKeyPressCallback('s', [&vol,&colorVol,&keyframes,&rgb_fl,w,h]() {SavePXM("save.vol", vol); SaveMeshlab(vol,keyframes,rgb_fl,rgb_fl,w/2,h/2); } );
  pangolin::RegisterKeyPressCallback('s', [&vol,&colorVol]() {roo::SaveMesh("mesh",vol,colorVol); } );
  pangolin::RegisterKeyPressCallback('/', [&]() {bStep = !bStep; } );
  //    pangolin::RegisterKeyPressCallback('s', [&vol]() {SavePXM("save.vol", vol); } );

  Sophus::SE3d current_pose, rel, start, last;
  bool started = false;
  reset = true;
  Eigen::Matrix<double, 3, 1> sdf_trans;
  sdf_trans << 0, 0, 0;
  Sophus::SE3d Tws = Sophus::SE3d(Eigen::Matrix3d::Identity(), sdf_trans);

  bool first;

  for(long frame=-1; !pangolin::ShouldQuit();)
  {

    Capture( &server, "localsim/DepthCam", depthBuffer, rel);
    const bool go = !viewonly && (frame==-1 || run);

    const float trunc_dist = trunc_dist_factor*length(vol.VoxelSizeUnits());

    if(pangolin::Pushed(reset)) {
      T_wl = Sophus::SE3d();
      fuse = false;
      started = false;

      vol.bbox = reset_bb;
      roo::SdfReset(vol, std::numeric_limits<float>::quiet_NaN() );
      keyframes.clear();

      frame = -1;
    }

    if(go || bStep) {
      bStep = false;
      cudaMemcpy(dKinectMeters.ptr, depthBuffer, w*h*sizeof(float), cudaMemcpyHostToDevice);
      roo::BilateralFilter<float,float>(kin_d[0],dKinectMeters,bigs,bigr,biwin,0.2);

      roo::BoxReduceIgnoreInvalid<float,MaxLevels,float>(kin_d);
      for(int l=0; l<MaxLevels; ++l) {
        roo::DepthToVbo<float>(kin_v[l], kin_d[l], K[l] );
        roo::NormalsFromVbo(kin_n[l], kin_v[l]);
      }
    }

    Sophus::SE3d T_vw(s_cam.GetModelViewMatrix());
    if(viewonly) {

      const roo::BoundingBox roi(T_vw.inverse().matrix3x4(), w, h, K, 0, 50);
      roo::BoundedVolume<roo::SDF_t> work_vol = vol.SubBoundingVolume( roi );
      if(work_vol.IsValid()) {
        roo::RaycastSdf(ray_d[0], ray_n[0], ray_i[0], work_vol, T_vw.inverse().matrix3x4(), K, 0.1, 50, trunc_dist, true );

        if(keyframes.size() > 0) {
          // populate kfs
          for( int k=0; k< kfs.Rows(); k++)
          {
            if(k < keyframes.size()) {
              kfs[k].img = keyframes[k]->img;
              kfs[k].T_iw = keyframes[k]->T_iw.matrix3x4();
              kfs[k].K = roo::ImageIntrinsics(rgb_fl, kfs[k].img);
            }else{
              kfs[k].img.ptr = 0;
            }
          }
          roo::TextureDepth<float4,uchar3,10>(ray_c[0], kfs, ray_d[0], ray_n[0], ray_i[0], T_vw.inverse().matrix3x4(), K);
        }
      }
    }else{
      const roo::BoundingBox roi(roo::BoundingBox(T_wl.inverse().matrix3x4(), w, h, K, 0, 50));
      roo::BoundedVolume<roo::SDF_t> work_vol = vol.SubBoundingVolume( roi );
      if(work_vol.IsValid()) {
        for(int l=0; l<MaxLevels; ++l) {
          if(its[l] > 0) {
            const roo::ImageIntrinsics Kl = K[l];
            roo::RaycastSdf(ray_d[l], ray_n[l], ray_i[l], work_vol, T_vw.inverse().matrix3x4(), Kl, knear,kfar, trunc_dist, true );
            roo::DepthToVbo<float>(ray_v[l], ray_d[l], Kl );
          }
        }

        if(fuse) {
          std::cout<<"Fusing . . . "<<std::endl;
          Sophus::SE3d tempPose = start.inverse() * last;
          const roo::BoundingBox roi(tempPose.matrix3x4(), w, h, K, knear,kfar);
          roo::BoundedVolume<roo::SDF_t> work_vol = vol.SubBoundingVolume( roi );
          if(work_vol.IsValid()) {
            const float trunc_dist = trunc_dist_factor*length(vol.VoxelSizeUnits());
            roo::SdfFuse(work_vol, kin_d[0], kin_n[0], tempPose.inverse().matrix3x4(), K, trunc_dist, max_w, mincostheta );
          }
          fuse = false;
        }
      }
    }

    glcamera.SetPose(current_pose.matrix());

    roo::BoundingBox bbox_work(current_pose.matrix3x4(), w, h, K.fu, K.fv, K.u0, K.v0, knear,kfar);
    bbox_work.Intersect(vol.bbox);
    glboxfrustum.SetBounds(roo::ToEigen(bbox_work.Min()), roo::ToEigen(bbox_work.Max()) );

    /////////////////////////////////////////////////////////////
    // Draw
    addebug.SetImage(dDebug.SubImage(0,0,w>>show_level,h>>show_level));
    adnormals.SetLevel(show_level);
    adrayimg.SetLevel(viewonly? 0 : show_level);

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glColor3f(1,1,1);
    pangolin::FinishFrame();
  }
  return 0;
}


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkBoostLogWeighting.cxx

-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "vtkArrayCoordinates.h"
#include "vtkCommand.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkBoostLogWeighting.h"
#include "vtkObjectFactory.h"
#include "vtkSmartPointer.h"
#include "vtkTypedArray.h"

#include <boost/math/special_functions/log1p.hpp>
#include <boost/version.hpp>
#if BOOST_VERSION < 103400
  #error "vtkBoostLogWeighting requires Boost 1.34.0 or later"
#endif

#include <cmath>
#include <stdexcept>

///////////////////////////////////////////////////////////////////////////////
// vtkBoostLogWeighting

vtkStandardNewMacro(vtkBoostLogWeighting);

vtkBoostLogWeighting::vtkBoostLogWeighting() :
  Base(BASE_E),
  EmitProgress(true)
{
}

vtkBoostLogWeighting::~vtkBoostLogWeighting()
{
}

void vtkBoostLogWeighting::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
  os << indent << "Base: " << this->Base << endl;
  os << indent << "EmitProgress: "
     << (this->EmitProgress ? "on" : "off") << endl;
}

int vtkBoostLogWeighting::RequestData(
  vtkInformation*,
  vtkInformationVector** inputVector,
  vtkInformationVector* outputVector)
{
  try
  {
    vtkArrayData* const input_data = vtkArrayData::GetData(inputVector[0]);
    if(!input_data)
      throw std::runtime_error("Missing input vtkArrayData on port 0.");
    if(input_data->GetNumberOfArrays() != 1)
      throw std::runtime_error("Input vtkArrayData must contain exactly one array.");
    vtkTypedArray<double>* const input_array = vtkTypedArray<double>::SafeDownCast(input_data->GetArray(0));
    if(!input_array)
      throw std::runtime_error("Unsupported input array type.");

    vtkTypedArray<double>* const output_array = vtkTypedArray<double>::SafeDownCast(input_array->DeepCopy());
    vtkArrayData* const output = vtkArrayData::GetData(outputVector);
    output->ClearArrays();
    output->AddArray(output_array);
    output_array->Delete();

    const vtkIdType value_count = input_array->GetNonNullSize();
    switch(this->Base)
    {
      case BASE_E:
      {
        if(this->EmitProgress)
        {
          for(vtkIdType i = 0; i != value_count; ++i)
          {
            output_array->SetValueN(i, boost::math::log1p(output_array->GetValueN(i)));

            double progress = static_cast<double>(i) / static_cast<double>(value_count);
            this->InvokeEvent(vtkCommand::ProgressEvent, &progress);
          }
        }
        else
        {
          for(vtkIdType i = 0; i != value_count; ++i)
          {
            output_array->SetValueN(i, boost::math::log1p(output_array->GetValueN(i)));
          }
        }
        break;
      }
      case BASE_2:
      {
        const double ln2 = log(2.0);
        if(this->EmitProgress)
        {
          for(vtkIdType i = 0; i != value_count; ++i)
          {
            output_array->SetValueN(i, 1.0 + log(output_array->GetValueN(i)) / ln2);

            double progress = static_cast<double>(i) / static_cast<double>(value_count);
            this->InvokeEvent(vtkCommand::ProgressEvent, &progress);
          }
        }
        else
        {
          for(vtkIdType i = 0; i != value_count; ++i)
          {
            output_array->SetValueN(i, 1.0 + log(output_array->GetValueN(i)) / ln2);
          }
        }
        break;
      }
      default:
        throw std::runtime_error("Unknown Base type.");
    }
  }
  catch(std::exception& e)
  {
    vtkErrorMacro(<< "unhandled exception: " << e.what());
    return 0;
  }
  catch(...)
  {
    vtkErrorMacro(<< "unknown exception");
    return 0;
  }

  return 1;
}


/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkSparseKernelTransform.txx,v $
  Language:  C++
  Date:      $Date: 2006-11-28 14:22:18 $
  Version:   $Revision: 1.1 $

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#ifndef _itkSparseKernelTransform_txx
#define _itkSparseKernelTransform_txx
#include "itkSparseKernelTransform.h"

// Report timings
#include <itkTimeProbe.h>
#include <itkTimeProbesCollectorBase.h>

#include <Eigen/Sparse>
#include <Eigen/SparseLU>

#include <vector>

namespace itk
{

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
SparseKernelTransform<TScalarType, NDimensions>::
SparseKernelTransform()
  : Transform<TScalarType, NDimensions,NDimensions>(1)
//:Superclass(
                         //   NDimensions,
                           // NDimensions )
  // the second NDimensions is associated is provided as
  // a tentative number for initializing the Jacobian.
  // The matrix can be resized at run time so this number
  // here is irrelevant. The correct size of the Jacobian
  // will be NDimension X NDimension.NumberOfLandMarks.
{

    // m_I.set_identity();
    m_I               = IMatrixType::Identity();
    m_SourceLandmarks = PointSetType::New();
    m_TargetLandmarks = PointSetType::New();
    m_Displacements   = VectorSetType::New();
    m_WMatrixComputed = false;

    m_LMatrixComputed  = false;
    m_LInverseComputed = false;

    m_Stiffness = 0.0;

    Eigen::setNbThreads(8);
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
SparseKernelTransform<TScalarType, NDimensions>::
~SparseKernelTransform()
{
}

template<class TScalarType, unsigned int NDimensions>
inline void SparseKernelTransform<TScalarType,
  NDimensions>::ComputeJacobianWithRespectToParameters(
    const InputPointType & in, JacobianType & jacobian) const {}

template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
SetSourceLandmarks(PointSetType * landmarks)
{
    itkDebugMacro("setting SourceLandmarks to " << landmarks );
    if (this->m_SourceLandmarks != landmarks)
    {
        this->m_SourceLandmarks = landmarks;
        this->UpdateParameters();
        this->Modified();

        // these are invalidated when the source lms change
        m_WMatrixComputed  = false;
        m_LMatrixComputed  = false;
        m_LInverseComputed = false;

        // you must recompute L and Linv - this does not require the targ lms
        // Linverse is only needed ofr Jacobian computation, I will defer this in case Jacobian is needed
        // we will assume by default that this transform is used only for warping, if it is a part of optimization
        // the GetJacobian will feel that the inverse is not computed and will compute it
        //this->ComputeLInverse();
        this->ComputeL();

    }
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
SetTargetLandmarks(PointSetType * landmarks)
{
    itkDebugMacro("setting TargetLandmarks to " << landmarks );
    if (this->m_TargetLandmarks != landmarks)
    {
        this->m_TargetLandmarks = landmarks;
        // this is invalidated when the target lms change
        m_WMatrixComputed=false;
        this->ComputeWMatrix();
        this->UpdateParameters();
        this->Modified();
    }

}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
const typename SparseKernelTransform<TScalarType, NDimensions>::GMatrixType &
SparseKernelTransform<TScalarType, NDimensions>::
ComputeG( const InputVectorType & ) const
{
    //
    // Should an Exception be thrown here  ?
    //
    itkWarningMacro(<< "ComputeG() should be reimplemented in the subclass !!");
    return m_GMatrix;
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
const typename SparseKernelTransform<TScalarType, NDimensions>::GMatrixType &
SparseKernelTransform<TScalarType, NDimensions>::
ComputeReflexiveG( PointsIterator ) const
{
    m_GMatrix = GMatrixType::Zero();
    for(unsigned d = 0; d < NDimensions; d++)
        m_GMatrix(d,d) = m_Stiffness;

    //m_GMatrix.fill( NumericTraits< TScalarType >::Zero );
    //m_GMatrix.fill_diagonal( m_Stiffness );

    return m_GMatrix;
}

/**
 * Default implementation of the the method. This can be overloaded
 * in transforms whose kernel produce diagonal G matrices.
 */
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
ComputeDeformationContribution( const InputPointType  & thisPoint,
                                OutputPointType & result     ) const
{

    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();

    PointsIterator sp  = m_SourceLandmarks->GetPoints()->Begin();

    for(unsigned int lnd=0; lnd < numberOfLandmarks; lnd++ )
    {
        const GMatrixType & Gmatrix = ComputeG( thisPoint - sp->Value() );
        for(unsigned int dim=0; dim < NDimensions; dim++ )
        {
            for(unsigned int odim=0; odim < NDimensions; odim++ )
            {
                result[ odim ] += Gmatrix(dim, odim ) * m_DMatrix(dim,lnd);
            }
        }
        ++sp;
    }

}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>
::ComputeD(void) const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();

    PointsIterator sp  = m_SourceLandmarks->GetPoints()->Begin();
    PointsIterator tp  = m_TargetLandmarks->GetPoints()->Begin();
    PointsIterator end = m_SourceLandmarks->GetPoints()->End();

    m_Displacements->Reserve( numberOfLandmarks );
    typename VectorSetType::Iterator vt = m_Displacements->Begin();

    while( sp != end )
    {
        vt->Value() = tp->Value() - sp->Value();
        vt++;
        sp++;
        tp++;
    }
    //	std::cout<<" Computed displacements "<<m_Displacements<<std::endl;
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>
::ComputeWMatrix(void) const
{
    itk::TimeProbe clock;

    //	std::cout<<"Computing W matrix"<<std::endl;

    //typedef vnl_svd<TScalarType>  SVDSolverType;

    if(!m_LMatrixComputed) {
        this->ComputeL();
    }
    this->ComputeY();

    //SVDSolverType svd( m_LMatrix, 1e-8 );
    //m_WMatrix = svd.solve( m_YMatrix );

    clock.Start();

    //Eigen::BiCGSTAB<LMatrixType>  solver;
    Eigen::BiCGSTAB<LMatrixType, Eigen::IncompleteLUT<double> >  solver;
    solver.preconditioner().setDroptol(1e-10);
    solver.preconditioner().setFillfactor(1000);

    //    Eigen::SparseLU<LMatrixType> solver;
    solver.compute(m_LMatrix);

    if(solver.info()!= Eigen::Success) {
        // decomposition failed
        std::cerr << "LMatrix failed to decompose ...!" << std::endl;
        return;
    }

    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();
    m_WMatrix = WMatrixType::Zero(NDimensions*(numberOfLandmarks+NDimensions+1), 1);
    m_WMatrix = solver.solve(m_YMatrix);

    //    m_WMatrix = WMatrixType::Random(NDimensions*(numberOfLandmarks+NDimensions+1), 1);
    //    solver.setMaxIterations(1);
    //    int i = 0;
    //    do {
    //        m_WMatrix = solver.solveWithGuess(m_YMatrix,m_WMatrix);
    //        std::cout << "#iteration: " << i << " " << "estimated error: " << solver.error() << std::endl;
    //        ++i;
    //    } while (solver.info()!= Eigen::Success && i<100);

    // std::cout << "#iterations: " << solver.iterations() << std::endl;
    // std::cout << "estimated error: " << solver.error() << std::endl;

    std::cout  << solver.error() << std::endl;
    if(solver.info() != Eigen::Success) {
        // solving failed
        std::cerr << "solving sparse system failed ...!" << std::endl;
        return;
    }

    clock.Stop();
    // std::cout << "Computing Wmatrix:" << std::endl;
    // std::cout << "Mean: " << clock.GetMean() << std::endl;
    // std::cout << "Total: " << clock.GetTotal() << std::endl;

    this->ReorganizeW();
    m_WMatrixComputed=true;
}

/**
 * postponed till needing the jacobian for this class
 */
//template <class TScalarType, unsigned int NDimensions>
//void SparseKernelTransform<TScalarType, NDimensions>::
//ComputeLInverse(void) const
//{
//    // Assumes that L has already been computed
//    // Necessary for the jacobian
//    if(!m_LMatrixComputed) {
//        this->ComputeL();
//    }
//    //std::cout<<"LMatrix is:"<<std::endl;
//    //std::cout<<m_LMatrix<<std::endl;

//    itk::TimeProbesCollectorBase timeCollector;
//    if (0){
//        timeCollector.Start( "ComputeLInverse" );
//        m_LMatrixInverse=vnl_matrix_inverse<TScalarType> (m_LMatrix);
//        timeCollector.Stop( "ComputeLInverse" );
//    }

//    // Convert to sparse matrix
//    // Because of the special storage scheme of a SparseMatrix, special care has to be taken when adding new nonzero entries.
//    // For instance, the cost of a single purely random insertion into a SparseMatrix is O(nnz),
//    // where nnz is the current number of non-zero coefficients.
//    // The simplest way to create a sparse matrix while guaranteeing good performance is thus to first build a list of
//    // so-called triplets, and then convert it to a SparseMatrix.
//    typedef Eigen::SparseMatrix<ScalarType> SpMat; // declares a column-major sparse matrix type of double
//    typedef Eigen::Triplet<ScalarType> Triplet;
//    SpMat A(m_LMatrix.rows(),m_LMatrix.cols());

//    std::vector<Triplet> tripletList;
//    for ( unsigned int r = 0; r < m_LMatrix.rows(); r++ )
//        for ( unsigned int c = 0; c < m_LMatrix.cols(); c++ )
//        {
//            ScalarType val = m_LMatrix.get( r, c );
//            if ( val != 0 )
//                tripletList.push_back(Triplet(r,c,val));
//        }
//    A.setFromTriplets(tripletList.begin(), tripletList.end());
//    timeCollector.Stop( "ConvertLToSparse" );

//    // Method 4: LU Decomposition
//    // Depends on local ITK vnl_sparse_lu modification
//    timeCollector.Start( "ComputeLSparseInverse" );
//    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();
//    //    vnl_sparse_symmetric_eigensystem eigSys;
//    //    eigSys.CalculateNPairs(lSparseMatrix,NDimensions*(numberOfLandmarks+NDimensions+1), false);
//    //    //LSparseMatrixType lMatrixInverse = vnl_sparse_lu( lSparseMatrix ).inverse();
//    //    timeCollector.Stop( "ComputeLSparseInverse" );

//    m_LInverseComputed=true;
//    //std::cout<<"LMatrix inverse is:"<<std::endl;
//    //std::cout<<m_LMatrixInverse<<std::endl;
//}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>::
ComputeL(void) const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();
    //vnl_matrix<TScalarType> O2(NDimensions*(NDimensions+1),
    //                           NDimensions*(NDimensions+1), 0);

    this->ComputeP();
    this->ComputeK();

    //m_LMatrix.set_size( NDimensions*(numberOfLandmarks+NDimensions+1),
    //                    NDimensions*(numberOfLandmarks+NDimensions+1) );
    //m_LMatrix.fill( 0.0 );
    m_LMatrix = LMatrixType( NDimensions*(numberOfLandmarks+NDimensions+1),
                             NDimensions*(numberOfLandmarks+NDimensions+1) );

    //std::vector<TripletType> tripletList;

    // putting KMATRIX
    //m_LMatrix.update( m_KMatrix, 0, 0 );
    //it.value();
    //it.row(); // row index
    //it.col(); // col index (here it is equal to k)
    //it.index(); // inner index, here it is equal to it.row()
    for (int k = 0; k < m_KMatrix.outerSize(); ++k) // column index
        for (typename KMatrixType::InnerIterator it(m_KMatrix,k); it; ++it)
            m_LMatrix.insert(it.row(), it.col()) = it.value() ;
    //tripletList.push_back( TripletType ( it.row(), it.col(), it.value() ) );

    // putting PMATRIX - will keep the lower only ??
    //m_LMatrix.update( m_PMatrix, 0, m_KMatrix.columns() );
    //m_LMatrix.update( m_PMatrix.transpose(), m_KMatrix.rows(), 0);
    for (int p = 0; p < m_PMatrix.outerSize(); ++p) // column index
        for (typename PMatrixType::InnerIterator it(m_PMatrix,p); it; ++it)
        {
            // fill P -> upper
            m_LMatrix.insert(it.row(), NDimensions*numberOfLandmarks + it.col()) = it.value() ;
            //tripletList.push_back( TripletType ( it.row(), NDimensions*numberOfLandmarks + it.col(), it.value() ) );

            // fill P.transpose -> lower
            m_LMatrix.insert(NDimensions*numberOfLandmarks + it.col(),  it.row()) = it.value() ;
            //tripletList.push_back( TripletType ( NDimensions*numberOfLandmarks + it.col(),  it.row(), it.value() ) );
        }
    //m_LMatrix.update( O2, m_KMatrix.rows(), m_KMatrix.columns());

    //    // shireen: for debugging let's make sure that the L matrix is sparse (based on gaussian basis)
    //    std::ofstream ofs;
    //    ofs.open("Lsparse.csv");
    //    for (int k = 0; k < m_LMatrix.outerSize(); ++k) // column index
    //        for (typename LMatrixType::InnerIterator it(m_LMatrix,k); it; ++it)
    //            ofs << it.row()  << ", " << it.col()  << ", " << it.value() << std::endl;
    //    ofs.close();

    // std::cout << "Lmatrix - nnz = "  << m_LMatrix.nonZeros() << std::endl;
    m_LMatrix.makeCompressed();

    //m_LMatrix.setFromTriplets(tripletList.begin(), tripletList.end());
    m_LMatrixComputed=1;
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>::
ComputeK(void) const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();
    GMatrixType G;
    //std::vector<TripletType> tripletList;

    m_KMatrix = KMatrixType( NDimensions * numberOfLandmarks,
                             NDimensions * numberOfLandmarks );
    //m_KMatrix.set_size( NDimensions * numberOfLandmarks,
    //                    NDimensions * numberOfLandmarks );

    //m_KMatrix.fill( 0.0 );

    PointsIterator p1  = m_SourceLandmarks->GetPoints()->Begin();
    PointsIterator end = m_SourceLandmarks->GetPoints()->End();

    // K matrix is symmetric, so only evaluate the upper triangle and
    // store the values in both the upper and lower triangle
    unsigned int i = 0;
    while( p1 != end )
    {
        PointsIterator p2 = p1; // start at the diagonal element
        unsigned int j = i;

        // Compute the block diagonal element, i.e. kernel for pi->pi
        //G = ComputeReflexiveG(p1);

        // force to compute the basis on the diagonal
        const InputVectorType s = p1.Value() - p1.Value();
        G = ComputeG(s); // the basis

        //m_KMatrix.update(G, i*NDimensions, i*NDimensions);
        for(unsigned int d = 0; d < NDimensions; d++)
        {
            if(G(d,d) != 0 )
                m_KMatrix.insert(i*NDimensions+d ,i*NDimensions+d) = G(d,d) + m_Stiffness; // this is as a regularizer
            //tripletList.push_back(TripletType(i*NDimensions+d ,i*NDimensions+d, m_GMatrix(d,d)));
        }

        p2++;
        j++;

        // Compute the upper (and copy into lower) triangular part of K
        // only save the lower part, don't need it
        while( p2 != end )
        {
            const InputVectorType s = p1.Value() - p2.Value();
            G = ComputeG(s); // the basis

            // write value in upper and lower triangle of matrix
            for(unsigned int ii = 0 ; ii < NDimensions; ii++)
                for(unsigned int jj = 0 ; jj < NDimensions; jj++)
                {
                    if (G(ii,jj) != 0)
                    {
                        m_KMatrix.insert(i*NDimensions+ii ,j*NDimensions+jj) = G(ii,jj); // upper
                        m_KMatrix.insert(j*NDimensions+ii ,i*NDimensions+jj) = G(ii,jj); // lower
                    }

                    //tripletList.push_back(TripletType(i*NDimensions+ii ,j*NDimensions+jj, G(ii,jj)));
                    //tripletList.push_back(TripletType(j*NDimensions+ii ,i*NDimensions+jj, G(ii,jj)));
                }

            // m_KMatrix.update(G, i*NDimensions, j*NDimensions);
            // m_KMatrix.update(G, j*NDimensions, i*NDimensions);
            p2++;
            j++;
        }
        p1++;
        i++;
    }
    //std::cout<<"K matrix: "<<std::endl;
    //std::cout<<m_KMatrix<<std::endl;

    //    // shireen: for debugging let's make sure that the L matrix is sparse (based on gaussian basis)
    //    std::ofstream ofs;
    //    ofs.open("Ksparse.csv");
    //    for (int k = 0; k < m_KMatrix.outerSize(); ++k) // column index
    //        for (typename KMatrixType::InnerIterator it(m_KMatrix,k); it; ++it)
    //            ofs << it.row()  << ", " << it.col()  << ", " << it.value() << std::endl;
    //    ofs.close();

    // std::cout << "Kmatrix - nnz = "  << m_KMatrix.nonZeros() << std::endl;
    m_KMatrix.makeCompressed();

    //m_KMatrix.setFromTriplets(tripletList.begin(), tripletList.end());
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>::
ComputeP() const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();

    //IMatrixType I = IMatrixType::Identity();
    //IMatrixType temp;
    InputPointType p;

    //I.set_identity();

    //std::vector<TripletType> tripletList;

    //m_PMatrix.set_size( NDimensions*numberOfLandmarks,
    //                    NDimensions*(NDimensions+1) );
    //m_PMatrix.fill( 0.0 );
    m_PMatrix = PMatrixType(NDimensions*numberOfLandmarks,
                            NDimensions*(NDimensions+1) );
    for (unsigned int i = 0; i < numberOfLandmarks; i++)
    {
        m_SourceLandmarks->GetPoint(i, &p);
        for (unsigned int j = 0; j < NDimensions; j++)
        {
            //temp = I * p[j];
            for(unsigned int d = 0 ; d < NDimensions; d++)
            {
                m_PMatrix.insert(i*NDimensions + d, j*NDimensions + d) = p[j];
                //tripletList.push_back( TripletType( i*NDimensions + d, j*NDimensions + d, p[j] ) );
            }
            //m_PMatrix.update(temp, i*NDimensions, j*NDimensions);
        }

        for(unsigned int d = 0 ; d < NDimensions; d++)
        {
            m_PMatrix.insert(i*NDimensions + d, NDimensions*NDimensions + d) = 1;
            //tripletList.push_back( TripletType( i*NDimensions + d, NDimensions*NDimensions + d, 1 ) );
        }
        //m_PMatrix.update(I, i*NDimensions, NDimensions*NDimensions);
    }

    // std::cout << "Pmatrix - nnz = "  << m_PMatrix.nonZeros() << std::endl;
    m_PMatrix.makeCompressed();
    //m_PMatrix.setFromTriplets(tripletList.begin(), tripletList.end());
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>::
ComputeY(void) const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();

    this->ComputeD();

    typename VectorSetType::ConstIterator displacement =
            m_Displacements->Begin();

    //m_YMatrix.set_size( NDimensions*(numberOfLandmarks+NDimensions+1), 1);
    //m_YMatrix.fill( 0.0 );
    m_YMatrix = YMatrixType::Zero(NDimensions*(numberOfLandmarks+NDimensions+1), 1);

    for (unsigned int i = 0; i < numberOfLandmarks; i++)
    {
        for (unsigned int j = 0; j < NDimensions; j++)
        {
            m_YMatrix(i*NDimensions+j, 0) = displacement.Value()[j];
            //m_YMatrix.put(i*NDimensions+j, 0, displacement.Value()[j]);
        }
        displacement++;
    }

    for (unsigned int i = 0; i < NDimensions*(NDimensions+1); i++)
    {
        m_YMatrix(numberOfLandmarks*NDimensions+i, 0) = 0;
        //m_YMatrix.put(numberOfLandmarks*NDimensions+i, 0, 0);
    }
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>
::ReorganizeW(void) const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();

    // The deformable (non-affine) part of the registration goes here
    m_DMatrix = DMatrixType::Zero(NDimensions,numberOfLandmarks);
    //m_DMatrix.set_size(NDimensions,numberOfLandmarks);

    unsigned int ci = 0;
    for(unsigned int lnd=0; lnd < numberOfLandmarks; lnd++ )
    {
        for(unsigned int dim=0; dim < NDimensions; dim++ )
        {
            //std::cout << m_WMatrix(ci,0) << std::endl;
            m_DMatrix(dim,lnd) = m_WMatrix(ci++,0);
        }
    }

    // This matrix holds the rotational part of the Affine component
    m_AMatrix = AMatrixType::Zero(NDimensions, NDimensions);
    for(unsigned int j=0; j < NDimensions; j++ )
    {
        for(unsigned int i=0; i < NDimensions; i++ )
        {
            m_AMatrix(i,j) = m_WMatrix(ci++,0);
        }
    }

    // This vector holds the translational part of the Affine component
    m_BVector = BMatrixType::Zero(NDimensions,1);
    for(unsigned int k=0; k < NDimensions; k++ )
    {
        m_BVector(k) = m_WMatrix(ci++,0);
    }

    // release WMatrix memory by assigning a small one.
    m_WMatrix = WMatrixType(1,1);

    m_WMatrixComputed=1;

    //    std::ofstream ofs;
    //    ofs.open("D.csv");
    //    for (int k = 0; k < m_DMatrix.outerSize(); ++k) // column index
    //        for (typename DMatrixType::InnerIterator it(m_DMatrix,k); it; ++it)
    //            ofs << it.row()  << ", " << it.col()  << ", " << it.value() << std::endl;
    //    ofs.close();
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
typename SparseKernelTransform<TScalarType, NDimensions>::OutputPointType
SparseKernelTransform<TScalarType, NDimensions>
::TransformPoint(const InputPointType& thisPoint) const
{

    OutputPointType result;

    typedef typename OutputPointType::ValueType ValueType;

    result.Fill( NumericTraits< ValueType >::Zero );

    this->ComputeDeformationContribution( thisPoint, result );

    // Add the rotational part of the Affine component
    for(unsigned int j=0; j < NDimensions; j++ )
    {
        for(unsigned int i=0; i < NDimensions; i++ )
        {
            result[i] += m_AMatrix(i,j) * thisPoint[j];
        }
    }

    // This vector holds the translational part of the Affine component
    for(unsigned int k=0; k < NDimensions; k++ )
    {
        result[k] += m_BVector(k) + thisPoint[k];
    }

    return result;

}

//// Compute the Jacobian in one position - POSTPONED
//template <class TScalarType, unsigned int NDimensions>
//const typename SparseKernelTransform<TScalarType,NDimensions>::JacobianType &
//SparseKernelTransform< TScalarType,NDimensions>::
//GetJacobian( const InputPointType & thisPoint) const
//{
//    if(!m_LInverseComputed) {
//        this->ComputeLInverse();
//    }
//    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();
//    Superclass::m_Jacobian.SetSize(NDimensions, numberOfLandmarks*NDimensions);
//    Superclass::m_Jacobian.Fill( 0.0 );

//    PointsIterator sp  = m_SourceLandmarks->GetPoints()->Begin();
//    for(unsigned int lnd=0; lnd < numberOfLandmarks; lnd++ )
//    {
//        const GMatrixType & Gmatrix = ComputeG( thisPoint - sp->Value() );
//        ///std::cout<<"G for landmark "<<lnd<<std::endl<<Gmatrix<<std::endl;
//        for(unsigned int dim=0; dim < NDimensions; dim++ )
//        {
//            for(unsigned int odim=0; odim < NDimensions; odim++ )
//            {
//                for(unsigned int lidx=0; lidx < numberOfLandmarks*NDimensions; lidx++ )
//                {
//                    Superclass::m_Jacobian[ odim ] [lidx] += Gmatrix(dim, odim ) *
//                            m_LMatrixInverse[lnd*NDimensions+dim][lidx];
//                }
//            }
//        }
//        ++sp;

//    }
//    for(unsigned int odim=0; odim < NDimensions; odim++ )
//    {
//        for(unsigned int lidx=0; lidx < numberOfLandmarks*NDimensions; lidx++ )
//        {
//            for(unsigned int dim=0; dim < NDimensions; dim++ )
//            {
//                Superclass::m_Jacobian[ odim ] [lidx] += thisPoint[dim] *
//                        m_LMatrixInverse[(numberOfLandmarks+dim)*NDimensions+odim][lidx];
//            }

//            Superclass::m_Jacobian[ odim ] [lidx] += m_LMatrixInverse[(numberOfLandmarks+NDimensions)*NDimensions+odim][lidx];
//        }
//    }

//    return Superclass::m_Jacobian;

//}

// Set to the identity transform - ie make the  Source and target lm the same
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
SetIdentity()
{
    this->SetParameters(this->GetFixedParameters());
}

// Set the parameters
// NOTE that in this transformation both the Source and Target
// landmarks could be considered as parameters. It is assumed
// here that the Target landmarks are provided by the user and
// are not changed during the optimization process required for
// registration.
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
SetParameters( const ParametersType & parameters )
{
    //	std::cout<<"Setting parameters to "<<parameters<<std::endl;
    typename PointsContainer::Pointer landmarks = PointsContainer::New();
    const unsigned int numberOfLandmarks =  parameters.Size() / NDimensions;
    landmarks->Reserve( numberOfLandmarks );

    PointsIterator itr = landmarks->Begin();
    PointsIterator end = landmarks->End();

    InputPointType  landMark;

    unsigned int pcounter = 0;
    while( itr != end )
    {
        for(unsigned int dim=0; dim<NDimensions; dim++)
        {
            landMark[ dim ] = parameters[ pcounter ];
            pcounter++;
        }
        itr.Value() = landMark;
        itr++;
    }

    // m_SourceLandmarks->SetPoints( landmarks );
    m_TargetLandmarks->SetPoints( landmarks );

    // W MUST be recomputed if the target lms are set
    this->ComputeWMatrix();

    //  if(!m_LInverseComputed) {
    //  this->ComputeLInverse();
    //  }

    // Modified is always called since we just have a pointer to the
    // parameters and cannot know if the parameters have changed.
    this->Modified();

}

// Set the fixed parameters
// Since the API of the SetParameters() function sets the
// source landmarks, this function was added to support the
// setting of the target landmarks, and allowing the Transform
// I/O mechanism to be supported.
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
SetFixedParameters( const ParametersType & parameters )
{
    typename PointsContainer::Pointer landmarks = PointsContainer::New();
    const unsigned int numberOfLandmarks =  parameters.Size() / NDimensions;

    landmarks->Reserve( numberOfLandmarks );

    PointsIterator itr = landmarks->Begin();
    PointsIterator end = landmarks->End();

    InputPointType  landMark;

    unsigned int pcounter = 0;
    while( itr != end )
    {
        for(unsigned int dim=0; dim<NDimensions; dim++)
        {
            landMark[ dim ] = parameters[ pcounter ];
            pcounter++;
        }
        itr.Value() = landMark;
        itr++;
    }

    //  m_TargetLandmarks->SetPoints( landmarks );
    m_SourceLandmarks->SetPoints( landmarks );

    // these are invalidated when the source lms change
    m_WMatrixComputed=false;
    m_LMatrixComputed=false;
    m_LInverseComputed=false;

    // you must recompute L and Linv - this does not require the targ lms
    //this->ComputeLInverse();
    this->ComputeL();

}

// Update parameters array
// They are the components of all the landmarks in the source space
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
UpdateParameters( void ) const
{
    this->m_Parameters = ParametersType( m_TargetLandmarks->GetNumberOfPoints() * NDimensions );

    PointsIterator itr = m_TargetLandmarks->GetPoints()->Begin();
    PointsIterator end = m_TargetLandmarks->GetPoints()->End();

    unsigned int pcounter = 0;
    while( itr != end )
    {
        InputPointType  landmark = itr.Value();
        for(unsigned int dim=0; dim<NDimensions; dim++)
        {
            this->m_Parameters[ pcounter ] = landmark[ dim ];
            pcounter++;
        }
        itr++;
    }
}

// Get the parameters
// They are the components of all the landmarks in the source space
template <class TScalarType, unsigned int NDimensions>
const typename SparseKernelTransform<TScalarType, NDimensions>::ParametersType &
SparseKernelTransform<TScalarType, NDimensions>::
GetParameters( void ) const
{
    this->UpdateParameters();
    return this->m_Parameters;

}

// Get the fixed parameters
// This returns the target landmark locations
// This was added to support the Transform Reader/Writer mechanism
template <class TScalarType, unsigned int NDimensions>
const typename SparseKernelTransform<TScalarType, NDimensions>::ParametersType &
SparseKernelTransform<TScalarType, NDimensions>::
GetFixedParameters( void ) const
{
    this->m_FixedParameters = ParametersType( m_SourceLandmarks->GetNumberOfPoints() * NDimensions );

    PointsIterator itr = m_SourceLandmarks->GetPoints()->Begin();
    PointsIterator end = m_SourceLandmarks->GetPoints()->End();

    unsigned int pcounter = 0;
    while( itr != end )
    {
        InputPointType  landmark = itr.Value();
        for(unsigned int dim=0; dim<NDimensions; dim++)
        {
            this->m_FixedParameters[ pcounter ] = landmark[ dim ];
            pcounter++;
        }
        itr++;
    }

    return this->m_FixedParameters;

}

template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
PrintSelf(std::ostream& os, Indent indent) const
{
    Superclass::PrintSelf(os,indent);
    if (m_SourceLandmarks)
    {
        os << indent << "SourceLandmarks: " << std::endl;
        m_SourceLandmarks->Print(os,indent.GetNextIndent());
    }
    if (m_TargetLandmarks)
    {
        os << indent << "TargetLandmarks: " << std::endl;
        m_TargetLandmarks->Print(os,indent.GetNextIndent());
    }
    if (m_Displacements)
    {
        os << indent << "Displacements: " << std::endl;
        m_Displacements->Print(os,indent.GetNextIndent());
    }
    os << indent << "Stiffness: " << m_Stiffness << std::endl;
}
} // namespace itk

#endif


#include "gilbreth_msgs/ObjectVoxel.h"
#include "std_msgs/Int32MultiArray.h"
#include "std_msgs/MultiArrayDimension.h"
#include <XmlRpcException.h>
#include <array>
#include <boost/dynamic_bitset.hpp>
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <iomanip> // setprecision
#include <iostream>
#include <pcl/common/common.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <string>
#include <vector>

// Algorithm params
static const unsigned int VOXEL_GRID_SIZE = 32;
static const unsigned int VOXEL_SQUARE = std::pow(VOXEL_GRID_SIZE,2);
static const unsigned int VOXEL_CUBE = std::pow(VOXEL_GRID_SIZE,3);

ros::Publisher pub;

//pcd to voxel functions
struct Voxel {
  unsigned int x;
  unsigned int y;
  unsigned int z;
  Voxel() : x(0), y(0), z(0){};
  Voxel(const unsigned int _x, const unsigned int _y, const unsigned int _z) : x(_x), y(_y), z(_z){};
};

template <typename PointT>
const bool loadPointCloud(const std::string &file_path,
                          typename pcl::PointCloud<PointT> &cloud_out) {
  if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_path.c_str(), cloud_out) == -1) {
    PCL_ERROR("Failed to load PCD file \n");
    return false;
  }

  return true;
}

// Get the linear index into a 1D array of voxels,
// for a voxel at location (ix, iy, iz).
const unsigned int getLinearIndex(const Voxel &voxel, const int grid_size) {
  return voxel.x * (grid_size * grid_size) + voxel.z * grid_size + voxel.y;
}

const Voxel getGridIndex(const pcl::PointXYZ &point, const pcl::PointXYZ &translate, const uint VOXEL_GRID_SIZE, const float scale) {
  // Needs to be signed to prevent overflow, because index can
  // be slightly negative which then gets rounded to zero.
  const int i = std::round(static_cast<float>(VOXEL_GRID_SIZE) * ((point.x - translate.x) / scale) - 0.5);
  const int j = std::round(static_cast<float>(VOXEL_GRID_SIZE) * ((point.y - translate.y) / scale) - 0.5);
  const int k = std::round(static_cast<float>(VOXEL_GRID_SIZE) * ((point.z - translate.z) / scale) - 0.5);
  return Voxel(i, j, k);
}

// Format a float number to ensure it always has at least one decimal place
// 0 --> 0.0
// 1.1 --> 1.1
// 1.10 --> 1.1
const std::string formatFloat(const float value) {
  std::stringstream ss;
  ss << std::setprecision(6) << std::fixed << value;
  std::string str;
  ss.str().swap(str);
  size_t last_zero_idx = str.find_last_not_of("0") + 1;
  if (last_zero_idx == str.length()) {
    // No trailing zeros
    return str;
  }
  if (str[last_zero_idx - 1] == '.') {
    // Last zero is after decimal point
    last_zero_idx += 1;
  }
  str.resize(last_zero_idx);
  return str;
}

void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) {
  pcl::PointCloud<pcl::PointXYZ>::Ptr scene(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::fromROSMsg(*cloud_msg, *scene);
  //create and publish voxel
  ros::Time start_time = ros::Time::now();
  pcl::PointXYZ min_point;
  pcl::PointXYZ max_point;
  pcl::getMinMax3D(*scene, min_point, max_point);
  // Calculate the scale factor so the longest side of the volume
  // is split into the desired number of voxels
  const float x_range = max_point.x - min_point.x;
  const float y_range = max_point.y - min_point.y;
  const float z_range = max_point.z - min_point.z;

  const float max_cloud_extent = std::max(std::max(x_range, y_range), z_range);
  const float voxel_size = max_cloud_extent / (static_cast<float>(VOXEL_GRID_SIZE) - 1.0);
  ROS_INFO("voxel size is %f", voxel_size);

  const float scale = (static_cast<float>(VOXEL_GRID_SIZE) * max_cloud_extent) / (static_cast<float>(VOXEL_GRID_SIZE) - 1.0);

  // Calculate the PointCloud's translation from the origin.
  // We need to subtract half the voxel size, because points
  // are located in the center of the voxel grid.
  float tx = min_point.x - voxel_size / 2.0;
  float ty = min_point.y - voxel_size / 2.0;
  float tz = min_point.z - voxel_size / 2.0;
  // Hack, change -0.0 to 0.0
  const float epsilon = 0.0000001;
  if ((tx > -epsilon) && (tx < 0.0)) {
    tx = -1.0 * tx;
  }
  if ((ty > -epsilon) && (ty < 0.0)) {
    ty = -1.0 * ty;
  }
  if ((tz > -epsilon) && (tz < 0.0)) {
    tz = -1.0 * tz;
  }
  const pcl::PointXYZ translate(tx, ty, tz);

  // Voxelize the PointCloud into a linear array
  boost::dynamic_bitset<> voxels_bitset(VOXEL_CUBE);
  for (pcl::PointCloud<pcl::PointXYZ>::iterator it = scene->begin(); it != scene->end(); ++it) {
    const Voxel voxel = getGridIndex(*it, translate, VOXEL_GRID_SIZE, scale);
    const unsigned int idx = getLinearIndex(voxel, VOXEL_GRID_SIZE);
    voxels_bitset[idx] = 1;
  }
  gilbreth_msgs::ObjectVoxel voxel_data;
  std_msgs::Int32MultiArray dat;
  // fill voxel into message:
  dat.layout.dim.push_back(std_msgs::MultiArrayDimension());
  dat.layout.dim.push_back(std_msgs::MultiArrayDimension());
  dat.layout.dim.push_back(std_msgs::MultiArrayDimension());
  dat.layout.dim[0].label = "x";
  dat.layout.dim[1].label = "y";
  dat.layout.dim[2].label = "z";
  dat.layout.dim[0].size = VOXEL_GRID_SIZE;
  dat.layout.dim[1].size = VOXEL_GRID_SIZE;
  dat.layout.dim[2].size = VOXEL_GRID_SIZE;
  dat.layout.dim[0].stride = VOXEL_CUBE;
  dat.layout.dim[1].stride = VOXEL_SQUARE;
  dat.layout.dim[1].stride = VOXEL_GRID_SIZE;
  dat.layout.data_offset = 0;
  std::vector<int> vec(VOXEL_CUBE, 0);
  for (int i = 0; i < VOXEL_CUBE; i++)
    vec[i] = voxels_bitset[i];
  dat.data = vec;
  voxel_data.voxel = dat;
  voxel_data.header.stamp = ros::Time::now();
  voxel_data.detection_time = cloud_msg->header.stamp;
  //fill pcd into msg
  sensor_msgs::PointCloud2 pcd_output;
  pcl::toROSMsg(*scene, pcd_output);
  voxel_data.pcd = pcd_output;
  //publish
  pub.publish(voxel_data);
  double elapsed_time = (ros::Time::now() - start_time).toSec();
  ROS_INFO("covert PCD to voxel time is %f seconds", elapsed_time);
}

int main(int argc, char **argv) {
  // define PCD file subscribed
  // Initialize ROS
  ros::init(argc, argv, "voxelizer_node");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub_1 = nh.subscribe<sensor_msgs::PointCloud2>("segmentation_result", 1, cloudCb);
  // ROS publisher
  pub = nh.advertise<gilbreth_msgs::ObjectVoxel>("voxel_data", 10);
  ROS_INFO("Voxelizer Node subscribed to %s", pub.getTopic().c_str());
  ROS_INFO("Voxelizer Node Ready ...");
  // Spin

  ros::spin();
}


/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/

#include <Eigen/Eigen>
#include <cmath>
#include <iostream>
#include <plan_env/raycast.h>

int signum(int x) {
  return x == 0 ? 0 : x < 0 ? -1 : 1;
}

double mod(double value, double modulus) {
  return fmod(fmod(value, modulus) + modulus, modulus);
}

double intbound(double s, double ds) {
  // Find the smallest positive t such that s+t*ds is an integer.
  if (ds < 0) {
    return intbound(-s, -ds);
  } else {
    s = mod(s, 1);
    // problem is now s+t*ds = 1
    return (1 - s) / ds;
  }
}

void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
             const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output) {
  //    std::cout << start << ' ' << end << std::endl;
  // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
  // by John Amanatides and Andrew Woo, 1987
  // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
  // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
  // Extensions to the described algorithm:
  //   • Imposed a distance limit.
  //   • The face passed through to reach the current cube is provided to
  //     the callback.

  // The foundation of this algorithm is a parameterized representation of
  // the provided ray,
  //                    origin + t * direction,
  // except that t is not actually stored; rather, at any given point in the
  // traversal, we keep track of the *greater* t values which we would have
  // if we took a step sufficient to cross a cube boundary along that axis
  // (i.e. change the integer part of the coordinate) in the variables
  // tMaxX, tMaxY, and tMaxZ.

  // Cube containing origin point.
  int x = (int)std::floor(start.x());
  int y = (int)std::floor(start.y());
  int z = (int)std::floor(start.z());
  int endX = (int)std::floor(end.x());
  int endY = (int)std::floor(end.y());
  int endZ = (int)std::floor(end.z());
  Eigen::Vector3d direction = (end - start);
  double maxDist = direction.squaredNorm();

  // Break out direction vector.
  double dx = endX - x;
  double dy = endY - y;
  double dz = endZ - z;

  // Direction to increment x,y,z when stepping.
  int stepX = (int)signum((int)dx);
  int stepY = (int)signum((int)dy);
  int stepZ = (int)signum((int)dz);

  // See description above. The initial values depend on the fractional
  // part of the origin.
  double tMaxX = intbound(start.x(), dx);
  double tMaxY = intbound(start.y(), dy);
  double tMaxZ = intbound(start.z(), dz);

  // The change in t when taking a step (always positive).
  double tDeltaX = ((double)stepX) / dx;
  double tDeltaY = ((double)stepY) / dy;
  double tDeltaZ = ((double)stepZ) / dz;

  // Avoids an infinite loop.
  if (stepX == 0 && stepY == 0 && stepZ == 0) return;

  double dist = 0;
  while (true) {
    if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) {
      output[output_points_cnt](0) = x;
      output[output_points_cnt](1) = y;
      output[output_points_cnt](2) = z;

      output_points_cnt++;
      dist = sqrt((x - start(0)) * (x - start(0)) + (y - start(1)) * (y - start(1)) +
                  (z - start(2)) * (z - start(2)));

      if (dist > maxDist) return;

      /*            if (output_points_cnt > 1500) {
                      std::cerr << "Error, too many racyast voxels." <<
         std::endl;
                      throw std::out_of_range("Too many raycast voxels");
                  }*/
    }

    if (x == endX && y == endY && z == endZ) break;

    // tMaxX stores the t-value at which we cross a cube boundary along the
    // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
    // chooses the closest cube boundary. Only the first case of the four
    // has been commented in detail.
    if (tMaxX < tMaxY) {
      if (tMaxX < tMaxZ) {
        // Update which cube we are now in.
        x += stepX;
        // Adjust tMaxX to the next X-oriented boundary crossing.
        tMaxX += tDeltaX;
      } else {
        z += stepZ;
        tMaxZ += tDeltaZ;
      }
    } else {
      if (tMaxY < tMaxZ) {
        y += stepY;
        tMaxY += tDeltaY;
      } else {
        z += stepZ;
        tMaxZ += tDeltaZ;
      }
    }
  }
}

void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
             const Eigen::Vector3d& max, std::vector<Eigen::Vector3d>* output) {
  //    std::cout << start << ' ' << end << std::endl;
  // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
  // by John Amanatides and Andrew Woo, 1987
  // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
  // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
  // Extensions to the described algorithm:
  //   • Imposed a distance limit.
  //   • The face passed through to reach the current cube is provided to
  //     the callback.

  // The foundation of this algorithm is a parameterized representation of
  // the provided ray,
  //                    origin + t * direction,
  // except that t is not actually stored; rather, at any given point in the
  // traversal, we keep track of the *greater* t values which we would have
  // if we took a step sufficient to cross a cube boundary along that axis
  // (i.e. change the integer part of the coordinate) in the variables
  // tMaxX, tMaxY, and tMaxZ.

  // Cube containing origin point.
  int x = (int)std::floor(start.x());
  int y = (int)std::floor(start.y());
  int z = (int)std::floor(start.z());
  int endX = (int)std::floor(end.x());
  int endY = (int)std::floor(end.y());
  int endZ = (int)std::floor(end.z());
  Eigen::Vector3d direction = (end - start);
  double maxDist = direction.squaredNorm();

  // Break out direction vector.
  double dx = endX - x;
  double dy = endY - y;
  double dz = endZ - z;

  // Direction to increment x,y,z when stepping.
  int stepX = (int)signum((int)dx);
  int stepY = (int)signum((int)dy);
  int stepZ = (int)signum((int)dz);

  // See description above. The initial values depend on the fractional
  // part of the origin.
  double tMaxX = intbound(start.x(), dx);
  double tMaxY = intbound(start.y(), dy);
  double tMaxZ = intbound(start.z(), dz);

  // The change in t when taking a step (always positive).
  double tDeltaX = ((double)stepX) / dx;
  double tDeltaY = ((double)stepY) / dy;
  double tDeltaZ = ((double)stepZ) / dz;

  output->clear();

  // Avoids an infinite loop.
  if (stepX == 0 && stepY == 0 && stepZ == 0) return;

  double dist = 0;
  while (true) {
    if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) {
      output->push_back(Eigen::Vector3d(x, y, z));

      dist = (Eigen::Vector3d(x, y, z) - start).squaredNorm();

      if (dist > maxDist) return;

      if (output->size() > 1500) {
        std::cerr << "Error, too many racyast voxels." << std::endl;
        throw std::out_of_range("Too many raycast voxels");
      }
    }

    if (x == endX && y == endY && z == endZ) break;

    // tMaxX stores the t-value at which we cross a cube boundary along the
    // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
    // chooses the closest cube boundary. Only the first case of the four
    // has been commented in detail.
    if (tMaxX < tMaxY) {
      if (tMaxX < tMaxZ) {
        // Update which cube we are now in.
        x += stepX;
        // Adjust tMaxX to the next X-oriented boundary crossing.
        tMaxX += tDeltaX;
      } else {
        z += stepZ;
        tMaxZ += tDeltaZ;
      }
    } else {
      if (tMaxY < tMaxZ) {
        y += stepY;
        tMaxY += tDeltaY;
      } else {
        z += stepZ;
        tMaxZ += tDeltaZ;
      }
    }
  }
}

bool RayCaster::setInput(const Eigen::Vector3d& start,
                         const Eigen::Vector3d& end /* , const Eigen::Vector3d& min,
                         const Eigen::Vector3d& max */) {
  start_ = start;
  end_ = end;
  // max_ = max;
  // min_ = min;

  x_ = (int)std::floor(start_.x());
  y_ = (int)std::floor(start_.y());
  z_ = (int)std::floor(start_.z());
  endX_ = (int)std::floor(end_.x());
  endY_ = (int)std::floor(end_.y());
  endZ_ = (int)std::floor(end_.z());
  direction_ = (end_ - start_);
  maxDist_ = direction_.squaredNorm();

  // Break out direction vector.
  dx_ = endX_ - x_;
  dy_ = endY_ - y_;
  dz_ = endZ_ - z_;

  // Direction to increment x,y,z when stepping.
  stepX_ = (int)signum((int)dx_);
  stepY_ = (int)signum((int)dy_);
  stepZ_ = (int)signum((int)dz_);

  // See description above. The initial values depend on the fractional
  // part of the origin.
  tMaxX_ = intbound(start_.x(), dx_);
  tMaxY_ = intbound(start_.y(), dy_);
  tMaxZ_ = intbound(start_.z(), dz_);

  // The change in t when taking a step (always positive).
  tDeltaX_ = ((double)stepX_) / dx_;
  tDeltaY_ = ((double)stepY_) / dy_;
  tDeltaZ_ = ((double)stepZ_) / dz_;

  dist_ = 0;

  step_num_ = 0;

  // Avoids an infinite loop.
  if (stepX_ == 0 && stepY_ == 0 && stepZ_ == 0)
    return false;
  else
    return true;
}

bool RayCaster::step(Eigen::Vector3d& ray_pt) {
  // if (x_ >= min_.x() && x_ < max_.x() && y_ >= min_.y() && y_ < max_.y() &&
  // z_ >= min_.z() && z_ <
  // max_.z())
  ray_pt = Eigen::Vector3d(x_, y_, z_);

  // step_num_++;

  // dist_ = (Eigen::Vector3d(x_, y_, z_) - start_).squaredNorm();

  if (x_ == endX_ && y_ == endY_ && z_ == endZ_) {
    return false;
  }

  // if (dist_ > maxDist_)
  // {
  //   return false;
  // }

  // tMaxX stores the t-value at which we cross a cube boundary along the
  // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
  // chooses the closest cube boundary. Only the first case of the four
  // has been commented in detail.
  if (tMaxX_ < tMaxY_) {
    if (tMaxX_ < tMaxZ_) {
      // Update which cube we are now in.
      x_ += stepX_;
      // Adjust tMaxX to the next X-oriented boundary crossing.
      tMaxX_ += tDeltaX_;
    } else {
      z_ += stepZ_;
      tMaxZ_ += tDeltaZ_;
    }
  } else {
    if (tMaxY_ < tMaxZ_) {
      y_ += stepY_;
      tMaxY_ += tDeltaY_;
    } else {
      z_ += stepZ_;
      tMaxZ_ += tDeltaZ_;
    }
  }

  return true;
}


/*
 * TreeIkSolverVel_wdls.hpp
 *
 *  Created on: Nov 28, 2008
 *      Author: rubensmits
 */

#ifndef TREEIKSOLVERVEL_WDLS_HPP_
#define TREEIKSOLVERVEL_WDLS_HPP_

#include "treeiksolver.hpp"
#include "treejnttojacsolver.hpp"
#include <Eigen/Core>

namespace KDL {

    class TreeIkSolverVel_wdls: public TreeIkSolverVel {
    public:
        static const int E_SVD_FAILED = -100; //! Child SVD failed

        TreeIkSolverVel_wdls(const Tree& tree, const std::vector<std::string>& endpoints);
        virtual ~TreeIkSolverVel_wdls();

        virtual double CartToJnt(const JntArray& q_in, const Twists& v_in, JntArray& qdot_out);

        /*
         * Set the joint space weighting matrix
         *
         * @param weight_js joint space weighting symmetric matrix,
         * default : identity.  M_q : This matrix being used as a
         * weight for the norm of the joint space speed it HAS TO BE
         * symmetric and positive definite. We can actually deal with
         * matrices containing a symmetric and positive definite block
         * and 0s otherwise. Taking a diagonal matrix as an example, a
         * 0 on the diagonal means that the corresponding joints will
         * not contribute to the motion of the system. On the other
         * hand, the bigger the value, the most the corresponding
         * joint will contribute to the overall motion. The obtained
         * solution q_dot will actually minimize the weighted norm
         * sqrt(q_dot'*(M_q^-2)*q_dot). In the special case we deal
         * with, it does not make sense to invert M_q but what is
         * important is the physical meaning of all this : a joint
         * that has a zero weight in M_q will not contribute to the
         * motion of the system and this is equivalent to saying that
         * it gets an infinite weight in the norm computation.  For
         * more detailed explanation : vincent.padois@upmc.fr
         */
        void setWeightJS(const Eigen::MatrixXd& Mq);
        const Eigen::MatrixXd& getWeightJS() const {return Wq;}

        /*
         * Set the task space weighting matrix
         *
         * @param weight_ts task space weighting symmetric matrix,
         * default: identity M_x : This matrix being used as a weight
         * for the norm of the error (in terms of task space speed) it
         * HAS TO BE symmetric and positive definite. We can actually
         * deal with matrices containing a symmetric and positive
         * definite block and 0s otherwise. Taking a diagonal matrix
         * as an example, a 0 on the diagonal means that the
         * corresponding task coordinate will not be taken into
         * account (ie the corresponding error can be really big). If
         * the rank of the jacobian is equal to the number of task
         * space coordinates which do not have a 0 weight in M_x, the
         * weighting will actually not impact the results (ie there is
         * an exact solution to the velocity inverse kinematics
         * problem). In cases without an exact solution, the bigger
         * the value, the most the corresponding task coordinate will
         * be taken into account (ie the more the corresponding error
         * will be reduced). The obtained solution will minimize the
         * weighted norm sqrt(|x_dot-Jq_dot|'*(M_x^2)*|x_dot-Jq_dot|).
         * For more detailed explanation : vincent.padois@upmc.fr
         */
        void setWeightTS(const Eigen::MatrixXd& Mx);
        const Eigen::MatrixXd& getWeightTS() const {return Wy;}

        void setLambda(const double& lambda);
        double getLambda () const {return lambda;}

    private:
        Tree tree;
        TreeJntToJacSolver jnttojacsolver;
        Jacobians jacobians;

        Eigen::MatrixXd J, Wy, Wq, J_Wq, Wy_J_Wq, U, V, Wy_U, Wq_V;
        Eigen::VectorXd t, Wy_t, qdot, tmp, S;
        double lambda;
    };

}

#endif /* TREEIKSOLVERVEL_WDLS_HPP_ */


// written by bastiaan konings schuiling 2008 - 2014
// this work is public domain. the code is undocumented, scruffy, untested, and should generally not be used for anything important.
// i do not offer support, so don't ask. to be used for inspiration :)

#include "bluntmath.hpp"

#include <cmath>

#include <boost/random.hpp>

#include <boost/thread/mutex.hpp>

namespace blunted {

  unsigned int fastrandseed;
  unsigned int max_uint;

  boost::mutex randMutex;

  typedef boost::mt19937 BaseGenerator;
  typedef boost::uniform_real<float> Distribution;
  typedef boost::variate_generator<BaseGenerator, Distribution> Generator;
  BaseGenerator base;
  Distribution dist;
  Generator rng(base, dist);

  real clamp(const real value, const real min, const real max) {
    assert(max >= min);
    if (min > value) return min;
    if (max < value) return max;
    return value;
  }

  real NormalizedClamp(const real value, const real min, const real max) {
    assert(max > min);
    real banana = clamp(value, min, max);
    banana = (banana - min) / (max - min);
    return banana;
  }

  real invsqrt(real fvalue) {
    return 1. / sqrt(fvalue);
  }

  float dot_product(real v1[3], real v2[3]) {
    return (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]);
  }

  void normalize(real v[3]) {
    real f = 1.0f / sqrt(dot_product(v, v));

    v[0] *= f;
    v[1] *= f;
    v[2] *= f;
  }

  bool sign(real n) {
    return n >= 0;
  }

  signed int signSide(real n) {
    return n >= 0 ? 1 : -1;
  }

  bool is_odd(int n) {
    return n & 1;
  }

  void randomseed() {
    rng.engine().seed(static_cast<unsigned int>(std::time(0)));
  }

  inline real boostrandom() {
    return rng();
  }

  real random(real min, real max) {
    float stretch = max - min;

    randMutex.lock();
    real value = min + (boostrandom() * stretch);
    randMutex.unlock();
    return value;
  }

  int pot(int x) {
    int val = 1;
    while (val < x) {
      val *= 2;
    }
    return val;
  }

  real ModulateIntoRange(real min, real max, real value) {
    real step = max - min;
    real newValue = value;
    while (newValue < min) newValue += step;
    while (newValue > max) newValue -= step;
    return newValue;
  }

}


//
// Expansion Hunter
// Copyright 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Sai Chen <schen6@illumina.com>
//         Egor Dolzhenko <edolzhenko@illumina.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//

#include "genotyping/SmallVariantGenotyper.hh"
#include <boost/math/distributions/poisson.hpp>
#include <numeric>

using boost::optional;
using boost::math::poisson_distribution;
using std::vector;

namespace ehunter
{

boost::optional<SmallVariantGenotype> SmallVariantGenotyper::genotype(int refCount, int altCount) const
{
    if (expectedAlleleCount_ > 2)
    {
        throw std::runtime_error(
            "Ploidy = " + std::to_string(expectedAlleleCount_) + ", but only ploidy <=2 is supported at this time.");
    }

    if (refCount < 0 || altCount < 0)
    {
        throw std::runtime_error("Invalid read counts: " + std::to_string(refCount) + " " + std::to_string(altCount));
    }

    auto possibleGenotypes = getPossibleGenotypes(expectedAlleleCount_);

    const int totalReadCount = refCount + altCount;
    if (totalReadCount == 0) // missing genotype
    {
        return optional<SmallVariantGenotype>();
    }

    optional<SmallVariantGenotype> mostLikelyGenotype;
    double bestLikelihood = -std::numeric_limits<double>::max();
    for (const auto& currentGenotype : possibleGenotypes)
    {
        const double currentLikelihood = genotypeLikelihood(currentGenotype, refCount, altCount);
        if (currentLikelihood > bestLikelihood)
        {
            bestLikelihood = currentLikelihood;
            mostLikelyGenotype = currentGenotype;
        }
    }

    return mostLikelyGenotype;
}

vector<SmallVariantGenotype> SmallVariantGenotyper::getPossibleGenotypes(int numAlleles) const
{
    if (numAlleles == 1)
    {
        return { AlleleType::kRef, AlleleType::kAlt };
    }
    else if (numAlleles == 2)
    {
        return { { AlleleType::kRef, AlleleType::kRef },
                 { AlleleType::kRef, AlleleType::kAlt },
                 { AlleleType::kAlt, AlleleType::kAlt } };
    }
    else
    {
        throw std::runtime_error("The given number of alleles is not supported: " + std::to_string(numAlleles));
    }
}

double
SmallVariantGenotyper::genotypeLikelihood(const SmallVariantGenotype& currentGenotype, int refCount, int altCount) const
{
    const poisson_distribution<> errorDistribution(errorRate_);

    const bool isHomozygous = currentGenotype.isHomRef() || currentGenotype.isHomAlt();
    const int copyNumberOfExistingAllele = isHomozygous ? 2 : 1;
    const poisson_distribution<> countDistribution(copyNumberOfExistingAllele * haplotypeDepth_);

    double genotypeLikelihood
        = currentGenotype.isHomRef() ? log(pdf(errorDistribution, altCount)) : log(pdf(countDistribution, altCount));

    genotypeLikelihood
        += currentGenotype.isHomAlt() ? log(pdf(errorDistribution, refCount)) : log(pdf(countDistribution, refCount));

    if (std::isinf(genotypeLikelihood))
    {
        genotypeLikelihood = -std::numeric_limits<double>::max();
    }

    return genotypeLikelihood;
}

}


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

// includes
// std
#include <iostream>

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

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

// RBDyn
#include "RBDyn/Joint.h"

const double TOL = 1e-10;

void testRevolute(rbd::Joint::OldType type, const Eigen::Vector3d & axis, bool forward)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace constants = boost::math::constants;

  Joint j(type, forward, "rev");
  double dir = forward ? 1. : -1;

  // test motion
  Vector6d S;
  S << dir * axis, 0., 0., 0.;

  // test motion
  MotionVecd motion(S);

  // test motion
  PTransformd rot90(AngleAxisd(-constants::pi<double>() / 2., dir * axis).matrix());

  // test accessor
  BOOST_CHECK_EQUAL(j.type(), Joint::Rev);
  BOOST_CHECK_EQUAL(j.params(), 1);
  BOOST_CHECK_EQUAL(j.dof(), 1);
  BOOST_CHECK_EQUAL(j.name(), "rev");
  BOOST_CHECK_EQUAL(j.motionSubspace(), S);

  // test zero
  std::vector<double> zeroP = {0.};
  std::vector<double> zeroD = {0.};
  std::vector<double> zp = j.zeroParam();
  std::vector<double> zd = j.zeroDof();
  BOOST_CHECK_EQUAL_COLLECTIONS(zp.begin(), zp.end(), zeroP.begin(), zeroP.end());
  BOOST_CHECK_EQUAL_COLLECTIONS(zd.begin(), zd.end(), zeroD.begin(), zeroD.end());

  BOOST_CHECK_EQUAL(j.pose<double>({constants::pi<double>() / 2.}), rot90);

  BOOST_CHECK_EQUAL(j.motion({2.}).vector(), (2. * motion).vector());
}

void testPrismatic(rbd::Joint::OldType type, const Eigen::Vector3d & axis, bool forward)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace constants = boost::math::constants;

  Joint j(type, forward, "prism");
  double dir = forward ? 1. : -1;

  // test motion
  Vector6d S;
  S << 0., 0., 0., dir * axis;

  // test motion
  MotionVecd motion(S);

  // test motion
  PTransformd trans2(Vector3d(dir * axis * 2.));

  // test accessor
  BOOST_CHECK_EQUAL(j.type(), Joint::Prism);
  BOOST_CHECK_EQUAL(j.params(), 1);
  BOOST_CHECK_EQUAL(j.dof(), 1);
  BOOST_CHECK_EQUAL(j.name(), "prism");
  BOOST_CHECK_EQUAL(j.motionSubspace(), S);

  // test zero
  std::vector<double> zeroP = {0.};
  std::vector<double> zeroD = {0.};
  std::vector<double> zp = j.zeroParam();
  std::vector<double> zd = j.zeroDof();
  BOOST_CHECK_EQUAL_COLLECTIONS(zp.begin(), zp.end(), zeroP.begin(), zeroP.end());
  BOOST_CHECK_EQUAL_COLLECTIONS(zd.begin(), zd.end(), zeroD.begin(), zeroD.end());

  // test motion
  BOOST_CHECK_EQUAL(j.pose<double>({2.}), trans2);

  // test motion
  BOOST_CHECK_EQUAL(j.motion({2.}).vector(), (2. * motion).vector());
}

BOOST_AUTO_TEST_CASE(JointTest)
{
  using namespace rbd;

  // test operator==
  Joint j1(Joint::RevX, true, "j1");
  Joint j2(Joint::RevX, false, "j2");

  BOOST_CHECK_EQUAL(j1, j1);
  BOOST_CHECK_NE(j1, j2);

  // Test operator!=
  BOOST_CHECK(!(j1 != j1));
  BOOST_CHECK(j1 != j2);

  // Test direction
  BOOST_CHECK_EQUAL(j1.direction(), 1.);
  BOOST_CHECK_EQUAL(j2.direction(), -1.);

  // Test forward (getter)
  BOOST_CHECK_EQUAL(j1.forward(), true);
  BOOST_CHECK_EQUAL(j2.forward(), false);

  // Test forward (setter)
  j1.forward(false);
  BOOST_CHECK_EQUAL(j1.direction(), -1.);
  BOOST_CHECK_EQUAL(j1.forward(), false);

  // sPose
  BOOST_CHECK_THROW(j1.sPose({0., 0.}), std::domain_error);
  BOOST_CHECK_THROW(j1.sPose({}), std::domain_error);
  BOOST_CHECK_NO_THROW(j1.sPose({0.}));
  BOOST_CHECK_EQUAL(j1.sPose({0.}), j1.pose<double>({0.}));

  // sMotion
  BOOST_CHECK_THROW(j1.sMotion({0., 0.}), std::domain_error);
  BOOST_CHECK_THROW(j1.sMotion({}), std::domain_error);
  BOOST_CHECK_NO_THROW(j1.sMotion({0.}));
  BOOST_CHECK_EQUAL(j1.sMotion({0.}), j1.motion({0.}));
}

BOOST_AUTO_TEST_CASE(RevXTest)
{
  using namespace Eigen;
  using namespace rbd;
  testRevolute(Joint::RevX, Vector3d::UnitX(), true);
  testRevolute(Joint::RevX, Vector3d::UnitX(), false);
}

BOOST_AUTO_TEST_CASE(RevYTest)
{
  using namespace Eigen;
  using namespace rbd;
  testRevolute(Joint::RevY, Vector3d::UnitY(), true);
  testRevolute(Joint::RevY, Vector3d::UnitY(), false);
}

BOOST_AUTO_TEST_CASE(RevZTest)
{
  using namespace Eigen;
  using namespace rbd;
  testRevolute(Joint::RevZ, Vector3d::UnitZ(), true);
  testRevolute(Joint::RevZ, Vector3d::UnitZ(), false);
}

BOOST_AUTO_TEST_CASE(PrismXTest)
{
  using namespace Eigen;
  using namespace rbd;
  testPrismatic(Joint::PrismX, Vector3d::UnitX(), true);
  testPrismatic(Joint::PrismX, Vector3d::UnitX(), false);
}

BOOST_AUTO_TEST_CASE(PrismYTest)
{
  using namespace Eigen;
  using namespace rbd;
  testPrismatic(Joint::PrismY, Vector3d::UnitY(), true);
  testPrismatic(Joint::PrismY, Vector3d::UnitY(), false);
}

BOOST_AUTO_TEST_CASE(PrismZTest)
{
  using namespace Eigen;
  using namespace rbd;
  testPrismatic(Joint::PrismZ, Vector3d::UnitZ(), true);
  testPrismatic(Joint::PrismZ, Vector3d::UnitZ(), false);
}

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

  Joint j(Joint::Spherical, true, "sphere");

  // subspace data
  MatrixXd S = MatrixXd::Zero(6, 3);
  S.block(0, 0, 3, 3).setIdentity();

  // pose data
  double rotX = constants::pi<double>() / 2.;
  double rotY = constants::pi<double>() / 4.;
  double rotZ = constants::pi<double>();

  Quaterniond quat =
      AngleAxisd(rotX, Vector3d::UnitX()) * AngleAxisd(rotY, Vector3d::UnitY()) * AngleAxisd(rotZ, Vector3d::UnitZ());

  std::vector<double> q = {quat.w(), quat.x(), quat.y(), quat.z()};

  PTransformd rot(quat.inverse());

  // motion data
  std::vector<double> alpha;
  Vector3d alphaE = Vector3d::Random();
  for(int i = 0; i < 3; ++i) alpha.push_back(alphaE(i));

  // test accessor
  BOOST_CHECK_EQUAL(j.type(), Joint::Spherical);
  BOOST_CHECK_EQUAL(j.params(), 4);
  BOOST_CHECK_EQUAL(j.dof(), 3);
  BOOST_CHECK_EQUAL(j.name(), "sphere");
  BOOST_CHECK_EQUAL(j.motionSubspace(), S);

  // test zero
  std::vector<double> zeroP = {1., 0., 0., 0.};
  std::vector<double> zeroD = {0., 0., 0.};
  std::vector<double> zp = j.zeroParam();
  std::vector<double> zd = j.zeroDof();
  BOOST_CHECK_EQUAL_COLLECTIONS(zp.begin(), zp.end(), zeroP.begin(), zeroP.end());
  BOOST_CHECK_EQUAL_COLLECTIONS(zd.begin(), zd.end(), zeroD.begin(), zeroD.end());

  // test pose
#ifdef __i386__
  BOOST_CHECK_SMALL((j.pose(q).matrix() - rot.matrix()).array().abs().sum(), TOL);
#else
  BOOST_CHECK_EQUAL(j.pose(q), rot);
#endif

  // test motion
  BOOST_CHECK_EQUAL(j.motion(alpha).vector(), S * alphaE);

  // test inverse polarity
  j.forward(false);
  BOOST_CHECK_EQUAL(j.motionSubspace(), -S);
#ifdef __i386__
  BOOST_CHECK_SMALL((j.pose(q).matrix() - rot.inv().matrix()).array().abs().sum(), TOL);
#else
  BOOST_CHECK_EQUAL(j.pose(q), rot.inv());
#endif
  BOOST_CHECK_EQUAL(j.motion(alpha).vector(), -S * alphaE);
}

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

  Joint j(Joint::Planar, true, "planar");

  // subspace data
  MatrixXd S = MatrixXd::Zero(6, 3);
  S.block(2, 0, 3, 3).setIdentity();

  // pose data
  double rotZ = constants::pi<double>() / 2.;
  double transX = 4.;
  double transY = 0.3;

  sva::PTransformd trans(RotZ(rotZ), RotZ(rotZ).transpose() * Vector3d(transX, transY, 0.));

  std::vector<double> q = {rotZ, transX, transY};

  // motion data
  std::vector<double> alpha;
  Vector3d alphaE = Vector3d::Random();
  for(int i = 0; i < 3; ++i) alpha.push_back(alphaE(i));

  // test accessor
  BOOST_CHECK_EQUAL(j.type(), Joint::Planar);
  BOOST_CHECK_EQUAL(j.params(), 3);
  BOOST_CHECK_EQUAL(j.dof(), 3);
  BOOST_CHECK_EQUAL(j.name(), "planar");
  BOOST_CHECK_EQUAL(j.motionSubspace(), S);

  // test zero
  std::vector<double> zeroP = {0., 0., 0.};
  std::vector<double> zeroD = {0., 0., 0.};
  std::vector<double> zp = j.zeroParam();
  std::vector<double> zd = j.zeroDof();
  BOOST_CHECK_EQUAL_COLLECTIONS(zp.begin(), zp.end(), zeroP.begin(), zeroP.end());
  BOOST_CHECK_EQUAL_COLLECTIONS(zd.begin(), zd.end(), zeroD.begin(), zeroD.end());

  // test pose
  BOOST_CHECK_EQUAL(j.pose(q), trans);

  // test motion
  BOOST_CHECK_EQUAL(j.motion(alpha).vector(), S * alphaE);

  // test inverse polarity
  j.forward(false);
  BOOST_CHECK_EQUAL(j.motionSubspace(), -S);
  BOOST_CHECK_EQUAL(j.pose(q), trans.inv());
  BOOST_CHECK_EQUAL(j.motion(alpha).vector(), -S * alphaE);
}

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

  Vector3d axis(Vector3d::Random().normalized());
  Joint j(Joint::Cylindrical, axis, true, "cylindrical");

  // subspace data
  MatrixXd S = MatrixXd::Zero(6, 2);
  S.col(0).head<3>() = axis;
  S.col(1).tail<3>() = axis;

  // pose data
  double rot = constants::pi<double>() / 2.;
  double trans = 5.;

  sva::PTransformd X(AngleAxisd(-rot, axis).matrix(), axis * trans);

  std::vector<double> q = {rot, trans};

  // motion data
  std::vector<double> alpha;
  Vector2d alphaE = Vector2d::Random();
  for(int i = 0; i < 2; ++i) alpha.push_back(alphaE(i));

  // test accessor
  BOOST_CHECK_EQUAL(j.type(), Joint::Cylindrical);
  BOOST_CHECK_EQUAL(j.params(), 2);
  BOOST_CHECK_EQUAL(j.dof(), 2);
  BOOST_CHECK_EQUAL(j.name(), "cylindrical");
  BOOST_CHECK_EQUAL(j.motionSubspace(), S);

  // test zero
  std::vector<double> zeroP = {0., 0.};
  std::vector<double> zeroD = {0., 0.};
  std::vector<double> zp = j.zeroParam();
  std::vector<double> zd = j.zeroDof();
  BOOST_CHECK_EQUAL_COLLECTIONS(zp.begin(), zp.end(), zeroP.begin(), zeroP.end());
  BOOST_CHECK_EQUAL_COLLECTIONS(zd.begin(), zd.end(), zeroD.begin(), zeroD.end());

  // test pose
  BOOST_CHECK_EQUAL(j.pose(q), X);

  // test motion
  BOOST_CHECK_EQUAL(j.motion(alpha).vector(), S * alphaE);

  // test inverse polarity
  j.forward(false);
  BOOST_CHECK_EQUAL(j.motionSubspace(), -S);
  BOOST_CHECK_SMALL((j.pose(q).matrix() - X.inv().matrix()).norm(), TOL);
  BOOST_CHECK_EQUAL(j.motion(alpha).vector(), -S * alphaE);
}

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

  Joint j(Joint::Free, true, "free");

  // subspace data
  MatrixXd S = MatrixXd::Identity(6, 6);

  // pose data
  double rotX = constants::pi<double>() / 2.;
  double rotY = constants::pi<double>() / 4.;
  double rotZ = constants::pi<double>();

  Quaterniond quat =
      AngleAxisd(rotX, Vector3d::UnitX()) * AngleAxisd(rotY, Vector3d::UnitY()) * AngleAxisd(rotZ, Vector3d::UnitZ());
  Vector3d trans = Vector3d::Random();

  std::vector<double> q = {quat.w(), quat.x(), quat.y(), quat.z(), trans.x(), trans.y(), trans.z()};

  PTransformd rot(quat.matrix().transpose(), trans);

  // motion data
  std::vector<double> alpha;
  Vector6d alphaE = Vector6d::Random();
  for(int i = 0; i < 6; ++i) alpha.push_back(alphaE(i));

  // test accessor
  BOOST_CHECK_EQUAL(j.type(), Joint::Free);
  BOOST_CHECK_EQUAL(j.params(), 7);
  BOOST_CHECK_EQUAL(j.dof(), 6);
  BOOST_CHECK_EQUAL(j.name(), "free");
  BOOST_CHECK_EQUAL(j.motionSubspace(), S);

  // test zero
  std::vector<double> zeroP = {1., 0., 0., 0., 0., 0., 0.};
  std::vector<double> zeroD = {0., 0., 0., 0., 0., 0.};
  std::vector<double> zp = j.zeroParam();
  std::vector<double> zd = j.zeroDof();
  BOOST_CHECK_EQUAL_COLLECTIONS(zp.begin(), zp.end(), zeroP.begin(), zeroP.end());
  BOOST_CHECK_EQUAL_COLLECTIONS(zd.begin(), zd.end(), zeroD.begin(), zeroD.end());

  // test pose
  BOOST_CHECK_SMALL((j.pose(q).matrix() - rot.matrix()).norm(), TOL);

  // test motion
  BOOST_CHECK_EQUAL(j.motion(alpha).vector(), S * alphaE);

  // test inverse polarity
  j.forward(false);
  BOOST_CHECK_EQUAL(j.motionSubspace(), -S);
  BOOST_CHECK_SMALL((j.pose(q).matrix() - rot.inv().matrix()).norm(), TOL);
  BOOST_CHECK_EQUAL(j.motion(alpha).vector(), -S * alphaE);
}

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

  Joint j(Joint::Fixed, true, "fixed");

  // subspace data
  MatrixXd S = MatrixXd::Zero(6, 0);

  // test accessor
  BOOST_CHECK_EQUAL(j.type(), Joint::Fixed);
  BOOST_CHECK_EQUAL(j.params(), 0);
  BOOST_CHECK_EQUAL(j.dof(), 0);
  BOOST_CHECK_EQUAL(j.name(), "fixed");
  BOOST_CHECK_EQUAL(j.motionSubspace(), S);

  // test pose
  BOOST_CHECK_EQUAL(j.pose<double>({}), PTransformd::Identity());

  // test motion
  BOOST_CHECK_EQUAL(j.motion({}).vector(), Vector6d::Zero());
}