/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : depth_segmentation_node.cpp                            **
** Module : ICLGeom                                                **
** Authors: Andre Ueckermann (original file in icl)                **
**          Fadri Furrer <fadri.furrer@mavt.ethz.ch> (ROS node)    **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/

// This has to be defined, otherwise we are getting namespace
// conflicts, as `cv` is defined in opencv and also in the ICL library.
#define ICL_NO_USING_NAMESPACES
#include <cv.h>

#include <ICLCore/CCFunctions.h>
#include <ICLCore/CoreFunctions.h>
#include <ICLCore/OpenCV.h>
#include <ICLCore/PseudoColorConverter.h>
#include <ICLGeom/ConfigurableDepthImageSegmenter.h>
#include <ICLGeom/Scene.h>
#include <ICLQt/Common.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/common/common_headers.h>
#include <pcl/common/pca.h>
#include <pcl/conversions.h>
#include <pcl/io/ply_io.h>
#include <pcl/pcl_config.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/image_encodings.h>
#include <std_msgs/Int64.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

// typedef pcl::PointXYZI PointType;
typedef pcl::PointXYZRGB PointType;

using namespace icl::core;
using namespace icl::geom;
using namespace icl::utils;
using namespace icl::qt;

icl::qt::HSplit gui;

ros::Publisher point_cloud2_pub;
ros::Publisher segment_size_pub;

// GenericGrabber grabDepth, grabColor;
int KINECT_CAM = 0, VIEW_CAM = 1;

Camera rgb_cam;
Camera depth_cam;

// ImgBase* depth_image;
ImgParams rgb_params;
ImgParams depth_params;
ImgBase* rgb_image;
ImgBase* depth_image;

uint64 rgb_width;
uint64 rgb_height;
uint64 depth_width;
uint64 depth_height;

constexpr uint64 kMinSegmentSize = 1000;
constexpr uint64 kMinLabeledPointSize = 100;
constexpr uint64 kColorConverterMaxValue = 4000;
constexpr double kMinZDistance = 0.2;
constexpr double kMaxZDistance = 2.0;
constexpr double kThresholdDist = 10000;
constexpr double kPcaMaxEigenValueScale = 1e-2;
constexpr bool kPointCloudIsOrganized = true;
constexpr bool kPointCloudWithNormals = true;
constexpr bool kPointCloudWithColors = true;
constexpr bool kPointCloudWithLabels = true;

bool write_segment_files = false;
bool filter_thin_elements = true;
bool rgb_info_ready = false;
bool depth_info_ready = false;
bool rgb_ready = false;
bool depth_ready = true;
bool initialized = false;

// DepthPreference depth_preference;
cv_bridge::CvImagePtr cv_depth_image;
cv_bridge::CvImagePtr cv_rgb_image;

PseudoColorConverter* pseudo_color_converter;
ConfigurableDepthImageSegmenter* segmentation;
PointCloudObject* point_cloud_object;
Scene scene;

struct AdaptedSceneMouseHandler : public MouseHandler {
  Mutex mutex;
  MouseHandler* h;

  AdaptedSceneMouseHandler(MouseHandler* h) : h(h) {}

  void process(const MouseEvent& e) {
    Mutex::Locker l(mutex);
    h->process(e);
  }

}* mouse = 0;

void rgbCameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg) {
  if (rgb_info_ready) {
    return;
  } else {
    rgb_width = msg->width;
    rgb_height = msg->height;
    rgb_params = ImgParams(rgb_width, rgb_height, formatRGB);
    rgb_cam.setPrincipalPointOffset(msg->K[2], msg->K[5]);
    rgb_cam.setFocalLength(msg->K[0]);
    rgb_image = imgNew(depth8u, rgb_params);
    rgb_cam = Camera();
    rgb_cam.setResolution(Size(rgb_width, rgb_height));
    rgb_info_ready = true;
    ROS_INFO("Got rgb camera info.");
  }
}

void depthCameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg) {
  if (depth_info_ready) {
    return;
  } else {
    depth_width = msg->width;
    depth_height = msg->height;
    depth_params = ImgParams(depth_width, depth_height, formatMatrix);
    depth_cam.setPrincipalPointOffset(msg->K[2], msg->K[5]);
    depth_cam.setFocalLength(msg->K[0]);
    depth_image = imgNew(depth32f, depth_params);
    depth_cam = Camera();
    depth_cam.setResolution(Size(depth_width, depth_height));
    depth_info_ready = true;
    ROS_INFO("Got depth camera info.");
  }
}

void rgbImageCallback(const sensor_msgs::Image::ConstPtr& msg) {
  if (!rgb_info_ready || !initialized) {
    return;
  }
  // TODO(ff): Add a mutex here.
  rgb_ready = false;
  cv_rgb_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
  cv::Mat rgb_mat;
  cv_rgb_image->image.convertTo(rgb_mat, CV_BGR2RGB);
  mat_to_img(&rgb_mat, rgb_image);
  rgb_ready = true;
}

void depthImageCallback(const sensor_msgs::Image::ConstPtr& msg) {
  if (!depth_info_ready || !initialized) {
    return;
  }
  // TODO(ff): Add a mutex here.
  depth_ready = false;
  cv_depth_image =
      cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1);

  cv::Mat float_depth_map;
  cv_depth_image->image.convertTo(float_depth_map, CV_32FC1, 1, 0.0f);

  cv::Mat truncated_depth_map;
  cv::threshold(float_depth_map, truncated_depth_map, kThresholdDist, 1,
                cv::THRESH_TRUNC);

  cv::Mat no_zero_values = truncated_depth_map.clone();
  no_zero_values.setTo(0.0f, (truncated_depth_map == kThresholdDist));

  mat_to_img(&no_zero_values, depth_image);
  depth_ready = true;
}

