#include <stdlib.h>
#include <boost/format.hpp>
#include <iostream>

#include <pcl/common/transforms.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/passthrough.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/surface/marching_cubes.h>
#include <pcl/surface/marching_cubes_hoppe.h>
#include <pcl/surface/marching_cubes_rbf.h>
#include <pcl/surface/poisson.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/surface/impl/texture_mapping.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h>

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

#include <opencv2/core/eigen.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/opencv.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <pcl/surface/mls.h>
#include <pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h>
#include <pcl/surface/vtk_smoothing/vtk_utils.h>
#include <pcl/visualization/common/actor_map.h>
#include <vtkFillHolesFilter.h>

#include "Frame3D/Frame3D.h"

#include <algorithm>  // std::max
#include <sys/stat.h> // mkdir
#include <sstream>

using namespace std;

using Pcl = pcl::PointCloud<pcl::PointXYZRGBNormal>;

boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud);

pcl::PointCloud<pcl::PointXYZ>::Ptr mat2IntegralPointCloud(const cv::Mat &depth_mat, const float focal_length,
                                                           const float max_depth);

pcl::PointCloud<pcl::PointXYZRGB>::Ptr mat2IntegralPointCloudRGB(const cv::Mat &depth_mat, const cv::Mat &rgb_image,
                                                                 const float focal_length, const float max_depth);

pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsWithTexture(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud) {
  // This function computes normals given a point cloud
  // !! Please note that you should remove NaN values from the pointcloud after
  // computing the surface normals.
  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);  // Output
                                                                                // datasets
  pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne;
  ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);
  ne.setMaxDepthChangeFactor(0.02f);
  ne.setNormalSmoothingSize(10.0f);
  ne.setInputCloud(cloud);
  ne.compute(*cloud_normals);
  pcl::copyPointCloud(*cloud, *cloud_normals);
  return cloud_normals;
}

pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
  // This function computes normals given a point cloud
  // !! Please note that you should remove NaN values from the pointcloud after computing the surface normals.
  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals(new pcl::PointCloud<pcl::PointNormal>);  // Output datasets
  pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
  ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);
  ne.setMaxDepthChangeFactor(0.02f);
  ne.setNormalSmoothingSize(10.0f);
  ne.setInputCloud(cloud);
  ne.compute(*cloud_normals);
  pcl::copyPointCloud(*cloud, *cloud_normals);
  return cloud_normals;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                                                           const Eigen::Matrix4f &transform) {
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
  pcl::transformPointCloud(*cloud, *transformed_cloud, transform);
  return transformed_cloud;
}

template <class T>
typename pcl::PointCloud<T>::Ptr transformPointCloudNormals(typename pcl::PointCloud<T>::Ptr cloud,
                                                            const Eigen::Matrix4f &transform) {
  typename pcl::PointCloud<T>::Ptr transformed_cloud(new typename pcl::PointCloud<T>());
  pcl::transformPointCloudWithNormals(*cloud, *transformed_cloud, transform);
  return transformed_cloud;
}

pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergingPointClouds(Frame3D frames[], float depth_threshold) {
  Pcl::Ptr modelCloud(new Pcl());

  for (int i = 0; i < 8; i++) {
    std::cout << boost::format("Merging frame %d") % i << std::endl;

    Frame3D frame = frames[i];
    cv::Mat depthImage = frame.depth_image_;
    double focalLength = frame.focal_length_;
    const Eigen::Matrix4f cameraPose = frame.getEigenTransform();

    // TODO(Student): Merge the i-th frame using predicted camera pose
    // to the global point cloud. ~ 20 lines.

    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud = mat2IntegralPointCloud(depthImage, focalLength, depth_threshold);

    pcl::PointCloud<pcl::PointNormal>::Ptr point_cloud_with_normals = computeNormals(point_cloud);

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr new_pcl(new pcl::PointCloud<pcl::PointXYZRGBNormal>());

    new_pcl->reserve(point_cloud_with_normals->size());
    for (auto &&pt : *point_cloud_with_normals) {
      if (!isnan(pt.normal_x) && !isnan(pt.normal_y) && !isnan(pt.normal_z)) {
        pcl::PointXYZRGBNormal new_pt;
        new_pt.x = pt.x;
        new_pt.y = pt.y;
        new_pt.z = pt.z;
        new_pt.normal_x = pt.normal_x;
        new_pt.normal_y = pt.normal_y;
        new_pt.normal_z = pt.normal_z;
        new_pt.b = new_pt.r = new_pt.g = 128;

        new_pcl->push_back(new_pt);
      }
    }

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformed_point_cloud = transformPointCloudNormals<pcl::PointXYZRGBNormal>(new_pcl, cameraPose);

    *modelCloud += (*transformed_point_cloud);
  }

  return modelCloud;
}

pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergingPointCloudsWithTexture(Frame3D frames[], float depth_threshold) {
  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr modelCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);

  for (int i = 0; i < 8; i++) {
    std::cout << boost::format("Merging frame %d") % i << std::endl;

    Frame3D frame = frames[i];
    cv::Mat depthImage = frame.depth_image_;
    double focalLength = frame.focal_length_;
    const Eigen::Matrix4f cameraPose = frame.getEigenTransform();

    // TODO(Student): The same as mergingPointClouds but now with texturing. ~ 50 lines.

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud = mat2IntegralPointCloudRGB(depthImage, frame.rgb_image_, focalLength, depth_threshold);

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr point_cloud_with_normals = computeNormalsWithTexture(point_cloud);

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr new_pcl(new pcl::PointCloud<pcl::PointXYZRGBNormal>());

    new_pcl->reserve(point_cloud_with_normals->size());
    for (auto &&pt : *point_cloud_with_normals) {
      if (!isnan(pt.normal_x) && !isnan(pt.normal_y) && !isnan(pt.normal_z)) {
        new_pcl->push_back(pt);
      }
    }

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformed_point_cloud =
        transformPointCloudNormals<pcl::PointXYZRGBNormal>(new_pcl, cameraPose);

    *modelCloud += (*transformed_point_cloud);
  }
  return modelCloud;
}

// Different methods of constructing mesh
enum CreateMeshMethod { PoissonSurfaceReconstruction = 0, MarchingCubes = 1 };

// Create mesh from point cloud using one of above methods
pcl::PolygonMesh createMesh(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloud, CreateMeshMethod method,
                            int method_specific_param, float pass, float hole_size) {
  std::cout << "Creating meshes" << std::endl;

  // The variable for the constructed mesh
  pcl::PolygonMesh triangles;

  pcl::Poisson<pcl::PointXYZRGBNormal> psr;
  pcl::MarchingCubesHoppe<pcl::PointXYZRGBNormal> mcr;

  pcl::PolygonMesh::Ptr unsmoothed_triangles(new pcl::PolygonMesh());

  int depth, resolution;

  switch (method) {
    case PoissonSurfaceReconstruction:
      // TODO(Student): Call Poisson Surface Reconstruction. ~ 5 lines.

      /* Around 8 */
      depth = method_specific_param;
      /* It's the maximum depth of the octree used to build the indicator function for the surface.
       * It is directly related to the maximum resolution allowed.
      /* Higher values enable a more fine-graned reconstruction, but more sensitive to noise
      /* Low values produce smoother surfaces but lose many details (e.g. nose, eyes, ...)
       */
      psr.setDepth(depth);

      /* the ratio between the diameter of the cube used for reconstruction and the diameter of the samples' bounding
       * cube */
      /*
       * 1 seems too small as part of one arm is not reconstructed properly
       * too large values deform the lower-part of the body and add surfaces outside the body
       * the default one and something aroud 1.25 works well
       */
      //~ psr.setScale(1.25);

      /* minimum number of sample points that should fall within an octree node as the octree construction is adapted to
       * sampling density */
      /*
       * The documentation suggests:
       * 		"For noise-free samples, small values in the range [1.0 - 5.0] can be used.
       * 		For more noisy samples, larger values in the range [15.0 - 20.0] may be needed to provide
       * 		a smoother, noise-reduced, reconstruction."
       * In our case the point cloud doesn't seem very noisy, and we saw law values work pretty well,
       * with little difference with the default one.
       * Increasing more the value, results in gradually smoother results
       */
      //~ psr.setSamplesPerNode(3);

      psr.setOutputPolygons(false);
      psr.setInputCloud(pointCloud);
      psr.reconstruct(*unsmoothed_triangles);

      break;
    case MarchingCubes:
      // TODO(Student): Call Marching Cubes Surface Reconstruction. ~ 5 lines.

      /* Something between 50 - 100*/
      resolution = method_specific_param;

      /* It's the number of cubes the space is divided in (number of ticks in each of the axes)
       * Higher values mean more cubes and, therefore, a more detailed reconstruction. However, using many cubes
       * the results are extremely more sensitive to noise. Moreover, using too many cubes requires too much memory.
       * Indeed, the default value results in the RAM to be completely filled before terminating.
       * Low values produces very bad results, preserving only the general shape of the surface
       */
      mcr.setGridResolution(resolution, resolution, resolution);

      /* They don't seem to improve the results if changed w.r.t. the default ones*/

      /* Is a threshold (in [0, 1]) for choosing the position of a point w.r.t the surface
       * Too high values (> 0.1) result in an empty mesh because no/few cubes with points on both side of the surface
       * are found. With 0.1 the mesh seems to be "reverted" w.r.t. the cloud (it builds 2 meshes with the shapes of the
       * space on the left and on the right of the body) With 0.0 (should be the default one) the results make more
       * sense
       */
      //~ mcr.setIsoLevel(0.1);

      /* enlarge the resulting mesh, the default value matches the size of the point cloud better*/
      //~ mcr.setPercentageExtendGrid(0.7);

      mcr.setInputCloud(pointCloud);
      mcr.reconstruct(*unsmoothed_triangles);

      break;
  }

  if (pass > 0) {
    /* It seems that it doesn't affect significantly the results.
     * Actually, there is no visual relevant difference.
     */

    printf("Smoothing\n");
    pcl::MeshSmoothingWindowedSincVTK vtk;
    vtk.setPassBand(pass);
    vtk.setInputMesh(unsmoothed_triangles);
    vtk.setNumIter(30);
    vtk.setFeatureEdgeSmoothing(true);
    vtk.setFeatureAngle(M_PI / 10);
    vtk.setBoundarySmoothing(true);
    vtk.setEdgeAngle(M_PI / 10);
    vtk.process(triangles);
  } else {
    triangles = *unsmoothed_triangles;
  }

  if (hole_size > 0.) {
    printf("Filtering\n");
    vtkSmartPointer<vtkPolyData> input;
    pcl::VTKUtils::mesh2vtk(triangles, input);

    vtkSmartPointer<vtkFillHolesFilter> fillHolesFilter = vtkSmartPointer<vtkFillHolesFilter>::New();

    fillHolesFilter->SetInputData(input);

    /* It looks like any value greater than or equal to 1 works, and there is no visual difference using higher values*/
    fillHolesFilter->SetHoleSize(hole_size);
    fillHolesFilter->Update();

    vtkSmartPointer<vtkPolyData> polyData = fillHolesFilter->GetOutput();

    pcl::VTKUtils::vtk2mesh(polyData, triangles);
  }

  return triangles;
}

