#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <geometry_msgs/Transform.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/LinearMath/Matrix3x3.h>
#include <tf_conversions/tf_eigen.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/filters/passthrough.h>
#include <pcl/registration/icp.h>
#include <pcl/console/parse.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <iostream>
#include <thread>
#include <chrono>
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/Eigen>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/Dense>
#include <Eigen/StdVector>

pcl::PointCloud<pcl::PointXYZ>::Ptr ref_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr ref_main_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scene_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final_output(new pcl::PointCloud<pcl::PointXYZ>);
// Reference to Rough Transformation matrix coming from Pose Estimation using DL
Eigen::Matrix4f Rough_T_matrix_from_pose_est = Eigen::Matrix4f::Identity();

// Parameters for SIFT Computation
const float min_scale =  0.005f;
const int n_octaves = 8;
const int n_scales_per_octave = 4;
const float min_contrast =  0.001f;
double leaf_size = 0.005f;
unsigned int text_id = 0;

void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event,
                            void* viewer_void)
{
  pcl::visualization::PCLVisualizer *viewer = static_cast<pcl::visualization::PCLVisualizer *> (viewer_void);
  if (event.getKeySym () == "r" && event.keyDown ())
  {
    std::cout << "r was pressed => removing all text" << std::endl;

    char str[512];
    for (unsigned int i = 0; i < text_id; ++i)
    {
      sprintf (str, "text#%03d", i);
      viewer->removeShape (str);
    }
    text_id = 0;
  }
}

void mouseEventOccurred (const pcl::visualization::MouseEvent &event,
                         void* viewer_void)
{
  pcl::visualization::PCLVisualizer *viewer = static_cast<pcl::visualization::PCLVisualizer *> (viewer_void);
  if (event.getButton () == pcl::visualization::MouseEvent::LeftButton &&
      event.getType () == pcl::visualization::MouseEvent::MouseButtonRelease)
  {
    std::cout << "Left mouse button released at position (" << event.getX () << ", " << event.getY () << ")" << std::endl;

    char str[512];
    sprintf (str, "text#%03d", text_id ++);
    viewer->addText ("clicked here", event.getX (), event.getY (), str);
  }
}

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

pcl::PointCloud<pcl::PointNormal>::Ptr estimateNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud_ptr,double p_normal_radius)
{
  double normal_radius = p_normal_radius;
  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::PointNormal> ne;
  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals(new pcl::PointCloud<pcl::PointNormal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n(new pcl::search::KdTree<pcl::PointXYZ>());
  std::cout<<"Started to compute normals in the function"<<std::endl;
  ne.setInputCloud(input_cloud_ptr);
  ne.setSearchMethod(tree_n);
  ne.setRadiusSearch(normal_radius);
  ne.compute(*cloud_normals);
  std::cout<<"Normals were computed"<<std::endl;
  for(int i = 0; i<cloud_normals->points.size(); ++i)
  {
    cloud_normals->points[i].x = input_cloud_ptr->points[i].x;
    cloud_normals->points[i].y = input_cloud_ptr->points[i].y;
    cloud_normals->points[i].z = input_cloud_ptr->points[i].z;
  }
  std::cout<<"Cloud normals are "<<cloud_normals->points.size()<<std::endl;
  return cloud_normals;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr estimateKeypointsSIFT(pcl::PointCloud<pcl::PointNormal>::Ptr input_cloud_normal_ptr)
{
  pcl::SIFTKeypoint<pcl::PointNormal, pcl::PointWithScale> sift;
  pcl::PointCloud<pcl::PointWithScale> result;
  pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal>());

  sift.setSearchMethod(tree);
  sift.setScales(min_scale, n_octaves, n_scales_per_octave);
  sift.setMinimumContrast(min_contrast);
  sift.setInputCloud(input_cloud_normal_ptr);
  sift.compute(result);

  // Copying the pointwithscale to pointxyz so as visualize the cloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result, *cloud_temp_ptr);
  std::cout << "SIFT keypoints in the cloud are " << cloud_temp_ptr->points.size () << std::endl;
  return cloud_temp_ptr;
}

pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhExtractor(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud_filtered_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud_ptr, pcl::PointCloud<pcl::PointNormal>::Ptr input_normals_ptr, double p_fpfh_radius)
{
  pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::PointNormal, pcl::FPFHSignature33> fpfh;
  for (int i = 0; i < input_normals_ptr->points.size(); i++)
  {
    if (!pcl::isFinite<pcl::PointNormal>(input_normals_ptr->points[i]))
    {
      PCL_WARN("normals[%d] is not finite\n", i);
    }
  }
  double fpfh_radius = p_fpfh_radius;
  fpfh.setInputCloud(input_cloud_ptr);
  fpfh.setSearchSurface(input_cloud_filtered_ptr);
  fpfh.setInputNormals(input_normals_ptr);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
  fpfh.setSearchMethod(tree);
  // Output datasets
  pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());
  // Radius here should be bigger than that radius used in estimation of normals
  fpfh.setRadiusSearch(fpfh_radius);
  fpfh.compute(*fpfhs);
  std::cout<<"FPFHs points size = "<<fpfhs->points.size()<<std::endl;
  std::cout<<"Input keypoint clouds size = "<<input_cloud_ptr->points.size()<<std::endl;
  return fpfhs;
}

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

    tf::Matrix3x3 rot(quat);
    //rot.getRotation(quat);
    tf::matrixTFToEigen(rot,tmat);
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
          Rough_T_matrix_from_pose_est(i,j) = tmat(i,j);
        }
    }
    Rough_T_matrix_from_pose_est(0,3) = x;
    Rough_T_matrix_from_pose_est(1,3) = y;
    Rough_T_matrix_from_pose_est(2,3) = z;
    Rough_T_matrix_from_pose_est(3,0) = 0;
    Rough_T_matrix_from_pose_est(3,1) = 0;
    Rough_T_matrix_from_pose_est(3,2) = 0;
    Rough_T_matrix_from_pose_est(3,3) = 1;
}

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

  int cycle = 0;
  const std::string PCD_PATH = "/home/dlar/catkin_ws/src/pcl_registration/Data/body_visible_rotated.pcd";
  const std::string PCD_MAIN_PATH = "/home/dlar/catkin_ws/src/pcl_registration/Data/body_visible_rotated.pcd";

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

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

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

  // Filtering Reference Cloud ---------------------------------------
  pcl::PointCloud<pcl::PointXYZ>::Ptr ref_cloud_filtered_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud (ref_cloud_ptr);
  sor.setLeafSize (leaf_size, leaf_size, leaf_size);
  sor.filter (*ref_cloud_filtered_ptr);

  pcl::PointCloud<pcl::PointNormal>::Ptr ref_cloud_normal_ptr(new pcl::PointCloud<pcl::PointNormal>);
  pcl::PointCloud<pcl::PointNormal>::Ptr scene_cloud_normal_ptr(new pcl::PointCloud<pcl::PointNormal>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr ref_SIFT_keypoints_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr scene_SIFT_keypoints_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::FPFHSignature33>::Ptr ref_fpfh_ptr(new pcl::PointCloud<pcl::FPFHSignature33>());
  pcl::PointCloud<pcl::FPFHSignature33>::Ptr scene_fpfh_ptr(new pcl::PointCloud<pcl::FPFHSignature33>());

  std::cout<<"Started to compute normals and keypoints of the reference object."<<std::endl;
  // Setting normal and fpfh search radius
  double normal_radius_ = 0.02f;
  double fpfh_radius_ = 0.02f;
  ref_cloud_normal_ptr = estimateNormals(ref_cloud_filtered_ptr,normal_radius_);
  ref_SIFT_keypoints_ptr = estimateKeypointsSIFT(ref_cloud_normal_ptr);
  ref_fpfh_ptr = fpfhExtractor(ref_cloud_filtered_ptr,ref_SIFT_keypoints_ptr,ref_cloud_normal_ptr,fpfh_radius_);
  // Filter parameters
  float depth_limit;
  float height_limit;
  float width_limit;

  // Rough Transformation matrix
  pcl::PointCloud<pcl::PointXYZ>::Ptr scene_cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr scene_cloud_transformed_dl(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_ref_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  std::cout<<"ROS Cycle starts"<<std::endl;

  while(ros::ok())
  {
    cycle++;

    if(cycle>1)
    {
      // First filter scene cloud data ---------------------------------
      // ...removing distant points
      pcl::PassThrough<pcl::PointXYZ> pass;
      pass.setInputCloud (scene_cloud_ptr);
      pass.setFilterFieldName ("z");
      pass.setFilterLimits (0, Rough_T_matrix_from_pose_est(2,3)+0.10);
      pass.filter (*scene_cloud_ptr);
      pass.setFilterFieldName("x");
      width_limit = Rough_T_matrix_from_pose_est(0,3);
      pass.setFilterLimits(width_limit-0.40,width_limit+0.40);
      pass.filter(*scene_cloud_ptr);
      pass.setFilterFieldName("y");
      height_limit = Rough_T_matrix_from_pose_est(1,3);
      pass.setFilterLimits(height_limit-0.10, height_limit+0.75);
      pass.filter(*scene_cloud_ptr);
      // Downsampling scene point cloud
      pcl::VoxelGrid<pcl::PointXYZ> sor;
      sor.setInputCloud (scene_cloud_ptr);
      sor.setLeafSize (0.005, 0.005, 0.005);
      sor.filter (*scene_cloud_ptr);

      // Remove Statistical Outliers -------------------------------------
      pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor_statistic_removal;
      sor_statistic_removal.setInputCloud (scene_cloud_ptr);
      sor_statistic_removal.setMeanK (50);
      sor_statistic_removal.setStddevMulThresh(1.0);
      sor_statistic_removal.filter (*scene_cloud_ptr);
      pcl::io::savePCDFileASCII("data_scene.pcd", *scene_cloud_ptr);

      // First Projection with Rough Transformation Matrix
      auto start = std::chrono::high_resolution_clock::now(); // To compute time interval ICP computation needs

      if(cycle<3)
      {
      pcl::transformPointCloud(*scene_cloud_ptr, *scene_cloud_transformed_dl, Rough_T_matrix_from_pose_est.inverse());
      pcl::io::savePCDFileASCII("data_dl.pcd", *scene_cloud_transformed_dl);
      std::cout << "Rought Transformation coming from Pose Estimation" << std::endl << Rough_T_matrix_from_pose_est.inverse() << std::endl;
      }

      // Iterative Closest Point Algorithm -----------------------------
      auto start_icp = std::chrono::high_resolution_clock::now();
      pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;
      icp.setInputSource(scene_cloud_transformed_dl);
      icp.setInputTarget(ref_cloud_filtered_ptr);
      icp.setTransformationEpsilon(1e-8);
      icp.setMaximumIterations(100000);
      icp.align(*scene_cloud_transformed_dl);
      std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
      std::cout << icp.getFinalTransformation()<< std::endl;
      auto end_icp= std::chrono::high_resolution_clock::now();
      auto duration_icp = std::chrono::duration_cast<std::chrono::milliseconds>(end_icp - start_icp);
      std::cout << "PCL CPU ICP duration: " << duration_icp.count() << std::endl;
      pcl::io::savePCDFileASCII("data_icp_CPU.pcd", *scene_cloud_transformed_dl);

      Eigen::Matrix4f icp_transformation = icp.getFinalTransformation();
      if(cycle<3)
      {
      // Transform reference model into scene points using general transformation inverse
      Eigen::Matrix4f gen_transformation = icp_transformation*Rough_T_matrix_from_pose_est.inverse();
      pcl::transformPointCloud (*ref_cloud_ptr, *final_output, gen_transformation.inverse());
      }
      else
      {
      pcl::transformPointCloud (*final_output, *final_output, icp_transformation.inverse());
      }
      auto end = std::chrono::high_resolution_clock::now();
      auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
	    std::cout << "PCL CPU duration: " << duration.count() << std::endl;
    }
    pcl::toROSMsg(*final_output,output);
    output.header.frame_id = "camera_depth_optical_frame";
    output.header.stamp = ros::Time::now();
    pub.publish (output);

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

  }
  return 0;
}


/*
 * Verifier.cpp
 *
 *  Created on: 26.10.2010
 *      Author: stephaniebayer
 */

#include "Verifier.h"
#include<vector>
#include "Cipher_elg.h"
#include "G_q.h"
#include "Mod_p.h"
#include "Functions.h"
#include "ElGammal.h"
#include "func_ver.h"
#include <fstream>

#include <time.h>
#include <NTL/ZZ.h>
NTL_CLIENT

extern G_q G;
extern G_q H;
extern Pedersen Ped;
extern ElGammal El;
extern double time_rw_v;

//Constructors
Verifier::Verifier() {
	// TODO Auto-generated constructor stub

}

Verifier::Verifier(vector<long> num) {
	// sets the values of the matrix according to the input
	m = num[1]; //number of rows
	n = num[2]; //number of columns

	c_A = new vector<Mod_p>(m+1); //allocate the storage for the commitments of Y
	c_B = new vector<Mod_p>(m); //allocate the storage for the commitments of T
	chal_x6 = new vector<ZZ>(2*m);// allocate the storage for the vector of Vandermonde challenges t, ... t^n
	chal_y6 = new vector<ZZ>(n);// allocate the storage for the vector of Vandermonde challenges t, ... t^n
	//allocates the storage needed for the vector e
	chal_x8 = new vector<ZZ>(2*m +1); //challenge from round 8

	//Commitments vectors
	c_Dh= new vector<Mod_p>(m); //commitments to the matrix W
	c_Ds= new vector<Mod_p>(m+1); //contains a_W*t_1
	c_Dl= new vector<Mod_p>(2*m+1); //commitments to the values Cl
	c_a= new vector<Mod_p>(2*m); //commitment to the values in the matrix a
	//Vector of product of the diagonals of permuted Ciphertexts from round 5
	E = new vector<Cipher_elg>(2*m);

	D_h_bar = new vector<ZZ>(n);//sum over the rows in D_h

	d_bar = new vector<ZZ>(n);// chal_x8*D_h(m-1) +d
	Delta_bar = new vector<ZZ>(n);//chal_x8*d_h+Delta
	B_bar  = new vector<ZZ>(n); // sum over the rows in B multiplied by chal^i

	A_bar = new vector<ZZ>(n); //sum over the rows in Y times the challenges
	Ds_bar = new vector<ZZ>(n); // sum over the rows in U times thes challenges
}

Verifier::~Verifier() {
	delete c_A;
	delete c_B;
	delete chal_x6;
	delete chal_y6;
	delete chal_x8;
	delete c_Dh;
	delete c_Ds;
	delete c_Dl;
	delete c_a;
	delete E;
	delete D_h_bar;
	delete d_bar;
	delete Delta_bar;
	delete B_bar;
	delete A_bar;
	delete Ds_bar;
}

string Verifier::round_2(string in_name){
	long i;
	string name;
	ZZ ord=H.get_ord();
	time_t rawtime;
	time ( &rawtime );

	//sets a_Y to the values in the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for (i = 0; i<m; i++){
		ist >> c_A->at(i);
	}

	chal_x2 = RandomBnd(ord);

	name = "round_2 ";
	name = name + ctime(&rawtime);
	ofstream ost(name.c_str());
	ost<<chal_x2;

	return name;
}

