#include <limits>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <cmath>
#include "ndt_map/ndt_map_hmt.h"
#include <fstream>
#include "ndt_map/ndt_cell.h"
#include "ros/ros.h"
class map_converter {
	std::string ndt_map_directory;
	std::string occ_map_name;
	double ndt_resolution;
	double occ_resolution;
	double lik_tr;
	double floor_height;
	double floor_deviation;
	double robot_height;
	double xs, ys;
	double v_size_x;
	double v_size_y;
	double v_size_z;
	double max_range;
	int width, height;
	perception_oru::NDTMapHMT *ndtMap;
	perception_oru::LazyGrid *mapGrid;
	bool floor_only;
	std::vector<double> floor_levels;
	std::vector<int> floor_count;
	std::vector<std::string> map_names;
	std::vector<std::vector<std::vector<int> > >maps;
	std::vector<std::vector<int> > map;
public:
	map_converter(ros::NodeHandle private_nh){
		private_nh.param<std::string>("ndt_map_directory", ndt_map_directory, "map");
		private_nh.param<double>("ndt_resolution", ndt_resolution, 0.4);
		private_nh.param<std::string>("occ_map_name", occ_map_name, "occ");
		private_nh.param<double>("occ_resolution", occ_resolution, 0.1);
		private_nh.param<double>("floor_height", floor_height, -0.3);
		private_nh.param<double>("floor_deviation", floor_deviation, 0.15);
		private_nh.param<double>("lik_tr", lik_tr, 0.1);
		private_nh.param<double>("robot_height", robot_height, 1.9);
		private_nh.param<bool>("floor_only", floor_only, false);
		private_nh.param<double>("v_size_x", v_size_x, 80.0);
		private_nh.param<double>("v_size_y", v_size_y, 80.0);
		private_nh.param<double>("v_size_z", v_size_z, 10.0);
		private_nh.param("max_range", max_range, 80.0);
		//InitializeOCCMap();
		LoadMap();
		BuildMapN();
		// SaveMap();
		// saveYAMLFile();
	}

private:
	int LoadMap(){

		ndtMap = new perception_oru::NDTMapHMT(ndt_resolution, 0, 0, 0, v_size_x, v_size_y, v_size_z, max_range, ndt_map_directory);
		Eigen::Vector3d newPos;
		newPos << 0, 0, 0;
		ndtMap->setInsertPosition(newPos);
		return 0;
	}

	double getAverageLevel(std::vector<perception_oru::NDTCell*> floors){
		double aver = 0;

		for(size_t i = 0; i < floors.size(); i++)
			aver += floors[i]->getMean()[2];
		return aver / double(floors.size());
	}

	void BuildMapN(){
		double X_min = 0.125;//-359.125;
		double Y_min = 0.125;//-299.125;
		double X_max = 199.125;
		double Y_max = 519.125;
		double step = 0.25;
		double Z_max = 1.9;
		double Z_min = -0.3;

		std::cout << "saving map" << std::endl;
		std::string png_map_file = occ_map_name;

		png_map_file += ".pgm";
		std::ofstream map_file(png_map_file.c_str(), std::ofstream::out);
		if(map_file.is_open()){
			map_file << "P2" <<std::endl;
			map_file << int(fabs(X_max - X_min) / step) << " " << int(fabs(Y_max - Y_min) / step) <<std::endl;
			map_file << "1"<<std::endl;
			map.resize(int(fabs(Y_max - Y_min) / step), std::vector<int> (int(fabs(X_max - X_min) / step), 0));
			for(size_t i_x = 0; i_x < fabs(X_max - X_min) / step; i_x++){
				for(size_t i_y = 0; i_y < fabs(Y_max - Y_min) / step; i_y++){
					bool traversable = true;
					Eigen::Vector3d newPos;
					newPos << X_min + i_x * step, Y_min + i_y * step, 0;
					ndtMap->setInsertPosition(newPos);
					for(size_t i_z = 0; i_z < int(fabs(Z_max-Z_min)/step); i_z++){
						Eigen::Vector3d newPos;
						pcl::PointXYZ pt(X_min + i_x * step, Y_min + i_y * step, Z_min +i_z*step);
						ROS_INFO_STREAM(pt);
						perception_oru::NDTCell *cell;
						if(ndtMap->getCellForPoint(pt, cell)){
							if (cell==NULL) {
								traversable = false;
								break;
							}
							double cz;
							cell->getCenter(cz, cz, cz);
							pcl::PointXYZ p1(X_min + i_x * step, Y_min + i_y * step, cz - ndt_resolution / 2.0);
							pcl::PointXYZ p2(X_min + i_x * step, Y_min + i_y * step, cz + ndt_resolution / 2.0);
							Eigen::Vector3d out;
							if(cell->computeMaximumLikelihoodAlongLine(p1, p2, out) > lik_tr){
								// ROS_INFO_STREAM(__LINE__);
								traversable = false;
								break;
							}
						}else {
							traversable = false;
							break;
						}
					}
					if(traversable){
						map[i_y][i_x] = 1;

					}
					else{
						map[i_y][i_x] = 0;
					}
					map_file << map[i_y][i_x] << " ";
					map_file.flush();
				}
				map_file << std::endl;
			}
		}
	}

};

	// void BuildMap(){
	//
	// 	std::vector<lslgeneric::NDTCell*> ndts = ndtMap->getAllCells();
	// 	std::vector<std::vector<lslgeneric::NDTCell*> > floors;
	// 	std::vector<float> av_level;
	// 	double sx, sy, sz;
	// 	ndtMap->getCellSizeInMeters(sx, sy, sz);
	// 	for(size_t i = 0; i < ndts.size(); i++){
	// 		if(ndts[i]->getClass() == lslgeneric::NDTCell::HORIZONTAL){
	// 			bool found = false;
	// 			for(size_t j = 0; j < floors.size(); j++){
	// 				if(av_level[j] - floor_deviation<ndts[i]->getMean()[2] && av_level[j] + floor_deviation>ndts[i]->getMean()[2]){
	// 					found = true;
	// 					floors[j].push_back(ndts[i]);
	// 					av_level[j] = getAverageLevel(floors[j]);
	// 					break;
	// 				}
	// 			}
	// 			if(!found){
	// 				av_level.push_back(ndts[i]->getMean()[2]);
	// 				std::vector<lslgeneric::NDTCell*> temp;
	// 				temp.push_back(ndts[i]);
	// 				floors.push_back(temp);
	// 			}
	//
	// 		}
	// 	}
	// 	double xs, ys, zs;
	// 	double xc, yc, zc;
	// 	double minx, miny, minz, maxx, maxy, maxz;
	// 	ndtMap->getCentroid(xc, yc, zc);
	// 	ndtMap->getGridSizeInMeters(xs, ys, zs);
	// 	minx = xc - xs / 2.0;
	// 	miny = yc - ys / 2.0;
	// 	maxx = xc + xs / 2.0;
	// 	maxy = yc + ys / 2.0;
	// 	height = abs(ceil(xs / occ_resolution));
	// 	width = abs(ceil(ys / occ_resolution));
	// 	std::vector<std::vector<std::vector<int> > >maps;
	// 	maps.resize(av_level.size(), std::vector<std::vector<int> >(width, std::vector<int>(height)));
	//
	// 	for(size_t j = 0; j < floors.size(); j++){
	// 		for(size_t k = 0; k < floors[j].size(); k++){
	// 			double ccx, ccy, ccz;
	// 			floors[j][k]->getCenter(ccx, ccy, ccz);
	// 			for(double lx = ccx - sx / 2.0 + occ_resolution / 2.0; lx <= ccx + sx / 2.0 - occ_resolution / 2.0; lx += occ_resolution){
	// 				for(double ly = ccy - sy / 2.0 + occ_resolution / 2.0; ly <= ccy + sy / 2.0 - occ_resolution / 2.0; ly += occ_resolution){
	// 					double lccz = ccz;
	// 					int X = floor((lx + sx / 2.0) / occ_resolution + 0.5) + height / 2.0;   // round((lx - (occ_resolution - xs) / 2.0) / occ_resolution);
	// 					int Y = floor((ly + sy / 2.0) / occ_resolution + 0.5) + width / 2.0;    //round((ly - (occ_resolution - ys) / 2.0) / occ_resolution);
	// 					pcl::PointXYZ p1(lx, ly, ccz - sz / 2.0);
	// 					pcl::PointXYZ p2(lx, ly, ccz + sz / 2.0);
	// 					Eigen::Vector3d out;
	// 					// ROS_INFO_STREAM(X<<" "<<Y);
	// 					// ROS_INFO_STREAM(p1<<" "<<p2<<" "<<floors[j][k]->computeMaximumLikelihoodAlongLine(p1, p2, out)<<" "<<X<<" "<<Y);
	//
	// 					if(floors[j][k]->computeMaximumLikelihoodAlongLine(p1, p2, out) > lik_tr){
	// 						bool traversable = true;
	// 						if(!floor_only){
	// 							while(lccz + 0.5 * sz - av_level[j] < robot_height){
	// 								lccz += sz;
	//
	// 								lslgeneric::NDTCell *cell;
	// 								pcl::PointXYZ refPoint(lx, ly, lccz);
	//
	// 								if(lccz + 0.5 * sz - av_level[j] < robot_height){
	// 									p1.z = lccz - sz / 2.0;
	// 									p2.z = lccz + sz / 2.0;
	// 								}
	// 								if(lccz + 0.5 * sz - av_level[j] >= robot_height){
	// 									p1.z = lccz - sz / 2.0;
	// 									p2.z = av_level[j] + robot_height;
	// 								}
	// 								//std::cout << av_level[j] << " " << p1 << " " << p2 << std::endl;
	// 								if(ndtMap->getCellAtPoint(refPoint, cell)){
	// 									if(cell->hasGaussian_ && cell->getOccupancyRescaled() > 0.9){
	//
	// 										if(cell->computeMaximumLikelihoodAlongLine(p1, p2, out) > lik_tr){
	// 											traversable = false;
	// 											//std::cout << cell->getCenter() << " break occ" << std::endl;
	// 											break;
	// 										}
	// 									}
	// 								}else{
	// 									traversable = false;
	// 									//std::cout << "break null" << std::endl;
	// 									break;
	// 								}
	//
	// 							}
	// 						}
	// 						if(traversable)
	// 							maps[j][Y][X] = 1;
	// 						//std::cout << "done" << std::endl;
	// 					}
	// 				}
	//
	// 			}
	// 		}
	// 		std::cout << "saving map" << std::endl;
	// 		std::string png_map_file = occ_map_name;
	// 		png_map_file += std::to_string(av_level[j]);
	// 		png_map_file += ".pgm";
	// 		std::ofstream map_file(png_map_file.c_str());
	// 		if(map_file.is_open()){
	// 			map_file << "P2\n";
	// 			map_file << width << " " << height << std::endl;
	// 			map_file << "1\n";
	// 			// 12.10.15 to fit the inverted map
	// 			//for(int w=0;w<width;w++){
	// 			//  for(int h=height-1;h>=0;h--){
	// 			////////////////////////////////////////////
	// 			for(int w = width - 1; w >= 0; w--){
	// 				for(int h = 0; h < height; h++)
	// 					map_file << maps[j][w][h] << " ";
	// 				map_file << std::endl;
	// 			}
	// 			map_file.close();
	// 		}else std::cout << "Unable to open file";
	// 	}
	// }

	// int InitializeOCCMap(){
	//  // get the size of the map in meters
	//  double sizeMeta;
	//
	//  std::string meta = ndt_map_directory;
	//  meta += "/metadata.txt";
	//  FILE* meta_f = fopen(meta.c_str(), "a+");
	//  if(meta_f == 0) return -1;
	//  char *line = NULL;
	//  size_t len;
	//  bool found = false;
	//  if(getline(&line, &len, meta_f) > 0 ){
	//   char *tk = strtok(line, " ");
	//   if(tk == NULL) return false;
	//   if(strncmp(tk, "VERSION", 7) == 0){
	//    tk = strtok(NULL, " ");
	//    if(tk == NULL) return false;
	//    if(strncmp(tk, "2.0", 3) == 0){
	//     if(!getline(&line, &len, meta_f) > 0 )
	//      return false;
	//     tk = strtok(line, " ");
	//     if(tk == NULL) return false;
	//     if(strncmp(tk, "SIZE", 4) != 0) return false;
	//     tk = strtok(NULL, " ");
	//     sizeMeta = atof(tk);
	//    }
	//   } else {
	//    //this is a version 1.0 file, close and re-open and go on
	//    std::cerr << "metafile version 1.0, no protection against different grid size\n";
	//    fclose(meta_f);
	//    meta_f = fopen(meta.c_str(), "a+");
	//   }
	//  }
	//  float min_x = std::numeric_limits<float>::max(),
	//        min_y = std::numeric_limits<float>::max(),
	//        max_x = std::numeric_limits<float>::min(),
	//        max_y = std::numeric_limits<float>::min();
	//  while(getline(&line, &len, meta_f) > 0 ){
	//   pcl::PointXYZ cen;
	//   char *token = strtok(line, " ");
	//   if(token == NULL) return -1;
	//   cen.x = atof(token);
	//   token = strtok(NULL, " ");
	//   if(token == NULL) return -1;
	//   cen.y = atof(token);
	//   token = strtok(NULL, " ");
	//   if(token == NULL) return -1;
	//   cen.z = atof(token);
	//   token = strtok(NULL, " ");
	//   if(token == NULL) return -1;
	//   std::string str(token);
	//   map_names.push_back(str);
	//   if(cen.x < min_x)
	//    min_x = cen.x;
	//   if(cen.y < min_y)
	//    min_y = cen.y;
	//   if(cen.x > max_x)
	//    max_x = cen.x;
	//   if(cen.y > max_y)
	//    max_y = cen.y;
	//  }
	//  min_x = min_x - sizeMeta / 2.0;
	//  min_y = min_y - sizeMeta / 2.0;
	//  max_x = max_x + sizeMeta / 2.0;
	//  max_y = max_y + sizeMeta / 2.0;
	//  xs = fabs(min_x - max_x);
	//  ys = fabs(min_y - max_y);
	//  height = abs(ceil(xs / occ_resolution));
	//  width = abs(ceil(ys / occ_resolution));
	//  for(size_t map_it = 0; map_it < map_names.size(); map_it++){
	//
	//   FILE * jffin;
	//   std::string sep = "/";
	//   std::string path = ndt_map_directory + sep + map_names[map_it];
	//   //ROS_INFO_STREAM(path);
	//   jffin = fopen(path.c_str(), "r+b");
	//   if(jffin==NULL)
	//    continue;
	//   mapGrid = new lslgeneric::LazyGrid(ndt_resolution);
	//   lslgeneric::NDTCell *ptCell = new lslgeneric::NDTCell();
	//      mapGrid->setCellType(ptCell);
	//      delete ptCell;
	//   mapGrid->loadFromJFF(jffin);
	//   ndtMap = new lslgeneric::NDTMap(mapGrid);
	//
	//   std::vector<lslgeneric::NDTCell*> ndts = ndtMap->getAllCells();
	//   double sx, sy, sz;
	//   ndtMap->getCellSizeInMeters(sx, sy, sz);
	//   for(size_t i = 0; i < ndts.size(); i++){
	//    if(ndts[i]->getClass() == lslgeneric::NDTCell::HORIZONTAL){
	//     bool found = false;
	//     for(size_t j = 0; j < floor_levels.size(); j++){
	//      if(floor_levels[j] - floor_deviation<ndts[i]->getMean()[2] && floor_levels[j] + floor_deviation>ndts[i]->getMean()[2]){
	//       found = true;
	//       floor_levels[j] = (floor_levels[j] * floor_count[j] + ndts[i]->getMean()[2]) / (floor_count[j] + 1);
	//       floor_count[j]++;
	//       break;
	//      }
	//     }
	//     if(!found){
	//      floor_levels.push_back(ndts[i]->getMean()[2]);
	//      floor_count.push_back(1);
	//     }
	//
	//    }
	//   }
	//   fclose(jffin);
	//  }
	//  maps.resize(floor_levels.size(), std::vector<std::vector<int> >(width, std::vector<int>(height)));
	// }
	//
	// void BuildMapTr(){
	//
	//  for(size_t map_it = 0; map_it < map_names.size(); map_it++){
	//
	//   FILE * jffin;
	//   std::string sep = "/";
	//   std::string path = ndt_map_directory + sep + map_names[map_it];
	//   ROS_INFO_STREAM(path);
	//   jffin = fopen(path.c_str(), "r+b");
	//
	//   if(jffin==NULL)
	//    continue;
	//   mapGrid = new lslgeneric::LazyGrid(ndt_resolution);
	//   lslgeneric::NDTCell *ptCell = new lslgeneric::NDTCell();
	//   mapGrid->setCellType(ptCell);
	//   delete ptCell;
	//   mapGrid->loadFromJFF(jffin);
	//   ndtMap = new lslgeneric::NDTMap(mapGrid);
	//   std::vector<lslgeneric::NDTCell*> ndts = ndtMap->getAllCells();
	//   ROS_INFO_STREAM( ndts .size());
	//   double sx, sy, sz;
	//   ndtMap->getCellSizeInMeters(sx, sy, sz);
	//   for(size_t i = 0; i < ndts.size(); i++){
	//    if(ndts[i]->getClass() == lslgeneric::NDTCell::HORIZONTAL){
	//     double min_dist = std::numeric_limits<double>::max();
	//     int level = -1;
	//     double z=ndts[i]->getMean()[2];
	//     for(size_t it_flor_levels = 0; it_flor_levels < floor_levels.size(); it_flor_levels++){
	//      if(fabs(floor_levels[it_flor_levels] - z) < min_dist){
	//       min_dist = fabs(floor_levels[it_flor_levels] - z);
	//       level = it_flor_levels;
	//      }
	//     }
	//     double ccx, ccy, ccz;
	//     ndts[i]->getCenter(ccx, ccy, ccz);
	//     for(double lx = ccx - sx / 2.0 + occ_resolution / 2.0; lx <= ccx + sx / 2.0 - occ_resolution / 2.0; lx += occ_resolution){
	//      for(double ly = ccy - sy / 2.0 + occ_resolution / 2.0; ly <= ccy + sy / 2.0 - occ_resolution / 2.0; ly += occ_resolution){
	//       double lccz = ccz;
	//       int X = floor((lx + sx / 2.0) / occ_resolution + 0.5) + height / 2.0;            // round((lx - (occ_resolution - xs) / 2.0) / occ_resolution);
	//       int Y = floor((ly + sy / 2.0) / occ_resolution + 0.5) + width / 2.0;             //round((ly - (occ_resolution - ys) / 2.0) / occ_resolution);
	//       pcl::PointXYZ p1(lx, ly, ccz - sz / 2.0);
	//       pcl::PointXYZ p2(lx, ly, ccz + sz / 2.0);
	//       Eigen::Vector3d out;
	//       if(ndts[i]->computeMaximumLikelihoodAlongLine(p1, p2, out) > lik_tr){
	//        bool traversable = true;
	//        if(!floor_only){
	//         while(lccz + 0.5 * sz - floor_levels[level] < robot_height){
	//          lccz += sz;
	//          lslgeneric::NDTCell *cell;
	//          pcl::PointXYZ refPoint(lx, ly, lccz);
	//          if(lccz + 0.5 * sz - floor_levels[level] < robot_height){
	//           p1.z = lccz - sz / 2.0;
	//           p2.z = lccz + sz / 2.0;
	//          }
	//          if(lccz + 0.5 * sz - floor_levels[level] >= robot_height){
	//           p1.z = lccz - sz / 2.0;
	//           p2.z = floor_levels[level] + robot_height;
	//          }
	//          if(ndtMap->getCellAtPoint(refPoint, cell)){
	//           if(cell->hasGaussian_ && cell->getOccupancyRescaled() > 0.9){
	//            if(cell->computeMaximumLikelihoodAlongLine(p1, p2, out) > lik_tr){
	//             traversable = false;
	//             break;
	//            }
	//           }
	//          }else{
	//           traversable = false;
	//           //std::cout << "break null" << std::endl;
	//           break;
	//          }
	//         }
	//        }
	//        if(traversable)
	//         maps[level][Y][X] = 1;
	//       }
	//      }
	//     }
	//    }
	//   }
	//  }
	//  for (size_t j = 0;  j<floor_levels.size() ; j++) {
	//
	//  std::cout << "saving map" << std::endl;
	//  std::string png_map_file = occ_map_name;
	//  png_map_file += std::to_string(floor_levels[j]);
	//  png_map_file += ".pgm";
	//  std::ofstream map_file(png_map_file.c_str());
	//  if(map_file.is_open()){
	//   map_file << "P2\n";
	//   map_file << width << " " << height << std::endl;
	//   map_file << "1\n";
	//   // 12.10.15 to fit the inverted map
	//   //for(int w=0;w<width;w++){
	//   //  for(int h=height-1;h>=0;h--){
	//   ////////////////////////////////////////////
	//   for(int w = width - 1; w >= 0; w--){
	//    for(int h = 0; h < height; h++)
	//     map_file << maps[j][w][h] << " ";
	//    map_file << std::endl;
	//   }
	//   map_file.close();
	//  }else std::cout << "Unable to open file";
	//  }
	// }