void init() {
  while (!rgb_info_ready || !depth_info_ready) {
    ROS_INFO("Spinning, waiting for rgb and depth camera info messages.");
    ros::spinOnce();
    ros::Duration(1.0).sleep();
  }

  Size size = pa("-size");

  if (pa("-d")) {  // get depth cam
    std::string depth_cam_name = pa("-d").as<std::string>();
    depth_cam = Camera(depth_cam_name);
    depth_cam.setResolution(size);
  } else {  // default depth cam
    depth_cam = Camera();
    depth_cam.setResolution(size);
    depth_cam.setName("Depth Camera");
  }

  // initialize depth heatmap mapping
  std::vector<PseudoColorConverter::Stop>
      color_converter_stops;  // create heatmap
  Color red(255, 0, 0);
  Color yellow(255, 255, 0);
  Color green(0, 255, 0);
  Color cyan(0, 255, 255);
  Color blue(0, 0, 255);
  // TODO(ff): Make the ColorConverter stops params.
  color_converter_stops.push_back(PseudoColorConverter::Stop(0.25, red));
  color_converter_stops.push_back(PseudoColorConverter::Stop(0.35, yellow));
  color_converter_stops.push_back(PseudoColorConverter::Stop(0.45, green));
  color_converter_stops.push_back(PseudoColorConverter::Stop(0.55, cyan));
  color_converter_stops.push_back(PseudoColorConverter::Stop(0.8, blue));
  pseudo_color_converter =
      new PseudoColorConverter(color_converter_stops, kColorConverterMaxValue);

  point_cloud_object = new PointCloudObject(
      size.width, size.height, kPointCloudIsOrganized, kPointCloudWithNormals,
      kPointCloudWithColors, kPointCloudWithLabels);

  if (pa("-fcpu")) {
    segmentation = new ConfigurableDepthImageSegmenter(
        ConfigurableDepthImageSegmenter::CPU, depth_cam,
        PointCloudCreator::DepthImageMode::DistanceToCamPlane);
  } else if (pa("-fgpu")) {
    segmentation = new ConfigurableDepthImageSegmenter(
        ConfigurableDepthImageSegmenter::GPU, depth_cam,
        PointCloudCreator::DepthImageMode::DistanceToCamPlane);
  } else {
    segmentation = new ConfigurableDepthImageSegmenter(
        ConfigurableDepthImageSegmenter::BEST, depth_cam,
        PointCloudCreator::DepthImageMode::DistanceToCamPlane);
  }
  // TODO(ff): Make these ROS params and generate a dynamic reconfigure.
  segmentation->setPropertyValue("general.enable segmentation", true);
  segmentation->setPropertyValue("general.stabelize segmentation", true);
  segmentation->setPropertyValue("general.depth scaling", 1.0);
  segmentation->setPropertyValue("general.use ROI", false);
  segmentation->setPropertyValue("general.ROI min x", -1200);
  segmentation->setPropertyValue("general.ROI max x", 1200);
  segmentation->setPropertyValue("general.ROI min y", -100);
  segmentation->setPropertyValue("general.ROI max y", 1050);
  segmentation->setPropertyValue("general.ROI min z", 0);
  segmentation->setPropertyValue("general.ROI max z", 1050);

  segmentation->setPropertyValue("pre.enable temporal smoothing", true);
  segmentation->setPropertyValue("pre.temporal smoothing size", 6);
  segmentation->setPropertyValue("pre.temporal smoothing diff", 10);
  segmentation->setPropertyValue("pre.filter", "median5x5");
  segmentation->setPropertyValue("pre.normal range", 1);
  segmentation->setPropertyValue("pre.averaging", true);
  segmentation->setPropertyValue("pre.averaging range", 2);
  segmentation->setPropertyValue("pre.smoothing", "linear");
  segmentation->setPropertyValue("pre.edge threshold", 0.9);
  segmentation->setPropertyValue("pre.edge angle method", "mean");
  segmentation->setPropertyValue("pre.edge neighborhood", 1.2);

  segmentation->setPropertyValue("surfaces.min surface size", 45);
  segmentation->setPropertyValue("surfaces.assignment radius", 7);
  segmentation->setPropertyValue("surfaces.assignment distance", 10.0);

  segmentation->setPropertyValue("cutfree.enable cutfree adjacency feature",
                                 true);
  segmentation->setPropertyValue("cutfree.ransac euclidean distance", 8.0);
  segmentation->setPropertyValue("cutfree.ransac passes", 20);
  segmentation->setPropertyValue("cutfree.ransac tolerance", 30);
  segmentation->setPropertyValue("cutfree.min angle", 30.0);

  segmentation->setPropertyValue("coplanarity.enable coplanarity feature",
                                 true);
  segmentation->setPropertyValue("coplanarity.max angle", 30.0);
  segmentation->setPropertyValue("coplanarity.distance tolerance", 1.5);
  segmentation->setPropertyValue("coplanarity.outlier tolerance", 5.0);
  segmentation->setPropertyValue("coplanarity.num triangles", 20);
  segmentation->setPropertyValue("coplanarity.num scanlines", 9);

  segmentation->setPropertyValue("curvature.enable curvature feature", true);
  segmentation->setPropertyValue("curvature.histogram similarity", 0.5);
  segmentation->setPropertyValue("curvature.enable open objects", true);
  segmentation->setPropertyValue("curvature.max distance", 10);
  segmentation->setPropertyValue("curvature.enable occluded objects", true);
  segmentation->setPropertyValue("curvature.max error", 10.0);
  segmentation->setPropertyValue("curvature.ransac passes", 20);
  segmentation->setPropertyValue("curvature.distance tolerance", 3.0);
  segmentation->setPropertyValue("curvature.outlier tolerance", 5.0);

  segmentation->setPropertyValue("remaining.enable remaining points feature",
                                 true);
  segmentation->setPropertyValue("remaining.min size", 10);
  segmentation->setPropertyValue("remaining.euclidean distance", 10.0);
  segmentation->setPropertyValue("remaining.radius", 0);
  segmentation->setPropertyValue("remaining.assign euclidean distance", 10.0);
  segmentation->setPropertyValue("remaining.support tolerance", 9);

  segmentation->setPropertyValue("graphcut.threshold", 0.5);

  GUI controls = HBox().minSize(12, 12);

  controls << (VBox() << Button("reset view").handle("resetView")
                      << Fps(10).handle("fps")
                      << Prop("segmentation").minSize(10, 8));

  gui << (VBox() << Draw3D().handle("hdepth").minSize(10, 8)
                 << Draw3D().handle("hcolor").minSize(10, 8))
      << (VBox() << Draw3D().handle("hedge").minSize(10, 8)
                 << Draw3D().handle("hnormal").minSize(10, 8))
      << (HSplit() << Draw3D().handle("draw3D").minSize(40, 30) << controls)
      << Show();

  // Depth camera
  scene.addCamera(depth_cam);

  // View camera
  scene.addCamera(depth_cam);

  if (pa("-d")) {
    scene.setDrawCoordinateFrameEnabled(true);
  } else {
    scene.setDrawCoordinateFrameEnabled(false);
  }

  scene.setDrawCamerasEnabled(true);

  scene.addObject(point_cloud_object);
  scene.setBounds(1000);

  DrawHandle3D draw_handle_3d = gui["draw3D"];

  mouse = new AdaptedSceneMouseHandler(scene.getMouseHandler(VIEW_CAM));
  draw_handle_3d->install(mouse);

  scene.setLightingEnabled(false);
  point_cloud_object->setPointSize(3);

  initialized = true;
  ROS_INFO("initialized");
}

void run() {
  if (!depth_ready || !rgb_ready || !initialized) {
    ros::spinOnce();
    return;
  }
  depth_ready = false;
  rgb_ready = false;
  ButtonHandle resetView = gui["resetView"];
  DrawHandle3D hdepth = gui["hdepth"];
  DrawHandle3D hcolor = gui["hcolor"];
  DrawHandle3D hedge = gui["hedge"];
  DrawHandle3D hnormal = gui["hnormal"];

  // reset camera view
  if (resetView.wasTriggered()) {
    scene.getCamera(VIEW_CAM) = scene.getCamera(KINECT_CAM);
  }

  // create heatmap
  static ImgBase* heatmap_image = 0;
  pseudo_color_converter->apply(depth_image, &heatmap_image);

  // segment
  segmentation->apply(*depth_image->as32f(), *point_cloud_object);

  std::vector<std::vector<int>> surfaces = segmentation->getSurfaces();
  std::vector<std::vector<int>> segmentation_segments =
      segmentation->getSegments();
  std::vector<std::vector<int>> segment_points;
  for (std::vector<int> surface_indices : segmentation_segments) {
    std::vector<int> point_ids;
    for (int surface_index : surface_indices) {
      for (int point_id : surfaces[surface_index]) {
        point_ids.push_back(point_id);
      }
    }
    segment_points.push_back(point_ids);
  }
  ros::Time ros_now = ros::Time::now();
  std::vector<sensor_msgs::PointCloud2> segments;
  uint64_t index = 0;
  for (std::vector<int> labeled_points : segment_points) {
    if (labeled_points.size() < kMinLabeledPointSize) {
      break;
    }
    bool forget_segment = false;
    pcl::PointCloud<PointType>::Ptr segmented_point_cloud_pcl(
        new pcl::PointCloud<PointType>);
    for (size_t point_id : labeled_points) {
      icl::math::FixedColVector<float, 3> xyz =
          point_cloud_object->selectXYZ()[point_id];
      icl::math::FixedColVector<float, 4> rgba =
          point_cloud_object->selectRGBA32f()[point_id];
      PointType point_pcl;
      point_pcl.x = xyz[1] / 1000.0f;
      point_pcl.y = xyz[0] / 1000.0f;
      point_pcl.z = -xyz[2] / 1000.0f;

      point_pcl.r = (int)(rgba[0] * 256);
      point_pcl.g = (int)(rgba[1] * 256);
      point_pcl.b = (int)(rgba[2] * 256);
      // TODO(ff): Improve this, for now, just skipping segments that contain
      // points too close to the camera and too far away.
      if (std::abs(point_pcl.z) <= kMinZDistance ||
          std::abs(point_pcl.z) > kMaxZDistance) {
        continue;
      }
      segmented_point_cloud_pcl->push_back(point_pcl);
    }
    // Throw away segments that have a too small size.
    if (segmented_point_cloud_pcl->size() <= kMinSegmentSize) {
      continue;
    }
    // Throw away segments that are too flat or thin, as they will merge with
    // all flat/thin segments.
    pcl::PCA<PointType> pca(*segmented_point_cloud_pcl);
    Eigen::VectorXf eigen_values = pca.getEigenValues();
    if (filter_thin_elements &&
        eigen_values.minCoeff() <
            kPcaMaxEigenValueScale * eigen_values.maxCoeff()) {
      continue;
    }

    if (write_segment_files) {
      pcl::io::savePLYFileASCII(std::to_string(ros_now.toNSec()) + "_segment_" +
                                    std::to_string(index) + ".ply",
                                *segmented_point_cloud_pcl);
    }
    sensor_msgs::PointCloud2 pcl2_msg;
    pcl::toROSMsg(*segmented_point_cloud_pcl, pcl2_msg);
    pcl2_msg.header.stamp = ros_now;
    pcl2_msg.header.frame_id = "/camera_depth_optical_frame";
    segments.push_back(pcl2_msg);
    ++index;
  }
  std_msgs::Int64 segment_size_msg;
  segment_size_msg.data = segments.size();
  segment_size_pub.publish(segment_size_msg);

  for (sensor_msgs::PointCloud2 segment : segments) {
    point_cloud2_pub.publish(segment);
  }

  Img8u edge_image = segmentation->getEdgeImage();
  Img8u normal_image = segmentation->getNormalImage();

  // display
  hdepth = heatmap_image;
  hcolor = rgb_image;
  hedge = &edge_image;
  hnormal = &normal_image;

  gui["fps"].render();
  hdepth.render();
  hcolor.render();
  hedge.render();
  hnormal.render();

  gui["draw3D"].link(scene.getGLCallback(VIEW_CAM));
  gui["draw3D"].render();
  ros::spinOnce();
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "depth_segmentation_node");
  ros::NodeHandle node_handle("~");
  ros::Subscriber rgb_info_sub;
  ros::Subscriber depth_info_sub;
  ros::Subscriber rgb_image_sub;
  ros::Subscriber depth_image_sub;

  std::string rgb_camera_info_topic;
  std::string depth_camera_info_topic;
  std::string rgb_image_topic;
  std::string depth_image_topic;

  node_handle.param<std::string>("rgb_camera_info_topic", rgb_camera_info_topic,
                                 "/camera/rgb/camera_info");
  node_handle.param<std::string>("depth_camera_info_topic",
                                 depth_camera_info_topic,
                                 "/camera/depth/camera_info");
  node_handle.param<std::string>("rgb_image_topic", rgb_image_topic,
                                 "/camera/rgb/image_raw");
  node_handle.param<std::string>("depth_image_topic", depth_image_topic,
                                 "/camera/depth/image_raw");

  depth_info_sub = node_handle.subscribe(depth_camera_info_topic, 1,
                                         depthCameraInfoCallback);
  rgb_info_sub =
      node_handle.subscribe(rgb_camera_info_topic, 1, rgbCameraInfoCallback);

  depth_image_sub =
      node_handle.subscribe(depth_image_topic, 1, depthImageCallback);
  rgb_image_sub = node_handle.subscribe(rgb_image_topic, 1, rgbImageCallback);
  point_cloud2_pub =
      node_handle.advertise<sensor_msgs::PointCloud2>("object_segment", 1000);
  segment_size_pub =
      node_handle.advertise<std_msgs::Int64>("segment_size", 1000);

  return ICLApp(argc, argv,
                "-size|-s(Size=VGA) -depth-cam|-d(file) -fcpu|force-cpu "
                "-fgpu|force-gpu)",
                init, run)
      .exec();
}


