#pragma once
#include <mtao/types.h>
#include <mtao/eigen_utils.h>
#include <Eigen/Sparse>
#include <boost/hana.hpp>

//Use DECMesh by inheriting it and manually implementing boundary / cell volumes / dual cell volumes / the cell counts per type
//
//CachedDECMesh caches the boundary computatoins / cell volume information for when it's expensive to compute these things
//
//DECMeshCore implements DEC h(hodge dual) d(exterior derivative) cod(codifferential operator as well as
//  the various laplacians (d cod / cod d / Laplace deRham (cod d + d cod) / weak laplacian (down) d cod h / weak laplacian (up) h cod d )
//  Note: standard cotan laplacian is weak_laplacian_up<0>()

namespace mtao { namespace geometry { namespace mesh {
template <typename Derived, typename T, int EmbeddedDim, int Dim_>
class DECMeshCore {
    public:
        using types = mtao::embedded_types<T,EmbeddedDim>;
        constexpr static int Dim = Dim_;
        using Scalar = typename types::Scalar;
        using VecX = typename types::VectorX;
        using SparseMatrix = Eigen::SparseMatrix<Scalar>;

        Derived& derived() { return *static_cast<Derived*>(this); }
        const Derived& derived() const { return *static_cast<const Derived*>(this); }

        template <int D>
        SparseMatrix boundary() const { return derived().template boundary<D>(); }
        template <int D>
        VecX volume() const { return derived().template volume<D>(); }
        template <int D>
        VecX dual_volume() const { return derived().template dual_volume<D>(); }

        //base operators
        template <int D>
        VecX h() const;
        template <int D>
        VecX hi() const;
        template <int D>
        SparseMatrix d() const;
        template <int D>
        SparseMatrix cod() const;

        template <int D>
        VecX h(const VecX& o) const { return h<D>().asDiagonal() * o; }
        //inverse hodge
        template <int D>
        VecX hi(const VecX& o) const { return hi<D>().asDiagonal() * o; }
        template <int D>
        VecX d(const VecX& o) const { return d<D>() * o; }
        //hi<D>dt<D-1>h<D>
        template <int D>
        VecX cod(const VecX& o) const { return cod<D>() * o; }

        //laplacians
        //d<D-1>hi<D>dt<D-1>h<D>
        template <int D>
        SparseMatrix laplacian_down() const;
        //hi<D>dt<D>h<D+1>d<D>
        template <int D>
        SparseMatrix laplacian_up() const;
        //d<D-1>hi<D>dt<D-1>h<D> + hi<D>dt<D>h<D+1>d<D>
        template <int D>
        SparseMatrix laplacian() const;
        //d<D-1>hi<D>dt<D-1>   (h<D>)
        template <int D>
        SparseMatrix weak_laplacian_down() const;
        //(hi<D>)    dt<D>h<D+1>d<D>
        template <int D>
        SparseMatrix weak_laplacian_up() const;

        template <int D>
        VecX weak_poisson_down(const VecX& o) const;
        template <int D>
        VecX weak_poisson_up(const VecX& o) const;
        template <int D>
        VecX weak_poisson_down_rhs(const VecX& o) const;
        template <int D>
        VecX weak_poisson_up_rhs(const VecX& o) const;

        //Find the ___ part according to HH decomposition
        //i.e o = da + hdhb + g

        //a
        template <int D>
        VecX coexact_down(const VecX& o) const ;
        //b
        template <int D>
        VecX exact_up(const VecX& o) const ;
        //da
        template <int D>
        VecX exact(const VecX& o) const ;
        //hdhb
        template <int D>
        VecX coexact(const VecX& o) const ;
        //da + g i.e o - hdhb
        template <int D>
        VecX closed(const VecX& o) const ;
        //hdhb + g i.e o - da
        template <int D>
        VecX coclosed(const VecX& o) const ;

};

template <typename Derived, typename T, int EmbeddedDim, int Dim_>
class DECMesh: public DECMeshCore<DECMesh<Derived, T, EmbeddedDim, Dim_>, T, EmbeddedDim, Dim_> {
    public:
        using types = mtao::embedded_types<T,EmbeddedDim>;
        constexpr static int Dim = Dim_;
        using Scalar = typename types::Scalar;
        using VecX = typename types::VectorX;
        using SparseMatrix = Eigen::SparseMatrix<Scalar>;
        using Mesh = DECMeshCore<DECMesh<Derived,T,EmbeddedDim,Dim>,T,EmbeddedDim,Dim>;

        Derived& derived() { return *static_cast<Derived*>(this); }
        const Derived& derived() const { return *static_cast<const Derived*>(this); }

        template <int D>
        SparseMatrix boundary() const { return derived().template boundary<D>(); }
        /*
        template <int C>
        auto volume(int i) const { return derived().template volume<C>(i); }
        template <int C>
        auto dual_volume(int i) const { return derived().template dual_volume<C>(i); }
        */
        template <int C>
        int cell_count() const { return derived().template cell_count<C>(); }
        template <int C>
        int dual_cell_count() const { return derived().template dual_cell_count<C>(); }
        template <int D>
        VecX volume() const { return derived().template volume<D>(); }
        template <int D>
        VecX dual_volume() const { return derived().template dual_volume<D>(); }
};
template <typename T, int EmbeddedDim, int Dim_>
class CachedDECMesh: public DECMesh<CachedDECMesh<T, EmbeddedDim,Dim_>, T, EmbeddedDim, Dim_> {
    public:
        using types = mtao::embedded_types<T,EmbeddedDim>;
        constexpr static int Dim = Dim_;
        using Scalar = typename types::Scalar;
        using VecX = typename types::VectorX;
        using SparseMatrix = Eigen::SparseMatrix<Scalar>;
        template <typename Derived>
            CachedDECMesh(const DECMesh<Derived,T,EmbeddedDim,Dim>& d) {
                using namespace boost;
                boost::hana::for_each(hana::make_range(hana::int_c<0>,hana::int_c<Dim+1>), [&](auto t) {
                        constexpr static int D = decltype(t)::value;
                        m_volume[D] = d.template volume<D>();
                        m_dual_volume[D] = d.template dual_volume<D>();
                        });
                boost::hana::for_each(hana::make_range(hana::int_c<1>,hana::int_c<Dim+1>), [&](auto t) {
                        constexpr static int D = decltype(t)::value;
                        m_boundary[D-1] = d.template boundary<D>();

                        });
            }

        template <int C>
        const SparseMatrix& boundary() const { return m_boundary[C-1]; }
        template <int C>
        auto volume(int i) const { return volume<C>()(i); }
        template <int C>
        auto dual_volume(int i) const { return dual_volume<C>()(i); }
        template <int C>
        int cell_count() const { return m_volume[C].rows();}
        template <int C>
        int dual_cell_count() const { return m_dual_volume[C].rows();}
        template <int C>
        const VecX& volume() const { return m_volume[C]; }
        template <int C>
        const VecX& dual_volume() const { return m_dual_volume[C]; }
    private:
        std::array<SparseMatrix, Dim> m_boundary;
        std::array<VecX, Dim+1> m_volume;
        std::array<VecX, Dim+1> m_dual_volume;

};

/*
template <typename Derived, typename T, int EmbeddedDim, int Dim>
template <int D>
auto DECMesh<Derived,T,EmbeddedDim,Dim>::volume() const -> VecX {
    int s = cell_count<D>();
    VecX v = VecX::Zero(s);
    for(int i = 0; i < s; ++i) {
        v(i) = volume<D>(i);
    }
    return v;
}
template <typename Derived, typename T, int EmbeddedDim, int Dim>
template <int D>
auto DECMesh<Derived,T,EmbeddedDim,Dim>::dual_volume() const -> VecX {
    int s = dual_cell_count<D>();
    VecX v = VecX::Zero(s);
    for(int i = 0; i < s; ++i) {
        v(i) = dual_volume<D>(i);
    }
    return v;
}
*/

//Base operators
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::h() const -> VecX {
    auto p = volume<D>();
    auto d = dual_volume<D>();

    VecX ret = d.cwiseQuotient(p);
    return mtao::eigen::finite(ret);
}
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::hi() const -> VecX {
    auto p = volume<D>();
    auto d = dual_volume<D>();

    VecX ret = p.cwiseQuotient(d);
    return mtao::eigen::finite(ret);
}
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::d() const -> SparseMatrix {
    return boundary<D+1>().transpose();
}

// * d *
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::cod() const -> SparseMatrix {
    return hi<D-1>().asDiagonal() * d<D-1>().transpose() * h<D>().asDiagonal();
}

//Laplacians
//d * d *
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>:: laplacian_down() const -> SparseMatrix {
    return d<D-1>() * cod<D>();
}
//* d * d
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>:: laplacian_up() const -> SparseMatrix {
    //h<1>
    return cod<D+1>() * d<D>();
}
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>:: laplacian() const -> SparseMatrix {
    if constexpr(D == 0) {
        return laplacian_up<D>();
    } else if(D == Dim) {
        return laplacian_down<D>();
    } else {
        return laplacian_down<D>() + laplacian_down<D>();
    }
}
//d * d that goes up down a dimension
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>:: weak_laplacian_down() const -> SparseMatrix {
    auto a = d<D-1>();
    return a * hi<D-1>().asDiagonal() * a.transpose();
}
//d * d that goes up a dimension
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>:: weak_laplacian_up() const -> SparseMatrix {
    auto a = d<D>();
    return a.transpose() * h<D+1>().asDiagonal() * a;
}
//d x
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::weak_poisson_down_rhs(const VecX& o) const -> VecX {
    return d<D>(o);
}
//d * x
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::weak_poisson_up_rhs(const VecX& o) const -> VecX {
    return d<D-1>().transpose() * h<D>(o);
}
//(d * d )^{-1} ( d x )
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::weak_poisson_down(const VecX& rhs) const -> VecX {
    auto L = weak_laplacian_down<D>();
    Eigen::ConjugateGradient<SparseMatrix, Eigen::Upper|Eigen::Lower> solver(L);
    VecX p = solver.solveWithGuess(rhs,VecX::Zero(rhs.rows()));
    return p;
}
//(d * d )^{-1} ( d * x )
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::weak_poisson_up(const VecX& rhs) const -> VecX {
    auto L = weak_laplacian_up<D>();
    Eigen::ConjugateGradient<SparseMatrix, Eigen::Upper|Eigen::Lower> solver(L);
    VecX p = solver.solveWithGuess(rhs,VecX::Zero(rhs.rows()));
    return p;
}

//Hodge decomposition components
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::exact_up(const VecX& o) const -> VecX {
    //d (u - HdHp) = 0
    //dHdHp = du
    //d^THdHp = du
    //Hp =(d^THd)^{-1} du

    //d<D> | hi<D>dt<D>   (h<D+1>)

    return hi<D+1>(weak_poisson_down<D+1>(weak_poisson_down_rhs<D>(o)));
}
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::coexact_down(const VecX& o) const -> VecX {
    //HdH (u - dp) = 0
    //HdHdp = HdHu
    //dHd^Tp = d^THu
    //p =(dHd^T)^{-1} dHu

    //(hi<D>)    dt<D-1>h<D> | d<D-1>
    return weak_poisson_up<D-1>(weak_poisson_up_rhs<D>(o));
}
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::exact(const VecX& o) const -> VecX {
    return d<D-1>(coexact_down<D>(o));
}
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::coexact(const VecX& o) const -> VecX {
    return cod<D+1>(exact_up<D>(o));
}
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::closed(const VecX& o) const -> VecX {
    return o - coexact<D>(o);
}
template <typename Derived, typename T, int EmbeddedDim, int Dim> template <int D>
auto DECMeshCore<Derived,T,EmbeddedDim,Dim>::coclosed(const VecX& o) const -> VecX {
    return o - exact<D>(o);
}
}}}