// void BuildMapLik(){
//
//  std::vector<lslgeneric::NDTCell*> ndts = ndtMap->getAllCells();
//  std::vector<std::vector<lslgeneric::NDTCell*> > floors;
//  std::vector<float> av_level;
//  double sx, sy, sz;
//  ndtMap->getCellSizeInMeters(sx, sy, sz);
//  for(size_t i = 0; i < ndts.size(); i++){
//   if(ndts[i]->getClass() == lslgeneric::NDTCell::HORIZONTAL){
//    bool found = false;
//    for(size_t j = 0; j < floors.size(); j++){
//     if(av_level[j] - floor_deviation<ndts[i]->getMean()[2] && av_level[j] + floor_deviation>ndts[i]->getMean()[2]){
//      found = true;
//      floors[j].push_back(ndts[i]);
//      av_level[j] = getAverageLevel(floors[j]);
//      break;
//     }
//    }
//    if(!found){
//     av_level.push_back(ndts[i]->getMean()[2]);
//     std::vector<lslgeneric::NDTCell*> temp;
//     temp.push_back(ndts[i]);
//     floors.push_back(temp);
//    }
//
//   }
//  }
//  double xs, ys, zs;
//  double xc, yc, zc;
//  double minx, miny, minz, maxx, maxy, maxz;
//  ndtMap->getCentroid(xc, yc, zc);
//  ndtMap->getGridSizeInMeters(xs, ys, zs);
//  minx = xc - xs / 2.0;
//  miny = yc - ys / 2.0;
//  maxx = xc + xs / 2.0;
//  maxy = yc + ys / 2.0;
//  height = abs(ceil(xs / occ_resolution));
//  width = abs(ceil(ys / occ_resolution));
//  std::vector<std::vector<std::vector<int> > >maps;
//  maps.resize(av_level.size(), std::vector<std::vector<int> >(width, std::vector<int>(height)));
//
//  for(size_t j = 0; j < floors.size(); j++){
//   for(size_t k = 0; k < floors[j].size(); k++){
//    double ccx, ccy, ccz;
//    floors[j][k]->getCenter(ccx, ccy, ccz);
//    for(double lx = ccx - sx / 2.0 + occ_resolution / 2.0; lx <= ccx + sx / 2.0 - occ_resolution / 2.0; lx += occ_resolution){
//     for(double ly = ccy - sy / 2.0 + occ_resolution / 2.0; ly <= ccy + sy / 2.0 - occ_resolution / 2.0; ly += occ_resolution){
//      double lccz = ccz;
//      int X = floor((lx + sx / 2.0) / occ_resolution + 0.5) + height / 2.0;        // round((lx - (occ_resolution - xs) / 2.0) / occ_resolution);
//      int Y = floor((ly + sy / 2.0) / occ_resolution + 0.5) + width / 2.0;         //round((ly - (occ_resolution - ys) / 2.0) / occ_resolution);
//      pcl::PointXYZ p1(lx, ly, ccz - sz / 2.0);
//      pcl::PointXYZ p2(lx, ly, ccz + sz / 2.0);
//      Eigen::Vector3d out;
//      // ROS_INFO_STREAM(X<<" "<<Y);
//      // ROS_INFO_STREAM(p1<<" "<<p2<<" "<<floors[j][k]->computeMaximumLikelihoodAlongLine(p1, p2, out)<<" "<<X<<" "<<Y);
//
//      if(floors[j][k]->computeMaximumLikelihoodAlongLine(p1, p2, out) > lik_tr){
//       bool traversable = true;
//       if(!floor_only){
//        while(lccz + 0.5 * sz - av_level[j] < robot_height){
//         lccz += sz;
//
//         lslgeneric::NDTCell *cell;
//         pcl::PointXYZ refPoint(lx, ly, lccz);
//
//         if(lccz + 0.5 * sz - av_level[j] < robot_height){
//          p1.z = lccz - sz / 2.0;
//          p2.z = lccz + sz / 2.0;
//         }
//         if(lccz + 0.5 * sz - av_level[j] >= robot_height){
//          p1.z = lccz - sz / 2.0;
//          p2.z = av_level[j] + robot_height;
//         }
//         //std::cout << av_level[j] << " " << p1 << " " << p2 << std::endl;
//         if(ndtMap->getCellAtPoint(refPoint, cell)){
//          if(cell->hasGaussian_ && cell->getOccupancyRescaled() > 0.9){
//
//           if(cell->computeMaximumLikelihoodAlongLine(p1, p2, out) > lik_tr){
//            traversable = false;
//            //std::cout << cell->getCenter() << " break occ" << std::endl;
//            break;
//           }
//          }
//         }else{
//          traversable = false;
//          //std::cout << "break null" << std::endl;
//          break;
//         }
//
//        }
//       }
//       if(traversable)
//        maps[j][Y][X] = 1;
//       //std::cout << "done" << std::endl;
//      }
//     }
//
//    }
//   }
//   std::cout << "saving map" << std::endl;
//   std::string png_map_file = occ_map_name;
//   png_map_file += std::to_string(av_level[j]);
//   png_map_file += ".pgm";
//   std::ofstream map_file(png_map_file.c_str());
//   if(map_file.is_open()){
//    map_file << "P2\n";
//    map_file << width << " " << height << std::endl;
//    map_file << "1\n";
//    // 12.10.15 to fit the inverted map
//    //for(int w=0;w<width;w++){
//    //  for(int h=height-1;h>=0;h--){
//    ////////////////////////////////////////////
//    for(int w = width - 1; w >= 0; w--){
//     for(int h = 0; h < height; h++)
//      map_file << maps[j][w][h] << " ";
//     map_file << std::endl;
//    }
//    map_file.close();
//   }else std::cout << "Unable to open file";
//  }
// }

