//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_PROD_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_PROD_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <boost/simd/operator/functions/multiplies.hpp>
#include <boost/simd/constant/constants/one.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief prod generic tag

     Represents the prod function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct prod_ : ext::reduction_<prod_, tag::multiplies_, tag::One>
    {
      /// @brief Parent hierarchy
      typedef ext::reduction_<prod_, tag::multiplies_, tag::One> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_prod_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::prod_, Site> dispatching_prod_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::prod_, Site>();
   }
   template<class... Args>
   struct impl_prod_;
  }
  /*!
    Returns the product of the elements of the SIMD vector

    @par Semantic:

    For every parameter of type T0

    @code
    scalar<T0> r = prod(a0);
    @endcode

    is similar to:

    @code
    scalar<T0> r = One;
    for(result_type i = 0; i != cardinal_of<T0>; ++i)
      r *= a0[i];
    @endcode

    @param a0

    @return a value of the scalar type associated to the parameter
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::prod_, prod, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::prod_, prod, 2)
} }
#endif


#include <clpoly/clpoly.hh>
#include <boost/container_hash/hash.hpp>
#include <iostream>
#include <fstream>
#include <string>
#include <time.h>
clpoly::polynomial_ZZ read_file(std::string s)
{
    long numn,numv;
    std::ifstream fin(s);
    if (!fin)
    {
        // std::cout<<"无法读取文件!\n";
        throw "无法读取文件!";
    }
    fin>>numn>>numv;
    std::vector<clpoly::variable> var;
    var.reserve(numv);
    for (char i='a';i<'a'+numv;var.push_back(std::string(1,i)),++i);
    std::vector<std::pair<clpoly::variable,int64_t>> m;
    std::vector<std::pair<clpoly::monomial,clpoly::ZZ>> p;
    p.reserve(numn);
    long tmp;
    for (long i=0;i<numn;++i)
    {
        m.reserve(numv);
        for (auto &j:var)
        {
            fin>>tmp;
            if (tmp!=0)
                m.push_back(std::pair<clpoly::variable,int64_t>(j,tmp));
        }
        fin>>tmp;
        if (tmp!=0)
            p.push_back(std::pair<clpoly::monomial,clpoly::ZZ>(std::move(m),clpoly::ZZ(tmp)));
    }
    fin.close();
    return clpoly::polynomial_ZZ(p);
}
int main(){
    clpoly::variable x("x");
    clpoly::variable y("y");
    clpoly::variable z("z");
    clpoly::variable d("d");
    clpoly::variable r("r");
    clpoly::variable x1("x1"),x2("x2"),x3("x3");
    clpoly::variable x7("x7"),x8("x8");
    clpoly::polynomial_ZZ p={{{{x,1}},1}};
    clpoly::lex_<clpoly::custom_var_order> mo;
    mo=clpoly::lex_<clpoly::custom_var_order>(clpoly::custom_var_order({x8,x7,x1}));
    clpoly::polynomial_<clpoly::ZZ,clpoly::lex_<clpoly::custom_var_order>> p_1(&mo);
    std::cout<<"p="<< p<<std::endl;
    p=1;
    p=2*(2-x1)*x8+x7-2;
    std::cout<<"p="<< p<<std::endl;
    p_1=p;
    std::cout<<"p_1="<< p_1<<std::endl;

    clpoly::monomial m=pow(x,3);
    p=z*z;
    time_t t;
    std::cout<< p<<std::endl;

    std::cout<< 2*x*x*y+1<<std::endl;
    // auto l=p.variables();
    // for (auto &i:l)
    //     std::cout<<i.first<<":"<<i.second<<" ";
    // std::cout<<std::endl;
    // clpoly::univariate_priority_order comp_z(z);
    // clpoly::polynomial_<clpoly::ZZ,clpoly::univariate_priority_order> p2(&comp_z);
    // clpoly::poly_convert(std::move(p),p2);
    // std::cout<<p2<<std::endl;
    // l=p2.variables();
    // for (auto &i:l)
    //     std::cout<<i.first<<":"<<i.second<<" ";
    // std::cout<<std::endl;
    //clpoly::__is_monomial_compression=true;
    // std::cout<<pow(p2,2)<<std::endl;
    // std::cout<<"4*pow(x,4)*pow(y,2)*pow(z,4)+4*pow(x,2)*pow(y,2)*d^3*pow(z,4)+pow(y,2)*d^6*pow(z,4)+4*pow(x,2)*y*pow(z,2)+2*y*d^3*pow(z,2)+1\n";
    // std::cout<< p<<std::endl;
    clpoly::polynomial_ZZ f=x*pow(y,2);
    clpoly::polynomial_ZZ g=2*pow(y,3)-pow(y,2)+pow(x,2)*y;

    f=2*(2-7*x1+pow(x1,2)*x2)-(x3-x1);
    std::cout<<"f:="<<f<<":"<<std::endl;
    f=-pow(x,2)*pow(z,3) - pow(x,4) - pow(z,4) + pow(x,2) + 2*pow(z,2) - 1;
    g=-pow(r,2)*pow(x,2) + pow(x,4) + pow(x,2)*pow(z,2) + pow(z,4) +x- 2*pow(z,2) + 1;
    // clpoly::polynomial_ZZ f=clpoly::random_polynomial<clpoly::ZZ>({x,y,z,d},10,0.2,10,-10);
    // clpoly::polynomial_ZZ g=clpoly::random_polynomial<clpoly::ZZ>({x,y,z,d},10,0.2,10,-10);
    clpoly::polynomial_ZZ o;
     clpoly::polynomial_ZZ o1;
    std::cout<<"f:="<<f<<":"<<std::endl;
    std::cout<<"g:="<<g<<":"<<std::endl;
    std::cout<<clpoly::coeff(g,z)<<std::endl;
    // std::cout<<"o:="<<clpoly::prem(g,f,y)<<":"<<std::endl;
    // std::cout<<"t:="<<(double(clock()-t)/CLOCKS_PER_SEC)<<";"<<std::endl;
    // std::cout<<"st := time():o1:= expand(prem(g, f, y)):time() - st;o1-o;"<<std::endl;

    // std::cout<<"t=Association[];t2=Association[];\n";

    // clpoly::polynomial_ZZ p=clpoly::random_polynomial<clpoly::ZZ>({x,y,z,d},5,0.05,10,-10);
    // std::cout<<"p="<<p<<std::endl;
    // auto l=p.variables();
    // for (auto &i:l)
    //     std::cout<<i.first<<":"<<i.second<<" ";
    // std::cout<<std::endl<<p.degree()<<std::endl;
    t=clock();
    clpoly::polynomial_ZZ PP;
    try
    {
        PP=read_file("j621_data.txt");
    }
    catch(const char* msg)
    {
        std::cout<<msg<<std::endl;
        return 1;
    }
    std::cout<<PP.size()<<std::endl;
    std::cout<<"( "<<double(clock()-t)/CLOCKS_PER_SEC<<"s)\n";
    t=clock();
    auto ll=PP.variables();
    for (auto &i:ll)
        std::cout<<i.first<<":"<<i.second<<" ";
    std::cout<<std::endl;
    std::cout<<"("<<double(clock()-t)/CLOCKS_PER_SEC<<"s)\n";
     t=clock();
    std::cout<<std::hash<clpoly::polynomial_ZZ>()(PP)<<std::endl;
    std::cout<<"("<<double(clock()-t)/CLOCKS_PER_SEC<<"s)\n";
    // t=clock();
    // std::cout<<PP.degree()<<std::endl;
    // std::cout<<"("<<double(clock()-t)/CLOCKS_PER_SEC<<"s)\n";

    // clpoly::Zp a(10,7);
    // clpoly::Zp b(-10,7);
    // a=-2;
    // std::cout<<a<<" "<<b<<" "<<a*b<<" "<<a/b<<" "<<-a<<std::endl;

    // g=-pow(r,2)*pow(x,2) + pow(x,4) + pow(x,2)*pow(z,2) + pow(z,4) - 2*pow(z,2) + 1;
    // std::cout<<g<<std::endl;
    // std::cout<<clpoly::polynomial_mod(std::move(g),7)<<std::endl;
    // a=std::move(b);
    // std::cout<<a<<b<<std::endl;
    // f=pow(x,4)+25*pow(x,3)+145*pow(x,2)-171*x-360;
    // g=pow(x,5)+14*pow(x,4)+15*pow(x,3)-pow(x,2)-14*x-15;
    // f=2*pow(x,4)-7*pow(x,3)-4*pow(x,2)-4*x-15;
    // g=4*pow(x,5)+4*pow(x,3)-7*pow(x,2)-2*pow(x,4)+x-12;

    // f=9*pow(x,5)+2*pow(x,4)*y*z-189*pow(x,3)*pow(y,3)*z+117*pow(x,3)*y*pow(z,2)+3*pow(x,3)-42*pow(x,2)*pow(y,4)*pow(z,2)
    //                 +26*pow(x,2)*pow(y,2)*pow(z,3)+18*pow(x,2)-63*x*pow(y,3)*z+39*x*y*pow(z,2)+4*x*y*z+6;
    // g=6*pow(x,6)-126*pow(x,4)*pow(y,3)*z+78*pow(x,4)*y*pow(z,2)+pow(x,4)*y+pow(x,4)*z+13*pow(x,3)
    //     -21*pow(x,2)*pow(y,4)*z-21*pow(x,2)*pow(y,3)*pow(z,2)+13*pow(x,2)*pow(y,2)*pow(z,2)+13*pow(x,2)*y*pow(z,3)
    //     -21*x*pow(y,3)*z+13*x*y*pow(z,2)+2*x*y+2*x*z+2;
    // g=-3*pow(y,8)*pow(d,2)-2*pow(y,5)*pow(d,5)-2*pow(d,10)+5*pow(y,8)*d+3*pow(y,5)*pow(d,4)
    //     -7*pow(y,4)*pow(d,5)+5*pow(y,3)*pow(d,6)+10*pow(d,6)+pow(d,5)+3*pow(y,3)+4;
    // f=-9*pow(x,8)*pow(z,2)+6*pow(x,5)*pow(z,5)+2*pow(x,4)*pow(z,6)+9*pow(x,2)*z-9;
    // g=-7*pow(x,8)*pow(z,2)+2*pow(x,6)*y*pow(z,3)-3*pow(x,5)*pow(y,3)*pow(z,2)+pow(x,5)*y*pow(z,4)+10*pow(x,5)*pow(z,5)-9*pow(x,4)*pow(y,3)*pow(z,3)-pow(x,2)*pow(y,5)*pow(z,3)-6*pow(x,2)*y*pow(z,7)
    //   -2*x*pow(z,9)+2*pow(x,5)*pow(y,4)-8*pow(x,2)*pow(y,3)*pow(z,4)-pow(x,2)*pow(z,7)-4*x*pow(y,5)*pow(z,2)+3*x*pow(y,4)*pow(z,2)-2*x*pow(y,2)*pow(z,4)-7*pow(x,5)*z+7*pow(x,3)*pow(y,3)-2*x*pow(y,2)*pow(z,3)+6*y*pow(z,5)+9*pow(x,2)*pow(y,3)-8*x*pow(z,4)+9*pow(x,3)*z+2*pow(x,2)*pow(z,2)+5;
    // f=-7*pow(x,6)*y*pow(z,3)-8*pow(x,4)*pow(y,5)*z+10*pow(x,3)*pow(y,5)*pow(z,2)-3*pow(x,2)*pow(y,3)*pow(z,5)+pow(x,2)*pow(y,2)*pow(z,6)-2*x*y*pow(z,8)+10*pow(y,5)*pow(z,5)+pow(x,6)*y*pow(z,2)
    //    +8*pow(x,5)*pow(y,4)-9*pow(y,7)*pow(z,2)+pow(y,5)*pow(z,4)-pow(x,5)*y*pow(z,2)+10*pow(x,4)*pow(z,4)-5*pow(x,2)*pow(y,6)-3*x*pow(y,5)*pow(z,2)-6*pow(x,2)*pow(y,3)*pow(z,2)-x*pow(y,6)-5*pow(y,5)*pow(z,2)
    //   -9*pow(z,7)+2*pow(x,4)*pow(y,2)+6*pow(x,4)*y*z+7*x*pow(z,5)-pow(y,4)*pow(z,2)+9*pow(x,4)*z+4*pow(x,2)*pow(y,3)-9*x*pow(y,3)*z-3*x*y*pow(z,3)+9*y*pow(z,4)-10*pow(x,4)-6*x*pow(z,3)-9*y*pow(z,3)+6*x*y*z+8;
    // g=6*pow(x,10)+9*pow(x,8)*pow(y,2)-2*pow(y,4)*pow(z,6)-9*pow(x,4)*pow(z,5)+2*pow(x,3)*pow(y,4)*pow(z,2)+5*pow(x,8)+9*pow(x,6)*pow(z,2)+pow(x,3)*pow(y,5)-2*pow(x,3)*pow(y,2)*pow(z,3)+6*pow(x,2)*pow(z,6)+4*pow(y,6)*pow(z,2)+5*pow(y,4)*pow(z,4)-pow(x,7)-10*pow(x,6)*z+10*pow(x,3)*pow(y,2)*pow(z,2)+8*pow(y,6)*z-4*pow(y,5)*pow(z,2)+10*pow(y,2)*pow(z,5)-5*pow(x,2)*pow(y,2)*pow(z,2)+2*pow(x,2)*y*pow(z,3)-7*x*pow(y,3)*pow(z,2)-10*pow(x,4)+5*pow(y,4)+8;
    // f=-2*pow(x,5)*pow(y,2)*pow(z,3)-3*x*pow(y,7)*pow(z,2)+10*x*pow(y,5)*pow(z,4)-4*pow(y,10)+4*pow(y,6)*pow(z,4)+7*pow(x,8)*z-5*pow(x,5)*pow(y,4)+4*pow(x,4)*pow(z,5)+7*x*pow(y,2)*pow(z,6)-4*pow(y,5)*pow(z,4)+6*pow(x,6)*y*z+7*pow(x,5)*pow(y,2)*z+5*pow(x,5)*pow(z,3)-7*pow(x,4)*pow(y,3)*z-10*pow(x,2)*pow(z,6)+2*x*pow(y,5)*pow(z,2)-2*pow(x,5)*y*z-10*pow(x,3)*pow(y,3)*z+x*pow(y,4)*z-5*pow(y,4)*pow(z,2)+9*pow(y,3)*pow(z,2)+2*pow(y,2)*pow(z,3)+8*pow(x,3)*y-5*x*y*pow(z,2)+10;
    // // clpoly::polynomial_<clpoly::ZZ,clpoly::lex> f_,g_;
    // // clpoly::poly_convert(f,f_);
    // // clpoly::poly_convert(g,g_);
    // std::cout<<"f:"<<f<<std::endl;
    // std::cout<<"g:"<<g<<std::endl;
    // // std::cout<<clpoly::polynomial_GCD(f*g,g*g)<<std::endl;
    // clpoly::lex_<clpoly::custom_var_order> mo(std::vector<clpoly::variable>({z,y,x}));
    // clpoly::polynomial_<clpoly::ZZ,clpoly::lex_<clpoly::custom_var_order>> p1(&mo);
    // clpoly::poly_convert(g,p1);
    // std::cout<< p1<<std::endl;
    return 0;

}


// #include <iostream>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <time.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
// #define _DEBUG

using  std::string;

typedef pcl::PointXYZ PointT;

// sensor_msgs::PointCloud2 cloud_msg;

// void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
// {
//     cloud_msg = *input;
// }

/** \brief Subscribe to topic \b input and detect cylinder
  * \input pointcloud2 topic \b input
  * \return cylinder model coefficients: position + cylinder axis + cylinder radius
  */