#include <iostream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>

#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <math.h>
#include <random>

using namespace std;

pcl::search::KdTree<pcl::PointXYZ> kdtreeLocalMap;
vector<int>     pointIdxRadiusSearch;
vector<float>   pointRadiusSquaredDistance;

random_device rd;
default_random_engine eng(rd());
uniform_real_distribution<double>  rand_x;
uniform_real_distribution<double>  rand_y;
uniform_real_distribution<double>  rand_w;
uniform_real_distribution<double>  rand_h;

ros::Publisher _local_map_pub;
ros::Publisher _all_map_pub;
ros::Subscriber _odom_sub;

vector<double> _state;

int _obs_num;
double _x_size, _y_size, _z_size;
double _x_l, _x_h, _y_l, _y_h, _w_l, _w_h, _h_l, _h_h;
double _z_limit, _sensing_range, _resolution, _sense_rate, _init_x, _init_y;

bool _map_ok = false;
bool _has_odom = false;

sensor_msgs::PointCloud2 localMap_pcd;
sensor_msgs::PointCloud2 globalMap_pcd;
pcl::PointCloud<pcl::PointXYZ> cloudMap;

void RandomMapGenerate()
{
      pcl::PointXYZ pt_random;

      rand_x = uniform_real_distribution<double>(_x_l, _x_h );
      rand_y = uniform_real_distribution<double>(_y_l, _y_h );
      rand_w = uniform_real_distribution<double>(_w_l, _w_h);
      rand_h = uniform_real_distribution<double>(_h_l, _h_h);

      for(int i = 0; i < _obs_num; i ++)
      {
         double x, y, w, h;
         x    = rand_x(eng);
         y    = rand_y(eng);
         w    = rand_w(eng);

         if(sqrt( pow(x-_init_x, 2) + pow(y-_init_y, 2) ) < 2.0 )
            continue;

         x = floor(x/_resolution) * _resolution + _resolution / 2.0;
         y = floor(y/_resolution) * _resolution + _resolution / 2.0;

         int widNum = ceil(w/_resolution);

         for(int r = -widNum/2.0; r < widNum/2.0; r ++ )
            for(int s = -widNum/2.0; s < widNum/2.0; s ++ ){
               h    = rand_h(eng);
               int heiNum = ceil(h/_resolution);
               for(int t = 0; t < heiNum; t ++ ){
                  pt_random.x = x + (r+0.5) * _resolution;
                  pt_random.y = y + (s+0.5) * _resolution;
                  pt_random.z = (t+0.5) * _resolution;
                  cloudMap.points.push_back( pt_random );
               }
            }
      }

      cloudMap.width = cloudMap.points.size();
      cloudMap.height = 1;
      cloudMap.is_dense = true;

      ROS_WARN("Finished generate random map ");

      kdtreeLocalMap.setInputCloud( cloudMap.makeShared() );

      _map_ok = true;
}

void rcvOdometryCallbck(const nav_msgs::Odometry odom)
{
      if (odom.child_frame_id == "X" || odom.child_frame_id == "O") return ;
      _has_odom = true;

      _state = {
         odom.pose.pose.position.x,
         odom.pose.pose.position.y,
         odom.pose.pose.position.z,
         odom.twist.twist.linear.x,
         odom.twist.twist.linear.y,
         odom.twist.twist.linear.z,
         0.0, 0.0, 0.0
      };
}

int i = 0;
void pubSensedPoints()
{
      if(i < 10 )
      {
         pcl::toROSMsg(cloudMap, globalMap_pcd);
         globalMap_pcd.header.frame_id = "world";
         _all_map_pub.publish(globalMap_pcd);
      }

      i ++;
      if(!_map_ok || !_has_odom)
         return;

      pcl::PointCloud<pcl::PointXYZ> localMap;

      pcl::PointXYZ searchPoint(_state[0], _state[1], _state[2]);
      pointIdxRadiusSearch.clear();
      pointRadiusSquaredDistance.clear();

      pcl::PointXYZ pt;

      if(isnan(searchPoint.x) || isnan(searchPoint.y) || isnan(searchPoint.z))
         return;

      if ( kdtreeLocalMap.radiusSearch (searchPoint, _sensing_range, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
      {
         for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
         {
            pt = cloudMap.points[pointIdxRadiusSearch[i]];
            localMap.points.push_back(pt);
         }

      }
      else{
         ROS_ERROR("[Map server] No obstacles .");
         return;
      }

      localMap.width = localMap.points.size();
      localMap.height = 1;
      localMap.is_dense = true;

      pcl::toROSMsg(localMap, localMap_pcd);
      localMap_pcd.header.frame_id = "world";
      _local_map_pub.publish(localMap_pcd);
}

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

      ros::init (argc, argv, "random_map_sensing");
      ros::NodeHandle n( "~" );

      _local_map_pub = n.advertise<sensor_msgs::PointCloud2>("random_forest", 1);
      _all_map_pub   = n.advertise<sensor_msgs::PointCloud2>("all_map", 1);

      _odom_sub = n.subscribe( "odometry", 50, rcvOdometryCallbck );

      n.param("init_state_x", _init_x,       0.0);
      n.param("init_state_y", _init_y,       0.0);

      n.param("map/x_size",  _x_size, 50.0);
      n.param("map/y_size",  _y_size, 50.0);
      n.param("map/z_size",  _z_size, 5.0 );
      n.param("map/obs_num", _obs_num,  30);
      n.param("map/resolution",  _resolution, 0.2);

      n.param("ObstacleShape/lower_rad", _w_l,   0.3);
      n.param("ObstacleShape/upper_rad", _w_h,   0.8);
      n.param("ObstacleShape/lower_hei", _h_l,   3.0);
      n.param("ObstacleShape/upper_hei", _h_h,   7.0);

      n.param("sensing/radius", _sensing_range, 10.0);
      n.param("sensing/radius", _sense_rate, 10.0);

      _x_l = - _x_size / 2.0;
      _x_h = + _x_size / 2.0;

      _y_l = - _y_size / 2.0;
      _y_h = + _y_size / 2.0;

      _obs_num = min(_obs_num, (int)_x_size * 10);
      _z_limit = _z_size;

      RandomMapGenerate();

      ros::Rate loop_rate(_sense_rate);

      while (ros::ok())
      {
        pubSensedPoints();
        ros::spinOnce();
        loop_rate.sleep();
      }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_MOD_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_MOD_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/mod.hpp>
#include <boost/simd/include/functions/simd/selsub.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/divfloor.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::mod_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return selsub(is_nez(a1), a0, divfloor(a0,a1)*a1);
    }
  };
} } }

#endif


#include <Eigen/Core>
#include <Eigen/Geometry>
// GVDB library
#include <cuda_runtime_api.h>
#include <mrfmap/GVDBInference.h>
#include <mrfmap/GVDBOctomapWrapper.h>
#include <mrfmap/KeyframeSelector.h>
#include <mrfmap/Viewer.h>
#include <yaml-cpp/yaml.h>

#include <algorithm>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#include <thread>

gvdb_params_t gvdb_params_;

template <class Matrix>
void read_binary(const std::string& filename, Matrix& matrix) {
  std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
  typename Matrix::Index rows = 0, cols = 0;
  in.read((char*)(&rows), sizeof(typename Matrix::Index));
  in.read((char*)(&cols), sizeof(typename Matrix::Index));
  matrix.resize(rows, cols);
  in.read((char*)matrix.data(), rows * cols * sizeof(typename Matrix::Scalar));
  in.close();
}
int main(int argc, const char** argv) {
  int num_cams_ = 12;
  std::string params_file = CONFIG_PATH;

  gvdb_params_.load_from_file(params_file);

  KeyframeSelector selector(0.1f, 0.1f);
  PangolinViewer viewer(std::string("MRFMap Viewer"), false);

  YAML::Node params = YAML::LoadFile(params_file);
  YAML::Node rosviewer_params_node = params["viewer_params_nodes"];
  YAML::Node cam_in_body_node = rosviewer_params_node["cam_in_body"];
  Eigen::MatrixXf cam_in_body = Eigen::Matrix4f::Identity(), pose = Eigen::Matrix4f::Identity();
  Eigen::MatrixXf depth_img = Eigen::MatrixXf::Zero(gvdb_params_.rows, gvdb_params_.cols);

  if (cam_in_body_node) {
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 4; ++j) {
        cam_in_body(i, j) = cam_in_body_node[i * 4 + j].as<float>();
      }
    }
  }

  for (uint cam_id = 0; cam_id < num_cams_; ++cam_id) {
    // Load image
    std::string data_path = DATA_PATH;
    read_binary(data_path + "pose_" + std::to_string(cam_id) + ".bin", pose);
    pose = pose * cam_in_body;
    read_binary(data_path + "depth_" + std::to_string(cam_id) + ".bin", depth_img);

    std::shared_ptr<GVDBImage> image_ptr = std::make_shared<GVDBImage>(depth_img);
    if (selector.is_keyframe(pose)) {
      viewer.add_keyframe(pose, depth_img);
    } else {
      viewer.add_frame(pose, depth_img);
    }
    sleep(1);
  }
  return 0;
}


#ifndef DELTA_REFERENCE_HPP
#define DELTA_REFERENCE_HPP

/// 3rd party includes
#include <Eigen/Core>
#include <opencv2/core.hpp>

using FEN = std::string;
using Photo = cv::Mat;

/**
 * complete state of the delta robot (with many redundant variables to trade-off having to compute forward dynamics
 * all the time)
 */
class State {
protected:
    Eigen::Matrix<double, 3, 3> joint_angles_;
    Eigen::Matrix<double, 3, 3> joint_ang_vel_; // joint angular velocities
    Eigen::Matrix<double, 3, 3> joint_ang_acc_; // joint angular accelerations
    Eigen::Matrix<double, 3, 1> ee_pos_; // end-effector position
    Eigen::Matrix<double, 3, 1> ee_vel_; // end-effector velocity
    Eigen::Matrix<double, 3, 1> ee_acc_; // end-effector acceleration

public:
    virtual ~State() = default;
};

/**
 * contains the same member data as the State class, but with covariance matrices for each variable
 */
class EstimatedState : public State {
protected:
    Eigen::Matrix<double, 3, 3> joint_angle_cov_;
    Eigen::Matrix<double, 3, 3> joint_ang_vel_cov_;
    Eigen::Matrix<double, 3, 3> joint_ang_acc_cov_;
    Eigen::Matrix<double, 3, 1> ee_pos_cov_;
    Eigen::Matrix<double, 3, 1> ee_vel_cov_;
    Eigen::Matrix<double, 3, 1> ee_acc_cov_;
};

/**
 * just the reference joint angle, angular velocity, and acceleration
 */
class Reference {
protected:
    Eigen::Matrix<double, 3, Eigen::Dynamic> joint_angles_;
    Eigen::Matrix<double, 3, Eigen::Dynamic> joint_ang_vel_; // joint angular velocities
    Eigen::Matrix<double, 3, Eigen::Dynamic> joint_ang_acc_; // joint angular accelerations
};

/**
 * discrete-time motor torques (output of control algorithm)
 */