#define BOOST_TEST_MODULE scfixed
#define BOOST_TEST_DYN_LINK

//#define DONT_USE_BOOST
#ifndef DONT_USE_BOOST
#include <boost/test/included/unit_test.hpp>
#else
#define BOOST_CHECK_EQUAL(a,b) ((a) == (b));
#define BOOST_AUTO_TEST_CASE(x) int sc_main(int argc, char* argv[])
#endif
//
#define CHECK_EQ(a,b) BOOST_CHECK_EQUAL(a,b)

#ifdef CHECK_TYPES
#define LOOP_ITERS 1
#define SHIFT_ITERS 1
#define CHECK_DOUBLE(a,b) TYPE_NAME(a); TYPE_NAME(b);
#elif CHECK_VALUES
#define LOOP_ITERS 32
#define SHIFT_ITERS 5
#define CHECK_DOUBLE(a,b) TYPE_SHORT(b); std::cout << (b).to_double() << "\n";
#else
#define LOOP_ITERS 32
#define SHIFT_ITERS 5
#define CHECK_DOUBLE(a,b) BOOST_CHECK_EQUAL((a).to_double(), (b).to_double());
#endif

#include "testData.h"
#include "get_type_info.h"
int status;

#ifndef DONT_USE_BOOST
// Keep SystemC compile happy!
int sc_main(int argc, char* argv[]) { return 0;}
#endif

using namespace std;

BOOST_AUTO_TEST_CASE(lr_shifts){

  testData Data;

  for(int i=0; i<LOOP_ITERS; i++) {
	Data.inc();
	Data.inc2();
	for(int j=0; j<SHIFT_ITERS; j++) {
#ifdef CHECK_VALUES
	  std::cout << "ref_s5_3 = " << Data.ref_s5_3 << " "
				<< "new_s5_3 = " << Data.new_s5_3 << " " << j << " ";
#endif
	  // with ints
	  Data.ref_int = j;
	  Data.new_int = j;
	  CHECK_DOUBLE((Data.ref_s5_3<<Data.ref_int), (Data.new_s5_3<<Data.new_int));
	  CHECK_DOUBLE((Data.ref_s5_3>>Data.ref_int), (Data.new_s5_3>>Data.new_int));
	  //
	  // Don't expect to match!!!
#ifdef CHECK_VALUES
	  std::cout << "ref_u5_3 = " << Data.ref_u5_3 << " "
				<< "new_u5_3 = " << Data.new_u5_3 << " " << j << "  ";
#endif

	  CHECK_DOUBLE((Data.ref_u5_3<<Data.ref_int), (Data.new_u5_3<<Data.new_int));
	  CHECK_DOUBLE((Data.ref_u5_2>>Data.ref_int), (Data.new_u5_2>>Data.new_int));
	}

	// with other s/u fixed
	// Don't expect to match!!!
#ifdef CHECK_VALUES
	  std::cout << "ref_s5_3 = " << Data.ref_s5_3 << " "
				<< "new_s5_3 = " << Data.new_s5_3 << " ";
	  std::cout << "ref_u5_3 = " << Data.ref_u5_3 << " "
				<< "new_u5_3 = " << Data.new_u5_3 << " ";
#endif

	CHECK_DOUBLE((Data.ref_s5_3<<Data.ref_u5_2), (Data.new_s5_3<<Data.new_u5_2));
	CHECK_DOUBLE((Data.ref_u5_3<<Data.ref_u5_2), (Data.new_u5_3<<Data.new_u5_2));
	CHECK_DOUBLE((Data.ref_s5_3<<Data.ref_s5_2), (Data.new_s5_3<<Data.new_s5_2));
	CHECK_DOUBLE((Data.ref_u5_3<<Data.ref_s5_2), (Data.new_u5_3<<Data.new_s5_2));

	CHECK_DOUBLE((Data.ref_s5_3>>Data.ref_u5_2), (Data.new_s5_3>>Data.new_u5_2));
	CHECK_DOUBLE((Data.ref_u5_3>>Data.ref_u5_2), (Data.new_u5_3>>Data.new_u5_2));
	CHECK_DOUBLE((Data.ref_s5_3>>Data.ref_s5_2), (Data.new_s5_3>>Data.new_s5_2));
	CHECK_DOUBLE((Data.ref_u5_3>>Data.ref_s5_2), (Data.new_u5_3>>Data.new_s5_2));
  }
#ifdef DONT_USE_BOOST
  return 0;
#endif

}


/*
 * multiplane_segmentation.hpp
 *
 *  Created on: Sep 25, 2013
 *      Author: aitor
 */

#ifndef FAAT_PCL_3D_REC_FRAMEWORK_MULTIPLANE_SEGMENTATION_HPP_
#define FAAT_PCL_3D_REC_FRAMEWORK_MULTIPLANE_SEGMENTATION_HPP_

#include <v4r/common/miscellaneous.h>
#include <v4r/segmentation/multiplane_segmentation.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
//#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/segmentation/organized_multi_plane_segmentation.h>
#include <pcl/segmentation/planar_polygon_fusion.h>
#include <pcl/segmentation/plane_coefficient_comparator.h>
#include <pcl/segmentation/euclidean_plane_coefficient_comparator.h>
#include <pcl/segmentation/rgb_plane_coefficient_comparator.h>
#include <pcl/segmentation/edge_aware_plane_comparator.h>
#include <pcl/features/integral_image_normal.h>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/connected_components.hpp>