void textureMesh(pcl::PolygonMesh *mesh, Frame3D frames[]) {

	pcl::PointCloud<pcl::PointXYZ> mesh_cloud;
	pcl::fromPCLPointCloud2(mesh->cloud, mesh_cloud);

	pcl::TextureMapping<pcl::PointXYZ> texture_map;
	pcl::PointCloud<pcl::PointXYZRGB> color_mesh_cloud;
	pcl::copyPointCloud(mesh_cloud, color_mesh_cloud);

	Eigen::Vector2f uv_coords;
	pcl::PointXYZRGB color_point;
	pcl::PointXYZ point;
	cv::Vec3b pixel;

	int colors[color_mesh_cloud.points.size()][4];
	for (int i=0; i<color_mesh_cloud.points.size(); i++){
		colors[i][0] = colors[i][1] = colors[i][2] = colors[i][3] = 0;
	}

	const double RESOLUTION = 0.01;

	for (int f = 0; f < 8; f++) {

		printf("Coloring using Frame %d\n", f);

		Frame3D frame = frames[f];

		// resize image to depth-map size (i.e. the range of coordinates in the point cloud)
		cv::Mat rgb_image_;
		cv::resize(frame.rgb_image_, rgb_image_, frame.depth_image_.size());

		pcl::TextureMapping<pcl::PointXYZ>::Camera camera;
		camera.focal_length = frame.focal_length_;
		camera.width = rgb_image_.cols;
		camera.height = rgb_image_.rows;

		// transform mesh points into camera frame
		auto cam_pose = frame.getEigenTransform();
		pcl::PointCloud<pcl::PointXYZ>::Ptr cam_cloud(new pcl::PointCloud<pcl::PointXYZ>());
		pcl::transformPointCloud(mesh_cloud, *cam_cloud, Eigen::Affine3f(cam_pose).inverse().matrix());

		pcl::TextureMapping<pcl::PointXYZ>::Octree::Ptr tree(new pcl::TextureMapping<pcl::PointXYZ>::Octree(RESOLUTION));
		tree->setInputCloud(cam_cloud);
		tree->addPointsFromInputCloud();

		// loop over all points (vertices) in the mesh
		int verbose = color_mesh_cloud.points.size() / 10;
		for (int vertex_id = 0; vertex_id < color_mesh_cloud.points.size(); vertex_id++) {

			if (vertex_id % verbose == 0)
				printf("\t%d %% done\n", 100*vertex_id / int(color_mesh_cloud.points.size()));

			point = cam_cloud->points.at(vertex_id);

			// check if the point is visible by the camera (it is not occluded)
			if (texture_map.isPointOccluded(point, tree))
				continue;

			// check if the point is inside the image
			if (!texture_map.getPointUVCoordinates(point, camera, uv_coords))
				continue;

			// retrieve the corresponding pixel
			int u = (int)(uv_coords[0] * camera.width);
			int v = (int)(camera.height - (uv_coords[1] * camera.height));

			pixel = rgb_image_.at<cv::Vec3b>(v, u);

			/* In order to average colors from multiple images
			/* we keep track of the sum of the colors and the number
			 * of frames in which the point was visible
			 */
			colors[vertex_id][3]++;
			colors[vertex_id][0] += pixel[2];
			colors[vertex_id][1] += pixel[1];
			colors[vertex_id][2] += pixel[0];

		}
	}

	/*
	 * Compute the average of the pixels from the view in which each point appears
	 */
	for (int i=0; i<color_mesh_cloud.points.size(); i++){
		if (colors[i][3] > 0){
			color_mesh_cloud.points.at(i).r = (uint8_t)(colors[i][0] / colors[i][3]);
			color_mesh_cloud.points.at(i).g = (uint8_t)(colors[i][1] / colors[i][3]);
			color_mesh_cloud.points.at(i).b = (uint8_t)(colors[i][2] / colors[i][3]);
		}
	}

	// put the colored points back into the mesh
	pcl::toPCLPointCloud2(color_mesh_cloud, mesh->cloud);
}

int main(int argc, char *argv[]) {
    if (argc < 4) {
      std::cout << "./final [3DFRAMES PATH] [RECONSTRUCTION MODE] [TEXTURE_MODE]" << std::endl;
      return 0;
    }

    srand(time(NULL));

    const CreateMeshMethod reconMode = static_cast<CreateMeshMethod>(std::stoi(argv[2]));

    // Loading 3D frames
    Frame3D frames[8];
    for (int i = 0; i < 8; ++i) {
      frames[i].load(boost::str(boost::format("%s/%05d.3df") % argv[1] % i));
    }

    float depth_threshold = 1.0;
    int param;

    switch (reconMode) {
      case PoissonSurfaceReconstruction:
        param = 8;
        break;
      case MarchingCubes:
        param = 100;
        break;
    }

    float hole_size = 100.0;
    float pass = -1;

    bool visualize_points = true;
    bool visualize_mesh = true;

    if (argc > 4) {
      depth_threshold = atof(argv[4]);
    }

    if (argc > 5) {
      param = atoi(argv[5]);
    }

    if (argc > 6) {
      pass = atof(argv[6]);
    }

    if (argc > 7) {
      hole_size = atof(argv[7]);
    }

    if (argc > 8) {
      visualize_points = atoi(argv[8]);
    }

    if (argc > 9) {
      visualize_mesh = atoi(argv[9]);
    }

	// Building the Experiment Directory
	std::string root_experiments = "./experiments/";
	mkdir(root_experiments.c_str(), S_IRUSR | S_IWUSR | S_IXUSR);

	std::stringstream dir_name;
	dir_name << root_experiments << "exp_" << (int)reconMode << "_" << argv[3][0] << "_" << depth_threshold << "_" << param << "_" << pass << "_" << hole_size << "/";

    mkdir(dir_name.str().c_str(), S_IRUSR | S_IWUSR | S_IXUSR);

	cout << endl << endl << "Experiment: " << dir_name.str() << endl;

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr texturedCloud;
    pcl::PolygonMesh triangles;

    if (argv[3][0] == 't') {
      // SECTION 4: Coloring 3D Model
      // Create one point cloud by merging all frames with texture using
      // the rgb images from the frames

      texturedCloud = mergingPointCloudsWithTexture(frames, depth_threshold);

      // Create a mesh from the textured cloud using a reconstruction method,
      // Poisson Surface or Marching Cubes
	  triangles = createMesh(texturedCloud, reconMode, param, pass, hole_size);

	  // Add texture to the mesh
	  textureMesh(&triangles, frames);

    } else {
      // SECTION 3: 3D Meshing & Watertighting

      // Create one point cloud by merging all frames with texture using
      // the rgb images from the frames
      texturedCloud = mergingPointClouds(frames, depth_threshold);

      // Create a mesh from the textured cloud using a reconstruction method,
      // Poisson Surface or Marching Cubes
	  triangles = createMesh(texturedCloud, reconMode, param, pass, hole_size);
    }

	// Storing results to files
	std::stringstream cloud_file;
	cloud_file << dir_name.str() << "cloud.pcd";

	pcl::io::savePCDFile(cloud_file.str().c_str(), *texturedCloud, true);

	std::stringstream mesh_file;
	mesh_file << dir_name.str() << "mesh.pcd";

	pcl::io::savePolygonFile(mesh_file.str().c_str(), triangles);

    if (visualize_points || visualize_mesh) {
		// Sample code for visualization.

		// Show viewer
		std::cout << "Finished texturing" << std::endl;
		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));

		if (visualize_points){
			// Add colored point cloud to viewer, because it does not support colored
			// meshes
			pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal> rgb(texturedCloud);
			viewer->addPointCloud<pcl::PointXYZRGBNormal>(texturedCloud, rgb, "cloud");
			viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
		}

		if (visualize_mesh) {
			// Add mesh
			viewer->addPolygonMesh(triangles, "meshes", 0);
		}

		viewer->setBackgroundColor(1, 1, 1);
		viewer->addCoordinateSystem(1.0);
		viewer->initCameraParameters();

		// Keep viewer open
		while (!viewer->wasStopped()) {
		  viewer->spinOnce(100);
		  boost::this_thread::sleep(boost::posix_time::microseconds(100000));
		}
	}

    return 0;
  }

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr mat2IntegralPointCloudRGB(const cv::Mat &depth_mat, const cv::Mat &rgb_image,
                                                                   const float focal_length, const float max_depth) {
    // This function converts a depth image to a point cloud
    assert(depth_mat.type() == CV_16U);

    //~ cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE/10 );// Create a window for display.
    //~ cv::imshow( "Display window", rgb_image);
    //~ cv::waitKey(20);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    const int half_width = depth_mat.cols / 2;
    const int half_height = depth_mat.rows / 2;
    const float inv_focal_length = 1.0 / focal_length;

    double downscale_x = (double)rgb_image.cols / (double)depth_mat.cols;
    double downscale_y = (double)rgb_image.rows / (double)depth_mat.rows;

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgb_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    for (int i = 0; i < rgb_image.rows; i++) {
      for (int j = 0; j < rgb_image.cols; j++) {
        cv::Vec3b pixel = rgb_image.at<cv::Vec3b>(i, j);
        rgb_cloud->points.emplace_back(pixel[2], pixel[1], pixel[0]);
        // expects that y axis start at [0,0] in top left position and goes down in depth image
        // x axis  goes to the right
        rgb_cloud->points.back().x = (double)j / downscale_x;
        rgb_cloud->points.back().y = (double)i / downscale_y;
        rgb_cloud->points.back().z = 0;
      }
    }

    pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
    kdtree.setInputCloud(rgb_cloud);

    int K = 1;
    float radius = 2;

    std::vector<int> idxs_radius;
    std::vector<float> distances_radius;

    //~ cv::Mat image(depth_mat.rows, depth_mat.cols, rgb_image.type());

    point_cloud->points.reserve(depth_mat.rows * depth_mat.cols);
    for (int y = 0; y < depth_mat.rows; y++) {
      for (int x = 0; x < depth_mat.cols; x++) {
        float z = depth_mat.at<ushort>(cv::Point(x, y)) * 0.001;
        if (z < max_depth && z > 0) {
          pcl::PointXYZRGB search_pt(0, 0, 0);
          search_pt.x = x;
          search_pt.y = y;

          pcl::PointXYZRGB new_pt;

          if (kdtree.radiusSearch(search_pt, radius, idxs_radius, distances_radius) > 0) {
            pcl::PointXYZRGB pt = rgb_cloud->points[idxs_radius[0]];

            new_pt = pcl::PointXYZRGB(pt);

          } else {
            new_pt = pcl::PointXYZRGB(0, 0, 0);
          }

          new_pt.x = static_cast<float>(x - half_width) * z * inv_focal_length;
          new_pt.y = static_cast<float>(y - half_height) * z * inv_focal_length;
          new_pt.z = z;

          point_cloud->points.push_back(new_pt);

        } else {
          point_cloud->points.emplace_back(0, 0, 0);
          point_cloud->points.back().x = x;
          point_cloud->points.back().y = y;
          point_cloud->points.back().z = NAN;
        }

        //~ cv::Vec3b color;
        //~ color[0] = point_cloud->points.back().r;
        //~ color[1] = point_cloud->points.back().g;
        //~ color[2] = point_cloud->points.back().b;

        //~ image.at<cv::Vec3b>(cv::Point(x,y)) = color;
      }
    }

    point_cloud->width = depth_mat.cols;
    point_cloud->height = depth_mat.rows;

    //~ cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE/10 );// Create a window for display.
    //~ cv::imshow( "Display window", image);
    //~ cv::waitKey(20);

    return point_cloud;
  }

  pcl::PointCloud<pcl::PointXYZ>::Ptr mat2IntegralPointCloud(const cv::Mat &depth_mat, const float focal_length,
                                                             const float max_depth) {
    // This function converts a depth image to a point cloud
    assert(depth_mat.type() == CV_16U);
    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    const int half_width = depth_mat.cols / 2;
    const int half_height = depth_mat.rows / 2;
    const float inv_focal_length = 1.0 / focal_length;
    point_cloud->points.reserve(depth_mat.rows * depth_mat.cols);
    for (int y = 0; y < depth_mat.rows; y++) {
      for (int x = 0; x < depth_mat.cols; x++) {
        float z = depth_mat.at<ushort>(cv::Point(x, y)) * 0.001;
        if (z < max_depth && z > 0) {
          point_cloud->points.emplace_back(static_cast<float>(x - half_width) * z * inv_focal_length,
                                           static_cast<float>(y - half_height) * z * inv_focal_length, z);
        } else {
          point_cloud->points.emplace_back(x, y, NAN);
        }
      }
    }
    point_cloud->width = depth_mat.cols;
    point_cloud->height = depth_mat.rows;
    return point_cloud;
  }

  boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud) {
    // --------------------------------------------
    // -----Open 3D viewer and add point cloud-----
    // --------------------------------------------
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(1, 1, 1);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    return (viewer);
  }


#define BOOST_TEST_MODULE Simple_Implementation test

#include <string>
#include <iostream>
#include <vector>
#include <boost/test/included/unit_test.hpp>

//quick and dirty trick to test private members
#define private public

#include "simple_implementation.h"

using namespace std;

using Implementation = simple_implementation::Simple_Implementation;
using HLException = simple_implementation::HLException;

BOOST_AUTO_TEST_CASE(constructors_test)
{

	//auto universe = Implementation(cols, rows);
	//checks for minimum dimensions
	BOOST_CHECK_THROW(Implementation(1,1), HLException);
	BOOST_CHECK_THROW(Implementation(5, 1), HLException);
	BOOST_CHECK_THROW(Implementation(0, 10), HLException);

	//invalid access:
	auto universe = Implementation(4, 4);
	BOOST_CHECK_THROW(universe.set(5, 0, 1), HLException);
	BOOST_CHECK_THROW(universe.set(0, 5, 1), HLException);

	//simple set and to string
	universe = Implementation(4, 4);
	universe.set(2, 3, 1);
	BOOST_TEST(universe.to_s() == "0000\n0000\n0001\n0000\n");
}