pcl::ModelCoefficients::Ptr detect_cylinder()
{
   // All the objects needed
    pcl::PassThrough<PointT> pass;
    pcl::NormalEstimation<PointT, pcl::Normal> ne;
    pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
    pcl::PCDWriter writer;
    pcl::ExtractIndices<PointT> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());

    // Datasets
    pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
    pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);

    pcl::fromROSMsg<PointT>( *ros::topic::waitForMessage<sensor_msgs::PointCloud2>("input"),*cloud);

    clock_t start_time = clock();

  // Build a passthrough filter to remove spurious NaNs
    pass.setInputCloud (cloud);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0, 1.0);
    pass.filter (*cloud_filtered);
    if (cloud_filtered->points.size()<=0){
          std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
          throw ("Input pointcloud has 0 points.");
    }

  // Estimate point normals
    ne.setSearchMethod (tree);
    ne.setInputCloud (cloud_filtered);
    ne.setKSearch (50);
    ne.compute (*cloud_normals);

  // Create the segmentation object for cylinder segmentation and set all the parameters
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_CYLINDER);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setNormalDistanceWeight (0.1);
    seg.setMaxIterations (10000);
    seg.setDistanceThreshold (0.05);
    seg.setRadiusLimits (0, 0.1);
    seg.setInputCloud (cloud_filtered);
    seg.setInputNormals (cloud_normals);

    // Obtain the cylinder inliers and coefficients
    seg.segment (*inliers_cylinder, *coefficients_cylinder);

    // mean normal of inliers
    extract_normals.setInputCloud (cloud_normals);
    extract_normals.setIndices (inliers_cylinder);
    extract_normals.setNegative (false);

    pcl::PointCloud<pcl::Normal>::Ptr cloud_cylinder_normal (new pcl::PointCloud<pcl::Normal> ());
    extract_normals.filter (*cloud_cylinder_normal);
    pcl::Normal mean_normal;
    tf::Vector3 y_axis_vector;
    for(pcl::PointCloud<pcl::Normal>::iterator it= cloud_cylinder_normal->begin();
                  it !=  cloud_cylinder_normal->end(); ++it)
    {
      y_axis_vector.setX(y_axis_vector.x() + (*it).normal_x);
      y_axis_vector.setY(y_axis_vector.y() + (*it).normal_y);
      y_axis_vector.setZ(y_axis_vector.z() + (*it).normal_z);
    }
    y_axis_vector = y_axis_vector / cloud_cylinder_normal->points.size();

  #ifdef _DEBUG
    std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl ;
    std::cerr <<"Detecting time taken:  " << (double)(clock() - start_time)/CLOCKS_PER_SEC << endl;
  #endif
    return coefficients_cylinder;
}

/** \brief Extract pose information from cylinder model coefficients. \b Note: cylinder coefficients are not enough to determine a pose
  * \param cylinder model coefficients
  * \return cylinder pose
  */
geometry_msgs::PoseStamped getCylinderPose(const pcl::ModelCoefficients::Ptr coefficients_cylinder){
  geometry_msgs::PoseStamped p;
  p.header.frame_id = coefficients_cylinder->header.frame_id;
  pcl_conversions::fromPCL(coefficients_cylinder->header,p.header);

  // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
  p.pose.position.x = coefficients_cylinder->values[0];
  p.pose.position.y = coefficients_cylinder->values[1];
  p.pose.position.z = coefficients_cylinder->values[2];

  tf::Vector3 axis_vector(coefficients_cylinder->values[3],
                          coefficients_cylinder->values[4],
                          coefficients_cylinder->values[5]);
  tf::Vector3 up_vector(0.0, 0.0, 1.0);
  tf::Vector3 right_vector = axis_vector.cross(up_vector);
  right_vector.normalized();
  tf::Quaternion q(right_vector, -1.0*acos(axis_vector.dot(up_vector)));
  q.normalize();
  geometry_msgs::Quaternion cylinder_orientation;
  tf::quaternionTFToMsg(q, cylinder_orientation);
  p.pose.orientation = cylinder_orientation;

  return p;
}

inline float getCylinderRadius(const pcl::ModelCoefficients::Ptr coefficients_cylinder){
  return coefficients_cylinder->values[6];
}

/** \brief Publish cylinder marker messages for visualization in Rviz
  * \param cylinder pose
  * \param cylinder radius
  * \param marker publisher
  * \output marker topic
  */
void publish_cylinder_marker(geometry_msgs::PoseStamped& p,float radius,ros::Publisher& marker_pub)
{
      visualization_msgs::Marker marker;

      marker.header.frame_id = p.header.frame_id;

      // Set the namespace and id for this marker.  This serves to create a unique ID
      // Any marker sent with the same namespace and id will overwrite the old one
      marker.ns = "detected_cylinder_pole";
      marker.id = 0;

      // Set the marker type.  CUBE, SPHERE, ARROW, CYLINDER etc.
      marker.type = visualization_msgs::Marker::CYLINDER;

      // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
      marker.action = visualization_msgs::Marker::ADD;

      // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
      marker.pose = p.pose;

      // Set the scale of the marker -- 1x1x1 here means 1m on a side
      marker.scale.x = 2*radius;
      marker.scale.y = 2*radius;
      marker.scale.z = 1.0;

      // Set the color
      marker.color.r = 0.0f;
      marker.color.g = 1.0f;
      marker.color.b = 0.0f;
      marker.color.a = 1.0;

      marker.lifetime = ros::Duration();

      // while (marker_pub.getNumSubscribers() < 1)
      // {
      //   if (!ros::ok()){
      //     return ;
      //   }
      //   ROS_WARN_ONCE("Please create a subscriber to the marker");
      //   sleep(1);
      // }
      marker.header.stamp = ros::Time::now();
      marker_pub.publish(marker);
}

void broadcastCylinderTF(geometry_msgs::PoseStamped& ps,tf::TransformBroadcaster& br){
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(ps.pose.position.x,ps.pose.position.y,ps.pose.position.z));
  transform.setRotation( tf::Quaternion(ps.pose.orientation.x,ps.pose.orientation.y,
                                        ps.pose.orientation.z,ps.pose.orientation.w) );
  br.sendTransform(tf::StampedTransform(transform, ps.header.stamp, ps.header.frame_id, "detected_pole"));
}

geometry_msgs::Pose generateCylinderGraspPose(geometry_msgs::PoseStamped& cylinder_pose){

}

void myTransformStampedTFToMsg(tf::StampedTransform& st,geometry_msgs::PoseStamped & ps){
  ps.header.stamp = st.stamp_;
  // ps.header.frame_id = st.frame_id_;
  ps.pose.position.x = st.getOrigin()[0];
  ps.pose.position.y = st.getOrigin()[1];
  ps.pose.position.z = st.getOrigin()[2];
  ps.pose.orientation.x = st.getRotation().getX();
  ps.pose.orientation.y = st.getRotation().getY();
  ps.pose.orientation.z = st.getRotation().getZ();
  ps.pose.orientation.w = st.getRotation().getW();
}

