#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/impl/kdtree.hpp>

#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;
using namespace Eigen;

ros::Publisher _all_map_pub;

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

bool _has_map  = false;

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

pcl::search::KdTree<pcl::PointXYZ> kdtreeMap;
vector<int>     pointIdxSearch;
vector<float>   pointSquaredDistance;

void RandomMapGenerate()
{
   random_device rd;
   default_random_engine eng(rd());

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

   uniform_real_distribution<double> rand_x_circle = uniform_real_distribution<double>(_x_l + 1.0, _x_h - 1.0);
   uniform_real_distribution<double> rand_y_circle = uniform_real_distribution<double>(_y_l + 1.0, _y_h - 1.0);
   uniform_real_distribution<double> rand_r_circle = uniform_real_distribution<double>(_w_c_l    , _w_c_h    );

   uniform_real_distribution<double> rand_roll      = uniform_real_distribution<double>(- M_PI,     + M_PI);
   uniform_real_distribution<double> rand_pitch     = uniform_real_distribution<double>(+ M_PI/4.0, + M_PI/2.0);
   uniform_real_distribution<double> rand_yaw       = uniform_real_distribution<double>(+ M_PI/4.0, + M_PI/2.0);
   uniform_real_distribution<double> rand_ellipse_c = uniform_real_distribution<double>(0.5, 2.0);
   uniform_real_distribution<double> rand_num       = uniform_real_distribution<double>(0.0, 1.0);

   pcl::PointXYZ pt_random;

   bool is_kdtree_empty = false;
   if(cloudMap.points.size() > 0)
      kdtreeMap.setInputCloud( cloudMap.makeShared() );
   else
      is_kdtree_empty = true;

   // // then, we put some pilar
   // 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 )
   //    if(sqrt( pow(x - _init_x, 2) + pow(y - _init_y, 2) ) < 0.8 )
   //       continue;

   //    pcl::PointXYZ searchPoint(x, y, (_h_l + _h_h)/2.0);
   //    pointIdxSearch.clear();
   //    pointSquaredDistance.clear();

   //    if(is_kdtree_empty == false)
   //    {
   //       if ( kdtreeMap.nearestKSearch (searchPoint, 1, pointIdxSearch, pointSquaredDistance) > 0 )
   //       {
   //          if(sqrt(pointSquaredDistance[0]) < 1.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 = 2.0 * ceil(h/_resolution);
   //          for(int t = 0; t < heiNum; t ++ ){
   //             pt_random.x = x + (r+0.0) * _resolution + 0.001;
   //             pt_random.y = y + (s+0.0) * _resolution + 0.001;
   //             pt_random.z =     (t+0.0) * _resolution * 0.5 + 0.001;
   //             //cout<<pt_random.x <<"  "<<pt_random.y <<"  "<<pt_random.z <<"  "<<endl;
   //             cloudMap.points.push_back( pt_random );
   //          }
   //       }
   //    }
   // }
   /**
        *
        *
        STEP 1: Set the my own environment
        *
        *
   */
   for(int i =0;i<50;i++){
      for(int j=0;j<50;j++){
         pt_random.x = i * _resolution;
         pt_random.y = j * _resolution;
         pt_random.z = 0;
         cloudMap.points.push_back( pt_random );
         pt_random.x = -i * _resolution;
         pt_random.y = j * _resolution;
         pt_random.z = 0;
         cloudMap.points.push_back( pt_random );
         pt_random.x = i * _resolution;
         pt_random.y = -j * _resolution;
         pt_random.z = 0;
         cloudMap.points.push_back( pt_random );
         pt_random.x = -i * _resolution;
         pt_random.y = -j * _resolution;
         pt_random.z = 0;
         cloudMap.points.push_back( pt_random );
      }
   }
   // double x_hill=2.5;
   // double y_hill=2.5;
   // for(int i =0;i<30;i++){
   //    for(int j=0;j<30;j++){
   //       pt_random.x = i * _resolution+x_hill;
   //       pt_random.y = j * _resolution+y_hill;
   //       pt_random.z = max(-0.1*((pt_random.x-x_hill)*(pt_random.x-x_hill)+(pt_random.y-y_hill)*(pt_random.y-y_hill))+1,double(0.0));
   //       cloudMap.points.push_back( pt_random );
   //       pt_random.x = -i * _resolution+x_hill;
   //       pt_random.y = j * _resolution+y_hill;
   //       pt_random.z = max(-0.1*((pt_random.x-x_hill)*(pt_random.x-x_hill)+(pt_random.y-y_hill)*(pt_random.y-y_hill))+1,double(0.0));
   //       cloudMap.points.push_back( pt_random );
   //       pt_random.x = i * _resolution+x_hill;
   //       pt_random.y = -j * _resolution+y_hill;
   //       pt_random.z = max(-0.1*((pt_random.x-x_hill)*(pt_random.x-x_hill)+(pt_random.y-y_hill)*(pt_random.y-y_hill))+1,double(0.0));
   //       cloudMap.points.push_back( pt_random );
   //       pt_random.x = -i * _resolution+x_hill;
   //       pt_random.y = -j * _resolution+y_hill;
   //       pt_random.z = max(-0.1*((pt_random.x-x_hill)*(pt_random.x-x_hill)+(pt_random.y-y_hill)*(pt_random.y-y_hill))+1,double(0.0));
   //       cloudMap.points.push_back( pt_random );

   //    }
   // }

   // x_hill=-2.5;
   // y_hill=2.5;
   // for(int i =0;i<30;i++){
   //    for(int j=0;j<30;j++){
   //       pt_random.x = i * _resolution+x_hill;
   //       pt_random.y = j * _resolution+y_hill;
   //       pt_random.z = min(0.1*((pt_random.x-x_hill)*(pt_random.x-x_hill)+(pt_random.y-y_hill)*(pt_random.y-y_hill))-1,double(0.0));
   //       cloudMap.points.push_back( pt_random );
   //       pt_random.x = -i * _resolution+x_hill;
   //       pt_random.y = j * _resolution+y_hill;
   //       pt_random.z = min(0.1*((pt_random.x-x_hill)*(pt_random.x-x_hill)+(pt_random.y-y_hill)*(pt_random.y-y_hill))-1,double(0.0));
   //       cloudMap.points.push_back( pt_random );
   //       pt_random.x = i * _resolution+x_hill;
   //       pt_random.y = -j * _resolution+y_hill;
   //       pt_random.z = min(0.1*((pt_random.x-x_hill)*(pt_random.x-x_hill)+(pt_random.y-y_hill)*(pt_random.y-y_hill))-1,double(0.0));
   //       cloudMap.points.push_back( pt_random );
   //       pt_random.x = -i * _resolution+x_hill;
   //       pt_random.y = -j * _resolution+y_hill;
   //       pt_random.z = min(0.1*((pt_random.x-x_hill)*(pt_random.x-x_hill)+(pt_random.y-y_hill)*(pt_random.y-y_hill))-1,double(0.0));
   //       cloudMap.points.push_back( pt_random );

   //    }
   // }

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

   _has_map = true;

   pcl::toROSMsg(cloudMap, globalMap_pcd);
   globalMap_pcd.header.frame_id = "world";
}

void pubSensedPoints()
{
   if( !_has_map ) return;

   _all_map_pub.publish(globalMap_pcd);
}