BOOST_AUTO_TEST_CASE(life_test)
{
	//simple check of the ruleset and execution
	//an horizontal len 3 bar is set. In 1 step it changes to a vertical bar.
	//also is in a corner
	auto universe = Implementation(5, 5);
	universe.set(3, 2, 1);
	universe.set(3, 3, 1);
	universe.set(3, 4, 1);

	universe.run();

	BOOST_TEST(universe.to_s() == "0000000\n0000000\n0001000\n0001000\n0001000\n");

	//a simple dot dissapears after 1 step.
	universe = Implementation(5, 5);
	universe.set(2, 2, 1);

	universe.run();

	BOOST_TEST(universe.to_s() == "00000\n00000\n00000\n00000\n00000\n");

	//a square persists in time.
	universe = Implementation(5, 5);
	universe.set(1, 1, 1);
	universe.set(1, 2, 1);
	universe.set(2, 1, 1);
	universe.set(2, 2, 1);

	universe.run();

	BOOST_TEST(universe.to_s() == "00000\n01100\n01100\n00000\n00000\n");

}

BOOST_AUTO_TEST_CASE(data_x_test)
{
	BOOST_TEST(1 == 1);
	auto universe = Implementation(4, 4);

	BOOST_TEST(!universe.resize_at_bottom_test());
	BOOST_TEST(!universe.resize_at_top_test());
	BOOST_TEST(!universe.resize_at_left_test());
	BOOST_TEST(!universe.resize_at_right_test());

	universe.set(0, 1, 1);
	BOOST_TEST((universe.resize_at_bottom_test() && !universe.resize_at_top_test() && !universe.resize_at_left_test() && !universe.resize_at_right_test()));

	universe.set(0, 1, 0);
	universe.set(1, 0, 1);
	BOOST_TEST((!universe.resize_at_bottom_test() && !universe.resize_at_top_test() && universe.resize_at_left_test() && !universe.resize_at_right_test()));

	universe.set(1, 0, 0);
	universe.set(3, 1, 1);
	BOOST_TEST((!universe.resize_at_bottom_test() && universe.resize_at_top_test() && !universe.resize_at_left_test() && !universe.resize_at_right_test()));

	universe.set(3, 1, 0);
	universe.set(1, 3, 1);
	BOOST_TEST((!universe.resize_at_bottom_test() && !universe.resize_at_top_test() && !universe.resize_at_left_test() && universe.resize_at_right_test()));

	//resize at top:
	universe = Implementation(4, 4);
	universe.set(3, 1, 1);
	universe.test_and_resize();
	BOOST_TEST(universe.to_s() == "0000\n0000\n0000\n0100\n0000\n0000\n");

	//resise at right:
	universe = Implementation(4, 4);
	universe.set(1, 3, 1);
	universe.test_and_resize();
	BOOST_TEST(universe.to_s() == "000000\n000100\n000000\n000000\n");

	//reize at bottom
	universe = Implementation(4, 4);
	universe.set(0, 1, 1);
	universe.set(1, 1, 2);
	universe.set(2, 1, 3);
	universe.test_and_resize();
	BOOST_TEST(universe.to_s() == "0000\n0000\n0100\n0200\n0300\n0000\n");

	//reize at left
	universe = Implementation(4, 4);
	universe.set(1, 0, 1);
	universe.set(2, 0, 2);
	universe.test_and_resize();
	BOOST_TEST(universe.to_s() == "000000\n001000\n002000\n000000\n");

}


//  (C) Copyright Eric Niebler 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <vector>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

template<typename T>
typename boost::enable_if<is_floating_point<T> >::type is_equal_or_close(T const &left, T const &right)
{
    BOOST_CHECK_CLOSE(left, right, 1e-5);
}

template<typename T>
typename boost::disable_if<is_floating_point<T> >::type is_equal_or_close(T const &left, T const &right)
{
    BOOST_CHECK_EQUAL(left, right);
}

template<typename T>
void is_equal(std::vector<T> const &left, std::vector<T> const &right)
{
    BOOST_CHECK_EQUAL(left.size(), right.size());
    if(left.size() == right.size())
    {
        for(std::size_t i = 0; i < left.size(); ++i)
        {
            is_equal_or_close(left[i], right[i]);
        }
    }
}

namespace std
{
    template<typename T>
    inline std::ostream &operator <<(std::ostream &sout, std::vector<T> const &arr)
    {
        sout << '(';
        for(std::size_t i = 0; i < arr.size(); ++i)
        {
            sout << arr[i] << ',';
        }
        sout << ')' << std::endl;
        return sout;
    }
}

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    typedef std::vector<int> sample_t;

    // test sum
    {
        accumulator_set<sample_t, stats<tag::sum> > acc(sample = sample_t(3,0));

        acc(sample_t(3,1));
        acc(sample_t(3,2));
        acc(sample_t(3,3));

        is_equal(sample_t(3,6), sum(acc));
    }

    // test min and max
    {
        int s1[] = {1,2,3}, s2[] = {0,3,4}, s3[] = {2,1,4}, min_res[] = {0,1,3}, max_res[] = {2,3,4};
        accumulator_set<sample_t, stats<tag::min, tag::max> > acc(sample = sample_t(3,0));

        acc(sample_t(s1,s1+3));
        acc(sample_t(s2,s2+3));
        acc(sample_t(s3,s3+3));

        is_equal(sample_t(min_res,min_res+3), (min)(acc));
        is_equal(sample_t(max_res,max_res+3), (max)(acc));
    }

    // test mean(lazy) and mean(immediate)
    {
        accumulator_set<sample_t, stats<tag::mean> > acc(sample = sample_t(3,0));

        acc(sample_t(3,1));
        is_equal(std::vector<double>(3, 1.), mean(acc));
        BOOST_CHECK_EQUAL(1u, count(acc));
        is_equal(sample_t(3,1), sum(acc));

        acc(sample_t(3,0));
        is_equal(std::vector<double>(3, 0.5), mean(acc));
        BOOST_CHECK_EQUAL(2u, count(acc));
        is_equal(sample_t(3,1), sum(acc));

        acc(sample_t(3,2));
        is_equal(std::vector<double>(3, 1.), mean(acc));
        BOOST_CHECK_EQUAL(3u, count(acc));
        is_equal(sample_t(3,3), sum(acc));

        accumulator_set<sample_t, stats<tag::mean(immediate)> > acc2(sample = sample_t(3,0));

        acc2(sample_t(3,1));
        is_equal(std::vector<double>(3,1.), mean(acc2));
        BOOST_CHECK_EQUAL(1u, count(acc2));

        acc2(sample_t(3,0));
        is_equal(std::vector<double>(3,0.5), mean(acc2));
        BOOST_CHECK_EQUAL(2u, count(acc2));

        acc2(sample_t(3,2));
        is_equal(std::vector<double>(3,1.), mean(acc2));
        BOOST_CHECK_EQUAL(3u, count(acc2));
    }

    // test weighted_mean
    {
        accumulator_set<sample_t, stats<tag::weighted_mean>, int> acc(sample = sample_t(3,0));

        acc(sample_t(3,10), weight = 2);            //  20
        BOOST_CHECK_EQUAL(2, sum_of_weights(acc));  //
                                                    //
        acc(sample_t(3,6), weight = 3);             //  18
        BOOST_CHECK_EQUAL(5, sum_of_weights(acc));  //
                                                    //
        acc(sample_t(3,4), weight = 4);             //  16
        BOOST_CHECK_EQUAL(9, sum_of_weights(acc));  //
                                                    //
        acc(sample_t(3,6), weight = 5);             //+ 30
        BOOST_CHECK_EQUAL(14, sum_of_weights(acc)); //
                                                    //= 84  / 14 = 6

        is_equal(std::vector<double>(3,6.), weighted_mean(acc));

        accumulator_set<sample_t, stats<tag::weighted_mean(immediate)>, int> acc2(sample = sample_t(3,0));

        acc2(sample_t(3,10), weight = 2);           //  20
        BOOST_CHECK_EQUAL(2, sum_of_weights(acc2)); //
                                                    //
        acc2(sample_t(3,6), weight = 3);            //  18
        BOOST_CHECK_EQUAL(5, sum_of_weights(acc2)); //
                                                    //
        acc2(sample_t(3,4), weight = 4);            //  16
        BOOST_CHECK_EQUAL(9, sum_of_weights(acc2)); //
                                                    //
        acc2(sample_t(3,6), weight = 5);            //+ 30
        BOOST_CHECK_EQUAL(14, sum_of_weights(acc2));//
                                                    //= 84  / 14 = 6

        is_equal(std::vector<double>(3,6.), weighted_mean(acc2));

    }
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("vector test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


//          Copyright Rein Halbersma 2014-2021.
// 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)

#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS
#define BOOST_MPL_LIMIT_VECTOR_SIZE 50

#include <bitset/compatible/bit_set.hpp>        // bit_set
#include <bitset/compatible/bitset.hpp>         // bitset
#include <bitset/compatible/dynamic_bitset.hpp> // dynamic_bitset
#include <bitset/exhaustive.hpp>                // empty_set, full_set, all_values, all_cardinality_sets, all_singleton_sets
#include <bitset/primitives.hpp>                // mem_set, mem_reset, op_compl, mem_flip, mem_count, mem_size, mem_test, mem_all, mem_any, mem_none
                                                // mem_at, op_bitand, op_bitor, op_xor, op_minus, mem_is_subset_of, mem_intersects
#include <xstd/bit_set.hpp>                     // bit_set
#include <boost/dynamic_bitset.hpp>             // dynamic_bitset
#include <boost/mpl/vector.hpp>                 // vector
#include <boost/test/unit_test.hpp>             // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE_TEMPLATE
#include <bitset>                               // bitset
#include <cstdint>                              // uint8_t, uint16_t, uint32_t, uint64_t

BOOST_AUTO_TEST_SUITE(Linear)

using namespace xstd;

using bitset_types = boost::mpl::vector
<       std::bitset<  0>
,       std::bitset<  1>
,       std::bitset< 31>
,       std::bitset< 32>
,       std::bitset< 33>
,       std::bitset< 63>
,       std::bitset< 64>
,       std::bitset< 65>
,       boost::dynamic_bitset<>
,       bit_set<  0, uint8_t>
,       bit_set<  1, uint8_t>
,       bit_set<  7, uint8_t>
,       bit_set<  8, uint8_t>
,       bit_set<  9, uint8_t>
,       bit_set< 15, uint8_t>
,       bit_set< 16, uint8_t>
,       bit_set< 17, uint8_t>
,       bit_set< 24, uint8_t>
,       bit_set<  0, uint16_t>
,       bit_set<  1, uint16_t>
,       bit_set< 15, uint16_t>
,       bit_set< 16, uint16_t>
,       bit_set< 17, uint16_t>
,       bit_set< 31, uint16_t>
,       bit_set< 32, uint16_t>
,       bit_set< 33, uint16_t>
,       bit_set< 48, uint16_t>
,       bit_set<  0, uint32_t>
,       bit_set<  1, uint32_t>
,       bit_set< 31, uint32_t>
,       bit_set< 32, uint32_t>
,       bit_set< 33, uint32_t>
,       bit_set< 63, uint32_t>
,       bit_set< 64, uint32_t>
,       bit_set< 65, uint32_t>
#if defined(__GNUG__) || defined(_MSC_VER) && defined(WIN64)
,       bit_set<  0, uint64_t>
,       bit_set<  1, uint64_t>
,       bit_set< 63, uint64_t>
,       bit_set< 64, uint64_t>
,       bit_set< 65, uint64_t>
#endif
#if defined(__GNUG__)
,       bit_set<  0, __uint128_t>
,       bit_set<  1, __uint128_t>
,       bit_set<127, __uint128_t>
,       bit_set<128, __uint128_t>
,       bit_set<129, __uint128_t>
#endif
>;

BOOST_AUTO_TEST_CASE_TEMPLATE(BitSet, T, bitset_types)
{
        all_singleton_sets<T>([&](auto& bs1) {
                op_shift_left_assign()(bs1);
        });
        all_singleton_sets<T>([&](auto& bs1) {
                op_shift_right_assign()(bs1);
        });

        all_cardinality_sets<T>(mem_set());
        all_singleton_sets<T>(mem_set());
        all_values<T>([](auto pos) {
                empty_set<T>([&](auto& bs0) {
                        mem_set()(bs0, pos);
                });
                empty_set<T>([&](auto& bs0) {
                        mem_set()(bs0, pos, true);
                });
                full_set<T>([&](auto& bsN) {
                        mem_set()(bsN, pos, false);
                });
        });

        all_cardinality_sets<T>(mem_reset());
        all_singleton_sets<T>(mem_reset());
        all_values<T>([](auto pos) {
                full_set<T>([&](auto& bsN) {
                        mem_reset()(bsN, pos);
                });
        });

        all_cardinality_sets<T>(op_compl());
        all_singleton_sets<T>(op_compl());

        all_cardinality_sets<T>(mem_flip());
        all_singleton_sets<T>(mem_flip());
        all_values<T>([](auto pos) {
                empty_set<T>([&](auto& bs0) {
                        mem_flip()(bs0, pos);
                });
                full_set<T>([&](auto& bsN) {
                        mem_flip()(bsN, pos);
                });
       });

        all_cardinality_sets<T>(mem_count());

        all_cardinality_sets<T>(mem_size());
        all_singleton_sets<T>(mem_size());

        all_singleton_sets<T>(mem_test());
        all_values<T>([](auto pos) {
                empty_set<T>([&](auto const& bs0) {
                        mem_test()(bs0, pos);
                });
                full_set<T>([&](auto const& bsN) {
                        mem_test()(bsN, pos);
                });
        });

        all_cardinality_sets<T>(mem_all());
        all_cardinality_sets<T>(mem_any());
        all_cardinality_sets<T>(mem_none());

        all_singleton_sets<T>([&](auto const& bs1) {
                op_shift_left()(bs1);
        });
        all_singleton_sets<T>([&](auto const& bs1) {
                op_shift_right()(bs1);
        });

        all_values<T>([](auto pos) {
                empty_set<T>([&](auto const& bs0) {
                        op_at()(bs0, pos);
                });
                full_set<T>([&](auto const& bsN) {
                        op_at()(bsN, pos);
                });
                empty_set<T>([&](auto& bs0) {
                        op_at()(bs0, pos);
                });
                empty_set<T>([&](auto& bs0) {
                        op_at()(bs0, pos, false);
                });
                empty_set<T>([&](auto& bs0) {
                        op_at()(bs0, pos, true);
                });
                full_set<T>([&](auto& bsN) {
                        op_at()(bsN, pos);
                });
                full_set<T>([&](auto& bsN) {
                        op_at()(bsN, pos, false);
                });
                full_set<T>([&](auto& bsN) {
                        op_at()(bsN, pos, true);
                });
        });

        all_singleton_sets<T>(op_bitand());
        all_singleton_sets<T>(op_bitor());
        all_singleton_sets<T>(op_xor());
        all_singleton_sets<T>(op_minus());

        all_cardinality_sets<T>(mem_is_subset_of());
        all_singleton_sets<T>(mem_is_subset_of());

        all_cardinality_sets<T>(mem_intersects());
        all_singleton_sets<T>(mem_intersects());

        all_cardinality_sets<T>(fn_iostream());
        all_singleton_sets<T>(fn_iostream());
}

BOOST_AUTO_TEST_SUITE_END()


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename MatrixA>
void test(MatrixA& A, const char* name)
{
    hessian_setup(A, 1.0);
    std::cout << "Hessian setup for " << name << "\n";
    if (num_rows(A) < 10)
	std::cout << "A is\n" << A;

    MTL_THROW_IF(num_rows(A) >= 4 && std::abs(A[3][2] - 5) > 0.001, mtl::runtime_error("A[3][2] should be 5"));
}

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

    unsigned size= 5;
    if (argc > 1) size= atoi(argv[1]);
    if (size < 2) size= 2;

    dense2D<double>                                       da(size, size-1);
    dense2D<double, mat::parameters<col_major> >       dca(size, size-1);
    dense2D<float>                                        fa(size, size-1);

    morton_dense<double,  morton_mask>                    mda(size, size-1);
    morton_dense<double, doppled_32_col_mask>             mca(size, size-1);
    morton_dense<double, doppled_32_row_mask>             mra(size, size-1);

    compressed2D<double>                                  cda(size, size-1);
    compressed2D<double, mat::parameters<col_major> >  cdca(size, size-1);

    test(da, "dense2D");
    test(dca, "dense2D col-major");
    test(fa, "dense2D float");

    test(mda, "pure Morton");
    test(mca, "Hybrid col-major");
    test(mra, "Hybrid row-major");

    test(cda, "compressed2D");
    test(cdca, "compressed2D col-major");

    return 0;
}