int main(int argc, char** argv){
  ros::init (argc, argv, "round_pole_detect");
  ros::NodeHandle nh;
  //ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);

  tf::TransformListener tf_lis;
  tf::TransformBroadcaster br;

  // ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("detected_pole", 1);
  ros::Publisher pole_pose_pub = nh.advertise<geometry_msgs::Pose>("pole_pose",1);

  while (ros::ok()){
    tf::StampedTransform transform;
    pcl::ModelCoefficients::Ptr coefficients_cylinder;
    geometry_msgs::PoseStamped cylinder_pose;

    //detect round pole
    try{
      coefficients_cylinder = detect_cylinder();
      cylinder_pose = getCylinderPose(coefficients_cylinder); //cylinder_pose relative to camera frame
    }catch(...){
      continue;
    }

    broadcastCylinderTF(cylinder_pose,br);
    // transform from base_frame to detected_pole
    try{
      tf_lis.lookupTransform("detected_pole","base_gripper1",cylinder_pose.header.stamp,transform);
      myTransformStampedTFToMsg(transform, cylinder_pose); //cylinder_pose relative to base_frame
      pole_pose_pub.publish(cylinder_pose.pose);
    }catch(...){

    }

    // publish_cylinder_marker(cylinder_pose,getCylinderRadius(coefficients_cylinder),marker_pub);

    ros::spinOnce();
  }

    return 0;
}


#include <boost/test/unit_test.hpp>
#include <boost/format.hpp>

#include <iostream>
#include <iomanip>
#include <string>

#include <madym/mdm_AIF.h>
#include <madym/tests/mdm_test_utils.h>
#include <madym/mdm_AIF.h>
#include <madym/dce_models/mdm_DCEModelGenerator.h>
#include <madym/mdm_DCEVoxel.h>
#include <madym/mdm_DCEModelFitter.h>

void test_model_time_fit(
	const std::string &modelName,
	const std::vector<int> fixedParams,
	mdm_AIF &AIF,
	const double paramTol,
	const double sseTol,
	bool test_IAUC = false)
{
	//Read in the model calibration file - this has noise added to it
	auto nTimes = AIF.AIFTimes().size();
	int nParams;
	std::vector<double> CtCalibration(nTimes);
	std::string modelFileName = mdm_test_utils::calibration_dir() + modelName + "_noise.dat";

	std::ifstream modelFileStream(modelFileName, std::ios::in | std::ios::binary);
	modelFileStream.read(reinterpret_cast<char*>(&nParams), sizeof(int));

	std::vector<double> trueParams(nParams);
	for (double &p : trueParams)
		modelFileStream.read(reinterpret_cast<char*>(&p), sizeof(double));
	for (double &c : CtCalibration)
		modelFileStream.read(reinterpret_cast<char*>(&c), sizeof(double));
	modelFileStream.close();
	BOOST_TEST_MESSAGE(boost::format(
		"Read time series for %1% from binary calibration file") % modelName);

	int nIAUC;
	std::vector<double> IAUCTimes;
	std::vector<double> IAUCVals;
	if (test_IAUC)
	{

		std::string iaucFileName = mdm_test_utils::calibration_dir() + modelName + "_IAUC.dat";

		std::ifstream iaucFileStream(iaucFileName, std::ios::in | std::ios::binary);
		iaucFileStream.read(reinterpret_cast<char*>(&nIAUC), sizeof(int));

		IAUCTimes.resize(nIAUC);
		IAUCVals.resize(nIAUC);
		for (double &itm : IAUCTimes)
		{
			double its;
			iaucFileStream.read(reinterpret_cast<char*>(&its), sizeof(double));
			itm = its / 60;
		}

		for (double &iv : IAUCVals)
			iaucFileStream.read(reinterpret_cast<char*>(&iv), sizeof(double));
		iaucFileStream.close();
		BOOST_TEST_MESSAGE(boost::format(
			"Read IAUC data for %1% from binary calibration file") % modelName);
	}

	//Now create the model and compute model time series
	auto modelType = mdm_DCEModelGenerator::ParseModelName(modelName);
	BOOST_REQUIRE_MESSAGE(modelType != mdm_DCEModelGenerator::UNDEFINED,
		"Model name " << modelName << " is undefined");

  AIF.setAIFType(mdm_AIF::AIF_TYPE::AIF_POP);
  AIF.setPIFType(mdm_AIF::PIF_TYPE::PIF_POP);
	auto model = mdm_DCEModelGenerator::createModel(AIF,
		modelType, {},
		{}, fixedParams, {}, {}, {}, {}, {});

  mdm_DCEModelFitter fitter(
    *model,
    0,
    nTimes,
    {},
		"BLEIC"
  );

	mdm_DCEVoxel vox(
		{},
		CtCalibration,
		AIF.prebolus(),
    AIF.AIFTimes(),
    IAUCTimes,
		false);
  vox.testEnhancing();
	fitter.initialiseModelFit(vox.CtData());
  fitter.fitModel(vox.status());

	//Check params match (should be within 0.01)
	BOOST_TEST_MESSAGE("Actual vs fitted params: ");

	for (int i = 0; i < nParams; i++)
	{
		BOOST_TEST_MESSAGE(boost::format("( %1$.2f, %2$.2f )")
			% trueParams[i] % model->params()[i]);
	}

	BOOST_TEST_MESSAGE(boost::format("Model SSE = %1$.4f") % fitter.modelFitError());
	BOOST_TEST_MESSAGE("Test DCE models, values match: " + modelName);
	BOOST_CHECK(mdm_test_utils::vectors_near_equal_rel(
		model->params(), trueParams, paramTol));

	BOOST_TEST_MESSAGE("Test DCE models, SSE < tol: " + modelName);
	BOOST_CHECK_SMALL(fitter.modelFitError(), sseTol);

	if (test_IAUC)
	{
		vox.computeIAUC();
		std::vector<double> computedIAUC(nIAUC);
		for (int i = 0; i < nIAUC; i++)
			computedIAUC[i] = vox.IAUCVal(i);

		BOOST_TEST_MESSAGE("Test IAUC values for " + modelName);
		BOOST_CHECK(mdm_test_utils::vectors_near_equal(
			computedIAUC, IAUCVals, 0.01));
	}
}

BOOST_AUTO_TEST_SUITE(test_mdm)

BOOST_AUTO_TEST_CASE(test_DCE_fit) {
	BOOST_TEST_MESSAGE("======= Testing DCE model optimisation =======");
	//Read in dynamic times from calibration file
	int nTimes;
	std::string timesFileName(mdm_test_utils::calibration_dir() + "dyn_times.dat");
	std::ifstream timesFileStream(timesFileName, std::ios::in | std::ios::binary);
	timesFileStream.read(reinterpret_cast<char*>(&nTimes), sizeof(int));

	std::vector<double> dynTimes(nTimes);
	for (double &t : dynTimes)
		timesFileStream.read(reinterpret_cast<char*>(&t), sizeof(double));
	timesFileStream.close();

	//For the AIF, read in the injection image, haematocrit correction and dose
	//then the values
	int injectionImage;
	double hct;
	double dose;

	std::string aifFileName(mdm_test_utils::calibration_dir() + "aif.dat");
	std::ifstream aifFileStream(aifFileName, std::ios::in | std::ios::binary);
	aifFileStream.read(reinterpret_cast<char*>(&injectionImage), sizeof(int));
	aifFileStream.read(reinterpret_cast<char*>(&hct), sizeof(double));
	aifFileStream.read(reinterpret_cast<char*>(&dose), sizeof(double));
	aifFileStream.close();

	//Create AIF
	mdm_AIF AIF;
	AIF.setAIFTimes(dynTimes);
	AIF.setPrebolus(injectionImage);
	AIF.setHct(hct);
	AIF.setDose(dose);

	//Create model concentrations for each model type
	test_model_time_fit(
		"ETM", {},
		AIF, 0.1, 0.0005, true);
	test_model_time_fit(
		"DIETM", {6},
		AIF, 0.5, 0.0005);
	test_model_time_fit(
		"AUEM", {7},
		AIF, 0.5, 0.0005);
	test_model_time_fit(
		"DISCM", {},
		AIF, 0.5, 0.0005);
	test_model_time_fit(
		"2CXM", {},
		AIF, 0.2, 0.0005);
	test_model_time_fit(
		"DI2CXM", {7},
		AIF, 0.5, 0.0005);
	test_model_time_fit(
		"DIBEM", {7},
		AIF, 0.5, 0.0005);
  test_model_time_fit(
    "PATLAK", {},
    AIF, 0.5, 0.0005);
}
BOOST_AUTO_TEST_SUITE_END() //


#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/ContactSensor.h>
#include <VirtualRobot/IK/HierarchicalIK.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/IK/CoMIK.h>
#include <Eigen/Dense>

#include "uxabiped/utils/Kinematics.h"

namespace Bipedal
{

void extractControlFrames(VirtualRobot::RobotPtr robot,
                          const Eigen::Matrix6Xf& leftFootTrajectory,
                          const Eigen::MatrixXf&  bodyTrajectory,
                          VirtualRobot::RobotNodeSetPtr bodyJoints,
                          VirtualRobot::RobotNodePtr node,
                          std::vector<Eigen::Matrix4f>& controlFrames)
{
    int N = leftFootTrajectory.cols();

    for (int i = 0; i < N; i++)
    {
        // Move basis along with the left foot
        Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
        VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1),
                                                leftFootTrajectory.block(3, i, 3, 1),  leftFootPose);
        robot->setGlobalPose(leftFootPose);
        bodyJoints->setJointValues(bodyTrajectory.col(i));

        controlFrames.push_back(node->getGlobalPose());
    }
}

void transformOrientationToGroundFrame(const VirtualRobot::RobotPtr& robot,
                                       const Eigen::Matrix6Xf& leftFootTrajectory,
                                       const VirtualRobot::RobotNodePtr& leftFoot,
                                       const VirtualRobot::RobotNodePtr& rightFoot,
                                       const VirtualRobot::RobotNodeSetPtr& bodyJoints,
                                       const Eigen::MatrixXf& bodyTrajectory,
                                       const Eigen::Matrix3Xf& trajectory,
                                       const std::vector<SupportInterval>& intervals,
                                       Eigen::Matrix3Xf& relativeTrajectory)
{
    Eigen::Matrix4f leftInitialPose = bodyJoints->getKinematicRoot()->getGlobalPose();
    int N = trajectory.cols();
    int M = trajectory.rows();
    relativeTrajectory.resize(M, N);

    BOOST_ASSERT(M > 0 && M <= 3);

    auto intervalIter = intervals.begin();
    for (int i = 0; i < N; i++)
    {
        while (i >= intervalIter->endIdx)
        {
            intervalIter = std::next(intervalIter);
        }
        // Move basis along with the left foot
        Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
        VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1),
                                                leftFootTrajectory.block(3, i, 3, 1),  leftFootPose);
        robot->setGlobalPose(leftFootPose);
        bodyJoints->setJointValues(bodyTrajectory.col(i));
        Eigen::Matrix3f worldToRef = computeGroundFrame(leftFoot->getGlobalPose(),
                                                        rightFoot->getGlobalPose(),
                                                        intervalIter->phase).block(0, 0, 3, 3);
        relativeTrajectory.block(0, i, M, 1) = worldToRef.colPivHouseholderQr().solve(trajectory.col(i)).block(0, 0, M, 1);
    }
}