template<typename PointT>
void
v4r::MultiPlaneSegmentation<PointT>::segment(bool force_unorganized)
{
  models_.clear();

  if(input_->isOrganized() && !force_unorganized)
  {
    pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
    if(!normals_set_)
    {
        pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
        ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
        ne.setMaxDepthChangeFactor (0.02f);
        ne.setNormalSmoothingSize (20.0f);
        ne.setBorderPolicy (pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE);
        ne.setInputCloud (input_);
        ne.compute (*normal_cloud);
    }
    else
    {
        normal_cloud = normal_cloud_;
    }

    pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
    mps.setMinInliers (min_plane_inliers_);
    mps.setAngularThreshold (0.017453 * 2.f); // 2 degrees
    mps.setDistanceThreshold (0.01); // 1cm
    mps.setMaximumCurvature(0.002);
    mps.setInputNormals (normal_cloud);
    mps.setInputCloud (input_);

    std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
    std::vector<pcl::ModelCoefficients> model_coefficients;
    std::vector<pcl::PointIndices> inlier_indices;
    pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
    std::vector<pcl::PointIndices> label_indices;
    std::vector<pcl::PointIndices> boundary_indices;

    typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp (
                                                                                new pcl::PlaneRefinementComparator<PointT,
                                                                                pcl::Normal, pcl::Label> ());
    ref_comp->setDistanceThreshold (0.01f, false);
    ref_comp->setAngularThreshold (0.017453 * 2.f);
    mps.setRefinementComparator (ref_comp);
    mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
    //mps.segment (model_coefficients, inlier_indices);

    //std::cout << model_coefficients.size() << std::endl;

    if(merge_planes_)
    {
      //sort planes by size
      //check if the first plane can be merged against the others, if yes, define a new plane combining both and add it to the cue

      typedef boost::adjacency_matrix<boost::undirectedS, int> GraphPlane;
      GraphPlane mergeable_planes (model_coefficients.size ());
      for(size_t i=0; i < model_coefficients.size(); i++)
      {

        Eigen::Vector3f plane_i = Eigen::Vector3f (model_coefficients[i].values[0], model_coefficients[i].values[1],
                                                   model_coefficients[i].values[2]);

        plane_i.normalize();
        for(size_t j=(i+1); j < model_coefficients.size(); j++)
        {
          Eigen::Vector3f plane_j = Eigen::Vector3f (model_coefficients[j].values[0], model_coefficients[j].values[1],
                                                     model_coefficients[j].values[2]);

          plane_j.normalize();

          //std::cout << "dot product:" << plane_i.dot(plane_j) << " diff:" << std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) << std::endl;
          if(plane_i.dot(plane_j) > 0.95)
          {
            if(std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) < 0.015)
            {
              boost::add_edge (static_cast<int>(i), static_cast<int>(j), mergeable_planes);
            }
          }
        }
      }

      boost::vector_property_map<int> components (boost::num_vertices (mergeable_planes));
      int n_cc = static_cast<int> (boost::connected_components (mergeable_planes, &components[0]));

      std::vector<int> cc_sizes;
      std::vector< std::vector<int> > cc_to_model_coeff;
      cc_sizes.resize (n_cc, 0);
      cc_to_model_coeff.resize(n_cc);

      for (size_t i = 0; i < model_coefficients.size (); i++)
      {
        cc_sizes[components[i]]++;
        cc_to_model_coeff[components[i]].push_back(i);
      }

      std::vector<pcl::ModelCoefficients> new_model_coefficients;
      std::vector<pcl::PointIndices> new_inlier_indices;

      for(size_t i=0; i < cc_sizes.size(); i++)
      {
        if(cc_sizes[i] < 2)
        {
          new_model_coefficients.push_back(model_coefficients[cc_to_model_coeff[i][0]]);
          new_inlier_indices.push_back(inlier_indices[cc_to_model_coeff[i][0]]);
          continue;
        }

        //std::cout << "going to merge CC:" << cc_sizes[i] << std::endl;
        pcl::ModelCoefficients model_coeff;
        model_coeff.values.resize(4);

        for(size_t k=0; k < 4; k++)
          model_coeff.values[k] = 0.f;

        pcl::PointIndices merged_indices;
        for(size_t j=0; j < cc_to_model_coeff[i].size(); j++)
        {
          for(size_t k=0; k < 4; k++)
            model_coeff.values[k] += model_coefficients[cc_to_model_coeff[i][j]].values[k];

          merged_indices.indices.insert(merged_indices.indices.end(), inlier_indices[cc_to_model_coeff[i][j]].indices.begin(),
                                                                      inlier_indices[cc_to_model_coeff[i][j]].indices.end());
        }

        for(size_t k=0; k < 4; k++)
          model_coeff.values[k] /= static_cast<float>(cc_to_model_coeff[i].size());

        new_model_coefficients.push_back(model_coeff);
        new_inlier_indices.push_back(merged_indices);
      }

      model_coefficients = new_model_coefficients;
      inlier_indices = new_inlier_indices;
    }

    for(size_t i=0; i < model_coefficients.size(); i++)
    {
      PlaneModel<PointT> pm;
      pm.coefficients_ = model_coefficients[i];
      pm.cloud_ = input_;
      pm.inliers_ = inlier_indices[i];

      //recompute coefficients based on distance to camera and normal?
      Eigen::Vector4f centroid;
      pcl::compute3DCentroid(*pm.cloud_, pm.inliers_, centroid);
      Eigen::Vector3f c(centroid[0],centroid[1],centroid[2]);

      Eigen::MatrixXf M_w(inlier_indices[i].indices.size(), 3);

      float sum_w = 0.f;
      for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
      {
          const int &idx = inlier_indices[i].indices[k];
          float d_c = (pm.cloud_->points[idx].getVector3fMap()).norm();
          float w_k = std::max(1.f - std::abs(1.f - d_c), 0.f);
          //w_k = 1.f;
          M_w.row(k) = w_k * (pm.cloud_->points[idx].getVector3fMap() - c);
          sum_w += w_k;
      }

      Eigen::Matrix3f scatter;
      scatter.setZero ();
      scatter = M_w.transpose() * M_w;

      Eigen::JacobiSVD<Eigen::MatrixXf> svd(scatter, Eigen::ComputeFullV);
      //std::cout << svd.matrixV() << std::endl;

      Eigen::Vector3f n = svd.matrixV().col(2);
      //flip normal if required
      if(n.dot(c*-1) < 0)
          n = n * -1.f;

      float d = n.dot(c) * -1.f;
      //std::cout << "normal:" << n << std::endl;
      //std::cout << "d:" << d << std::endl;

      pm.coefficients_.values[0] = n[0];
      pm.coefficients_.values[1] = n[1];
      pm.coefficients_.values[2] = n[2];
      pm.coefficients_.values[3] = d;

      pcl::PointIndices clean_inlier_indices;
      float dist_threshold_ = 0.01f;

      for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
      {
          const int &idx = inlier_indices[i].indices[k];
          Eigen::Vector3f p = pm.cloud_->points[idx].getVector3fMap();
          float val = n.dot(p) + d;

          if(std::abs(val) <= dist_threshold_)
              clean_inlier_indices.indices.push_back( idx );
      }

      pm.inliers_ = clean_inlier_indices;
      models_.push_back(pm);
    }
  }
  else
  {
    // Create the filtering object: downsample the dataset using a leaf size of 1cm
    pcl::VoxelGrid<PointT> vg;
    PointTCloudPtr cloud_filtered (new PointTCloud);
    vg.setInputCloud (input_);
    float leaf_size_ = 0.005f;
    float dist_threshold_ = 0.01f;
    vg.setLeafSize (leaf_size_, leaf_size_, leaf_size_);
    vg.filter (*cloud_filtered);

    std::vector<bool> pixel_has_not_been_labelled( cloud_filtered->points.size(), true );

    // Create the segmentation object for the planar model and set all the parameters
    pcl::SACSegmentation<PointT> seg;
    pcl::ModelCoefficients coefficients;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setMaxIterations (1000);
    seg.setDistanceThreshold (dist_threshold_);

    PointTCloudPtr cloud_filtered_leftover (new PointTCloud (*cloud_filtered));

    while (1)
    {
      // Segment the largest planar component from the remaining cloud
      seg.setInputCloud (cloud_filtered_leftover);
      pcl::PointIndices inliers_in_leftover;
      seg.segment (inliers_in_leftover, coefficients);

      std::cout << "inliers in left over: " << inliers_in_leftover.indices.size() << " of " << cloud_filtered_leftover->points.size() << std::endl;

      if ( (int)inliers_in_leftover.indices.size () < min_plane_inliers_) // Could not estimate a(nother) planar model big enough for the given cloud.
        break;

      // make indices correspond to complete downsampled cloud
      pcl::PointIndices indices_in_original_cloud;
      size_t current_index_in_leftover = 0;
      size_t px=0;
      indices_in_original_cloud.indices.resize(inliers_in_leftover.indices.size());
      for(size_t inl_id=0; inl_id < inliers_in_leftover.indices.size(); inl_id++) {    // assumes indices are sorted in ascending order
          bool found = false;
          do {
              if( pixel_has_not_been_labelled[px] ) {
                  if( current_index_in_leftover == inliers_in_leftover.indices [inl_id] ) {
                      indices_in_original_cloud.indices[ inl_id ] = px;
                      found = true;
                  }
                  current_index_in_leftover++;
              }
              px++;
          } while( !found );
      }

      for(size_t i=0; i<indices_in_original_cloud.indices.size(); i++)
          pixel_has_not_been_labelled[ indices_in_original_cloud.indices[i] ] = false;

      //save coefficients
        PlaneModel<PointT> pm;
        pm.coefficients_ = coefficients;
        pm.cloud_ = cloud_filtered;
        pm.inliers_ = indices_in_original_cloud;
        models_.push_back(pm);

        pcl::copyPointCloud(*cloud_filtered, pixel_has_not_been_labelled, *cloud_filtered_leftover);
    }

    std::cout << "Number of planes found:" << models_.size() << "organized:" << static_cast<int>(input_->isOrganized() && !force_unorganized) << std::endl;
  }
}

#endif /* FAAT_PCL_3D_REC_FRAMEWORK_MULTIPLANE_SEGMENTATION_HPP_ */


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