int main (int argc, char** argv)
{
   ros::init (argc, argv, "random_complex_scene");
   ros::NodeHandle n( "~" );

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

   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,  10);
   n.param("map/circle_num", _cir_num,  30);
   n.param("map/resolution", _resolution, 0.1);

   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("CircleShape/lower_circle_rad", _w_c_l, 0.3);
   n.param("CircleShape/upper_circle_rad", _w_c_h, 0.8);

   n.param("sensing/rate", _sense_rate, 1.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;

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


#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

const std::string FOLDER("/home/stan/projects/robotics-playground/klt/test_imgs/");
const std::string TEST_IMG_1 = FOLDER + std::string("sea_floor_1.jpg");
const std::string TEST_IMG_2 = FOLDER + std::string("elephant.jpg");
const std::string TEST_IMG_3 = FOLDER + std::string("piggy.png");

int main () {

  cv::Mat img = cv::imread(TEST_IMG_1, 0);
  cv::Mat filtered;
  cv::Mat kernel;

  cv::Point anchor;
  double delta;
  int ddepth;
  int kernel_size;
  std::string window_name = "filter2d demo";

  int c;

  if (!img.data) {
    return -1;
  }

  cv::namedWindow(window_name, CV_WINDOW_AUTOSIZE);

  // set up filter
  anchor = cv::Point(-1, -1);
  delta = 0;
  ddepth = -1;

  double kernel_vals[2] = {0.0f, 1.0f};
  kernel = cv::Mat(1, 2 , CV_32F, &kernel_vals);
  kernel = cv::Mat(3, 1 , CV_32F, &kernel_vals);
  cv::filter2D(img, filtered, ddepth, kernel); // , anchor, delta, cv::BORDER_DEFAULT);
  // cv::Sobel(img, filtered, -1, 1, 0, 3, 1, 100);
  cv::imshow(window_name, filtered);
  cv::waitKey(0);

  return 0;
}


//
// Copyright Antoine Leblanc 2010 - 2015
// Distributed under the MIT license.
//
// http://nauths.fr
// http://github.com/nicuveo
// mailto://antoine.jp.leblanc@gmail.com
//

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Includes

#include <cstdlib>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "nauths/mml/mml.hh"

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Local code

namespace
{

  typedef mml::Shape<mml::Num> Shape;
  typedef mml::Point<mml::Num> Point;
  typedef mml::perlin<> Perlin;

}

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Implementation

BOOST_AUTO_TEST_SUITE(perlin)

BOOST_AUTO_TEST_CASE(PERLIN_BASIC)
{
  Perlin::NoiseSum s = Perlin::harmonic_sum(1, 4, 0.25);

  BOOST_REQUIRE(s.size() == 4);

  for (int y = 0; y <= 100; ++y)
    for (int x = 0; x <= 100; ++x)
    {
      double noise = s(double(x) / 100, double(y) / 100);
      BOOST_REQUIRE_GE(noise, 0);
      BOOST_REQUIRE_LE(noise, 1.);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/Polygon_mesh_processing/triangulate_faces.h>
#include <CGAL/boost/graph/copy_face_graph.h>

#include <boost/foreach.hpp>
#include <boost/unordered_map.hpp>

#include <fstream>
#include <map>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3                    Point;
typedef CGAL::Surface_mesh<Point>          Surface_mesh;
typedef boost::graph_traits<Surface_mesh>::face_descriptor face_descriptor;

class Insert_iterator
{
  typedef boost::unordered_map<face_descriptor,face_descriptor> Container;
  Container& container;
public:

  Insert_iterator(Container &c)
  : container(c) {}

  Insert_iterator&
  operator=(const std::pair<face_descriptor, face_descriptor>& p)
  {
    container[p.second] = p.first;
    return *this;
  }

  Insert_iterator&
  operator*() { return *this; }

  Insert_iterator
  operator++(int) { return *this; }

};

struct Visitor
{
   typedef boost::unordered_map<face_descriptor,face_descriptor> Container;

  Container& container;
  face_descriptor qfd;

  Visitor(Container& container)
    : container(container)
  {}

  void before_subface_creations(face_descriptor fd)
  {
    std::cout << "split : " << fd << " into:" << std::endl;
    Container::iterator it = container.find(fd);
    qfd = it->second;
    container.erase(it);
  }

  void after_subface_created(face_descriptor fd)
  {
    std::cout << "  " << fd;
    container[fd]=qfd;
  }

  void after_subface_creations()
  {
    std::cout << std::endl;
  }
};

int main(int argc, char* argv[])
{
  const std::string filename = (argc > 1) ? argv[1] : CGAL::data_file_path("meshes/P.off");
  std::ifstream input(filename);

  Surface_mesh mesh;
  if (!input || !(input >> mesh) || mesh.is_empty())
  {
    std::cerr << "Not a valid off file." << std::endl;
    return 1;
  }

  boost::unordered_map<face_descriptor,face_descriptor> t2q;

  Surface_mesh copy;

  CGAL::copy_face_graph(mesh, copy, CGAL::parameters::face_to_face_output_iterator(Insert_iterator(t2q)));

  Visitor v(t2q);
  CGAL::Polygon_mesh_processing::triangulate_faces(copy,
                                                   CGAL::Polygon_mesh_processing::parameters::visitor(v));

  for(boost::unordered_map<face_descriptor,face_descriptor>::iterator it = t2q.begin(); it != t2q.end(); ++it){
    std::cout << it->first << "  "  << it->second << std::endl;
  }

  return 0;
}


#ifndef CONF2CORR_HPP
#define CONF2CORR_HPP

#include <string>
#include <map>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <typeinfo>
#include <fstream>
#include <iostream>
#include <functional>
#include <boost/algorithm/string.hpp>
#include <Eigen/Core>
#include <Eigen/LU>
#include <unordered_map>
#include <memory>
#include <cfloat>

#include "./parser.hpp"
#include "./input.hpp"

using allclusters = std::vector<std::vector<std::vector<std::vector<int>>>>;
using indexorders = std::vector<std::vector<std::vector<std::vector<int>>>>;
using basisfunc   = std::vector<std::vector<double>>;

class Conf2corr {
	private:
		std::vector<double> compositions_before;
		/* spins consists of index of spince */
		std::vector<int> spins;
		/*  tuple[spin_index, spin_before, spin_after] */
		std::vector<std::array<int, 3>> vec_changed_spins;
		/* all_calculated_basis_functions[order][spin -> basisfunc] */
		std::vector<std::vector<double>> all_calculated_basis_functions;

		/* vec[basis][degree] vec[basis]->polynomials */
		std::shared_ptr<basisfunc> pbasis_functions;
		/*
		 * vec[num_in_cluster][index][combinations][order]
		 * 1 <= order <= compositions.size()-1
		*/
		std::shared_ptr<indexorders> pindex_orders;
		/*  allclusters index->site->multiplicity->clusters */
		std::shared_ptr<allclusters> pall_clusters;

		const std::shared_ptr<ParsePoscar> pposcar_spin;

		/* --------- Input parameter for Conf2corr --------- */
		std::vector<double> spinposcar;
		std::vector<double> spince;  // if 24 8 in poscar at line t, [0]-> -1 for 24 atoms and [1] -> 1 for 8 atoms
		std::vector<int> index_for_check_inside;
		double spin_for_check_inside = -10000;
		/* ------------------------------------------------- */

		std::mt19937 mt;
		std::uniform_int_distribution<int> rnd_int_N;
		std::uniform_int_distribution<int> rnd_int_spince_index;
		std::uniform_real_distribution<double> rnd_real;

	/* for efficiency */
	protected:
		std::vector<double> compositions;
		std::vector<std::vector<double>> correlation_functions;

	public:
		Conf2corr(char* filename,
			std::vector<double> _spinposcar,
			std::vector<double> _spince,
			std::shared_ptr<labels> _plabels,
			std::shared_ptr<allclusters> _pall_clusters,
			std::shared_ptr<basisfunc>   _pbasis_functions = nullptr,
			std::shared_ptr<indexorders> _pindex_orders = nullptr
		);
		Conf2corr(char* filename,
			std::shared_ptr<Input> _in,
			std::shared_ptr<labels> _plabels,
			std::shared_ptr<allclusters> _pall_clusters,
			std::shared_ptr<basisfunc>   _pbasis_functions = nullptr,
			std::shared_ptr<indexorders> _pindex_orders = nullptr
		);
		~Conf2corr(){};

		void setSpins(std::shared_ptr<labels> plabels);
		void setSpins(std::vector<int> _spins){ this->spins = _spins; }
		void setSpins(int i, double spin){ this->spins.at(i) = spin; }
		void setSpinsRandom(){
			std::shuffle(this->spins.begin(), this->spins.end(), mt);
			this->setInitialCorrelationFunction();
		}
		void setCompositions();

		void setBasisCoefficient();
		void setIndexOrders();
		void setBasisFunction();

		void setInitialCorrelationFunction();
		void setCorrelationFunction_flip(int lattice_point = -1, int after_spin = -1);
		void setCorrelationFunction_exchange();

		double calcCorrelationFunctionNorm(double p);

		virtual void setMemento();
		virtual void Memento();

		bool isInNthNearestNeighborPair(int lattice_point);

		void dispCorr();
		void outputPoscar(std::string prefix="out");

		int RandN(){return rnd_int_N(mt);};
		double RandReal(){ return rnd_real(mt);};

		std::vector<int> const& getSpins() const { return spins; };
		std::vector<double> const& getCompositions() const{ return compositions; };
		double const& getCompositions(int i) const{ return compositions[i]; };
		std::vector<double> const& getSpinCE() const{ return spince; };
		std::vector<double> const& getSpinPoscar() const{ return spinposcar; };
		std::vector<std::vector<double>> const& getCorrelationFunctions() const { return correlation_functions; };
		double const& getCorrelationFunctions(int i, int j) const{ return correlation_functions[i][j]; };

		std::vector<double> const& getBeforeCompositions() const{ return compositions_before; };
		std::vector<std::vector<double>> const& getBeforeCorrelationFunctions() const{ return correlation_functions; };
		std::shared_ptr<basisfunc> const& getBasisFunc() const{ return pbasis_functions;}
		std::shared_ptr<indexorders> const& getIndexOrders() const{ return pindex_orders;}
		std::shared_ptr<allclusters> const& getAllClusters() const{ return pall_clusters;}
		std::mt19937 const& getMt() const{ return mt;}

};

#endif


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

  @copyright 2015 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_PREDICATES_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-predicates Predicates functions

      Those functions provides scalar and SIMD algorithms for testing values properties.
      Tests like checks for validity, infinity, sign and others are provided. As for @ref
      group-boolean, those functions handles and returns typed logical types.
  **/

  /*!
    @ingroup group-callable
    @defgroup group-callable-predicates Predicates Callable Objects
    Callable objects version of @ref group-predicates

    Their specific semantic limitations are similar to those of their function
    equivalents as described in the @ref group-predicates section.
  **/
} }

#include <boost/simd/function/ge.hpp>
#include <boost/simd/function/gt.hpp>
#include <boost/simd/function/is_denormal.hpp>
#include <boost/simd/function/is_equal_with_equal_nans.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_even.hpp>
#include <boost/simd/function/is_finite.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/is_ge.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/is_greater_equal.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_gt.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/is_imag.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_invalid.hpp>   q
#include <boost/simd/function/is_le.hpp>
#include <boost/simd/function/is_less_equal.hpp>
#include <boost/simd/function/is_lessgreater.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/is_lez.hpp>
#include <boost/simd/function/is_lt.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/is_nge.hpp>
#include <boost/simd/function/is_ngez.hpp>
#include <boost/simd/function/is_ngt.hpp>
#include <boost/simd/function/is_ngtz.hpp>
#include <boost/simd/function/is_nle.hpp>
#include <boost/simd/function/is_nlez.hpp>
#include <boost/simd/function/is_nlt.hpp>
#include <boost/simd/function/is_nltz.hpp>
#include <boost/simd/function/is_normal.hpp>
#include <boost/simd/function/is_not_denormal.hpp>
#include <boost/simd/function/is_not_equal_with_equal_nans.hpp>
#include <boost/simd/function/is_not_finite.hpp>
#include <boost/simd/function/is_not_greater_equal.hpp>
#include <boost/simd/function/is_not_greater.hpp>
#include <boost/simd/function/is_not_imag.hpp>
#include <boost/simd/function/is_not_infinite.hpp>
#include <boost/simd/function/is_not_invalid.hpp>
#include <boost/simd/function/is_not_less_equal.hpp>
#include <boost/simd/function/is_not_less.hpp>
#include <boost/simd/function/is_not_nan.hpp>
#include <boost/simd/function/is_not_real.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/is_ord.hpp>
#include <boost/simd/function/is_positive.hpp>
#include <boost/simd/function/is_real.hpp>
#include <boost/simd/function/is_unord.hpp>
#include <boost/simd/function/le.hpp>
#include <boost/simd/function/lt.hpp>
#include <boost/simd/function/majority.hpp>

#endif


#include <iostream>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/time.h>
#include <errno.h>
#include <math.h>
#include <string.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <termios.h>
#include <stdint.h>
#include <stdbool.h>
#include <vector>
#include <algorithm>
#include <Eigen/Dense>

// ROS Headers
#include <ros/ros.h>
#include <geometry_msgs/Accel.h>
#include <geometry_msgs/Wrench.h>
#include <geometry_msgs/TransformStamped.h>
#include <gazebo/gazebo.hh>
#include <gazebo_msgs/ModelState.h>
#include <gazebo_msgs/SetModelState.h>
#include <tf/transform_listener.h>

#include "ogm_ae/voxel.h"
#include "ogm_ae/aux_fun.h"

using namespace std;

void DoYawRotation(Voxel& voxel, geometry_msgs::PoseStamped& robotPose, geometry_msgs::TwistStamped& robotVel, geometry_msgs::AccelStamped& robotAccel, gazebo_msgs::ModelState& ModelStateMsg, double& yawRotDuration);

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

    // Initializations
    ros::init(argc, argv, "paths_to_cmd");
    Voxel voxel;
    ros::param::get("/exploration/simulate_gazebo", voxel.explore.simulateGazebo);
    string objective = "exploration";
    int numExplorationSensorsAndRobots, robotNumber(-1);
    voxel.e1 << 1.0, 0.0, 0.0;
    voxel.e2 << 0.0, 1.0, 0.0;
    voxel.e3 << 0.0, 0.0, 1.0;
    string mapFrame, modelName, sensorFrame, robotFrame;
    ros::param::get("/mapping/frame", mapFrame);
    bool haltExploration(false); double haltStamp = ros::Time::now().toSec();
    double lastMsgTimeStamp = ros::Time::now().toSec();
    ogm_ae::PolyLeastSquaresTraj trajInfo;

    // Private Parameters: Robot & Sensor Reference Frames
    ros::NodeHandle privateNH("~");
    privateNH.param("robot_number", robotNumber, robotNumber);
    bool runAutonomousExploration(false);
//    bool runAutonomousExplorationLastStep(false);
    voxel.GetRobotParameters(objective, numExplorationSensorsAndRobots);
    voxel.GetExplorationParameters();
    modelName   = voxel.robotInformation[robotNumber].ns         ;
    robotFrame  = voxel.robotInformation[robotNumber].robotFrame ;
    sensorFrame = voxel.robotInformation[robotNumber].exploreFrame;
    ROS_INFO("Robot %d (namespace %s): Body Frame = %s, Sensor Frame = %s",
             robotNumber, modelName.c_str(), robotFrame.c_str(), sensorFrame.c_str());

    // Times for Gazebo Loading & Initial Yaw Rotation
    double tSecsToLoad, yawRotDuration;
    if(voxel.explore.simulateGazebo)
      ros::param::get("/exploration/gazebo_load_time", tSecsToLoad);
    else
      tSecsToLoad = 1.0;// 1 sec minimum for map reduction parameters
    ros::param::get("/exploration/yaw_rotation_time", yawRotDuration);
    bool doYawRot(false);

    // Subscribers
    GenSubscriber<ogm_ae::PolyLeastSquaresTraj> trajInfoSubscription;
    voxel.trajInfoSub = voxel.nh.subscribe
        (voxel.robotInformation[robotNumber].trajInfoTopic, 1, &GenSubscriber<ogm_ae::PolyLeastSquaresTraj>::callback, &trajInfoSubscription, ros::TransportHints().unreliable().tcpNoDelay());
//    ros::TransportHints().unreliable();
//    ros::TransportHints().tcpNoDelay();

    // Publishers
    voxel.desPosePub  = voxel.nh.advertise
        <geometry_msgs::PoseStamped >
        (voxel.robotInformation[robotNumber].desiredPoseTopic,  1, true);
    voxel.desCamPub   = voxel.nh.advertise
        <geometry_msgs::PoseStamped >
        (voxel.robotInformation[robotNumber].desiredCamTopic,   1, true);
    voxel.desTwistPub = voxel.nh.advertise
        <geometry_msgs::TwistStamped>
        (voxel.robotInformation[robotNumber].desiredTwistTopic, 1, true);
    voxel.desAccelPub = voxel.nh.advertise
        <geometry_msgs::AccelStamped>
        (voxel.robotInformation[robotNumber].desiredAccelTopic, 1, true);
    voxel.gazeboPub   = voxel.nh.advertise
        <gazebo_msgs::ModelState    >
        (voxel.explore.gazeboTopic,                   1, true);

    geometry_msgs::Point zeroPt;
    zeroPt.x = 0.0;
    zeroPt.y = 0.0;
    zeroPt.z = 0.0;
    geometry_msgs::Vector3 zeroVector3;
    zeroVector3.x = 0.0;
    zeroVector3.y = 0.0;
    zeroVector3.z = 0.0;

    geometry_msgs::PoseStamped  robotPose, robotPoseSave, cameraPose;
    robotPose.pose.position = zeroPt;
    ros::param::get("/exploration/height", voxel.explore.height);
    robotPose.pose.position.z = voxel.explore.height;

    geometry_msgs::TwistStamped robotVel;
    robotVel.twist.linear  = zeroVector3;
    robotVel.twist.angular = zeroVector3;

    geometry_msgs::AccelStamped robotAccel;
    robotAccel.accel.linear  = zeroVector3;
    robotAccel.accel.angular = zeroVector3;

    robotPose .header.frame_id = mapFrame;
    cameraPose.header.frame_id = mapFrame;
    robotVel  .header.frame_id = mapFrame;
    robotAccel.header.frame_id = mapFrame;

    gazebo_msgs::ModelState ModelStateMsg;
    ModelStateMsg.model_name = modelName;
    ModelStateMsg.reference_frame = voxel.map.frame;

    ROS_INFO("Waiting %e seconds for Gazebo to load...", tSecsToLoad);
    usleep(tSecsToLoad*1000000);
    ROS_INFO("Beginning desired trajectory commands.");

    // Get TFs
    voxel.GetRobotCameraTFs(sensorFrame, robotFrame, robotNumber);

    // Initialized Last Pose as Current
    robotPoseSave = robotPose;
    tf::StampedTransform robotTF;

    bool firstPass(true), msgOK(false);

    // Spin with 3 Subscribers (paths of position, velocity, & acceleration)
    while (ros::ok()){

      // Current Robot Pose
      robotTF = LoopUntilTransformAcquired(voxel.explore.tfListener, mapFrame, robotFrame);
      robotPose.pose.position.x = robotTF.getOrigin().x();
      robotPose.pose.position.y = robotTF.getOrigin().y();
      robotPose.pose.position.z = voxel.explore.height   ;
      robotPose.pose.orientation.x = robotTF.getRotation().x();
      robotPose.pose.orientation.y = robotTF.getRotation().y();
      robotPose.pose.orientation.z = robotTF.getRotation().z();
      robotPose.pose.orientation.w = robotTF.getRotation().w();

      if(firstPass){
        robotPoseSave = robotPose;
        firstPass = false;
      }

      // Check if Autonomous Exploration Underway (Boolean) & New Messages
      ros::param::get("run_autonomous_exploration", runAutonomousExploration);

      // Yaw Rotation at Beginning or Later on Command
      ros::param::get("do_yaw_rotation", doYawRot);
      if(doYawRot && runAutonomousExploration){

        // Temoparily Set Exploration Off
        ros::param::set("run_autonomous_exploration", false);

        // Do a Yaw Rotation
        DoYawRotation(voxel, robotPose, robotVel, robotAccel, ModelStateMsg, yawRotDuration);
        robotPoseSave = robotPose;

        // Don't Repeat This
        ros::param::set("do_yaw_rotation", false);
        ros::param::set("run_autonomous_exploration", true);

      }

      // Check Message
      if(trajInfoSubscription.msg.header.stamp.toSec() <= ros::Time::now().toSec()
      && trajInfoSubscription.msg.timeStartSeg.size() > 0){
        trajInfo = trajInfoSubscription.msg;
        msgOK = true;
        if(lastMsgTimeStamp != trajInfo.header.stamp.toSec()){
          haltExploration = false;
          ros::param::set("exploration/halt", haltExploration);
          lastMsgTimeStamp = trajInfo.header.stamp.toSec();
        }
        else
          ros::param::get("exploration/halt", haltExploration);
      }
      else
        ros::param::get("exploration/halt", haltExploration);

      if(haltExploration && haltStamp != trajInfo.header.stamp.toSec()){
        ROS_WARN("Halting exploration...");
        haltStamp = trajInfo.header.stamp.toSec();
      }

      // Pose Data from Function Parameters
      if(msgOK && runAutonomousExploration && !haltExploration){

        // Current Time
        ros::Time rosTimeNow = ros::Time::now();

        // Robot Trajectory Function
        voxel.GetXYTrajFromCoeff(robotNumber,
              trajInfo, rosTimeNow,
              cameraPose, robotPose, robotVel, robotAccel);

        // Publish Camera
        voxel.desCamPub.publish(cameraPose);

        // Save Latest Pose
        robotPoseSave = robotPose;

      }
      else{

        // Fixed Location
        robotPose = robotPoseSave;

        // Zero Time Derivatives
        robotVel.twist  .linear  = zeroVector3;
        robotVel.twist  .angular = zeroVector3;
        robotAccel.accel.linear  = zeroVector3;
        robotAccel.accel.angular = zeroVector3;

        // Time Stamps
        robotPose.header.stamp  = ros::Time::now();
        robotVel.header.stamp   = ros::Time::now();
        robotAccel.header.stamp = ros::Time::now();
      }

      // Publish Robot
      voxel.desPosePub .publish(robotPose );
      voxel.desTwistPub.publish(robotVel  );
      voxel.desAccelPub.publish(robotAccel);

      // Gazebo Simulation
      if(voxel.explore.simulateGazebo){
        ModelStateMsg.pose  = robotPose.pose ;
        ModelStateMsg.twist = robotVel .twist;
        voxel.gazeboPub.publish(ModelStateMsg);
      }

      ros::spinOnce();

    }
    return 0;
}