SupportPhaseSensor::SupportPhaseSensor(const VirtualRobot::ContactSensorPtr& leftFootSensor,
                                       const VirtualRobot::ContactSensorPtr& rightFootSensor)
: sameStateThreshold(0.05)
, sameStateTime(0)
, phase(SUPPORT_NONE)
, nextPhase(SUPPORT_NONE)
, initialized(false)
, leftFootSensor(leftFootSensor)
, rightFootSensor(rightFootSensor)
{
}

SupportPhase SupportPhaseSensor::getContactPhase() const
{
    auto floorComp = [](const VirtualRobot::ContactSensor::ContactForce& f)
                     {
                         return f.bodyName == "Floor";
                     };
    bool leftContact = std::any_of(leftFootSensor->getContacts().forces.begin(),
                                   leftFootSensor->getContacts().forces.end(),
                                    floorComp);
    bool rightContact = std::any_of(rightFootSensor->getContacts().forces.begin(),
                                    rightFootSensor->getContacts().forces.end(),
                                    floorComp);
    if (leftContact && rightContact)
        return SUPPORT_BOTH;
    if (leftContact)
        return SUPPORT_LEFT;
    if (rightContact)
        return SUPPORT_RIGHT;
    return SUPPORT_NONE;
}

void SupportPhaseSensor::update(float dt)
{
    SupportPhase sensorPhase = getContactPhase();

    if (sensorPhase == nextPhase)
    {
        sameStateTime += dt;
    }
    else
    {
        if (sameStateTime > 0)
        {
            sameStateTime -= dt;
        }
        else
        {
            sameStateTime = 0;
            nextPhase = sensorPhase;
        }
    }

    if (sameStateTime >= sameStateThreshold)
    {
        phase = nextPhase;
        sameStateTime = 0;
        initialized = true;
    }

    // before the phase has setteled just return the unfiltered values
    if (!initialized)
    {
        phase = sensorPhase;
    }
}

}


#ifndef STAN_MCMC_CHAINS_HPP
#define STAN_MCMC_CHAINS_HPP

#include <stan/util/io/stan_csv_reader.hpp>
#include <stan/math/prim/mat.hpp>
#include <stan/util/analyze/mcmc/compute_effective_sample_size.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/accumulators/statistics/p_square_quantile.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/covariance.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/additive_combine.hpp>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <map>
#include <stdexcept>
#include <string>
#include <sstream>
#include <utility>
#include <vector>
#include <cstdlib>

namespace stan {
  namespace mcmc {
    using Eigen::Dynamic;

    /**
     * An mcmc::chains object stores parameter names and
     * dimensionalities along with samples from multiple chains.
     *
     *
Synchronization: For arbitrary concurrent use, the
     * read and write methods need to be read/write locked.  Multiple
     * writers can be used concurrently if they write to different
     * chains.  Readers for single chains need only be read/write locked
     * with writers of that chain.  For reading across chains, full
     * read/write locking is required.  Thus methods will be classified
     * as global or single-chain read or write methods.
     *
     *
Storage Order: Storage is column/last-index major.
     */
    template <class RNG = boost::random::ecuyer1988>
    class chains {
    private:
      Eigen::Matrix<std::string, Dynamic, 1> param_names_;
      Eigen::Matrix<Eigen::MatrixXd, Dynamic, 1> samples_;
      Eigen::VectorXi warmup_;

      static double mean(const Eigen::VectorXd& x) {
        return (x.array() / x.size()).sum();
      }

      static double variance(const Eigen::VectorXd& x) {
        double m = mean(x);
        return ((x.array() - m) / std::sqrt((x.size() - 1.0))).square().sum();
      }

      static double sd(const Eigen::VectorXd& x) {
        return std::sqrt(variance(x));
      }

      static double covariance(const Eigen::VectorXd& x,
                               const Eigen::VectorXd& y,
                               std::ostream* err = 0) {
        if (x.rows() != y.rows() && err)
          *err << "warning: covariance of different length chains";
        using boost::accumulators::accumulator_set;
        using boost::accumulators::stats;
        using boost::accumulators::tag::variance;
        using boost::accumulators::tag::covariance;
        using boost::accumulators::tag::covariate1;

        accumulator_set<double, stats<covariance<double, covariate1> > > acc;

        int M = std::min(x.size(), y.size());
        for (int i = 0; i < M; i++)
          acc(x(i), boost::accumulators::covariate1 = y(i));

        return boost::accumulators::covariance(acc) * M / (M-1);
      }

      static double correlation(const Eigen::VectorXd& x,
                                const Eigen::VectorXd& y,
                                std::ostream* err = 0) {
        if (x.rows() != y.rows() && err)
          *err << "warning: covariance of different length chains";
        using boost::accumulators::accumulator_set;
        using boost::accumulators::stats;
        using boost::accumulators::tag::variance;
        using boost::accumulators::tag::covariance;
        using boost::accumulators::tag::covariate1;

        accumulator_set<double, stats<variance,
                                      covariance<double, covariate1> > > acc_xy;
        accumulator_set<double, stats<variance> > acc_y;

        int M = std::min(x.size(), y.size());
        for (int i = 0; i < M; i++) {
          acc_xy(x(i), boost::accumulators::covariate1 = y(i));
          acc_y(y(i));
        }

        double cov = boost::accumulators::covariance(acc_xy);
        if (cov > -1e-8 && cov < 1e-8)
          return cov;
        return cov / std::sqrt(boost::accumulators::variance(acc_xy)
                               * boost::accumulators::variance(acc_y));
      }

      static double quantile(const Eigen::VectorXd& x, const double prob) {
        using boost::accumulators::accumulator_set;
        using boost::accumulators::left;
        using boost::accumulators::quantile;
        using boost::accumulators::quantile_probability;
        using boost::accumulators::right;
        using boost::accumulators::stats;
        using boost::accumulators::tag::tail;
        using boost::accumulators::tag::tail_quantile;
        double M = x.rows();
        // size_t cache_size = std::min(prob, 1-prob)*M + 2;
        size_t cache_size = M;

        if (prob < 0.5) {
          accumulator_set<double, stats<tail_quantile<left> > >
            acc(tail<left>::cache_size = cache_size);
          for (int i = 0; i < M; i++)
            acc(x(i));
          return quantile(acc, quantile_probability = prob);
        }
        accumulator_set<double, stats<tail_quantile<right> > >
          acc(tail<right>::cache_size = cache_size);
        for (int i = 0; i < M; i++)
          acc(x(i));
        return quantile(acc, quantile_probability = prob);
      }

      static Eigen::VectorXd
      quantiles(const Eigen::VectorXd& x, const Eigen::VectorXd& probs) {
        using boost::accumulators::accumulator_set;
        using boost::accumulators::left;
        using boost::accumulators::quantile_probability;
        using boost::accumulators::quantile;
        using boost::accumulators::right;
        using boost::accumulators::stats;
        using boost::accumulators::tag::tail;
        using boost::accumulators::tag::tail_quantile;
        double M = x.rows();

        // size_t cache_size = M/2 + 2;
        size_t cache_size = M;  // 2 + 2;

        accumulator_set<double, stats<tail_quantile<left> > >
          acc_left(tail<left>::cache_size = cache_size);
        accumulator_set<double, stats<tail_quantile<right> > >
          acc_right(tail<right>::cache_size = cache_size);

        for (int i = 0; i < M; i++) {
          acc_left(x(i));
          acc_right(x(i));
        }

        Eigen::VectorXd q(probs.size());
        for (int i = 0; i < probs.size(); i++) {
          if (probs(i) < 0.5)
            q(i) = quantile(acc_left,
                            quantile_probability = probs(i));
          else
            q(i) = quantile(acc_right,
                            quantile_probability = probs(i));
        }
        return q;
      }

      static Eigen::VectorXd autocorrelation(const Eigen::VectorXd& x) {
        using std::vector;
        using stan::math::index_type;
        typedef typename index_type<vector<double> >::type idx_t;

        std::vector<double> ac;
        std::vector<double> sample(x.size());
        for (int i = 0; i < x.size(); i++)
          sample[i] = x(i);
        stan::math::autocorrelation(sample, ac);

        Eigen::VectorXd ac2(ac.size());
        for (idx_t i = 0; i < ac.size(); i++)
          ac2(i) = ac[i];
        return ac2;
      }

      static Eigen::VectorXd autocovariance(const Eigen::VectorXd& x) {
        using std::vector;
        using stan::math::index_type;
        typedef typename index_type<vector<double> >::type idx_t;

        std::vector<double> ac;
        std::vector<double> sample(x.size());
        for (int i = 0; i < x.size(); i++)
          sample[i] = x(i);
        stan::math::autocovariance(sample, ac);

        Eigen::VectorXd ac2(ac.size());
        for (idx_t i = 0; i < ac.size(); i++)
          ac2(i) = ac[i];
        return ac2;
      }

      /**
       * Return the split potential scale reduction (split R hat)
       * for the specified parameter.
       *
       * Current implementation takes the minimum number of samples
       * across chains as the number of samples per chain.
       *
       * @param VectorXd
       * @param Dynamic
       * @param samples
       *
       * @return
       */
      double
      split_potential_scale_reduction(
                                      const Eigen::Matrix<Eigen::VectorXd,
                                      Dynamic, 1> &samples) const {
        int chains = samples.size();
        int n_samples = samples(0).size();
        for (int chain = 1; chain < chains; chain++) {
          n_samples = std::min(n_samples,
                               static_cast<int>(samples(chain).size()));
        }
        if (n_samples % 2 == 1)
          n_samples--;
        int n = n_samples / 2;

        Eigen::VectorXd split_chain_mean(2*chains);
        Eigen::VectorXd split_chain_var(2*chains);

        for (int chain = 0; chain < chains; chain++) {
          split_chain_mean(2*chain) = mean(samples(chain).topRows(n));
          split_chain_mean(2*chain+1) = mean(samples(chain).bottomRows(n));

          split_chain_var(2*chain) = variance(samples(chain).topRows(n));
          split_chain_var(2*chain+1) = variance(samples(chain).bottomRows(n));
        }

        double var_between = n * variance(split_chain_mean);
        double var_within = mean(split_chain_var);

        // rewrote [(n-1)*W/n + B/n]/W as (n-1+ B/W)/n
        return sqrt((var_between/var_within + n-1)/n);
      }

    public:
      explicit chains(const Eigen::Matrix<std::string, Dynamic, 1>& param_names)
        : param_names_(param_names) { }

      explicit chains(const std::vector<std::string>& param_names)
        : param_names_(param_names.size()) {
        for (size_t i = 0; i < param_names.size(); i++)
          param_names_(i) = param_names[i];
      }

      explicit chains(const stan::io::stan_csv& stan_csv)
        : param_names_(stan_csv.header) {
        if (stan_csv.samples.rows() > 0)
          add(stan_csv);
      }

      inline int num_chains() const {
        return samples_.size();
      }

      inline int num_params() const {
        return param_names_.size();
      }

      const Eigen::Matrix<std::string, Dynamic, 1>& param_names() const {
        return param_names_;
      }

      const std::string& param_name(int j) const {
        return param_names_(j);
      }

      int index(const std::string& name) const {
        int index = -1;
        for (int i = 0; i < param_names_.size(); i++)
          if (param_names_(i) == name)
            return i;
        return index;
      }