#ifndef fiber_surface_div_3d_functor_hpp
#define fiber_surface_div_3d_functor_hpp

#include "../common/common.hpp"

#include "basis_data.hpp"
#include "geometrical_data.hpp"
#include "collection_of_3d_arrays.hpp"
#include "shape_transformation_functor_wrappers.hpp"

#include <boost/array.hpp>

namespace Fiber {

template <typename CoordinateType_> class SurfaceDiv3dElementaryFunctor {
public:
  typedef CoordinateType_ CoordinateType;

  int argumentDimension() const { return 2; }
  int resultDimension() const { return 1; }

  void addDependencies(size_t &basisDeps, size_t &geomDeps) const {
    basisDeps |= DERIVATIVES;
    geomDeps |= INTEGRATION_ELEMENTS;
  }

  template <typename ValueType>
  void evaluate(const ConstBasisDataSlice<ValueType> &basisData,
                const ConstGeometricalDataSlice<CoordinateType> &geomData,
                _1dSliceOf3dArray<ValueType> &result) const {
    assert(basisData.componentCount() == 2);

    result(0) = (basisData.derivatives(0, 0) + basisData.derivatives(1, 1)) /
                geomData.integrationElement();
  }
};

// Note: in C++11 we'll be able to make a "template typedef", or more precisely
// a using declaration, instead of this spurious inheritance
/** \ingroup functors
 *  \brief Functor calculating the surface div of a scalar field in 3D. */
template <typename CoordinateType_>
class SurfaceDiv3dFunctor
    : public ElementaryShapeTransformationFunctorWrapper<
          SurfaceDiv3dElementaryFunctor<CoordinateType_>> {
public:
  typedef CoordinateType_ CoordinateType;
};

} // namespace Fiber

#endif


/*  $Id:  $
 *
 *  Copyright 2010-2011 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef CUTSIM_H
#define CUTSIM_H

#include <QObject>

#include <string>
#include <iostream>
#include <cmath>
#include <vector>
#include <ctime>

#include <boost/bind.hpp>
#include <boost/timer.hpp>

#include <opencamlib/point.hpp>
#include <opencamlib/triangle.hpp>
#include <opencamlib/numeric.hpp>
#include <opencamlib/octree.hpp>
#include <opencamlib/octnode.hpp>

#include <opencamlib/volume.hpp>
#include <opencamlib/marching_cubes.hpp>
#include <opencamlib/gldata.hpp>

/// a Cutsim stores an Octree stock model, uses an iso-surface extraction
/// algorithm to generate surface triangles, and communicates with
/// the corresponding GLData surface which is used for rendering
class Cutsim : public QObject {
    Q_OBJECT

public:
    // std::cout << ocl::revision() << "\n";
    // std::cout << " Experimental C++ cutting simulation.\n";
    // Octree(root_scale, max_depth, cp)
    Cutsim () {
        ocl::Point octree_center(0,0,0);
        unsigned int max_depth = 7;
        tree = new ocl::Octree(10.0, max_depth, octree_center );
        std::cout << " tree before init: " << tree->str() << "\n";
        tree->init(2u);
        tree->debug=false;
        std::cout << " tree after init: " << tree->str() << "\n";

        ocl::SphereOCTVolume stock_sphere;
        stock_sphere.radius = 7;
        stock_sphere.center = ocl::Point(0,0,0);
        stock_sphere.calcBB();
        stock_sphere.invert = true;

        tree->diff_negative( &stock_sphere );

        std::cout << " tree after pane-cut: " << tree->str() << "\n";

        /*
        ocl::SphereOCTVolume s;
        s.radius = 2;
        s.center = ocl::Point(4,4,4);
        s.calcBB();
        std::cout << " before diff: " << tree->str() << "\n";
        tree->diff_negative( &s );
        std::cout << " AFTER diff: " << tree->str() << "\n";
        */

        mc = new ocl::MarchingCubes();
        tree->setIsoSurf(mc);
        //tree->debug=true;
    }
    void setGLData(ocl::GLData* gldata) {
        // this is the GLData that corresponds to the tree
        g = gldata;
        g->setTriangles(); // mc: triangles, dual_contour: quads
        g->setPosition(0,0,0); // position offset (?used)
        g->setUsageDynamicDraw();
        tree->setGLData(g);
    }
    void updateGL() {
        // traverse the octree and update the GLData correspondingly
        tree->updateGL();
    }

public slots:
    void cut() { // demo slot of doing a cutting operation on the tree with a volume.
        std::cout << " cut! called \n";
        ocl::SphereOCTVolume s;
        s.radius = 2;
        s.center = ocl::Point(4,4,4);
        s.calcBB();
        //std::cout << " before diff: " << tree->str() << "\n";
        std::clock_t start, stop;
        boost::timer tim;

        tim.restart();
        start = std::clock();
        tree->diff_negative( &s );
        stop = std::clock();

        std::cout << " diff(): " << ( ( stop - start ) / (double)CLOCKS_PER_SEC ) <<'\n';
        std::cout << " diff(): " << tim.elapsed() <<'\n';

        tim.restart();
        start = std::clock();
        //g->updateVBO();
        updateGL();
        stop = std::clock();

        std::cout << " updateGL(): " << ( ( stop - start ) / (double)CLOCKS_PER_SEC ) <<'\n';
        std::cout << " updateGL(): " << tim.elapsed() <<'\n';
        //std::cout << " AFTER diff: " << tree->str() << "\n";

        tim.restart();
        start = std::clock();
        g->updateVBO();
        stop = std::clock();
        std::cout << " updateVBO(): " << ( ( stop - start ) / (double)CLOCKS_PER_SEC ) <<'\n';
        std::cout << " updateVBO(): " << tim.elapsed() <<'\n';
    }

private:
    ocl::MarchingCubes* mc; // simplest isosurface-extraction algorithm
    //std::vector<ocl::Triangle> tris; // do we need to store all tris here?? no!
    ocl::Octree* tree; // this is the stock model
    ocl::GLData* g; // this is the graphics object drawn on the screen
};

#endif


#include <stdio.h>
#include <boost/shared_ptr.hpp>
#include <list>
#include <classad/classad.h>
#include <classad/classad_distribution.h>
#include "include/attributes.h"

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

    ClassAd ad;
    ad.InsertAttr(ATTR_NEED_VCPU, 1);
    ad.InsertAttr(ATTR_NEED_MEMORY, 1024);
    ClassAdParser parser;
    printf("%s\n", EXP_TASK_RANK.c_str());
    ExprTree* expr = parser.ParseExpression(EXP_TASK_RANK);
    ad.Insert(ATTR_TASK_RANK, expr);
    ExprTree* re_expr = parser.ParseExpression(EXP_TASK_REQUIREMENT);
    ad.Insert(ATTR_TASK_REQUIREMENT, re_expr);

    ClassAd c1;
    c1.InsertAttr(ATTR_ALL_VCPU, 4);
    c1.InsertAttr(ATTR_AVAIL_VCPU, 4);
    c1.InsertAttr(ATTR_ALL_MEMORY, 6 * 1024);
    c1.InsertAttr(ATTR_AVAIL_MEMORY, 6 * 1024);

    ad.alternateScope = &c1;
    bool is_match = false;
    ad.EvaluateAttrBool(ATTR_TASK_REQUIREMENT, is_match);
    if(is_match == true) {
        double d_value;
        ad.EvaluateAttrNumber(ATTR_TASK_RANK, d_value);
        printf("task rank:%f\n", d_value);
    } else {
        printf("nima\n");
    }

    return 0;
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <random>
#include <chrono>
#include <functional>
#include <cctype>
#include <locale>
#include <algorithm>
#include <regex>
#include <iterator>
#include <vector>
#include <thread>
#include <atomic>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include "perceptron.h"
#include "segmentor.h"
#include "featurizer.h"
#include "naivebayes.h"
#include "util.h"

using namespace std;

const string EXP_STR = ".thread_test.";

void multi_fit(Segmentor* seg, vector<string> sentences, vector<vector<int> > labels, chrono::milliseconds time_resource, chrono::system_clock::time_point genesis) {
    for (int i = 0; i < 1000000; ++i) {
        auto showtime = std::chrono::system_clock::now();
        auto time_left = time_resource - chrono::duration_cast<chrono::milliseconds>(showtime - genesis);

        unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
        shuffle(sentences.begin(), sentences.end(), std::default_random_engine(seed));
        shuffle(labels.begin(), labels.end(), std::default_random_engine(seed));

        // this loop will terminate if we spent more than given time resource
        bool no_more_time = seg->fit(sentences, labels, time_left);

        if (no_more_time) {
            seg->model->average();
            break;
        }
    }
}