void DoYawRotation(Voxel& voxel,
  geometry_msgs::PoseStamped& robotPose, geometry_msgs::TwistStamped& robotVel, geometry_msgs::AccelStamped& robotAccel,
  gazebo_msgs::ModelState& ModelStateMsg, double& yawRotDuration){

  // Initial Values for Reseting at End
  geometry_msgs::Quaternion quatInit = robotPose.pose.orientation;
  double angVelZInit = robotVel.twist.angular.z, thetaInit;
  QuatToYawRot(thetaInit, quatInit);

  ros::Time yawRotTimeNow = ros::Time::now();
  double yawRotTimeStartSecs = yawRotTimeNow.toSec();
  robotVel.twist.angular.z = 2*M_PI/yawRotDuration;
  while(yawRotTimeNow.toSec() < yawRotTimeStartSecs+yawRotDuration){

    // Pose
    double yawRotTheta
        = 2*M_PI*(yawRotTimeNow.toSec()-yawRotTimeStartSecs)/yawRotDuration
        +thetaInit;
    geometry_msgs::Quaternion quat;
    YawRotToQuat(yawRotTheta, quat);
    robotPose.pose.orientation = quat;
    robotPose.header.stamp = yawRotTimeNow;
    voxel.desPosePub.publish(robotPose);

    // Velocity
    robotVel.header.stamp = yawRotTimeNow;
    voxel.desTwistPub.publish(robotVel);

    // Acceleration
    robotAccel.header.stamp = yawRotTimeNow;
    voxel.desAccelPub.publish(robotAccel);

    // Gazebo
    if(voxel.explore.simulateGazebo){
      ModelStateMsg.pose  = robotPose.pose ;
      ModelStateMsg.twist = robotVel .twist;
      voxel.gazeboPub.publish(ModelStateMsg);
    }

    // Current Time & Spin
    yawRotTimeNow = ros::Time::now();
    ros::spinOnce();
  }

  // Reset Changed Values
  robotPose.pose.orientation = quatInit;
  robotVel.twist.angular.z = angVelZInit;

  return;
}


#include "fluid_solver.h"
#include <deal.II/physics/elasticity/kinematics.h>
#include <deal.II/physics/elasticity/standard_tensors.h>

namespace Fluid
{
  template <int dim>
  BlockVector<double> FluidSolver<dim>::get_current_solution() const
  {
    return present_solution;
  }

  template <int dim>
  FluidSolver<dim>::FluidSolver(Triangulation<dim> &tria,
                                const Parameters::AllParameters &parameters,
                                std::shared_ptr<Function<dim>> bc)
    : triangulation(tria),
      fe(FE_Q<dim>(parameters.fluid_velocity_degree),
         dim,
         FE_Q<dim>(parameters.fluid_pressure_degree),
         1),
      scalar_fe(parameters.fluid_velocity_degree),
      dof_handler(triangulation),
      scalar_dof_handler(triangulation),
      volume_quad_formula(parameters.fluid_velocity_degree + 1),
      face_quad_formula(parameters.fluid_velocity_degree + 1),
      time(parameters.end_time,
           parameters.time_step,
           parameters.output_interval,
           parameters.refinement_interval,
           parameters.save_interval),
      timer(std::cout, TimerOutput::never, TimerOutput::wall_times),
      parameters(parameters),
      boundary_values(bc)
  {
  }

  template <int dim>
  void FluidSolver<dim>::setup_dofs()
  {
    // The first step is to associate DoFs with a given mesh.
    dof_handler.distribute_dofs(fe);
    scalar_dof_handler.distribute_dofs(scalar_fe);

    // We renumber the components to have all velocity DoFs come before
    // the pressure DoFs to be able to split the solution vector in two blocks
    // which are separately accessed in the block preconditioner.
    DoFRenumbering::Cuthill_McKee(dof_handler);
    std::vector<unsigned int> block_component(dim + 1, 0);
    block_component[dim] = 1;
    DoFRenumbering::component_wise(dof_handler, block_component);

    dofs_per_block.resize(2);
    DoFTools::count_dofs_per_block(
      dof_handler, dofs_per_block, block_component);
    unsigned int dof_u = dofs_per_block[0];
    unsigned int dof_p = dofs_per_block[1];

    std::cout << "   Number of active fluid cells: "
              << triangulation.n_active_cells() << std::endl
              << "   Number of degrees of freedom: " << dof_handler.n_dofs()
              << " (" << dof_u << '+' << dof_p << ')' << std::endl;
  }