string Verifier::round_4(string in_name){
	long i;
	ZZ ord = H.get_ord();
	string name;
	time_t rawtime;
	time ( &rawtime );
	 //sets a_T to the values in the file
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for (i = 0; i<m; i++){
		ist >> c_B->at(i);
	}

	//Set name of the output file and open stream
	name = "round_4 ";
	name = name + ctime(&rawtime);

	chal_y4 = RandomBnd(ord);
	chal_z4 = RandomBnd(ord);

	ofstream ost(name.c_str());
	ost<< chal_z4<<"\n";
	ost<<chal_y4 ;
	return name;
}

string Verifier::round_6(string in_name){
	ZZ tem;
	ZZ ord = H.get_ord();
	long i,l;
	Mod_p temp;
	string name;
	time_t rawtime;
	time ( &rawtime );
	//reads the values out of the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	ist>>c_z;
	for (i = 0; i<m; i++){
		ist >>c_Dh->at(i);
	}
	ist>>c_B0;
	l=2*m;
	for (i = 0; i<l; i++){
		ist >>  c_a->at(i);
	}
	for (i = 0; i<l; i++){
		ist >> E->at(i);
	}
	func_ver::fill_vector(chal_x6);
	func_ver::fill_vector(chal_y6);
	name = "round_6 ";
	name = name + ctime(&rawtime);
	ofstream ost(name.c_str());
	for (i=0; i<2*m; i++){
		ost << chal_x6->at(i)<<" ";
	}
	ost<<"\n";
	for (i=0; i<n; i++){
		ost << chal_y6->at(i)<<" ";
	}
	ost << "\n";

	return name;

}

string  Verifier::round_8(string in_name){
	ZZ chal; //Challenges picked at random;
	ZZ ord = H.get_ord();
	long l;
	long i;

	string name;
	time_t rawtime;
	time ( &rawtime );
	//reads the values out of the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	l=2*m;
	for (i = 0; i<=l; i++){
		ist >> c_Dl->at(i);
	}
	ist>>c_D0;
	ist>>c_Dm;
	ist>>c_d;
	ist>>c_Delta;
	ist>>c_dh;
	for (i = 0; i<n; i++){
		ist >>B_bar->at(i);
	}
	ist>> r_B_bar;
	ist >>a_bar;
	ist >> r_a_bar;
	ist >> rho_bar;

	//sets e as Vandermode vector with value chal
	l= chal_x8->size(); //length of vector chal_x8;
	func_ver::fill_vector(chal_x8);
	name = "round_8 ";
	name = name + ctime(&rawtime);
	ofstream ost(name.c_str());
	for (i = 0; i<l; i++){
		ost << chal_x8->at(i)<< " ";
	}

	return name;
}

ZZ Verifier:: round_10(string in_name,vector<vector<Cipher_elg>* >* enc, vector<vector<Cipher_elg>* >* C){
	int b;
	long i;

	//reads the values out of the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for (i = 0; i<n; i++){
		ist >>D_h_bar->at(i);
	}
	ist>>r_Dh_bar;
	for(i=0; i<n; i++){
		ist>>d_bar->at(i);
	}
	ist>> r_d_bar;
	for(i=0; i<n; i++){
		ist>> Delta_bar->at(i);
	}
	ist>>r_Delta_bar;

	for (i = 0; i<n; i++){
		ist >>A_bar->at(i);
	}
	ist>>r_A_bar;
	for(i=0; i<n; i++){
		ist>>Ds_bar->at(i);
	}
	ist>>r_Ds_bar;
	ist>>r_Dl_bar;

	//Check that the D_hi's are constructed correctly
	b=func_ver::check_Dh(c_Dh, chal_x8, D_h_bar, r_Dh_bar);

	if(b==1){
		//Check that the matrix D is constructed correctly according to the permutation
		b=func_ver::check_D(c_D0, c_z, c_A, c_B, chal_x8, chal_y4, A_bar, r_A_bar, n);
		if(b==1){
			//Check that the matrix D_s is constructed correctly
			b=func_ver::check_Ds(c_Ds, c_Dh, c_Dm, chal_x6, chal_x8, Ds_bar, r_Ds_bar);
			if(b==1){
				//Check that the Dl are constructed correctly
				b=func_ver::check_Dl(c_Dl, chal_x8, A_bar, Ds_bar, chal_y6, r_Dl_bar);
				if(b==1){
					//Check that vector d was constructed correctly
					b=func_ver::check_d(c_Dh, c_d, chal_x8, d_bar, r_d_bar);
					if(b==1){
						//Check that Deltas are constructed correctly
						b=func_ver::check_Delta(c_dh, c_Delta, chal_x8, Delta_bar, d_bar, r_Delta_bar, chal_x2, chal_z4, chal_y4);
						if(b==1 ){

							//Check that the commitments c_B contain the right values
							b=func_ver::check_B(c_B, c_B0, chal_x6, B_bar, r_B_bar);
							if(b==1){
								//Check that the reecncryption was done correctly
								b=func_ver::check_a(c_a, c_Dl, chal_x6, a_bar, r_a_bar);
								if(b==1){
									//Check that E->at(m) contains c
									b = func_ver::check_c(enc, E, chal_x2);
									if(b==1){
										//Check correctness of the ciphertexts
										b =func_ver::check_E(C, E, chal_x6, B_bar, a_bar, rho_bar);
										if(b==1 ){
											//cout<<" Accept ";
										}
									}
								}
							}
						}
					}
				}
			}
		}
	}
	return to_ZZ(-1);
}


#include <cassert>
#include <Eigen/Dense>
#include<Eigen/StdVector>
#include <string>
#include <fstream>
#include <iostream>
#include "scorematrix.h"
#include "weightedmatrix.h"
#include "utility.h"

using namespace std;

namespace ScoreMatrix {
// for weighted matrices
    vector< AminoMatrix, Eigen::aligned_allocator<AminoMatrix> > matrices;
    vector<double> weights;
}

bool ScoreMatrix::isWeightedMatrixFile(const string& fileName)
{
    ifstream ifs(fileName.c_str());
    if(!ifs) return false;
    string buff;
    getline(ifs, buff);
    return (buff == "#WEIGHTED MATRIX#");
}

/*!
 \brief 重み置換行列ファイルを読み込み設定する.

 \param fileName
 \return bool
*/
bool ScoreMatrix::loadWeightedMatrix(const string& fileName)
{
    AminoMatrix locAM;
    double locWeight;

    amatrix.setZero();

    // File format: Comma separated file.
    // [weight],[subsutitution matrix file name]
    ifstream ifs(fileName.c_str());
    string buff;

    // omit the first comment line.
    assert(ifs && "[ScoreMatrix::loadWeightedMatrix] FILE IS NOT FOUND.");
    getline(ifs, buff);

    while(ifs && getline(ifs, buff))
    {
        vector<string> list;
        vector<string> vs;
        mySplit(buff, ", ", vs);
        for(vector<string>::iterator i=vs.begin(); i!=vs.end(); ++i)list.push_back(*i);

        if(vs.size() > 0)
        {
            locWeight = string2binary<double>(vs.at(0));
            const string matrixName = vs.at(1);
            loadMatrix(fixFilePath(matrixName), true, locAM);
            cout << "w=" << locWeight << ":" << matrixName << endl;
            //amatrix += locWeight * locAM;
            amatrix = locAM;
        }
    }

    ifs.close();

    // Calc. amatrix
    // Mode 00: normalized (distance), average
    // Mode 01: normalized (distance), square root
    // Mode 10: non-normalized, average
    // Mode 11: non-normalized, square root
    // 現在値がバグる
    return true;
}


#include <boost/simd/pack.hpp>

#include "compute.hpp"

#include <cstdlib>
#include <iostream>
#include <vector>

int main()
{
  int size = 1000;
  std::vector<float> a(size), b(size), res(size);
  //![runtime-main]
  namespace bs = boost::simd;

  if (bs::avx2.is_supported()) {
    compute(a.data(), b.data(), res.data(), size, bs::avx2_{});
  } else if (bs::avx.is_supported()) {
    compute(a.data(), b.data(), res.data(), size, bs::avx_{});
  } else if (bs::sse4_2.is_supported()) {
    compute(a.data(), b.data(), res.data(), size, bs::sse4_2_{});
  } else if (bs::sse4_1.is_supported()) {
    compute(a.data(), b.data(), res.data(), size, bs::sse4_1_{});
  } else if (bs::sse2.is_supported()) {
    compute(a.data(), b.data(), res.data(), size, bs::sse3_{});
  }
  //![runtime-main]
}