// void BuildMap(){
//  double xs, ys, zs;
//  double xc, yc, zc;
//  double minx, miny, minz, maxx, maxy, maxz;
//
//  ndtMap->getCentroid(xc, yc, zc);
//  ndtMap->getGridSizeInMeters(xs, ys, zs);
//  minx = xc - xs / 2.0;
//  miny = yc - ys / 2.0;
//  maxx = xc + xs / 2.0;
//  maxy = yc + ys / 2.0;
//  height = round(xs / occ_resolution);
//  width = round(ys / occ_resolution);
//  std::cout << height << " " << width << std::endl;
//  int h = 0;
//  map.resize(width, std::vector<int>(height));
//  for(double current_x = -xs / 2 + occ_resolution; current_x < xs - occ_resolution; current_x += occ_resolution){
//   int w = 0;
//   for(double current_y = -ys / 2 + occ_resolution; current_y < ys - occ_resolution; current_y += occ_resolution){
//
//
//    lslgeneric::NDTCell *check_cell;
//    pcl::PointXYZ p;
//    p.x = current_x;
//    p.y = current_y;
//    p.z = 0.0;
//    if(ndtMap->getCellAtPoint(p, check_cell)){
//     //std::cout<<"init"<<std::endl;
//     if(check_cell->getOccupancy() < 0.0)
//      map[w][h] = 1;
//     else if(check_cell->getOccupancy() > 0.0){
//      //std::cout<<"lik "<<check_cell->getLikelihood(p)<<std::endl;
//      if(check_cell->getLikelihood(p) < lik_tr)
//       // Eigen::Vector3d m=check_cell->getMean();
//       // Eigen::Vector3d e=check_cell->getEvals();
//       // double d=sqrt((m[0]-current_x)*(m[0]-current_x)+(m[1]-current_y)*(m[1]-current_y));
//       // double de=sqrt(e[0]*e[0]+e[1]*e[1]);
//       // if(d<=de)
//       map[w][h] = 1;
//     }
//    }
//
//
//
//    w++;
//   }
//   h++;
//  }
// }
//
// void SaveMap(){
//  std::cout << "saving map" << std::endl;
//  std::string png_map_file = occ_map_name;
//  png_map_file += ".png";
//  std::ofstream map_file(png_map_file.c_str());
//  if(map_file.is_open()){
//   map_file << "P2\n";
//   map_file << width << " " << height << std::endl;
//   map_file << "1\n";
//   // 12.10.15 to fit the inverted map
//   //for(int w=0;w<width;w++){
//   //  for(int h=height-1;h>=0;h--){
//   ////////////////////////////////////////////
//   for(int w = width - 1; w >= 0; w--){
//    for(int h = 0; h < height; h++)
//     map_file << map[w][h] << " ";
//    map_file << std::endl;
//   }
//   map_file.close();
//  }else std::cout << "Unable to open file";
// }
//
// void saveYAMLFile(){
//  double xs, ys, zs;
//  double xc, yc, zc;
//  double cenx, ceny;
//
//  ndtMap->getCentroid(xc, yc, zc);
//  ndtMap->getGridSizeInMeters(xs, ys, zs);
//  cenx = xc - xs / 2.0;
//  ceny = yc - ys / 2.0;
//  std::cout << "saving yaml file" << std::endl;
//  std::string yaml_map_file = occ_map_name;
//  yaml_map_file += ".yaml";
//  std::ofstream map_file(yaml_map_file.c_str());
//  if(map_file.is_open()){
//   map_file << "image: " << occ_map_name << ".png\n";
//   map_file << "resolution: " << occ_resolution << std::endl;
//   map_file << "origin: [" << cenx << ", " << ceny << ", 0.0]\n";
//   map_file << "occupied_thresh: 0.5\n";
//   map_file << "free_thresh: 0.5\n";
//   map_file << "negate: 0\n";
//   std::cout << "YAML FILE SAVED, PLEAS EDIT PATH TO MAP!" << std::endl;
//   map_file.close();
//  }else std::cout << "Unable to open file";
// }
//};