  template <int dim>
  void FluidSolver<dim>::make_constraints()
  {
    nonzero_constraints.clear();
    zero_constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler, nonzero_constraints);
    DoFTools::make_hanging_node_constraints(dof_handler, zero_constraints);
    for (auto itr = parameters.fluid_dirichlet_bcs.begin();
         itr != parameters.fluid_dirichlet_bcs.end();
         ++itr)
      {
        // First get the id, flag and value from the input file
        unsigned int id = itr->first;
        unsigned int flag = itr->second.first;
        std::vector<double> value = itr->second.second;

        // To make VectorTools::interpolate_boundary_values happy,
        // a vector of bool and a vector of double which are of size
        // dim + 1 are required.
        std::vector<bool> mask(dim + 1, false);
        std::vector<double> augmented_value(dim + 1, 0.0);
        // 1-x, 2-y, 3-xy, 4-z, 5-xz, 6-yz, 7-xyz
        switch (flag)
          {
          case 1:
            mask[0] = true;
            augmented_value[0] = value[0];
            break;
          case 2:
            mask[1] = true;
            augmented_value[1] = value[0];
            break;
          case 3:
            mask[0] = true;
            mask[1] = true;
            augmented_value[0] = value[0];
            augmented_value[1] = value[1];
            break;
          case 4:
            mask[2] = true;
            augmented_value[2] = value[0];
            break;
          case 5:
            mask[0] = true;
            mask[2] = true;
            augmented_value[0] = value[0];
            augmented_value[2] = value[1];
            break;
          case 6:
            mask[1] = true;
            mask[2] = true;
            augmented_value[1] = value[0];
            augmented_value[2] = value[1];
            break;
          case 7:
            mask[0] = true;
            mask[1] = true;
            mask[2] = true;
            augmented_value[0] = value[0];
            augmented_value[1] = value[1];
            augmented_value[2] = value[2];
            break;
          default:
            AssertThrow(false, ExcMessage("Unrecogonized component flag!"));
            break;
          }
        if (parameters.use_hard_coded_values == 1)
          {
            VectorTools::interpolate_boundary_values(
              MappingQGeneric<dim>(parameters.fluid_velocity_degree),
              dof_handler,
              id,
              *boundary_values,
              nonzero_constraints,
              ComponentMask(mask));
          }
        else
          {
            VectorTools::interpolate_boundary_values(
              MappingQGeneric<dim>(parameters.fluid_velocity_degree),
              dof_handler,
              id,
              Functions::ConstantFunction<dim>(augmented_value),
              nonzero_constraints,
              ComponentMask(mask));
          }
        VectorTools::interpolate_boundary_values(
          MappingQGeneric<dim>(parameters.fluid_velocity_degree),
          dof_handler,
          id,
          Functions::ZeroFunction<dim>(dim + 1),
          zero_constraints,
          ComponentMask(mask));
      }
    nonzero_constraints.close();
    zero_constraints.close();
  }

  template <int dim>
  void FluidSolver<dim>::setup_cell_property()
  {
    cell_property.initialize(
      triangulation.begin_active(), triangulation.end(), 1);
    for (auto cell = triangulation.begin_active(); cell != triangulation.end();
         ++cell)
      {
        const std::vector<std::shared_ptr<CellProperty>> p =
          cell_property.get_data(cell);
        p[0]->indicator = 0;
        p[0]->fsi_acceleration = 0;
        p[0]->fsi_stress = 0;
      }
  }

  template <int dim>
  void FluidSolver<dim>::initialize_system()
  {
    system_matrix.clear();
    mass_matrix.clear();
    mass_schur.clear();

    BlockDynamicSparsityPattern dsp(dofs_per_block, dofs_per_block);
    DoFTools::make_sparsity_pattern(dof_handler, dsp, nonzero_constraints);
    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);
    mass_matrix.reinit(sparsity_pattern);

    present_solution.reinit(dofs_per_block);
    solution_increment.reinit(dofs_per_block);
    system_rhs.reinit(dofs_per_block);

    // Compute the sparsity pattern for mass schur in advance.
    // It should be the same as \f$BB^T\f$.
    DynamicSparsityPattern schur_pattern(dofs_per_block[1], dofs_per_block[1]);
    schur_pattern.compute_mmult_pattern(sparsity_pattern.block(1, 0),
                                        sparsity_pattern.block(0, 1));
    mass_schur_pattern.copy_from(schur_pattern);
    mass_schur.reinit(mass_schur_pattern);

    // Cell property
    setup_cell_property();

    stress = std::vector<std::vector<Vector<double>>>(
      dim,
      std::vector<Vector<double>>(dim,
                                  Vector<double>(scalar_dof_handler.n_dofs())));
  }

  template <int dim>
  void FluidSolver<dim>::refine_mesh(const unsigned int min_grid_level,
                                     const unsigned int max_grid_level)
  {
    TimerOutput::Scope timer_section(timer, "Refine mesh");

    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());
    FEValuesExtractors::Vector velocity(0);
    using type = std::map<types::boundary_id, const Function<dim, double> *>;
    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      QGauss<dim - 1>(parameters.fluid_velocity_degree),
      type(),
      present_solution,
      estimated_error_per_cell,
      fe.component_mask(velocity));
    GridRefinement::refine_and_coarsen_fixed_fraction(
      triangulation, estimated_error_per_cell, 0.6, 0.4);
    if (triangulation.n_levels() > max_grid_level)
      {
        for (auto cell = triangulation.begin_active(max_grid_level);
             cell != triangulation.end();
             ++cell)
          {
            cell->clear_refine_flag();
          }
      }

    for (auto cell = triangulation.begin_active(min_grid_level);
         cell != triangulation.end_active(min_grid_level);
         ++cell)
      {
        cell->clear_coarsen_flag();
      }

    BlockVector<double> buffer(present_solution);
    SolutionTransfer<dim, BlockVector<double>> solution_transfer(dof_handler);

    triangulation.prepare_coarsening_and_refinement();
    solution_transfer.prepare_for_coarsening_and_refinement(buffer);

    triangulation.execute_coarsening_and_refinement();

    setup_dofs();
    make_constraints();
    initialize_system();

    solution_transfer.interpolate(buffer, present_solution);
    nonzero_constraints.distribute(present_solution);
  }

  template <int dim>
  void FluidSolver<dim>::output_results(const unsigned int output_index) const
  {
    TimerOutput::Scope timer_section(timer, "Output results");

    std::cout << "Writing results..." << std::endl;
    std::vector<std::string> solution_names(dim, "velocity");
    solution_names.push_back("pressure");

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      data_component_interpretation(
        dim, DataComponentInterpretation::component_is_part_of_vector);
    data_component_interpretation.push_back(
      DataComponentInterpretation::component_is_scalar);
    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(present_solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);

    // Indicator
    Vector<float> ind(triangulation.n_active_cells());
    int i = 0;
    for (auto cell = triangulation.begin_active(); cell != triangulation.end();
         ++cell)
      {
        auto p = cell_property.get_data(cell);
        ind[i++] = p[0]->indicator;
      }
    data_out.add_data_vector(ind, "Indicator");

    // stress
    data_out.add_data_vector(scalar_dof_handler, stress[0][0], "Sxx");
    data_out.add_data_vector(scalar_dof_handler, stress[0][1], "Sxy");
    data_out.add_data_vector(scalar_dof_handler, stress[1][1], "Syy");
    if (dim == 3)
      {
        data_out.add_data_vector(scalar_dof_handler, stress[0][2], "Sxz");
        data_out.add_data_vector(scalar_dof_handler, stress[1][2], "Syz");
        data_out.add_data_vector(scalar_dof_handler, stress[2][2], "Szz");
      }

    data_out.build_patches(parameters.fluid_pressure_degree);

    std::string basename = "fluid";
    std::string filename =
      basename + "-" + Utilities::int_to_string(output_index, 6) + ".vtu";

    std::ofstream output(filename);
    data_out.write_vtu(output);

    static std::vector<std::pair<double, std::string>> times_and_names;
    times_and_names.push_back({time.current(), filename});
    std::ofstream pvd_output(basename + ".pvd");
    DataOutBase::write_pvd_record(pvd_output, times_and_names);
  }

  template <int dim>
  void FluidSolver<dim>::update_stress()
  {
    for (unsigned int i = 0; i < dim; ++i)
      {
        for (unsigned int j = 0; j < dim; ++j)
          {
            stress[i][j] = 0.0;
          }
      }
    std::vector<int> surrounding_cells(scalar_dof_handler.n_dofs(), 0);
    // The stress tensors are stored as 2D vectors of shape dim*dim
    // at cell and quadrature point level.
    std::vector<std::vector<Vector<double>>> cell_stress(
      dim,
      std::vector<Vector<double>>(dim,
                                  Vector<double>(scalar_fe.dofs_per_cell)));
    std::vector<std::vector<Vector<double>>> quad_stress(
      dim,
      std::vector<Vector<double>>(dim,
                                  Vector<double>(volume_quad_formula.size())));

    // The projection matrix from quadrature points to the dofs.
    FullMatrix<double> qpt_to_dof(scalar_fe.dofs_per_cell,
                                  volume_quad_formula.size());
    FETools::compute_projection_from_quadrature_points_matrix(
      scalar_fe, volume_quad_formula, volume_quad_formula, qpt_to_dof);

    FEValues<dim> fe_values(fe,
                            volume_quad_formula,
                            update_values | update_gradients |
                              update_quadrature_points | update_JxW_values);
    const unsigned int n_q_points = volume_quad_formula.size();
    const FEValuesExtractors::Vector velocities(0);
    const FEValuesExtractors::Scalar pressure(dim);
    std::vector<SymmetricTensor<2, dim>> sym_grad_v(n_q_points);
    std::vector<double> p(n_q_points);

    auto cell = dof_handler.begin_active();
    auto scalar_cell = scalar_dof_handler.begin_active();
    std::vector<types::global_dof_index> dof_indices(scalar_fe.dofs_per_cell);
    for (; cell != dof_handler.end(); ++cell, ++scalar_cell)
      {
        scalar_cell->get_dof_indices(dof_indices);
        fe_values.reinit(cell);

        // Fluid symmetric velocity gradient
        fe_values[velocities].get_function_symmetric_gradients(present_solution,
                                                               sym_grad_v);
        // Fluid pressure
        fe_values[pressure].get_function_values(present_solution, p);

        // Loop over all quadrature points to set FSI forces.
        for (unsigned int q = 0; q < volume_quad_formula.size(); ++q)
          {
            SymmetricTensor<2, dim> sigma =
              -p[q] * Physics::Elasticity::StandardTensors<dim>::I +
              2 * parameters.viscosity * sym_grad_v[q];
            for (unsigned int i = 0; i < dim; ++i)
              {
                for (unsigned int j = 0; j < dim; ++j)
                  {
                    quad_stress[i][j][q] = sigma[i][j];
                  }
              }
          }

        for (unsigned int i = 0; i < dim; ++i)
          {
            for (unsigned int j = 0; j < dim; ++j)
              {
                qpt_to_dof.vmult(cell_stress[i][j], quad_stress[i][j]);
                for (unsigned int k = 0; k < scalar_fe.dofs_per_cell; ++k)
                  {
                    stress[i][j][dof_indices[k]] += cell_stress[i][j][k];
                    if (i == 0 && j == 0)
                      surrounding_cells[dof_indices[k]]++;
                  }
              }
          }
      }

    for (unsigned int i = 0; i < dim; ++i)
      {
        for (unsigned int j = 0; j < dim; ++j)
          {
            for (unsigned int k = 0; k < scalar_dof_handler.n_dofs(); ++k)
              {
                stress[i][j][k] /= surrounding_cells[k];
              }
          }
      }
  }

  template class FluidSolver<2>;
  template class FluidSolver<3>;
} // namespace Fluid