class MotorTorques {
protected:
    Eigen::Matrix<double, 3, Eigen::Dynamic> tau_; // discrete-time torques (at every kth instant) -> I think I'm fundamentally limited by the PWM duty cycle (or whatever it's called) for my motor (I think it's around 20ms -> 50Hz)
};

/**
 * discrete-time motor commands (transformed motor torques that are actionable by the I/O thread)
 */
class MotorCommands {
protected:
    Eigen::Matrix<double, 3, Eigen::Dynamic> commands_; // discrete-time commands for the motors in terms of angle to go to next
};

/**
 * concatenation of sensor data for single IMU and 3 motor encoders at fixed time points for an indefinite number of
 * time points
 */
class SensorReadings {
protected:
    Eigen::Matrix<double, 9, Eigen::Dynamic> imu_data_; // not sure what the exact physical quantities returned by the HillCrest FSM 300 are, or how many there are, but I'm guessing maybe 9
    Eigen::Matrix<double, 3, Eigen::Dynamic> mot_encoder_data_; // this is not really data it's an inference of what the motor position should be given the history of inputs
};

#endif //DELTA_REFERENCE_HPP


#include <stan/math/prim/scal.hpp>
#include <boost/math/distributions.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>

TEST(ProbDistributionsExpModNormal, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::exp_mod_normal_rng(10.0, 2.0, 1.0, rng));

  EXPECT_THROW(stan::math::exp_mod_normal_rng(10.0, 2.0,-1.0, rng),
               std::domain_error);
  EXPECT_THROW(stan::math::exp_mod_normal_rng(10.0,-2.0, 1.0, rng),
               std::domain_error);
  EXPECT_THROW(stan::math::exp_mod_normal_rng(10.0, 2,
                                              stan::math::positive_infinity(),
                                              rng),
               std::domain_error);
  EXPECT_THROW(stan::math::exp_mod_normal_rng(stan::math::positive_infinity(), 2,
                                              1.0, rng)
               ,std::domain_error);
}


// This file is part of the pyMOR project (http://www.pymor.org).
// Copyright 2013-2018 pyMOR developers and contributors. All rights reserved.
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#include "elasticity.hh"

#include <deal.II/lac/solver_bicgstab.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/numerics/solution_transfer.h>

#include <fstream>

//! Wrapper intended for use w/ CG algorithms
template <class Number>
class MatrixSum {

public:
  typedef Number value_type;
  typedef std::vector<const dealii::SparseMatrix<Number>*> Matrices;
  MatrixSum(Matrices&& m, std::vector<Number> weights)
    : matrices_(m) {
    assert(m.size() > 0);
    assert(m.size() == weights.size());
    sum_.reinit(matrices_[0]->get_sparsity_pattern());
    sum_.copy_from(*matrices_[0]);
    sum_ *= weights[0];
    for (size_t i = 1; i < m.size(); ++i) {
      sum_.add(weights[i], *matrices_[1]);
    }
  }

  dealii::SparseMatrix<Number>& sum() { return sum_; }

  const dealii::SparseMatrix<Number>& sum() const { return sum_; }

private:
  const Matrices matrices_;
  dealii::SparseMatrix<Number> sum_;
};

ElasticityExample::ElasticityExample(int refine_steps)
  : dof_handler_(triangulation_)
  , fe_(FE_Q<dim>(1), dim) {
  GridGenerator::hyper_cube(triangulation_, -1, 1);
  refine_global(refine_steps);
}

ElasticityExample::~ElasticityExample() { dof_handler_.clear(); }

void ElasticityExample::setup_system() {
  dof_handler_.clear();
  dof_handler_.distribute_dofs(fe_);
  sparsity_pattern_.reinit(dof_handler_.n_dofs(), dof_handler_.n_dofs(), dof_handler_.max_couplings_between_dofs());
  DoFTools::make_sparsity_pattern(dof_handler_, sparsity_pattern_);

  sparsity_pattern_.compress();

  h1_matrix_.reinit(sparsity_pattern_);
  lambda_system_matrix_.reinit(sparsity_pattern_);
  mu_system_matrix_.reinit(sparsity_pattern_);
  system_rhs_.reinit(dof_handler_.n_dofs());
  tmp_data_.reinit(dof_handler_.n_dofs());
}

void ElasticityExample::assemble_h1() {
  QGauss<dim> quadrature_formula(2);

  FEValues<dim> fe_values(fe_, quadrature_formula,
                          update_values | update_gradients | update_quadrature_points | update_JxW_values);

  const unsigned int dofs_per_cell = fe_.dofs_per_cell;
  const unsigned int n_q_points = quadrature_formula.size();

  FullMatrix<Number> h1_cell_matrix(dofs_per_cell, dofs_per_cell);
  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

  // Now we can begin with the loop over all cells:
  typename DoFHandler<dim>::active_cell_iterator cell = dof_handler_.begin_active(), endc = dof_handler_.end();
  for (; cell != endc; ++cell) {
    h1_cell_matrix = 0;
    fe_values.reinit(cell);

    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
      const unsigned int component_i = fe_.system_to_component_index(i).first;

      for (unsigned int j = 0; j < dofs_per_cell; ++j) {
        const unsigned int component_j = fe_.system_to_component_index(j).first;

        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
          h1_cell_matrix(i, j) += fe_values.shape_grad(i, q_point)[component_i] *
                                  fe_values.shape_grad(j, q_point)[component_j] * fe_values.JxW(q_point);
        }
      }
    }

    cell->get_dof_indices(local_dof_indices);
    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
      for (unsigned int j = 0; j < dofs_per_cell; ++j) {
        h1_matrix_.add(local_dof_indices[i], local_dof_indices[j], h1_cell_matrix(i, j));
      }
    }
  }

  std::map<types::global_dof_index, Number> boundary_values;
  VectorTools::interpolate_boundary_values(dof_handler_, 0, ZeroFunction<dim>(dim), boundary_values);
  MatrixTools::apply_boundary_values(boundary_values, h1_matrix_, tmp_data_, system_rhs_);
}

void ElasticityExample::assemble_system() {
  QGauss<dim> quadrature_formula(2);

  FEValues<dim> fe_values(fe_, quadrature_formula,
                          update_values | update_gradients | update_quadrature_points | update_JxW_values);

  const unsigned int dofs_per_cell = fe_.dofs_per_cell;
  const unsigned int n_q_points = quadrature_formula.size();

  FullMatrix<Number> lambda_cell_matrix(dofs_per_cell, dofs_per_cell), mu_cell_matrix(dofs_per_cell, dofs_per_cell);
  Vector<Number> cell_rhs(dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

  // As was shown in previous examples as well, we need a place where to
  // store the values of the coefficients at all the quadrature points on a
  // cell. In the present situation, we have two coefficients, lambda and
  // mu.
  std::vector<Number> lambda_values(n_q_points);
  std::vector<Number> mu_values(n_q_points);

  // Well, we could as well have omitted the above two arrays since we will
  // use constant coefficients for both lambda and mu, which can be declared
  // like this. They both represent functions always returning the constant
  // value 1.0. Although we could omit the respective factors in the
  // assemblage of the matrix, we use them here for purpose of
  // demonstration.
  Functions::ConstantFunction<dim> lambda(1), mu(1);

  // Then again, we need to have the same for the right hand side. This is
  // exactly as before in previous examples. However, we now have a
  // vector-valued right hand side, which is why the data type of the
  // rhs_values array is changed. We initialize it by
  // n_q_points elements, each of which is a
  // Vector@<double@> with dim elements.
  RightHandSide<dim> right_hand_side;
  std::vector<Vector<Number>> rhs_values(n_q_points, Vector<Number>(dim));

  // Now we can begin with the loop over all cells:
  typename DoFHandler<dim>::active_cell_iterator cell = dof_handler_.begin_active(), endc = dof_handler_.end();
  for (; cell != endc; ++cell) {
    lambda_cell_matrix = 0;
    mu_cell_matrix = 0;
    cell_rhs = 0;

    fe_values.reinit(cell);

    // Next we get the values of the coefficients at the quadrature
    // points. Likewise for the right hand side:
    lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
    mu.value_list(fe_values.get_quadrature_points(), mu_values);

    right_hand_side.vector_value_list(fe_values.get_quadrature_points(), rhs_values);

    // Then assemble the entries of the local stiffness matrix and right
    // hand side vector. This follows almost one-to-one the pattern
    // described in the introduction of this example.  One of the few
    // comments in place is that we can compute the number
    // comp(i), i.e. the index of the only nonzero vector
    // component of shape function i using the
    // fe.system_to_component_index(i).first function call
    // below.
    //
    // (By accessing the first variable of the return value
    // of the system_to_component_index function, you might
    // already have guessed that there is more in it. In fact, the
    // function returns a std::pair@<unsigned int, unsigned
    // int@>, of which the first element is comp(i)
    // and the second is the value base(i) also noted in the
    // introduction, i.e.  the index of this shape function within all the
    // shape functions that are nonzero in this component,
    // i.e. base(i) in the diction of the introduction. This
    // is not a number that we are usually interested in, however.)
    //
    // With this knowledge, we can assemble the local matrix
    // contributions:
    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
      const unsigned int component_i = fe_.system_to_component_index(i).first;

      for (unsigned int j = 0; j < dofs_per_cell; ++j) {
        const unsigned int component_j = fe_.system_to_component_index(j).first;

        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
          lambda_cell_matrix(i, j) +=
              // The first term is (lambda d_i u_i, d_j v_j) + (mu d_i
              // u_j, d_j v_i).  Note that
              // shape_grad(i,q_point) returns the
              // gradient of the only nonzero component of the i-th
              // shape function at quadrature point q_point. The
              // component comp(i) of the gradient, which
              // is the derivative of this only nonzero vector
              // component of the i-th shape function with respect to
              // the comp(i)th coordinate is accessed by the appended
              // brackets.
              fe_values.shape_grad(i, q_point)[component_i] * fe_values.shape_grad(j, q_point)[component_j] *
              lambda_values[q_point] * fe_values.JxW(q_point);
          mu_cell_matrix(i, j) +=
              (fe_values.shape_grad(i, q_point)[component_j] * fe_values.shape_grad(j, q_point)[component_i] *
                   mu_values[q_point] +
               // The second term is (mu nabla u_i, nabla v_j).  We
               // need not access a specific component of the
               // gradient, since we only have to compute the scalar
               // product of the two gradients, of which an
               // overloaded version of the operator* takes care, as
               // in previous examples.
               //
               // Note that by using the ?: operator, we only do this
               // if comp(i) equals comp(j), otherwise a zero is
               // added (which will be optimized away by the
               // compiler).
               ((component_i == component_j)
                    ? (fe_values.shape_grad(i, q_point) * fe_values.shape_grad(j, q_point) * mu_values[q_point])
                    : 0)) *
              fe_values.JxW(q_point);
        }
      }
    }

    // Assembling the right hand side is also just as discussed in the
    // introduction:
    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
      const unsigned int component_i = fe_.system_to_component_index(i).first;

      for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
        cell_rhs(i) += fe_values.shape_value(i, q_point) * rhs_values[q_point](component_i) * fe_values.JxW(q_point);
    }

    // The transfer from local degrees of freedom into the global matrix
    // and right hand side vector does not depend on the equation under
    // consideration, and is thus the same as in all previous
    // examples. The same holds for the elimination of hanging nodes from
    // the matrix and right hand side, once we are done with assembling
    // the entire linear system:
    cell->get_dof_indices(local_dof_indices);
    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
      for (unsigned int j = 0; j < dofs_per_cell; ++j) {
        lambda_system_matrix_.add(local_dof_indices[i], local_dof_indices[j], lambda_cell_matrix(i, j));
        mu_system_matrix_.add(local_dof_indices[i], local_dof_indices[j], mu_cell_matrix(i, j));
      }

      system_rhs_(local_dof_indices[i]) += cell_rhs(i);
    }
  }
  // The interpolation of the boundary values needs a small modification:
  // since the solution function is vector-valued, so need to be the
  // boundary values. The ZeroFunction constructor accepts a
  // parameter that tells it that it shall represent a vector valued,
  // constant zero function with that many components. By default, this
  // parameter is equal to one, in which case the ZeroFunction
  // object would represent a scalar function. Since the solution vector has
  // dim components, we need to pass dim as number
  // of components to the zero function as well.
  std::map<types::global_dof_index, Number> boundary_values;
  VectorTools::interpolate_boundary_values(dof_handler_, 0, ZeroFunction<dim>(dim), boundary_values);
  MatrixTools::apply_boundary_values(boundary_values, mu_system_matrix_, tmp_data_, system_rhs_);
  MatrixTools::apply_boundary_values(boundary_values, lambda_system_matrix_, tmp_data_, system_rhs_);
}