int main() {
    auto genesis = std::chrono::system_clock::now();

    NBUnigramBOW* nb = new NBUnigramBOW();

    int num_threads = 8;
    vector<Segmentor*> segment_vector;
    for (int i = 0; i < num_threads; ++i) {
        AveragedPerceptron* perceptron = new AveragedPerceptron();
        Featurizer* feat = new Featurizer();
        Segmentor* seg = new Segmentor(perceptron, feat);
        map<string, int> index_table;
        feat->index_table = index_table;

        segment_vector.push_back(seg);
    }

    vector<string> sentences;
    vector<vector<int> > labels;
    vector<vector<string> > tokens_list;

    ifstream training_data ("../data/train.txt");
    string line;
    stringstream ss;

    if (training_data.is_open()) {
        while (getline(training_data, line)) {
            transform(line.begin(), line.end(), line.begin(), ::tolower);
            replace_if(line.begin(), line.end(), std::not1(std::ptr_fun(ofinterest)), ' ');
            line.erase(remove_if(line.begin(), line.end(), isapos), line.end());
            line = trim(line);

            if (line.empty()) {
                continue;
            }

            vector<int> label;
            vector<string> tokens;

            vector<string> tokenized_strings = split(line, ' ');
            for (auto t: tokenized_strings) {
                if (t.empty()) {
                    continue;
                }
                vector<int> tmp(t.size()-1, -1);
                label.insert(label.end(), tmp.begin(), tmp.end());
                label.insert(label.end(), 1);

                tokens.push_back(t);
            }

            line.erase(remove_if(line.begin(), line.end(), ::isspace), line.end());

            sentences.push_back(line);
            labels.push_back(label);
            tokens_list.push_back(tokens);
        }
    }

    // train Naive Bayes model
    for (auto tokens: tokens_list) {
        nb->fit(tokens);
    }

    cout << "NB unigram count: " << nb->total << endl;

    tokens_list.clear();
    vector<string> query_sentences;
    vector<vector<int> > query_labels;

    ifstream training_query ("../data/train.qry");
    if (training_query.is_open()) {
        while (getline(training_query, line)) {
            transform(line.begin(), line.end(), line.begin(), ::tolower);
            replace_if(line.begin(), line.end(), std::not1(std::ptr_fun(ofinterest)), ' ');
            line.erase(remove_if(line.begin(), line.end(), isapos), line.end());
            line = trim(line);

            if (line.empty()) {
                continue;
            }

            vector<int> label;
            vector<string> tokens;

            vector<string> tokenized_strings = split(line, ' ');
            int counter = 0;
            for (auto t: tokenized_strings) {
                counter++;
                if (counter == 1) {
                    continue;
                }

                if (t.empty()) {
                    continue;
                }
                vector<int> tmp(t.size()-1, -1);
                label.insert(label.end(), tmp.begin(), tmp.end());
                label.insert(label.end(), 1);

                tokens.push_back(t);
            }
            line.erase(remove_if(line.begin(), line.end(), ::isspace), line.end());

            query_sentences.push_back(line);
            query_labels.push_back(label);
            tokens_list.push_back(tokens);
        }
    }

    for (auto tokens: tokens_list) {
        nb->fit(tokens);
    }

    cout << "NB unigram count: " << nb->total << endl;

    ss.str("");
    ss.clear();

    ss << "models/nb";
    ss << EXP_STR;
    ss << "dat";

    ofstream os(ss.str(), ios::binary);
    boost::archive::binary_oarchive nb_ar(os);
    nb_ar << nb;

    ss.str("");
    ss.clear();

    // Averaged perceptron training
    std::random_device rd;

    /* chrono::milliseconds time_resource (1000 * 60 * 10 - 10000);  // 10 min - 10 sec */

    vector<thread> threads;

    chrono::milliseconds time_resource (1000 * 60 * 10 - 5000);  // 10 min - 5 sec = 9 min 55 sec
    /* chrono::milliseconds time_resource (1000 * 15 - 10000);  // 15 sec - 10 sec = 5 sec */
    for (int i = 0; i < 8; ++i) {
        Segmentor* seg = segment_vector[i];
        threads.push_back(thread(multi_fit, seg, sentences, labels, time_resource, genesis));
    }

    for (auto& t: threads) {
        t.join();
    }
//
//     vector<thread> query_threads;
//
//     auto mini_genesis = std::chrono::system_clock::now();
//     chrono::milliseconds train_query_time_resource (1000 * 30 - 5000);  // 30 sec - 5 sec
//
//     for (int i = 0; i < 8; ++i) {
//         Segmentor* seg = segment_vector[i];
//         query_threads.push_back(thread(multi_fit, seg, query_sentences, query_labels, train_query_time_resource, mini_genesis));
//     }
//
//     for (auto& t: query_threads) {
//         t.join();
//     }

    auto train_done = std::chrono::system_clock::now();

    cout << "Training done in " << chrono::duration_cast<chrono::milliseconds>(train_done - genesis).count() << "ms" <<  endl;

    for (auto seg: segment_vector) {
        cout << "# features : " << seg->feat->num << endl;
    }

    cout << "Dumping model..." << endl;

    for (int i = 0; i < num_threads; ++i) {
        Segmentor* seg = segment_vector[i];

        ss << "models/feature_index";
        ss << EXP_STR;
        ss << i;
        ss << ".dat";
        ofstream ofeature(ss.str(), ios::binary);
        boost::archive::binary_oarchive feature_ar(ofeature);
        feature_ar << seg->feat;

        ss.str("");
        ss.clear();

        ss << "models/perceptron";
        ss << EXP_STR;
        ss << i;
        ss << ".dat";

        ofstream operceptron(ss.str(), ios::binary);
        boost::archive::binary_oarchive perceptron_ar(operceptron);
        perceptron_ar << seg->model;

        ss.str("");
        ss.clear();
    }

    return 0;
};


#include "ZeroOrderAssembler.hpp"

#include <vector>

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

#include "Assembler.hpp"
#include "DOFVector.hpp"
#include "FiniteElemSpace.hpp"
#include "Operator.hpp"
#include "QPsiPhi.hpp"
#include "Quadrature.hpp"

using namespace std;

namespace AMDiS
{
  ThreadPrivate<vector<SubAssembler*>> ZeroOrderAssembler::optimizedSubAssemblers;

  ThreadPrivate<vector<SubAssembler*>> ZeroOrderAssembler::standardSubAssemblers;

  ZeroOrderAssembler::ZeroOrderAssembler(Operator* op,
                                         Assembler* assembler,
                                         Quadrature* quad,
                                         bool optimized)
    : SubAssembler(op, assembler, quad, 0, optimized)
  {}

  ZeroOrderAssembler* ZeroOrderAssembler::getSubAssembler(Operator* op,
      Assembler* assembler,
      Quadrature* quad,
      bool optimized)
  {
    // check if an assembler is needed at all
    if (!op->zeroOrderTerms())
      return NULL;

    auto& subAssemblers = optimized ? optimizedSubAssemblers.get()
				    : standardSubAssemblers.get();

    vector<OperatorTerm*> opTerms = op->getZeroOrder();
    sort(opTerms.begin(), opTerms.end());

    // check if a new assembler is needed
    if (quad)
    {
      for (SubAssembler* subAssembler : subAssemblers)
      {
        vector<OperatorTerm*> assTerms = *(subAssembler->getTerms());
        sort(assTerms.begin(), assTerms.end());

        if (opTerms == assTerms && subAssembler->getQuadrature() == quad)
          return dynamic_cast<ZeroOrderAssembler*>(subAssembler);
      }
    }

    // check if all terms are pw_const
    bool pwConst = std::all_of(
      begin(op->getZeroOrder()), end(op->getZeroOrder()),
      [](OperatorTerm* term)
      {
	return term->isPWConst();
      });

    // create new assembler
    ZeroOrderAssembler* newAssembler;
    if (!optimized)
      newAssembler = new StandardZOA(op, assembler, quad);
    else
    {
      if (pwConst)
        newAssembler = new PrecalcZOA(op, assembler, quad);
      else
        newAssembler = new FastQuadZOA(op, assembler, quad);
    }

    subAssemblers.push_back(newAssembler);

    return newAssembler;
  }

  StandardZOA::StandardZOA(Operator* op, Assembler* assembler, Quadrature* quad)
    : ZeroOrderAssembler(op, assembler, quad, false)
  {
    name = "standard zero order assembler";
  }