// Copyright Louis Dionne 2016
// Distributed under the Boost Software License, Version 1.0.

#define BOOST_HANA_CONFIG_ENABLE_STRING_UDL
#include <boost/hana.hpp>

#include <cassert>
#include <iostream>
#include <string>
namespace hana = boost::hana;
using namespace hana::literals;

template <typename T, typename U, typename V>
struct Triple {
    T first;
    U second;
    V third;
};

struct A { char data; };
struct B { int data; };
struct C { double data; };

// sample(main)
template <typename Types>
auto packed_triple(Types const& types) {
    auto sorted = hana::sort(types, [](auto t, auto u) {
        return hana::alignof_(t) > hana::alignof_(u);
    });

    return hana::unpack(sorted, hana::template_<Triple>);
}

int main() {
    auto types = hana::tuple_t<A, B, C>;
    using Packed = decltype(packed_triple(types))::type;
    Packed triple{/* ... */};
}
// end-sample


#define GLM_ENABLE_EXPERIMENTAL
#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <geometry_msgs/Transform.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/LinearMath/Matrix3x3.h>
#include <tf_conversions/tf_eigen.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/filters/passthrough.h>
#include <pcl/registration/icp.h>
#include <pcl/console/parse.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <iostream>
#include <thread>
#include <chrono>
#include <sstream>
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/Eigen>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <glm/gtx/string_cast.hpp>
#include "pcl_register_CUDA.hpp"
#include "scanmatch.h"
// CONSTANTS of ICP CUDA
#define VISUALIZE 1
#define STEP true
#define CPU false
#define GPU_NAIVE false
#define GPU_OCTREE true
#define GPU_KDTREE false
#define MODEL true
#define UNIFORM_GRID 0
#define COHERENT_GRID 0
GLFWwindow *window;
int N_FOR_VIS = 5000;
// glm vectors for ICP CUDA
std::vector<glm::vec3> glm_scene;
std::vector<glm::vec3> glm_ref;
// KDTree Global variables
glm::vec4 *YbufferTree;
glm::ivec3 *track;
int p_treesize;
int p_tracksize;
// Point Cloud pointers
pcl::PointCloud<pcl::PointXYZ>::Ptr ref_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr ref_cloud_transformed_dl(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr ref_cloud_transformed_ICP(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scene_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);

// Reference to Rough Transformation matrix coming from Pose Estimation using DL
Eigen::Matrix4f Rough_T_matrix_from_pose_est = Eigen::Matrix4f::Identity();
Eigen::Matrix4f icp_cuda_transformation;
double leaf_size = 0.005f;

void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{
  pcl::fromROSMsg(*input, *scene_cloud_ptr);
}
pcl::visualization::PCLVisualizer::Ptr simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
  viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();
  return (viewer);
}

void callbackMatrix(const geometry_msgs::Transform::ConstPtr& msg)
{
    Eigen::Matrix<double,3,3> tmat;
    float x = msg->translation.x;
    float y = msg->translation.y;
    float z = msg->translation.z;
    float beta_x = msg->rotation.x;
    float beta_y = msg->rotation.y;
    float beta_z = msg->rotation.z;
    float beta_w = msg->rotation.w;
    tf::Quaternion quat = tf::Quaternion(beta_x,beta_y,beta_z,beta_w);

    tf::Matrix3x3 rot(quat);
    //rot.getRotation(quat);
    tf::matrixTFToEigen(rot,tmat);
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
          Rough_T_matrix_from_pose_est(i,j) = tmat(i,j);
        }
    }
    Rough_T_matrix_from_pose_est(0,3) = x;
    Rough_T_matrix_from_pose_est(1,3) = y;
    Rough_T_matrix_from_pose_est(2,3) = z;
    Rough_T_matrix_from_pose_est(3,0) = 0;
    Rough_T_matrix_from_pose_est(3,1) = 0;
    Rough_T_matrix_from_pose_est(3,2) = 0;
    Rough_T_matrix_from_pose_est(3,3) = 1;
}
std::vector<glm::vec3> pcl_to_glmvec(pcl::PointCloud<pcl::PointXYZ>::Ptr ptr_pcl)
{
  std::vector<glm::vec3> coords;
  int i=0;
  while(i<ptr_pcl->size())
  {
	coords.push_back(glm::vec3(ptr_pcl->points[i].x, ptr_pcl->points[i].y, ptr_pcl->points[i].z));
	i++;
  }
  return coords;
}

void runICPCUDA(std::vector<glm::vec3> glm_scene_vec, std::vector<glm::vec3> glm_ref_vec, glm::mat3 &R, glm::vec3 &t, int cycle)
{
  // Initialization
  int n_points_scene = glm_scene_vec.size();
  int n_points_ref = glm_ref_vec.size();
  #if GPU_NAIVE
    ScanMatch::initSimulationGPU(n_points_scene, n_points_ref, glm_scene_vec, glm_ref_vec);
  #endif
  #if GPU_OCTREE
    ScanMatch::initSimulationGPUOCTREE(n_points_scene, n_points_ref, glm_scene_vec, glm_ref_vec);
  #endif
  #if GPU_KDTREE
    ScanMatch::initSimulationGPUKDTREE(n_points_scene, n_points_ref, glm_scene_vec, glm_ref_vec, YbufferTree, track, p_treesize, p_tracksize);
  #endif
  updateCamera();
  glEnable(GL_DEPTH_TEST);
  std::cout<<"Now started to buffer"<<std::endl;
  // Run CUDA
  float *dptr = NULL;
  float *dptrVertPositions = NULL;
  float *dptrVertVelocities = NULL;
  cudaGLMapBufferObject((void**)&dptrVertPositions, boidVBO_positions);
  cudaGLMapBufferObject((void**)&dptrVertVelocities, boidVBO_velocities);
  #if GPU_NAIVE
    std::cout<<" started to run stepICPGPU_NAIVE"<<std::endl;
    ScanMatch::stepICPGPU_NAIVE(R,t);
  #endif
  #if GPU_OCTREE
    std::cout<<" started to run stepICPGPU_OCTREE"<<std::endl;
    ScanMatch::stepICPGPU_OCTREE(R,t);
  #endif
  #if GPU_KDTREE
    std::cout<<" started to run stepICPGPU_KDTREE"<<std::endl;
    ScanMatch::stepICPGPU_KDTREE(R,t);
  #endif
  #if VISUALIZE
	  ScanMatch::copyPointCloudToVBO(dptrVertPositions, dptrVertVelocities, CPU);
  #endif
  // unmap buffer object
  cudaGLUnmapBufferObject(boidVBO_positions);
  cudaGLUnmapBufferObject(boidVBO_velocities);
}

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

  int cycle = 0;
  const std::string PCD_PATH = "/home/dlar/catkin_ws/body_visible_shifted.pcd";

  // Initialize ROS
  ros::init (argc, argv, "point_cloud_register_using_detection");
  ros::NodeHandle nh;
  ros::Rate r(10);

  // Create a ROS subscriber for the input point cloud from the sensor
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1, cloud_cb);
  ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2> ("xtion/depth_registered/registered_points", 1);
  ros::Subscriber T_matrix_sub = nh.subscribe<geometry_msgs::Transform>("/transformation_wrt_depth_optical_frame", 1, callbackMatrix);

  // Ros Message to publish
  sensor_msgs::PointCloud2 output;
  // Import reference point clouds data
  if (pcl::io::loadPCDFile<pcl::PointXYZ> (PCD_PATH, *ref_cloud_ptr) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file reference point cloud. \n");
    return (-1);
  }

  // Downsampling Reference Cloud ---------------------------------------
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud (ref_cloud_ptr);
  sor.setLeafSize (leaf_size, leaf_size, leaf_size);
  sor.filter (*ref_cloud_ptr);

  float depth_limit;
  float height_limit;
  float width_limit;

  //======================= CUDA SETTINGS ===============================
  cudaDeviceProp deviceProp;
  int gpuDevice = 0;
  int device_count = 0;
  cudaGetDeviceCount(&device_count);
  if(gpuDevice>device_count)
  {
    std::cout
    << "Error: GPU device number is greater than the number of devices!"
    << " Perhaps a CUDA-capable GPU is not installed?"
    << std::endl;
    return false;
  }

  cudaGetDeviceProperties(&deviceProp, gpuDevice);

  int major = deviceProp.major;
  int minor = deviceProp.minor;
  const char *projectName = "pcl_registration_cuda";
  std::ostringstream ss;
  std::string deviceName;
  ss << projectName << " [SM " << major << "." << minor << " " << deviceProp.name << "]";
  deviceName = ss.str();

   // Window setup stuff

  glfwSetErrorCallback(errorCallback);

  if (!glfwInit()) {
    std::cout
    << "Error: Could not initialize GLFW!"
    << " Perhaps OpenGL 3.3 isn't available?"
    << std::endl;
    return false;
  }

  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
  glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
  glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

  window = glfwCreateWindow(width, height, deviceName.c_str(), NULL, NULL);
  if (!window) {
    glfwTerminate();
    return false;
  }
  glfwMakeContextCurrent(window);
  glfwSetKeyCallback(window, keyCallback);
  glfwSetCursorPosCallback(window, mousePositionCallback);
  glfwSetMouseButtonCallback(window, mouseButtonCallback);

  glewExperimental = GL_TRUE;

  if (glewInit() != GLEW_OK) {
    //return false;
    std::cout<<"init failed"<<std::endl;
  }

  std::cout<<"CUDA Device is set to zero"<<std::endl;
  cudaGLSetGLDevice(0);
  initShaders(program);

  // glm Rotation and Translation variables for ICP CUDA
  glm::mat3 R;
  glm::vec3 t;

  bool flag_first_iteration = true;

  std::cout<<"ROS Cycle starts"<<std::endl;
  while(ros::ok())
  {
    cycle++;

    if(scene_cloud_ptr->size() > 0)
    {

      // ============================ FILTERING SCENE POINT CLOUD ===============================
      if(flag_first_iteration == true)
      {
        // Save unfiltered scene cloud
        pcl::io::savePCDFileASCII("data_scene_unfiltered.pcd", *scene_cloud_ptr);
      }
      // ...removing distant points
      pcl::PassThrough<pcl::PointXYZ> pass;
      pass.setInputCloud (scene_cloud_ptr);
      pass.setFilterFieldName ("z");
      depth_limit = Rough_T_matrix_from_pose_est(2,3);
      pass.setFilterLimits (0, depth_limit+0.15);
      pass.filter (*scene_cloud_ptr);
      pass.setFilterFieldName("x");
      width_limit = Rough_T_matrix_from_pose_est(0,3);
      pass.setFilterLimits(width_limit-0.40,width_limit+0.40);
      pass.filter(*scene_cloud_ptr);
      pass.setFilterFieldName("y");
      height_limit = Rough_T_matrix_from_pose_est(1,3);
      pass.setFilterLimits(height_limit-0.20, height_limit+0.75);
      pass.filter(*scene_cloud_ptr);
      pass.setFilterFieldName("z");
      pass.setNegative (true);
      pass.setFilterLimits(0,depth_limit-0.15);
      pass.filter(*scene_cloud_ptr);

      // Downsampling scene point cloud
      pcl::VoxelGrid<pcl::PointXYZ> sor;
      sor.setInputCloud (scene_cloud_ptr);
      sor.setLeafSize (leaf_size, leaf_size, leaf_size);
      sor.filter (*scene_cloud_ptr);
      // Remove Statistical Outliers -------------------------------------
      pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor_statistic_removal;
      sor_statistic_removal.setInputCloud (scene_cloud_ptr);
      sor_statistic_removal.setMeanK (50);
      sor_statistic_removal.setStddevMulThresh(1.0);
      sor_statistic_removal.filter (*scene_cloud_ptr);

      // Save filtered scene cloud
      pcl::io::savePCDFileASCII("data_scene_filtered.pcd", *scene_cloud_ptr);

      // ========================== FIRST PROJECTION with Rough Transformation Matrix ====================================
      auto start = std::chrono::high_resolution_clock::now(); // To compute time interval ICP computation needs
      if(flag_first_iteration == true)
      {
        pcl::transformPointCloud(*ref_cloud_ptr, *ref_cloud_transformed_dl, Rough_T_matrix_from_pose_est);
        // Save the scene after initial transformation
        pcl::io::savePCDFileASCII("data_ref_transformed_dl.pcd", *ref_cloud_transformed_dl);
        std::cout << "Rought Transformation coming from Pose Estimation" << std::endl << Rough_T_matrix_from_pose_est<< std::endl;
      }

      // ========================= ICP ITERATION ==========================================================================
      glm_scene = pcl_to_glmvec(scene_cloud_ptr);
      // Filling source cloud vector
      if(flag_first_iteration == true)
      {
        // Converting filtered ref cloud to glm cloud for initial transformation
        glm_ref = pcl_to_glmvec(ref_cloud_transformed_dl);
      }
      else
      {
        // Converting filtered scene cloud to glm cloud
        glm_ref = pcl_to_glmvec(ref_cloud_transformed_ICP);
      }

      // -----------------  Initialize and run ICP CUDA implementation ------------------
      N_FOR_VIS = glm_scene.size()+glm_ref.size();
      initVAO(N_FOR_VIS);
      std::cout<<"initVAO ends to be processed"<<std::endl;
      cudaGLRegisterBufferObject(boidVBO_positions);
      cudaGLRegisterBufferObject(boidVBO_velocities);
      // Run ICP CUDA algorithm
      runICPCUDA(glm_scene, glm_ref, R, t, cycle);
      std::cout<<"now prepares transformation matrix" << std::endl;
      //Eigen matrix for ICP CUDA transformation to be filled from glm results
      icp_cuda_transformation(0,0) = R[0][0];
      icp_cuda_transformation(0,1) = R[0][1];
      icp_cuda_transformation(0,2) = R[0][2];
      icp_cuda_transformation(1,0) = R[1][0];
      icp_cuda_transformation(1,1) = R[1][1];
      icp_cuda_transformation(1,2) = R[1][2];
      icp_cuda_transformation(2,0) = R[2][0];
      icp_cuda_transformation(2,1) = R[2][1];
      icp_cuda_transformation(2,2) = R[2][2];
      icp_cuda_transformation(0,3) = t[0];
      icp_cuda_transformation(1,3) = t[1];
      icp_cuda_transformation(2,3) = t[2];
      icp_cuda_transformation(3,0) = 0;
      icp_cuda_transformation(3,1) = 0;
      icp_cuda_transformation(3,2) = 0;
      icp_cuda_transformation(3,3) = 1;

      if(flag_first_iteration == true) // for initial iteration
      {
        pcl::transformPointCloud(*ref_cloud_transformed_dl, *ref_cloud_transformed_ICP, icp_cuda_transformation.inverse());
      }
      else //
      {
        pcl::transformPointCloud(*ref_cloud_transformed_ICP, *ref_cloud_transformed_ICP, icp_cuda_transformation.inverse());
        // Save multiple iteration of ICP CUDA
        pcl::io::savePCDFileASCII("data_cuda_next_iterations.pcd", *ref_cloud_transformed_ICP);
      }

      auto end = std::chrono::high_resolution_clock::now();
      auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
	    std::cout << "PCL GPU duration: " << duration.count() << std::endl;

      flag_first_iteration = false;
      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
      #if VISUALIZE
      glUseProgram(program[PROG_BOID]);
      glBindVertexArray(boidVAO);
      glPointSize((GLfloat)pointSize);
      glDrawElements(GL_POINTS, N_FOR_VIS + 1, GL_UNSIGNED_INT, 0);
      glPointSize(1.0f);
      glUseProgram(0);
      glBindVertexArray(0);
      glfwSwapBuffers(window);
      #endif

      ScanMatch::endSimulation();

      //glfwDestroyWindow(window);
	  //glfwTerminate();

    }

    pcl::toROSMsg(*ref_cloud_transformed_ICP,output);
    output.header.frame_id = "camera_depth_optical_frame";
    output.header.stamp = ros::Time::now();
    pub.publish (output);

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

  }

  return 0;
}