      void set_warmup(const int chain, const int warmup) {
        warmup_(chain) = warmup;
      }

      void set_warmup(const int warmup) {
        warmup_.setConstant(warmup);
      }

      const Eigen::VectorXi& warmup() const {
        return warmup_;
      }

      int warmup(const int chain) const {
        return warmup_(chain);
      }

      int num_samples(const int chain) const {
        return samples_(chain).rows();
      }

      int num_samples() const {
        int n = 0;
        for (int chain = 0; chain < num_chains(); chain++)
          n += num_samples(chain);
        return n;
      }

      int num_kept_samples(const int chain) const {
        return num_samples(chain) - warmup(chain);
      }

      int num_kept_samples() const {
        int n = 0;
        for (int chain = 0; chain < num_chains(); chain++)
          n += num_kept_samples(chain);
        return n;
      }

      void add(const int chain,
               const Eigen::MatrixXd& sample) {
        if (sample.cols() != num_params())
          throw std::invalid_argument("add(chain, sample): number of columns"
                                      " in sample does not match chains");
        if (num_chains() == 0 || chain >= num_chains()) {
          int n = num_chains();

          // Need this block for Windows. conservativeResize
          // does not keep the references.
          Eigen::Matrix<Eigen::MatrixXd, Dynamic, 1>
            samples_copy(num_chains());
          Eigen::VectorXi warmup_copy(num_chains());
          for (int i = 0; i < n; i++) {
            samples_copy(i) = samples_(i);
            warmup_copy(i) = warmup_(i);
          }

          samples_.resize(chain+1);
          warmup_.resize(chain+1);
          for (int i = 0; i < n; i++) {
            samples_(i) = samples_copy(i);
            warmup_(i) = warmup_copy(i);
          }
          for (int i = n; i < chain+1; i++) {
            samples_(i) = Eigen::MatrixXd(0, num_params());
            warmup_(i) = 0;
          }
        }
        int row = samples_(chain).rows();
        Eigen::MatrixXd new_samples(row+sample.rows(), num_params());
        new_samples << samples_(chain), sample;
        samples_(chain) = new_samples;
      }

      void add(const Eigen::MatrixXd& sample) {
        if (sample.rows() == 0)
          return;
        if (sample.cols() != num_params())
          throw std::invalid_argument("add(sample): number of columns in"
                                      " sample does not match chains");
        add(num_chains(), sample);
      }

      /**
       * Convert a vector of vector<double> to Eigen::MatrixXd
       *
       * This method is added for the benefit of software wrapping
       * Stan (e.g., PyStan) so that it need not additionally wrap Eigen.
       *
       */
      void add(const std::vector<std::vector<double> >& sample) {
        int n_row = sample.size();
        if (n_row == 0)
          return;
        int n_col = sample[0].size();
        Eigen::MatrixXd sample_copy(n_row, n_col);
        for (int i = 0; i < n_row; i++) {
          sample_copy.row(i)
            = Eigen::VectorXd::Map(&sample[i][0], sample[0].size());
        }
        add(sample_copy);
      }

      void add(const stan::io::stan_csv& stan_csv) {
        if (stan_csv.header.size() != num_params())
          throw std::invalid_argument("add(stan_csv): number of columns in"
                                      " sample does not match chains");
        if (!param_names_.cwiseEqual(stan_csv.header).all()) {
          throw std::invalid_argument("add(stan_csv): header does not match"
                                      " chain's header");
        }
        add(stan_csv.samples);
        if (stan_csv.metadata.save_warmup)
          set_warmup(num_chains()-1, stan_csv.metadata.num_warmup);
      }

      Eigen::VectorXd samples(const int chain, const int index) const {
        return samples_(chain).col(index).bottomRows(num_kept_samples(chain));
      }

      Eigen::VectorXd samples(const int index) const {
        Eigen::VectorXd s(num_kept_samples());
        int start = 0;
        for (int chain = 0; chain < num_chains(); chain++) {
          int n = num_kept_samples(chain);
          s.middleRows(start, n) = samples_(chain).col(index).bottomRows(n);
          start += n;
        }
        return s;
      }

      Eigen::VectorXd samples(const int chain, const std::string& name) const {
        return samples(chain, index(name));
      }

      Eigen::VectorXd samples(const std::string& name) const {
        return samples(index(name));
      }

      double mean(const int chain, const int index) const {
        return mean(samples(chain, index));
      }

      double mean(const int index) const {
        return mean(samples(index));
      }

      double mean(const int chain, const std::string& name) const {
        return mean(chain, index(name));
      }

      double mean(const std::string& name) const {
        return mean(index(name));
      }

      double sd(const int chain, const int index) const {
        return sd(samples(chain, index));
      }

      double sd(const int index) const {
        return sd(samples(index));
      }

      double sd(const int chain, const std::string& name) const {
        return sd(chain, index(name));
      }

      double sd(const std::string& name) const {
        return sd(index(name));
      }

      double variance(const int chain, const int index) const {
        return variance(samples(chain, index));
      }

      double variance(const int index) const {
        return variance(samples(index));
      }

      double variance(const int chain, const std::string& name) const {
        return variance(chain, index(name));
      }

      double variance(const std::string& name) const {
        return variance(index(name));
      }

      double
      covariance(const int chain, const int index1, const int index2) const {
        return covariance(samples(chain, index1), samples(chain, index2));
      }

      double covariance(const int index1, const int index2) const {
        return covariance(samples(index1), samples(index2));
      }

      double covariance(const int chain, const std::string& name1,
                        const std::string& name2) const {
        return covariance(chain, index(name1), index(name2));
      }

      double
      covariance(const std::string& name1, const std::string& name2) const {
        return covariance(index(name1), index(name2));
      }

      double
      correlation(const int chain, const int index1, const int index2) const {
        return correlation(samples(chain, index1), samples(chain, index2));
      }

      double correlation(const int index1, const int index2) const {
        return correlation(samples(index1), samples(index2));
      }

      double correlation(const int chain, const std::string& name1,
                         const std::string& name2) const {
        return correlation(chain, index(name1), index(name2));
      }

      double
      correlation(const std::string& name1, const std::string& name2) const {
        return correlation(index(name1), index(name2));
      }

      double
      quantile(const int chain, const int index, const double prob) const {
        return quantile(samples(chain, index), prob);
      }

      double quantile(const int index, const double prob) const {
        return quantile(samples(index), prob);
      }

      double quantile(int chain, const std::string& name, double prob) const {
        return quantile(chain, index(name), prob);
      }

      double quantile(const std::string& name, const double prob) const {
        return quantile(index(name), prob);
      }

      Eigen::VectorXd
      quantiles(int chain, int index, const Eigen::VectorXd& probs) const {
        return quantiles(samples(chain, index), probs);
      }

      Eigen::VectorXd quantiles(int index, const Eigen::VectorXd& probs) const {
        return quantiles(samples(index), probs);
      }

      Eigen::VectorXd
      quantiles(int chain, const std::string& name,
                const Eigen::VectorXd& probs) const {
        return quantiles(chain, index(name), probs);
      }

      Eigen::VectorXd
      quantiles(const std::string& name, const Eigen::VectorXd& probs) const {
        return quantiles(index(name), probs);
      }

      Eigen::Vector2d central_interval(int chain, int index,
                                       double prob) const {
        double low_prob = (1-prob)/2;
        double high_prob = 1-low_prob;

        Eigen::Vector2d interval;
        interval
          << quantile(chain, index, low_prob),
          quantile(chain, index, high_prob);
        return interval;
      }

      Eigen::Vector2d central_interval(int index, double prob) const {
        double low_prob = (1-prob)/2;
        double high_prob = 1-low_prob;

        Eigen::Vector2d interval;
        interval << quantile(index, low_prob), quantile(index, high_prob);
        return interval;
      }

      Eigen::Vector2d
      central_interval(int chain, const std::string& name,
                       double prob) const {
        return central_interval(chain, index(name), prob);
      }

      Eigen::Vector2d central_interval(const std::string& name,
                                       double prob) const {
        return central_interval(index(name), prob);
      }

      Eigen::VectorXd autocorrelation(const int chain, const int index) const {
        return autocorrelation(samples(chain, index));
      }

      Eigen::VectorXd autocorrelation(int chain,
                                      const std::string& name) const {
        return autocorrelation(chain, index(name));
      }

      Eigen::VectorXd autocovariance(const int chain, const int index) const {
        return autocovariance(samples(chain, index));
      }

      Eigen::VectorXd autocovariance(int chain, const std::string& name) const {
        return autocovariance(chain, index(name));
      }

      // FIXME: reimplement using autocorrelation.
      double effective_sample_size(const int index) const {
        int n_chains = num_chains();
        std::vector<const double*> draws(n_chains);
        std::vector<size_t> sizes(n_chains);
        int n_kept_samples = 0;
        for (int chain = 0; chain < n_chains; ++chain) {
          n_kept_samples = num_kept_samples(chain);
          draws[chain]
            = samples_(chain).col(index).bottomRows(n_kept_samples).data();
          sizes[chain] = n_kept_samples;
        }
        return analyze::compute_effective_sample_size(draws, sizes);
      }

      double effective_sample_size(const std::string& name) const {
        return effective_sample_size(index(name));
      }

      double split_effective_sample_size(const int index) const {
        int n_chains = num_chains();
        std::vector<const double*> draws(n_chains);
        std::vector<size_t> sizes(n_chains);
        int n_kept_samples = 0;
        for (int chain = 0; chain < n_chains; ++chain) {
          n_kept_samples = num_kept_samples(chain);
          draws[chain]
            = samples_(chain).col(index).bottomRows(n_kept_samples).data();
          sizes[chain] = n_kept_samples;
        }
        return analyze::compute_split_effective_sample_size(draws, sizes);
      }

      double split_effective_sample_size(const std::string& name) const {
        return split_effective_sample_size(index(name));
      }

      double split_potential_scale_reduction(const int index) const {
        Eigen::Matrix<Eigen::VectorXd, Dynamic, 1>
          samples(num_chains());
        for (int chain = 0; chain < num_chains(); chain++) {
          samples(chain) = this->samples(chain, index);
        }
        return split_potential_scale_reduction(samples);
      }

      double split_potential_scale_reduction(const std::string& name) const {
        return split_potential_scale_reduction(index(name));
      }
    };

  }
}

#endif


/******************************************************************************
 * Copyright 2018 The Apollo Authors. 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 "modules/perception/camera/test/camera_common_undistortion.h"

#include <npp.h>
#include <yaml-cpp/yaml.h>
#include <boost/filesystem.hpp>
#include <opencv2/opencv.hpp>

namespace apollo {
namespace perception {
namespace camera {

/* Initialization of the GPU routines for camera data preprocessing
 *
 * Return: 0 - success; other - failure
 * Params:  intrinsics_path - intrinsics path
 *          dev - dev number of the GPU device
 * Note: returns OK if already been inited.
 */