void ElasticityExample::_solve(Parameter param, VectorType& solution) {
  SolverControl solver_control(20000, 1e-12);
  SolverCG<> cg(solver_control);

  deallog << "Solving for " << dof_handler_.n_dofs() << " unknowns" << std::endl;
  Number lambda(param["lambda"][0]), mu(param["mu"][0]);
  MatrixSum<Number> msum({&lambda_system_matrix_, &mu_system_matrix_}, {lambda, mu});

  auto& sum = msum.sum();
  PreconditionSSOR<> preconditioner;
  preconditioner.initialize(sum, 1.2);
  cg.solve(sum, solution, system_rhs_, preconditioner);
}

void ElasticityExample::visualize(const VectorType& solution, std::string filename) const {
  std::ofstream output(filename);

  DataOut<dim> data_out;
  data_out.attach_dof_handler(dof_handler_);

  // As said above, we need a different name for each component of the
  // solution function. To pass one name for each component, a vector of
  // strings is used. Since the number of components is the same as the
  // number of dimensions we are working in, the following
  // switch statement is used.
  //
  // We note that some graphics programs have restriction as to what
  // characters are allowed in the names of variables. The library therefore
  // supports only the minimal subset of these characters that is supported
  // by all programs. Basically, these are letters, numbers, underscores,
  // and some other characters, but in particular no whitespace and
  // minus/hyphen. The library will throw an exception otherwise, at least
  // if in debug mode.
  //
  // After listing the 1d, 2d, and 3d case, it is good style to let the
  // program die if we run upon a case which we did not consider. Remember
  // that the Assert macro generates an exception if the
  // condition in the first parameter is not satisfied. Of course, the
  // condition false can never be satisfied, so the program
  // will always abort whenever it gets to the default statement:
  std::vector<std::string> solution_names;
  switch (dim) {
    case 1:
      solution_names.push_back("displacement");
      break;
    case 2:
      solution_names.push_back("x_displacement");
      solution_names.push_back("y_displacement");
      break;
    case 3:
      solution_names.push_back("x_displacement");
      solution_names.push_back("y_displacement");
      solution_names.push_back("z_displacement");
      break;
    default:
      throw ExcNotImplemented();
  }

  // After setting up the names for the different components of the solution
  // vector, we can add the solution vector to the list of data vectors
  // scheduled for output. Note that the following function takes a vector
  // of strings as second argument, whereas the one which we have used in
  // all previous examples accepted a string there. In fact, the latter
  // function is only a shortcut for the function which we call here: it
  // puts the single string that is passed to it into a vector of strings
  // with only one element and forwards that to the other function.
  data_out.add_data_vector(solution, solution_names);
  data_out.build_patches();
  data_out.write_vtk(output);
}

void ElasticityExample::refine_global(int refine_steps) {
  triangulation_.refine_global(refine_steps);
  setup_system();
  assemble_h1();
  assemble_system();
}

ElasticityExample::VectorType ElasticityExample::solve(const ElasticityExample::Parameter& param) {
  _solve(param, tmp_data_);
  return VectorType(tmp_data_);
}

const dealii::SparseMatrix<ElasticityExample::Number>& ElasticityExample::lambda_mat() const {
  return lambda_system_matrix_;
}

const dealii::SparseMatrix<ElasticityExample::Number>& ElasticityExample::mu_mat() const { return mu_system_matrix_; }

const dealii::SparseMatrix<ElasticityExample::Number>& ElasticityExample::h1_mat() const { return h1_matrix_; }

const dealii::Vector<ElasticityExample::Number>& ElasticityExample::rhs() const { return system_rhs_; }

ElasticityExample::Number ElasticityExample::h1_0_semi_norm(const Vector<Number>& v) const {
  return std::sqrt(h1_matrix_.matrix_norm_square(v));
}

ElasticityExample::Number ElasticityExample::energy_norm(const Vector<ElasticityExample::Number>& v) const {
  return std::sqrt(mu_system_matrix_.matrix_norm_square(v));
}

ElasticityExample::VectorType ElasticityExample::transfer_to(int refine_steps, const ElasticityExample::VectorType& v) {
  triangulation_.prepare_coarsening_and_refinement();
  dealii::SolutionTransfer<dim, VectorType> trans(dof_handler_);
  trans.prepare_for_pure_refinement();
  refine_global(refine_steps);

  VectorType ret(dof_handler_.n_dofs());
  trans.refine_interpolate(v, ret);
  return ret;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_IDIVFLOOR_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_IDIVFLOOR_HPP_INCLUDED

#include <boost/simd/toolbox/arithmetic/functions/idivfloor.hpp>
#include <boost/simd/include/functions/simd/idivceil.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/simd/floor.hpp>
#include <boost/simd/include/functions/simd/negate.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivfloor_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return -idivceil(-a0,a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivfloor_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<unsigned_<A0>,X>))((simd_<unsigned_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return rdivide(a0,a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivfloor_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return toint(floor(a0/a1)); }
  };
} } }

#endif


#pragma once
#include <ros/ros.h>
#include <Eigen/Dense>
#include <laser_geometry/laser_geometry.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_2D.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/tf.h>

typedef pcl::PointXYZ Point;
typedef pcl::PointCloud<Point> PointCloud;
typedef pcl::registration::CorrespondenceEstimation<Point, Point> CorrespondenceEstimation;
typedef pcl::registration::TransformationEstimation2D<Point, Point> TransformationEstimation2D;

namespace Turtlebot
{

class PoseEstimationICP
{
public:
    PoseEstimationICP(){}
    ~PoseEstimationICP(){}

    const tf::Pose getTransform(const sensor_msgs::LaserScan &source_scan, const sensor_msgs::LaserScan &target_scan);

private:
    const PointCloud::Ptr convertToPCL(const sensor_msgs::LaserScan &scan);
    const Eigen::Matrix4f calcTransformICP(const PointCloud::Ptr &source_cloud, const PointCloud::Ptr &target_cloud);
    const tf::Pose convertMatrixToPose(const Eigen::Matrix4f &mat);

    laser_geometry::LaserProjection m_projector;
};

}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/function/bitfloating.hpp>
#include <boost/simd/function/bitinteger.hpp>
#include <boost/simd/function/if_allbits_else.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/is_not_equal.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/assert.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(successor_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return if_plus(is_not_equal(a0, Valmax<A0>()), a0, One<A0>());
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(successor_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return if_allbits_else(is_nan(a0), bitfloating(saturated_(inc)(bitinteger(a0))));
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(successor_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::integer_<A0>, X>
                          , bs::pack_<bd::integer_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        BOOST_ASSERT_MSG(assert_all(is_gez(a1)), "successor rank must be non negative");
        return if_else( bs::is_greater(Valmax<A0>()-a1, a0),
                        a0+a1,
                        Valmax<A0>());
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(successor_
                          , (typename A0, typename A1, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::integer_<A1>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const  A1&  a1) const BOOST_NOEXCEPT
      {
        BOOST_ASSERT_MSG(assert_all(is_gez(a1)), "successor rank must be non negative");
        return if_allbits_else(is_nan(a0), bitfloating(saturated_(plus)(bitinteger(a0), a1)));
      }
   };

} } }

#endif


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

 * QuadricSLAM Copyright 2020, ARC Centre of Excellence for Robotic Vision,
 Queensland University of Technology (QUT)
 * Brisbane, QLD 4000
 * All Rights Reserved
 * Authors: Lachlan Nicholson, et al. (see THANKS for the full author list)
 * See LICENSE for the license information

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

/**
 * @file ConstrainedDualQuadric.cpp
 * @date Apr 14, 2020
 * @author Lachlan Nicholson
 * @brief a constrained dual quadric
 */

#include <gtsam_quadrics/base/Utilities.h>
#include <gtsam_quadrics/geometry/AlignedBox2.h>
#include <gtsam_quadrics/geometry/ConstrainedDualQuadric.h>
#include <gtsam_quadrics/geometry/QuadricCamera.h>

#include <Eigen/Eigenvalues>
#include <iostream>

using namespace std;