/**
 * Performs some tests using the CTU data
 */

#define BOOST_TEST_MAIN TestCTU
#include <boost/test/unit_test.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <deque>

#include <sam/tuples/VastNetflow.hpp>
#include <sam/tuples/Edge.hpp>
#include <sam/tuples/Tuplizer.hpp>
#include <sam/FeatureSubscriber.hpp>
#include <sam/ReadCSV.hpp>
#include <sam/LabelProducer.hpp>
#include <sam/ExponentialHistogramSum.hpp>
#include <sam/ExponentialHistogramVariance.hpp>
#include <sam/ZeroMQPushPull.hpp>

using namespace sam;
using namespace sam::vast_netflow;

typedef VastNetflow TupleType;
typedef SingleBoolLabel LabelType;
typedef Edge<size_t, LabelType, TupleType> EdgeType;
typedef TuplizerFunction<EdgeType, MakeVastNetflow> Tuplizer;

BOOST_AUTO_TEST_CASE( test_sample )
{
  // valid if called from SAM/build with tests/TestCTU.  TODO:
  // make more robust to different locations of build directory
  //std::string dataFile = "../TestSrc/Data/CTU1SampleAsVast.csv";
  std::string dataFile = "../../TestSrc/Data/147.32.84.229.csv";

  Tuplizer tuplizer;

  // Calculating some stats
  std::ifstream infile;
  infile.open( dataFile );
  if (infile.is_open())
  {
    int numPos = 0;
    int numNeg = 0;
    int total = 0;

    std::string line;
    while (std::getline(infile, line))
    {
      auto edge = tuplizer(total, line);
      int label = boost::lexical_cast<int>(std::get<0>(edge.label));
      if (label == 0) {
        numNeg++;
      } else {
        numPos++;
      }
      total++;
    }

    std::cout << "Num Negative " <<  numNeg
              << " Num Positive " << numPos << std::endl;

    infile.close();

    std::size_t nodeId = 0; ///> The node id of this node
    int capacity = 100000;
    auto featureMap = std::make_shared<FeatureMap>(capacity);
    std::string outputfile = "TestCTUOutputFile.txt";
    auto subscriber =
      std::make_shared<FeatureSubscriber>(outputfile, capacity);
    auto receiver =
      std::make_shared<ReadCSV<EdgeType, Tuplizer>>(nodeId, dataFile);
    std::size_t numNodes = 1; ///> The number of nodes in the cluster

    // Get the label
    // Doesn't really need a key, but provide one anyway to the template.
    //TODO Label
    std::string identifier = "label";

    auto label = std::make_shared<LabelProducer<EdgeType, DestIp>>
                (nodeId, featureMap, identifier);
    receiver->registerConsumer(label);
    label->registerSubscriber(subscriber, identifier);

    identifier = "averageSrcTotalBytes";
    int N = 189;
    auto averageSrcTotalBytes = std::make_shared<
                      ExponentialHistogramAve<double, EdgeType,
                                                 SrcTotalBytes,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
    receiver->registerConsumer(averageSrcTotalBytes);
    averageSrcTotalBytes->registerSubscriber(subscriber, identifier);

    identifier = "varSrcTotalBytes";
    auto varSrcTotalBytes =
      std::make_shared<ExponentialHistogramVariance<
        double, EdgeType, SrcTotalBytes, DestIp>>
                               (N, 2, nodeId, featureMap, identifier);
    receiver->registerConsumer(varSrcTotalBytes);
    varSrcTotalBytes->registerSubscriber(subscriber, identifier);

    subscriber->init();

    if (!receiver->connect()) {
      throw std::runtime_error("Problems opening data file");
    }

    receiver->receive();
    //delete consumer;

    int numPosFound = 0;
    int numNegFound = 0;

    // Open the original file
    std::ifstream origfile;
    origfile.open( dataFile );

    // Open the result file
    std::ifstream resultfile = std::ifstream(outputfile);

    // Going to read through the original data file and the result string
    // simultaneously.  There should be a one-to-one correspondence.
    if (resultfile.is_open() && origfile.is_open())
    {
      //Sum of SrcTotalBytes per dest ip
      std::map<std::string, std::deque<long>> valuesSrcTotalBytes;
      int numLines = 0;
      double totalDiffMeanSrcTotalBytes = 0;
      double totalDiffVarSrcTotalBytes = 0;

      bool anotherLine = false;
      // Get a line from the original data file
      std::string orig;
      std::string result;
      while (std::getline(origfile, orig) && std::getline(resultfile, result))
      {

        // Tokenize the line from the original data file
        //std::vector<std::string> lineVector = convertToTokens(orig);
        auto edge = tuplizer(numLines, orig);
        auto tuple = edge.tuple;
        // Get the destIp, which is the key for imux operation
        std::string destIp = std::get<DestIp>(tuple);
        std::cout << "destIP " << destIp << std::endl;

        // Calculate the exact value of the features from the original
        // data file.

        // Exact average SrcTotalBytes over last N netflows with same DestIp
        long srcTotalBytes = boost::lexical_cast<long>(std::get<SrcTotalBytes>(tuple));
        std::cout << "srcTotalBytes" << srcTotalBytes << std::endl;
        if (valuesSrcTotalBytes.count(destIp) == 0) {//Create deque if not there
          valuesSrcTotalBytes[destIp] = std::deque<long>( );
        }
        if (valuesSrcTotalBytes[destIp].size() >= N) { //Pop item if too many
          valuesSrcTotalBytes[destIp].pop_back();
        }
        valuesSrcTotalBytes[destIp].push_front(srcTotalBytes); //Push new value
        double expMeanSrcTotalBytes = calcMean( valuesSrcTotalBytes[destIp] );

        // Exact variance SrcTotalBytes over last N netflows with same DestIp
        double expVarSrcTotalBytes = calcStandardDeviation(
                                      valuesSrcTotalBytes[destIp] );
        expVarSrcTotalBytes *= expVarSrcTotalBytes;

        // Convert the current line from the result string to tokens
        std::vector<std::string> tokenVector = convertToTokens(result);

        // Updating counts of num negative examples and num positive examples
        if (tokenVector[0] == "0") numNegFound++;
        if (tokenVector[0] == "1") numPosFound++;

        std::cout << "result " << result << std::endl;
        double predMeanSrcTotalBytes = boost::lexical_cast<double>(
                                          tokenVector[1]);
        double predVarSrcTotalBytes = boost::lexical_cast<double>(
                                          tokenVector[2]);
        std::cout <<"destIp " << destIp << " srcTotalBytes " << srcTotalBytes
                  << " expMeanSrcTotalBytes " << expMeanSrcTotalBytes
                  << " predMeanSrcTotalBytes " << predMeanSrcTotalBytes
                  << " expVarSrcTotalBytes " << expVarSrcTotalBytes
                  << " predVarSrcTotalBytes " << predVarSrcTotalBytes
                  << " size "<< valuesSrcTotalBytes[destIp].size() << std::endl;

        //std::cout << "totalDiffVarSrcTotalBytes " << totalDiffVarSrcTotalBytes
        //          << std::endl;
        if (expMeanSrcTotalBytes < predMeanSrcTotalBytes) {
          totalDiffMeanSrcTotalBytes +=
            predMeanSrcTotalBytes / expMeanSrcTotalBytes;
        } else {
          totalDiffMeanSrcTotalBytes +=
            expMeanSrcTotalBytes / predMeanSrcTotalBytes;
        }
        if (expVarSrcTotalBytes > 0 && predVarSrcTotalBytes > 0) {
          if (expVarSrcTotalBytes < predVarSrcTotalBytes) {
            totalDiffVarSrcTotalBytes +=
              std::sqrt(predVarSrcTotalBytes) / std::sqrt(expVarSrcTotalBytes);
          } else {
            totalDiffVarSrcTotalBytes +=
              std::sqrt(expVarSrcTotalBytes) / std::sqrt(predVarSrcTotalBytes);
          }
        }
        std::cout << "totalDiffVarSrcTotalBytes " << totalDiffVarSrcTotalBytes
                  << std::endl;

        numLines++;
        // Comparing predicated and expected SrcTotalBytes
        // Individual differences can be big, especially when there is a
        // deletion of a large item.  Later we gather overall differences,
        // which is much smaller
        BOOST_CHECK_CLOSE(predMeanSrcTotalBytes, expMeanSrcTotalBytes, 300);
        BOOST_CHECK_CLOSE(std::sqrt(predVarSrcTotalBytes),
                          std::sqrt(expVarSrcTotalBytes), 2100);

        // SrcTotalBytes can't be negative
        BOOST_CHECK(predMeanSrcTotalBytes >= 0);
        BOOST_CHECK(predVarSrcTotalBytes >= 0);
      }
      std::cout << "average diff " << totalDiffMeanSrcTotalBytes / numLines
                << std::endl;
      std::cout << "average diff " << totalDiffVarSrcTotalBytes / numLines
                << std::endl;
      BOOST_CHECK_CLOSE(1, totalDiffMeanSrcTotalBytes/ numLines, 50);
      BOOST_CHECK_CLOSE(1, totalDiffVarSrcTotalBytes/ numLines, 50);

      BOOST_CHECK_EQUAL(numNeg, numNegFound);
      BOOST_CHECK_EQUAL(numPos, numPosFound);

    } else {
      throw std::runtime_error("Problems opening data file");
    }
    remove(outputfile.c_str());
  } else {
    std::cout << "Couldn't read file " << std::endl;
    throw std::runtime_error("Problem with test.  Couldn't read data file");
  }
  std::cout << "The end " << std::endl;
}