int ImageGpuPreprocessHandler::init(const std::string &intrinsics_path,
                                    int dev) {
  if (_inited) {
    return 0;
  }

  std::vector<double> D;
  std::vector<double> K;
  if (load_camera_intrinsics(intrinsics_path, &_width, &_height, &D, &K) != 0) {
    std::cerr << "Failed to load camera intrinsics!" << std::endl;
    return -1;
  }

  _dev_no = dev;

  if (set_device(_dev_no) != 0) {
    return -1;
  }

  size_t img_size = _height * _width;
  _in_size = static_cast<int>(img_size * CHANNEL * sizeof(uint8_t));

  _out_size = static_cast<int>(img_size * CHANNEL * sizeof(uint8_t));
  // if input is rgb, its size is same as output's
  BASE_CUDA_CHECK(cudaMalloc(&_d_rgb, _out_size));
  BASE_CUDA_CHECK(cudaMalloc(&_d_dst, _out_size));

  cv::Mat_<double> I = cv::Mat_<double>::eye(3, 3);
  cv::Mat map1(_height, _width, CV_32FC1);
  cv::Mat map2(_height, _width, CV_32FC1);

  cv::Mat cm = cv::Mat(3, 3, CV_64F, K.data()).clone();
  cv::Mat dc = cv::Mat(1, 5, CV_64F, D.data()).clone();
  cv::Mat ncm = cm.clone();

  cv::initUndistortRectifyMap(cm, dc, I, ncm, cv::Size(_width, _height),
                              CV_32FC1, map1, map2);

  std::vector<float> mapx;
  std::vector<float> mapy;
  mapx.resize(img_size);
  mapy.resize(img_size);

  for (int r = 0; r < _height; ++r) {
    for (int c = 0; c < _width; ++c) {
      mapx[r * _width + c] = map1.at<float>(r, c);
      mapy[r * _width + c] = map2.at<float>(r, c);
    }
  }

  BASE_CUDA_CHECK(cudaMalloc(&_d_mapx, img_size * sizeof(float)));
  BASE_CUDA_CHECK(cudaMalloc(&_d_mapy, img_size * sizeof(float)));

  BASE_CUDA_CHECK(cudaMemcpy(_d_mapx, mapx.data(), img_size * sizeof(float),
                             cudaMemcpyHostToDevice));
  BASE_CUDA_CHECK(cudaMemcpy(_d_mapy, mapy.data(), img_size * sizeof(float),
                             cudaMemcpyHostToDevice));
  _inited = true;
  return 0;
}

/* Processing each image
 *
 * Return: 0 - success; other - failure
 * Params: src - input image array (of type yuyv)
 *         dst - output image array (of type rgb)
 *
 */
int ImageGpuPreprocessHandler::handle(uint8_t *src, uint8_t *dst) {
  if (!_inited) {
    return -1;
  }

  BASE_CUDA_CHECK(cudaMemcpy(_d_rgb, src, _in_size, cudaMemcpyHostToDevice));

  NppiSize Remapsize;
  NppiInterpolationMode RemapMode = NPPI_INTER_LINEAR;
  Remapsize.width = _width;
  Remapsize.height = _height;
  NppiRect RemapRect = {0, 0, _width, _height};

  NppStatus eStatusNPP =
      nppiRemap_8u_C3R(_d_rgb, Remapsize, (_width * CHANNEL), RemapRect,
                       _d_mapx, (_width * static_cast<int>(sizeof(float))),
                       _d_mapy, (_width * static_cast<int>(sizeof(float))),
                       _d_dst, (_width * CHANNEL), Remapsize, RemapMode);

  if (eStatusNPP != NPP_SUCCESS) {
    std::cerr << "NPP_CHECK_NPP - eStatusNPP = " << eStatusNPP << std::endl;
    return static_cast<int>(eStatusNPP);
  }

  BASE_CUDA_CHECK(cudaMemcpy(dst, _d_dst, _out_size, cudaMemcpyDeviceToHost));
  return 0;
}

/* Release the resources
 *
 * Return: 0 - success; other - failure
 *
 */
int ImageGpuPreprocessHandler::release(void) {
  if (_d_mapy) {
    BASE_CUDA_CHECK(cudaFree(_d_mapy));
    _d_mapy = nullptr;
  }
  if (_d_mapx) {
    BASE_CUDA_CHECK(cudaFree(_d_mapx));
    _d_mapx = nullptr;
  }
  if (_d_dst) {
    BASE_CUDA_CHECK(cudaFree(_d_dst));
    _d_dst = nullptr;
  }
  if (_d_rgb) {
    BASE_CUDA_CHECK(cudaFree(_d_rgb));
    _d_rgb = nullptr;
  }
  _inited = false;
  return 0;
}

int ImageGpuPreprocessHandler::load_camera_intrinsics(
    const std::string &intrinsics_path, int *width, int *height,
    std::vector<double> *D, std::vector<double> *K) {
  if (!(boost::filesystem::exists(intrinsics_path))) {
    return -1;
  }
  YAML::Node node = YAML::LoadFile(intrinsics_path);
  if (node.IsNull()) {
    return -1;
  }
  D->resize(node["D"].size());
  for (int i = 0; i < static_cast<int>(node["D"].size()); ++i) {
    (*D)[i] = node["D"][i].as<double>();
  }
  K->resize(node["K"].size());
  for (int i = 0; i < static_cast<int>(node["K"].size()); ++i) {
    (*K)[i] = node["K"][i].as<double>();
  }
  *width = node["width"].as<int>();
  *height = node["height"].as<int>();
  return 0;
}

}  // namespace camera
}  // namespace perception
}  // namespace apollo


#include "mpi_fsi.h"
#include "mpi_scnsim.h"
#include "mpi_shared_hypo_elasticity.h"
#include "parameters.h"
#include "utilities.h"
#include <deal.II/grid/grid_in.h>
#include <deal.II/grid/grid_out.h>

extern template class Fluid::MPI::SCnsIM<3>;
extern template class Solid::MPI::SharedHypoElasticity<3>;
extern template class MPI::FSI<3>;

using namespace dealii;

int main(int argc, char *argv[])
{
  using namespace dealii;

  try
    {
      Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);

      std::string infile("parameters.prm");
      if (argc > 1)
        {
          infile = argv[1];
        }
      Parameters::AllParameters params(infile);

      if (params.dimension == 3)
        {
          // Read solid mesh
          Triangulation<3> tria_solid;
          dealii::GridGenerator::subdivided_hyper_rectangle(tria_solid,
                                                            {20u, 20u, 8u},
                                                            Point<3>(0, 0, 0),
                                                            Point<3>(1, 1, 0.4),
                                                            true);

          // Read fluid mesh
          parallel::distributed::Triangulation<3> tria_fluid(MPI_COMM_WORLD);
          dealii::GridGenerator::subdivided_hyper_rectangle(tria_fluid,
                                                            {10u, 10u, 40u},
                                                            Point<3>(0, 0, 0),
                                                            Point<3>(1, 1, 4),
                                                            true);
          for (auto cell : tria_fluid.active_cell_iterators())
            {
              auto center = cell->center();
              if (center[2] >= 2 && center[2] <= 2.4)
                cell->set_refine_flag();
            }
          tria_fluid.execute_coarsening_and_refinement();

          // Translate solid mesh
          Tensor<1, 3> offset({0, 0, 2});
          GridTools::shift(offset, tria_solid);

          Fluid::MPI::SCnsIM<3> fluid(tria_fluid, params);
          Solid::MPI::SharedHypoElasticity<3> solid(
            tria_solid, params, 0.05, 1.3);
          MPI::FSI<3> fsi(fluid, solid, params);
          fsi.run();
        }
      else
        {
          AssertThrow(false, ExcNotImplemented());
        }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// kolmogorov_smirnov::statistic.hpp                                         //
//                                                                           //
//  Copyright 2010 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KOLMOGOROV_SMIRNOV_STATISTIC_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_KOLMOGOROV_SMIRNOV_STATISTIC_HPP_ER_2010
#include <boost/type_traits.hpp>
#include <boost/range.hpp>

#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/if.hpp>

#include <boost/foreach.hpp>
#include <boost/parameter/binding.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/count.hpp>

#include <boost/statistics/detail/non_parametric/empirical_distribution/count.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace empirical_distribution{
namespace impl{

    // Warning : See empirical_distribution::impl::count
    template<typename T>
    class kolmogorov_smirnov : public boost::accumulators::accumulator_base{

        typedef std::size_t size_;
        typedef boost::accumulators::dont_care dont_care_;

        public:

        kolmogorov_smirnov(dont_care_){};

        typedef size_ size_type;
        typedef T sample_type;
        typedef void result_type;

        void operator()(dont_care_)const{}

        template<typename Args>
        result_type result(dont_care_) const
        {
        }
	};

}
namespace tag
{
    struct kolmogorov_smirnov
      : boost::accumulators::depends_on<
      	statistics::detail::empirical_distribution::tag::ordered_sample,
        boost::accumulators::tag::count
    >
    {
        struct impl{
            template<typename T,typename W>
            struct apply{
                typedef detail::kolmogorov_smirnov::impl::kolmogorov_smirnov<T> type;
            };
        };
    };
}
namespace result_of{

    template<typename T1,typename AccSet,typename D>
    struct kolmogorov_smirnov_statistic
    {
        typedef T1 type;
    };

}

    // Usage : statistic<T1>(acc,dist)
    template<typename T1,typename AccSet,typename D>
    typename kolmogorov_smirnov
        ::result_of::template kolmogorov_smirnov_statistic<T1,AccSet,D>::type
    kolmogorov_smirnov_statistic(AccSet const& acc,const D& dist)
    {
            namespace ed = boost::statistics::detail::empirical_distribution;
            namespace ks = boost::statistics::detail::kolmogorov_smirnov;
            typedef T1 val_;
            typedef std::size_t size_;
            typedef boost::accumulators::tag::count tag_n_;
            typedef ed::tag::ordered_sample tag_os_;

            typedef typename ed::result_of::ordered_sample<
                AccSet>::type ref_os_;
            typedef typename boost::remove_const< //in case ref changed to cref
            	typename boost::remove_reference<
                	ref_os_
                >::type
            >::type os_;
            typedef typename boost::range_reference<os_>::type ref_elem_;

            ref_os_ ref_os
                = boost::accumulators::extract_result<tag_os_>( acc );

            val_ m1 = static_cast<val_>(0);
            size_ i = 0;
            size_ n = boost::accumulators::extract::count( acc );

            BOOST_FOREACH(ref_elem_ e,ref_os){
                i += e.second;
                val_ ecdf = static_cast<val_>(i) / static_cast<val_>(n);
                val_ true_cdf = cdf( dist, e.first );
                val_ m2
                	= (true_cdf > ecdf)?(true_cdf - ecdf) : (ecdf - true_cdf);
                if(m2 > m1){ m1 = m2; }
            }

            return m1;
    }

}// kolgorov_statistic
}// empirical_distribution
}// detail
}// statistics
}// boost

#endif


//
// Created by h3ct0r on 28/09/20.
//

#include <iostream>
#include <fstream>
#include <chrono>
#include <algorithm>
#include <iterator>

#include <octomap/octomap.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap_msgs/GetOctomap.h>
#include <tf/transform_broadcaster.h>
#include <ros/console.h>
#include <octomap/math/Utils.h>
#include <octomap/math/Vector3.h>
#include <map>
#include <limits>
#include "boost/tuple/tuple_comparison.hpp"
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/distances.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/range/algorithm.hpp>

#include <math.h>
#include <limits>

#define degToRad(angleInDegrees) ((angleInDegrees) * M_PI / 180.0)
#define radToDeg(angleInRadians) ((angleInRadians) * 180.0 / M_PI)
#define DEBUG  1