/*
 kpoint.cpp

 Copyright (c) 2014, 2015, 2016 Terumasa Tadano

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include "mpi_common.h"
#include "kpoint.h"
#include "memory.h"
#include "error.h"
#include "system.h"
#include "phonon_dos.h"
#include "symmetry_core.h"
#include "dynamical.h"
#include "timer.h"
#include <iostream>
#include <iomanip>
#include <cmath>
#include <set>
#include <map>
#include <numeric>
#include "parsephon.h"
#include "relaxation.h"
#include "mathfunctions.h"

#ifdef _USE_BOOST
#include <boost/lexical_cast.hpp>
#endif

#ifdef _USE_EIGEN
#include <Eigen/Core>
#include <Eigen/LU>
#endif

using namespace PHON_NS;

Kpoint::Kpoint(PHON *phon): Pointers(phon) {}

Kpoint::~Kpoint() {

    if (kpoint_mode < 3) {
        memory->deallocate(xk);
        memory->deallocate(kvec_na);

        if (kpoint_mode == 2) {
            memory->deallocate(knum_minus);
        }

        if (kpoint_mode == 1) {
            if (mympi->my_rank == 0) memory->deallocate(kaxis);
        }
    }
}

void Kpoint::kpoint_setups(std::string mode)
{
    symmetry->symmetry_flag = true;

    unsigned int i, j;
    std::string str_tmp;

    MPI_Bcast(&kpoint_mode, 1, MPI_INT, 0, MPI_COMM_WORLD);

    if (mympi->my_rank == 0) {

        std::cout << " k points" << std::endl;
        std::cout << " ========" << std::endl << std::endl;

    }

    switch (kpoint_mode) {
    case 0:

        if (mympi->my_rank == 0) {
            std::cout << "  KPMODE = 0 : Calculation on given k points" << std::endl;
        }

        setup_kpoint_given(kpInp, nk, xk, kvec_na);

        if (mympi->my_rank == 0) {
            std::cout << "  Number of k points : " << nk << std::endl << std::endl;
            std::cout << "  List of k points : " << std::endl;
            for (i = 0; i < nk; ++i) {
                std::cout << std::setw(5) << i + 1 << ":";
                for (j = 0; j < 3; ++j) {
                    std::cout << std::setw(15) << xk[i][j];
                }
                std::cout << std::endl;
            }
            std::cout << std::endl;
        }

        break;

    case 1:

        if (mympi->my_rank == 0) {
            std::cout << "  KPMODE = 1: Band structure calculation" << std::endl;
        }

        setup_kpoint_band(kpInp, nk, xk, kvec_na, kaxis);

        if (mympi->my_rank == 0) {
            std::cout << "  Number of paths : " << kpInp.size() << std::endl << std::endl;
            std::cout << "  List of k paths : " << std::endl;

            for (i = 0; i < kpInp.size(); ++i) {
                std::cout << std::setw(4) << i + 1 << ":";
                std::cout << std::setw(3) << kpInp[i].kpelem[0];
                std::cout << " (";
                for (int k = 0; k < 3; ++k) {
                    std::cout << std::setprecision(4) << std::setw(8) << std::atof((kpInp[i].kpelem[k + 1]).c_str());
                }
                std::cout << ")";
                std::cout << std::setw(3) << kpInp[i].kpelem[4];
                std::cout << " (";
                for (int k = 0; k < 3; ++k) {
                    std::cout << std::setprecision(4) << std::setw(8) << std::atof((kpInp[i].kpelem[k + 5]).c_str());
                }
                std::cout << ")";
                std::cout << std::setw(4) << kpInp[i].kpelem[8] << std::endl;
            }
            std::cout << std::endl;
            std::cout << "  Number of k points : " << nk << std::endl << std::endl;

        }

        break;

    case 2:

        if (mympi->my_rank == 0) {
            std::cout << "  KPMODE = 2: Uniform grid" << std::endl;
        }

        setup_kpoint_mesh(kpInp, nk, nkx, nky, nkz, xk, kvec_na, symmetry->symmetry_flag, kpoint_irred_all);

        nk_reduced = kpoint_irred_all.size();

        weight_k.clear();
        for (i = 0; i < kpoint_irred_all.size(); ++i) {
            weight_k.push_back(static_cast<double>(kpoint_irred_all[i].size())/static_cast<double>(nk));
        }

        if (mympi->my_rank == 0) {
            std::cout << "  Gamma-centered uniform grid with the following mesh density: " << std::endl;
            std::cout << "  nk1:" << std::setw(4) << nkx << std::endl;
            std::cout << "  nk2:" << std::setw(4) << nky << std::endl;
            std::cout << "  nk3:" << std::setw(4) << nkz << std::endl;
            std::cout << std::endl;
            std::cout << "  Number of k points : " << nk << std::endl;
            std::cout << "  Number of irreducible k points : " << kpoint_irred_all.size() << std::endl << std::endl;
            std::cout << "  List of irreducible k points (reciprocal coordinate, weight) : " << std::endl;

            for (i = 0; i < kpoint_irred_all.size(); ++i){
                std::cout << "  " << std::setw(5) << i + 1 << ":";
                for (j = 0; j < 3; ++j){
                    std::cout << std::setprecision(5) << std::setw(14)
                        << std::scientific << kpoint_irred_all[i][0].kval[j];
                }
                std::cout << std::setprecision(6) << std::setw(11) << std::fixed << weight_k[i] << std::endl;
            }
            std::cout << std::endl;
        }

        memory->allocate(knum_minus, nk);
        gen_nkminus(nk, knum_minus, xk);

        for (i = 0; i < nk_reduced; ++i) {
            for (j = 0; j < kpoint_irred_all[i].size(); ++j) {
                kmap_to_irreducible.insert(std::map<int,int>::value_type(kpoint_irred_all[i][j].knum, i));
            }
        }

        break;

    case 3:

        if (mympi->my_rank == 0) {
            std::cout << "  KPMODE = 3: Momentum-resolved final state amplitude" << std::endl;
        }

        setup_kpoint_plane(kpInp, nplanes, kp_planes);

        if (mympi->my_rank == 0) {
            std::cout << "  Number of planes : " << nplanes << std::endl;
            for (i = 0; i < nplanes; ++i) {
                std::cout << "  The number of k points in plane " << std::setw(3) << i + 1 << " :" ;
                std::cout << std::setw(8) << kp_planes[i].size() << std::endl;
            }
            std::cout << std::endl;
        }

        break;

    default:
        error->exit("read_kpoints", "This cannot happen.");
    }
}

void Kpoint::setup_kpoint_given(std::vector<KpointInp> &kpinfo, unsigned int &n, double **&k, double **&kdirec)
{

    int i, j;
    double norm;

    n = kpinfo.size();
    MPI_Bcast(&n, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);

    memory->allocate(k, n, 3);
    memory->allocate(kdirec, n, 3);

    if (mympi->my_rank == 0) {
        j = 0;
        for (std::vector<KpointInp>::const_iterator it = kpinfo.begin(); it != kpinfo.end(); ++it) {
            for (i = 0; i < 3; ++i) {
#ifdef _USE_BOOST
                k[j][i] = boost::lexical_cast<double>((*it).kpelem[i]);
#else
                k[j][i] = std::atof(((*it).kpelem[i]).c_str());
#endif
            }

            rotvec(kdirec[j], k[j], system->rlavec_p, 'T');

            norm = kdirec[j][0] * kdirec[j][0] + kdirec[j][1] * kdirec[j][1] + kdirec[j][2] * kdirec[j][2];

            if (norm > eps) {
                for (i = 0; i < 3; ++i) kdirec[j][i] /= std::sqrt(norm);
            }

            ++j;
        }
    }

    MPI_Bcast(&k[0][0], 3 * n, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&kdirec[0][0], 3 * n, MPI_DOUBLE, 0, MPI_COMM_WORLD);
}

void Kpoint::setup_kpoint_band(std::vector<KpointInp> &kpinfo, unsigned int &n,
                               double **&xk, double **&kdirec, double *&axis)
{
    int i, j, k;

    if (mympi->my_rank == 0) {

        unsigned int npath;
        std::string **kp_symbol;
        unsigned int *nk_path;
        double **k_start, **k_end;

        npath = kpinfo.size();

        memory->allocate(kp_symbol, npath, 2);
        memory->allocate(k_start, npath, 3);
        memory->allocate(k_end, npath, 3);
        memory->allocate(nk_path, npath);

        n = 0;
        i = 0;
        for (std::vector<KpointInp>::const_iterator it = kpinfo.begin(); it != kpinfo.end(); ++it) {
            kp_symbol[i][0] = (*it).kpelem[0];
            kp_symbol[i][1] = (*it).kpelem[4];
#ifdef _USE_BOOST
            for (j = 0; j < 3; ++j) {
                k_start[i][j] = boost::lexical_cast<double>((*it).kpelem[j + 1]);
                k_end[i][j] = boost::lexical_cast<double>((*it).kpelem[j + 5]);
            }
            nkp[i] = boost::lexical_cast<int>((*it).kpelem[8]);
#else
            for (j = 0; j < 3; ++j) {
                k_start[i][j] = std::atof(((*it).kpelem[j + 1]).c_str());
                k_end[i][j] = std::atof(((*it).kpelem[j + 5]).c_str());

            }
            nk_path[i] = std::atoi(((*it).kpelem[8]).c_str());
#endif
            n += nk_path[i];
            ++i;
        }

        memory->allocate(xk, n, 3);
        memory->allocate(kdirec, n, 3);
        memory->allocate(axis, n);

        unsigned int ik = 0;
        double norm;
        double direc_tmp[3], tmp[3];

        for (i = 0; i < npath; ++i) {
            for (j = 0; j < 3; ++j) {
                direc_tmp[j] = k_end[i][j] - k_start[i][j];
            }

            rotvec(direc_tmp, direc_tmp, system->rlavec_p, 'T');
            norm = std::pow(direc_tmp[0], 2) + std::pow(direc_tmp[1], 2) + std::pow(direc_tmp[2], 2);
            norm = std::sqrt(norm);

            if (norm > eps) {
                for (j = 0; j < 3; ++j) direc_tmp[j] /= norm;
            }

            for (j = 0; j < nk_path[i]; ++j) {
                for (k = 0; k < 3; ++k) {
                    xk[ik][k] = k_start[i][k]
                    + (k_end[i][k] - k_start[i][k]) * static_cast<double>(j) / static_cast<double>(nk_path[i] - 1);

                    kdirec[ik][k] = direc_tmp[k];
                }

                if (ik == 0) {
                    axis[ik] = 0.0;
                } else {
                    if (j == 0) {
                        axis[ik] = axis[ik - 1];
                    } else {
                        for (k = 0; k < 3; ++k) tmp[k] = xk[ik][k] - xk[ik - 1][k];
                        rotvec(tmp, tmp, system->rlavec_p, 'T');
                        axis[ik] = axis[ik - 1] + std::sqrt(tmp[0] * tmp[0] + tmp[1] * tmp[1] + tmp[2] * tmp[2]);
                    }
                }
                ++ik;
            }
        }
        memory->deallocate(nk_path);
        memory->deallocate(k_start);
        memory->deallocate(k_end);
        memory->deallocate(kp_symbol);
    }

    MPI_Bcast(&n, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);

    if (mympi->my_rank > 0) {
        memory->allocate(xk, n, 3);
        memory->allocate(kdirec, n, 3);
    }

    MPI_Bcast(&xk[0][0], 3 * n, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&kdirec[0][0], 3 * n, MPI_DOUBLE, 0, MPI_COMM_WORLD);
}

void PHON_NS::Kpoint::setup_kpoint_mesh(std::vector<KpointInp> &kpinfo, unsigned int &nk,
                                        unsigned int &nkx, unsigned int &nky, unsigned int &nkz,
                                        double **&xk, double **&kdirec, const bool usesym,
                                        std::vector<std::vector<KpointList> > &kp_irreducible)
{
    int i, j;
    unsigned int nk_tmp[3];
    double norm;

    if (mympi->my_rank == 0) {

#ifdef _USE_BOOST
        nkx = boost::lexical_cast<int>(kpinfo[0].kpelem[0]);
        nky = boost::lexical_cast<int>(kpinfo[0].kpelem[1]);
        nkz = boost::lexical_cast<int>(kpinfo[0].kpelem[2]);
#else
        nkx = std::atoi((kpinfo[0].kpelem[0]).c_str());
        nky = std::atoi((kpinfo[0].kpelem[1]).c_str());
        nkz = std::atoi((kpinfo[0].kpelem[2]).c_str());
#endif
        nk = nkx * nky * nkz;

        memory->allocate(xk, nk, 3);
        memory->allocate(kdirec, nk, 3);

        nk_tmp[0] = nkx;
        nk_tmp[1] = nky;
        nk_tmp[2] = nkz;

        gen_kmesh(usesym, nk_tmp, xk, kp_irreducible);

        for (i = 0; i < nk; ++i) {
            for (j = 0; j < 3; ++j) kdirec[i][j] = xk[i][j];

            rotvec(kdirec[i], kdirec[i], system->rlavec_p, 'T');
            norm = kdirec[i][0] * kdirec[i][0] + kdirec[i][1] * kdirec[i][1] + kdirec[i][2] * kdirec[i][2];

            if (norm > eps) {
                for (j = 0; j < 3; ++j) kdirec[i][j] /= std::sqrt(norm);
            }
        }
    }

    MPI_Bcast(&nk, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&nkx, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&nky, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&nkz, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);

    if (mympi->my_rank > 0) {
        memory->allocate(xk, nk, 3);
        memory->allocate(kdirec, nk, 3);
    }

    MPI_Bcast(&xk[0][0], 3 * nk, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&kdirec[0][0], 3 * nk, MPI_DOUBLE, 0, MPI_COMM_WORLD);

    mpi_broadcast_kpoint_vector(kp_irreducible);
}

void PHON_NS::Kpoint::mpi_broadcast_kpoint_vector( std::vector<std::vector<KpointList> > &kp_irreducible)
{

    int i, j, k, ik;
    double **xk_tmp;
    unsigned int *knum_tmp;
    unsigned int *kequiv_tmp;

    std::vector<KpointList> kp_group;
    std::vector<double> ktmp;

    // Broadcast kp_irredicible to all threads

    unsigned int nk_irred = kp_irreducible.size();
    MPI_Bcast(&nk_irred, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);

    memory->allocate(xk_tmp, nk, 3);
    memory->allocate(knum_tmp, nk);
    memory->allocate(kequiv_tmp, nk_irred);

    if (mympi->my_rank == 0) {
        ik = 0;

        for (i = 0; i < nk_irred; ++i) {
            kequiv_tmp[i] = kp_irreducible[i].size();

            for (j = 0; j < kequiv_tmp[i]; ++j) {
                knum_tmp[ik] = kp_irreducible[i][j].knum;
                for (k = 0; k < 3; ++k) {
                    xk_tmp[ik][k] = kp_irreducible[i][j].kval[k];
                }
                ++ik;
            }
        }
    }

    MPI_Bcast(&knum_tmp[0], nk, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&kequiv_tmp[0], nk_irred, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&xk_tmp[0][0], 3 * nk, MPI_DOUBLE, 0, MPI_COMM_WORLD);

    if (mympi->my_rank > 0) {
        kp_irreducible.clear();
        kp_group.clear();

        ik = 0;

        for (i = 0; i < nk_irred; ++i) {
            kp_group.clear();
            for (j = 0; j < kequiv_tmp[i]; ++j) {
                ktmp.clear();
                ktmp.push_back(xk_tmp[ik][0]);
                ktmp.push_back(xk_tmp[ik][1]);
                ktmp.push_back(xk_tmp[ik][2]);
                kp_group.push_back(KpointList(knum_tmp[ik], ktmp));
                ++ik;
            }
            kp_irreducible.push_back(kp_group);
        }
    }

    memory->deallocate(xk_tmp);
    memory->deallocate(knum_tmp);
    memory->deallocate(kequiv_tmp);
}

void PHON_NS::Kpoint::mpi_broadcast_kplane_vector(const unsigned int nplane, std::vector<KpointPlane> *&kp_plane)
{
    int i, j;
    int nkp;
    int **naxis;
    double **xk_plane;

    for (i = 0; i < nplane; ++i) {
        nkp = kp_plane[i].size();

        MPI_Bcast(&nkp, 1, MPI_INT, 0, MPI_COMM_WORLD);

        memory->allocate(naxis, nkp, 2);
        memory->allocate(xk_plane, nkp, 3);

        if (mympi->my_rank == 0) {
            for (j = 0; j < nkp; ++j) {
                naxis[j][0] = kp_plane[i][j].n[0];
                naxis[j][1] = kp_plane[i][j].n[1];
                xk_plane[j][0] = kp_plane[i][j].k[0];
                xk_plane[j][1] = kp_plane[i][j].k[1];
                xk_plane[j][2] = kp_plane[i][j].k[2];
            }
        }

        MPI_Bcast(&naxis[0][0], 2 * nkp, MPI_INT, 0, MPI_COMM_WORLD);
        MPI_Bcast(&xk_plane[0][0], 3 * nkp, MPI_DOUBLE, 0, MPI_COMM_WORLD);

        if (mympi->my_rank > 0) {
            for (j = 0; j < nkp; ++j) {
                kp_plane[i].push_back(KpointPlane(xk_plane[j], naxis[j]));
            }
        }
        memory->deallocate(naxis);
        memory->deallocate(xk_plane);
    }
}

void Kpoint::setup_kpoint_plane(std::vector<KpointInp> &kpinfo, unsigned int &nplane, std::vector<KpointPlane> *&kp_plane)
{
    nplane = kpinfo.size();
    MPI_Bcast(&nplane, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);

    memory->allocate(kp_plane, nplane);
    memory->allocate(kp_planes_tri, nplane);

    if (mympi->my_rank == 0) {
        gen_kpoints_plane(kpinfo, kp_plane, kp_planes_tri);

//        for (i = 0; i < nplane; ++i) {
//            for (std::vector<KpointPlaneTriangle>::const_iterator it = kp_planes_tri[i].begin(); it != kp_planes_tri[i].end(); ++it) {
//                std::cout << (*it).index << std::endl;
//                std::cout << (*it).xk1[0] << " " << (*it).xk1[1] << " " << (*it).xk1[2] << std::endl;
//                std::cout << (*it).xk2[0] << " " << (*it).xk2[1] << " " << (*it).xk2[2] << std::endl;
//                std::cout << (*it).xk3[0] << " " << (*it).xk3[1] << " " << (*it).xk3[2] << std::endl;
//                std::cout << (*it).inside_FBZ << std::endl;
//            }
//        }
    }

    mpi_broadcast_kplane_vector(nplane, kp_plane);
}

void Kpoint::gen_kmesh(const bool usesym, const unsigned int nk_in[3], double **xk_out,
                       std::vector<std::vector<KpointList> > &kplist_out)
{
    unsigned int ix, iy, iz;
    unsigned int i, ik;
    double **xkr;

    unsigned int nk_tot = nk_in[0] * nk_in[1] * nk_in[2];
    int nsym;

    memory->allocate(xkr, nk_tot, 3);

    for (ix = 0; ix < nk_in[0]; ++ix){
        for (iy = 0; iy < nk_in[1]; ++iy){
            for (iz = 0; iz < nk_in[2]; ++iz){
                ik = iz + iy * nk_in[2] + ix * nk_in[2] * nk_in[1];
                xkr[ik][0] = static_cast<double>(ix) / static_cast<double>(nk_in[0]);
                xkr[ik][1] = static_cast<double>(iy) / static_cast<double>(nk_in[1]);
                xkr[ik][2] = static_cast<double>(iz) / static_cast<double>(nk_in[2]);
            }
        }
    }

    if (usesym) {
        nsym = symmetry->SymmList.size();
    } else {
        nsym = 1;
    }
    reduce_kpoints(nsym, xkr, nk_in, kplist_out);

    for (ik = 0; ik < nk_tot; ++ik){
        for (i = 0; i < 3; ++i){
            xk_out[ik][i] = xkr[ik][i] - static_cast<double>(nint(xkr[ik][i]));
        }
    }

    memory->deallocate(xkr);
}

void Kpoint::reduce_kpoints(const unsigned int nsym, double **xkr, const unsigned int nk_in[3],
                            std::vector<std::vector<KpointList> > &kplist_out)
{
    unsigned int ik;
    unsigned int i, j;
    int nloc, isym;
    unsigned int nk_tot;

    bool *k_found;

    std::vector<KpointList> k_group;
    std::vector<double> ktmp;

    double srot[3][3];
    double xk_sym[3], xk_orig[3];
    double srot_inv[3][3], srot_inv_t[3][3];

    double ***symop_k;

    memory->allocate(symop_k, nsym, 3, 3);

    for (isym = 0; isym < nsym; ++isym) {

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                srot[i][j] = static_cast<double>(symmetry->SymmList[isym].rot[i][j]);
            }
        }

        invmat3(srot_inv, srot);
        transpose3(srot_inv_t, srot_inv);

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                symop_k[isym][i][j] = srot_inv_t[i][j];
            }
        }
    }

    kplist_out.clear();

    nk_tot = nk_in[0] * nk_in[1] * nk_in[2];
    memory->allocate(k_found, nk_tot);

    for (ik = 0; ik < nk_tot; ++ik) k_found[ik] = false;

    for (ik = 0; ik < nk_tot; ++ik) {

        if(k_found[ik]) continue;

        k_group.clear();

        for (i = 0; i < 3; ++i) xk_orig[i] = xkr[ik][i];

        for (isym = 0; isym < nsym; ++isym) {

            rotvec(xk_sym, xk_orig, symop_k[isym]);

            for (i = 0; i < 3; ++i) xk_sym[i] = xk_sym[i] - nint(xk_sym[i]);

//            nloc = get_knum(xk_sym[0], xk_sym[1], xk_sym[2]);

            nloc = get_knum(xk_sym, nk_in);

            if (nloc == -1) {

                error->exit("reduce_kpoints", "Cannot find the kpoint");

            } else {

                if (!k_found[nloc]) {
                    k_found[nloc] = true;
                    ktmp.clear();
                    ktmp.push_back(xk_sym[0]);
                    ktmp.push_back(xk_sym[1]);
                    ktmp.push_back(xk_sym[2]);

                    k_group.push_back(KpointList(nloc, ktmp));
                }

            }

            // Time-reversal symmetry

            if (symmetry->time_reversal_sym) {

                for (i = 0; i < 3; ++i) xk_sym[i] *= -1.0;
//                nloc = get_knum(xk_sym[0], xk_sym[1], xk_sym[2]);
                nloc = get_knum(xk_sym, nk_in);

                if (nloc == -1) {

                    error->exit("reduce_kpoints", "Cannot find the kpoint");

                } else {

                    if (!k_found[nloc]) {
                        k_found[nloc] = true;
                        ktmp.clear();
                        ktmp.push_back(xk_sym[0]);
                        ktmp.push_back(xk_sym[1]);
                        ktmp.push_back(xk_sym[2]);

                        k_group.push_back(KpointList(nloc, ktmp));
                    }
                }
            }
        }
        kplist_out.push_back(k_group);
    }

    memory->deallocate(k_found);
    memory->deallocate(symop_k);
}

void Kpoint::gen_kpoints_plane(std::vector<KpointInp> kplist,
                               std::vector<KpointPlane> *kpout,
                               std::vector<KpointPlaneTriangle> *kpout_tri) {

    int i, j;
    int nplane = kplist.size();

    int N1, N2;
    int ik1, ik2;

    int number_of_tiles, number_of_triangle_tiles;
    int **triangle;

    double frac1, frac2;
    double xk_tmp[3];
    double xk0[3], xk1[3], xk2[3], xk3[3];
    double **xk;
    double dprod, norm1, norm2, costheta;
    int n_in[2];
    int m;
    int n1, n2, n3, n4;
    bool is_inside_FBZ;
    int itri;

    for (i = 0; i < nplane; ++i) {
        N1 = std::atoi(kplist[i].kpelem[3].c_str());
        N2 = std::atoi(kplist[i].kpelem[7].c_str());

        n_in[0] = N1;
        n_in[1] = N2;

        frac1 = 1.0 / static_cast<double>(N1-1);
        frac2 = 1.0 / static_cast<double>(N2-1);

        for (j = 0; j < 3; ++j) {
            xk0[j] = 0.0;
            xk1[j] = std::atof(kplist[i].kpelem[j].c_str());
            xk2[j] = std::atof(kplist[i].kpelem[4 + j].c_str());
        }

        dprod = 0.0;
        norm1 = 0.0;
        norm2 = 0.0;

        for (j = 0; j < 3; ++j) {
            dprod += xk1[j] * xk2[j];
            norm1 += xk1[j] * xk1[j];
            norm2 += xk2[j] * xk2[j];
        }
        costheta = dprod / std::sqrt(norm1 * norm2);
        if (std::abs(std::abs(costheta) - 1.0) < eps12) {
            error->exit("gen_kpoints_plane", "Two vectors have to be linearly independent with each other.");
        }

        kp_plane_geometry.push_back(KpointPlaneGeometry(xk0, xk1, xk2, n_in));

        for (ik1 = 0; ik1 < N1; ++ik1) {
            for (ik2 = 0; ik2 < N2; ++ik2) {

                for (j = 0; j < 3; ++j) {
                    xk_tmp[j] = static_cast<double>(ik1) * frac1 * xk1[j] + static_cast<double>(ik2) * frac2 * xk2[j];
                }
                if (in_first_BZ(xk_tmp)) {
                    n_in[0] = ik1;
                    n_in[1] = ik2;
                    kpout[i].push_back(KpointPlane(xk_tmp, n_in));
                }
            }
        }

        memory->allocate(xk, N1*N2, 3);

        m = 0;
        for (ik1 = 0; ik1 < N1; ++ik1) {
            for (ik2 = 0; ik2 < N2; ++ik2) {

                for (j = 0; j < 3; ++j) {
                    xk[m][j] = static_cast<double>(ik1) * frac1 * xk1[j] + static_cast<double>(ik2) * frac2 * xk2[j];
                }
                ++m;
            }
        }

        number_of_tiles = (N1 - 1) * (N2 - 1);
        number_of_triangle_tiles = 2 * number_of_tiles;

        memory->allocate(triangle, number_of_triangle_tiles, 3);

        for (ik1 = 0; ik1 < N1 - 1; ++ik1) {
            for (ik2 = 0; ik2 < N2 - 1; ++ik2) {

                n1 = ik2 + ik1 * N2;
                n2 = ik2 + (ik1 + 1) * N2;
                n3 = (ik2 + 1) + ik1 * N2;
                n4 = (ik2 + 1) + (ik1 + 1) * N2;

                m = 2 * (ik2 + ik1 * (N2 - 1));

                triangle[m][0] = n1;
                triangle[m][1] = n2;
                triangle[m][2] = n4;

                ++m;

                triangle[m][0] = n1;
                triangle[m][1] = n3;
                triangle[m][2] = n4;

            }
        }

        for (itri = 0; itri < number_of_triangle_tiles; ++itri) {

            for (j = 0; j < 3; ++j) {
                xk1[j] = xk[triangle[itri][0]][j];
                xk2[j] = xk[triangle[itri][1]][j];
                xk3[j] = xk[triangle[itri][2]][j];
            }

            is_inside_FBZ = in_first_BZ(xk1) || in_first_BZ(xk2) || in_first_BZ(xk3);
            if (is_inside_FBZ) {
                kpout_tri[i].push_back(KpointPlaneTriangle(itri, triangle[itri]));
            }
        }

        memory->deallocate(xk);
        memory->deallocate(triangle);
    }
}

void Kpoint::gen_nkminus(const unsigned int nk, unsigned int *minus_k, double **xk_in)
{
    unsigned int ik;
    int ik_minus;

    for (ik = 0; ik < nk; ++ik) {

        ik_minus = get_knum(-xk[ik][0], -xk[ik][1], -xk[ik][2]);

        if (ik_minus == -1) error->exit("gen_nkminus", "-xk doesn't exist on the mesh point.");
        if (ik_minus < ik) continue;

        minus_k[ik] = ik_minus;
        minus_k[ik_minus] = ik;
    }
}

int Kpoint::get_knum(const double kx, const double ky, const double kz)
{

    double diff[3];
    double dkx = static_cast<double>(nkx);
    double dky = static_cast<double>(nky);
    double dkz = static_cast<double>(nkz);

    diff[0] = static_cast<double>(nint(kx*dkx)) - kx*dkx;
    diff[1] = static_cast<double>(nint(ky*dky)) - ky*dky;
    diff[2] = static_cast<double>(nint(kz*dkz)) - kz*dkz;

    double norm = std::sqrt(diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]);

    if (norm >= eps12) {

        return -1;

    } else {

        int iloc, jloc, kloc;

        iloc = (nint(kx*dkx + 2.0 * dkx)) % nkx;
        jloc = (nint(ky*dky + 2.0 * dky)) % nky;
        kloc = (nint(kz*dkz + 2.0 * dkz)) % nkz;

        return kloc + nkz * jloc + nky * nkz * iloc;
    }
}

int Kpoint::get_knum(const double xk[3], const unsigned int nk[3])
{
    int i;
    double diff[3];
    double dnk[3];
    double norm;

    for (i = 0; i < 3; ++i) dnk[i]= static_cast<double>(nk[i]);
    for (i = 0; i < 3; ++i) diff[i] = static_cast<double>(nint(xk[i]*dnk[i])) - xk[i] * dnk[i];

    norm = std::sqrt(diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]);

    if (norm >= eps12) {

        return -1;

    } else {

        int iloc, jloc, kloc;

        iloc = (nint(xk[0]*dnk[0] + 2.0 * dnk[0])) % nk[0];
        jloc = (nint(xk[1]*dnk[1] + 2.0 * dnk[1])) % nk[1];
        kloc = (nint(xk[2]*dnk[2] + 2.0 * dnk[2])) % nk[2];

        return kloc + nk[2] * jloc + nk[1] * nk[2] * iloc;
    }
}

bool Kpoint::in_first_BZ(double *xk_in) {

    int i, j, k;
    int nmax = 1;
    double tmp[3];
    double dist, dist_min;
    int iloc;
    int ncount;

    bool ret;

    for (i = 0; i < 3; ++i) tmp[i] = xk_in[i];

    rotvec(tmp, tmp, system->rlavec_p, 'T');

    dist_min = std::sqrt(tmp[0] * tmp[0] + tmp[1] * tmp[1] + tmp[2] * tmp[2]);

    ncount = 0;

    iloc = ncount;

    for (i = -nmax; i <= nmax; ++i) {
        for (j = -nmax; j <= nmax; ++j) {
            for (k = -nmax; k <= nmax; ++k) {

                if (i == 0 && j == 0 && k == 0) continue;

                ++ncount;

                tmp[0] = xk_in[0] - static_cast<double>(i);
                tmp[1] = xk_in[1] - static_cast<double>(j);
                tmp[2] = xk_in[2] - static_cast<double>(k);

                rotvec(tmp, tmp, system->rlavec_p, 'T');
                dist = std::sqrt(tmp[0] * tmp[0] + tmp[1] * tmp[1] + tmp[2] * tmp[2]);

                if (dist < dist_min) {
                    iloc = ncount;
                    break;
                }
            }
        }
    }

    if (iloc == 0) {
        ret = true;
    } else {
        ret = false;
    }

    return ret;
}

void Kpoint::generate_irreducible_kmap(int *kequiv, unsigned int &nk_irreducible, std::vector<int> &k_irreducible,
                                       const unsigned int n1, const unsigned int n2, const unsigned int n3,
                                       double **xk_in, const int nsymop, int ***symrot)
{
    int i, j, k;
    int isym;
    int nktot = n1 * n2 * n3;
    int ksym;

    double xk_orig[3], xk_sym[3];
    double srot[3][3], srot_inv[3][3], srot_inv_t[3][3];

    for (i = 0; i < nktot; ++i) kequiv[i] = i;

    for (i = 0; i < nktot; ++i) {

        if (kequiv[i] < i) continue;

        for (j = 0; j < 3; ++j) xk_orig[j] = xk_in[i][j];

        for (isym = 0; isym < nsymop; ++isym) {

            for (j = 0; j < 3; ++j) {
                for (k = 0; k < 3; ++k) {
                    srot[j][k] = symrot[isym][j][k];
                }
            }

            invmat3(srot_inv, srot);
            transpose3(srot_inv_t, srot_inv);

            rotvec(xk_sym, xk_orig, srot_inv_t);

            ksym = get_knum(xk_sym[0], xk_sym[1], xk_sym[2]);

            if (ksym > kequiv[i]) {
                kequiv[ksym] = i;
            }
        }
    }

    nk_irreducible = 0;
    k_irreducible.clear();

    std::map<int, int> map_to_irreducible_index;

    for (i = 0; i < nktot; ++i) {
        if (kequiv[i] == i) {
            map_to_irreducible_index.insert(std::map<int, int>::value_type(i, nk_irreducible));
            ++nk_irreducible;
            k_irreducible.push_back(i);
        }
    }

    for (i = 0; i < nktot; ++i) {
        kequiv[i] = map_to_irreducible_index[kequiv[i]];
    }

    map_to_irreducible_index.clear();
}


#ifndef CORRESPONDENCEFILTER_HPP
#define CORRESPONDENCEFILTER_HPP

#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <stdio.h>
#include "../global.hpp"
#include "NeighbourFinder.hpp"
#include "helper_functions.hpp"
#include "BaseCorrespondenceFilter.hpp"

typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::Matrix< float, 1, registration::NUM_FEATURES> FeatureVec; //matrix Mx6 of type float
typedef Eigen::Vector3f Vec3Float;
typedef Eigen::SparseMatrix<float, 0, int> SparseMat;
typedef Eigen::Triplet<float> Triplet;
typedef Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic> MatDynInt; //matrix MxN of type unsigned int
typedef Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic> MatDynFloat;

namespace registration {

class CorrespondenceFilter : public BaseCorrespondenceFilter
{

    /*
    # GOAL
    For each element in inFloatingFeatures, we're going to find corresponding
    features in the elements of inTargetFeatures.
    The used weight is one over the distance squared.

    # INPUTS
    -inFloatingFeatures
    -inTargetFeatures
    -inTargetFlags

    # PARAMETERS
    -numNeighbours(=3):
    number of nearest neighbours
    -flagThreshold:
    threshold that the weighted corresponding flag needs to make in order to be flagged as 1.0.
    Otherwise, it receives flag 0.0f.

    # OUTPUT
    -outCorrespondingFeatures
    -outCorrespondingFlags
    */

    public:
        //CorrespondenceFilter(); //default constructor
        //~CorrespondenceFilter(); //destructor

        void set_floating_input(const FeatureMat * const inFloatingFeatures,
                                const VecDynFloat * const inFloatingFlags);
        void set_target_input(const FeatureMat * const inTargetFeatures,
                            const VecDynFloat * const inTargetFlags);
        SparseMat get_affinity() const {return _affinity;}
        void set_parameters(const size_t numNeighbours,
                            const float flagThreshold);
        void set_affinity_normalization(const bool normalizeAffinity = true);
        void update();

    protected:

    private:

        //# Internal Data structures
        NeighbourFinder<FeatureMat> _neighbourFinder;

        //# Internal Parameters
        size_t _numAffinityElements = 0;
        //# Normalize affinity matrix
        bool _normalizeAffinity = true;

        //# Internal Functions
        //## Function to update the sparse affinity matrix
        void _update_affinity();
        //## Function to convert the sparse affinity weights into corresponding
        //## features and flags
        void _affinity_to_correspondences();

};

}//namespace registration