namespace gtsam_quadrics {

/* ************************************************************************* */
ConstrainedDualQuadric::ConstrainedDualQuadric() {
  pose_ = gtsam::Pose3();
  radii_ = gtsam::Vector3(1, 1, 1);
}

/* ************************************************************************* */
ConstrainedDualQuadric::ConstrainedDualQuadric(const gtsam::Matrix44& dQ) {
  *this = ConstrainedDualQuadric::constrain(dQ);
}

/* ************************************************************************* */
ConstrainedDualQuadric ConstrainedDualQuadric::constrain(
    const gtsam::Matrix4& dual_quadric) {
  // normalize if required
  gtsam::Matrix4 normalized_dual_quadric(dual_quadric);
  if (dual_quadric(3, 3) != 1.0) {
    normalized_dual_quadric = dual_quadric / dual_quadric(3, 3);
  }

  // extract translation
  gtsam::Point3 translation(normalized_dual_quadric.block(0, 3, 3, 1));

  // calculate the point quadric matrix
  gtsam::Matrix4 point_quadric = normalized_dual_quadric.inverse();
  gtsam::Matrix4 normalized_point_quadric = point_quadric;
  if (point_quadric(3, 3) != 1.0) {
    normalized_point_quadric = point_quadric / point_quadric(3, 3);
  }

  // extract shape
  auto lambdaa = normalized_point_quadric.block(0, 0, 3, 3).eigenvalues();
  gtsam::Vector3 shape =
      Eigen::sqrt(-1.0 * normalized_point_quadric.determinant() /
                  normalized_point_quadric.block(0, 0, 3, 3).determinant() *
                  1.0 / lambdaa.array())
          .abs();

  // extract rotation
  Eigen::EigenSolver<Eigen::Matrix<double, 3, 3>> s(
      normalized_point_quadric.block(0, 0, 3, 3));
  gtsam::Matrix3 rotation_matrix = s.eigenvectors().real();

  // ensure rotation is right-handed
  if (!(fabs(1.0 - rotation_matrix.determinant()) < 1e-8)) {
    rotation_matrix *= -1.0 * gtsam::Matrix3::Identity();
  }
  gtsam::Rot3 rotation(rotation_matrix);

  return ConstrainedDualQuadric(rotation, translation, shape);
}

/* ************************************************************************* */
gtsam::Matrix44 ConstrainedDualQuadric::matrix(
    gtsam::OptionalJacobian<16, 9> dQ_dq) const {
  gtsam::Matrix44 Z = pose_.matrix();
  gtsam::Matrix44 Qc = (gtsam::Vector4() << (radii_).array().pow(2), -1.0)
                           .finished()
                           .asDiagonal();
  gtsam::Matrix44 Q = Z * Qc * Z.transpose();

  if (dQ_dq) {
    Eigen::Matrix<double, 16, 6> dZ_dx;
    utils::matrix(pose_, dZ_dx);  // NOTE: this will recalculate pose.matrix
    Eigen::Matrix<double, 16, 9> dZ_dq = gtsam::Matrix::Zero(16, 9);
    dZ_dq.block(0, 0, 16, 6) = dZ_dx;

    Eigen::Matrix<double, 16, 9> dQc_dq = gtsam::Matrix::Zero(16, 9);
    dQc_dq(0, 6) = 2.0 * radii_(0);
    dQc_dq(5, 7) = 2.0 * radii_(1);
    dQc_dq(10, 8) = 2.0 * radii_(2);

    using utils::kron;
    static gtsam::Matrix4 I44 = gtsam::Matrix::Identity(4, 4);
    static Eigen::Matrix<double, 16, 16> T44 = utils::TVEC(4, 4);
    *dQ_dq = kron(I44, Z * Qc) * T44 * dZ_dq +
             kron(Z, I44) * (kron(I44, Z) * dQc_dq + kron(Qc, I44) * dZ_dq);
  }
  return Q;
}

/* ************************************************************************* */
gtsam::Matrix44 ConstrainedDualQuadric::normalizedMatrix(void) const {
  gtsam::Matrix44 Q = this->matrix();
  return Q / Q(3, 3);
}

/* ************************************************************************* */
// TODO: vectorize
AlignedBox3 ConstrainedDualQuadric::bounds() const {
  gtsam::Matrix44 dE = this->matrix();
  double x_min =
      (dE(0, 3) + std::sqrt(dE(0, 3) * dE(0, 3) - (dE(0, 0) * dE(3, 3)))) /
      dE(3, 3);
  double y_min =
      (dE(1, 3) + std::sqrt(dE(1, 3) * dE(1, 3) - (dE(1, 1) * dE(3, 3)))) /
      dE(3, 3);
  double z_min =
      (dE(2, 3) + std::sqrt(dE(2, 3) * dE(2, 3) - (dE(2, 2) * dE(3, 3)))) /
      dE(3, 3);
  double x_max =
      (dE(0, 3) - std::sqrt(dE(0, 3) * dE(0, 3) - (dE(0, 0) * dE(3, 3)))) /
      dE(3, 3);
  double y_max =
      (dE(1, 3) - std::sqrt(dE(1, 3) * dE(1, 3) - (dE(1, 1) * dE(3, 3)))) /
      dE(3, 3);
  double z_max =
      (dE(2, 3) - std::sqrt(dE(2, 3) * dE(2, 3) - (dE(2, 2) * dE(3, 3)))) /
      dE(3, 3);
  return AlignedBox3((gtsam::Vector6() << std::min(x_min, x_max),
                      std::max(x_min, x_max), std::min(y_min, y_max),
                      std::max(y_min, y_max), std::min(z_min, z_max),
                      std::max(z_min, z_max))
                         .finished());
}

/* ************************************************************************* */
bool ConstrainedDualQuadric::isBehind(const gtsam::Pose3& cameraPose) const {
  gtsam::Pose3 rpose = cameraPose.between(this->pose());
  if (rpose.z() < 0.0) {
    return true;
  }
  return false;
}

/* ************************************************************************* */
bool ConstrainedDualQuadric::contains(const gtsam::Pose3& cameraPose) const {
  gtsam::Vector4 cameraPoint =
      (gtsam::Vector4() << cameraPose.translation(), 1.0).finished();
  double pointError =
      cameraPoint.transpose() * this->matrix().inverse() * cameraPoint;
  if (pointError <= 0.0) {
    return true;
  }
  return false;
}

/* ************************************************************************* */
ConstrainedDualQuadric ConstrainedDualQuadric::Retract(
    const gtsam::Vector9& v) {
  gtsam::Pose3 pose = gtsam::Pose3::Retract(v.head<6>());
  gtsam::Vector3 radii = v.tail<3>();
  return ConstrainedDualQuadric(pose, radii);
}

/* ************************************************************************* */
gtsam::Vector9 ConstrainedDualQuadric::LocalCoordinates(
    const ConstrainedDualQuadric& q) {
  gtsam::Vector9 v = gtsam::Vector9::Zero();
  v.head<6>() = gtsam::Pose3::LocalCoordinates(q.pose_);
  v.tail<3>() = q.radii_;
  return v;
}

/* ************************************************************************* */
ConstrainedDualQuadric ConstrainedDualQuadric::retract(
    const gtsam::Vector9& v) const {
  gtsam::Pose3 pose = pose_.retract(v.head<6>());
  gtsam::Vector3 radii = radii_ + v.tail<3>();
  return ConstrainedDualQuadric(pose, radii);
}

/* ************************************************************************* */
gtsam::Vector9 ConstrainedDualQuadric::localCoordinates(
    const ConstrainedDualQuadric& other) const {
  gtsam::Vector9 v = gtsam::Vector9::Zero();
  v.head<6>() = pose_.localCoordinates(other.pose_);
  v.tail<3>() = other.radii_ - radii_;
  return v;
}

/* ************************************************************************* */
void ConstrainedDualQuadric::print(const std::string& s) const {
  cout << s;
  cout << this->matrix() << endl;
}

/* ************************************************************************* */
bool ConstrainedDualQuadric::equals(const ConstrainedDualQuadric& other,
                                    double tol) const {
  return this->normalizedMatrix().isApprox(other.normalizedMatrix(), tol);
}

/* ************************************************************************* */
void ConstrainedDualQuadric::addToValues(gtsam::Values& v,
                                         const gtsam::Key& k) {
  v.insert(k, *this);
}

/* ************************************************************************* */
ConstrainedDualQuadric ConstrainedDualQuadric::getFromValues(
    const gtsam::Values& v, const gtsam::Key& k) {
  return v.at<ConstrainedDualQuadric>(k);
}

}  // namespace gtsam_quadrics


#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/asio.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/videoio/videoio.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#define CONFIG_NUM_WORKERS 2

cv::Mat gray, prevgray;

void camera_process(boost::shared_ptr<cv::Mat> frame) {
	cv::Mat flow;
	cvtColor(*frame, gray, cv::COLOR_BGR2GRAY);
	if(!prevgray.empty()) {
		calcOpticalFlowFarneback(prevgray, gray, flow, 0.5, 3, 15, 3, 5, 1.2, 0);
	}
	cv::Point2f total;
	const int step = 16;
	for(unsigned int y = 0; y < flow.rows; y += step) {
		for(unsigned int x = 0; x < flow.cols; x += step) {
			total += flow.at<cv::Point2f>(y, x);
		}
	}
	if(sqrt(total.x * total.x + total.y * total.y) > 500) {
		std::cout << total << std::endl;
	}
	std::swap(prevgray, gray);
}

void camera_loop(boost::shared_ptr<boost::asio::io_service> io_service, cv::VideoCapture vid) {
	auto frame = boost::make_shared<cv::Mat>();
	cv::Mat sobel, kpframe;
	vid >> *frame;
	cvtColor(*frame, sobel, cv::COLOR_BGR2GRAY);
	for(unsigned int n = 1; n < 7; n += 2) {
		cv::GaussianBlur(sobel, sobel, cv::Size(n, n), 0, 0);
	}
	cv::Sobel(sobel, sobel, CV_8U, 1, 1, 5);
	cv::normalize(sobel, sobel, 0, 1024, cv::NORM_MINMAX, CV_8U);
	for(unsigned int n = 1; n < 5; n += 2) {
		cv::GaussianBlur(sobel, sobel, cv::Size(n, n), 0, 0);
	}

	std::vector<cv::KeyPoint> kp;
	auto dt = cv::FastFeatureDetector::create(120);
	dt->detect(sobel, kp);
	cv::drawKeypoints(sobel, kp, *frame, cv::Scalar::all(1), 1);

	//io_service->post(std::bind(camera_process, frame));
	cv::imshow("optflow", *frame);
	if(cv::waitKey(30) < 0) {
		io_service->post(std::bind(camera_loop, io_service, vid));
	}
}

void camera_main(boost::shared_ptr<boost::asio::io_service> io_service) {
	cv::VideoCapture vid(0);
	if(!vid.isOpened()) { throw std::runtime_error("failed to open video capture device"); }
	cv::namedWindow("optflow");
	io_service->post(std::bind(camera_loop, io_service, vid));
}

int slow_main(int argc, char *argv[]) {
	cv::VideoCapture vid(0);
	if(!vid.isOpened()) {
		return -1;
	}
	cv::Mat flow, frame;
	cv::UMat gray, prevgray, uflow;
	cv::namedWindow("optflow");
	for(;;) {
		vid >> frame;
		cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
		if(!prevgray.empty()) {
			calcOpticalFlowFarneback(prevgray, gray, uflow, 0.5, 3, 15, 3, 5, 1.2, 0);
			uflow.copyTo(flow);
			cv::imshow("optflow", frame);
		}
		cv::Point2f total;
		const int step = 16;
		for(unsigned int y = 0; y < flow.rows; y += step) {
			for(unsigned int x = 0; x < flow.cols; x += step) {
				total += flow.at<cv::Point2f>(y, x);
			}
		}
		if(sqrt(total.x * total.x + total.y * total.y) > 500) {
			std::cout << total << std::endl;
		}
		if(cv::waitKey(30) >= 0) {
			break;
		} else {
			std::swap(prevgray, gray);
		}
	}
	return 0;
}

void worker_main(boost::shared_ptr<boost::asio::io_service> io_service) {
	io_service->run();
}

int main(int argc, char *argv[]) {
	//slow_main(argc, argv);
	auto io_service = boost::make_shared<boost::asio::io_service>();
	auto work       = boost::make_shared<boost::asio::io_service::work>(*io_service);
	auto strand     = boost::make_shared<boost::asio::io_service::strand>(*io_service);
	boost::thread_group workers;
	for(unsigned int w = 0; w < CONFIG_NUM_WORKERS; ++w) {
		workers.create_thread(boost::bind(worker_main, io_service));
	}
	io_service->post(boost::bind(camera_main, io_service));
	work.reset();
	workers.join_all();
	return 0;
}


#ifndef ACCELEROMETERCALIBRATION_HPP
#define ACCELEROMETERCALIBRATION_HPP
#include <string>
#include <Eigen/Core>
#include <yaml-cpp/yaml.h>
#include <fstream>

namespace Jaco2Calibration {
struct EIGEN_ALIGN16 AccelerometerCalibrationParam {

    AccelerometerCalibrationParam(){}

    AccelerometerCalibrationParam(const std::string& name, double biasX, double biasY, double biasZ,
                                   double mXX, double mXY, double mXZ,
                                   double mYX, double mYY, double mYZ,
                                   double mZX, double mZY, double mZZ)
        : acc_name(name),
          bias(biasX, biasY, biasZ)

    {
        misalignment << mXX, mXY, mXZ,
                mYX, mYY, mYZ,
                mZX, mZY, mZZ;
    }