/***************************************************************************
 *   Copyright (C) 2006 - 2016 by                                          *
 *      Tarek Taha, KURI  <tataha@tarektaha.com>                           *
 *      Randa Almadhoun, KURI  <randa.almadhoun@gmail.com>                 *
 *                                                                         *
 *   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 2 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, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   51 Franklin Steet, Fifth Floor, Boston, MA  02111-1307, USA.          *
 ***************************************************************************/

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/GlobalPositionTarget.h>
#include <geometry_msgs/PoseArray.h>

#include <signal.h>
#include <termios.h>
#include <iostream>
#include <queue>
#include <sstream>
#include <string>
#include <time.h>
#include <algorithm>
#include <unistd.h>
#include <pthread.h>
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <string.h>
#include <float.h>
#include <std_msgs/Float64.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <stdio.h>
#include <stdlib.h>
#include <boost/filesystem.hpp>
#include <ros/package.h>

#define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
#define CONSTANTS_AIR_GAS_CONST				287.1f 			/* J/(kg * K)		*/
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
#define CONSTANTS_RADIUS_OF_EARTH	6371000	/* meters (m)		*/

/* lat/lon are in radians */
struct map_projection_reference_s {
    long double lat_rad;
    long double lon_rad;
    long double sin_lat;
    long double cos_lat;
    bool init_done;
    uint64_t timestamp;
};

struct globallocal_converter_reference_s {
    double alt;
    bool init_done;
};

static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0};
static struct globallocal_converter_reference_s gl_ref = {0.0f, false};

ros::ServiceClient takeoff;

geometry_msgs::Point        real;
geometry_msgs::PoseArray    waypoints;
geometry_msgs::PoseArray    globalWaypoints;
geometry_msgs::PoseStamped  goalPose;
mavros_msgs::State          current_state;

int count       = 0 ;
double tolerance = 0.3;
double errorX    = 0;
double errorY    = 0;
double errorZ    = 0;

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

bool map_projection_initialized(const struct map_projection_reference_s *ref)
{
    return ref->init_done;
}

bool map_projection_global_initialized()
{
    return map_projection_initialized(&mp_ref);
}

int map_projection_reproject(const struct map_projection_reference_s *ref, double x, double y, long double *lat,
                      long double *lon)
{
    if (!map_projection_initialized(ref)) {
        return -1;
    }

    long double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
    long double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
    long double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
    long double sin_c = sin(c);
    long double cos_c = cos(c);

    long double lat_rad;
    long double lon_rad;

    if (fabs(c) > DBL_EPSILON) {
        lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
        lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));

    } else {
        lat_rad = ref->lat_rad;
        lon_rad = ref->lon_rad;
    }

    *lat = lat_rad * 180.0 / M_PI;
    *lon = lon_rad * 180.0 / M_PI;

    return 0;
}

int map_projection_global_reproject(double x, double y, long double *lat, long double *lon)
{
    return map_projection_reproject(&mp_ref, x, y, lat, lon);
}

int globallocalconverter_toglobal(double x, double y, double z,  long double *lat,long double *lon, double *alt)
{
    if (!map_projection_global_initialized()) {
        //return -1;
    }

    map_projection_global_reproject(x, y, lat, lon);
    *alt = gl_ref.alt - z;

    return 0;
}

void globalPoseCallback(const geometry_msgs :: PoseStamped :: ConstPtr& msg)
{
    real.x=msg ->pose.position.x;
    real.y=msg ->pose.position.y;
    real.z=msg ->pose.position.z;

    errorX =  waypoints.poses[count].position.x - real.x;
    errorY =  waypoints.poses[count].position.y - real.y;
    errorZ =  waypoints.poses[count].position.z - real.z;

    if ((fabs(errorX) < tolerance) && (fabs(errorY) < tolerance) && (fabs(errorZ) < tolerance))
    {
        count++;
        if(count<waypoints.poses.size())
        {
            mavros_msgs::CommandTOL point;

            point.request.latitude = globalWaypoints.poses[count].position.x;
            point.request.longitude = globalWaypoints.poses[count].position.y;
            point.request.altitude = globalWaypoints.poses[count].position.z;
            bool result = takeoff.call(point);
            std::cout<<"done ...> next"<<std::endl;

        }

    }

}