#endif // CORRESPONDENCEFILTER_HPP


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/random.hpp>
#include <boost/iterator/counting_iterator.hpp>
using namespace boost;

///////////////////////////////////////

void case1()
{
    counting_iterator<int> i(100);

    assert(*i++ == 100);
    assert(*i == 101);
    assert(*++i == 102);

    vector<int> v;
    std::copy(
        make_counting_iterator(0),
        make_counting_iterator(10),
        back_inserter(v)
        );
}

///////////////////////////////////////

template<typename R>
class rand_int
{
private:
    R &r;
    int count;
public:
    rand_int(R& _r, int c = 0):
        r(_r), count(c) {}
    rand_int(rand_int const &other) = default;
    rand_int& operator=(rand_int const &other) = default;

    void operator++()
    {   ++count;}

    friend bool operator==(rand_int const &l, rand_int const &r)
    {   return l.count == r.count;}

    operator typename R::result_type () const
    {   return r(); }
};

void case2()
{
    typedef counting_iterator<rand_int<rand48>,
        boost::single_pass_traversal_tag, int> RandIter;

    boost::rand48 r;

    rand_int<rand48> r1(r, 0), r2(r, 10);
    RandIter first(r1), last(r2);

    vector<int> v;
    std::copy(first, last, back_inserter(v));
    assert(v.size() == 10);

}