    AccelerometerCalibrationParam(const std::string& name, const Eigen::Matrix<double, 3,1>& vec,
                                   const Eigen::Matrix<double, 3,3>& ms,
                                   const Eigen::Matrix<double, 3,3>& scale)
        : acc_name(name),
          bias(vec),
          misalignment(ms*scale)

    {}

    void setMisalignmant(const Eigen::Matrix<double, 3, 3>& ms, const Eigen::Matrix<double, 3,3> scale)
    {
        misalignment = ms * scale;
    }

    std::string acc_name;
    Eigen::Matrix<double, 3,1> bias;
    Eigen::Matrix<double, 3,3> misalignment;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

};

inline void save(const std::string& filename, const std::vector<Jaco2Calibration::AccelerometerCalibrationParam>& params)
{
    std::ofstream file(filename);
    YAML::Emitter yamlEmit(file);
    YAML::Node doc;
    for(AccelerometerCalibrationParam param : params){
        YAML::Node pNode;
        pNode["acc_name"] = param.acc_name;
        pNode["bias_x"] = param.bias(0);
        pNode["bias_y"] = param.bias(1);
        pNode["bias_z"] = param.bias(2);
        Eigen::Matrix< double, 3 , 3> ms_mat = param.misalignment;
        pNode["ms_xx"] = ms_mat(0,0);
        pNode["ms_xy"] = ms_mat(0,1);
        pNode["ms_xz"] = ms_mat(0,2);
        pNode["ms_yx"] = ms_mat(1,0);
        pNode["ms_yy"] = ms_mat(1,1);
        pNode["ms_yz"] = ms_mat(1,2);
        pNode["ms_zx"] = ms_mat(2,0);
        pNode["ms_zy"] = ms_mat(2,1);
        pNode["ms_zz"] = ms_mat(2,2);
        doc["parameter"].push_back(pNode);
    }
    yamlEmit << doc;
}

inline void loadAccCalib(const std::string& filename, std::vector<Jaco2Calibration::AccelerometerCalibrationParam>& params)
{
    YAML::Node doc = YAML::LoadFile(filename);
    doc = doc["parameter"];
    if(!doc.IsDefined())
    {
        throw std::runtime_error("illegal document!");
    }
    for(auto it = doc.begin(); it != doc.end(); ++it){
        auto pNode = *it;
        std::string name(pNode["acc_name"].as<std::string>());
        double x = pNode["bias_x"].as<double>();
        double y = pNode["bias_y"].as<double>();
        double z = pNode["bias_z"].as<double>();
        double mXX = pNode["ms_xx"].as<double>();
        double mXY = pNode["ms_xy"].as<double>();
        double mXZ = pNode["ms_xz"].as<double>();
        double mYX = pNode["ms_yx"].as<double>();
        double mYY = pNode["ms_yy"].as<double>();
        double mYZ = pNode["ms_yz"].as<double>();
        double mZX = pNode["ms_zx"].as<double>();
        double mZY = pNode["ms_zy"].as<double>();
        double mZZ = pNode["ms_zz"].as<double>();
        AccelerometerCalibrationParam param(name, x, y,z,
                                             mXX, mXY, mXZ,
                                             mYX, mYY, mYZ,
                                             mZX, mZY, mZZ);

        params.push_back(param);
    }
}

inline bool getParam(const std::string& acc_name, const std::vector<Jaco2Calibration::AccelerometerCalibrationParam>& params,
                     Jaco2Calibration::AccelerometerCalibrationParam& result)
{
    for(auto param : params){
        if(param.acc_name == acc_name){
            result.acc_name = acc_name;
            result.bias = param.bias;
            result.misalignment = param.misalignment;
            return true;
        }
    }
    return false;
}

}
#endif // ACCELEROMETERCALIBRATION_HPP


#include <bits/stdc++.h>
using namespace std;
//#include <atcoder/all>
//using namespace atcoder;
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
#define rep(i,n) for (ll i = 0; i < (n); ++i)
// using ll = long long;
using ll = cpp_int;
using P = pair<int,int>;

int main() {
  ll x, k, d;
  ll ii = (long long)(1e15);
  ll ji = (long long)(1e10);
  ll ki = (long long)(1e9);
    // cin >> x >> k >> d;
    x = ii; k = ji; d = ki;
    x = abs(x);
  cout << k*d << endl;
    if(k*d > x){
      ll s = k - x/d;
      k = x/d + s%2;
    }
    ll ans1 = abs(x - k*d);

    x = ii; k = ji; d = ki;
    x = abs(x);
    ll t = min(k, x/d);
    k -= t;
    x -= t*d;
    ll ans2 = 0;
    if(k%2 == 0){
      ans2 = x;
    }else{
      ans2 = d-x;
    }
    if(ans1 != ans2){
      printf("%d %d %d\n", ii, ji, ki);
    }
  return 0;
}


#include <chrono>
#include <math.h>
#include <opencv2/opencv.hpp>
#include <TrtNet.h>
#include <yaml-cpp/yaml.h>
#include <YoloLayer.h>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <nav_msgs/OccupancyGrid.h>
#include <jsk_recognition_msgs/BoundingBoxArray.h>
#include <jsk_recognition_msgs/BoundingBox.h>
#include <khmot_msgs/BoundingBoxWithCovarianceArray.h>
#include <khmot_msgs/BoundingBoxWithCovariance.h>

#include "perception.hpp"
#include "bonnet.hpp"
#include "box3d.hpp"
#include "utils.hpp"

using namespace std;