/*
Copyright 2017. All rights reserved.
Computer Vision Group, Visual Computing Institute
RWTH Aachen University, Germany

This file is part of the rwth_mot framework.
Authors: Aljosa Osep (osep -at- vision.rwth-aachen.de)

rwth_mot framework is free software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software
Foundation; either version 3 of the License, or any later version.

rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with
rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin
Street, Fifth Floor, Boston, MA 02110-1301, USA
*/

#include <tracking/kalman_filter.h>

// eigen
#include <Eigen/LU>

// std
#include <iostream>
#include <fstream>

namespace GOT {
    namespace tracking {

        // -------------------------------------------------------------------------------
        // +++ Implementation: Kalman Filter Base +++
        // -------------------------------------------------------------------------------

        KalmanFilter::KalmanFilter(const Parameters &params) : state_vector_dim_(0), params_(params)  {

        }

        void KalmanFilter::ComputePrediction(const Eigen::VectorXd &u, Eigen::VectorXd &x_prio,
                                             Eigen::MatrixXd &P_prio) const {
            x_prio = A_*x_ + u; // Linear

            // Project covariance ahead
            P_prio = A_*P_*A_.transpose() + G_ ;
        }

        void KalmanFilter::Prediction() {
            assert(this->state_vector_dim_==A_.rows());
            assert(this->state_vector_dim_==A_.cols());
            assert(this->state_vector_dim_==G_.rows());
            assert(this->state_vector_dim_==G_.cols());

            Eigen::VectorXd u_zero, x_prio;
            Eigen::MatrixXd P_prio;
            u_zero.setZero(x_.size());
            this->ComputePrediction(u_zero, x_prio, P_prio);

            if (x_prio.size() != this->params_.state_vector_dim_) {
                return;
            }

            x_ = x_prio;
            P_ = P_prio;

            // Optionally, store whole history. Eg. for visualization.
            if (params_.store_history_) {
                this->state_predictions_.push_back(x_);
                this->cov_predictions_.push_back(P_);
            }
        }

        void KalmanFilter::Prediction(const Eigen::VectorXd &u) {
            assert(this->state_vector_dim_==A_.rows());
            assert(this->state_vector_dim_==A_.cols());
            assert(this->state_vector_dim_==G_.rows());
            assert(this->state_vector_dim_==G_.cols());

            Eigen::VectorXd x_prio;
            Eigen::MatrixXd P_prio;
            this->ComputePrediction(u, x_prio, P_prio);

            if (x_prio.size() != this->params_.state_vector_dim_) {
                return;
            }

            x_ = x_prio;
            P_ = P_prio;

            // Optionally, store whole history. Eg. for visualization.
            if (params_.store_history_) {
                this->state_predictions_.push_back(x_);
                this->cov_predictions_.push_back(P_);
            }
        }

        /// H is the observation model matrix (residual = z_t - H*x)
        void KalmanFilter::Correction(const Eigen::VectorXd z_t, const Eigen::MatrixXd &observation_cov_t, const Eigen::MatrixXd &H) {
            Eigen::MatrixXd H_transposed = H.transpose();

            // Compute Kalman gain
            Eigen::MatrixXd HPH_plus_obs_cov= (H*P_*H_transposed)+observation_cov_t;
            Eigen::MatrixXd K = P_*H_transposed*HPH_plus_obs_cov.inverse();

            // Update the estimate
            Eigen::VectorXd measurement_residual = z_t - H*x_;

            x_ = x_ + K*measurement_residual; //(z_t - H_*x_);

            // Update the covariance
            Eigen::MatrixXd  KH = K*H;
            P_ = (Eigen::MatrixXd::Identity(KH.rows(), KH.cols()) - KH)*P_;

            // Optionally, store whole history. Eg. for visualization.
            if (params_.store_history_) {
                this->state_corrections_.push_back(x_);
                this->cov_corrections_.push_back(P_);
                this->measurements_.push_back(z_t);
                this->cov_measurements_.push_back(observation_cov_t);
                this->kalman_gain_.push_back(K);
            }
        }

        /// Format: /path/to/dir/name_%s.txt
        /// Note: state dim. is not stored. At the client side, it is assumed to be simply 'known'.
        void KalmanFilter::SaveHistoryToFile(const char *filename) const {
            if (!this->params_.store_history_) {
                std::cout << "KalmanFilter::Error: Attempting to save Kalman history, but the params.save_history flag is off! Abort!" << std::endl;
                return;
            }

            /// States
            char buff[500];
            snprintf(buff, 500, filename, "state_predictions");
            std::ofstream state_pred_stream(buff);

            snprintf(buff, 500, filename, "state_corrections");
            std::ofstream state_corr_stream(buff);

            snprintf(buff, 500, filename, "state_measurements");
            std::ofstream state_meas_stream(buff);

            for (const auto &vec : state_predictions_)
                state_pred_stream << vec.transpose() << std::endl;

            for (const auto &vec : state_corrections_)
                state_corr_stream << vec.transpose() << std::endl;

            for (const auto &vec : measurements_)
                state_meas_stream << vec.transpose() << std::endl;

            //! Covariances
            snprintf(buff, 500, filename, "covariance_predictions");
            std::ofstream cov_pred_stream(buff);

            snprintf(buff, 500, filename, "covariance_corrections");
            std::ofstream cov_corr_stream(buff);

            snprintf(buff, 500, filename, "covariance_measurements");
            std::ofstream cov_meas_stream(buff);

            for (const auto &mat : cov_predictions_)
                cov_pred_stream << mat << std::endl;

            for (const auto &mat : cov_corrections_)
                cov_corr_stream << mat << std::endl;

            for (const auto &mat : cov_measurements_)
                cov_meas_stream << mat << std::endl;

            // Close file streams
            state_pred_stream.close();
            state_corr_stream.close();
            state_meas_stream.close();

            cov_pred_stream.close();
            cov_corr_stream.close();
            cov_meas_stream.close();
        }

        // Setters / Getters
        const Eigen::VectorXd& KalmanFilter::x() const {
            return x_;
        }

        void KalmanFilter::set_x(const Eigen::VectorXd &state) {
            this->x_ = state;
        }

        const Eigen::MatrixXd& KalmanFilter::P() const {
            return this->P_;
        }

        void KalmanFilter::set_G(const Eigen::MatrixXd& G) {
            this->G_ = G;
        }

        const Eigen::MatrixXd& KalmanFilter::G() const {
            return this->G_;
        }

        const std::vector<Eigen::VectorXd>& KalmanFilter::state_predictions() const {
            return this->state_predictions_;
        }

        const std::vector<Eigen::VectorXd>& KalmanFilter::state_corrections() const {
            return this->state_corrections_;
        }

        const std::vector<Eigen::MatrixXd>& KalmanFilter::cov_predictions() const {
            return this->cov_predictions_;
        }

        const std::vector<Eigen::MatrixXd>& KalmanFilter::cov_corrections() const {
            return this->cov_corrections_;
        }

        const std::vector<Eigen::VectorXd>& KalmanFilter::measurements() const {
            return this->measurements_;
        }

        const std::vector<Eigen::MatrixXd>& KalmanFilter::cov_measurements() const {
            return this->cov_measurements_;
        }

        const KalmanFilter::Parameters KalmanFilter::parameters() const {
            return params_;
        }

        void KalmanFilter::set_A(const Eigen::MatrixXd &A) {
            this->A_ = A;
        }

        const Eigen::MatrixXd &KalmanFilter::A() const {
            return this->A_;
        }

        const std::vector<Eigen::MatrixXd> &KalmanFilter::kalman_gains() const {
            return this->kalman_gain_;
        }

        // -------------------------------------------------------------------------------
        // +++ Implementation: Little example: simple Const-Velocity model +++
        // -------------------------------------------------------------------------------

        ConstantVelocityKalmanFilter::ConstantVelocityKalmanFilter(const Parameters &params) : params_(params), KalmanFilter(
                static_cast<KalmanFilter::Parameters>(params)) {
        }