///////////////////////////////////////

int main()
{
    std::cout << "hello counting_iterator" << std::endl;

    case1();
    case2();
}


#ifndef _PRIM_HPP
#define _PRIM_HPP

/////////////////////////////////////////////////////////////////////////////
// Name:        src/lev/prim.hpp
// Purpose:     header for primitive expressions
// Author:      Akiva Miura <akiva.miura@gmail.com>
// Created:     01/05/2011
// Copyright:   (C) 2010-2011 Akiva Miura
// Licence:     MIT License
/////////////////////////////////////////////////////////////////////////////

#include "base.hpp"

#include <boost/shared_ptr.hpp>
#include <luabind/luabind.hpp>
#include <string>

extern "C" {
  int luaopen_lev_prim(lua_State *L);
}

namespace lev
{

  class color : public base
  {
    public:
      typedef boost::shared_ptr<color> ptr;

      // ctors
      color(const color &orig);
      color(unsigned char r = 0, unsigned char g = 0, unsigned char b = 0, unsigned char a = 255);
      color(unsigned long argb_code);

      color::ptr clone();
      static color::ptr create(unsigned char r, unsigned char g,
                               unsigned char b, unsigned char a);
      static int create_l(lua_State *L);
      unsigned char get_a() const { return a; }
      unsigned char get_b() const { return b; }
      unsigned char get_g() const { return g; }
      unsigned char get_r() const { return r; }
      unsigned long get_code32() const;
      const std::string get_codestr() const;
      bool set_a(unsigned char new_a) { a = new_a; return true; }
      bool set_b(unsigned char new_b) { b = new_b; return true; }
      bool set_g(unsigned char new_g) { g = new_g; return true; }
      bool set_r(unsigned char new_r) { r = new_r; return true; }
      virtual type_id get_type_id() const { return LEV_TCOLOR; }