  void StandardZOA::calculateElementMatrixImpl(ElInfo const* elInfo, ElementMatrix& mat)
  {
    BasisFunction const* psi = rowFeSpace->getBasisFcts();
    BasisFunction const* phi = colFeSpace->getBasisFcts();

    int nPoints = quadrature->getNumPoints();
    DenseVector<double> c(nPoints, 0.0);
    DenseVector<double> phival(nCol);

    for (auto& term : terms)
      (static_cast<ZeroOrderTerm*>(term))->getC(elInfo, nPoints, c);

    // TODO: use the case that psi == phi and phival == psival
    if (symmetric)
    {
      TEST_EXIT_DBG(nCol == nRow)("nCol != nRow, but symmetric assembling!\n");

      for (int iq = 0; iq < nPoints; iq++)
      {
        c[iq] *= elInfo->getDet();

        // calculate phi at QPs only once!
        for (int i = 0; i < nCol; i++)
          phival[i] = (*(phi->getPhi(i)))(quadrature->getLambda(iq));

        for (int i = 0; i < nRow; i++)
        {
          double psival = (*(psi->getPhi(i)))(quadrature->getLambda(iq));
          mat[i][i] += quadrature->getWeight(iq) * c[iq] * psival * phival[i];
          for (int j = i + 1; j < nCol; j++)
          {
            double val = quadrature->getWeight(iq) * c[iq] * psival * phival[j];
            mat[i][j] += val;
            mat[j][i] += val;
          }
        }
      }
    }
    else          //  non symmetric assembling
    {
      for (int iq = 0; iq < nPoints; iq++)
      {
        c[iq] *= elInfo->getDet();

        // calculate phi at QPs only once!
        for (int i = 0; i < nCol; i++)
          phival[i] = (*(phi->getPhi(i)))(quadrature->getLambda(iq));

        for (int i = 0; i < nRow; i++)
        {
          double psival = (*(psi->getPhi(i)))(quadrature->getLambda(iq));
          for (int j = 0; j < nCol; j++)
            mat[i][j] += quadrature->getWeight(iq) * c[iq] * psival * phival[j];
        }
      }
    }
  }

  // TODO: add combination of elementMatrix and elementVector assembling
  void StandardZOA::calculateElementVectorImpl(ElInfo const* elInfo, DenseVector<double>& vec)
  {
    int nPoints = quadrature->getNumPoints();
    DenseVector<double> c(nPoints, 0.0);

    for (auto& term : terms)
      (static_cast<ZeroOrderTerm*>(term))->getC(elInfo, nPoints, c);

    for (int iq = 0; iq < nPoints; iq++)
    {
      c[iq] *= elInfo->getDet();

      for (int i = 0; i < nRow; i++)
      {
        double psi = (*(rowFeSpace->getBasisFcts()->getPhi(i)))
                     (quadrature->getLambda(iq));
        vec[i] += quadrature->getWeight(iq) * c[iq] * psi;
      }
    }
  }

  FastQuadZOA::FastQuadZOA(Operator* op, Assembler* assembler, Quadrature* quad)
    : ZeroOrderAssembler(op, assembler, quad, true)
  {
    name = "fast quadrature zero order assembler";
  }

  void FastQuadZOA::calculateElementMatrixImpl(ElInfo const* elInfo, ElementMatrix& mat)
  {
    int nPoints = quadrature->getNumPoints();

    if (firstCall)
    {
      BasisFunction const* basFcts = rowFeSpace->getBasisFcts();
      psiFast = updateFastQuadrature(psiFast, basFcts, INIT_PHI);
      basFcts = colFeSpace->getBasisFcts();
      phiFast = updateFastQuadrature(phiFast, basFcts, INIT_PHI);
      firstCall = false;
    }

    if (num_rows(c) < static_cast<size_t>(nPoints))
      c.change_dim(nPoints);
    set_to_zero(c);

    for (auto& term : terms)
      (static_cast<ZeroOrderTerm*>(term))->getC(elInfo, nPoints, c);

    DenseMatrix<double> const& psi = psiFast->getPhi();
    DenseMatrix<double> const& phi = phiFast->getPhi();

    if (symmetric)
    {
      TEST_EXIT_DBG(nCol == nRow)("nCol != nRow, but symmetric assembling!\n");

      for (int iq = 0; iq < nPoints; iq++)
      {
        c[iq] *= elInfo->getDet() * quadrature->getWeight(iq);

        for (int i = 0; i < nRow; i++)
        {
          mat[i][i] += c[iq] * psi[iq][i] * phi[iq][i];
          for (int j = i + 1; j < nCol; j++)
          {
            double val = c[iq] * psi[iq][i] * phi[iq][j];
            mat[i][j] += val;
            mat[j][i] += val;
          }
        }
      }
    }
    else          /*  non symmetric assembling   */
    {
      for (int iq = 0; iq < nPoints; iq++)
        c[iq] *= elInfo->getDet() * quadrature->getWeight(iq);

      for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
        {
          double v = 0.0;
          for (int iq = 0; iq < nPoints; iq++)
            v += c[iq] * psi[iq][i] * phi[iq][j];
          mat[i][j] += v;
        }
    }
  }

  void FastQuadZOA::calculateElementVectorImpl(ElInfo const* elInfo, DenseVector<double>& vec)
  {
    int nPoints = quadrature->getNumPoints();

    if (firstCall)
    {
      BasisFunction const* basFcts = rowFeSpace->getBasisFcts();
      psiFast = updateFastQuadrature(psiFast, basFcts, INIT_PHI);
      basFcts = colFeSpace->getBasisFcts();
      phiFast = updateFastQuadrature(phiFast, basFcts, INIT_PHI);
      firstCall = false;
    }

    DenseVector<double> c(nPoints, 0.0);
    for (auto& term : terms)
      (static_cast<ZeroOrderTerm*>(term))->getC(elInfo, nPoints, c);

    DenseMatrix<double> const& psi = psiFast->getPhi();

    for (int iq = 0; iq < nPoints; iq++)
    {
      c[iq] *= elInfo->getDet();

      for (int i = 0; i < nRow; i++)
        vec[i] += quadrature->getWeight(iq) * c[iq] * psi[iq][i];
    }
  }

  PrecalcZOA::PrecalcZOA(Operator* op, Assembler* assembler, Quadrature* quad)
    : ZeroOrderAssembler(op, assembler, quad, true)
  {
    name = "precalculated zero order assembler";
  }

  void PrecalcZOA::calculateElementMatrixImpl(ElInfo const* elInfo, ElementMatrix& mat)
  {
    if (firstCall)
    {
      q00 = Q00PsiPhi::provideQ00PsiPhi(rowFeSpace->getBasisFcts(),
                                        colFeSpace->getBasisFcts(),
                                        quadrature);
      q0 = Q0Psi::provideQ0Psi(rowFeSpace->getBasisFcts(), quadrature);
      firstCall = false;
    }

    DenseVector<double> c(1, 0.0);
    for (auto& term : terms)
      (static_cast<ZeroOrderTerm*>(term))->getC(elInfo, 1, c);

    c[0] *= elInfo->getDet();

    if (symmetric)
    {
      for (int i = 0; i < nRow; i++)
      {
        mat[i][i] += c[0] * q00->getValue(i,i);
        for (int j = i + 1; j < nCol; j++)
        {
          double val = c[0] * q00->getValue(i, j);
          mat[i][j] += val;
          mat[j][i] += val;
        }
      }
    }
    else
    {
      for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
          mat[i][j] += c[0] * q00->getValue(i, j);
    }
  }

  void PrecalcZOA::calculateElementVectorImpl(ElInfo const* elInfo, DenseVector<double>& vec)
  {
    if (firstCall)
    {
      q00 = Q00PsiPhi::provideQ00PsiPhi(rowFeSpace->getBasisFcts(),
                                        colFeSpace->getBasisFcts(),
                                        quadrature);
      q0 = Q0Psi::provideQ0Psi(rowFeSpace->getBasisFcts(), quadrature);
      firstCall = false;
    }

    DenseVector<double> c(1, 0.0);
    for (auto& term : terms)
      (static_cast<ZeroOrderTerm*>(term))->getC(elInfo, 1, c);

    c[0] *= elInfo->getDet();

    for (int i = 0; i < nRow; i++)
      vec[i] += c[0] * q0->getValue(i);
  }

} // end namespace AMDiS


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

/*!
 * \ingroup boost_simd_arithmetic
 * \defgroup boost_simd_arithmetic_abs abs
 *
 * \par Description
 * The function always returns a value of the same type than the entry.
 * \par
 * Take care that for signed integers the absolute value of Valmin is Valmin
 * (thus negative!).
 * This is a side effect of the 2-complement representation  of integers.
 * To avoid this,  you can use the \c abss saturated functor or convert the
 * input parameter to a larger type before taking the abs value.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/abs.hpp>
 * \endcode
 *
 * \par Alias
 * \arg modulus
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::abs_(A0)>::type
 *     abs(const A0 & a0);
 * }
 * \endcode
 *
 * \param a0 the unique parameter of abs
 *
 * \return a value of the same type as the parameter
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag abs_ of functor abs
     *        in namespace boost::simd::tag for toolbox boost.simd.arithmetic
    **/
    struct abs_ : ext::elementwise_<abs_> { typedef ext::elementwise_<abs_> parent; };
  }

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::abs_, abs, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::abs_, modulus, 1)
} }