int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0,
        uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{

    ref->lat_rad = lat_0 * M_PI / 180.0;
    ref->lon_rad = lon_0 * M_PI / 180.0;
    ref->sin_lat = sin(ref->lat_rad);
    ref->cos_lat = cos(ref->lat_rad);

    ref->timestamp = timestamp;
    ref->init_done = true;

    return 0;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

//    double lat_ref = 47.397742;
//    double lon_ref =8.5455938;
//    std_msgs::Float64 lat_ref1 = (std_msgs::Float64)47.397742;
//    std_msgs::Float64 lon_ref1 =(std_msgs::Float64)8.5455938;

    //read setpoints from file
    std::string str1 = ros::package::getPath("kuri_mbzirc_sim")+"/config/con_2_100_GPU_arena_ch3.txt";
    const char * filename1 = str1.c_str();

    assert(filename1 != NULL);
    filename1 = strdup(filename1);
    FILE *file1 = fopen(filename1, "r");
    if (!file1)
    {
        std::cout<<"\nCan not open File";
        fclose(file1);
    }

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("/uav_1/mavros/state", 10, state_cb);
    ros::Publisher pos_pub = nh.advertise<mavros_msgs::GlobalPositionTarget>
            ("/uav_1/mavros/setpoint_raw/global", 10);
    ros::Publisher local_pos_pub = nh.advertise<mavros_msgs::PositionTarget>
            ("/uav_1/mavros/setpoint_raw/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("/uav_1/mavros/cmd/arming");
    takeoff = nh.serviceClient<mavros_msgs::CommandTOL>
            ("/uav_1/mavros/cmd/takeoff");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("/uav_1/mavros/set_mode");
    ros::Subscriber globalPoseSub = nh.subscribe
            ("/uav_1/mavros/local_position/pose", 1000, globalPoseCallback);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    double locationx,locationy,locationz,qy;
    geometry_msgs::Pose pose;
    geometry_msgs::Pose globalPose;

    double lat_ref = ref_la;
    double lon_ref = ref_lo;
    std::cout<<(double)lat_ref<<" "<<(double)lon_ref<<std::endl;
    map_projection_init_timestamped(&mp_ref,lat_ref,lon_ref,1);

    while (!feof(file1))
    {
        fscanf(file1,"%lf %lf %lf %lf\n",&locationx,&locationy,&locationz,&qy);
        pose.position.x = locationx;
        pose.position.y = locationy;
        pose.position.z = locationz;
        pose.orientation.x = qy;
        waypoints.poses.push_back(pose);

        long double lat;
        long double lon;
        double alt;

        globallocalconverter_toglobal(locationy,locationx,locationz,&lat,&lon,&alt);
        globalPose.position.x = lat;
        globalPose.position.y = lon;
        globalPose.position.z = alt*-1;
        std::cout<<"global : lat "<<lat<<" lon "<<lon<<" alt "<<alt<<std::endl;
        std::cout<<"local : x "<<locationx<<" y "<<locationy<<" z "<<locationz<<std::endl;

        globalWaypoints.poses.push_back(globalPose);

    }

    mavros_msgs::GlobalPositionTarget pp;
//    pp.header.frame_id = "fcu";
    pp.latitude = 95;
    pp.longitude= 707406378;
    pp.altitude = -10;
    pp.coordinate_frame = mavros_msgs::GlobalPositionTarget::FRAME_GLOBAL_INT;
    pp.type_mask = (1 << 11) | (7 << 6) | (7 << 3);

    mavros_msgs::PositionTarget p;
    p.position.x= 5;
    p.position.y = -5;
    p.position.z = 10;
    p.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
    p.type_mask = (1 << 11) | (7 << 6) | (7 << 3);

    //send a few setpoints before starting
//     for(int i = 100; ros::ok() && i > 0; --i){
//         pos_pub.publish(pp);
//         local_pos_pub.publish(p);
//         ros::spinOnce();
//         rate.sleep();
//     }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "AUTO.TAKEOFF";

    mavros_msgs::CommandBool arm_cmd;
    mavros_msgs::CommandTOL point;
    point.request.altitude = 10;
    point.request.latitude = 47.3977003;//47.3977419
    point.request.longitude = 8.5455939;//8.5455939

    ros::Time last_request = ros::Time::now();
    bool armed=false;
    bool flag=false;
    while(ros::ok()){
            arm_cmd.request.value = true;
        if( current_state.mode != "AUTO.TAKEOFF" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.success){
                ROS_INFO("AUTO.TAKEOFF enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0)) && !(armed)){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                    armed=true;
                }
                last_request = ros::Time::now();
            }
        }

//        local_pos_pub.publish(p);
//        pos_pub.publish(pp);

        if(!flag)
        {
            mavros_msgs::CommandTOL point;

            point.request.latitude = globalWaypoints.poses[count].position.x;
            point.request.longitude = globalWaypoints.poses[count].position.y;
            point.request.altitude = globalWaypoints.poses[count].position.z;
            bool result = takeoff.call(point);
            flag=true;
        }

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

    return 0;
}


//          Copyright Gavin Band 2008 - 2012.
// 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 RELATEDNESS_COMPONENT_MEAN_CENTRE_GENOTYPES_HPP
#define RELATEDNESS_COMPONENT_MEAN_CENTRE_GENOTYPES_HPP

#include <Eigen/Core>

namespace pca {
	template< typename Vector1, typename Vector2 >
	void mean_centre_genotypes(
		Vector1* threshholded_genotypes,
		Vector2 const& non_missingness_matrix,
		double allele_frequency
	) {
		std::size_t const number_of_samples = threshholded_genotypes->size() ;
		assert( std::size_t( non_missingness_matrix.size() ) == number_of_samples ) ;
		for( std::size_t sample_i = 0; sample_i < number_of_samples; ++sample_i ) {
			if( non_missingness_matrix( sample_i ) ) {
				(*threshholded_genotypes)( sample_i ) -= 2.0 * allele_frequency ;
			}
			else {
				(*threshholded_genotypes)( sample_i ) = 0.0 ; // this sample does not contribute for this SNP.
			}
		}
	}
}

#endif


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <orea/engine/parametricvar.hpp>

#include <orea/engine/riskfilter.hpp>
#include <orea/scenario/shiftscenariogenerator.hpp>
#include <ored/utilities/csvfilereader.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>

#include <qle/math/deltagammavar.hpp>

#include <ql/math/matrixutilities/pseudosqrt.hpp>
#include <ql/math/matrixutilities/symmetricschurdecomposition.hpp>

#include <boost/regex.hpp>

using namespace QuantLib;

namespace ore {
namespace analytics {

ParametricVarCalculator::ParametricVarCalculator(
    const std::map<std::string, std::set<string>>& tradePortfolios, const std::string& portfolioFilter,
    const boost::shared_ptr<SensitivityStream>& sensitivities,
    const std::map<std::pair<RiskFactorKey, RiskFactorKey>, Real> covariance, const std::vector<Real>& p,
    const std::string& method, const Size mcSamples, const Size mcSeed, const bool breakdown,
    const bool salvageCovarianceMatrix)
    : tradePortfolios_(tradePortfolios), portfolioFilter_(portfolioFilter), sensitivities_(sensitivities),
      covariance_(covariance), p_(p), method_(method), mcSamples_(mcSamples), mcSeed_(mcSeed), breakdown_(breakdown),
      salvageCovarianceMatrix_(salvageCovarianceMatrix) {}

void ParametricVarCalculator::calculate(ore::data::Report& report) {
    LOG("Parametric VaR calculation started...");

    // prepare report
    report.addColumn("Portfolio", string()).addColumn("RiskClass", string()).addColumn("RiskType", string());
    for (Size i = 0; i < p_.size(); ++i)
        report.addColumn("Quantile_" + std::to_string(p_[i]), double(), 6);

    // build portfolio filter, if given
    bool hasFilter = false;
    boost::regex filter;
    if (portfolioFilter_ != "") {
        hasFilter = true;
        filter = boost::regex(portfolioFilter_);
        LOG("Portfolio filter: " << portfolioFilter_);
    } else {
        LOG("No portfolio filter will be applied.");
    }

    // read sensitivities and preaggregate them per prtfolio
    LOG("Preaggregate sensitivities per portfolio");
    std::set<RiskFactorKey> sensiKeysTmp;
    std::set<std::string> portfoliosTmp;
    std::map<std::string, std::map<std::pair<RiskFactorKey, RiskFactorKey>, Real>> value1, value2;
    std::map<std::pair<RiskFactorKey, RiskFactorKey>, Real> value1All, value2All;
    while (SensitivityRecord sr = sensitivities_->next()) {
        std::set<std::string> portfolios;
        auto pn = tradePortfolios_.find(sr.tradeId);
        if (pn != tradePortfolios_.end()) {
            if (pn->second.empty())
                portfolios = {"(empty)"};
            else
                portfolios = pn->second;
        } else
            portfolios = {"(unknown)"};
        RiskFactorKey k1 = sr.key_1;
        RiskFactorKey k2 = sr.key_2;
        auto key = std::make_pair(k1, k2);
        if (k1 != RiskFactorKey())
            sensiKeysTmp.insert(k1);
        if (k2 != RiskFactorKey())
            sensiKeysTmp.insert(k2);
        bool relevant = false;
        for (auto const& p : portfolios) {
            if (!hasFilter || boost::regex_match(p, filter)) {
                relevant = true;
                portfoliosTmp.insert(p);
                if (sr.isCrossGamma()) {
                    value1[p][key] += sr.gamma;
                } else {
                    value1[p][key] += sr.delta;
                }
                if (!sr.isCrossGamma()) {
                    value2[p][key] += sr.gamma;
                }
            }
        }
        if (relevant) {
            if (sr.isCrossGamma()) {
                value1All[key] += sr.gamma;
            } else {
                value1All[key] += sr.delta;
            }
            if (!sr.isCrossGamma()) {
                value2All[key] += sr.gamma;
            }
        }
    }
    std::vector<RiskFactorKey> sensiKeys(sensiKeysTmp.begin(), sensiKeysTmp.end());
    std::vector<bool> sensiKeyHasNonZeroVariance(sensiKeys.size(), false);
    std::vector<std::string> portfolios(portfoliosTmp.begin(), portfoliosTmp.end());
    LOG("Have " << sensiKeys.size() << " sensitivity keys in " << portfolios.size() << " portfolios");

    // build global covariance matrix
    Matrix omega(sensiKeys.size(), sensiKeys.size(), 0.0);
    Size unusedCovariance = 0;
    for (const auto& c : covariance_) {
        auto k1 = std::find(sensiKeys.begin(), sensiKeys.end(), c.first.first);
        auto k2 = std::find(sensiKeys.begin(), sensiKeys.end(), c.first.second);
        if (k1 != sensiKeys.end() && k2 != sensiKeys.end()) {
            omega(k1 - sensiKeys.begin(), k2 - sensiKeys.begin()) = c.second;
            if (k1 == k2)
                sensiKeyHasNonZeroVariance[k1 - sensiKeys.begin()] = true;
        } else {
            ++unusedCovariance;
        }
    }
    LOG("Found " << covariance_.size() << " covariance matrix entries, " << unusedCovariance
                 << " do not match a portfolio sensitivity and will not be used.");
    for (Size i = 0; i < sensiKeyHasNonZeroVariance.size(); ++i) {
        if (!sensiKeyHasNonZeroVariance[i]) {
            WLOG("Zero variance assigned to sensitivity key " << sensiKeys[i]);
        }
    }

    // make covariance matrix positive semi-definite
    boost::shared_ptr<QuantExt::CovarianceSalvage> covarianceSalvage;
    LOG("Covariance matrix has dimension " << sensiKeys.size() << " x " << sensiKeys.size());
    if (salvageCovarianceMatrix_) {
        LOG("Make covariance matrix positive semi-definite using spectral method");
        covarianceSalvage = boost::make_shared<QuantExt::SpectralCovarianceSalvage>();
    } else {
        LOG("Covariance matrix is no salvaged, check for positive semi-definiteness");
        SymmetricSchurDecomposition ssd(omega);
        Real evMin = ssd.eigenvalues().back();
        QL_REQUIRE(evMin > 0.0 || close_enough(evMin, 0.0),
                   "ParametricVar: input covariance matrix is not positive semi-definite, smallest eigenvalue is "
                       << evMin);
        LOG("Smallest eigenvalue is " << evMin);
        covarianceSalvage = boost::make_shared<QuantExt::NoCovarianceSalvage>();
    }
    LOG("Done.");

    // loop over portfolios (index 0 = all portfolios)
    for (Size i = 0; i <= (!breakdown_ || portfolios.size() == 1 ? 0 : portfolios.size()); ++i) {
        std::string portfolioName = i == 0 ? (portfolios.size() > 1 ? "(all)" : portfolios.front()) : portfolios[i - 1];
        // build delta and gamma for given portfolio
        const auto& val1 = (i == 0 ? value1All : value1[portfolios[i - 1]]);
        const auto& val2 = (i == 0 ? value2All : value2[portfolios[i - 1]]);
        Array delta(sensiKeys.size(), 0.0);
        Matrix gamma(sensiKeys.size(), sensiKeys.size(), 0.0);
        for (auto const& p : val1) {
            auto k1 = p.first.first;
            auto k2 = p.first.second;
            Size idx1 = std::find(sensiKeys.begin(), sensiKeys.end(), k1) - sensiKeys.begin();
            QL_REQUIRE(idx1 < sensiKeys.size(), "ParametricVarCalculator::computeVar: key1 \""
                                                    << k1 << "\" in value1 not found, this is unexpected.");
            if (k2 == RiskFactorKey()) {
                // delta
                delta[idx1] += p.second;
            } else {
                // cross gamma
                Size idx2 = std::find(sensiKeys.begin(), sensiKeys.end(), k2) - sensiKeys.begin();
                QL_REQUIRE(idx2 < sensiKeys.size(), "ParametricVarCalculator::computeVar: key2 \""
                                                        << k2 << "\" in value1 not found, this is unexpected.");
                gamma[idx1][idx2] = gamma[idx2][idx1] = p.second;
            }
        }
        for (auto const& p : val2) {
            // diagonal gamma
            auto k1 = p.first.first;
            Size idx1 = std::find(sensiKeys.begin(), sensiKeys.end(), k1) - sensiKeys.begin();
            QL_REQUIRE(idx1 < sensiKeys.size(), "ParametricVarCalculator::computeVar: key1 \""
                                                    << k1 << "\" in value2 not found, this is unexpected.");
            gamma[idx1][idx1] = p.second;
        }
        // loop over risk class and type filters (index 0 == all risk types)
        for (Size j = 0; j < (breakdown_ ? RiskFilter::numberOfRiskClasses() : 1); ++j) {
            for (Size k = 0; k < (breakdown_ ? RiskFilter::numberOfRiskTypes() : 1); ++k) {
                // TODO should we rather project on the set of indices with non-zero sensis
                // instead of copying the initial sensis and setting the non-releveant entries
                // to zero?
                Array deltaFiltered(delta);
                Matrix gammaFiltered(gamma);
                RiskFilter rf(j, k);
                LOG("Compute parametric var for portfolio \"" << portfolioName << "\""
                                                              << ", risk class " << rf.riskClassLabel()
                                                              << ", risk type " << rf.riskTypeLabel());
                // set sensis which do not belong to risk type filter to zero
                for (Size idx = 0; idx < sensiKeys.size(); ++idx) {
                    if (!rf.allowed(sensiKeys[idx].keytype)) {
                        deltaFiltered[idx] = 0.0;
                        for (Size ii = 0; ii < sensiKeys.size(); ++ii) {
                            gammaFiltered[idx][ii] = gammaFiltered[ii][idx] = 0.0;
                        }
                    }
                }
                // are all sensis zero, then skip the computation
                bool zeroSensis = close_enough(QuantExt::detail::absMax(deltaFiltered), 0.0) &&
                                  close_enough(QuantExt::detail::absMax(gammaFiltered), 0.0);
                // compute var and write to report
                std::vector<Real> var = zeroSensis
                                            ? std::vector<Real>(p_.size(), 0.0)
                                            : computeVar(omega, deltaFiltered, gammaFiltered, p_, *covarianceSalvage);
                if (!close_enough(QuantExt::detail::absMax(var), 0.0)) {
                    report.next();
                    report.add(portfolioName);
                    report.add(rf.riskClassLabel());
                    report.add(rf.riskTypeLabel());
                    for (auto const& v : var)
                        report.add(v);
                }
            } // for k (risk types)
        }     // for j (risk classes)
    }         // for i (portfolios)
    LOG("parametric var computation done.");
    report.end();

} // calculate

std::vector<Real> ParametricVarCalculator::computeVar(const Matrix& omega, const Array& delta, const Matrix& gamma,
                                                      const std::vector<Real>& p,
                                                      const QuantExt::CovarianceSalvage& covarianceSalvage) {
    if (method_ == "Delta") {
        std::vector<Real> res(p.size());
        for (Size i = 0; i < p.size(); ++i) {
            res[i] = QuantExt::deltaVar(omega, delta, p[i], covarianceSalvage);
        }
        return res;
    } else if (method_ == "DeltaGammaNormal") {
        std::vector<Real> res(p.size());
        for (Size i = 0; i < p.size(); ++i) {
            res[i] = QuantExt::deltaGammaVarNormal(omega, delta, gamma, p[i], covarianceSalvage);
        }
        return res;
    } else if (method_ == "MonteCarlo") {
        QL_REQUIRE(mcSamples_ != Null<Size>(),
                   "ParametricVarCalculator::computeVar(): method MonteCarlo requires mcSamples");
        QL_REQUIRE(mcSeed_ != Null<Size>(),
                   "ParametricVarCalculator::computeVar(): method MonteCarlo requires mcSamples");
        return QuantExt::deltaGammaVarMc<PseudoRandom>(omega, delta, gamma, p, mcSamples_, mcSeed_, covarianceSalvage);
    } else {
        QL_FAIL("ParametricVarCalculator::computeVar(): method " << method_ << " not known.");
    }
}

void loadCovarianceDataFromCsv(std::map<std::pair<RiskFactorKey, RiskFactorKey>, Real>& data,
                               const std::string& fileName, const char delim) {
    LOG("Load Covariance Data from file " << fileName);
    ore::data::CSVFileReader reader(fileName, false);
    std::vector<std::string> dummy;
    while (reader.next()) {
        data[std::make_pair(*parseRiskFactorKey(reader.get(0), dummy), *parseRiskFactorKey(reader.get(1), dummy))] =
            ore::data::parseReal(reader.get(2));
    }
    LOG("Read " << data.size() << " valid data lines from file " << fileName);
}

} // namespace analytics
} // namespace ore


#pragma once
#ifdef WIN32
	#include <intrin.h>
#endif
#include "../matrix.hpp"
#include "../random.hpp"
#include "../path.hpp"
#include "../ulps.hpp"
#include "../check_macro.hpp"
#include "../pose.hpp"
#include <type_traits>
#include <gtest/gtest.h>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/utility.hpp>

namespace boost {
	namespace archive {
		class text_oarchive;
		class text_iarchive;
		class binary_oarchive;
		class binary_iarchive;
		class xml_oarchive;
		class xml_iarchive;
	}
}
//TODO reg128のテスト
namespace spn {
	namespace test {
		extern PathBlock g_apppath;
		template <int M, int N>
		const float* SetValue(MatT<M,N,true>& m, const float* src) {
			for(int i=0 ; i<M ; i++)
				for(int j=0 ; j<N ; j++)
					m.ma[i][j] = *src++;
			return src;
		}
		template <int M, int N>
		const float* CheckValue(MatT<M,N,true>& m, const float* src) {
			for(int i=0 ; i<M ; i++)
				for(int j=0 ; j<N ; j++)
					EXPECT_FLOAT_EQ(m.ma[i][j], *src++);
			return src;
		}
		template <int N>
		const float* SetValue(VecT<N,true>& v, const float* src) {
			for(int i=0 ; i<N ; i++)
				v.m[i] = *src++;
			return src;
		}
		template <int N>
		const float* CheckValue(VecT<N,true>& v, const float* src) {
			for(int i=0 ; i<N ; i++)
				EXPECT_FLOAT_EQ(v.m[i], *src++);
			return src;
		}
		namespace {
			#pragma GCC diagnostic push
			#pragma GCC diagnostic ignored "-Wunused-function"
			namespace test_i {
				DEF_HASTYPE(width)
				template <class T, std::enable_if_t<!std::is_floating_point<T>::value>*& = Enabler>
				void _CheckIt(const T& v0, const T& v1, std::false_type) {
					EXPECT_EQ(v0, v1);
				}
				constexpr float c_ulps(ULPs_C(1.0f, 1.0f+1e-6f));
				// (テキストにfloating pointを保存する場合は細かい値が変動する場合がある為)
				template <class T, std::enable_if_t<std::is_floating_point<T>::value>*& = Enabler>
				void _CheckIt(const T& v0, const T& v1, std::false_type) {
					EXPECT_TRUE(EqULPs(v0, v1, c_ulps));
				}
				template <class T>
				void _CheckIt(const T& v0, const T& v1, std::true_type) {
					EXPECT_TRUE(EqULPs(v0, v1, c_ulps));
				}
				template <class T>
				void CheckIt(const T& v0, const T& v1) {
					_CheckIt(v0, v1, HasType_width<T>(nullptr));
				}
				void CheckIt(const Pose3D& v0, const Pose3D& v1) {
					CheckIt(v0.getOffset(), v1.getOffset());
					CheckIt(v0.getRot(), v1.getRot());
					CheckIt(v0.getScale(), v1.getScale());
				}
				void CheckIt(const Pose2D& v0, const Pose2D& v1) {
					CheckIt(v0.getOffset(), v1.getOffset());
					CheckIt(v0.getAngle(), v1.getAngle());
					CheckIt(v0.getScale(), v1.getScale());
				}
			}
			#pragma GCC diagnostic pop
		}
		template <class OA, class IA, class T>
		void CheckSerializedData(const T& src) {
			std::stringstream buffer;
			OA oa(buffer);
			oa << BOOST_SERIALIZATION_NVP(src);
			T loaded;
			IA ia(buffer);
			ia >> boost::serialization::make_nvp("src", loaded);
			test_i::CheckIt(src, loaded);
		}
		namespace test_i {}

		template <class T>
		void CheckSerializedDataBin(const T& src) {
			CheckSerializedData<boost::archive::binary_oarchive,
								boost::archive::binary_iarchive>(src);
		}
		template <class T>
		void CheckSerializedDataText(const T& src) {
			CheckSerializedData<boost::archive::text_oarchive,
								boost::archive::text_iarchive>(src);
		}
		template <class T>
		void CheckSerializedDataXML(const T& src) {
			CheckSerializedData<boost::archive::xml_oarchive,
								boost::archive::xml_iarchive>(src);
		}

		//! non-copyableな値
		template <class T>
		struct MoveOnly {
			using move_type = T;
			T	value;
			template <class Ar>
			void serialize(Ar& /*ar*/, const unsigned int) {}

			MoveOnly(const T& v): value(v) {}
			MoveOnly(T&& v): value(std::move(v)) {}
			MoveOnly(const MoveOnly&) = delete;
			MoveOnly(MoveOnly&& m): value(std::move(m.value)) {}
			void operator = (const MoveOnly&) = delete;
			MoveOnly& operator = (MoveOnly&& m) {
				value = std::move(m.value);
				return *this;
			}
			template <class T2>
			static const T2& _getValue(const T2& t, ...) {
				return t;
			}
			template <class T3>
			static auto _getValue(const MoveOnly<T3>& t, ...) -> decltype(t.getValue()){
				return t.getValue();
			}
			auto getValue() const -> decltype(_getValue(value, nullptr)) {
				return _getValue(value, nullptr);
			}
			bool operator == (const MoveOnly& m) const {
				return getValue() == m.getValue();
			}
			bool operator != (const MoveOnly& m) const {
				return !(this->operator == (m));
			}
			bool operator < (const MoveOnly& m) const {
				return getValue() < m.getValue();
			}
			bool operator > (const MoveOnly& m) const {
				return getValue() > m.getValue();
			}
			const T& get() const {
				return value;
			}
		};
		template <class T>
		bool operator == (const T& value, const MoveOnly<T>& value1) noexcept { return value == value1.get(); }
		template <class T>
		bool operator == (const MoveOnly<T>& value, const T& value1) noexcept { return value.get() == value1; }
		template <class T>
		bool operator != (const T& value, const MoveOnly<T>& value1) noexcept { return value != value1.get(); }
		template <class T>
		bool operator != (const MoveOnly<T>& value, const T& value1) noexcept { return value.get() != value1; }
		template <class T>
		std::ostream& operator << (std::ostream& os, const MoveOnly<T>& value) {
			return os << value.getValue();
		}

		void PrintReg128(std::ostream& os, reg128 r);
		namespace {
			namespace test_i2 {
				template <class BASE>
				class RandomTestInitializer : public BASE {
					public:
						constexpr static MTRandomMgr::ID cs_rnd = 0xff;
					protected:
						void SetUp() override {
							mgr_random.initEngine(cs_rnd);
						}
						void TearDown() override {
							mgr_random.removeEngine(cs_rnd);
						}
						MTRandom getRand() {
							return mgr_random.get(cs_rnd);
						}
				};
			}
		}
		template <class T>
		class RandomTestInitializerP : public test_i2::RandomTestInitializer<::testing::TestWithParam<T>> {};
		class RandomTestInitializer : public test_i2::RandomTestInitializer<::testing::Test> {};
		namespace test_i2 {}

		#define SEQ_VECTOR (2)(3)(4)
		#define VECTOR_TYPES(z, align, elem) \
			(integralI_Vec<(elem << 4) | align>)

		// ベクトルの全パターン組み合わせ
		/*	NUM & 0x01 = Align
			NUM & 0xf0 = N */
		template <int N>
		using integralI_Vec = VecT<((N&0xf0)>>4), N&0x01>;
		using VecTP = ::testing::Types<
			BOOST_PP_SEQ_ENUM(
				BOOST_PP_SEQ_FOR_EACH(VECTOR_TYPES, 0x00, SEQ_VECTOR)
				BOOST_PP_SEQ_FOR_EACH(VECTOR_TYPES, 0x01, SEQ_VECTOR)
			)
		>;
		#undef SEQ_VECTOR
		#undef VECTOR_TYPES

		// 行列の全パターン組み合わせ
		/*	NUM & 0x001 = Align
			NUM & 0xf00 = M
			NUM & 0x0f0 = N */
		template <int N>
		using integralI_Mat = MatT<((N&0xf00)>>8), ((N&0x0f0)>>4), N&0x001>;
		#define MATRIX_TYPES(z, align, elem) \
			(integralI_Mat<(BOOST_PP_TUPLE_ELEM(0,elem) << 8) | (BOOST_PP_TUPLE_ELEM(1,elem) << 4) | align>)
		using MatTP = ::testing::Types<
			BOOST_PP_SEQ_ENUM(
				BOOST_PP_SEQ_FOR_EACH(MATRIX_TYPES, 0x00, SEQ_MATDEF)
				BOOST_PP_SEQ_FOR_EACH(MATRIX_TYPES, 0x01, SEQ_MATDEF)
			)
		>;
		#undef MATRIX_TYPES
	}
}
namespace boost {
	namespace serialization {
		template <class Ar, class T>
		inline void load_construct_data(Ar& ar, spn::test::MoveOnly<T>* mv, const unsigned int) {
			T value;
			ar	& boost::serialization::make_nvp("value", value);
			new(mv) spn::test::MoveOnly<T>(std::move(value));
		}
		template <class Ar, class T>
		inline void save_construct_data(Ar& ar, const spn::test::MoveOnly<T>* mv, const unsigned int) {
			ar	& boost::serialization::make_nvp("value", mv->value);
		}
	}
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2018 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include <iostream>

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_distance_geographic_linear_linear
#endif

#include <boost/range.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/test/included/unit_test.hpp>
#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/strategies/strategies.hpp>

#include "test_distance_geo_common.hpp"
#include "test_empty_geometry.hpp"

//===========================================================================

template <typename Point, typename Strategy>
void test_distance_segment_segment(Strategy const& strategy_ps)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "segment/segment distance tests" << std::endl;
#endif

    typedef bg::model::segment<Point> segment_type;

    typedef test_distance_of_geometries<segment_type, segment_type> tester;

    tester::apply("s-s-01",
                  "SEGMENT(0 0,1 1)",
                  "SEGMENT(2 0,3 0)",
                  ps_distance<Point>("POINT(2 0)",
                                     "SEGMENT(0 0,1 1)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("s-s-02",
                  "SEGMENT(2 1,3 1)",
                  "SEGMENT(2 0,3 0)",
                  ps_distance<Point>("POINT(2 0)",
                                     "SEGMENT(2 1,3 1)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("s-s-03",
                  "SEGMENT(2.5 1,3.5 1)",
                  "SEGMENT(2 0,3 0)",
                  ps_distance<Point>("POINT(2.5 0)",
                                     "SEGMENT(2.5 1,3.5 1)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("s-s-04",
                  "SEGMENT(2.5 1,3.5 1)",
                  "SEGMENT(2 2,3 2)",
                  ps_distance<Point>("POINT(3 2)",
                                     "SEGMENT(2.5 1,3.5 1)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("s-s-05",
                  "SEGMENT(2.5 2.1,3.5 1)",
                  "SEGMENT(2 2,3 2)",
                  0, strategy_ps, true, true, false);
    tester::apply("s-s-06",
                  "SEGMENT(2.5 2.1,3.5 1)",
                  "SEGMENT(2 2,3.5 1)",
                  0, strategy_ps, true, true, false);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_distance_segment_linestring(Strategy const& strategy_ps)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "segment/linestring distance tests" << std::endl;
#endif

    typedef bg::model::segment<Point> segment_type;
    typedef bg::model::linestring<Point> linestring_type;

    typedef test_distance_of_geometries<segment_type, linestring_type> tester;

    tester::apply("s-l-01",
                  "SEGMENT(0 0,1 1)",
                  "LINESTRING(2 0,3 0)",
                  ps_distance<Point>("POINT(2 0)",
                                     "SEGMENT(0 0,1 1)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("s-l-02",
                  "SEGMENT(0 0,1 1)",
                  "LINESTRING(2 0,3 0,2 2,0.5 0.7)",
                  ps_distance<Point>("POINT(1 1)",
                                     "SEGMENT(0.5 0.7,2 2)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("s-l-03",
                  "SEGMENT(0 0,2 2)",
                  "LINESTRING(2 0,3 0,2 2,0.5 0.7)",
                  0, strategy_ps, true, true, false);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_distance_linestring_linestring(Strategy const& strategy_ps)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "linestring/linestring distance tests" << std::endl;
#endif

    typedef bg::model::linestring<Point> linestring_type;

    typedef test_distance_of_geometries
        <
            linestring_type, linestring_type
        > tester;

    tester::apply("l-l-01",
                  "LINESTRING(0 0,1 1)",
                  "LINESTRING(2 0,3 0)",
                  ps_distance<Point>("POINT(2 0)",
                                     "SEGMENT(0 0,1 1)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("l-l-02",
                  "LINESTRING(0 0,1 1,2 2)",
                  "LINESTRING(2 0,3 0,4 1)",
                  ps_distance<Point>("POINT(2 0)",
                                     "SEGMENT(1 1,2 2)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("l-l-03",
                  "LINESTRING(0 0,1 1,2 2)",
                  "LINESTRING(2 0)",
                  ps_distance<Point>("POINT(2 0)",
                                     "SEGMENT(1 1,2 2)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("l-l-04",
                  "LINESTRING(0 0,1 1,2 2)",
                  "LINESTRING(3 3,1 1)",
                  0, strategy_ps, true, true, false);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_distance_segment_multilinestring(Strategy const& strategy_ps)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "segment/multilinestring distance tests" << std::endl;
#endif

    typedef bg::model::segment<Point> segment_type;
    typedef bg::model::linestring<Point> linestring_type;
    typedef bg::model::multi_linestring<linestring_type> multi_linestring_type;

    typedef test_distance_of_geometries
        <
            segment_type, multi_linestring_type
        > tester;

    tester::apply("s-ml-01",
                  "SEGMENT(0 0,1 1)",
                  "MULTILINESTRING((2 0,3 0)(2 5, 5 5, 2 -1))",
                  ps_distance<Point>("POINT(2 0)",
                                     "SEGMENT(0 0,1 1)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("s-ml-02",
                  "SEGMENT(0 0,1 1)",
                  "MULTILINESTRING((2 0,3 0))",
                  ps_distance<Point>("POINT(2 0)",
                                     "SEGMENT(0 0,1 1)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("s-ml-03",
                  "SEGMENT(0 0,2 0)",
                  "MULTILINESTRING((2 0,3 0)(2 5, 5 5, 2 -1))",
                  0, strategy_ps, true, true, false);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_distance_linestring_multilinestring(Strategy const& strategy_ps)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "linestring/multilinestring distance tests" << std::endl;
#endif

    typedef bg::model::linestring<Point> linestring_type;
    typedef bg::model::multi_linestring<linestring_type> multi_linestring_type;

    typedef test_distance_of_geometries
        <
            linestring_type, multi_linestring_type
        > tester;

    tester::apply("l-ml-01",
                  "LINESTRING(0 0,1 1,2 2,3 3,4 4,6 6)",
                  "MULTILINESTRING((2 0,3 0)(2 1, 5 5))",
                  ps_distance<Point>("POINT(5 5)",
                                     "SEGMENT(4 4,6 6)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("l-ml-02",
                  "LINESTRING(0 0,1 1,2 2,3 3,4 4,6 6)",
                  "MULTILINESTRING((2 0,3 0)(2 5, 5 5))",
                  0,
                  strategy_ps, true, true, false);

}

//===========================================================================

template <typename Point, typename Strategy>
void test_distance_multilinestring_multilinestring(Strategy const& strategy_ps)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multilinestring/multilinestring distance tests" << std::endl;
#endif

    typedef bg::model::linestring<Point> linestring_type;
    typedef bg::model::multi_linestring<linestring_type> multi_linestring_type;

    typedef test_distance_of_geometries
        <
            multi_linestring_type, multi_linestring_type
        > tester;

    tester::apply("s-ml-01",
                  "MULTILINESTRING((0 0,1 1)(-1 0, -1 -1))",
                  "MULTILINESTRING((2 0,3 0)(2 5, 5 5))",
                  ps_distance<Point>("POINT(2 0)",
                                     "SEGMENT(0 0,1 1)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("s-ml-02",
                  "MULTILINESTRING((0 0,1 1)(-1 0, -1 -1)(5 0, 5 6))",
                  "MULTILINESTRING((2 0,3 0)(2 5, 5 5))",
                  0,
                  strategy_ps, true, true, false);
}

//===========================================================================
//===========================================================================
//===========================================================================

template <typename Point, typename Strategy>
void test_all_l_l(Strategy ps_strategy)
{
    test_distance_segment_segment<Point>(ps_strategy);
    test_distance_segment_linestring<Point>(ps_strategy);
    test_distance_linestring_linestring<Point>(ps_strategy);
    test_distance_segment_multilinestring<Point>(ps_strategy);
    test_distance_linestring_multilinestring<Point>(ps_strategy);
    test_distance_multilinestring_multilinestring<Point>(ps_strategy);

    test_more_empty_input_linear_linear<Point>(ps_strategy);
}

BOOST_AUTO_TEST_CASE( test_all_linear_linear )
{
    typedef bg::model::point
            <
                double, 2,
                bg::cs::spherical_equatorial<bg::degree>
            > sph_point;

    test_all_l_l<sph_point>(spherical_ps());

    typedef bg::model::point
            <
                double, 2,
                bg::cs::geographic<bg::degree>
            > geo_point;

    test_all_l_l<geo_point>(vincenty_ps());
    test_all_l_l<geo_point>(thomas_ps());
    test_all_l_l<geo_point>(andoyer_ps());
}


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IF_ELSE_ALLBITS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IF_ELSE_ALLBITS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-boolean
    This function object conditionally returns @c x (respectively @ref Allbits) if @c c is @ref True
    (respectively  @ref False)

    @par Header <boost/simd/function/if_else_allbits.hpp>

    @par Example:

      @snippet if_else_allbits.cpp if_else_allbits

    @par Possible output:

      @snippet if_else_allbits.txt if_else_allbits

    @par Alias:
     if_else_nan

  **/
  Value1 if_else_allbits(Value0 const& c, Value1 const& x);
} }
#endif

#include <boost/simd/function/scalar/if_else_allbits.hpp>
#include <boost/simd/function/simd/if_else_allbits.hpp>

#endif


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

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

     Represents the is_positive function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct is_positive_ : ext::elementwise_<is_positive_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<is_positive_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_is_positive_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };}
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::is_positive_, Site> dispatching_is_positive_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::is_positive_, Site>();
   }
   template<class... Args>
   struct impl_is_positive_;
  }
  /*!
    Returns True if a0 is positive else False.

    This function differs from is_gtz from floating point argument,
    because Zero is positive but not greater than zero, and Mzero is
    not positive and not greater than zero, It's probably is_gtz that
    you want.

    @par Semantic:

    @code
    logical<T> r = is_positive(a0);
    @endcode

    is similar to:

    @code
    if T is signed
      logical<T> r = bitofsign(a0) == 0;
    else
      logical<T> r = True;
    @endcode

    @par Note:

    Mzero is the floating point 'minus zero',
    i.e. all bits are zero but the sign bit.
    Such a value is treated as zero by ieee standards.

    @param a0

    @return a logical value
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::is_positive_, is_positive, 1)
} }

#endif


#include "../src/hash.h"
#include "../src/point.h"

#define BOOST_TEST_MODULE ClassTest
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <map>
#include <algorithm>
#include <string>

BOOST_AUTO_TEST_CASE(GridHash_Test) {
  GridHash gh;
  gh.double_r();
  gh.double_r();
  gh.double_r();
  gh.double_r();
  std::map<int, int> ct;
  for (int i = 0; i < gh.get_R(); ++i) {
    ct[i] = 0;
  }
  for (int i = 0; i < 10000; i++) {
    int d = gh(i * i);
    ct[d]++;
  }
  std::vector<int> cc;
  for (auto p : ct) {
    cc.push_back(p.second);
  }

  auto mi = *std::min_element(cc.begin(), cc.end());
  auto ma = *std::max_element(cc.begin(), cc.end());
  BOOST_CHECK_MESSAGE(1.4 * mi > ma, std::to_string(mi) + " " << std::to_string(ma) );
}


#include <fstream>
#include <iostream>

#include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>

#include <yaml-cpp/yaml.h>

#include <libMultiRobotPlanning/JPSSIPP.hpp>
#include <libMultiRobotPlanning/JPSSIPPAN.hpp>
#include <libMultiRobotPlanning/sipp.hpp>
#include "timer.hpp"

using libMultiRobotPlanning::JPSSIPP;
using libMultiRobotPlanning::JPSSIPPAN;
using libMultiRobotPlanning::SIPP;
using libMultiRobotPlanning::Neighbor;
using libMultiRobotPlanning::PlanResult;

enum class Action {
  Up  ,
  Down  ,
  Left  ,
  Right ,
  Wait  ,
  All   ,
};

struct State {
  State(int x, int y) : x(x), y(y){}

  bool operator==(const State& other) const {
    return std::tie(x, y) == std::tie(other.x, other.y);
  }

  bool operator!=(const State& other) const {
    return std::tie(x, y) != std::tie(other.x, other.y);
  }

  bool operator<(const State& other) const {
    return std::tie(x, y) < std::tie(other.x, other.y);
  }

  friend std::ostream& operator<<(std::ostream& os, const State& s) {
    return os << "(" << s.x << "," << s.y << ")";
  }

  int x;
  int y;
//  Action a;
};

namespace std {
template <>
struct hash<State> {
  size_t operator()(const State& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    return seed;
  }
};
}  // namespace std

std::ostream& operator<<(std::ostream& os, const Action& a) {
  switch (a) {
    case Action::Up:
      os << "Up";
      break;
    case Action::Down:
      os << "Down";
      break;
    case Action::Left:
      os << "Left";
      break;
    case Action::Right:
      os << "Right";
      break;
    case Action::Wait:
      os << "Wait";
      break;
    case Action::All:
    	os << "All";
  }
  return os;
}

class Environment {
 public:
  Environment(size_t dimx, size_t dimy, std::unordered_set<State> obstacles, std::unordered_set<State> m_jumppoint,
              std::vector<std::vector<bool>>jump_point_map, State goal)
      : m_dimx(dimx),
        m_dimy(dimy),
        m_obstacles(std::move(obstacles)),
		m_jumppoint(std::move(m_jumppoint)),
		jump_point_map(std::move(jump_point_map)),
        m_goal(goal) {}

  float admissibleHeuristic(const State& s) {
    return std::abs(s.x - m_goal.x) + std::abs(s.y - m_goal.y);
  }

  bool isSolution(const State& s) { return s == m_goal; }

  State getLocation(const State& s) { return s; }

  void getNeighbors(const State& s,
                    std::vector<Neighbor<State, Action, int> >& neighbors) {
    neighbors.clear();

    State left(s.x - 1, s.y);
    if (stateValid(left)) {
      neighbors.emplace_back(
          Neighbor<State, Action, int>(left, Action::Left, 1));
    }
    State right(s.x + 1, s.y);
    if (stateValid(right)) {
      neighbors.emplace_back(
          Neighbor<State, Action, int>(right, Action::Right, 1));
    }

    State up(s.x, s.y + 1);
    if (stateValid(up)) {
      neighbors.emplace_back(Neighbor<State, Action, int>(up, Action::Up, 1));
    }
    State down(s.x, s.y - 1);
    if (stateValid(down)) {
      neighbors.emplace_back(
          Neighbor<State, Action, int>(down, Action::Down, 1));
    }
  }

  void onExpandNode(const State& /*s*/, int /*fScore*/, int /*gScore*/) {
    // std::cout << "expand: " << s << "g: " << gScore << std::endl;
  }

  void onDiscover(const State& /*s*/, int /*fScore*/, int /*gScore*/) {
    // std::cout << "  discover: " << s << std::endl;
  }

  bool isCommandValid(
      const State& /*s1*/, const State& /*s2*/, const Action& /*a*/,
      int earliestStartTime,      // can start motion at this time
      int /*latestStartTime*/,    // must have left s by this time
      int earliestArrivalTime,    // can only arrive at (s+cmd)
      int /*latestArrivalTime*/,  // needs to arrive by this time at (s+cmd)
      int& t, const int& cost_c) {
    t = std::max<int>(earliestArrivalTime, earliestStartTime + cost_c);

    // TODO(whoenig): need to check for swaps here...

    // return t - 1 <= latestStartTime;
    return true;
  }

  bool isCommandValid(
      const State& /*s1*/, const State& /*s2*/, const Action& /*a*/,
      int earliestStartTime,      // can start motion at this time
      int /*latestStartTime*/,    // must have left s by this time
      int earliestArrivalTime,    // can only arrive at (s+cmd)
      int /*latestArrivalTime*/,  // needs to arrive by this time at (s+cmd)
      int& t) {
    t = std::max<int>(earliestArrivalTime, earliestStartTime + 1);

    // TODO(whoenig): need to check for swaps here...

    // return t - 1 <= latestStartTime;
    return true;
  }

 public:
  bool stateValid(const State& s) {
    return s.x >= 0 && s.x < m_dimx && s.y >= 0 && s.y < m_dimy &&
           m_obstacles.find(s) == m_obstacles.end();
  }

  bool isObstacle(const State& s){
	  return s.x >= 0 && s.x < m_dimx && s.y >= 0 && s.y < m_dimy &&
	           m_obstacles.find(s) != m_obstacles.end();
  }

  bool isJumpPoint(const State& s) {
	  return jump_point_map[s.x][s.y];
  }

  void setJumpPoint(const State& s){
	  jump_point_map[s.x][s.y] = true;
	  m_jumppoint.insert(s);
  }

  void setJPS(){
	  is_jps = true;
  }
  void setNoJPS(){
	  is_jps = false;
  }
  bool isJPS(){
	  return is_jps;
  }

  void Reset(){
	  num_generation = 0;
	  num_expansion =0;
  }
 public:
  int num_generation = 0;
  int num_expansion = 0;
  int limit_jump = 5;
  std::vector<std::vector<bool>> jump_point_map;
  bool is_limit = false;

 private:
  int m_dimx;
  int m_dimy;
  std::unordered_set<State> m_obstacles;
  std::unordered_set<State> m_jumppoint;
  State m_goal;
  bool is_jps = true;
};

int main(int argc, char* argv[]) {
  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");
  std::string inputFile;
  std::string outputFile;
  std::string res;
  desc.add_options()("help", "produce help message")(
      "input,i", po::value<std::string>(&inputFile)->required(),
      "input file (YAML)")("output,o",
                           po::value<std::string>(&outputFile)->required(),
                           "output file (YAML)")
						   //("results, txt", po::value<std::string>(&res)->required(), "results file (TXT)")
      // ("url",
      // po::value<std::string>(&url)->default_value("http://0.0.0.0:8080"),
      // "server URL")
      ;

  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") != 0u) {
      std::cout << desc << "\n";
      return 0;
    }
  } catch (po::error& e) {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  std::fstream res_sta("Berlin_all_8_8_3", std::ios::app);
  std::fstream res_Good("Berlin_all_8_8_good_10", std::ios::app);

  // Configure SIPP based on config file
  YAML::Node config = YAML::LoadFile(inputFile);

  std::unordered_set<State> obstacles;
  std::unordered_set<State> jumppoint;
  std::vector<State> goals;
  std::vector<State> startStates;
  std::vector<std::vector<bool>> map_1;

  const auto& dim = config["map"]["dimensions"];
  int dimx = dim[0].as<int>();
  int dimy = dim[1].as<int>();

  for (const auto& node : config["map"]["obstacles"]) {
     obstacles.insert(State(node[0].as<int>(), node[1].as<int>()));
  }

  map_1.resize(dimx + 2);
  for(int i = 0; i < map_1.size();i++){
	  map_1[i].resize(dimy + 2);
  }

  for (const auto& ob:obstacles){
	  State temp1 = ob,temp2 = ob;
	  temp1.x = ob.x + 1;
	  temp2.y = ob.y + 1;

	  if(temp1.x >= 0 && temp1.x <= dimx && temp2.y >=0 && temp2.y <= dimy
			  && obstacles.find(temp1)==obstacles.end() && obstacles.find(temp2) == obstacles.end()){
		  jumppoint.insert(State(ob.x + 1, ob.y + 1));
		  map_1[ob.x + 1][ob.y + 1] = true;
	  }

	  temp1 = ob; temp2 = ob;
	  temp1.x = ob.x + 1;
	  temp2.y = ob.y - 1;
	  if(temp1.x >= 0 && temp1.x <= dimx && temp2.y >=0 && temp2.y <= dimy
			  && obstacles.find(temp1)==obstacles.end() && obstacles.find(temp2) == obstacles.end()){
		  jumppoint.insert(State(ob.x + 1, ob.y - 1));
		  map_1[ob.x + 1][ob.y - 1] = true;
	  }

	  temp1 = ob; temp2 = ob;
	  temp1.x = ob.x - 1;
	  temp2.y = ob.y + 1;
	  if(temp1.x >= 0 && temp1.x <= dimx && temp2.y >=0 && temp2.y <= dimy
			  && obstacles.find(temp1)==obstacles.end() && obstacles.find(temp2) == obstacles.end()){
		  jumppoint.insert(State(ob.x - 1, ob.y + 1));
		  map_1[ob.x - 1][ob.y + 1] = true;
	  }

	  temp1 = ob; temp2 = ob;
	  temp1.x = ob.x - 1;
	  temp2.y = ob.y - 1;
	  if(temp1.x >= 0 && temp1.x <= dimx && temp2.y >=0 && temp2.y <= dimy
			  && obstacles.find(temp1)==obstacles.end() && obstacles.find(temp2) == obstacles.end()){
		  jumppoint.insert(State(ob.x - 1, ob.y - 1));
		  map_1[ob.x - 1][ob.y - 1] = true;
	  }

  }

  for (const auto& node : config["agents"]) {
	  const auto& start = node["start"];
	  const auto& goal = node["goal"];
	  startStates.emplace_back(State(start[0].as<int>(), start[1].as<int>()));
	  goals.emplace_back(State(goal[0].as<int>(), goal[1].as<int>()));
  }
  typedef JPSSIPP<State, State, Action, int, Environment> jps_sipp;
  typedef JPSSIPPAN<State, State, Action, int, Environment> jps_an_sipp;
  typedef SIPP<State, State, Action, int, Environment> sipp_t;

  std::ofstream out(outputFile);
  out << "schedule:" << std::endl;

  // Plan (sequentially)
  std::map<State, std::vector<jps_sipp::interval>> allCollisionIntervals;
  std::map<State, std::vector<jps_sipp::edgeCollision>> allEdgeCollisions;

  std::map<State, std::vector<jps_an_sipp::interval>> allCollisionIntervals_jpsan;
  std::map<State, std::vector<jps_an_sipp::edgeCollision>> allEdgeCollisions_jpsan;

  std::map<State, std::vector<sipp_t::interval>> allCollisionIntervals_sipp;
  std::map<State, std::vector<sipp_t::edgeCollision>> allEdgeCollisions_sipp;

//  allCollisionIntervals[State(1,1)].push_back(jps_sipp::interval(5, 5));
//  allCollisionIntervals[State(1,1)].push_back(jps_sipp::interval(10, 15));
//  allCollisionIntervals[State(2,1)].push_back(jps_sipp::interval(10, 16));

//  allCollisionIntervals_sipp[State(1,1)].push_back(sipp_t::interval(5, 5));
//  allCollisionIntervals_sipp[State(1,1)].push_back(sipp_t::interval(10, 15));
//  allCollisionIntervals_sipp[State(2,1)].push_back(sipp_t::interval(10, 16));

  long cost = 0;
  for (size_t i = 0; i < goals.size(); ++i) {
    std::cout << "Planning for agent " << i << std::endl;
    out << "  agent" << i << ":" << std::endl;

    Environment env(dimx, dimy, obstacles, jumppoint, map_1, goals[i]);
    jps_sipp jpssipp(env);
    jps_an_sipp jps_an_sipp(env);
    sipp_t sipp(env);

    for (const auto& collisionIntervals : allCollisionIntervals) {
      jpssipp.setCollisionIntervals(collisionIntervals.first, collisionIntervals.second);
      State current_state = collisionIntervals.first;
/*      if(env.stateValid(State(current_state.x + 1, current_state.y))){
    	  env.setJumpPoint(State(current_state.x + 1, current_state.y));
      }

      if(env.stateValid(State(current_state.x - 1, current_state.y))){
    	  env.setJumpPoint(State(current_state.x - 1, current_state.y));
      }

      if(env.stateValid(State(current_state.x, current_state.y - 1))){
    	  env.setJumpPoint(State(current_state.x, current_state.y - 1));
      }

      if(env.stateValid(State(current_state.x, current_state.y + 1))){
    	  env.setJumpPoint(State(current_state.x, current_state.y + 1));
      }*/

      if(env.stateValid(State(current_state.x + 1, current_state.y - 1))){
    	  env.setJumpPoint(State(current_state.x + 1, current_state.y - 1));
      }

      if(env.stateValid(State(current_state.x - 1, current_state.y - 1))){
    	  env.setJumpPoint(State(current_state.x - 1, current_state.y - 1));
      }

      if(env.stateValid(State(current_state.x + 1, current_state.y + 1))){
    	  env.setJumpPoint(State(current_state.x + 1, current_state.y + 1));
      }

      if(env.stateValid(State(current_state.x - 1, current_state.y + 1))){
    	  env.setJumpPoint(State(current_state.x - 1, current_state.y + 1));
      }
    }

    for (const auto& ec: allEdgeCollisions) {
    	jpssipp.setEdgeCollisions(ec.first, ec.second);
    }

    for (const auto& collisionIntervals_jpsan : allCollisionIntervals_jpsan) {
       jps_an_sipp.setCollisionIntervals(collisionIntervals_jpsan.first, collisionIntervals_jpsan.second);
    }
    for (const auto& ec: allEdgeCollisions_jpsan) {
    	jps_an_sipp.setEdgeCollisions(ec.first, ec.second);
    }
    for (const auto& collisionIntervals_sipp : allCollisionIntervals_sipp) {
       sipp.setCollisionIntervals(collisionIntervals_sipp.first, collisionIntervals_sipp.second);
    }
    for (const auto& ec: allEdgeCollisions_sipp) {
    	sipp.setEdgeCollisions(ec.first, ec.second);
    }

    env.Reset();
    // Plan
    PlanResult<State, Action, int> solution;
    Timer t;
    t.reset();
    bool success = jpssipp.search(startStates[i], Action::Wait, solution);
    t.stop();
    std::cout<< t.elapsedSeconds() << std::endl;
    int num_expansion1 = env.num_expansion;
    int num_generation1 = env. num_generation;
    double time1 = t.elapsedSeconds();
    if (success) {
      std::cout << "JPS Planning successful! Total cost: " << solution.cost << " Expansion:"
    		    << env.num_expansion << " Generation: " << env.num_generation
                << std::endl;

      // print solution
      for (size_t i = 0; i < solution.actions.size(); ++i) {
        std::cout << solution.states[i].second << ": " << solution.states[i].first
                  << "->" << solution.actions[i].first
                  << "(cost: " << solution.actions[i].second << ")" << std::endl;
      }
      std::cout << solution.states.back().second << ": "
                << solution.states.back().first << std::endl;

      for (size_t i = 0; i < solution.states.size(); ++i) {
        out << "    - x: " << solution.states[i].first.x << std::endl
            << "      y: " << solution.states[i].first.y << std::endl
            << "      t: " << solution.states[i].second << std::endl;
      }
    } else {
      std::cout << "Planning NOT successful!" << std::endl;
      out << "    []" << std::endl;
    }

    env.Reset();
    // Plan
    PlanResult<State, Action, int> solution3;
    t.reset();
    bool success3 = jps_an_sipp.search(startStates[i], Action::Wait, solution3);
    t.stop();
    std::cout<< t.elapsedSeconds() << std::endl;
    int num_expansion3 = env.num_expansion;
    int num_generation3 = env. num_generation;
    double time3 = t.elapsedSeconds();
/*    if (success3) {
      std::cout << "JPSAN Planning successful! Total cost: " << solution3.cost << " Expansion:"
    		    << env.num_expansion << " Generation: " << env.num_generation
                << std::endl;

      // print solution
      for (size_t i = 0; i < solution3.actions.size(); ++i) {
        std::cout << solution3.states[i].second << ": " << solution3.states[i].first
                  << "->" << solution3.actions[i].first
                  << "(cost: " << solution3.actions[i].second << ")" << std::endl;
      }
      std::cout << solution3.states.back().second << ": "
                << solution3.states.back().first << std::endl;

      for (size_t i = 0; i < solution3.states.size(); ++i) {
        out << "    - x: " << solution3.states[i].first.x << std::endl
            << "      y: " << solution3.states[i].first.y << std::endl
            << "      t: " << solution3.states[i].second << std::endl;
      }
    } else {
      std::cout << "Planning NOT successful!" << std::endl;
      out << "    []" << std::endl;
    }
*/

    env.Reset();
    env.setNoJPS();
    PlanResult<State, Action, int> solution2;
    t.reset();
    bool success_temp = sipp.search(startStates[i], Action::Wait, solution2);
    t.stop();

     int num_expansion2 = env.num_expansion;
     int num_generation2 = env.num_generation;

     double time2 = t.elapsedSeconds();
     std::cout<< t.elapsedSeconds() << std::endl;

    if (success_temp) {
      std::cout << "NoJPS Planning successful! Total cost: " << solution2.cost << " Expansion:"
    		    << env.num_expansion << " Generation: " << env.num_generation
                << std::endl;

      // update collision intervals
      auto lastState = solution2.states[0];
      for (size_t i = 1; i < solution2.states.size(); ++i) {
        if (solution2.states[i].first != lastState.first) {
          allCollisionIntervals[lastState.first].push_back(
            jps_sipp::interval(lastState.second, solution2.states[i].second - 1));
          allCollisionIntervals_sipp[lastState.first].push_back(
            sipp_t::interval(lastState.second, solution2.states[i].second - 1));
          allCollisionIntervals_jpsan[lastState.first].push_back(
            jps_an_sipp::interval(lastState.second, solution2.states[i].second - 1));

          lastState = solution2.states[i];
        }
      }
      allCollisionIntervals[solution2.states.back().first].push_back(
            jps_sipp::interval(solution2.states.back().second, std::numeric_limits<int>::max()));
      allCollisionIntervals_sipp[solution2.states.back().first].push_back(
            sipp_t::interval(solution2.states.back().second, std::numeric_limits<int>::max()));

      allCollisionIntervals_jpsan[solution2.states.back().first].push_back(
            jps_an_sipp::interval(solution2.states.back().second, std::numeric_limits<int>::max()));
//      std::cout << ""
      // update statistics
      cost += solution2.cost;
      // print solution
      for (size_t i = 0; i < solution2.actions.size(); ++i) {
    	  if(solution2.actions[i].first != Action::Wait ){
    		  allEdgeCollisions[solution2.states[i].first].push_back(jps_sipp::edgeCollision(solution2.states[i].second,solution2.actions[i].first));
    	  }
    	  if(solution2.actions[i].first != Action::Wait ){
    	      allEdgeCollisions_sipp[solution2.states[i].first].push_back(sipp_t::edgeCollision(solution2.states[i].second,solution2.actions[i].first));
    	   }

    	  if(solution2.actions[i].first != Action::Wait ){
    	      allEdgeCollisions_jpsan[solution2.states[i].first].push_back(jps_an_sipp::edgeCollision(solution2.states[i].second,solution2.actions[i].first));
    	   }
        std::cout << solution2.states[i].second << ": " << solution2.states[i].first
                  << "->" << solution2.actions[i].first
                  << "(cost: " << solution2.actions[i].second << ")" << std::endl;
      }
      std::cout << solution2.states.back().second << ": "
                << solution2.states.back().first << std::endl;

      for (size_t i = 0; i < solution2.states.size(); ++i) {
        out << "    - x: " << solution2.states[i].first.x << std::endl
            << "      y: " << solution2.states[i].first.y << std::endl
            << "      t: " << solution2.states[i].second << std::endl;
      }
    } else {
      std::cout << inputFile  <<  " SIPP Planning NOT successful from Agent: " << i << std::endl;
      out << "    []" << std::endl;
      break;
    }

    if(solution.cost != solution2.cost){
    	std::cout << inputFile <<  "Agent " << i << ": Not equal1" << std::endl;
    	break;
    }

    if(solution.cost != solution3.cost){
    	std::cout << inputFile <<  "Agent " << i << ": Not equal" << std::endl;
    	break;
    }

    if(num_expansion1 > num_expansion2){
    	std::cout << inputFile << " Not Expansion-1 Agent " << i << " " << num_expansion1 << " " << num_expansion1 - num_expansion2 << "\n";
    }

    if(num_expansion3 > num_expansion2){
    	std::cout << inputFile << " Not Expansion-3 Agent " << i << " " << num_expansion3 << " " << num_expansion3 - num_expansion2 << "\n";
    }

    if(num_expansion1 > num_expansion2){
    	std::cout << inputFile << " Not Expansion-1 Agent " << i << " " << num_expansion1 << " " << num_expansion1 - num_expansion2 << "\n";
    }

    if(num_expansion3 > num_expansion2){
    	std::cout << inputFile << " Not Expansion-3 Agent " << i << " " << num_expansion3 << " " << num_expansion3 - num_expansion2 << "\n";
    }

    if(num_generation1 > 5*num_generation2){
    	std::cout << inputFile << " Not Generation-1 Agent " << i << " " << num_generation1 << " " << num_generation2 << " " << num_generation1 - num_generation2 << "\n";
    }

/*    if(num_generation3 > num_generation2){
    	std::cout << inputFile << " Not Generation-3 Agent " << i << " " << num_generation3 << " " << num_generation3 - num_generation2 << "\n";
    }*/
    std::cout << inputFile << " All-Generation Agent " << i << " " << num_generation1  << " " << num_generation2 << "\n";
    std::cout << inputFile << " All-Expansion Agent " << i << " " << num_expansion1  << " " << num_expansion2 << "\n";

//    if(num_generation1 - num_generation3 > 20){
//    	std::cout << inputFile << " Not Generation-1 Agent " << i << " "<< num_generation1 - num_generation3 << "\n";
//    }

/*    if(num_expansion1 > num_expansion3){
    	std::cout << inputFile << " Not Expansion-3 Agent " << i << " "<< num_expansion1 - num_expansion3 << "\n";
    }

    if(num_expansion1 < num_expansion3){
    	std::cout << inputFile << " Not Expansion-4 Agent " << i << " "<< num_expansion1 - num_expansion3 << "\n";
    }

    if(num_generation1 > num_generation2){
    	std::cout << inputFile << " Not Generation-1 Agent " << i << " "<< num_generation1 - num_generation2 << "\n";
    }
*/
    res_sta << inputFile << " Agent " << i << " JPSSIPP: " << " cost: " << solution.cost << " " << time1 << " " << num_expansion1 << " "
    		<< num_generation1 << " " << num_expansion1 - num_expansion2 <<"\n";
    res_sta << inputFile << " Agent " << i << " JPSSIPPAN: " << " cost: " << solution3.cost << " " << time3 << " " << num_expansion3 << " " << num_generation3 <<"\n";
    res_sta << inputFile << " Agent " << i << " SIPP: " << " cost: " << solution2.cost << " " <<time2 << " " << num_expansion2 << " " << num_generation2 <<"\n";

    if (time1 < time2){
        res_Good << inputFile << " Agent " << i << " JPSSIPP: "  <<" cost " << solution.cost << " " << time1 << " " << num_expansion1 << " " << num_generation1 <<"\n";
        res_Good << inputFile << " Agent " << i << " SIPP: "  <<" cost " << solution.cost << " "<< time2 << " " << num_expansion2 << " " << num_generation2 <<"\n";
    }
//   if(i == 5) break;
//    break;
  }

  out << "statistics:" << std::endl;
  out << "  cost: " << cost << std::endl;
  // out << "  makespan: " << makespan << std::endl;
  // out << "  runtime: " << timer.elapsedSeconds() << std::endl;

  // for (const auto& node : config["environment"]["collisionIntervals"]) {
  //   State state(node["location"][0].as<int>(), node["location"][1].as<int>());

  //   std::vector<jps_sipp::interval> collisionIntervals;

  //   for (const auto& interval : node["intervals"]) {
  //     collisionIntervals.emplace_back(
  //         jps_sipp::interval(interval[0].as<int>(), interval[1].as<int>()));
  //   }
  //   sipp.setCollisionIntervals(state, collisionIntervals);
  // }

  return 0;
}


#ifndef Z_PARAMETERS_HPP
#define Z_PARAMETERS_HPP

//STL
#include <string>
#include <vector>

//Eigen
#include <Eigen/Dense>

//Boost
#include <boost/smart_ptr/shared_ptr.hpp>

//Holds shared config parameter values
struct Parameters
{
    //Display
    bool verbose;

    //Synchronization
    float max_time_diff;

    //Segmentation
    bool visualize;

    float sigma_s;
    float sigma_r;

    float max_change_factor;
    float smoothing_size;

    float inliers_ratio;
    float angular_th;
    float distance_th;

    //Calibration
    float max_angle;
    float max_distance;

    float approx_rotation;
    float approx_translation;

    std::string rawlog_file;
    std::string output_dir;

    std::vector< std::string > sensor_labels;
    std::vector< Eigen::Matrix4f > init_pose;
};

typedef boost::shared_ptr<Parameters> ParametersPtr;

#endif


#ifndef VOESTIMATOR_HPP_
#define VOESTIMATOR_HPP_

#include <iostream>
#include <stdio.h>
#include <signal.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>

#include <lcm/lcm-cpp.hpp>
#include <bot_frames/bot_frames.h>
#include <bot_frames_cpp/bot_frames_cpp.hpp>
#include <lcmtypes/bot_core.hpp>

#include <pronto_utils/pronto_vis.hpp> // visualize pt clds

// A simple class which maintains an estimate of a head position
// and can publish it to LCM
class VoEstimator
{
public:
  VoEstimator(boost::shared_ptr<lcm::LCM> &lcm_, BotFrames* botframes_,
              std::string channel_extension_ = "");
  ~VoEstimator();

  void updatePosition(int64_t utime, int64_t utime_prev, Eigen::Isometry3d delta_camera);

  Eigen::Isometry3d getCameraPose(){ return local_to_head_*camera_to_head_.inverse(); }
  Eigen::Isometry3d getHeadPose(){ return local_to_head_; }
  void setHeadPose(Eigen::Isometry3d local_to_head_in){
    local_to_head_ = local_to_head_in;
    pose_initialized_ = true;
  }

  Eigen::Vector3d getHeadLinearRate(){ return head_lin_rate_; }
  Eigen::Vector3d getHeadRotationRate(){ return head_rot_rate_; }

  void publishPose(int64_t utime, std::string channel, Eigen::Isometry3d pose,
                   Eigen::Vector3d vel_lin, Eigen::Vector3d vel_ang);
  void publishUpdate(int64_t utime, Eigen::Isometry3d local_to_head, std::string channel, bool output_alpha_filter);

private:
  boost::shared_ptr<lcm::LCM> lcm_;
  pronto_vis* pc_vis_;

  // have we received the first pose estimate:?
  bool pose_initialized_;
  bool vo_initialized_;

  BotFrames* botframes_;
  bot::frames* botframes_cpp_;

  std::string channel_extension_;
  Eigen::Isometry3d camera_to_head_;
  Eigen::Isometry3d local_to_head_;

  Eigen::Isometry3d local_to_head_prev_;
  Eigen::Isometry3d delta_head_prev_;

  // Cache of rates: All are stored as RPY
  Eigen::Vector3d head_rot_rate_, head_lin_rate_;
  Eigen::Vector3d head_rot_rate_alpha_, head_lin_rate_alpha_;

};

#endif