int main(int argc, char **argv){
	ROS_INFO("Starting node");
	ros::init(argc, argv, "off_line_velodyne_test");
	ros::NodeHandle nh;
	ros::NodeHandle parameters("~");
	map_converter mc(parameters);
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program 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 (at your option) any later version.
//
// This program 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 this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_AVERAGE_HH
#define GRAPH_AVERAGE_HH

#include <algorithm>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/numeric/conversion/cast.hpp>

namespace graph_tool
{
using namespace std;
using namespace boost;

template <class Val1, class Val2>
void operator+=(std::vector<Val1>& a, const std::vector<Val2>& b)
{
    a.resize(std::max(a.size(), b.size()));
    for (size_t i = 0; i < std::min(a.size(), b.size()); ++i)
        a[i] += b[i];
}

template <class Val1, class Val2>
std::vector<Val1> operator*(const std::vector<Val1>& a, const std::vector<Val2>& b)
{
    std::vector<Val1> c(std::max(a.size(), b.size()));
    for (size_t i = 0; i < std::min(a.size(), b.size()); ++i)
        c[i] = a[i] * b[i];
    return c;
}

template <class Val1, class Val2>
std::vector<Val1> operator-(const std::vector<Val1>& a, const std::vector<Val2>& b)
{
    std::vector<Val1> c(std::max(a.size(), b.size()));
    for (size_t i = 0; i < std::min(a.size(), b.size()); ++i)
        c[i] = a[i] - b[i];
    for (size_t i = a.size(); i < std::max(a.size(), b.size()); ++i)
        c[i] = -b[i];
    return c;
}

struct get_avg_type
{
    template <class Type>
    struct apply
    {
        typedef typename mpl::if_<typename std::is_same<Type, python::object>::type,
                                  python::object, long double>::type
            type;
    };

    template <class Type>
    struct apply<vector<Type>>
    {
        typedef vector<long double> type;
    };
};

class VertexAverageTraverse
{
public:
    template <class Graph, class DegreeSelector, class ValueType>
    void operator()(Graph& g, typename graph_traits<Graph>::vertex_descriptor v,
                    DegreeSelector& deg, ValueType& a, ValueType& aa,
                    size_t& count)
    {
        const auto& x = deg(v, g);
        a += x;
        aa += x * x;
        count++;
    }
};

class EdgeAverageTraverse
{
public:
    template <class Graph, class EdgeProperty, class ValueType>
    void operator()(Graph& g, typename graph_traits<Graph>::vertex_descriptor v,
                    EdgeProperty& eprop, ValueType& a, ValueType& aa,
                    size_t& count)
    {
        for (auto e : out_edges_range(v, g))
        {
            const auto& x = eprop[e];
            a += x;
            aa += x * x;
            count++;
        }
    }
};

// explicit special initialization to get around python::object
template <class Val>
void init_avg(Val& v)
{
    v = Val(0.);
};

template <class Val>
void init_avg(std::vector<Val>&)
{
};

// generalized functor to obtain average of different types of "degrees"
template <class AverageTraverse>
struct get_average
{
    get_average(boost::python::object& a, boost::python::object& dev,
                size_t& count)
        : _a(a), _dev(dev), _count(count) {}

    template <class Graph, class DegreeSelector>
    void operator()(Graph& g, DegreeSelector deg) const
    {
        typedef typename DegreeSelector::value_type val_t;
        dispatch(g, deg, typename std::is_pod<val_t>::type());
    }

    template <class Graph, class DegreeSelector>
    void dispatch(Graph& g, DegreeSelector deg, std::true_type) const
    {
        typedef typename get_avg_type::apply<typename DegreeSelector::value_type>::type val_t;
        val_t a = 0., aa = 0.;
        size_t count = 0;

        AverageTraverse traverse;
        #pragma omp parallel if (num_vertices(g) > OPENMP_MIN_THRESH) \
            reduction(+:a,aa,count)
        parallel_vertex_loop_no_spawn
            (g,
             [&](auto v)
             {
                 traverse(g, v, deg, a, aa, count);
             });

        _a = boost::python::object(a);
        _dev = boost::python::object(aa);
        _count = count;
    }

    template <class Graph, class DegreeSelector>
    void dispatch(Graph& g, DegreeSelector deg, std::false_type) const
    {
        typedef typename get_avg_type::apply<typename DegreeSelector::value_type>::type val_t;
        val_t a, aa;
        init_avg(a);
        init_avg(aa);
        size_t count = 0;

        AverageTraverse traverse;
        for (auto v : vertices_range(g))
            traverse(g, v, deg, a, aa, count);

        _a = boost::python::object(a);
        _dev = boost::python::object(aa);
        _count = count;
    }

    boost::python::object& _a;
    boost::python::object& _dev;
    size_t& _count;
};

} // graph_tool namespace

#endif // GRAPH_AVERAGE_HH


/* Software License Agreement (BSD License)
 *
 * Copyright (c) 2014, Ross Linscott (rossklin@gmail.com)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *     Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *     Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in
 *     the documentation and/or other materials provided with the
 *     distribution.
 *
 *     The names of its contributors may not be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#if defined(NDEBUG)
#undef NDEBUG
#endif

#include <cstdlib>
#include <cmath>
#include <cstring>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#ifdef STANDALONE
#include <RInside.h>
#endif

#include "../inst/include/SimpleSDESampler.h"

NumericMatrix lpoly_sde(nlopt_stepper &stepper
			, NumericVector start
			, double from, double to, int steps
			, double x_tol
			, const char* algorithm) {

  const double dt = (to - from)/steps;
  vector<double> state = as<vector<double> >(start);
  NumericMatrix result(steps+1, start.size());

  for(int j = 0; j < start.size(); ++j){
    result(0, j) = state[j];
  }

  for(int i = 1; i <= steps; ++i) {
    stepper.do_step(state, (i-1)*dt);
    for(int j = 0; j < start.size(); ++j){
      result(i, j) = state[j];
    }
  }

  return result;
}


////////////////////////////////////////////////////////////////////////////////////////////////////
// fold.cpp
//
//  Copyright 2012 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/mpl/int.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/min_max.hpp>
#include <boost/fusion/container/list/cons.hpp>
#include <boost/proto/v5/proto.hpp>
#include <boost/fusion/include/for_each.hpp>
#include "./unit_test.hpp"

namespace mpl = boost::mpl;
namespace fusion = boost::fusion;
namespace proto = boost::proto;
using proto::_;

template<typename Int>
struct placeholder
  : Int
{};

namespace
{
    constexpr auto const & _1 = proto::utility::static_const<proto::literal<placeholder<mpl::int_<0>>>>::value;
    constexpr auto const & _2 = proto::utility::static_const<proto::literal<placeholder<mpl::int_<1>>>>::value;
    constexpr auto const & _3 = proto::utility::static_const<proto::literal<placeholder<mpl::int_<2>>>>::value;
}

BOOST_PROTO_IGNORE_UNUSED(_1, _2, _3);

// The lambda grammar, with the transforms for calculating the max arity
template<typename Fold, int I = 0>
struct lambda_arity
  : proto::def<
        proto::if_(
            proto::matches_( proto::terminal( placeholder<_> ) )
          , proto::make( mpl::next< proto::_value >() )
          , proto::if_(
                proto::matches_( proto::terminal(_) )
              , mpl::int_<0>()
              , Fold(
                    _
                  , mpl::int_<0>()
                  , proto::make( mpl::max<lambda_arity<Fold>, proto::_state>() )
                )
            )
        )
    >
{};

void test_fold()
{
    auto result0 = lambda_arity<proto::fold>()(proto::literal<int>(42) + 36);
    static_assert(decltype(result0)::value == 0, "");

    auto result1 = lambda_arity<proto::fold>()(proto::literal<int>(42) + _1);
    static_assert(decltype(result1)::value == 1, "");

    auto result2 = lambda_arity<proto::fold>()(_2 + _1);
    static_assert(decltype(result2)::value == 2, "");

    auto result3 = lambda_arity<proto::fold>()(_2 + _1 * _3);
    static_assert(decltype(result3)::value == 3, "");
}

void test_reverse_fold()
{
    auto result0 = lambda_arity<proto::reverse_fold>()(proto::literal<int>(42) + 36);
    static_assert(decltype(result0)::value == 0, "");

    auto result1 = lambda_arity<proto::reverse_fold>()(proto::literal<int>(42) + _1);
    static_assert(decltype(result1)::value == 1, "");

    auto result2 = lambda_arity<proto::reverse_fold>()(_2 + _1);
    static_assert(decltype(result2)::value == 2, "");

    auto result3 = lambda_arity<proto::reverse_fold>()(_2 + _1 * _3);
    static_assert(decltype(result3)::value == 3, "");
}

using fusion::nil;
using fusion::cons;

template<typename RecursiveFold, int I = 0>
struct to_cons_list
  : proto::def<
        RecursiveFold(
            _
          , nil()
          , proto::make( cons<proto::_value, proto::_state>(proto::_value, proto::_state) )
        )
    >
{};

void test_recursive_fold()
{
    cons<char, cons<char const(&)[6], cons<int>>> list0 =
        to_cons_list<proto::recursive_fold>()( proto::literal<int>(42) >> "hello" >> 'a' );

    BOOST_CHECK_EQUAL(list0.car, 'a');
    BOOST_CHECK_EQUAL(std::strcmp(list0.cdr.car, "hello"), 0);
    BOOST_CHECK_EQUAL(list0.cdr.cdr.car, 42);
}

void test_reverse_recursive_fold()
{
    cons<int, cons<char const(&)[6], cons<char>>> list0 =
        to_cons_list<proto::reverse_recursive_fold>()( proto::literal<int>(42) >> "hello" >> 'a' );

    BOOST_CHECK_EQUAL(list0.car, 42);
    BOOST_CHECK_EQUAL(std::strcmp(list0.cdr.car, "hello"), 0);
    BOOST_CHECK_EQUAL(list0.cdr.cdr.car, 'a');
}

using namespace boost::unit_test;
////////////////////////////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("test fold algorithms");

    test->add(BOOST_TEST_CASE(&test_fold));
    test->add(BOOST_TEST_CASE(&test_reverse_fold));
    test->add(BOOST_TEST_CASE(&test_recursive_fold));
    test->add(BOOST_TEST_CASE(&test_reverse_recursive_fold));

    return test;
}


#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>
#include <cstdint>

#include <giecs/bits.hpp>

BOOST_AUTO_TEST_SUITE(bits);

using namespace giecs;

BOOST_AUTO_TEST_CASE(length)
{
    BOOST_CHECK( sizeof(uint8_t) == sizeof(Bits<8>) );
    BOOST_CHECK( sizeof(uint16_t) == sizeof(Bits<16>) );
    BOOST_CHECK( sizeof(uint32_t) == sizeof(Bits<32>) );
}

BOOST_AUTO_TEST_CASE(overflow)
{
    Bits<14> a;
    const int max = (1 << 14) - 1;

    a = max - 1;
    BOOST_CHECK(a == max - 1);

    a = max;
    BOOST_CHECK(a == max);

    a = max + 1;
    BOOST_CHECK(a == 0);
}

BOOST_AUTO_TEST_SUITE_END();


#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

#include "src/includes.hpp"

using namespace std;
using namespace Eigen;

template <typename M>
M load_csv1(const std::string &path) {
  // Use this if csv is separated with ","
  std::ifstream indata;
  indata.open(path);
  std::string line;
  std::vector<int> values;
  uint rows = 0;
  while (std::getline(indata, line)) {
    std::stringstream lineStream(line);
    std::string cell;
    while (std::getline(lineStream, cell, ',')) {
      values.push_back(std::stod(cell));
    }
    ++rows;
  }
  return Map<const Matrix<typename M::Scalar, M::RowsAtCompileTime,
                          M::ColsAtCompileTime, RowMajor>>(
      values.data(), rows, values.size() / rows);
}

template <typename M>
M load_csv2(const std::string &path) {
  // use this if csv is searated with spaces " "
  std::ifstream indata;
  indata.open(path);
  std::string line;
  std::vector<int> values;
  uint rows = 0;
  while (std::getline(indata, line)) {
    std::stringstream lineStream(line);
    std::string cell;
    while (std::getline(lineStream, cell, ' ')) {
      values.push_back(std::stod(cell));
    }
    ++rows;
  }
  return Map<const Matrix<typename M::Scalar, M::RowsAtCompileTime,
                          M::ColsAtCompileTime, RowMajor>>(
      values.data(), rows, values.size() / rows);
}

int main(int argc, char const *argv[]) {
  cout << "Credible-balls test" << endl;

  if (argc != 6) {
    throw domain_error(
        "Syntax : ./run_cb filename_mcmc filename_pe filename_out loss rate");
  }

  string filename_mcmc =
      argv[1];  // name of the file containing the mcmc samples
  string filename_pe = argv[2];   // name of the file with the point estimate
  string filename_out = argv[3];  // name of the file to save the output
  int loss_type =
      std::stoi(argv[4]);  // 0: BinderLoss, 1: VI, 2: Normalized VI
  double learning_rate =
      stoi(argv[5]);  // a good choice for "learning_rate" is 1 to 5
  cout << "Learning rate: " << learning_rate << endl;

  Eigen::MatrixXi mcmc, pe_tmp;
  Eigen::VectorXi pe;

  mcmc = load_csv2<MatrixXi>(filename_mcmc);
  pe_tmp = load_csv1<MatrixXi>(filename_pe);

  pe = pe_tmp.row(0);
  cout << "Dimension of the point estimate: " << pe_tmp.cols() << endl;

  cout << "Matrix with dimensions : " << mcmc.rows() << "*" << mcmc.cols()
       << " found." << endl;

  CredibleBall CB =
      CredibleBall(static_cast<LOSS_FUNCTION>(loss_type), mcmc, 0.05, pe);
  CB.calculateRegion(learning_rate);
  double r = CB.getRadius();
  cout << "radius: " << r << "\n";

  cout << "Vertical Upper Bound\n";
  Eigen::VectorXi VUB = CB.VerticalUpperBound();
  cout << "VUB cardinality: " << VUB.size() << " \n";
  for (int i = 0; i < VUB.size(); i++) {
    cout << "Index of the VUB element:" << VUB(i) << " ";
  }
  cout << "\n";
  cout << "Vertical Lower Bound\n";
  Eigen::VectorXi VLB = CB.VerticalLowerBound();
  cout << "VLB cardinality: " << VLB.size() << " \n";
  for (int i = 0; i < VLB.size(); i++) {
    cout << "Index of the VLB element:" << VLB(i) << " ";
  }
  cout << "\n";
  cout << "Horizontal Bound\n";
  Eigen::VectorXi HB = CB.HorizontalBound();
  cout << "HB cardinality: " << HB.size() << " \n";
  for (int i = 0; i < HB.size(); i++) {
    cout << "Index of the HB element:" << HB(i) << " ";
  }
  cout << "\n";

  CB.sumary(HB, VUB, VLB, filename_out);

  return 0;
}


#pragma once
#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <queue>

using namespace std;
using namespace boost;

namespace subgraph {

typedef adjacency_list <vecS, vecS, undirectedS> Graph;

typedef typename  boost::graph_traits< Graph >::vertex_descriptor VertexDescriptor;

typedef typename boost::property_map< Graph, boost::vertex_index_t>::type VertexIndexMap;

typedef typename graph_traits < Graph >::adjacency_iterator Adjacency_iterator;

void random_subgraph(const int n_ver, const int n_edg, const uint32_t * Eu, const uint32_t * Ev, int subgraph_size
                     , uint8_t * selected_edges,  uint8_t * selected_vertices)

{   //C-style interface

        if (n_ver < subgraph_size)
        {
            for (uint32_t i_edg = 0; i_edg < n_edg; i_edg++)
            {
                selected_edges[i_edg] = 1;
            }
            for (uint32_t i_ver = 0; i_ver < n_ver; i_ver++)
            {
                selected_vertices[n_ver] = 1;
            }
            return;
        }

        Graph G(n_ver);

        VertexIndexMap vertex_index_map = get(boost::vertex_index, G);
        VertexDescriptor ver_current;
        Adjacency_iterator ite_ver_adj,ite_ver_adj_end;
        int node_seen = 0, seed_index;
        queue<VertexDescriptor> ver_queue;

        for (uint32_t i_edg = 0; i_edg < n_edg; i_edg++)
        {   //building graph
            add_edge(vertex(Eu[i_edg],G), vertex(Ev[i_edg],G), G);
        }

        while(node_seen < subgraph_size)
        {
            //add seed vertex
            seed_index = rand() % n_ver;
            if (selected_vertices[seed_index])
            {
                continue;
            }
            ver_queue.push(vertex(seed_index,G));
            selected_vertices[vertex_index_map(ver_queue.front())] = 1;
            node_seen = node_seen + 1;

            while(!ver_queue.empty())
            {
                //pop the top of the queue and mark it as seen
                ver_current = ver_queue.front();
                ver_queue.pop();

                //add the neighbors of that vertex
                for (tie(ite_ver_adj, ite_ver_adj_end) = adjacent_vertices(ver_current, G); ite_ver_adj != ite_ver_adj_end; ite_ver_adj++)
                {
                    int i_ver_adj = vertex_index_map(*ite_ver_adj);
                    if ((selected_vertices[i_ver_adj]==0) && (node_seen <= subgraph_size))
                    {//vertex not already seen
                        node_seen = node_seen + 1;
                        selected_vertices[i_ver_adj] = 1;
                        ver_queue.push(*ite_ver_adj);
                    }

                    if (node_seen >= subgraph_size)
                    {//enough vertices
                        break;
                    }

                }
            }
        }

        for (int i_edg = 0; i_edg < n_edg; i_edg++)
        {   //add edges between selected vertices
            selected_edges[i_edg] = selected_vertices[vertex_index_map(vertex(Eu[i_edg],G))]
                                  * selected_vertices[vertex_index_map(vertex(Ev[i_edg],G))];
        }
    return;
}
}


//==================================================================================================
/*
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [transform-unary]
#include <boost/simd/algorithm/transform.hpp>
#include <boost/simd/function/sqr.hpp>
#include <iostream>
#include <vector>

int main()
{
  std::vector<float> d{1.f, 2.f, 3.f, 4.f, 5.f, 6.f, 7.f};

  boost::simd::transform( d.data(), d.data()+7, d.data(), boost::simd::sqr );

  for(auto e : d )
    std::cout << e << " ";

  return 0;
}
//! [transform-unary]


/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2003-2017 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.apache.org/licenses/LICENSE-2.0              //
//                                                         //
/////////////////////////////////////////////////////////////

#include "Geometry/SimpleBlock.h"
#include <stdexcept>
#include <fstream>
#include <sstream>
#include <iomanip>

#include <boost/limits.hpp>

namespace esys
{
  namespace lsm
  {
    template <typename TmplParticle>
    SimpleBlockGenerator<TmplParticle>::SimpleBlockGenerator(
      unsigned int numX,
      unsigned int numY,
      unsigned int numZ,
      double radius
    )
      : m_radius(radius),
        m_dimCounts(numX, numY, numZ)
    {
    }

    template <typename TmplParticle>
    Vec3 SimpleBlockGenerator<TmplParticle>::getPos(const Vec3L &idx)
    {
      return
        Vec3(
          idx[0]*2.0*getRadius() + getRadius(),
          idx[1]*2.0*getRadius() + getRadius(),
          idx[2]*2.0*getRadius() + ((m_dimCounts[2] > 1) ? getRadius() : 0.0)
        );
    }

    template <typename TmplParticle>
    int SimpleBlockGenerator<TmplParticle>::getId(const Vec3L &idx)
    {
      return
        idx[0]
        +
        idx[1]*m_dimCounts[0]
        +
        idx[2]*m_dimCounts[0]*m_dimCounts[1];
    }

    template <typename TmplParticle>
    template <typename TmplParticleCollection>
    void
    SimpleBlockGenerator<TmplParticle>::createParticles(
      TmplParticleCollection &particleCollection
    )
    {
      Vec3L idx(0, 0, 0);
      for (idx[2]=0; idx[2] < m_dimCounts[2]; (idx[2])++)
      {
        for (idx[1]=0; idx[1] < m_dimCounts[1]; (idx[1])++)
        {
          for (idx[0]=0; idx[0] < m_dimCounts[0]; (idx[0])++)
          {
            particleCollection.createParticle(
              TmplParticle(
                getPos(idx),
                getRadius(),
                getId(idx),
                0
              )
            );
          }
        }
      }
    }

    template <typename TmplParticle>
    SimpleBlockGenerator<TmplParticle>::~SimpleBlockGenerator()
    {
    }

    template <typename TmplParticle>
    double SimpleBlockGenerator<TmplParticle>::getRadius() const
    {
      return m_radius;
    }

    template <typename TmplParticle>
    SimpleBlock<TmplParticle>::SimpleBlock(
      unsigned int numX,
      unsigned int numY,
      unsigned int numZ,
      double radius
    )
      : ParticleCollection<TmplParticle>(),
        m_generator(numX, numY, numZ, radius)
    {
      createParticles();
    }

    template <typename TmplParticle>
    SimpleBlock<TmplParticle>::~SimpleBlock()
    {
    }

    template <typename TmplParticle>
    void SimpleBlock<TmplParticle>::createParticles()
    {
      m_generator.createParticles(*this);
    }

    template <typename TmplParticle>
    double SimpleBlock<TmplParticle>::getRadius() const
    {
      return m_generator.getRadius();
    }
  }
}


// MIT License
//
// Copyright (c) 2018 Michal Siedlaczek
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

//! \file
//! \author     Michal Siedlaczek
//! \copyright  MIT License

#define CATCH_CONFIG_MAIN

#include <sstream>

#include <boost/filesystem.hpp>
#include <catch2/catch.hpp>

#include <irkit/index.hpp>
#include <irkit/index/score.hpp>
#include <irkit/index/source.hpp>
#include <irkit/io.hpp>
#include "common.hpp"

using namespace irk::test;
using irk::index::detail::StatTuple;
using Catch::Matchers::Equals;
using float_codec = irk::copy_codec<double>;

TEST_CASE("Unzip a StatTuple vector", "[scorestats]")
{
    std::vector<StatTuple> tuples = {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}};
    SECTION("max")
    {
        auto max_vec = irk::index::detail::unzip(
            tuples, [](const auto& t) { return t.max; });
        REQUIRE_THAT(max_vec, Equals(std::vector<float>{1, 4, 7}));
    }
}

TEST_CASE("Write float table", "[scorestats]")
{
    auto test_dir = tmpdir();
    auto table_path = test_dir / "float.table";
    GIVEN("A vector of floats") {
        std::vector<float> floats = {1, 4, 7};
        WHEN("Write table to file")
        {
            irk::index::detail::write_table(floats, table_path);
            THEN("Loaded table has the same elements") {
                auto loaded = irk::io::read_vector<float>(table_path);
                REQUIRE_THAT(loaded, Equals(floats));
            }
        }
    }
}

TEST_CASE("Score stats for index", "[scorestats]")
{
    GIVEN("A mock inverted index")
    {
        struct MockPosting {
            irk::index::document_t doc_;
            double score_;
            auto document() const { return doc_; }
            auto score() const { return score_; }
        };
        struct MockIndex {
            std::vector<std::vector<MockPosting>> postings{
                {{0, 12.0}},
                {{0, 12.0}, {1, 24.0}},
                {{0, 12.0}, {1, 24.0}, {2, 36.0}}};
        } index;

        WHEN("Calculated BM25 statistics")
        {
            auto dir = tmpdir();
            irk::index::detail::ScoreStatsFn fn{dir, "bm25"};
            fn(std::vector<int>{0, 1, 2},
               [&](auto term_id) { return index.postings[term_id]; });
            THEN("Stored statistics are correct")
            {
                auto max = irk::io::read_vector<float>(dir / "bm25.max");
                auto mean = irk::io::read_vector<float>(dir / "bm25.mean");
                auto var = irk::io::read_vector<float>(dir / "bm25.var");
                REQUIRE_THAT(max, Equals(std::vector<float>{12, 24, 36}));
                REQUIRE_THAT(mean, Equals(std::vector<float>{12, 18, 24}));
                REQUIRE_THAT(var, Equals(std::vector<float>{0, 36, 96}));
            }
        }
    }
}


#include <boost/iterator/permutation_iterator.hpp>
#include <iostream>
#include <vector>
#include <list>

int main(int argc, char *argv[]) {
    int index[] = {9, 1, 2, 3, 4, 5, 6, 7, 8, 0};
    int vector[] = {45, 34, 33, 12, 4, 54, 6, 57, 68, 79};
    int n = sizeof(vector)/sizeof(int);

    // This iterator provides a different view of a given range.
    boost::permutation_iterator<int*, int*> begin = boost::make_permutation_iterator(vector, index);
    boost::permutation_iterator<int*, int*> end = boost::make_permutation_iterator(vector + n, index + n);

    std::copy(begin, end, std::ostream_iterator<int>(std::cout, " "));

    return EXIT_SUCCESS;
}


/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef DART_MATH_RANDOM_HPP_
#define DART_MATH_RANDOM_HPP_

#include <random>

#include <Eigen/Core>

#ifdef DART_USE_ARBITRARY_PRECISION
#include "mpreal.h"
#endif

namespace dart {
namespace math {

class Random final
{
public:
  using GeneratorType = std::mt19937;

  template <typename FloatType>
  using UniformRealDist = std::uniform_real_distribution<FloatType>;

  template <typename IntType>
  using UniformIntDist = std::uniform_int_distribution<IntType>;

  template <typename FloatType>
  using NormalRealDist = std::normal_distribution<FloatType>;

  /// Returns a mutable reference to the random generator
  static GeneratorType& getGenerator();

  /// Sets the seed value.
  ///
  /// The same seed gives the same sequence of random values so that you can
  /// regenerate the same sequencial random values as long as you knot the seed
  /// value.
  static void setSeed(unsigned int seed);

  /// Generates a seed value using the default random device.
  ///
  /// \param[in] applyGeneratedSeed Whether to apply the generated seed.
  /// \return The new seed value.
  static unsigned int generateSeed(bool applyGeneratedSeed = false);

  /// \return The current seed value.
  static unsigned int getSeed();

  /// Returns a random number from an uniform distribution.
  ///
  ///
  /// This template function can generate different scalar types of random
  /// numbers as:
  /// - Floating-point number: \c float, \c s_t, \c long s_t
  /// - Integer number: [\c unsigned] \c short, [\c unsigned] \c int,
  ///   [\c unsigned] \c long, [\c unsigned] \c long \c long
  ///
  /// and vectors and matrices as:
  /// - Fixed-size: Eigen::Vector3i, Eigen::Vector3s, Eigen::Matrix4d, and so
  ///   on.
  /// - Dynamic-size: Eigen::VectorXi, Eigen::VectorXs, Eigen::MatrixXs, and so
  ///   on.
  ///
  /// Example:
  /// \code
  /// // Generate a random int in [0, 10]
  /// int intVal1 = Random::uniform(0, 10);
  /// int intVal2 = Random::uniform<int>(0, 10);
  ///
  /// // Generate a random s_t in [0.0, 10.0)
  /// s_t dblVal1 = Random::uniform(0.0, 10.0);
  /// s_t dblVal2 = Random::uniform<s_t>(0, 10);
  ///
  /// // Generate a random vector in [lb, ub)
  /// Eigen::Vector3s lb = Eigen::Vector3s::Constant(1);
  /// Eigen::Vector3s ub = Eigen::Vector3s::Constant(4);
  /// Eigen::Vector3s vecVal1 = Random::uniform(lb, ub);
  /// Eigen::Vector3s vecVal2 = Random::uniform<Eigen::Vector3s>(lb, ub);
  ///
  /// // Generate a random matrix in [lb, ub)
  /// Eigen::Matrix4f lb = Eigen::Matrix4f::Constant(1);
  /// Eigen::Matrix4f ub = Eigen::Matrix4f::Constant(4);
  /// Eigen::Matrix4f vecVal1 = Random::uniform(lb, ub);
  /// Eigen::Matrix4f vecVal2 = Random::uniform<Eigen::Matrix4f>(lb, ub);
  /// \endcode
  ///
  /// Note that the end of the range is closed for integer types (i.e.,
  /// [int_min, int_max]), but open for floating-point types (i.e., [float_min,
  /// float_max)).
  ///
  /// \tparam S The type of random value.
  /// \param[in] min Lower bound of the distribution.
  /// \param[in] max Upper bound of the distribution.
  ///
  /// \sa normal()
  template <typename S>
  static S uniform(S min, S max);

  /// Returns a random vector or matrix from an uniform distribution.
  ///
  /// This is a helper function for the case that the each of lower and upper
  /// bound has an uniform element value in it. For example, the lower bound is
  /// [1, 1, 1] or [-2, -2].
  ///
  /// This variant is meant to be used for fixed-size vector or matrix types.
  /// For dynamic-size types, please use other variants that takes the size of
  /// vector or matrix.
  ///
  /// Example:
  /// \code
  /// // Generate random vectors
  /// Eigen::VectorXi vecXi = Random::uniform<Eigen::VectorXi>(0, 10);
  /// Eigen::VectorXs vecXd = Random::uniform<Eigen::VectorXs>(0.0, 10.0);
  /// \endcode
  ///
  /// \tparam FixedSizeT The type of fixed-size vector or fixed-size matrix.
  /// \param[in] min The constant value of the lower bound.
  /// \param[in] max The constant value of the upper bound.
  ///
  /// \sa uniform()
  template <typename FixedSizeT>
  static FixedSizeT uniform(
      typename FixedSizeT::Scalar min, typename FixedSizeT::Scalar max);

  /// Returns a random vector from an uniform distribution.
  ///
  /// This variant is meant to be used for dynamic-size vector.
  ///
  /// Example:
  /// \code
  /// // Generate random matrices
  /// Eigen::MatrixXi matXi = Random::uniform<Eigen::MatrixXi>(0, 10);
  /// Eigen::MatrixXs matXd = Random::uniform<Eigen::MatrixXs>(0.0, 10.0);
  /// \endcode
  ///
  /// \tparam DynamicSizeVectorT The type of dynamic-size vector.
  /// \param[in] size The size of the vectors.
  /// \param[in] min The constant value of the lower bound vector.
  /// \param[in] max The constant value of the upper bound vector.
  template <typename DynamicSizeVectorT>
  static DynamicSizeVectorT uniform(
      int size,
      typename DynamicSizeVectorT::Scalar min,
      typename DynamicSizeVectorT::Scalar max);

  /// Returns a random matrix from an uniform distribution.
  ///
  /// This variant is meant to be used for dynamic-size matrix.
  ///
  /// \tparam DynamicSizeMatrixT The type of dynamic-size matrix.
  /// \param[in] rows The row size of the matrices.
  /// \param[in] cols The col size of the matrices.
  /// \param[in] min The constant value of the lower bound matrix.
  /// \param[in] max The constant value of the upper bound matrix.
  ///
  /// \sa uniform()
  template <typename DynamicSizeMatrixT>
  static DynamicSizeMatrixT uniform(
      int rows,
      int cols,
      typename DynamicSizeMatrixT::Scalar min,
      typename DynamicSizeMatrixT::Scalar max);

  /// Returns a random number from a normal distribution.
  ///
  /// This template function can generate different scalar types of random
  /// numbers as:
  /// - Floating-point number: \c float, \c s_t, \c long s_t
  /// - Integer number: [\c unsigned] \c short, [\c unsigned] \c int,
  ///   [\c unsigned] \c long, [\c unsigned] \c long \c long
  ///
  /// Example:
  /// \code
  /// // Generate a random int
  /// int intVal = Random::normal(0, 10);
  ///
  /// // Generate a random s_t
  /// s_t dblVal = Random::normal(0.0, 10.0);
  /// \endcode
  ///
  /// \param[in] mean Mean of the normal distribution.
  /// \param[in] sigma Standard deviation of the distribution.
  ///
  /// \sa uniform()
  template <typename S>
  static S normal(S mean, S sigma);

private:
  /// \return A mutable reference to the seed.
  static unsigned int& getSeedMutable();
};

} // namespace math
} // namespace dart

#include "dart/math/detail/Random-impl.hpp"

#endif // DART_MATH_RANDOM_HPP_


// Copyright (C) 2011 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "test_entity.hpp"
#include "grid/armadillo_helpers.hpp"
#include "grid/entity_iterator.hpp"
#include "grid/geometry.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include "../num_template.hpp"

using namespace Bempp;

const double EPSILON = 1e-14;

BOOST_FIXTURE_TEST_SUITE(GridFactory_Triangular, TriangularEntityManager)

BOOST_AUTO_TEST_CASE(number_of_faces_is_correct)
{
    size_t expectedValue = 2*N_ELEMENTS_X*N_ELEMENTS_Y;
    BOOST_CHECK_EQUAL(bemppGrid->levelView(0)->entityCount(0), expectedValue);
}

BOOST_AUTO_TEST_CASE(number_of_vertices_is_correct)
{
    size_t expectedValue = (N_ELEMENTS_X + 1) * (N_ELEMENTS_Y + 1);
    BOOST_CHECK_EQUAL(bemppGrid->levelView(0)->entityCount(2), expectedValue);
}

BOOST_AUTO_TEST_CASE(second_face_is_a_triangle)
{
    const int codim = 0;
    std::auto_ptr<EntityPointer<codim> > ep = getPointerToSecondEntityOnLevel0<codim>();
    BOOST_CHECK(ep->entity().type().isTriangle());
}

BOOST_AUTO_TEST_CASE(elements_are_in_the_z_plane)
{
    std::auto_ptr<Bempp::GridView> bemppGridView = bemppGrid->levelView(0);
    std::auto_ptr<Bempp::EntityIterator<2> > it = bemppGridView->entityIterator<2>();

    typedef double ctype;
    ctype max_abs_z = 0.;
    arma::Col<ctype> center;
    while(!it->finished()) {
        it->entity().geometry().getCenter(center);
        max_abs_z = std::max(max_abs_z, fabs(center(2)));
        it->next();
    }

    BOOST_CHECK_SMALL(max_abs_z, EPSILON);
}

BOOST_AUTO_TEST_CASE(elements_are_cover_the_unit_square)
{
    std::auto_ptr<Bempp::GridView> bemppGridView = bemppGrid->levelView(0);
    std::auto_ptr<Bempp::EntityIterator<2> > it = bemppGridView->entityIterator<2>();

    typedef double ctype;
    ctype min_x =  1e100;
    ctype max_x = -1e100;
    ctype min_y =  1e100;
    ctype max_y = -1e100;

    arma::Col<ctype> center;
    while(!it->finished()) {
        it->entity().geometry().getCenter(center);
        max_x = std::max(max_x, center(0));
        max_y = std::max(max_y, center(1));
        min_x = std::min(min_x, center(0));
        min_y = std::min(min_y, center(1));
        it->next();
    }

    BOOST_CHECK_CLOSE(min_x, 0., EPSILON);
    BOOST_CHECK_CLOSE(max_x, 1., EPSILON);
    BOOST_CHECK_CLOSE(min_y, 0., EPSILON);
    BOOST_CHECK_CLOSE(max_y, 1., EPSILON);
}

BOOST_AUTO_TEST_CASE(jacobian_is_constant_everywhere_on_the_second_face)
{
    const int codim = 0;
    //const int dimGlobal = DuneGrid::dimensionworld;
    const int dimLocal = DuneGrid::dimension - codim;
    const int nPoints = 5;

    std::auto_ptr<EntityPointer<codim> > ep = getPointerToSecondEntityOnLevel0<codim>();
    const Geometry& geo = ep->entity().geometry();

    typedef double ctype;
    arma::Mat<ctype> local(dimLocal, nPoints);
    Dune::FieldVector<ctype, dimLocal> duneLocal;
    for (int j = 0; j < nPoints; ++j)
        for (int i = 0; i < dimLocal; ++i)
            local(i,j) = 0.1 * (i + 1) + 0.01 * (j + 1);

    arma::Row<ctype> intElement;
    geo.getIntegrationElements(local, intElement);

    BOOST_CHECK_SMALL(stddev(intElement), EPSILON);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <math.h>
#include "dmatrix.h"
#include "perf.h"
#include <boost/array.hpp>

using namespace std;
dmatrix::dmatrix()
{
  nrr=0;
  ncc=0;
  length=0;
  ary.reserve(0);
}
dmatrix::dmatrix(int nr, int nc)
{
  nrr=nr;
  ncc=nc;
  length=nr*nc;
  if(length<1)
  {
      length=1;
      nrr=ncc=0;
  }
  ary.resize(length);
  this->zero();
}

dmatrix::dmatrix(const dmatrix& other)
  {
  nrr=other.nrr;
  ncc=other.ncc;
  length=other.length;
  ary=other.ary;
  }

dmatrix::~dmatrix()
{
//if(ary!=NULL) delete [] ary;
}

double &dmatrix::operator()(int rowindex, int colindex)
{
  int out_of_range=0;
  if (rowindex>=nrr) out_of_range=1;
  if (rowindex<0) out_of_range=1;
  if (colindex>=ncc) out_of_range=1;
  if (colindex<0) out_of_range=1;
  if (out_of_range)
	throw dmatrix_index_error(nrr,ncc,rowindex,colindex);
  return (ary[rowindex+(nrr)*(colindex)]);
}
//
// subtle difference here.  This one returns a pointer to the
// requested element
//
double* dmatrix::get_address(int rowindex, int colindex)
{
  double *ptr;
  int out_of_range=0;
  if (rowindex>=nrr) out_of_range=1;
  if (rowindex<0) out_of_range=1;
  if (colindex>=ncc) out_of_range=1;
  if (colindex<0) out_of_range=1;
  if (out_of_range)
        throw dmatrix_index_error(nrr,ncc,rowindex,colindex);
  ptr=&(ary[rowindex+(nrr)*(colindex)]);
  return(ptr);
}

dmatrix& dmatrix::operator=(const dmatrix& other)
{
    if(&other!=this)
    {
	ncc=other.ncc;
	nrr=other.nrr;
	length=other.length;
        ary=other.ary;
    }
    return *this;
}

void dmatrix::operator+=(const dmatrix& other)
 {
int i;
  if ((nrr!=other.nrr)||(length!=other.length))
	throw dmatrix_size_error(nrr, ncc, other.nrr, other.length);
for(i=0;i<length;i++)
  ary[i]+=other.ary[i];
 }

void dmatrix::operator-=(const dmatrix& other)
 {
int i;
  if ((nrr!=other.nrr)||(length!=other.length))
	throw dmatrix_size_error(nrr, ncc, other.nrr, other.length);
for(i=0;i<length;i++)
  ary[i]-=other.ary[i];
 }

dmatrix operator+(const dmatrix &x1, const dmatrix &x2)
  {
int i;
  if ((x1.nrr!=x2.nrr)||(x1.length!=x2.length))
	throw dmatrix_size_error(x1.nrr, x1.ncc, x2.nrr, x2.length);
 dmatrix tempmat(x1.nrr,x1.ncc);
  for(i=0;i<x1.length;i++) tempmat.ary[i]=x1.ary[i]+x2.ary[i];
return tempmat;
}

dmatrix operator-(const dmatrix &x1, const dmatrix &x2)
  {
int i;
  if ((x1.nrr!=x2.nrr)||(x1.length!=x2.length))
	throw dmatrix_size_error(x1.nrr, x1.ncc, x2.nrr, x2.length);
  dmatrix tempmat(x1.nrr,x1.ncc);
  for(i=0;i<x1.length;i++) tempmat.ary[i]=x1.ary[i]-x2.ary[i];
return tempmat;
}

dmatrix operator*(const dmatrix& x1,const dmatrix& b)
{
	int i,j,k;
	double xval,bval;
        /* The computed length in last arg to the error object is a relic*/
	if(const_cast<dmatrix&>(x1).columns()!=const_cast<dmatrix&>(b).rows())
		throw dmatrix_size_error(const_cast<dmatrix&>(x1).rows(), const_cast<dmatrix&>(x1).columns(),
                        const_cast<dmatrix&>(b).rows(), const_cast<dmatrix&>(b).rows()*const_cast<dmatrix&>(b).columns());
	dmatrix prod(const_cast<dmatrix&>(x1).rows(),const_cast<dmatrix&>(b).columns());
	for(i=0;i<const_cast<dmatrix&>(x1).rows();i++)
	  for(j=0;j<const_cast<dmatrix&>(b).columns();j++)
	  {
              double *x1ptr,*bptr;
              x1ptr=const_cast<dmatrix&>(x1).get_address(i,0);
              bptr=const_cast<dmatrix&>(b).get_address(0,j);
              prod(i,j)=ddot(const_cast<dmatrix&>(x1).columns(),x1ptr,const_cast<dmatrix&>(x1).rows(),bptr,1);
	  }
	return prod;
}