      static color::ptr black() { return color::create(0, 0, 0, 255); }
      static color::ptr blue()  { return color::create(0, 0, 255, 255); }
      static color::ptr green() { return color::create(0, 255, 0, 255); }
      static color::ptr red()   { return color::create(255, 0, 0, 255); }
      static color::ptr white() { return color::create(255, 255, 255, 255); }
      static color::ptr transparent() { return color::create(0, 0, 0, 0); }

    protected:
      unsigned char r, g, b, a;
  };

  class size : public base
  {
    public:
      typedef boost::shared_ptr<size> ptr;

      size(int w, int h, int d = 0) : w(w), h(h), d(d), base() {}
      bool assign(int w, int h, int d = 0);
      bool assign_size(const size &sz) { return assign(sz.w, sz.h, sz.d); }
      static boost::shared_ptr<size> create(int w, int h, int d = 0);
      static int create_l(lua_State *L);
      int get_d() const { return d; }
      int get_h() const { return h; }
      int get_w() const { return w; }
      virtual type_id get_type_id() const { return LEV_TSIZE; }
      bool set_d(int new_d) { d = new_d; return true; }
      bool set_h(int new_h) { h = new_h; return true; }
      bool set_w(int new_w) { w = new_w; return true; }
    private:
      int w;
      int h;
      int d;
  };

  class vector : public base
  {
    public:
      vector(const vector &orig);
      vector(int x = 0, int y = 0, int z = 0) : x(x), y(y), z(z) { }
      bool assign(int x = 0, int y = 0, int z = 0);
      bool assign_vector(const vector &vec) { return assign(vec.x, vec.y, vec.z); }
      static boost::shared_ptr<vector> create(int x = 0, int y = 0, int z = 0);
      static int create_l(lua_State *L);
      int get_x() const { return x; }
      int get_y() const { return y; }
      int get_z() const { return z; }
      virtual type_id get_type_id() const { return LEV_TVECTOR; }
      bool set_x(int new_x) { x = new_x; }
      bool set_y(int new_y) { y = new_y; }
      bool set_z(int new_z) { z = new_z; }
      vector operator+(vector &rhs)
      { return vector(x + rhs.x, y + rhs.y, z + rhs.z); }
    protected:
      int x;
      int y;
      int z;
  };

  class point : public base
  {
    protected:
      point();
    public:
      virtual ~point();
      bool clear_color();
      static boost::shared_ptr<point> create(vector *vec, color *col);
      static int create_l(lua_State *L);
      boost::shared_ptr<color> get_color()  { return col; }
      boost::shared_ptr<vector> get_vertex() { return vertex; }
      virtual type_id get_type_id() const { return LEV_TPOINT; }
      bool set_color(color *c);
      bool set_vertex(vector *v);
    protected:
      boost::shared_ptr<vector> vertex;
      boost::shared_ptr<color> col;
  };

  class rect : public base
  {
    public:
      typedef boost::shared_ptr<rect> ptr;

      rect(int x = 0, int y = 0, int w = 0, int h = 0);
      rect(const vector &pos, const size &sz);
      rect(const rect &r);
      virtual ~rect();
      bool assign(int x, int y, int w, int h);
      bool assign_position_size(const vector &v, const size &sz);
      bool assign_rect(const rect &r);
      static boost::shared_ptr<rect> create(int x, int y, int w, int h);
      static int create_l(lua_State *L);
      int get_bottom() const { return get_y() + sz.get_h(); }
      int get_h() const { return sz.get_h(); }
      int get_left() const { return pos.get_x(); }
      vector* get_position() { return &pos; }
      int get_right() const { return get_x() + sz.get_w(); }
      size*   get_size() { return &sz; }
      int get_top() const { return pos.get_y(); }
      int get_x() const { return pos.get_x(); }
      int get_y() const { return pos.get_y(); }
      int get_w() const { return sz.get_w(); }
      virtual type_id get_type_id() const { return LEV_TRECT; }
      bool include(int x, int y) const;
      bool include1(const vector &p) const { return include(p.get_x(), p.get_y()); }
      bool set_bottom(int bottom) { return sz.set_h(bottom - get_y()); }
      bool set_h(int h) { return sz.set_h(h); }
      bool set_left(int left) { return pos.set_x(left); }
      bool set_position(const vector &vec);
      bool set_right(int right) { return sz.set_w(right - get_x()); }
      bool set_size(const size &new_size);
      bool set_top(int top) { return pos.set_y(top); }
      bool set_x(int x) { return pos.set_x(x); }
      bool set_y(int y) { return pos.set_y(y); }
      bool set_w(int w) { return sz.set_w(w); }

      const rect& operator=(const rect &rhs);
    protected:
      vector pos;
      size   sz;
  };

}

#endif // _PRIM_HPP


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ConstExprReduceProdTest

#include <boost/optional.hpp>
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/names.hpp>
#include <popart/tensordata.hpp>
#include <popart/testdevice.hpp>

using namespace popart;

const Shape SHAPE{2, 3, 5};
const int64_t RANK = SHAPE.size();