namespace perception
{

EPerception::EPerception(ros::NodeHandle nh, ros::NodeHandle priv_nh): it_(nh)
{
  priv_nh.param<string>("config_file", config_file_, "config.yaml");

  map_pub_   = nh.advertise<nav_msgs::OccupancyGrid>("drivable_area", 1);
  bbox_pub_  = nh.advertise<khmot_msgs::BoundingBoxWithCovarianceArray>("observations", 1);

  YAML::Node config;
  try
  {
    config = YAML::LoadFile(config_file_);
  }
  catch (const YAML::BadFile&)
  {
    ROS_ERROR_STREAM("Failed to read config file at "<<config_file_);
    ros::shutdown();
    return;
  }

  // get config
  // TODO: catch YAML expection
  cam_frame_id_               = config["cam_frame_id"].as<string>();
  cam_width_                  = config["cam_width"].as<int>();
  cam_height_                 = config["cam_height"].as<int>();
  cam_height_from_ground_     = config["cam_height_from_ground"].as<float>();
  top_width_                  = config["top_width"].as<int>();
  top_height_                 = config["top_height"].as<int>();
  top_res_                    = config["top_res"].as<float>();
  top2_res_                   = config["top2_res"].as<float>();
  do_viz_                     = config["do_viz"].as<bool>();
  string bonnet_engine        = config["bonnet_engine"].as<string>();
  string yolo_engine          = config["yolo_engine"].as<string>();
  string box3d_engine         = config["box3d_engine"].as<string>();
  vector<vector<float>> H_vec = config["H"].as<vector<vector<float>>>();
  vector<vector<float>> K_vec = config["K"].as<vector<vector<float>>>();

  // prepare CV matrices
  H_ = toMat(H_vec);
  K_ = toMat(K_vec);
  fx_ = K_.at<float>(0,0);
  fy_ = K_.at<float>(1,1);
  u0_ = K_.at<float>(0,2);
  v0_ = K_.at<float>(1,2);
  // projection (extrinsic)
  // assume world frame aligned with camera frame
  P_ << K_vec[0][0],K_vec[0][1],K_vec[0][2],0.,
        K_vec[1][0],K_vec[1][1],K_vec[1][2],0.,
        K_vec[2][0],K_vec[2][1],K_vec[2][2],0.;

  // occupancy grid size with desired resolution (10cm/px)
  top2_width_  = top_width_ * (top_res_/top2_res_);
  top2_height_ = top_height_ * (top_res_/top2_res_);
  // camera origin from topdown in meters
  cam_origin_x_ = cam_height_from_ground_*(fy_/v0_);
  cam_origin_y_ = (top2_width_/2)*top2_res_;
  bonnet_scale_ = min(float(bonnet::W)/cam_width_,float(bonnet::H)/cam_height_);
  map_msg_.data.resize(top2_width_*top2_height_);

  if (do_viz_)
  {
    viz_pub_ = nh.advertise<sensor_msgs::Image>("image_viz", 2);
  }

  // yolo (TensorRT)
  // TODO: it can fail silently, check if initialized.
  yolo_.reset(new Tn::trtNet(yolo_engine));

  // bonnet (TensorRT)
  bonnet_net_.reset(new bonnet::Bonnet(bonnet_engine));
  if (!bonnet_net_->initialized)
  {
    ROS_ERROR("Failed to initialize bonnet");
    ros::shutdown();
    return;
  }

  // box3d (TensorRT)
  box3d_net_.reset(new box3d::Box3D(box3d_engine));
  if (!box3d_net_->initialized)
  {
    ROS_ERROR("Failed to initialize box3d");
    ros::shutdown();
    return;
  }

  // finally subscribe to images
  image_sub_ = it_.subscribe("image", 1, &EPerception::imageCallback, this);
}

void EPerception::imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  try
  {
    bridge_ = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  // zero-copy
  const cv::Mat frame=bridge_->image;

  auto t_start = chrono::high_resolution_clock::now();
  ros::Time frame_stamp=msg->header.stamp;

  // YOLO
  vector<float> input_data = prepare_image(frame);
  int output_count = yolo_->getOutputSize()/sizeof(float);
  unique_ptr<float[]> output_data(new float[output_count]);

  yolo_->doInference(input_data.data(), output_data.get(), 1);

  //Get Output
  auto output = output_data.get();
  //first detect count
  int detCount = output[0];
  //later detect result
  vector<Yolo::Detection> result;
  result.resize(detCount);
  memcpy(result.data(), &output[1], detCount*sizeof(Yolo::Detection));
  auto bboxes_all = postprocess_image(frame, result, YOLO_NUM_CLS);

  // 3D part that estimates 3d bounding boxes:
  // - estimate only cars, because we have compute limited capacity
  // - estimate pedestrians 3d box w/o NN, just assume yaw=0;
  vector<YoloBbox> bboxes;
  vector<YoloBbox> bboxes_peds;
  for(const auto& bbox: bboxes_all)
  {
    YoloBbox b = bbox;
    // all except pedestrian
    if ((b.classId==2) or
        (b.classId==5) or
        (b.classId==7) or
        (b.classId==1) or
        (b.classId==6))
    {
      int coco_class = b.classId;
      b.classId=COCO_TO_VOC.at(coco_class);
      bboxes.push_back(b);
    // pedestrian
    } else if (b.classId==0)
    {
      int coco_class = b.classId;
      b.classId=COCO_TO_VOC.at(coco_class);
      bboxes_peds.push_back(b);
    }
  }

  // sort by distance from ego car
  sort(bboxes.begin(), bboxes.end(), &YoloBboxCompareDistance);

  int batch_size;
  if (bboxes.size()>box3d::MAX_BATCH_SIZE)
  {
    batch_size = box3d::MAX_BATCH_SIZE;
  } else {
    batch_size = bboxes.size();
  }

  vector<float> b3d_input_data(batch_size * box3d::INPUT_SIZE);
  vector<float> b3d_out_dims(batch_size * box3d::DIMS_SIZE);
  vector<float> b3d_out_ori(batch_size * box3d::ORI_SIZE);
  vector<float> b3d_out_conf(batch_size * box3d::CONF_SIZE);
  {
    auto buf = b3d_input_data.data();
    cv::Mat patch;
    vector<cv::Mat> b3d_input_channels(box3d::C);
    for (int b = 0; b < batch_size; ++b) {
      // prepare patch
      YoloBbox bbox = bboxes[b];
      cv::Rect patchRect(
        cv::Point(bbox.left,bbox.top),
        cv::Point(bbox.right,bbox.bot)
      );
      cv::resize(frame(patchRect), patch, cv::Size(box3d::W, box3d::H), 0, 0, CV_INTER_CUBIC);
      patch.convertTo(patch, CV_32FC3);
      subtract(patch,NORM_CAFFE,patch); // Caffe norm
      // split channels (HWC->CHW) and copy into buffer
      split(patch, b3d_input_channels);
      int channel_len = box3d::W * box3d::H;
      for (int i = 0; i < box3d::C; ++i) {
          memcpy(buf, b3d_input_channels[i].data, channel_len * sizeof(float));
          buf += channel_len;
      }
    }
  }
  if (batch_size > 0)
  {
    box3d_net_->doInference(batch_size, b3d_input_data, b3d_out_dims,
                            b3d_out_ori, b3d_out_conf);
  }

  // estimate 3D bounding box based on NN outputs
  vector<Bbox3D> bboxes3d;
  for (int b = 0; b < batch_size; ++b)
  {
    // find bin ID with max confidence
    Eigen::Matrix<float, box3d::BIN_NUM, 1> conf;
    for (int i = 0; i < box3d::BIN_NUM; ++i)
      conf[i]=b3d_out_conf[b*box3d::CONF_SIZE+i];

    int max_a; conf.maxCoeff(&max_a);
    float cos_v = b3d_out_ori[b*box3d::ORI_SIZE + max_a*2];
    float sin_v = b3d_out_ori[b*box3d::ORI_SIZE + max_a*2 + 1];

    // compute yaw
    int xmin = bboxes[b].left;
    int ymin = bboxes[b].top;
    int xmax = bboxes[b].right;
    int ymax = bboxes[b].bot;
    float theta_ray = atan2(fx_, ((xmin+xmax)/2.)-u0_);
    float angle_offset = (sin_v > 0.) ? acos(cos_v) : -acos(cos_v);
    float wedge = (2.*M_PI)/box3d::BIN_NUM;
    float theta_loc = angle_offset + max_a * wedge;
    float theta = theta_loc + theta_ray;
    float yaw = fmod((M_PI/2)-theta, 2*M_PI); // make yaw in betwen [-2Pi,2Pi]

    // fill the values
    Bbox3D bbox3d;
    bbox3d.h=b3d_out_dims[b*box3d::DIMS_SIZE]     + DIMS_AVG[bboxes[b].classId][0];
    bbox3d.w=b3d_out_dims[b*box3d::DIMS_SIZE + 1] + DIMS_AVG[bboxes[b].classId][1];
    bbox3d.l=b3d_out_dims[b*box3d::DIMS_SIZE + 2] + DIMS_AVG[bboxes[b].classId][2];
    bbox3d.yaw=yaw;
    bbox3d.theta_ray=theta_ray;
    bbox3d.xmin=xmin;
    bbox3d.ymin=ymin;
    bbox3d.xmax=xmax;
    bbox3d.ymax=ymax;
    // hmm, just skip?
    if (!compute_3D_box(bbox3d, P_))
      continue;

    // filter boxes with big estimatation error
    if (bbox3d.estimation_error > B3D_FILTER_ESTIMATION_ERROR)
      continue;

    // filter boxes vertical boxex near sides (this does not include people!!!)
    bool vertical = (abs(bbox3d.xmin-bbox3d.xmax)/abs(bbox3d.ymin-bbox3d.ymax) < 1.0);
    bool near_side = (((cam_width_-bbox3d.xmax)<B3D_FILTER_SIDE_PX) or (bbox3d.xmin<B3D_FILTER_SIDE_PX));
    if ((near_side) and (vertical))
      continue;

    bboxes3d.push_back(bbox3d);
  }

  // estimtate 3D bounding box for pedestrians (w/o NN outputs)
  // for the sake of performance assumed yaw=0 and dimentions are average
  for(const auto& bbox: bboxes_peds)
  {
    Bbox3D bbox3d;
    bbox3d.h=DIMS_AVG[3][0];
    bbox3d.w=DIMS_AVG[3][1];
    bbox3d.l=DIMS_AVG[3][2];
    bbox3d.yaw=0.0;
    bbox3d.theta_ray=0.0;
    bbox3d.xmin=bbox.left;
    bbox3d.ymin=bbox.top;
    bbox3d.xmax=bbox.right;
    bbox3d.ymax=bbox.bot;
    // hmm, just skip?
    if (!compute_3D_box(bbox3d, P_))
      continue;

    bboxes3d.push_back(bbox3d);
  }

  // bonnet
  bonnet_net_->doInference(frame, bonnet_output_);

  khmot_msgs::BoundingBoxWithCovarianceArray bbox_arr_msg;
  bbox_arr_msg.header.stamp = frame_stamp;
  bbox_arr_msg.header.frame_id = cam_frame_id_;

  // 1. H - Homography matrix was computed with calibrate.py for !!!512x154!!! --> 500x500
  // 2. top_res - Resolution px/cm was computed using known size objects
  // 3. top2_res is desired resolution (All top2 values are derived from top1)
  // 4. CAM_TO_BOTTOM_EDGE camera original position from bottom edge of image
  //    was computed by using known height and assuming zero pitch and yaw
  cv::Mat1b top;
  cv::warpPerspective(bonnet_output_, top, H_,
                      cv::Size(top_width_, top_height_),
                      CV_INTER_LINEAR,
                      cv::BORDER_CONSTANT, 255);
  cv::resize(top, top, cv::Size(top2_width_, top2_height_), 0, 0);
  cv::flip(top, top, 0);
  map_msg_.header.stamp = frame_stamp;
  map_msg_.header.frame_id = cam_frame_id_;
  map_msg_.info.map_load_time = frame_stamp;
  map_msg_.info.resolution = top2_res_;
  map_msg_.info.width = top2_width_;
  map_msg_.info.height = top2_height_;
  map_msg_.info.origin.position.x = cam_origin_x_;
  map_msg_.info.origin.position.y = cam_origin_y_;
  map_msg_.info.origin.position.z = 0.0;
  map_msg_.info.origin.orientation.x = 0.0;
  map_msg_.info.origin.orientation.y = 0.0;
  map_msg_.info.origin.orientation.z = -0.707;
  map_msg_.info.origin.orientation.w = 0.707;
  uchar* p = top.data;
  for( int i = 0; i < top2_width_*top2_height_; ++i)
  {
    int v=p[i];
    if (v>100)
    {
      map_msg_.data[i]=255;
    }
    else if (v>ROAD_THRESH)
    {
      map_msg_.data[i]=0;
    }
    else
    {
      map_msg_.data[i]=100;
    }
  }

  for(const auto& bbox3d: bboxes3d)
  {
    khmot_msgs::BoundingBoxWithCovariance bbox_msg;
    bbox_msg.header.stamp = frame_stamp;
    bbox_msg.header.frame_id = cam_frame_id_;
    bbox_msg.label = bbox3d.label;
    bbox_msg.dimensions.x = bbox3d.l;
    bbox_msg.dimensions.y = bbox3d.w;
    bbox_msg.dimensions.z = bbox3d.h;
    tf2::Quaternion q;
    q.setRPY(0,0,-bbox3d.yaw); // invert yaw
    bbox_msg.pose.pose.orientation.x = q[0];
    bbox_msg.pose.pose.orientation.y = q[1];
    bbox_msg.pose.pose.orientation.z = q[2];
    bbox_msg.pose.pose.orientation.w = q[3];
    tuple<int,int> coords = reproject_to_ground(bbox3d.proj_center_x*bonnet_scale_,
                                                bbox3d.proj_center_y*bonnet_scale_, H_);
    // x axis in ros is (img_height-y axis) in image
    // y axis in ros is -x axis in image
    int x = ((top_height_ - get<1>(coords)) * top_res_) + cam_origin_x_;
    int y = (-get<0>(coords) * top_res_) + cam_origin_y_;
    bbox_msg.pose.pose.position.x = x;
    bbox_msg.pose.pose.position.y = y;
    bbox_msg.pose.pose.position.z = bbox3d.h/2;
    bbox_msg.pose.covariance[0] = VARIANCE_XX;
    bbox_msg.pose.covariance[7] = VARIANCE_YY;
    bbox_msg.pose.covariance[35] = VARIANCE_YawYaw;
    bbox_msg.value = 0;
    bbox_arr_msg.boxes.push_back(bbox_msg);
  }

  // finally publish map and bboxes
  if (ros::ok())
  {
    bbox_pub_.publish(bbox_arr_msg);
    map_pub_.publish(map_msg_);
  }
  auto t_end = chrono::high_resolution_clock::now();
  float total = chrono::duration<float, milli>(t_end - t_start).count();

  if (do_viz_)
  {
    frame_viz_ = frame.clone();

    //draw YOLO boxes on image
    for(const auto& bbox: bboxes)
    {
      cv::rectangle(
        frame_viz_,
        cv::Point(bbox.left,bbox.top),
        cv::Point(bbox.right,bbox.bot),
        cv::Scalar(0,0,255), 3, 8, 0
      );
    }

    for(const auto& bbox3d: bboxes3d)
    {
      draw_3D_box(frame_viz_, bbox3d.pts2d);
    }

    cv::putText(frame_viz_, string("FPS ") + to_string(1000/total), cv::Point(30,30),
      cv::FONT_HERSHEY_COMPLEX_SMALL, 2.0, cv::Scalar(0,0,255), 1, CV_AA);

    cv_bridge::CvImage ros_image_viz;
    ros_image_viz.header.stamp=frame_stamp;
    ros_image_viz.header.frame_id=cam_frame_id_;
    ros_image_viz.encoding="bgr8";
    ros_image_viz.image=frame_viz_;
    viz_pub_.publish(ros_image_viz.toImageMsg());
  }
}

} // namespace perception


#ifndef STATE_HPP_INCLUDED
#define STATE_HPP_INCLUDED

#include "vector.hpp"
#include <boost/numeric/ublas/matrix.hpp>
#include <iosfwd>

class GState;
typedef boost::numeric::ublas::c_matrix<double, 6, 6> gen_mat;

/*! \class State
	\brief Class describing a generic particle state.
	\details This class describes a generic particle state without the use of templates. This simplifies code a
			lot, at the cost of runtime speed. Therefore, this class is only used in non-speed-critical sections of the code,
			for which measurements have shown no noticeable performance impact.
			These are
				generation of initial conditions
				processing of data in state objects.
			Because old versions of this class performed dynamic memory allocations, copying is currently disabled. Should it be required for a
			new algorithm, it should be tested whether copying still hurts performance.
*/
class State
{
public:
	// constructors
	explicit State(std::size_t dimension);

	// prevent copying
	State( const State& ) = delete;
	State( State&& ) = default;
	explicit State( const GState& ode_state );

	// inline getters
	const gen_vect& getPosition() const { return mPosition; };
	const gen_vect& getVelocity() const { return mVelocity; };
	const gen_mat&  getMatrix() const { return mMatrix; };

	gen_vect& editPos() { return mPosition; };
	gen_vect& editVel() { return mVelocity; };
	gen_mat&  editMat() { return mMatrix; };

	std::size_t getDimension() const { return mDimension; };

	// other functions

	/// read a state from the ode-solver and convert it into this state, which is more convenient for further processing
	void readState(const GState& state);