dmatrix operator*(const double& x, const dmatrix &zx)
  {
int i;
  dmatrix tempmat(zx.nrr,zx.ncc);
  for(i=0;i<zx.length;i++) tempmat.ary[i]=x*zx.ary[i];
return tempmat;
  }

dmatrix operator/(const dmatrix &zx, const double& x)
  {
int i;
  dmatrix tempmat(zx.nrr,zx.ncc);
  for(i=0;i<zx.length;i++) tempmat.ary[i]=zx.ary[i]/x;
return tempmat;
  }

dmatrix tr(const dmatrix& x1)
{
int i,j;
dmatrix temp(x1.ncc,x1.nrr);
for(i=0; i<x1.nrr; i++)
   for(j=0; j<x1.ncc;j++)
   {
	temp(j,i)=const_cast<dmatrix&>(x1)(i,j);
   }
return temp;
}

ostream& operator<<(ostream& os, dmatrix& x1)
  {
  int i,j;
  for(i=0;i<x1.nrr;i++)
  {
  for(j=0;j<x1.ncc;j++) os << " "<< x1(i,j);
  os<<"\n";
  }
  return os;
  }

istream& operator>>(istream& is, dmatrix& x1)
  {
  int i,j;
  for(i=0;i<x1.nrr;i++)
  {
  for(j=0;j<x1.ncc;j++) is >> x1(i,j);
  }
  return is;
  }