void check_combination(nonstd::optional_lite::optional<Shape> axes,
                       int64_t keepdims) {
  // Print out axes and keepdims
  BOOST_TEST_MESSAGE("keepdims: " + std::to_string(keepdims));
  std::string axes_string;
  if (axes.has_value()) {
    for (auto axis : axes.value()) {
      axes_string += std::to_string(axis) + " ";
    }
    BOOST_TEST_MESSAGE("axes: " + axes_string);
  } else
    BOOST_TEST_MESSAGE("no axes provided");
  BOOST_TEST_MESSAGE("");

  // Build the graph
  std::vector<float> raw_const_data(SHAPE[0] * SHAPE[1] * SHAPE[2], 1.0);
  ConstVoidData const_data = {raw_const_data.data(), {"FLOAT", SHAPE}};

  TensorInfo X_info{"FLOAT", SHAPE};

  auto builder = Builder::create();
  auto X       = builder->addInputTensor(X_info);
  auto aiOnnx  = builder->aiOnnxOpset11();
  auto C       = aiOnnx.constant(const_data);

  TensorId reduceprod_node_1, reduceprod_node_2;
  reduceprod_node_1 = aiOnnx.reduceprod({C}, axes, keepdims);
  reduceprod_node_2 = aiOnnx.reduceprod({X}, axes, keepdims);

  auto output = aiOnnx.add({reduceprod_node_1, reduceprod_node_2});

  auto proto       = builder->getModelProto();
  auto model_proto = io::getModelFromString(proto);

  auto data_flow = DataFlow(1, {{output, AnchorReturnType("All")}});
  auto device    = createTestDevice(TEST_TARGET);

  // Create the IR, adding output as an anchor
  Ir ir;
  ir.prepare({model_proto,
              InputShapeInfo(),
              data_flow,
              TensorId(),
              nullptr,
              *device,
              {}, // no SessionOptions
              Patterns({}).enableRuntimeAsserts(false)});

  // 1) check that the Add Op is present
  BOOST_CHECK_MESSAGE(
      ir.opsOfType(Onnx::AiOnnx::OpSet11::Add).size() == 1,
      "Number of Add nodes is incorrect: " +
          std::to_string(ir.opsOfType(Onnx::AiOnnx::OpSet11::Add).size()));
  // 2) check that one of the ReduceProd ops has been constfolded
  BOOST_CHECK_MESSAGE(
      ir.opsOfType(Onnx::AiOnnx::OpSet11::ReduceProd).size() == 1,
      "Number of ReduceProd nodes is incorrect: " +
          std::to_string(
              ir.opsOfType(Onnx::AiOnnx::OpSet11::ReduceProd).size()));
  // This test should also error out if constant folding produces a different
  // shape than ReduceProd on X
}

BOOST_AUTO_TEST_CASE(ConstExprTest_ReduceProd) {
  // clang-format off
  //
  // With C being a constant, this test contructs the following graph:
  //
  // (C) --> [ReduceProd] --> [Add] --> (*)
  // (X) --> [ReduceProd] ----^
  //
  // After constant folding the graph should look like this:
  //
  //          (reduced C) --> [Add] --> (*)
  // (X) --> [ReduceProd] ----^
  //
  // The reduction is tried over all possible combinations of axes and keepdims values for ReduceProd.
  //
  // clang-format on

  // positive axes
  for (int combination = 0; combination < (1 << RANK); ++combination) {
    Shape axes;
    for (int i = 0; i < RANK; ++i) {
      if ((combination >> i) & 1)
        axes.push_back(i);
    }
    check_combination(axes, 0);
    check_combination(axes, 1);
  }

  // negative axes
  for (int combination = 1; combination < (1 << RANK); ++combination) {
    Shape axes;
    for (int i = 0; i < RANK; i++) {
      if ((combination >> i) & 1)
        axes.push_back(-i - 1);
    }
    check_combination(axes, 0);
    check_combination(axes, 1);
  }

  // no axes
  check_combination(nonstd::nullopt, 0);
  check_combination(nonstd::nullopt, 1);

  // duplicated axes
  check_combination(Shape{1, 1}, 0);
  check_combination(Shape{1, -2}, 1);

  // mixed negative and positive axes
  check_combination(Shape{1, -1}, 0);
  check_combination(Shape{1, -3}, 1);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef ITL_BASE_SOLVER_INCLUDE
#define ITL_BASE_SOLVER_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/itl/iteration/cyclic_iteration.hpp>
#include <boost/numeric/itl/iteration/default_iteration.hpp>

namespace itl {

/// Base class (CRTP) for iterative solver classes like \ref cg_solver
template <typename Solver, typename LinearOperator>
struct base_solver
{
    typedef typename mtl::Magnitude<typename mtl::Collection<LinearOperator>::value_type>::type magnitude_type;
    typedef cyclic_iteration<magnitude_type>                                                    iteration_type;

    base_solver(const LinearOperator& A, iteration_type iter= default_iteration<magnitude_type>())
      : A(A), my_iteration(iter), level(1)
    { my_iteration.set_quite(true); }

    /// Perform one iteration on linear system
    template <typename HilbertSpaceB, typename HilbertSpaceX>
    int step(HilbertSpaceX& x, const HilbertSpaceB& b) const
    {
	itl::basic_iteration<double> iter(b, 1, 0, 0);
	return static_cast<Solver const*>(this)->solve(x, b, iter);
    }

    /// Solve using the iteration (resets initial residuum)
    template <typename HilbertSpaceB, typename HilbertSpaceX>
    int operator()(HilbertSpaceX& x, const HilbertSpaceB& b)
    {
	my_iteration.restart();
	if (two_norm(x) == 0)
	    my_iteration.set_norm_r0(two_norm(b));
	else {
	    HilbertSpaceB r(b);
	    r-= A * x;
	    my_iteration.set_norm_r0(two_norm(r));
	}
	return static_cast<Solver const*>(this)->solve(x, b, my_iteration);
    }

    /// Set log level to \p level
    /** Concrete meaning of log level depends on specific solver.
	The general understanding is:
	- Level 0: no logging at all
	- Level 1: print final log (convergence and alike)
	- Level 2: print log on iterations
	- Level 3 or higher: treated as 2   **/
    void set_log_level(unsigned level)
    {
	this->level= level;
	my_iteration.set_quite(level < 2);
	my_iteration.suppress_resume(level == 0);
    }

    /// Get log level
    unsigned log_level() const { return level; }

    /// Return iteration object, \sa log_level, set_log_level
    iteration_type iteration() const { return my_iteration; }

    /// Reference to iteration object (to be used with care), \sa log_level, set_log_level
    iteration_type& iteration_ref() { return my_iteration; }

  protected:
    const LinearOperator& A;
    iteration_type my_iteration;
    unsigned level;
};

} // namespace itl

#endif // ITL_BASE_SOLVER_INCLUDE


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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 FCPPT_MATH_DETAIL_ARRAY_EQUAL_HPP_INCLUDED
#define FCPPT_MATH_DETAIL_ARRAY_EQUAL_HPP_INCLUDED

#include <fcppt/detail/equal.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/utility/enable_if.hpp>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace math
{
namespace detail
{

template<
	typename T1,
	typename T2
>
inline
typename
boost::enable_if<
	boost::mpl::equal_to<
		typename
		T1::static_size,
		typename
		T2::static_size
	>,
	bool
>::type
array_equal(
	T1 const &_v1,
	T2 const &_v2
)
{
	auto const &storage1(
		_v1.storage()
	);

	return
		fcppt::detail::equal(
			storage1.begin(),
			storage1.end(),
			_v2.storage().begin()
		);
}

}
}
}

#endif


//////////////////////////////////////////////////////////////////////////////
// detail::multi_array::serialization::save.hpp                             //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_MULTI_ARRAY_SERIALIZATION_SAVE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_MULTI_ARRAY_SERIALIZATION_SAVE_HPP_ER_2009
#include <boost/multi_array.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>
//#include <boost/archive/text_oarchive.hpp>

namespace boost{ namespace serialization{

    template<typename Archive,typename T>
    inline void save(
        Archive & ar,
        const boost::multi_array<T,2> & t,
        const unsigned int file_version
    )
	{
    	typedef boost::multi_array<T,2> multi_array_;
        typedef typename multi_array_::size_type size_;

        size_ n0 = (t.shape()[0]);
        ar << BOOST_SERIALIZATION_NVP(n0);
        size_ n1 = (t.shape()[1]);
        ar << BOOST_SERIALIZATION_NVP(n1);
        ar << boost::serialization::make_array(t.data(), t.num_elements());
    }

}
}

#endif


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

#include <iostream>
#include <unordered_map>

#include <Eigen/Dense>
#include <third_party/cmdLine/cmdLine.h>
#include <third_party/progress/progress_display.hpp>
#include <third_party/stlplus3/filesystemSimplified/file_system.hpp>

#include <openMVG/sfm/sfm_data.hpp>
#include <openMVG/sfm/sfm_data_io.hpp>

#include <higher_order/structure_estimation.hpp>

using namespace openMVG;
using namespace openMVG::sfm;
using namespace openMVG::geometry;

void EstimateNormalsForLandmark(
  const SfM_Data& sfm_data,
  Landmark& landmark,
  bool use_observed_region_centers = false);

void FlipNormals(SfM_Data& sfm_data);

int main(int argc, char** argv) {
  CmdLine cmd;

  std::string sSfM_Data_Filename;
  std::string sOutDir = "";

  cmd.add(make_option('i', sSfM_Data_Filename, "sfmdata"));
  cmd.add(make_option('o', sOutDir, "outdir"));

  try {
    if (argc == 1) {
      throw std::string("Invalid command line parameter.");
    }
    cmd.process(argc, argv);
  }
  catch (const std::string& s) {
    std::cerr << "Usage: " << argv[0] << '\n'
      << "[-i|--sfmdata] filename, the SfM_Data file\n"
      << "[-o|--outdir] path where refined SfM_Data with refined LAFs\n"
      << std::endl;

    std::cerr << s << std::endl;
    return EXIT_FAILURE;
  }

  if (!stlplus::folder_exists(sOutDir)) {
    stlplus::folder_create(sOutDir);
  }

  // Read the input SfM scene
  SfM_Data sfm_data;
  if (!Load(sfm_data, sSfM_Data_Filename, ESfM_Data(ALL))) {
    std::cerr << std::endl
      << "The input SfM_Data file \"" << sSfM_Data_Filename
      << "\" cannot be read." << std::endl;
    return EXIT_FAILURE;
  }

  // Perform surface normal estimation
  C_Progress_display my_progress_bar(sfm_data.GetLandmarks().size(), std::cout,
    "\n- PROCESSING LANDMARKS (Estimating surface normals) -\n");
  for (auto& landmark : sfm_data.structure) {
    auto& obs = landmark.second.obs;

    EstimateNormalsForLandmark(sfm_data, landmark.second);

    ++my_progress_bar;
  }

  // Make the surface normals orient towards viewpoints
  FlipNormals(sfm_data);

  // Save the output scene as PLY file
  std::string output_Filename = stlplus::create_filespec(
    sOutDir, stlplus::basename_part(sSfM_Data_Filename), "ply");
  if (!Save(sfm_data, output_Filename, ESfM_Data(ALL))) {
    std::cerr << std::endl
      << "Cannot write to output PLY file \"" << output_Filename
      << "\"." << std::endl;
    return EXIT_FAILURE;
  }
}

void EstimateNormalsForLandmark(
  const SfM_Data& sfm_data,
  Landmark& landmark,
  bool use_observed_region_centers)
{
  using namespace higher_order;

  auto& obs = landmark.obs;

  std::vector<Mat23> gradients(obs.size());
  std::vector<Mat2> Ms(obs.size());

  // populate LAFs and pre-evaluate bearing vectors and gradients, etc
  auto index = 0;
  for (const auto& observation : obs) {
    auto viewID = observation.first;
    auto& ob = observation.second;
    const Mat2 M = ob.M;

    const auto view = sfm_data.views.at(viewID).get();
    const auto intrinsic = sfm_data.intrinsics.at(view->id_intrinsic).get();
    const auto& pose = sfm_data.GetPoseOrDie(view);

    const Vec3 Y = use_observed_region_centers
      // case1: without scale, using the bearing vector
      ? intrinsic->operator()(intrinsic->get_ud_pixel(ob.x))
      // case2: using correct and accurate scale (known depth)
      : pose(landmark.X);

    gradients[index] =
      intrinsic->project_gradient(Y, false) *
      pose.rotation();

    Ms[index] = ob.M;

    ++index;
  }

  // Estimate normal using FNE
  landmark.N = stereo::EstimateNormal_FNE(
    gradients[0], gradients[1],
    Ms[1] * Ms[0].inverse());
}

void FlipNormals(SfM_Data& sfm_data) {
  for (auto& landmark : sfm_data.structure) {
    int num_inverted = 0;
    for (const auto& ob : landmark.second.obs) {
      const auto& view = sfm_data.GetViews().at(ob.first).get();
      const auto& pose = sfm_data.GetPoses().at(view->id_pose);
      const auto& intrinsic =
        sfm_data.GetIntrinsics().at(view->id_intrinsic).get();

      Vec3 v = pose.rotation().transpose() * intrinsic->operator()(ob.second.x);
      if (landmark.second.N.dot(v) > 0) ++num_inverted;
    }

    if (num_inverted > landmark.second.obs.size() / 2) landmark.second.N *= -1;
  }
}