using namespace std;
using namespace std::chrono;

ros::Publisher pub_obstacles_markers;
ros::Publisher pub_closest_obstacle_marker;
ros::Publisher pub_closest_obstacle_pt;

sensor_msgs::PointCloud2 cloud_msg;

//pcl::visualization::PCLVisualizer viewer("cloud_viewer");
//#ifndef DEBUG
//    viewer.close();
//#endif

void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input){
    if(cloud_msg.height <= 0){
        ROS_INFO_STREAM("cloud received...");
    }

    cloud_msg = *input;
}

visualization_msgs::Marker prepare_marker(double x, double y, double z, double scale, int id, double r, double g,
        double b, string frame_id){
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();

    marker.id = id;
    marker.type = 2;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position.x = x;
    marker.pose.position.y = y;
    marker.pose.position.z = z;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    marker.scale.x = scale;
    marker.scale.y = scale;
    marker.scale.z = scale;
    marker.color.r = r;
    marker.color.g = g;
    marker.color.b = b;
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration(2);

    return marker;
}

double fov_segmentation(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double min_fov, double max_fov, pcl::PointCloud<pcl::PointXYZ>::Ptr keypoint_cloud){
    boost::accumulators::accumulator_set<double, boost::accumulators::stats<boost::accumulators::tag::sum , boost::accumulators::tag::variance, boost::accumulators::tag::mean > > acc;

    for (size_t i = 0; i < (*cloud).points.size(); ++i) {
        double x = (*cloud).points[i].x;
        double y = (*cloud).points[i].y;
        double angle = atan2(y, x);

        if(angle >= degToRad(-max_fov) && angle <= degToRad(-min_fov)){
            (*keypoint_cloud).push_back((*cloud).points[i]);
            acc((*cloud).points[i].z);
        }
    }

    return sqrt(boost::accumulators::variance(acc));
}

void process_cloud(){
    if(cloud_msg.height <= 0){
        ROS_WARN_ONCE("Cloud not received yet, waiting for new pointcloud msg...");
        return;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_original(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PCLPointCloud2 cloud_tmp;
    pcl_conversions::toPCL(cloud_msg, cloud_tmp); // sensor_msgs::PointCloud2 to pcl::PCLPointCloud2
    pcl::fromPCLPointCloud2(cloud_tmp, *cloud_original); // pcl::PCLPointCloud2 to PointCloud

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    *cloud = *cloud_original;

    ROS_INFO_STREAM("cloud_original size:" << cloud_original->size());
    ROS_INFO_STREAM("cloud size:" << cloud->size());

    double zMin, zMax, xMin, xMax, yMin, yMax = 2.5;
    pcl::PassThrough<pcl::PointXYZ> filter;
    filter.setInputCloud(cloud);

    filter.setFilterFieldName("z");
    filter.setFilterLimits(-3.0, 3.0);
    filter.setNegative(false);
    filter.filter(*cloud);

    filter.setFilterFieldName("z");
    filter.setFilterLimits(-0.38, -0.3);
    filter.setNegative(true);
    filter.filter(*cloud);

    filter.setFilterFieldName("y");
    filter.setFilterLimits(-3.0, 3.0);
    filter.setNegative(false);
    filter.filter(*cloud);

    filter.setFilterFieldName("x");
    filter.setFilterLimits(-3.0, 3.0);
    filter.setNegative(false);
    filter.filter(*cloud);

    ROS_INFO_STREAM("cloud size:" << cloud->size());

    // Perform the actual filtering
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud);
    sor.setLeafSize(0.01, 0.01, 0.01);
    sor.filter(*cloud_filtered);

    ROS_INFO_STREAM("cloud_filtered size:" << cloud_filtered->size());

    // binning by angle and filter by std deviation on Z values
    double n_bins = 72;
    double bin_size = 360/n_bins;
    vector<int> fov_sign{-1, 1};
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_binned (new pcl::PointCloud<pcl::PointXYZ>);

    for (size_t i = 0; i < (n_bins / 2); ++i) {
        for (size_t j = 0; j < fov_sign.size(); ++j) {
            double sign = fov_sign[j];
            double bin_start = (i * bin_size) * sign;
            double bin_end = ((i + 1) * bin_size) * sign;

            if(sign < 0){
                double temp = bin_start;
                bin_start = bin_end;
                bin_end = temp;
            }

            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_fov (new pcl::PointCloud<pcl::PointXYZ>);
            double std_dev = fov_segmentation(cloud_filtered, bin_start, bin_end, cloud_fov);

            if(std_dev > 0.1){
                *cloud_binned += *cloud_fov;
            }
        }
    }

    // Creating the KdTree object for the search method of the extraction
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clustered (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud_binned);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.1);
    ec.setMinClusterSize(200);
    ec.setMaxClusterSize(25000);
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud_binned);
    ec.extract(cluster_indices);

    visualization_msgs::MarkerArray marker_array;
    pcl::PointXYZ origin = pcl::PointXYZ(0, 0, 0);
    pcl::PointXYZ closest_point;
    double closest_point_dist = std::numeric_limits<double>::infinity();
    int c_idx = -1;

    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
    {
        c_idx++;
        boost::accumulators::accumulator_set<double, boost::accumulators::stats<boost::accumulators::tag::sum, boost::accumulators::tag::variance, boost::accumulators::tag::mean>> acc;

        pcl::PointCloud<pcl::PointXYZ>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZ>);
        for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit){
            cluster->push_back((*cloud_binned).points[*pit]);
            acc((*cloud_binned).points[*pit].z);
        }

        double std_dev = sqrt(boost::accumulators::variance(acc));

        if(std_dev > 0.1){
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter_outlier (new pcl::PointCloud<pcl::PointXYZ>);
            pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
            sor.setInputCloud(cluster);
            sor.setMeanK(50);
            sor.setStddevMulThresh(1.0);
            sor.filter(*cloud_filter_outlier);

            /*pcl::PointXYZ centroid;
            pcl::computeCentroid(*cloud_filter_outlier, centroid);

            //double x, double y, double z, double scale, int id, double r, double, g, double b
            visualization_msgs::Marker marker = prepare_marker(centroid.x, centroid.y, centroid.z, 0.1, c_idx, 0.9, 0.1, 0.1);
            marker_array.markers.push_back(marker);*/

            pcl::KdTree<pcl::PointXYZ>::Ptr cluster_tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
            cluster_tree->setInputCloud(cloud_filter_outlier);

            std::vector<int> nn_indices (1);
            std::vector<float> nn_dists (1);

            cluster_tree->nearestKSearch(origin, 1, nn_indices, nn_dists);

            visualization_msgs::Marker marker = prepare_marker(
                    cloud_filter_outlier->points[nn_indices[0]].x,
                    cloud_filter_outlier->points[nn_indices[0]].y,
                    cloud_filter_outlier->points[nn_indices[0]].z,
                    0.1,
                    c_idx,
                    0.9, 0.1, 0.1,
                    cloud_msg.header.frame_id);

            marker_array.markers.push_back(marker);

            *cloud_clustered += *cloud_filter_outlier;

            double d = pcl::squaredEuclideanDistance(origin, cloud_filter_outlier->points[nn_indices[0]]);
            if(d < closest_point_dist){
                closest_point_dist = d;
                closest_point = cloud_filter_outlier->points[nn_indices[0]];
            }

        }

        ROS_INFO_STREAM("cluster size:" << cluster->size() << " std_dev:" << std_dev);
    }

    ROS_INFO_STREAM("closer point dist:" << closest_point_dist);

    if(closest_point_dist < std::numeric_limits<double>::infinity()){
        visualization_msgs::Marker closest_marker = prepare_marker(
                closest_point.x,
                closest_point.y,
                closest_point.z,
                0.25,
                0,
                1.0, 0.0, 0.0,
                cloud_msg.header.frame_id);

        geometry_msgs::Point closest_pt_msg = geometry_msgs::Point();
        closest_pt_msg.x =  closest_point.x;
        closest_pt_msg.y =  closest_point.y;
        closest_pt_msg.z =  closest_point.z;

        pub_closest_obstacle_pt.publish(closest_pt_msg);
        pub_closest_obstacle_marker.publish(closest_marker);
        pub_obstacles_markers.publish(marker_array);
    }

//    #ifdef DEBUG
//        viewer.addPointCloud<pcl::PointXYZ>(cloud_clustered, "", 0);
//        viewer.spinOnce();
//        viewer.removeAllPointClouds();
//        viewer.removeAllShapes();
//    #endif

    // reset the pointcloud
    cloud_msg = sensor_msgs::PointCloud2();
}

int main(int argc, char **argv) {
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
    ros::console::notifyLoggerLevelsChanged();

    ros::init(argc, argv, "obstacle_detection_pcd");
    ros::NodeHandle nh;
    ros::Rate r(10);

    pub_obstacles_markers = nh.advertise<visualization_msgs::MarkerArray>("/obstacles_points_markers", 1);
    pub_closest_obstacle_marker = nh.advertise<visualization_msgs::Marker>("/closest_obstacle_marker", 1);
    pub_closest_obstacle_pt = nh.advertise<geometry_msgs::Point>("/closest_obstacle_point", 1);

    ros::Subscriber sub = nh.subscribe("/os1_cloud_node/points", 1, cloud_cb);

    auto start = chrono::high_resolution_clock::now();
    auto stop = chrono::high_resolution_clock::now();

    ros::Duration(3.0).sleep();

    while (ros::ok())
    {
        start = chrono::high_resolution_clock::now();

        process_cloud();

        stop = chrono::high_resolution_clock::now();
        ROS_INFO_STREAM("Time to process cloud: " << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0) << " seconds");

        ros::spinOnce();
        r.sleep();
    }

    return 0;
}


#ifndef INAQS_EHRENFEST_HPP
#define INAQS_EHRENFEST_HPP

#include <armadillo>
#include "bomd.hpp"
#include "electronic.hpp"
#include "configreader.hpp"

class Ehrenfest: public BOMD {
public:
  explicit Ehrenfest(arma::mat& qm_grd, arma::mat& mm_grd): BOMD{qm_grd, mm_grd} {}
  ~Ehrenfest() { }
  double update_gradient(void) override;

protected:
  // Called within BOMD initialization
  void get_reader_data(ConfigBlockReader& reader) override;
  ConfigBlockReader setup_reader() override;

  arma::mat U, T, V;
  arma::vec phases;
  arma::cube qm_grds, mm_grds;

  double dtc;
  double dtq;

  size_t min_state;
  int nstates;

  std::string amplitude_file;

  // size_t active_state; inherited from BOMD
  Electronic c {};
};

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#define EIGEN_NO_DEBUG_SMALL_PRODUCT_BLOCKS
#include "sparse_solver.h"

#include <Eigen/SuperLUSupport>

void test_superlu_support()
{
  SuperLU<SparseMatrix<double> > superlu_double_colmajor;
  SuperLU<SparseMatrix<std::complex<double> > > superlu_cplxdouble_colmajor;
  CALL_SUBTEST_1( check_sparse_square_solving(superlu_double_colmajor)      );
  CALL_SUBTEST_2( check_sparse_square_solving(superlu_cplxdouble_colmajor)  );
  CALL_SUBTEST_1( check_sparse_square_determinant(superlu_double_colmajor)      );
  CALL_SUBTEST_2( check_sparse_square_determinant(superlu_cplxdouble_colmajor)  );
}