	/// write whole state into a phase space vector
	c_vector<6> getPhaseSpaceVector() const;
private:
	// configuration
	std::size_t mDimension;

	// trivial data
	gen_vect mPosition;
	gen_vect mVelocity;

	gen_mat  mMatrix;
};

std::ostream& operator<<( std::ostream& stream, const State& state );

#endif // STATE_HPP_INCLUDED


#ifndef TEST_UNIT_MATH_REV_MAT_VECTORIZE_EXPECT_REV_ROW_VECTOR_VALUE_HPP
#define TEST_UNIT_MATH_REV_MAT_VECTORIZE_EXPECT_REV_ROW_VECTOR_VALUE_HPP

#include <stan/math/rev/core/var.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/mat/vectorize/build_rev_matrix.hpp>
#include <test/unit/math/rev/mat/vectorize/expect_val_deriv_eq.hpp>
#include <Eigen/Dense>
#include <vector>

template <typename F>
void expect_rev_row_vector_value() {
  using stan::math::var;
  using std::vector;
  typedef Eigen::Matrix<var, 1, Eigen::Dynamic> RowVectorXvar;

  size_t num_inputs = F::valid_inputs().size();
  RowVectorXvar template_vector(num_inputs);

  for (size_t i = 0; i < num_inputs; ++i) {
    RowVectorXvar b = build_rev_matrix<F>(template_vector);
    RowVectorXvar c = build_rev_matrix<F>(template_vector);
    RowVectorXvar fc = F::template apply<RowVectorXvar>(c);
    EXPECT_EQ(c.size(), fc.size());
    expect_val_deriv_eq(F::apply_base(b(i)), b(i), fc(i), c(i));
  }

  size_t vector_vector_size = 2;
  for (size_t i = 0; i < vector_vector_size; ++i) {
    for (size_t j = 0; j < num_inputs; ++j) {
      vector<RowVectorXvar> d;
      vector<RowVectorXvar> e;
      for (size_t k = 0; k < num_inputs; ++k) {
        d.push_back(build_rev_matrix<F>(template_vector));
        e.push_back(build_rev_matrix<F>(template_vector));
      }
      vector<RowVectorXvar> fe = F::template apply<vector<RowVectorXvar> >(e);
      EXPECT_EQ(e[i].size(), fe[i].size());
      EXPECT_EQ(e.size(), fe.size());
      expect_val_deriv_eq(F::apply_base(d[i](j)), d[i](j), fe[i](j), e[i](j));
    }
  }
}

#endif


//
// Created by yche on 1/22/18.
//

#include <boost/program_options.hpp>

#include "../../yche_refactor/flpmc_yche.h"
#include "../../yche_refactor/simrank.h"

using namespace std;
using namespace boost::program_options;

int main(int argc, char *argv[]) {
    // 1st: compute reduce local push
    string g_name = argv[1];
    string path = get_edge_list_path(g_name);
    GraphYche g(path);
    double c = 0.6;
    double eps = 0.01;

    auto n = g.n;
    auto lp = new Reduced_LocalPush(g, g_name, c, eps, n);
    if (!lp_file_exists(g_name, c, eps, n, false)) { // test wether the local push index exists
        cout << "local push offline index doesn't exists.. " << endl;
        auto start_time = std::chrono::high_resolution_clock::now();
        lp->local_push(g);
        auto end_time = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double> elapsed = end_time - start_time;
        cout << format("total indexing cost: %s s") % elapsed.count() << endl; // record the pre-processing time
        cout << format("building compete, saving to %s ") % lp->get_file_path_base() << endl;
        lp->save();
        cout << "saved." << endl;
    } else {
        cout << "offline index exists..loading " << endl;
        lp->load();
    }

    // 2nd: verify the correcness
    if (n < 10000) {
        TruthSim ts(g_name, g, c, eps);
        auto max_err = 0.0;

#pragma omp parallel for reduction(max:max_err) schedule(dynamic, 1)
        for (auto i = 0u; i < n; i++) {
            for (auto j = i; j < n; j++) {
                auto res = lp->query_P(i, j);
                max_err = max(max_err, abs(ts.sim(i, j) - res));
                if (abs(ts.sim(i, j) - res) > eps) {
#pragma omp critical
                    cout << i << ", " << j << "," << ts.sim(i, j) << "," << res << endl;
                }
            }
        }
        cout << "max err:" << max_err << endl;
    };
}


//=======================================================================
// Copyright 2002 Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================

#include <boost/graph/graph_as_tree.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/cstdlib.hpp>
#include <iostream>

class tree_printer {
public:
  template <typename Node, typename Tree>
  void preorder(Node, Tree&) {
    std::cout << "(";
  }
  template <typename Node, typename Tree>
  void inorder(Node n, Tree& t)
  {
    std::cout << get(boost::vertex_name, t)[n];
  }
  template <typename Node, typename Tree>
  void postorder(Node, Tree&) {
    std::cout << ")";
  }

};

int main()
{
  using namespace boost;
  typedef adjacency_list<vecS, vecS, directedS,
    property<vertex_name_t, std::string> > graph_t;
  typedef graph_traits<graph_t>::vertex_descriptor vertex_t;

  graph_t g;

  vertex_t a = add_vertex(g),
    b = add_vertex(g),
    c = add_vertex(g);

  add_edge(a, b, g);
  add_edge(a, c, g);

  typedef property_map<graph_t, vertex_name_t>::type vertex_name_map_t;
  vertex_name_map_t name = get(vertex_name, g);
  name[a] = "A";
  name[b] = "B";
  name[c] = "C";

  typedef iterator_property_map<std::vector<vertex_t>::iterator,
    property_map<graph_t, vertex_index_t>::type> parent_map_t;
  std::vector<vertex_t> parent(num_vertices(g));
  typedef graph_as_tree<graph_t, parent_map_t> tree_t;
  tree_t t(g, a, make_iterator_property_map(parent.begin(),
                                            get(vertex_index, g)));

  tree_printer vis;
  traverse_tree(a, t, vis);

  return exit_success;
}


#ifndef TYPES_HPP
#define TYPES_HPP

// In this file we alias our data structures, so that they may be changed if
// neccessary.

#include <armadillo>

template <typename T>
using matrix = arma::Mat<T>;

template <typename T>
using mvec = arma::Col<T>;

// #include <map>

// template <typename T>
// using SparseData = std::map<int, T>;

#include <unordered_map>

template <typename T>
using SparseData = std::unordered_map<int, T>;

// maybe replace unordered_map with map?

#endif


#include <boost/test/unit_test.hpp>
#include "logs.h"
#include "route.h"
#include "track.h"
using namespace GPS;

BOOST_AUTO_TEST_SUITE( Route_numPositions )

const bool isFileName = true;
const bool notFileName = false;

//TESTING THAT THE NUMBER OF POSITIONS IS AT LEAST 1 THE MINIMUM
BOOST_AUTO_TEST_CASE(numPositions_atLeastOne_route) {
  Route route = Route(LogFiles::GPXRoutesDir + "N0650950_Routes.gpx", isFileName, 1);
  unsigned int noofpos = route.numPositions();
  BOOST_CHECK( noofpos >= 1 );
}

//TESTING THAT THE NUMBER OF POSITIONS FUNCTION OUTPUTS THE CORRECT TOTAL VALUE
BOOST_AUTO_TEST_CASE(numPositions_correct_route) {
  Route route = Route(LogFiles::GPXRoutesDir + "N0650950_Routes.gpx", isFileName, 5);
  unsigned int noofpos = route.numPositions();
  unsigned int numberWeKnowItShouldBe = 12;
  BOOST_CHECK_EQUAL(noofpos, numberWeKnowItShouldBe);
}

//TESTING GRANULARITY @ 50 METERS
BOOST_AUTO_TEST_CASE(numPositions_GranCheck50_route) {
  //All the positions are more than 50 metres apart so no positions should be thrown away
  Route route = Route(LogFiles::GPXRoutesDir + "N0650950_Routes.gpx", isFileName, 50);
  unsigned int noofpos = route.numPositions();
  BOOST_CHECK_EQUAL(noofpos, 12);
}

//TESTING GRANULARITY @ 100 METERS
BOOST_AUTO_TEST_CASE(numPositions_GranCheck100_route) {
  //Only 11 of the positions are more than 100 metres apart so 1 position should be thrown away
  Route route = Route(LogFiles::GPXRoutesDir + "N0650950_Routes.gpx", isFileName, 100);
  unsigned int noofpos = route.numPositions();
  BOOST_CHECK_EQUAL(noofpos, 11);
}

//TESTING GRANULARITY @ 150 METERS
BOOST_AUTO_TEST_CASE(numPositions_GranCheck150_route) {
  //Only 10 of the positions are more than 150 metres apart so 2 positions should be thrown away
  Route route = Route(LogFiles::GPXRoutesDir + "N0650950_Routes.gpx", isFileName, 150);
  unsigned int noofpos = route.numPositions();
  BOOST_CHECK_EQUAL(noofpos, 10);
}

//TESTING GRANULARITY @ 200 METERS
BOOST_AUTO_TEST_CASE(numPositions_GranCheck200_route) {
  //Only 8 of the positions are more than 200 metres apart so 4 positions should be thrown away
  Route route = Route(LogFiles::GPXRoutesDir + "N0650950_Routes.gpx", isFileName, 200);
  unsigned int noofpos = route.numPositions();
  BOOST_CHECK_EQUAL(noofpos, 8);
}

//TESTING GRANULARITY @ 250 METERS
BOOST_AUTO_TEST_CASE(numPositions_GranCheck250_route) {
  //Only 7 of the positions are more than 200 metres apart so 5 positions should be thrown away
  Route route = Route(LogFiles::GPXRoutesDir + "N0650950_Routes.gpx", isFileName, 250);
  unsigned int noofpos = route.numPositions();
  BOOST_CHECK_EQUAL(noofpos, 7);
}

//TESTING GRANULARITY @ 1000 METERS
BOOST_AUTO_TEST_CASE(numPositions_GranCheck1000_route) {
  //Only 4 of the positions are more than 200 metres apart so 8 positions should be thrown away
  Route route = Route(LogFiles::GPXRoutesDir + "N0650950_Routes.gpx", isFileName, 1000);
  unsigned int noofpos = route.numPositions();
  BOOST_CHECK_EQUAL(noofpos, 4);
}

//TESTING LOWER EDGE CASE GRANULARITY
BOOST_AUTO_TEST_CASE(numPositions_lowerEdgeCaseGran_route) {
  //The distance between the two points in "route2" is 4.92 metres. The granularity is
  // 4.88 metres so the route class should throw 0 of the positions away
  Route route2 = Route(LogFiles::GPXRoutesDir + "N0650950_Routes2.gpx", isFileName, 4.88);
  unsigned int noofpos = route2.numPositions();
  BOOST_CHECK_EQUAL(noofpos, 2);
}

//TESTING UPPER EDGE CASE GRANULARITY
BOOST_AUTO_TEST_CASE(numPositions_upperEdgeCaseGran_route) {
  //The distance between the two points in "route2" is 4.92 metres. The granularity is
  // 4.96 metres so the route class should throw 1 of the positions away
  Route route2 = Route(LogFiles::GPXRoutesDir + "N0650950_Routes2.gpx", isFileName, 4.96);
  unsigned int noofpos = route2.numPositions();
  BOOST_CHECK_EQUAL(noofpos, 1);
}
BOOST_AUTO_TEST_SUITE_END()