void dmatrix::zero()
{
    for(int i=0;i<length;++i) ary[i]=0.0;
}
int *dmatrix::size()
{
	int *sz;
	sz=new int[2];
	sz[0]=nrr;
	sz[1]=ncc;
	return(sz);
}
// simpler versions of same

int dmatrix::rows()
{
	return(nrr);
}
int dmatrix::columns()
{
	return(ncc);
}

// vector methods
dvector& dvector::operator=(const dvector& other)
{
	if(this != &other)
	{
		ncc=1;
		nrr=other.nrr;
		length=other.length;
                ary=other.ary;
	}
	return *this;
}
dvector::dvector(const dvector& other)
{
	ncc=1;
	nrr=other.nrr;
	length=other.length;
        ary=other.ary;
}
double &dvector::operator()(int rowindex)
{
  if (rowindex>=nrr)
	throw dmatrix_index_error(nrr,1,rowindex,1);
  return (ary[rowindex]);
}
dvector operator*(const dmatrix& x1,const dvector& b)
{
	int i,j,k;
	double xval,bval;
        int ncx1=const_cast<dmatrix&>(x1).columns();
        int nrx1=const_cast<dmatrix&>(x1).rows();
        int nrb=const_cast<dvector&>(b).rows();
	if(ncx1!=nrb)
		throw dmatrix_size_error(nrx1, ncx1, nrb, 1);
	dvector prod(nrx1);
	for(i=0;i<nrx1;i++)
		prod(i)=ddot(nrb,
			const_cast<dmatrix&>(x1).get_address(i,0),ncx1,
			const_cast<dvector&>(b).get_address(0,0),1);
	return prod;
}