        void ConstantVelocityKalmanFilter::Init(const Eigen::VectorXd &x_0, const Eigen::MatrixXd &P_0) {
            // Set params
            this->state_vector_dim_ = params_.state_vector_dim_;
            const double dt = params_.dt; // Assume delta_t parameter is given

            assert(this->state_vector_dim_==x_0.size());
            assert(this->state_vector_dim_==P_0.rows());
            assert(this->state_vector_dim_==P_0.cols());

            // Set initial state
            this->x_ = x_0;
            this->P_ = P_0;

            // Set up transition matrix A (simply adds change in velocity to pos.)
            A_.setIdentity(state_vector_dim_, state_vector_dim_);
            A_(0,2) = dt;
            A_(1,3) = dt;

            G_ = Eigen::Matrix4d::Identity() *0.1*0.1; // Default process noise.
        }
    }
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/lexical_cast.hpp>
using namespace std;
int main() {
    string n; cin >> n;
    int m = n.length();
    vector<int> v(m, 0);
    long long int ans = 0;
    for (int i = 0; i < m; i++) v[i] = i + 1;
    do {
        string p = "";
        for (int i = 0; i < m; i++) p += n[v[i] - 1];
        for (int i = 1; i <= m - 1; i++) {
            string a = p.substr(0, i), b = p.substr(i, m - i);
            if (a[0] == '0' || b[0] == '0') continue;
            ans = max(ans, boost::lexical_cast<long long int>(a) * boost::lexical_cast<long long int>(b));
        }
    } while(next_permutation(v.begin(), v.end()));
    cout << ans << endl;
}


#include <fstream>
#include <sstream>
#include <iterator>
#include <iostream>
#include <fmt/format.h>
#include <string>
#include <numeric>
#include <execution>
#include <utility>
#include <algorithm>
#include <map>
#include <cstdint>
#include <boost/integer/mod_inverse.hpp>
using std::int64_t;
using gint = int64_t;
using std::int64_t;
template<class T>
constexpr T mymod(T n, T N) noexcept
{
  n %= N;
  if (n < 0) {
    n += N;
  }
  return n;
}
struct TimeStamp
{

  static constexpr int64_t firstvalid(int r, int pos) noexcept
  {
    std::array<std::pair<int64_t, int64_t>, 2> primeposs{
      std::pair{ 91l, r },
      std::pair{ pos, 0l },
    };

    int64_t sum = 0;

    int64_t M = 1;
    for (auto [p, i] : primeposs) {
      M *= p;
    }
    for (int i = 0; i < primeposs.size(); ++i) {
      auto [mi, a] = primeposs[i];
      auto Mi = M / mi;
      auto ti = boost::integer::mod_inverse(Mi, mi);
      sum += Mi * ti * a;
    }
    return sum % M;
  }
};
int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    int r, p;
    int sum = 0;
    std::vector<std::pair<int, int>> v;
    for (int i = 0; ifs >> r >> p; ++i) {
      v.push_back(std::pair{ r, 2 * (p - 1) });
    }
    bool notfound = true;
    auto t = 0;
    for (; notfound; ++t) {
      bool alltrue = true;
      for (auto [f, s] : v) {
        if (!((t + f) % s)) {
          //fmt::print("f:{},s:{}\n", f, s);
          alltrue = false;
          break;
        }
      }
      if (alltrue) {
        //fmt::print("foundt:{}\n", t);
        break;
      }
      notfound = !alltrue;
    }
    fmt::print("t:{}\n", t);
  }
}


#ifndef BENCHMARK__BENCH_TYPES_HPP_
#define BENCHMARK__BENCH_TYPES_HPP_

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <smooth/feedback/qp.hpp>

#include <chrono>
#include <fstream>
#include <iostream>
#include <random>

/**
 * @brief Create a random quadratic program of dimension m x n.
 * @param m number on inequalities
 * @param n number of variables
 * @param density approximate proportion of nonzeros in P and A
 * @param RNG random number generator
 */
template<typename RNG>
smooth::feedback::QuadraticProgram<-1, -1> random_qp(int m, int n, double density, RNG & rng)
{
  std::bernoulli_distribution bdist(density);
  std::uniform_real_distribution<double> udist(-1, 1);

  Eigen::MatrixXd A =
    Eigen::MatrixXd::NullaryExpr(m, n, [&](int, int) { return bdist(rng) ? udist(rng) : 0.; });
  Eigen::MatrixXd Lrand =
    Eigen::MatrixXd::NullaryExpr(n, n, [&](int, int) { return bdist(rng) ? udist(rng) : 0.; });
  Eigen::MatrixXd L                         = Eigen::MatrixXd::Zero(n, n);
  L.template triangularView<Eigen::Lower>() = Lrand.template triangularView<Eigen::Lower>();
  for (int i = 0; i < n; ++i) { L(i, i) = std::max({L(i, i), -L(i, i), 0.05}); }

  Eigen::VectorXd v     = Eigen::VectorXd::NullaryExpr(n, [&](int) { return udist(rng); });
  Eigen::VectorXd delta = Eigen::VectorXd::NullaryExpr(m, [&](int) { return udist(rng); });

  return smooth::feedback::QuadraticProgram<-1, -1>{
    .P = L * L.transpose(),
    .q = Eigen::VectorXd::NullaryExpr(n, [&](int) { return udist(rng); }),
    .A = A,
    .l = Eigen::VectorXd::Constant(m, -std::numeric_limits<double>::infinity()),
    .u = A * v + delta,
  };
}

/**
 * @brief Create a random sparse quadratic program of dimension m x n.
 */
inline smooth::feedback::QuadraticProgramSparse<double>
qp_dense_to_sparse(const smooth::feedback::QuadraticProgram<-1, -1, double> & qp)
{
  smooth::feedback::QuadraticProgramSparse<double> qps;

  qps.P = qp.P.sparseView();
  qps.P.prune(1e-6);
  qps.P.makeCompressed();
  qps.q = qp.q;
  qps.A = qp.A.sparseView();
  qps.A.prune(1e-6);
  qps.A.makeCompressed();
  qps.l = qp.l;
  qps.u = qp.u;

  return qps;
}

struct BenchResult
{
  smooth::feedback::QPSolutionStatus status;
  std::chrono::high_resolution_clock::duration dt;
  uint64_t iter;
  Eigen::VectorXd solution;
  double objective;
};

struct BatchResult
{
  std::vector<BenchResult> results{};

  std::size_t num_optimal{0};

  double total_duration_success{0};
  double total_duration_fail{0};

  double avg_duration_success{0};
  double avg_duration_fail{0};
};

template<typename SolverWrapper, std::ranges::range R>
BatchResult solve_batch(const R & qps, const smooth::feedback::QPSolverParams & prm)
{
  auto work_range =
    std::views::transform(qps, [&prm](const auto & qp) { return SolverWrapper(qp, prm); });

  BatchResult ret{};

  for (const auto & work : work_range) { ret.results.push_back(work()); }

  // calculate accuracies
  for (const auto & result : ret.results) {
    if (result.status == smooth::feedback::QPSolutionStatus::Optimal) {
      ++ret.num_optimal;
      ret.total_duration_success += std::chrono::duration<double>(result.dt).count();
    } else {
      ret.total_duration_fail += std::chrono::duration<double>(result.dt).count();
    }
  }

  std::size_t num_fail = ret.results.size() - ret.num_optimal;

  ret.avg_duration_success =
    ret.num_optimal > 0 ? ret.total_duration_success / ret.num_optimal : -1;
  ret.avg_duration_fail = num_fail > 0 ? ret.total_duration_fail / num_fail : -1;

  return ret;
}

#endif  // BENCHMARK__BENCH_TYPES_HPP_


#include <boost/preprocessor/arithmetic/div.hpp>


#ifndef STAN_OPTIMIZATION_LBFGS_UPDATE_HPP
#define STAN_OPTIMIZATION_LBFGS_UPDATE_HPP

#include <stan/math/prim/fun/Eigen.hpp>
#include <boost/circular_buffer.hpp>
#include <tuple>
#include <vector>

namespace stan {
namespace optimization {
/**
 * Implement a limited memory version of the BFGS update.  This
 * class maintains a circular buffer of inverse Hessian updates
 * which can be applied to compute the search direction.
 **/
template <typename Scalar = double, int DimAtCompile = Eigen::Dynamic>
class LBFGSUpdate {
 public:
  typedef Eigen::Matrix<Scalar, DimAtCompile, 1> VectorT;
  typedef Eigen::Matrix<Scalar, DimAtCompile, DimAtCompile> HessianT;
  // NOLINTNEXTLINE(build/include_what_you_use)
  typedef std::tuple<Scalar, VectorT, VectorT> UpdateT;

  explicit LBFGSUpdate(size_t L = 5) : _buf(L) {}

  /**
   * Set the number of inverse Hessian updates to keep.
   *
   * @param L New size of buffer.
   **/
  void set_history_size(size_t L) { _buf.rset_capacity(L); }

  /**
   * Add a new set of update vectors to the history.
   *
   * @param yk Difference between the current and previous gradient vector.
   * @param sk Difference between the current and previous state vector.
   * @param reset Whether to reset the approximation, forgetting about
   * previous values.
   * @return In the case of a reset, returns the optimal scaling of the
   * initial Hessian
   * approximation which is useful for predicting step-sizes.
   **/
  inline Scalar update(const VectorT &yk, const VectorT &sk,
                       bool reset = false) {
    Scalar skyk = yk.dot(sk);

    Scalar B0fact;
    if (reset) {
      B0fact = yk.squaredNorm() / skyk;
      _buf.clear();
    } else {
      B0fact = 1.0;
    }

    // New updates are pushed to the "back" of the circular buffer
    Scalar invskyk = 1.0 / skyk;
    _gammak = skyk / yk.squaredNorm();
    _buf.push_back();
    _buf.back() = std::tie(invskyk, yk, sk);

    return B0fact;
  }

  /**
   * Compute the search direction based on the current (inverse) Hessian
   * approximation and given gradient.
   *
   * @param[out] pk The negative product of the inverse Hessian and gradient
   * direction gk.
   * @param[in] gk Gradient direction.
   **/
  inline void search_direction(VectorT &pk, const VectorT &gk) const {
    std::vector<Scalar> alphas(_buf.size());
    typename boost::circular_buffer<UpdateT>::const_reverse_iterator buf_rit;
    typename boost::circular_buffer<UpdateT>::const_iterator buf_it;
    typename std::vector<Scalar>::const_iterator alpha_it;
    typename std::vector<Scalar>::reverse_iterator alpha_rit;

    pk.noalias() = -gk;
    for (buf_rit = _buf.rbegin(), alpha_rit = alphas.rbegin();
         buf_rit != _buf.rend(); buf_rit++, alpha_rit++) {
      Scalar alpha;
      const Scalar &rhoi(std::get<0>(*buf_rit));
      const VectorT &yi(std::get<1>(*buf_rit));
      const VectorT &si(std::get<2>(*buf_rit));

      alpha = rhoi * si.dot(pk);
      pk -= alpha * yi;
      *alpha_rit = alpha;
    }
    pk *= _gammak;
    for (buf_it = _buf.begin(), alpha_it = alphas.begin(); buf_it != _buf.end();
         buf_it++, alpha_it++) {
      Scalar beta;
      const Scalar &rhoi(std::get<0>(*buf_it));
      const VectorT &yi(std::get<1>(*buf_it));
      const VectorT &si(std::get<2>(*buf_it));

      beta = rhoi * yi.dot(pk);
      pk += (*alpha_it - beta) * si;
    }
  }

 protected:
  boost::circular_buffer<UpdateT> _buf;
  Scalar _gammak;
};
}  // namespace optimization
}  // namespace stan

#endif