/*
 * Copyright (c) 2012, The Robot Studio
 *  All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *	* Redistributions of source code must retain the above copyright notice, this
 *	  list of conditions and the following disclaimer.
 *
 *	* Redistributions in binary form must reproduce the above copyright notice,
 *	  this list of conditions and the following disclaimer in the documentation
 *	  and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Jul 25, 2012
 *      Author: Cyril Jourdan (cyril.jourdan@therobotstudio.com)
 */

/*** Includes ***/
#include <ros/ros.h>
#include <ros/package.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <sensor_msgs/JointState.h>
#include <trs_msgs/MotorCmdSet.h>
#include <trs_msgs/MotorDataSet.h>
#include <std_msgs/Bool.h>
#include "trs_control/getMotorCmdSet.h"
#include "trs_control/switchNode.h"
#include <flann/flann.hpp> //used for the kdtree search
#include <stdio.h>
#include "robotDefines.h"

/*** Defines ***/
#define LOOP_RATE				HEART_BEAT	//1  //20
#define SPACE_DIM  				4  //4 angles : alpha, beta, theta and phi
#define REF_DATA_DIM 			1  //only one reference point in 4d space to find its nearest neighboor.
#define NUM_NN 					5  //number of nearest neighboors
#define DOF_DIM 				13 //degrees of freedom = number of motors //(no rotator and hand)

using namespace flann;

/*** Variables ***/
bool switch_node = false; //disable by default
trs_msgs::MotorCmdSet motorCmdSet;
int dataset_dim = 0; //this will depend on the size of the bag, number of lines of data

//left arm
Matrix<float> dataset_L_angles; //matrix for the angles
Matrix<int> dataset_L_positions; //matrix for the positions
Matrix<float> query_L(new float[SPACE_DIM*REF_DATA_DIM], REF_DATA_DIM, SPACE_DIM); //matrix for the query, just one line
Matrix<int> indices_L(new int[query_L.rows*SPACE_DIM], query_L.rows, NUM_NN);
Matrix<float> dists_L(new float[query_L.rows*SPACE_DIM], query_L.rows, NUM_NN);

//right arm
Matrix<float> dataset_R_angles; //matrix for the angles
Matrix<int> dataset_R_positions; //matrix for the positions
Matrix<float> query_R(new float[SPACE_DIM*REF_DATA_DIM], REF_DATA_DIM, SPACE_DIM); //matrix for the query, just one line
Matrix<int> indices_R(new int[query_R.rows*SPACE_DIM], query_R.rows, NUM_NN);
Matrix<float> dists_R(new float[query_R.rows*SPACE_DIM], query_R.rows, NUM_NN);

//ROS publisher
ros::Publisher pub_cmdSet; //this allows to send a cmd msg packet to the mbed and drive the motors

//debug function to display a matrix
void displayMatrixFloat(const Matrix<float> matrix, char* name)
{
	std::cout << name << " matrix :" << std::endl;

	int count = 0;
	for(int i=0; i<matrix.rows; i++)
	{
		std::cout << count << " | ";

		for(int j=0; j<matrix.cols; j++)
		{
			std::cout << matrix.ptr()[matrix.cols*i+j] << "\t";
		}

		count += 1;
		std::cerr << std::endl;
	}
}

void displayMatrixInt(const Matrix<int> matrix, char* name)
{
	std::cout << name << " matrix :" << std::endl;

	int count = 0;
	for(int i=0; i<matrix.rows; i++)
	{
		std::cout << count << " | ";

		for(int j=0; j<matrix.cols; j++)
		{
			std::cout << matrix.ptr()[matrix.cols*i+j] << "\t";
		}

		count += 1;
		std::cerr << std::endl;
	}
}

/*** Callback functions ***/
void tracking_cb(const sensor_msgs::JointStateConstPtr& js)
{
	if(switch_node)
	{
		if(js->name.size() != 0)
		{
			//update the query
			for(int i=0; i<query_R.cols; i++) //query cols should be 1
			{
				query_L.ptr()[i] = js->position[i];
				query_R.ptr()[i] = js->position[SPACE_DIM + i];
			}

			/*** Left ***/
			// construct a randomized kd-tree index using 4 kd-trees
			Index<L2<float> > data_L_index(dataset_L_angles, flann::KDTreeIndexParams(SPACE_DIM));
			data_L_index.buildIndex();

			// do a knn search, using 128 checks
			data_L_index.knnSearch(query_L, indices_L, dists_L, SPACE_DIM, flann::SearchParams(128));
			int data_L_idx = indices_L.ptr()[0];

			//ROS_INFO("data_L_idx = %d", data_L_idx);
			//displayMatrixFloat(query_L, "Query_L");

			/*** Right ***/
			// construct a randomized kd-tree index using 4 kd-trees
			Index<L2<float> > data_R_index(dataset_R_angles, flann::KDTreeIndexParams(SPACE_DIM));
			data_R_index.buildIndex();

			// do a knn search, using 128 checks
			data_R_index.knnSearch(query_R, indices_R, dists_R, SPACE_DIM, flann::SearchParams(128));
			int data_R_idx = indices_R.ptr()[0];

			//ROS_INFO("data_R_idx = %d", data_R_idx);
			//displayMatrixFloat(query_R, "Query_R");

			//init of the packet
			for(int i=0; i<DOF_DIM; i++)
			{
				//L
				motorCmdSet.motorCmd[i].nodeID = i+1;
				motorCmdSet.motorCmd[i].mode = 0;
				motorCmdSet.motorCmd[i].value = dataset_L_positions.ptr()[dataset_L_positions.cols*data_L_idx+i];

				//R
				motorCmdSet.motorCmd[NUMBER_MAX_EPOS2_PER_SLAVE+i].nodeID = NUMBER_MAX_EPOS2_PER_SLAVE+i+1;
				motorCmdSet.motorCmd[NUMBER_MAX_EPOS2_PER_SLAVE+i].mode = 0;
				motorCmdSet.motorCmd[NUMBER_MAX_EPOS2_PER_SLAVE+i].value = dataset_R_positions.ptr()[dataset_R_positions.cols*data_R_idx+i];
			}
		}
	}
}

/*** Services ***/
bool switchNode(trs_control::switchNode::Request  &req, trs_control::switchNode::Response &res)
{
	switch_node = req.state;
	return true;
}

bool getKdTreeAnglesCmd(trs_control::getMotorCmdSet::Request  &req, trs_control::getMotorCmdSet::Response &res)
{
	//send the cmd set by the callback function
	if(switch_node)
	{
		res.motorCmdSet = motorCmdSet;
		return true;
	}
	else
	{
		return false;
	}
}

/*** Main ***/
int main (int argc, char** argv)
{
	// Initialize ROS
	ros::init (argc, argv, "trs_kdtreeAnglesToArms_server_node");
	ros::NodeHandle nh;
	ros::Rate r(LOOP_RATE);

	//Subscribers
	// Create a ROS subscriber for the input joint_states_kinect topic, given by the trs_kinectToAngles node
	//that will be the requests for the kd-trees
	ros::Subscriber sub = nh.subscribe ("/anglesArmsDescription", 10, tracking_cb);

	//Services
	ros::ServiceServer service_switchNode = nh.advertiseService("switch_kd_tree_angles", switchNode);
	ros::ServiceServer service_getKdTreeAnglesCmd = nh.advertiseService("get_kd_tree_angles_cmd", getKdTreeAnglesCmd);

	//Initialization of the kd tree
	ROS_INFO("Initialization of the 2 kd-tree :");

	//read a bag to generate 2 datasets, one for each arm
	rosbag::Bag bag(ros::package::getPath("trs_control") + "/bag/anglesToPosture_1.bag"); //change this bag
	rosbag::View view_joint(bag, rosbag::TopicQuery("/anglesArmsDescription")); //angle info
	rosbag::View view_posture(bag, rosbag::TopicQuery("/motorDataSet")); //motor data position info

	//create the 2 dataset_angle matrix
	dataset_dim = view_joint.size(); //set the dataset dim equal to the number of lines in the bag file
	Matrix<float> tempL1(new float[SPACE_DIM*dataset_dim], dataset_dim, SPACE_DIM);
	dataset_L_angles = tempL1;
	Matrix<float> tempR1(new float[SPACE_DIM*dataset_dim], dataset_dim, SPACE_DIM);
	dataset_R_angles = tempR1;

	ROS_INFO("Create angle matrix");
	int line = 0;
	BOOST_FOREACH(rosbag::MessageInstance const m, view_joint)
	{
		sensor_msgs::JointState::Ptr i = m.instantiate<sensor_msgs::JointState>();

	    if(i != NULL)
	    {
	    	//Build the right dataset_angles matrix
	    	for(int j=0; j<SPACE_DIM; j++)
	    	{
	    		dataset_L_angles.ptr()[dataset_L_angles.cols*line+j] = i->position[j]; //offset of 4 for left
	    		dataset_R_angles.ptr()[dataset_R_angles.cols*line+j] = i->position[SPACE_DIM + j];
	    	}
	    }
		else
			std::cout << "null" << std::endl;

	    line++;
	}

	ROS_INFO("Create posture matrix");
	//create the dataset_positions matrix
	int dataset_post_dim = view_posture.size(); //set the dataset dim equal to the number of lines in the bag file
	Matrix<int> tempL2(new int[DOF_DIM*dataset_post_dim], dataset_post_dim, DOF_DIM);
	dataset_L_positions = tempL2;
	Matrix<int> tempR2(new int[DOF_DIM*dataset_post_dim], dataset_post_dim, DOF_DIM);
	dataset_R_positions = tempR2;

	line = 0;
	BOOST_FOREACH(rosbag::MessageInstance const m, view_posture) //error compiles ok
	{
		trs_msgs::MotorDataSet::Ptr i = m.instantiate<trs_msgs::MotorDataSet>();

		if(i != NULL)
		{
			//i->header.stamp = ros::Time::now();

			//Build the dataset_positions matrix
			for(int j=0; j<DOF_DIM; j++)
			{
				dataset_L_positions.ptr()[dataset_L_positions.cols*line+j] = i->motorData[j].encPosition;
				dataset_R_positions.ptr()[dataset_R_positions.cols*line+j] = i->motorData[NUMBER_MAX_EPOS2_PER_SLAVE + j].encPosition;
			}
		}
		else
			std::cout << "null" << std::endl;

		line++;
	}

	bag.close();

	//#ifdef TRS_DEBUG
	displayMatrixFloat(dataset_L_angles, "dataset_L_angles");
	displayMatrixInt(dataset_L_positions, "dataset_L_positions");
	displayMatrixFloat(dataset_R_angles, "dataset_R_angles");
	displayMatrixInt(dataset_R_positions, "dataset_R_positions");
	//#endif

	while(ros::ok())
	{
		ros::spinOnce();
		//ROS_INFO("spinOnce");
		r.sleep();
	}

	//free matrix pointers from memory

	delete[] dataset_L_angles.ptr();
	delete[] dataset_L_positions.ptr();
	delete[] query_L.ptr();
	delete[] indices_L.ptr();
	delete[] dists_L.ptr();

	delete[] dataset_R_angles.ptr();
	delete[] dataset_R_positions.ptr();
	delete[] query_R.ptr();
	delete[] indices_R.ptr();
	delete[] dists_R.ptr();

	return 0;
}