/*
    Copyright 2013 Adobe
    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*/
/**************************************************************************************************/

#ifndef ADOBE_ITERATOR_DISTANCE_HPP
#define ADOBE_ITERATOR_DISTANCE_HPP

#include <adobe/config.hpp>

#include <iterator>

#include <boost/range/difference_type.hpp>

/**************************************************************************************************/

namespace adobe {

/**************************************************************************************************/

/*!
\ingroup adobe_iterator
*/

template <typename I> // I models InputRange
inline boost::difference_type<I>::type distance(I& range) {
    return std::distance(boost::begin(range), boost::end(range));
}

/**************************************************************************************************/

} // namespace adobe

/**************************************************************************************************/

#endif
// ADOBE_ITERATOR_DISTANCE_HPP


/* =========================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------
*/ /*!

   \file    demo11t.cpp
   \brief   Example 11t: Threaded wrapper-based feature selection with Floating Search
   \example demo11t.cpp
            Implements \ref example11t, see also
            Example 11t source code
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see
            Contacts
   \date    March 2011
   \version 3.1.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44)
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz)
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <exception>
#include <iostream>
#include <cstdlib>
#include <string>
#include <vector>
#include "error.hpp"
#include "global.hpp"
#include "subset.hpp"

#include "data_intervaller.hpp"
#include "data_splitter.hpp"
#include "data_splitter_5050.hpp"
#include "data_splitter_cv.hpp"
//#include "data_splitter_holdout.hpp"
//#include "data_splitter_leave1out.hpp"
//#include "data_splitter_resub.hpp"
//#include "data_splitter_randrand.hpp"
//#include "data_splitter_randfix.hpp"
#include "data_scaler.hpp"
#include "data_scaler_void.hpp"
//#include "data_scaler_to01.hpp"
//#include "data_scaler_white.hpp"
#include "data_accessor_splitting_memTRN.hpp"
#include "data_accessor_splitting_memARFF.hpp"

//#include "criterion_normal_bhattacharyya.hpp"
//#include "criterion_normal_gmahalanobis.hpp"
//#include "criterion_normal_divergence.hpp"
//#include "criterion_multinom_bhattacharyya.hpp"
#include "criterion_wrapper.hpp"
//#include "criterion_wrapper_bias_estimate.hpp"
//#include "criterion_subsetsize.hpp"
//#include "criterion_sumofweights.hpp"
//#include "criterion_negative.hpp"

#include "distance_euclid.hpp"
//#include "distance_L1.hpp"
//#include "distance_Lp.hpp"
#include "classifier_knn.hpp"
//#include "classifier_normal_bayes.hpp"
//#include "classifier_multinom_naivebayes.hpp"
//#include "classifier_svm.hpp"

//#include "search_bif.hpp"
//#include "search_bif_threaded.hpp"
//#include "search_monte_carlo.hpp"
//#include "search_monte_carlo_threaded.hpp"
//#include "search_exhaustive.hpp"
//#include "search_exhaustive_threaded.hpp"
//#include "branch_and_bound_predictor_averaging.hpp"
//#include "search_branch_and_bound_basic.hpp"
//#include "search_branch_and_bound_improved.hpp"
//#include "search_branch_and_bound_partial_prediction.hpp"
//#include "search_branch_and_bound_fast.hpp"
//#include "seq_step_straight.hpp"
#include "seq_step_straight_threaded.hpp"
//#include "seq_step_hybrid.hpp"
//#include "seq_step_ensemble.hpp"
//#include "search_seq_sfs.hpp"
#include "search_seq_sffs.hpp"
//#include "search_seq_sfrs.hpp"
//#include "search_seq_os.hpp"
//#include "search_seq_dos.hpp"
//#include "result_tracker_dupless.hpp"
//#include "result_tracker_regularizer.hpp"
//#include "result_tracker_feature_stats.hpp"
//#include "result_tracker_stabileval.hpp"

/*! \section example11t Example 11t: Threaded wrapper-based feature selection with Floating Search

As alternative to standard sequential evaluation of feature subset candidates
FST3 enables threaded candidate subset evaluation. All FST3 sequential search methods
can be easily parallelized by using \c Sequential_Step_Straight_Threaded
instead of \c Sequential_Step_Straight evaluator object. The actual
search speed gain depends on particular problem setting.
In small-sample, low-dimensional settings, or when criterion evaluation
is very fast, the actual gain can remain negligible or even negative due
to thread management overhead (note that this also considerably depends on compiler).
This example is the threaded version of \ref example11 with the 15-dimensional (i.e., low-dimensional)
small-sample \e speech dataset. It illustrates well the threading performance issue.
When compiled under cygwin with gcc 4.3.x and Boost 1.33.1 it operates slower
than the equivalent sequential code in \ref example11. In contrary, when compiled
under Visual C++ 2010 with Boost 1.44, it operates faster.

\note Threading is generally capable of substantially
accelerating the search process when applied in computationally more demanding scenarios;
see \ref example12t or \ref example25t or \ref example33t or \ref example40t, etc., for such cases.

\note The maximum permitted number of threads to run at once is to be
user-specified with respect to hardware capabilities.
*/
int main()
{
	try{
	const unsigned int max_threads=2;
	typedef double RETURNTYPE; 	typedef double DATATYPE;  typedef double REALTYPE;
	typedef unsigned int IDXTYPE;  typedef unsigned int DIMTYPE;  typedef short BINTYPE;
	typedef FST::Subset<BINTYPE, DIMTYPE> SUBSET;
	typedef FST::Data_Intervaller<std::vector<FST::Data_Interval<IDXTYPE> >,IDXTYPE> INTERVALLER;
	typedef boost::shared_ptr<FST::Data_Splitter<INTERVALLER,IDXTYPE> > PSPLITTER;
	typedef FST::Data_Splitter_CV<INTERVALLER,IDXTYPE> SPLITTERCV;
	typedef FST::Data_Splitter_5050<INTERVALLER,IDXTYPE> SPLITTER5050;
	typedef FST::Data_Accessor_Splitting_MemTRN<DATATYPE,IDXTYPE,INTERVALLER> DATAACCESSOR;
	typedef FST::Distance_Euclid<DATATYPE,DIMTYPE,SUBSET> DISTANCE;
	typedef FST::Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE> CLASSIFIERKNN;
	typedef FST::Criterion_Wrapper<RETURNTYPE,SUBSET,CLASSIFIERKNN,DATAACCESSOR> WRAPPERKNN;
	typedef FST::Sequential_Step_Straight_Threaded<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,max_threads> EVALUATOR;

		std::cout << std::endl << "Starting Example 11t: Threaded wrapper-based feature selection with Floating Search..." << std::endl;
	// keep second half of data for independent testing of final classification performance
		PSPLITTER dsp_outer(new SPLITTER5050());
	// in the course of search use the first half of data by 3-fold cross-validation in wrapper FS criterion evaluation
		PSPLITTER dsp_inner(new SPLITTERCV(3));
	// do not scale data
		boost::shared_ptr<FST::Data_Scaler<DATATYPE> > dsc(new FST::Data_Scaler_void<DATATYPE>());
	// set-up data access
		boost::shared_ptr<std::vector<PSPLITTER> > splitters(new std::vector<PSPLITTER>);
		splitters->push_back(dsp_outer); splitters->push_back(dsp_inner);
		boost::shared_ptr<DATAACCESSOR> da(new DATAACCESSOR("data/speech_15.trn",splitters,dsc));
		da->initialize();
	// initiate access to split data parts
		da->setSplittingDepth(0); if(!da->getFirstSplit()) throw FST::fst_error("50/50 data split failed.");
		da->setSplittingDepth(1); if(!da->getFirstSplit()) throw FST::fst_error("3-fold cross-validation failure.");
	// initiate the storage for subset to-be-selected
		boost::shared_ptr<SUBSET> sub(new SUBSET(da->getNoOfFeatures()));  sub->deselect_all();
	// set-up 3-Nearest Neighbor classifier based on Euclidean distances
		boost::shared_ptr<CLASSIFIERKNN> cknn(new CLASSIFIERKNN); cknn->set_k(3);
	// wrap the 3-NN classifier to enable its usage as FS criterion (criterion value will be estimated by 3-fold cross-val.)
		boost::shared_ptr<WRAPPERKNN> wknn(new WRAPPERKNN);
		wknn->initialize(cknn,da);
	// set-up the standard sequential search step object (option: hybrid, ensemble, etc.)
		boost::shared_ptr<EVALUATOR> eval(new EVALUATOR);
	// set-up Sequential Forward Floating Selection search procedure
		FST::Search_SFFS<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,EVALUATOR> srch(eval);
		srch.set_search_direction(FST::FORWARD);
	// run the search
		std::cout << "Feature selection setup:" << std::endl << *da << std::endl << srch << std::endl << *wknn << std::endl << std::endl;
		RETURNTYPE critval_train, critval_test;
		if(!srch.search(0,critval_train,sub,wknn,std::cout)) throw FST::fst_error("Search not finished.");
	// (optionally) the following line is included here just for illustration because srch.search() reports results in itself
		std::cout << std::endl << "Search result: " << std::endl << *sub << std::endl << "Criterion value=" << critval_train << std::endl << std::endl;
	// (optionally) validate result by estimating kNN accuracy on selected feature sub-space on independent test data
		da->setSplittingDepth(0);
		cknn->train(da,sub);
		cknn->test(critval_test,da);
		std::cout << "Validated "<<cknn->get_k()<<"-NN accuracy=" << critval_test << std::endl << std::endl;
	// (optionally) list the best known solutions for each cardinality as recorded throughout the course of search
		std::cout << "Best recorded solution for subset size:" << std::endl;
		for(DIMTYPE d=1;d<=sub->get_n();d++)
		if(srch.get_result(d,critval_train,sub)) std::cout << d << ": val="<< critval_train << ", "<<*sub << std::endl;
	}
	catch(FST::fst_error &e) {std::cerr<<"FST ERROR: "<< e.what() << ", code=" << e.code() << std::endl;}
	catch(std::exception &e) {std::cerr<<"non-FST ERROR: "<< e.what() << std::endl;}
	return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/arithmetic/include/functions/two_add.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/zero.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/vector_tie.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL( two_add, NT2_REAL_TYPES)
{
  using nt2::two_add;
  using nt2::tag::two_add_;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<two_add_(T,T)>::type)
                  , (std::pair<T,T>)
                  );

  T inf_    = nt2::Inf<T>();
  T zero_   = nt2::Zero<T>();
  T one_    = nt2::One<T>();
  T half_   = nt2::Half<T>();
  T eps_    = nt2::Eps<T>();
  T eps_2_  = eps_/T(2);

  {
    T s,r;

    two_add(inf_,zero_, s, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    two_add(zero_, inf_, s, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    two_add(half_+ eps_2_, half_, s, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);

    two_add(half_, half_+ eps_2_, s, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);
  }

  {
    T s,r;

    s = two_add(inf_,zero_, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    s = two_add(zero_, inf_, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    s = two_add(half_+ eps_2_, half_, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);

    s = two_add(half_, half_+ eps_2_, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);
  }

  {
    T s,r;

    boost::fusion::vector_tie(s,r) = two_add(inf_,zero_);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    boost::fusion::vector_tie(s,r) = two_add(zero_, inf_);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    boost::fusion::vector_tie(s,r) = two_add(half_+ eps_2_, half_);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);

    boost::fusion::vector_tie(s,r) = two_add(half_, half_+ eps_2_);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);
  }

  {
    std::pair<T,T> p;

    p = two_add(inf_,zero_);
    NT2_TEST_EQUAL(p.first, inf_);
    NT2_TEST_EQUAL(p.second, zero_);

    p = two_add(zero_, inf_);
    NT2_TEST_EQUAL(p.first, inf_);
    NT2_TEST_EQUAL(p.second, zero_);

    p = two_add(half_+ eps_2_, half_);
    NT2_TEST_EQUAL(p.first, one_);
    NT2_TEST_EQUAL(p.second, eps_2_);

    p = two_add(half_, half_+ eps_2_);
    NT2_TEST_EQUAL(p.first, one_);
    NT2_TEST_EQUAL(p.second, eps_2_);
  }
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <typeinfo>
#include <boost/core/demangle.hpp>
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

template <typename Scalar>
IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
    boost::shared_ptr<DifferentialActionModelAbstract> model, const Scalar time_step, const bool with_cost_residual)
    : Base(model, time_step, with_cost_residual) {}

template <typename Scalar>
IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
    boost::shared_ptr<DifferentialActionModelAbstract> model,
    boost::shared_ptr<ControlParametrizationModelAbstract> control, const Scalar time_step,
    const bool with_cost_residual)
    : Base(model, control, time_step, with_cost_residual) {}