#endif

// modified by jt the 25/12/2010


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 exponential toolbox - nthroot/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of exponential components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 08/12/2010
///
#include <nt2/toolbox/exponential/include/functions/nthroot.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/toolbox/constant/constant.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/include/functions/splat.hpp>

#include <nt2/include/functions/load.hpp>

NT2_TEST_CASE_TPL ( nthroot_real__2_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::nthroot;
  using nt2::tag::nthroot_;
  using nt2::load;
  using boost::simd::native;
  using nt2::meta::cardinal_of;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename nt2::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename nt2::meta::call<nthroot_(vT,ivT)>::type r_t;
  typedef typename nt2::meta::call<nthroot_(T,iT)>::type sr_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(-8),nt2::splat<ivT>(3))[0], sr_t(-2), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(256),nt2::splat<ivT>(4))[0], sr_t(4), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(8),nt2::splat<ivT>(3))[0], sr_t(2), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Inf<vT>(),nt2::splat<ivT>(3))[0], nt2::Inf<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Inf<vT>(),nt2::splat<ivT>(4))[0], nt2::Inf<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Minf<vT>(),nt2::splat<ivT>(3))[0], nt2::Minf<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Minf<vT>(),nt2::splat<ivT>(4))[0], nt2::Nan<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Mone<vT>(),nt2::splat<ivT>(3))[0], nt2::Mone<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Mone<vT>(),nt2::splat<ivT>(4))[0], nt2::Nan<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Nan<vT>(),nt2::splat<ivT>(3))[0], nt2::Nan<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Nan<vT>(),nt2::splat<ivT>(4))[0], nt2::Nan<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::One<vT>(),nt2::splat<ivT>(3))[0], nt2::One<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::One<vT>(),nt2::splat<ivT>(4))[0], nt2::One<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Zero<vT>(),nt2::splat<ivT>(3))[0], nt2::Zero<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Zero<vT>(),nt2::splat<ivT>(4))[0], nt2::Zero<sr_t>(), 0.5);
} // end of test for floating_

NT2_TEST_CASE_TPL ( nthroot_sintgt_16__2_0,  NT2_SIMD_SIGNED_INT_GT_16_TYPES)
{
  using nt2::nthroot;
  using nt2::tag::nthroot_;
  using nt2::load;
  using boost::simd::native;
  using nt2::meta::cardinal_of;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename nt2::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename nt2::meta::call<nthroot_(vT,ivT)>::type r_t;
  typedef typename nt2::meta::call<nthroot_(T,iT)>::type sr_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(-8),nt2::splat<ivT>(3))[0], sr_t(-2), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(256),nt2::splat<ivT>(4))[0], sr_t(4), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(8),nt2::splat<ivT>(3))[0], sr_t(2), 0.5);
} // end of test for sintgt_16_

NT2_TEST_CASE_TPL ( nthroot_uintgt_16__2_0,  NT2_SIMD_UNSIGNED_INT_GT_16_TYPES)
{
  using nt2::nthroot;
  using nt2::tag::nthroot_;
  using nt2::load;
  using boost::simd::native;
  using nt2::meta::cardinal_of;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename nt2::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename nt2::meta::call<nthroot_(vT,ivT)>::type r_t;
  typedef typename nt2::meta::call<nthroot_(T,iT)>::type sr_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(256),nt2::splat<ivT>(4))[0], sr_t(4), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(8),nt2::splat<ivT>(3))[0], sr_t(2), 0.5);
} // end of test for uintgt_16_


//
// Copyright (c) 2004-2006
// Andreas Kloeckner
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appear in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  The authors make no representations
// about the suitability of this software for any purpose.
// It is provided "as is" without express or implied warranty.
//

#ifndef HEADER_SEEN_META_HPP
#define HEADER_SEEN_META_HPP

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

// #include <helpers.hpp>
#include <pyublas/generic_ublas.hpp>

using namespace boost;
namespace ublas = boost::numeric::ublas;

using pyublas::is_vector;

// change_value_type
template <typename MatrixType, typename NewValueType>
struct change_value_type {
};

template <typename OLD, typename NEW>
struct change_value_type<ublas::unbounded_array<OLD>, NEW>
{ typedef ublas::unbounded_array<NEW> type; };

template <typename OldValueType, typename NewValueType>
struct change_value_type<ublas::matrix<OldValueType>, NewValueType>
{ typedef ublas::matrix<NewValueType> type; };

template <typename OLD, typename NEW, typename F, std::size_t IB, typename IA>
struct change_value_type<ublas::coordinate_matrix<OLD, F, IB, IA>, NEW>
{ typedef ublas::coordinate_matrix<NEW, F, IB, IA> type; };

template <typename OLD, typename NEW, typename F, std::size_t IB, typename IA>
struct change_value_type<ublas::compressed_matrix<OLD, F, IB, IA>, NEW>
{ typedef ublas::compressed_matrix<NEW, F, IB, IA> type; };

template <typename OldValueType, typename NewValueType>
struct change_value_type<ublas::vector<OldValueType>, NewValueType>
{ typedef ublas::vector<NewValueType> type; };

// generic instantiation infrastructure ---------------------------------------
template <typename Exposer, typename ValueType>
static void exposeForAllSimpleTypes(const std::string &python_eltname, const Exposer &exposer, ValueType)
{
  exposer.expose("Matrix" + python_eltname, ublas::matrix<ValueType>());
  exposer.expose("SparseExecuteMatrix" + python_eltname,
      ublas::compressed_matrix<ValueType,
      ublas::column_major, 0, ublas::unbounded_array<int> >());
  exposer.expose("SparseBuildMatrix" + python_eltname,
      ublas::coordinate_matrix<ValueType, ublas::column_major>());
}

template <typename Exposer, typename T>
static void exposeForAllMatrices(const Exposer &exposer, T)
{
  exposeForAllSimpleTypes("Float64", exposer, T());
}

template <typename Exposer, typename T>
static void exposeForAllMatrices(const Exposer &exposer, std::complex<T>)
{
  exposeForAllSimpleTypes("Complex128", exposer, std::complex<T>());
}

template <typename Exposer>
static void exposeForAllMatrices(const Exposer &exposer)
{
  exposeForAllMatrices(exposer, double());
  exposeForAllMatrices(exposer, std::complex<double>());
}

template <typename Exposer,typename T>
static void exposeForMatricesConvertibleTo(const Exposer &exposer, T)
{
  exposeForAllMatrices(exposer, T());
}

template <typename Exposer,typename T>
static void exposeForMatricesConvertibleTo(const Exposer &exposer, std::complex<T>)
{
  exposeForAllMatrices(exposer);
}

#endif


/*
 Point Cloud Registration Tool

 BSD 2-Clause License
 Copyright (c) 2017, Dario Rethage
 See LICENSE at package root for full license
 */

#ifndef util_hpp
#define util_hpp

// Standard
#include <stdio.h>
#include <string>
#include <iostream>
#include <locale>
#include <random>
#include <fstream>
#include <sys/stat.h>

// Boost
#include <boost/bind.hpp>

// Eigen
#include <Eigen/Dense>

// PCL
#include <pcl/common/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/console/time.h>

// PRT
#include "types.hpp"
#include "hsvrgb.hpp"
#include "tinyply.h"

// GFLAGS
#include <gflags/gflags.h>
DECLARE_bool(verbose);
using namespace PRT;

namespace util
{

    void print4x4Matrix (const Eigen::Matrix4f & matrix);
    double uniformRandom();
    int loadPointCloud(std::string filepath, PointCloudT &cloud);
    int writePointCloudToPLY(std::string filepath, PointCloudT &cloud);
    int writePointCloudToPLY(std::string filepath, PointCloudRGBT &cloud);
    void transformPointCloud(PointCloudT &cloud, double alpha, double beta, double gamma, double tx, double ty, double tz);
    void randomTransformPointCloud(PointCloudT::Ptr &cloud);
    void hsv2rgb(double& r, double& g, double& b, double& h, double& s, double& v);
    void rgb2hsv(double& r, double& g, double& b, double& h, double& s, double& v);
    std::string stringTail(const std::string s, const size_t l);
    void vectorMin(DoubleVectorPtr &v, double min);
    void removeElementsAboveThreshold(DoubleVectorPtr &v, double threshold);
    void vectorRescale(DoubleVectorPtr &v, double max_element);
    int writeMatrixToFile(const Eigen::MatrixXf &matrix, const std::string filepath);
    FilepairVectorPtr readBatchProcessingFile(std::string filepath);
    int ensureUniqueFilepath(std::string &filepath);
    bool fileExists(const std::string& filepath);
    std::string extractFileExtension(const std::string &filepath);
    std::string removeFileExtension(const std::string &filepath);
    void writeStringToFile(const std::string& filepath, const std::string s);
    int find_nexttolast(const std::string filepath);
}

#endif /* util_hpp */