template <typename Scalar>
IntegratedActionModelEulerTpl<Scalar>::~IntegratedActionModelEulerTpl() {}

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::calc(const boost::shared_ptr<ActionDataAbstract>& data,
                                                 const Eigen::Ref<const VectorXs>& x,
                                                 const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
  }
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
  }

  const std::size_t nv = differential_->get_state()->get_nv();

  // Static casting the data
  const boost::shared_ptr<Data>& d = boost::static_pointer_cast<Data>(data);

  // Computing the acceleration and cost
  control_->calc(d->control, 0., u);
  differential_->calc(d->differential, x, d->control->w);

  // Computing the next state (discrete time)
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
      x.tail(differential_->get_state()->get_nv());
  const VectorXs& a = d->differential->xout;
  if (enable_integration_) {
    d->dx.head(nv).noalias() = v * time_step_ + a * time_step2_;
    d->dx.tail(nv).noalias() = a * time_step_;
    differential_->get_state()->integrate(x, d->dx, d->xnext);
    d->cost = time_step_ * d->differential->cost;
  } else {
    d->dx.setZero();
    d->xnext = x;
    d->cost = d->differential->cost;
  }

  // Updating the cost value
  if (with_cost_residual_) {
    d->r = d->differential->r;
  }
}

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
                                                     const Eigen::Ref<const VectorXs>& x,
                                                     const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
  }
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
  }

  const std::size_t nv = differential_->get_state()->get_nv();

  // Static casting the data
  const boost::shared_ptr<Data>& d = boost::static_pointer_cast<Data>(data);

  // Computing the derivatives for the time-continuous model (i.e. differential model)
  control_->calc(d->control, 0., u);
  differential_->calcDiff(d->differential, x, d->control->w);

  if (enable_integration_) {
    const MatrixXs& da_dx = d->differential->Fx;
    const MatrixXs& da_du = d->differential->Fu;
    d->Fx.topRows(nv).noalias() = da_dx * time_step2_;
    d->Fx.bottomRows(nv).noalias() = da_dx * time_step_;
    d->Fx.topRightCorner(nv, nv).diagonal().array() += Scalar(time_step_);

    control_->multiplyByJacobian(d->control, da_du, d->da_du);
    d->Fu.topRows(nv).noalias() = time_step2_ * d->da_du;
    d->Fu.bottomRows(nv).noalias() = time_step_ * d->da_du;

    differential_->get_state()->JintegrateTransport(x, d->dx, d->Fx, second);
    differential_->get_state()->Jintegrate(x, d->dx, d->Fx, d->Fx, first, addto);
    differential_->get_state()->JintegrateTransport(x, d->dx, d->Fu, second);

    d->Lx.noalias() = time_step_ * d->differential->Lx;
    control_->multiplyJacobianTransposeBy(d->control, d->differential->Lu, d->Lu);
    d->Lu *= time_step_;
    d->Lxx.noalias() = time_step_ * d->differential->Lxx;
    control_->multiplyByJacobian(d->control, d->differential->Lxu, d->Lxu);
    d->Lxu *= time_step_;
    control_->multiplyByJacobian(d->control, d->differential->Luu, d->Lwu);
    control_->multiplyJacobianTransposeBy(d->control, d->Lwu, d->Luu);
    d->Luu *= time_step_;
  } else {
    differential_->get_state()->Jintegrate(x, d->dx, d->Fx, d->Fx);
    d->Fu.setZero();
    d->Lx = d->differential->Lx;
    d->Lu = d->differential->Lu;
    d->Lxx = d->differential->Lxx;
    d->Lxu = d->differential->Lxu;
    d->Luu = d->differential->Luu;
  }
}

template <typename Scalar>
boost::shared_ptr<ActionDataAbstractTpl<Scalar> > IntegratedActionModelEulerTpl<Scalar>::createData() {
  if (control_->get_nu() > differential_->get_nu())
    std::cerr
        << "Warning: It is useless to use an Euler integrator with a control parametrization larger than PolyZero"
        << std::endl;
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
}

template <typename Scalar>
bool IntegratedActionModelEulerTpl<Scalar>::checkData(const boost::shared_ptr<ActionDataAbstract>& data) {
  boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
  if (data != NULL) {
    return differential_->checkData(d->differential);
  } else {
    return false;
  }
}

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data,
                                                        Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
                                                        const std::size_t maxiter, const Scalar tol) {
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
  }
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
  }

  // Static casting the data
  const boost::shared_ptr<Data>& d = boost::static_pointer_cast<Data>(data);

  d->control->w *= 0.;
  differential_->quasiStatic(d->differential, d->control->w, x, maxiter, tol);
  control_->params(d->control, 0., d->control->w);
  u = d->control->u;
}

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::print(std::ostream& os) const {
  os << "IntegratedActionModelEuler {dt=" << time_step_ << ", " << *differential_ << "}";
}

}  // namespace crocoddyl


#include "odoflow/FixedPointDetector.h"
#include "argus_utils/utils/ParamUtils.h"

#include <boost/foreach.hpp>
#include <iostream>
#include <sstream>

namespace argus
{
FixedPointDetector::FixedPointDetector( ros::NodeHandle& nh, ros::NodeHandle& ph )
	: InterestPointDetector( nh, ph ), _cachedGridSize( 0, 0 )
{
	_gridDim.InitializeAndRead( ph, 30, "grid_dim",
	                            "Interest point grid width and height." );
	_gridDim.AddCheck<GreaterThan>( 1 );
	_gridDim.AddCheck<IntegerValued>( ROUND_CEIL );

	UpdateInterestPoints();
}

InterestPoints FixedPointDetector::FindInterestPoints( const cv::Mat& image )
{
	UpdateInterestPoints();

	// NOTE We recompute the pixel coordinates using each image's size, as
	// this might change
	cv::Size imgSize = image.size();
	InterestPoints scaledPoints;
	scaledPoints.reserve( _cachedGrid.size() );

	BOOST_FOREACH( const InterestPoint &point, _cachedGrid )
	{
		InterestPoint p( point.x * imgSize.width, point.y * imgSize.height );
		scaledPoints.push_back( p );
	}

	return scaledPoints;
}

/*! \brief Recompute the normalized interest point coordinates.
 */
void FixedPointDetector::UpdateInterestPoints()
{
	GridSize gridSize( _gridDim, _gridDim );

	if( gridSize == _cachedGridSize ) { return; }

	_cachedGrid.clear();
	unsigned int width = gridSize.first;
	unsigned int height = gridSize.second;
	_cachedGrid.reserve( width * height );
	for( unsigned int i = 0; i < width; i++ )
	{
		double x = (i + 1) / ( (double) width + 1);
		for( unsigned int j = 0; j < height; j++ )
		{
			double y = (j + 1) / ( (double) height + 1);
			_cachedGrid.emplace_back( x, y );
		}
	}

	_cachedGridSize = gridSize;
}
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/include/functions/scalar/rol.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/exceptions.hpp>

NT2_TEST_CASE_TPL ( rol_integer, BOOST_SIMD_INTEGRAL_TYPES)
{
  using boost::simd::rol;
  using boost::simd::tag::rol_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<rol_(T,T)>::type
                  , T
                  );

  T w = sizeof(T)*CHAR_BIT;

  for(T i=0;i<w;++i)
    NT2_TEST_EQUAL(rol(T(1),T(i)), T(T(1)<<i) );

  NT2_TEST_ASSERT(rol(T(1),T(-1)));
  NT2_TEST_ASSERT(rol(T(1),T(w+1)));
}

NT2_TEST_CASE_TPL ( rol_real, BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::rol;
  using boost::simd::tag::rol_;
  using boost::simd::bitwise_cast;

  typedef typename boost::dispatch::meta::as_integer<T>::type iT;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<rol_(T,iT)>::type
                  , T
                  );

  iT w = sizeof(T)*CHAR_BIT;

  for(iT i=0;i<w;++i)
    NT2_TEST_EQUAL( rol(bitwise_cast<T>(iT(1)),iT(i))
                  , bitwise_cast<T>(iT(1)<<i)
                  );

  NT2_TEST_ASSERT(rol(T(1),iT(-1)));
  NT2_TEST_ASSERT(rol(T(1),iT(w+1)));
}