// ORBSLAM2_get_trajectory.cpp

// This program reads the Pose output from ORBSLAM2 (message type PoseStamped) and combines them into an array to form a nav_msgs/Path message.
	// Subscribed line: /orb_slam2_rgbd/pose
	// Published lineL /orb_slam2_rgbd/trajectory

#include <iostream>
#include <string>
#include <sstream>
#include <vector>
#include <signal.h>
#include <fstream>
#include <array>
#include <cmath>

// Use Eigen3 library for vector math
#include <Eigen/Dense>
#include <Eigen/Geometry>

// Use Boost QVM library for Quaternion calculations
#include <boost/qvm/quat.hpp>
#include <boost/qvm/quat_operations.hpp>
#include <boost/qvm/quat_access.hpp>
#include <boost/qvm/mat.hpp>
#include <boost/qvm/mat_operations.hpp>
#include <boost/qvm/mat_access.hpp>
#include <boost/qvm/vec.hpp>
#include <boost/qvm/vec_operations.hpp>
#include <boost/qvm/vec_access.hpp>

// Could use all.hpp to include all QVM headers but not recommended
// #include <boost/qvm/all.hpp>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <rosbag/bag.h>

#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <tf2_msgs/TFMessage.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

// For pointcloud transformation
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <tf/LinearMath/Transform.h>
#include <tf/LinearMath/Quaternion.h>

using namespace std;
using namespace boost::qvm;

// Rotate by this much
	// TODO Improvement: Make this a launch arg, allow rotation in other directions
double theta = -0.47; // = 26.92 deg

// vec<float,3> camOffset(0.035, 0.0003, 0.053); // x,y,z camera offset from end effector
vec<float,3> camOffset = {0, 0, 0};

// Quaternion to store the initial translation and orientation of the camera rotated by initial camera position
quat<double> armTransOffset = {0, 0, 0, 0};
quat<double> armOrientOffset = {0, 0, 0, 1};

// File to save trajectory to - is located in .ros folder
string final_save_path = "trajectoryFinal.bag";
string full_save_path = "trajectoryFull.bag";

// Signal-safe flag for whether shutdown is requested
sig_atomic_t volatile g_request_shutdown = 0;

// Define paths as global variables for simplicity
nav_msgs::Path path_ORBSLAM2;
nav_msgs::Path path_arm;

sensor_msgs::PointCloud2 pointcloudArray[1000];
int nPC = 0;

// Have publisher as global variable
ros::Publisher ORBSLAM2_traj_pub;
ros::Publisher arm_traj_pub;

// Marker to represent model and base of arm
visualization_msgs::MarkerArray markerArray;
visualization_msgs::Marker marker;

int firstTF = 0;

int n_ORBSLAM2_pose = 1;
int n_arm_pose = 1;
int n_debug_image = 1;

// Debug check
int n = 0;

rosbag::Bag fullBag;

// Override on exiting program
void mySigIntHandler(int sig){
	// Save the trajectory to a bag - saves to /.ros/trajectory.bag
	rosbag::Bag bag;
	bag.open(final_save_path, rosbag::bagmode::Write);

	bag.write("/orb_slam2_rgbd/trajectory", ros::Time::now(), path_ORBSLAM2);
	bag.write("/tf/trajectory", ros::Time::now(), path_arm);
	bag.write("/visual_marker", ros::Time::now(), markerArray);
	bag.write("/orb_slam2_rgbd/map_points", ros::Time::now(), pointcloudArray[nPC]);
	bag.close();
	fullBag.close();

	g_request_shutdown = 1;
}

void debug_image_Callback(const sensor_msgs::ImageConstPtr& image){
	ROS_INFO_STREAM("Received debug image: " << n_debug_image);
	fullBag.write("orb_slam2_rgbd/debug_image", ros::Time::now(), image);
	n_debug_image++;
}

void pointcloud_Callback(const sensor_msgs::PointCloud2::ConstPtr& points){
	ROS_INFO_STREAM("Received pointcloud: " << nPC + 1);

	if(firstTF == 0) {
	// Always store last pointcloud, if over 1000 discard any except the last one
		pointcloudArray[nPC] = *points;
		fullBag.write("/orb_slam2_rgbd/map_points", ros::Time::now(), points);
	}
	else { // Transform into arm frame of reference

		// Construct a pure rotation, then pure translation matrix - must be Eigen 4x4 matrix

		// Convert to Eigen quaternion (note: Initialised in wxyz, stored as xyzw)
		Eigen::Quaternionf quat(boost::qvm::S(armOrientOffset), X(armOrientOffset), Y(armOrientOffset), Z(armOrientOffset));
		Eigen::Matrix3f rotMat = quat.normalized().toRotationMatrix();
		Eigen::Matrix4f transformRotation;
		transformRotation.setIdentity(); // Set identity matrix to make bottom right a 1
		transformRotation.block<3,3>(0,0) = rotMat; // Insert rotation matrix manually

		Eigen::Vector3f transVec(boost::qvm::X(armTransOffset), Y(armTransOffset), Z(armTransOffset));
		Eigen::Matrix4f transformTranslation;
		transformTranslation.setIdentity(); // Set identity matrix to make bottom right a 1
		transformTranslation.block<3,1>(0,3) = transVec; // Insert rotation matrix manually

		// tf::Transform transformRotation;
		// tf::Quaternion quat(boost::qvm::X(armOrientOffset), Y(armOrientOffset), Z(armOrientOffset), S(armOrientOffset));
		// transformRotation.setRotation(quat);
		// transformRotation.setOrigin(tf::Vector3(0,0,0));

		// tf::Transform transformTranslation;
		// tf::Quaternion baseQuat(0,0,0,1);
		// transformTranslation.setOrigin(tf::Vector3(boost::qvm::X(armTransOffset), Y(armTransOffset), Z(armTransOffset)));
		// transformTranslation.setRotation(baseQuat);

//		ROS_INFO_STREAM("armTransOffset=" << X(armTransOffset) << "," << Y(armTransOffset) << "," <<Z(armTransOffset));
//		ROS_INFO_STREAM("Transform=" << transform.getOrigin().x() << "," << transform.getOrigin().y() << "," <<transform.getOrigin().z());

		// Apply rotation first
		sensor_msgs::PointCloud2 pointsOutTemp;
		pcl_ros::transformPointCloud(transformRotation, *points, pointsOutTemp);
		sensor_msgs::PointCloud2 pointsOut;
		pcl_ros::transformPointCloud(transformTranslation, pointsOutTemp, pointsOut);

//		ROS_INFO_STREAM("Finished transforming pointcloud.");

		pointcloudArray[nPC] = pointsOut;
		fullBag.write("/orb_slam2_rgbd/map_points", ros::Time::now(), pointsOut);
	}

	// Always store last trajectory, if over 1000 discard any except the last one
	if(nPC < 999){
			nPC++;
	}

}

void pose_ORBSLAM2_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
	float xPosInit, yPosInit, zPosInit;

	geometry_msgs::PoseStamped pose;

	// Verify pose received
	ROS_INFO_STREAM("Received ORBSLAM2 pose: " << n_ORBSLAM2_pose);
	n_ORBSLAM2_pose++;

	// If first arm tf message never received, directly output pose instead
	if(firstTF == 0) {
		path_ORBSLAM2.header = msg->header;
		pose.header = msg->header;
		pose.pose = msg->pose;
	}
	else {
		path_ORBSLAM2.header = msg->header;
		pose.header = msg->header;

		// Use this only if the object is flat, i.e. aligned with the z-axis
		// pose.pose = msg->pose;

		// Extract x,y,z pose, place into quaternion
		boost::qvm::quat<double> ORBSLAM2TransQuat = {0, msg->pose.position.x, msg->pose.position.y, msg->pose.position.z};
		// Extract orientation
		quat<double> ORBSLAM2OrientQuat = {msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z};

		// Pose = Pose of arm + qv(q^-1), q = initial rotation of arm, v = ORBSLAM2 vector
		quat<double> finalTransQuat = armTransOffset + operator*(operator*(armOrientOffset, ORBSLAM2TransQuat), conjugate(armOrientOffset));

		// Rotation = q3 * q2 * q1, q3 = initial pose of arm, q2 = ORBSLAM2 pose of arm, q1 = initial pose in ORBSLAM2 (0,0,0,1) so q3 * q2
		quat<double> finalOrientQuat = operator*(armOrientOffset,ORBSLAM2OrientQuat);

		// Assign to new pose
		pose.pose.orientation.x = X(finalOrientQuat);
		pose.pose.orientation.y = Y(finalOrientQuat);
		pose.pose.orientation.z = Z(finalOrientQuat);
		pose.pose.orientation.w = S(finalOrientQuat);

		pose.pose.position.x = X(finalTransQuat);
		pose.pose.position.y = Y(finalTransQuat);
		pose.pose.position.z = Z(finalTransQuat);

	//	ROS_INFO_STREAM("ORBSLAM2 raw position: " << xPosInit << " y: " << yPosInit << " z: " << zPosInit);

		/*/////////////////
		Manual Rotation
		////////////////////

		// Need to rotate the trajectory so that it is flat with the z-axis
		xPosInit = msg->pose.position.x;
		yPosInit = msg->pose.position.y;
		zPosInit = msg->pose.position.z;

		double cosTheta = cos(theta);
		double sinTheta = sin(theta);

		// Rotate about y-axis and shift to position of arm
		// Flip x and z as they are upside down, and offset by starting position of tf
		pose.pose.position.x = xOffset - (xPosInit * cosTheta + zPosInit * sinTheta);
		pose.pose.position.y = yOffset + (yPosInit);
		pose.pose.position.z = zOffset - (zPosInit * cosTheta - xPosInit * sinTheta);

	//	ROS_INFO_STREAM("ORBSLAM2 Pose.pose.position.y = " << pose.pose.position.y);

		using namespace boost::qvm;

		// Apply 180 degree rotation about y-axis
		quat<double> ORBSLAMquat = {msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z};

		// Rotation by 180 deg in y-axis (account for skew correction)
		quat<double> rotQuat = roty_quat(3.14159f + theta);

		// Rotate ORBSLAMquat by rotQuat
		quat<double> finalOrientQuat = boost::qvm::operator*(rotQuat, ORBSLAMquat);

		pose.pose.orientation.x = boost::qvm::X(finalOrientQuat);
		pose.pose.orientation.y = boost::qvm::Y(finalOrientQuat);
		pose.pose.orientation.z = boost::qvm::Z(finalOrientQuat);
		pose.pose.orientation.w = boost::qvm::S(finalOrientQuat);

		*/

		// Caclulate transform from ORBSLAM2 frame of reference (Origin at 0,0,0, rotation 0,0,0,1) to camera frame of reference

		//	pose.pose.orientation = msg->pose.orientation;

		// Debug: Print header of msg
		// ROS_INFO_STREAM("\nReceived pose: " << msg->header);

	}

	// path_ORBSLAM2.header = msg->header;
	path_ORBSLAM2.poses.push_back(pose); // Add new pose to path array

	// Debug - print path - does not work
	// ROS_INFO_STREAM("\nPath: " << path_ORBSLAM2.poses[0].pose);
	// n = n + 1;

	// Can publish directly to view in rviz or similar
	//	traj_pub.publish(path_ORBSLAM2);
	ORBSLAM2_traj_pub.publish(path_ORBSLAM2);

	// Write to complete bag
	fullBag.write("/orb_slam2_rgbd/trajectory", ros::Time::now(), path_ORBSLAM2);

}

void pose_arm_Callback(/*const sensor_msgs::ImageConstPtr& image,*/ const tf2_msgs::TFMessage::ConstPtr& msg){
//	ROS_INFO_STREAM("Got tf\n"); // Debug line

// Verify pose received
	ROS_INFO_STREAM("Received arm pose: " << n_arm_pose);
	n_arm_pose++;

	geometry_msgs::TransformStamped single_tf;
	geometry_msgs::PoseStamped pose;

	// Get tf from last seen value
	single_tf = msg->transforms.back();

	// z cannot be 0 (in table), so break if blank data received
	if(single_tf.transform.translation.z == 0 && firstTF == 0){
		return;
	}

	// Only grab tf frames related to child frame "tool0_controller"
	if(single_tf.child_frame_id == "tool0_controller"){

	//	ROS_INFO_STREAM("Got tf for tool0_controller\n"); // Debug line
		path_arm.header = single_tf.header;
		path_arm.header.frame_id = "map"; // To match in line with ORBSLAM2 naming conventions

		pose.header = single_tf.header;
		pose.header.frame_id = "map";

		// Debug test - override quaternion rotation
//		Vector3f quatXYZ(0.7071068, 0, 0);
//		float quatW = 0.7071068;

		// Place orientation into quaternion
		boost::qvm::quat<double> armQuat = {single_tf.transform.rotation.w, single_tf.transform.rotation.x, single_tf.transform.rotation.y, single_tf.transform.rotation.z};

		// Convert camera offset to quaaternion with real part 0
		quat<double> camOffsetQuat = {0, A<0>(camOffset), A<1>(camOffset), A<2>(camOffset)};

		// Rotate about z-axis 45 deg
		quat<double> rotQuat = boost::qvm::rotz_quat(0.7854f);

		// Calculate pose rotation by p' = qp(q^-1) where q = armQuat and p = camOffsetQuat;

		// Rotate into end effector frame of reference first
		quat<double> camOffsetRotated =  boost::qvm::operator*(operator*(armQuat, camOffsetQuat), conjugate(armQuat));

			// But have to remember camera is tilted by 45 deg relative to axis of end effector - possible that we need to rotate by 45 deg around z-axis of end effector
		// Rotate about z-axis 45 degrees to reach camera angle
//		quat<double> camOrientOffsetFinal = operator*(operator*(rotQuat, camOffsetRotated), conjugate(rotQuat));

		// If 45 degree rotation is already accounted for:
		quat<double> camOrientOffsetFinal = camOffsetRotated;

		// Calculating each part separately
		//quat<double> camOffsetTemp = boost::qvm::operator*(armQuat, camOffsetQuat);
		// quat<double> camOffsetConj = conjugate(camOffsetQuat);
		// quat<double> camOffsetRotated = boost::qvm::operator*(camOffsetTemp, camOffsetConj);

//		camOffsetQuat = conjugate(armQuat);

//		ROS_INFO_STREAM("\nBoost values: s= " << S(camOffsetRotated) << "\nx= " << X(camOffsetRotated) << "\ny= "<< Y(camOffsetRotated) << "\nz= "<< Z(camOffsetRotated));

		// Eigen::Vector3f quatXYZ(single_tf.transform.rotation.x, single_tf.transform.rotation.y, single_tf.transform.rotation.z);
		// float quatW = single_tf.transform.rotation.w;

		// Calculate offset of camera, rotated according to the end effector
		// Rotated vector v' = 2 * dot(u,v) * u + (s^2 - dot(u,u)) * v + 2s (u x v) - where u = [q.x,q.y,q.z]
		// Eigen::Vector3f camOffsetRotatedEigen = 2.0f * (camOffset.dot(quatXYZ)) * quatXYZ
		// 			+ (quatW * quatW - quatXYZ.dot(quatXYZ)) * camOffset
		// 			+ 2.0f * quatW * quatXYZ.cross(camOffset);

		// ROS_INFO_STREAM("\ncamOffsetRotated values: " << camOffsetRotatedEigen);

		// Have to reassign translation/position as they are different formats
		pose.pose.position.x = single_tf.transform.translation.x + boost::qvm::X(camOrientOffsetFinal);
		pose.pose.position.y = single_tf.transform.translation.y + Y(camOrientOffsetFinal);
		pose.pose.position.z = single_tf.transform.translation.z + Z(camOrientOffsetFinal);

//		ROS_INFO_STREAM("camOffsetRotated= x:" << X(camOffsetRotated) << ", y:" << Y(camOffsetRotated) << ", z:" << Z(camOffsetRotated));

		// DEBUG: Override value
//		rotQuat = rotz_quat(0.0f);

		// Rotate about 45 degrees about the z-axis
		quat<double> finalOrientQuat = boost::qvm::operator*(armQuat, rotQuat);

		// Remember qvm::quat is in form {w, x, y, z}
		pose.pose.orientation.x = boost::qvm::X(finalOrientQuat);
		pose.pose.orientation.y = Y(finalOrientQuat);
		pose.pose.orientation.z = Z(finalOrientQuat);
		pose.pose.orientation.w = S(finalOrientQuat);

		// Get offsets if it is the first TF message
		if(firstTF == 0) {
			firstTF = 1;
			armTransOffset = {0, pose.pose.position.x, pose.pose.position.y, pose.pose.position.z};
			armOrientOffset = finalOrientQuat; // Use the camera pose rather than the end effector pose

			// Print starting offsets
			ROS_INFO_STREAM("\n Offsets are: \n x:"<< X(armTransOffset) << "\n y:" << Y(armTransOffset) << "\n z:" << Z(armTransOffset));
		}

		path_arm.poses.push_back(pose); // Add new pose to path array

		arm_traj_pub.publish(path_arm); // Publish pose

		// Write to complete bag
		fullBag.write("/tf/trajectory", ros::Time::now(), path_arm);
		fullBag.write("/visual_marker", ros::Time::now(), markerArray);

	}
}

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

	// Initialise ROS, and override SIGINT handler (what happens with ctrl+c) with my own code
	ros::init(argc, argv, "ORBSLAM2_get_trajectory", ros::init_options::NoSigintHandler);
	signal(SIGINT, mySigIntHandler);

	ros::NodeHandle traj_nh;

	// Retreive camera offset values from launch file
	float camOffsetX, camOffsetY, camOffsetZ;
	traj_nh.getParam("camOffsetX", camOffsetX);
	traj_nh.getParam("camOffsetY", camOffsetY);
	traj_nh.getParam("camOffsetZ", camOffsetZ);

	// Define if will capture ORBSLAM2 debug image
	bool captureImageFlag;
	traj_nh.getParam("captureImageFlag", captureImageFlag);

	// Get camera offset from input arguments, store into vector
	boost::qvm::A<0>(camOffset) = camOffsetX;
	A<1>(camOffset) = camOffsetY;
	A<2>(camOffset) = camOffsetZ;

	ROS_INFO_STREAM("camOffset = " << camOffsetX << "," << camOffsetY << "," << camOffsetZ);

	// QUATERNION TESTING
	// boost::qvm::quat<double> q12 = {1,3,4,3};
	// boost::qvm::quat<double> q23 = {4,3.9,-1,-3};

	// boost::qvm::quat<double> q34 = boost::qvm::operator*(q12, q23);

	// // Access x,y,z, scalar components of quaternion
	// ROS_INFO_STREAM("quat: = " << boost::qvm::X(q34) << "," << boost::qvm::Y(q34) << "," << boost::qvm::Z(q34) << "," << boost::qvm::S(q34));

	// Define subscriber line to listen to pose from ORBSLAM2
	ros::Subscriber sub_ORBSLAM2_pose = traj_nh.subscribe("/orb_slam2_rgbd/pose", 10, pose_ORBSLAM2_Callback);

	// Define second subscriber for obtaining the tf from the arm
	ros::Subscriber sub_arm_pose = traj_nh.subscribe("/tf", 1, pose_arm_Callback);

	ros::Subscriber sub_pointcloud = traj_nh.subscribe("/orb_slam2_rgbd/map_points", 10, pointcloud_Callback);

	// Need to define subscriber here or it is deleted when exiting 'if' block
	ros::Subscriber sub_debug_image;
	if(captureImageFlag) {
		ROS_INFO_STREAM("Will capture debug image.");
		sub_debug_image = traj_nh.subscribe("/orb_slam2_rgbd/debug_image", 10, debug_image_Callback);
	}

    // message_filters::Subscriber<sensor_msgs::Image> image_sub(traj_nh, "/camera/rgb/image_raw", 1);
    // message_filters::Subscriber<tf2_msgs::TFMessage> tf_sub(traj_nh, "/tf", 1);
    // message_filters::TimeSynchronizer<sensor_msgs::Image, tf2_msgs::TFMessage> sync(image_sub, tf_sub, 10);

    // sync.registerCallback(boost::bind(&pose_arm_Callback, _1, _2));

	// Set up publishing line for path to be sent on
	arm_traj_pub = traj_nh.advertise<nav_msgs::Path>("/tf/trajectory", 1000);
	ORBSLAM2_traj_pub = traj_nh.advertise<nav_msgs::Path>("/orb_slam2_rgbd/trajectory", 1000);

	// Set up a marker to represent the target
	ros::Publisher markerPub = traj_nh.advertise<visualization_msgs::MarkerArray>("/visual_marker", 0);

	// Bag to write all the trajectory and pointcloud into
	fullBag.open(full_save_path, rosbag::bagmode::Write);

	// Define marker for model stand-in - DO NOT NEED ANYMORE AS CAN TRANSFORM POINTCLOUD
	// marker.header.frame_id = "map";
	// marker.header.stamp = ros::Time();
	// marker.ns = "my_namespace";
	// marker.id = 0;
	// marker.type = visualization_msgs::Marker::CUBE;
	// marker.action = visualization_msgs::Marker::ADD;
	// marker.pose.position.x = 0.4;
	// marker.pose.position.y = 0.05;
	// marker.pose.position.z = 0.1;
	// marker.pose.orientation.x = 0.0;
	// marker.pose.orientation.y = 0.0;
	// marker.pose.orientation.z = 0.0;
	// marker.pose.orientation.w = 1.0;
	// marker.scale.x = 0.2;
	// marker.scale.y = 0.2;
	// marker.scale.z = 0.2;
	// marker.color.a = 1.0; // Don't forget to set the alpha!
	// marker.color.r = 0.0;
	// marker.color.g = 1.0;
	// marker.color.b = 0.0;

//	markerArray.markers.push_back(marker);

	// Define marker for base of arm
	marker.header.frame_id = "map";
	marker.header.stamp = ros::Time();
	marker.ns = "my_namespace";
	marker.id = 1;
	marker.type = visualization_msgs::Marker::CYLINDER;
	marker.action = visualization_msgs::Marker::ADD;
	marker.pose.position.x = 0;
	marker.pose.position.y = 0;
	marker.pose.position.z = 0.15;
	marker.pose.orientation.x = 0.0;
	marker.pose.orientation.y = 0.0;
	marker.pose.orientation.z = 0.0;
	marker.pose.orientation.w = 1.0;
	marker.scale.x = 0.1;
	marker.scale.y = 0.1;
	marker.scale.z = 0.3;
	marker.color.a = 1.0; // Don't forget to set the alpha!
	marker.color.r = 0.6; // Colour grey
	marker.color.g = 0.6;
	marker.color.b = 0.6;

	markerArray.markers.push_back(marker);

	// Set publishing to run at 60Hz
	ros::Rate loop_rate(15);

	// Run the node until terminated
	while(!g_request_shutdown)
	{
		ros::spinOnce();
		markerPub.publish(markerArray);
		// Write markers to bag
		loop_rate.sleep();
	}

	// Manual shutdown after ctrl+c
	ros::shutdown();
}


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "cuNDArray_fileio.h"
#include "cuNDArray.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoNDArray_utils.h"
#include "hoCuPartialDerivativeOperator.h"
#include "cuTvOperator.h"
#include "cuTv1dOperator.h"
#include "cuTvPicsOperator.h"
#include "CBSubsetOperator.h"
#include "osSPSSolver.h"
#include "osMOMSolver.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <boost/make_shared.hpp>
#include "cuSolverUtils.h"
using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

boost::shared_ptr<hoCuNDArray<float> > calculate_prior(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& projections, std::vector<size_t> is_dims, floatd3 imageDimensions){
  std::cout << "Calculating FDK prior" << std::endl;
	boost::shared_ptr<CBCT_binning> binning_pics= binning->get_3d_binning();
	    std::vector<size_t> is_dims3d = is_dims;
	    is_dims3d.pop_back();
	    boost::shared_ptr< hoCuConebeamProjectionOperator >
	      Ep( new hoCuConebeamProjectionOperator() );
	    Ep->setup(ps,binning_pics,imageDimensions);
	    Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
	    Ep->set_domain_dimensions(&is_dims3d);
	      Ep->set_use_filtered_backprojection(true);
	    boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
	    Ep->mult_MH(&projections,prior3d.get());

	    hoCuNDArray<float> tmp_proj(*ps->get_projections());
	    Ep->mult_M(prior3d.get(),&tmp_proj);
	    float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
	    *prior3d *= s;
	    boost::shared_ptr<hoCuNDArray<float> > prior(new hoCuNDArray<float>(*expand( prior3d.get(), is_dims.back() )));
	    std::cout << "Prior complete" << std::endl;
	    return prior;
}

int main(int argc, char** argv)
{
  string acquisition_filename;
  string outputFile;
  uintd3 imageSize;
  floatd3 voxelSize;
  int device;
  unsigned int downsamples;
  unsigned int iterations;
  unsigned int subsets;
  float rho;
  float tv_weight,pics_weight;

  po::options_description desc("Allowed options");

  desc.add_options()
    ("help", "produce help message")
    ("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    ("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    ("output,f", po::value<string>(&outputFile)->default_value("reconstruction.real"), "Output filename")
    ("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    ("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    ("SAG","Use exact SAG correction if present")
    ("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    ("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    ("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    ("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    ("downsample,D",po::value<unsigned int>(&downsamples)->default_value(0),"Downsample projections this factor")
    ("subsets,u",po::value<unsigned int>(&subsets)->default_value(10),"Number of subsets to use")
    ("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight")
    ("PICS",po::value<float>(&pics_weight)->default_value(0),"PICS weight")
    ("use_prior","Use an FDK prior")
    ;

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

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  std::cout << "Command line options:" << std::endl;
  for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
    boost::any a = it->second.value();
    std::cout << it->first << ": ";
    if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
    else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
    else if (a.type() == typeid(unsigned int)) std::cout << it->second.as<unsigned int>();
    else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
    else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
    else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
    else if (a.type() == typeid(vector_td<unsigned int,3>)) std::cout << it->second.as<vector_td<unsigned int,3> >();
    else std::cout << "Unknown type" << std::endl;
    std::cout << std::endl;
  }

  cudaSetDevice(device);
  cudaDeviceReset();

  //Really weird stuff. Needed to initialize the device?? Should find real bug.
  cudaDeviceManager::Instance()->lockHandle();
  cudaDeviceManager::Instance()->unlockHandle();

  boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
  ps->load(acquisition_filename);
  ps->get_geometry()->print(std::cout);
	ps->downsample(downsamples);

  float SDD = ps->get_geometry()->get_SDD();
  float SAD = ps->get_geometry()->get_SAD();

  boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
  if (vm.count("binning")){
    std::cout << "Loading binning data" << std::endl;
    binning->load(vm["binning"].as<string>());
  } else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
  binning->print(std::cout);

  floatd3 imageDimensions;
  if (vm.count("dimensions")){
    imageDimensions = vm["dimensions"].as<floatd3>();
    voxelSize = imageDimensions/imageSize;
  }
  else imageDimensions = voxelSize*imageSize;

  float lengthOfRay_in_mm = norm(imageDimensions);
  unsigned int numSamplesPerPixel = 3;
  float minSpacing = min(voxelSize)/numSamplesPerPixel;

  unsigned int numSamplesPerRay;
  if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
  else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

  float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
  size_t numProjs = ps->get_projections()->get_size(2);
  size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);
  std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

  std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
  std::cout << "Image size " << imageDimensions << std::endl;

  is_dims.push_back(binning->get_number_of_bins());
  osMOMSolver<cuNDArray<float>> solver;

  if (pics_weight > 0){
  	std::cout << "Calculating PICS prior" << std::endl;
  	hoCuConebeamProjectionOperator op;
  	auto bin3D = binning->get_3d_binning();
  	op.setup(ps,bin3D,imageDimensions);
  	hoCuNDArray<float> proj(*ps->get_projections());
  	op.offset_correct(&proj);
  	op.set_use_filtered_backprojection(true);

  	std::vector<size_t> is_dims3D = to_std_vector((uint64d3)imageSize);

  	hoCuNDArray<float> image(is_dims3D);

  	op.mult_MH(&proj,&image,false);
  	auto prior = boost::make_shared<cuNDArray<float>>(image);
  	auto PICS = boost::make_shared<cuTvPicsOperator<float,3>>();
  	PICS->set_prior(prior);
  	PICS->set_weight(pics_weight);
  	solver.add_nonlinear_operator(PICS);

  	//write_nd_array(prior.get(),"fdk_prior.real");
  }

  // Define encoding matrix
  auto E = boost::make_shared<CBSubsetOperator<cuNDArray> >(subsets);

  //E->setup(ps,binning,imageDimensions);
  E->setup(ps,binning,imageDimensions);
  E->set_domain_dimensions(&is_dims);
  E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

  /*auto weight_array = boost::make_shared<cuNDArray<float>>(is_dims);
  {
  	boost::shared_ptr<linearOperator<cuNDArray<float>>> E2(E);
  	cuNDArray<float> tmp_proj(*ps->get_projections()->get_dimensions());
  	fill(&tmp_proj,1.0f);
  	E2->mult_MH(&tmp_proj,weight_array.get(),false);
  	clamp_min(weight_array.get(),1.0f);
  	reciprocal_inplace(weight_array.get());
  	//*weight_array *= *weight_array;
  }
  write_nd_array(weight_array.get(),"weights.real");
*/
  //hoCuCgDescentSolver<float> solver;

  //osSPSSolver<hoNDArray<float>> solver;
  //hoCuNCGSolver<float> solver;
  solver.set_encoding_operator(E);
  //solver.set_domain_dimensions(&is_dims);
  solver.set_max_iterations(iterations);
  solver.set_output_mode(osSPSSolver<cuNDArray<float>>::OUTPUT_VERBOSE);
  //solver.set_non_negativity_constraint(true);
  //solver.set_rho(rho);

  if (tv_weight > 0){

  	auto total_variation = boost::make_shared<cuTvOperator<float,4>>();
  	total_variation->set_weight(tv_weight);
  	//total_variation->set_weight_array(weight_array);
  	solver.add_nonlinear_operator(total_variation);
  	solver.set_kappa(tv_weight);
  	/*
  	auto total_variation_t = boost::make_shared<cuTv1DOperator<float,4>>();
  	total_variation_t->set_weight(tv_weight);
  	solver.add_nonlinear_operator(total_variation_t);
  	*/
/*
  	auto total_variation2 = boost::make_shared<cuWTvOperator<float,4>>();
  	total_variation2->set_step(2);
  	total_variation2->set_weight(tv_weight);
  	total_variation2->set_weight_array(weight_array);
  	solver.add_nonlinear_operator(total_variation2);
  	auto total_variation3 = boost::make_shared<cuWTvOperator<float,4>>();
  	total_variation3->set_step(3);
  	total_variation3->set_weight(tv_weight);
  	total_variation3->set_weight_array(weight_array);
  	solver.add_nonlinear_operator(total_variation3);
*/
  }

  cuNDArray<float> projections(*ps->get_projections());
  std::cout << "Projection norm:" << nrm2(&projections) << std::endl;
  //E->set_use_offset_correction(false);
  E->offset_correct(&projections);
  std::cout << "Projection norm:" << nrm2(&projections) << std::endl;

  {
  	E->set_use_offset_correction(false);
  	linearOperator<cuNDArray<float>>* E_all = E.get();
  	auto precon_image = boost::make_shared<cuNDArray<float>>(is_dims);
  	fill(precon_image.get(),1.0f);
  	cuNDArray<float> tmp_proj(projections.get_dimensions());

  	E_all->mult_M(precon_image.get(),&tmp_proj);
  	E_all->mult_MH(&tmp_proj,precon_image.get());

   	clamp_min(precon_image.get(),1e-6f);
  	reciprocal_inplace(precon_image.get());
  	solver.set_preconditioning_image(precon_image);

  	std::cout << "Precon mean: " << mean(precon_image.get()) << std::endl;
  	E->set_use_offset_correction(true);

  }

/*
    boost::shared_ptr<hoCuNDArray<float> > prior;

  if (vm.count("use_prior")) {
  	prior = calculate_prior(binning,ps,projections,is_dims,imageDimensions);
  	solver.set_x0(prior);
  }
*/

  auto result = solver.solve(&projections);

  write_nd_array<float>( result.get(), outputFile.c_str());
}


#include "corsika-reader/I3CORSIKAService.h"
#include "I3CORSIKAReaderUtils.h"

#include <iostream>
#include <cmath>
#include <chrono>
#include <fstream>

#include <dataclasses/physics/I3MCTreeUtils.h>
#include <phys-services/surfaces/Cylinder.h>
#include <phys-services/I3GSLRandomService.h>

#include <neutrino-generator/legacy/I3NuGIncrementalEventGenerator.h>
#include <neutrino-generator/Steering.h>
#include <neutrino-generator/Steering.h>

#include <MuonGun/Cylinder.h>

#include <earthmodel-service/EarthModelService.h>

#include <weighting/fluxes.h>
#include <boost/range/irange.hpp>

double muon_energy_threshold(double max_range)
{
	double a=0.212/1.2, b=0.251e-3/1.2;
	return (std::exp(max_range*b) - 1)*a/b;
}

void hoboplopia(I3MCTree &tree, I3MuonGun::SamplingSurface &surface, I3RandomService &rng)
{
#if 1
	auto generator = GaisserH3a(6e2, 1e11);

	double t0 = tree.get_heads().at(0).GetTime();
	double window = 40*I3Units::microsecond;
	double rate = surface.GetAcceptance(0,1)*generator.total()*I3Units::hertz;
	int n = rng.Poisson(window*rate);

	for (auto i : boost::irange(0,n,1)) {
		I3Particle::ParticleType type;
		double energy;
		I3Position pos;
		I3Direction dir;
		std::tie(type, energy) = generator(rng);
		surface.SampleImpactRay(pos, dir, rng, 0, 1);

		I3Particle p;
		p.SetType(type);
		if (energy/nucleonNumber(type) < muon_energy_threshold(I3CORSIKAReaderUtils::GetSlantDepth(dir, pos)))
		{
			continue;
		}
		p.SetEnergy(energy);
		p.SetTime(t0 + rng.Uniform(-window/2, window/2));
		p.SetDir(dir);
		p.SetPos(pos);

		I3MCTreeUtils::AddPrimary(tree, p);
	}
	// log_info_stream("skipped " << n+1-I3MCTreeUtils::GetPrimaries(tree).size() << " of " << n << " coincident showers");
#endif
}

void run_single_event(const std::string &corsika_executable, int nshowers=1, bool neutrinos=true)
{
	auto rng = boost::make_shared<I3GSLRandomService>(0);

	// Accept InIce particles (neutrino secondaries), plus any muons that can
	// reach the sampling surface. Other particles are dropped.
	I3MuonGun::Cylinder surface(1600,800);
	auto inice_filter = [&surface](const I3Particle &p) {
		if (p.GetLocationType() == I3Particle::InIce)
			return true;
		else if (p.GetType() != I3Particle::MuMinus && p.GetType() != I3Particle::MuPlus)
			return false;
		double a=0.212/1.2, b=0.251e-3/1.2;
		double max_range = std::log(1 + p.GetEnergy()*b/a)/b;
		// i3_assert(surface.GetIntersection(p.GetPos(), p.GetDir()).first < max_range);
		return surface.GetIntersection(p.GetPos(), p.GetDir()).first < max_range;
	};

	// Set up CORSIKA
	boost::shared_ptr<CorsikaService> caas(new CorsikaService(corsika_executable));

	// Set up NeutrinoGenerator
	boost::shared_ptr<I3CosmicEventGenerator> generator;
	if (neutrinos) {
		auto earthmodel = boost::make_shared<earthmodel::EarthModelService>();
		auto steering = boost::make_shared<nugen::Steering>(earthmodel);
		auto interaction_info = boost::make_shared<I3NuGInteractionInfo>(rng, steering, "csms");
		interaction_info->Initialize();
		auto neutrino_propagator = boost::make_shared<I3NuGIncrementalEventGenerator>(rng, steering, interaction_info);
		auto neutrino_selector = boost::make_shared<I3NuGSelector>(steering, rng, 10);

		// Mix them all together in a big jug
		generator.reset(new I3CosmicEventGenerator(caas, inice_filter, neutrino_selector, neutrino_propagator));
	} else {
		generator.reset(new I3CosmicEventGenerator(caas, inice_filter));
	}

	I3MCTree source_tree;
	{
		I3Frame frame;
		I3MCTree tree;

		// Make a primary
		I3Particle p;
		p.SetType(I3Particle::PPlus);
		p.SetEnergy(1e2);
		p.SetTime(0);
		p.SetDir(I3Direction(70*I3Units::degree, 37*I3Units::degree));
		p.SetPos(I3Position(-8,7,3));
		I3MCTreeUtils::AddPrimary(tree, p);

		// Add coincident showers
		hoboplopia(tree, surface, *rng);
		source_tree = tree;
	}

	typedef I3Map<I3ParticleID, std::tuple<CorsikaClient::BiasParticleType, double, double>> ShowerBiasMap;

	typedef std::chrono::high_resolution_clock clock;
	auto start = clock::now();
	for (int i=0; i < nshowers; i++) {
		try {
#if 1
			I3Frame frame;
			I3MCTree tree;

			// Make a primary
			I3Particle p;
			p.SetType(I3Particle::PPlus);
			p.SetEnergy(1e2);
			p.SetTime(0);
			p.SetDir(I3Direction(70*I3Units::degree, 37*I3Units::degree));
			p.SetPos(I3Position(-8,7,3));
			I3MCTreeUtils::AddPrimary(tree, p);

			// Add coincident showers
			// hoboplopia(tree, surface, *rng);

			auto biases = boost::make_shared<ShowerBiasMap>();
			biases->emplace(p.GetID(), ShowerBiasMap::mapped_type(CorsikaClient::Mu,1e-3,1));
			frame.Put("ShowerBias", biases);
#else
			I3Frame frame;
			I3MCTree tree = source_tree;
#endif
			// Callback to feed particles down the simulation chain as they are
			// produced. This could enqueue particles into CLSim, or simulate the
			// IceTop tank response (or both!)
			auto emitParticle = [](I3Particle &p) {
				// std::cout << p.GetTypeString() << " " << p.GetEnergy() << std::endl;
			};
			// Simulate showers from all primaries in the tree, be they cosmic rays
			// or neutrinos
			generator->Generate(tree, frame, emitParticle);

			for (auto &primary : tree.get_heads()) {
				auto node = tree.find(primary);
				if (I3MCTree::sibling_iterator(node) == tree.end_sibling())
					tree.erase(node);
			}
			// std::cout << tree << std::endl;

		} catch (std::exception &e) {
			log_error_stream(e.what());
		}
		if (i % std::max(nshowers/100, 10000) == 0) {
			auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(clock::now()-start);
			log_info_stream(i+1 << " of " << nshowers << ": " << elapsed.count()/double(i+1) << " ms/shower");
		}
	}
	auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(clock::now()-start);
	std::cout << elapsed.count()/double(nshowers) << " ms/shower" << std::endl;
}

// see: https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
class welford_gap_timer {

public:
	welford_gap_timer(bool storeit=false) : t0(clock::now()), count(0), mean(0), mean2(0), store(storeit) {}

	void update()
	{
		auto t1 = clock::now();
		int value = std::chrono::duration_cast<std::chrono::microseconds>(t1-t0).count();
		t0 = t1;
		count++;
		double delta = value - mean;
		mean += delta / count;
		double delta2 = value - mean;
		mean2 += delta*delta2;
		if (store)
			storage.push_back(value);
	}

	void report()
	{
		double variance = mean2/(count-1);
		if (count >= 2)
			log_info_stream(mean << " +- " << std::sqrt(variance) << " microseconds between events ("<<count<<")");

	}

	void dump(const std::string &fname)
	{
		std::fstream fs(fname.c_str(), std::ios::out | std::ios::binary);
		fs.write((char*)(storage.data()), sizeof(int)*storage.size());
		fs.close();
	}
	std::vector<int> storage;

private:
	typedef std::chrono::high_resolution_clock clock;
	decltype(clock::now()) t0;
	size_t count;
	double mean, mean2;
	bool store;
};

void run_powerlaw(const std::string &corsika_executable, int nshowers, double emin, double emax, double gamma, bool skip_subthreshold=false)
{
	auto rng = boost::make_shared<I3GSLRandomService>(0);

	auto spectrum = PowerLaw(1, emin, emax, gamma);

	// Accept InIce particles (neutrino secondaries), plus any muons that can
	// reach the sampling surface. Other particles are dropped.
	I3MuonGun::Cylinder surface(1600,800);
	auto inice_filter = [&surface](const I3Particle &p) {
		if (p.GetLocationType() == I3Particle::InIce)
			return true;
		else if (p.GetType() != I3Particle::MuMinus && p.GetType() != I3Particle::MuPlus)
			return false;
		double a=0.212/1.2, b=0.251e-3/1.2;
		double max_range = std::log(1 + p.GetEnergy()*b/a)/b;
		// i3_assert(surface.GetIntersection(p.GetPos(), p.GetDir()).first < max_range);
		return surface.GetIntersection(p.GetPos(), p.GetDir()).first < max_range;
	};

	// Set up CORSIKA
	boost::shared_ptr<CorsikaService> caas(new CorsikaService(corsika_executable));

	// Set up NeutrinoGenerator
	boost::shared_ptr<I3CosmicEventGenerator> generator;
	generator.reset(new I3CosmicEventGenerator(caas, inice_filter));

	typedef std::chrono::high_resolution_clock clock;
	auto start = clock::now();
	welford_gap_timer gap_timer(true);
	welford_gap_timer event_timer(true);

	for (int i=0; i < nshowers; i++) {
		try {
			I3Frame frame;
			I3MCTree tree;

			// Make a primary
			I3Particle p;
			p.SetType(I3Particle::PPlus);

			p.SetEnergy(spectrum(*rng));
			p.SetTime(0);
			I3Direction dir;
			I3Position pos;
			surface.SampleImpactRay(pos, dir, *rng, 0, 1);
			p.SetDir(dir);
			p.SetPos(pos);
			if (p.GetEnergy()/nucleonNumber(p.GetType()) < muon_energy_threshold(I3CORSIKAReaderUtils::GetSlantDepth(dir, pos)))
			{
				if (skip_subthreshold)
					continue;
			}
			I3MCTreeUtils::AddPrimary(tree, p);

			// Callback to feed particles down the simulation chain as they are
			// produced. This could enqueue particles into CLSim, or simulate the
			// IceTop tank response (or both!)
			auto emitParticle = [&gap_timer](I3Particle &p) {
				if (std::abs(p.GetType()) == std::abs(I3Particle::MuMinus))
					gap_timer.update();
				// std::cout << p.GetTypeString() << " " << p.GetEnergy() << std::endl;
			};
			// Simulate showers from all primaries in the tree, be they cosmic rays
			// or neutrinos
			generator->Generate(tree, frame, emitParticle);

			for (auto &primary : tree.get_heads()) {
				auto node = tree.find(primary);
				if (I3MCTree::sibling_iterator(node) == tree.end_sibling())
					tree.erase(node);
			}
			event_timer.update();
			// std::cout << tree << std::endl;

		} catch (std::exception &e) {
			log_error_stream(e.what());
		}
		if (i % std::max(nshowers/20, 10) == 0) {
			auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(clock::now()-start);
			log_info_stream(i+1 << " of " << nshowers << ": " << elapsed.count()/double(i+1) << " ms/shower");
			gap_timer.report();
		}
	}
	auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(clock::now()-start);
	std::cout << elapsed.count()/double(nshowers) << " ms/shower" << std::endl;
	gap_timer.report();
	gap_timer.dump("corsika-remotecontrol-gaps.dat");
	event_timer.dump("corsika-remotecontrol-event-gaps.dat");

}

void run_single_shower(const std::string &corsika_executable)
{
	CorsikaClient::config_type config;
	config.rng[0].seed = 42;
	config.rng[0].ncalls = 17;
	config.rng[1].seed = 538;
	config.atmosphere = 11;
	// config.elcuts[2] = 1;
	// config.elcuts[3] = 1;
	CorsikaClient caas(corsika_executable, config);

	typedef std::chrono::high_resolution_clock clock;
	auto start = clock::now();
	int nshowers = 10000;
	double total_weight = 0;
	for (int i=0; i < nshowers; i++) {
		std::array<double,4> elcuts = {{320.,320.,1e21,1e21}};
		caas.StartShower(14, 1e6, 70*M_PI/180, 0, CorsikaClient::NuE, 1e-3, elcuts);
		bool finished = false;
		int muons = 0;
		while (true) {
			std::vector<double> particle = caas.NextParticle();
			if (particle.empty())
				break;

			int ptype = particle[0];
			double gamma = particle[1];
			double ct = particle[2];
			double height = particle[5];
			double x = particle[7];
			double y = particle[8];

#if 0
			std::cout << "obslevel  "
				<< " type " << ptype
				<< " gamma " << gamma
				<< " ct " << ct
				<< " height " << height
				<< " x " << x
				<< " y " << y
				<< " fields " << particle.size()

				<< std::endl;
#endif

			if (ptype == 5 || ptype == 6) {
				muons++;
				if (muons % 100 == 0) {
					std::cout << "--- " << muons << std::endl;
				}
			}

		}
		auto header = caas.GetEventHeader();
		auto trailer = caas.GetEventEnd();
		if (i % 100 == 0)
			std::cout << i << ": " << muons << " muons, bias: " << header[219] << ", weight: " << trailer[266] << ", max_x: " << trailer[267] << std::endl;
		total_weight += trailer[266];
	}
	auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(clock::now()-start);
	std::cout << "total weight: " << total_weight << " ("<<nshowers<<" showers)" << std::endl;
	std::cout << elapsed.count()/double(nshowers) << " ms/shower" << std::endl;

}

int main (int argc, char const *argv[])
{
	if (argc < 2) {
		std::cerr << "need at least 1 arguments (got "<<argc-1<<")" << std::endl;
		exit(1);
	}

	// run_powerlaw(argv[1], 10000,  1e6, 1e9, -2, true);
	// run_powerlaw(argv[1], 1000000,  6e2, 1e5, -2.6, true);
	// run_powerlaw(argv[1], 100000,  3e4, 1e6, -2.6, true);

	// run_powerlaw(argv[1], 10,  1e4, 1e5, -2, true);

	run_single_event(argv[1], 100, false);
	// run_single_shower(argv[1]);

	/* code */
	return 0;
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/ext/std/integer_sequence.hpp>
#include <boost/hana/unpack.hpp>

#include <tuple>
#include <utility>
namespace hana = boost::hana;

auto add = [](auto a, auto b, auto c) { return a + b + c; };

int main() {
    std::tuple<int, long, double> tuple{1, 2l, 3.3};

    auto sum = hana::unpack(std::integer_sequence<int, 0, 1, 2>{}, [&](auto ...i) {
        // the `i`s are `std::integral_constant<int, ...>`s
        return add(std::get<decltype(i)::value>(tuple)...);
    });

    BOOST_HANA_RUNTIME_CHECK(sum == 6.3);
}


// Copyright (C) 2016 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/undistort_image.h"

#include <Eigen/Core>
#include <algorithm>
#include <limits>

#include "theia/image/image.h"
#include "theia/sfm/camera/camera.h"
#include "theia/sfm/camera/camera_intrinsics_model.h"
#include "theia/sfm/camera/fisheye_camera_model.h"
#include "theia/sfm/camera/pinhole_camera_model.h"
#include "theia/sfm/camera/pinhole_radial_tangential_camera_model.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/track.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view.h"

namespace theia {
namespace {

// Set all radial distortion parameters to zero. This requires adjusting
// different parameters for each camera intrinsics model type.
void SetLensDistortionToZero(Camera* camera) {
  double* intrinsics = camera->mutable_intrinsics();
  switch (camera->GetCameraIntrinsicsModelType()) {
    case CameraIntrinsicsModelType::PINHOLE:
      intrinsics[PinholeCameraModel::RADIAL_DISTORTION_1] = 0.0;
      intrinsics[PinholeCameraModel::RADIAL_DISTORTION_2] = 0.0;
      break;
    case CameraIntrinsicsModelType::PINHOLE_RADIAL_TANGENTIAL:
      intrinsics[PinholeRadialTangentialCameraModel::RADIAL_DISTORTION_1] = 0.0;
      intrinsics[PinholeRadialTangentialCameraModel::RADIAL_DISTORTION_2] = 0.0;
      intrinsics[PinholeRadialTangentialCameraModel::RADIAL_DISTORTION_3] = 0.0;
      intrinsics[PinholeRadialTangentialCameraModel::TANGENTIAL_DISTORTION_1] =
          0.0;
      intrinsics[PinholeRadialTangentialCameraModel::TANGENTIAL_DISTORTION_2] =
          0.0;
      break;
    case CameraIntrinsicsModelType::FISHEYE:
      intrinsics[FisheyeCameraModel::RADIAL_DISTORTION_1] = 0.0;
      intrinsics[FisheyeCameraModel::RADIAL_DISTORTION_2] = 0.0;
      intrinsics[FisheyeCameraModel::RADIAL_DISTORTION_3] = 0.0;
      intrinsics[FisheyeCameraModel::RADIAL_DISTORTION_4] = 0.0;
      break;
    case CameraIntrinsicsModelType::FOV:
      intrinsics[FisheyeCameraModel::RADIAL_DISTORTION_1] = 0.0;
      break;
    default:
      LOG(FATAL) << "Invalid camera intrinsics type.";
      break;
  }
}

// We seek a mapping from undistorted pixels to distorted pixels such that a
// valid mapping exists for all undistorted pixels. This is effectively
// undistorting the image then cropping it so that no unmapped (black) pixels
// are in the undistorted image. To do this, we need to find the min and max x/y
// values of unmapped pixels so that we can determine the size and location of
// the crop.
void FindUndistortedImageBoundary(const Camera& distorted_camera,
                                  const Camera& undistorted_camera,
                                  Eigen::Vector4d* bounds) {
  const CameraIntrinsicsModel& distorted_intrinsics =
      *distorted_camera.CameraIntrinsics();
  const CameraIntrinsicsModel& undistorted_intrinsics =
      *undistorted_camera.CameraIntrinsics();

  // Find the max and min locations of the undistorted pixels.
  double left_max_x = std::numeric_limits<double>::lowest();
  double right_min_x = std::numeric_limits<double>::max();
  for (size_t y = 0; y < distorted_camera.ImageHeight(); ++y) {
    // Left border.
    const Eigen::Vector3d distorted_point1 =
        distorted_intrinsics.ImageToCameraCoordinates(
            Eigen::Vector2d(0.5, y + 0.0));
    const Eigen::Vector2d undistorted_point1 =
        undistorted_intrinsics.CameraToImageCoordinates(distorted_point1);
    left_max_x = std::max(left_max_x, undistorted_point1(0));

    // Right border.
    const Eigen::Vector3d distorted_point2 =
        distorted_intrinsics.ImageToCameraCoordinates(
            Eigen::Vector2d(distorted_camera.ImageWidth() - 0.5, y + 0.5));
    const Eigen::Vector2d undistorted_point2 =
        undistorted_intrinsics.CameraToImageCoordinates(distorted_point2);
    right_min_x = std::min(right_min_x, undistorted_point2(0));
  }

  // Determine min, max coordinates along left / right image border.
  double top_max_y = std::numeric_limits<double>::lowest();
  double bottom_min_y = std::numeric_limits<double>::max();
  for (size_t x = 0; x < distorted_camera.ImageWidth(); ++x) {
    // Top border.
    const Eigen::Vector3d distorted_point1 =
        distorted_intrinsics.ImageToCameraCoordinates(
            Eigen::Vector2d(x + 0.5, 0.5));
    const Eigen::Vector2d undistorted_point1 =
        undistorted_intrinsics.CameraToImageCoordinates(distorted_point1);
    top_max_y = std::max(top_max_y, undistorted_point1(1));

    // Bottom border.
    const Eigen::Vector3d distorted_point2 =
        distorted_intrinsics.ImageToCameraCoordinates(
            Eigen::Vector2d(x + 0.5, distorted_camera.ImageHeight() - 0.5));
    const Eigen::Vector2d undistorted_point2 =
        undistorted_intrinsics.CameraToImageCoordinates(distorted_point2);
    bottom_min_y = std::min(bottom_min_y, undistorted_point2(1));
  }

  *bounds = Eigen::Vector4d(left_max_x, right_min_x, top_max_y, bottom_min_y);
}

// Create an undistorted image from the distorted image given the distorted and
// undistorted camera parameters. This function only maps the pixels to create
// the undistorted image and assumes the camera parameters and image sizes have
// already been solved for.
void RemoveImageLensDistortion(const Camera& distorted_camera,
                               const FloatImage& distorted_image,
                               const Camera& undistorted_camera,
                               FloatImage* undistorted_image) {
  const CameraIntrinsicsModel& distorted_intrinsics =
      *distorted_camera.CameraIntrinsics();
  const CameraIntrinsicsModel& undistorted_intrinsics =
      *undistorted_camera.CameraIntrinsics();

  // For each pixel in the undistorted image, find the coordinate in the
  // distorted image and set the pixel color accordingly.
  const int num_channels = distorted_image.Channels();
  OpenImageIO::ImageBuf& undistorted_img =
      undistorted_image->GetOpenImageIOImageBuf();
  OpenImageIO::ImageBuf::Iterator<float> undistorted_it(undistorted_img);
  for (; !undistorted_it.done(); ++undistorted_it) {
    Eigen::Vector2d image_point(undistorted_it.x() + 0.5,
                                undistorted_it.y() + 0.5);

    // Camera models assume that the upper left pixel center is (0.5, 0.5).
    const Eigen::Vector3d distorted_point =
        undistorted_intrinsics.ImageToCameraCoordinates(image_point);
    const Eigen::Vector2d distorted_pixel =
        distorted_intrinsics.CameraToImageCoordinates(distorted_point);
    const Eigen::Vector2d pixel(std::round<int>(distorted_pixel.x() - 0.5),
                                std::round<int>(distorted_pixel.y() - 0.5));
    // Set all color channels appropriately. Note that we do not need to check
    // if the distorted pixel is within the image bounds since the
    // FindUndistortedImageBoundary function has guaranteed that all pixels will
    // be within the image borders.
    for (int c = 0; c < num_channels; c++) {
      undistorted_it[c] = distorted_image.BilinearInterpolate(
          distorted_pixel.x(), distorted_pixel.y(), c);
    }
  }
}

}  // namespace

bool UndistortImage(const Camera& distorted_camera,
                    const FloatImage& distorted_image,
                    const Camera& undistorted_camera,
                    FloatImage* undistorted_image) {
  // Undistort the image.
  if (distorted_image.Channels() == 1) {
    undistorted_image->ConvertToGrayscaleImage();
  } else {
    undistorted_image->ConvertToRGBImage();
  }
  undistorted_image->Resize(undistorted_camera.ImageWidth(),
                            undistorted_camera.ImageHeight());

  // Remap the distorted pixels into the undistorted image.
  RemoveImageLensDistortion(distorted_camera,
                            distorted_image,
                            undistorted_camera,
                            undistorted_image);

  return true;
}

// Create the undistorted camera by removing radial distortion parameters.
bool UndistortCamera(const Camera& distorted_camera,
                     Camera* undistorted_camera) {
  *undistorted_camera = distorted_camera;
  SetLensDistortionToZero(undistorted_camera);

  Eigen::Vector4d undistorted_image_boundaries;
  FindUndistortedImageBoundary(distorted_camera,
                               *undistorted_camera,
                               &undistorted_image_boundaries);

  // Given the locations of the min/max undistorted pixels, compute the scale
  // factor to resize the undistorted image.
  const double cx = undistorted_camera->PrincipalPointX();
  const double cy = undistorted_camera->PrincipalPointY();
  const double left_max_x = undistorted_image_boundaries(0);
  const double right_min_x = undistorted_image_boundaries(1);
  const double top_max_y = undistorted_image_boundaries(2);
  const double bottom_min_y = undistorted_image_boundaries(3);

  // Scale undistorted camera dimensions.
  const double scale_x =
      1.0 /
      std::max(cx / (cx - left_max_x),
               (distorted_camera.ImageWidth() - 0.5 - cx) / (right_min_x - cx));
  const double scale_y =
      1.0 / std::max(cy / (cy - top_max_y),
                     (distorted_camera.ImageHeight() - 0.5 - cy) /
                         (bottom_min_y - cy));

  undistorted_camera->SetImageSize(
      static_cast<int>(
          std::max(1.0, scale_x * undistorted_camera->ImageWidth())),
      static_cast<int>(
          std::max(1.0, scale_y * undistorted_camera->ImageHeight())));

  // Scale the principal point according to the new dimensions of the image.
  undistorted_camera->SetPrincipalPoint(
      undistorted_camera->PrincipalPointX() *
          static_cast<double>(undistorted_camera->ImageWidth()) /
          distorted_camera.ImageWidth(),
      undistorted_camera->PrincipalPointY() *
          static_cast<double>(undistorted_camera->ImageHeight()) /
          distorted_camera.ImageHeight());

  return true;
}

bool UndistortReconstruction(Reconstruction* reconstruction) {
  const auto view_ids = reconstruction->ViewIds();
  for (const ViewId view_id : view_ids) {
    View* view = reconstruction->MutableView(view_id);
    if (view == nullptr || !view->IsEstimated()) {
      continue;
    }

    // Undistort the image to obtain the undistorted camera.
    const Camera distorted_camera = view->Camera();
    Camera* undistorted_camera = view->MutableCamera();
    if (!UndistortCamera(distorted_camera, undistorted_camera)) {
      return false;
    }

    // The camera intrinsics models describe how to distort and undistort the
    // pixels.
    const CameraIntrinsicsModel& distorted_intrinsics =
        *distorted_camera.CameraIntrinsics();
    const CameraIntrinsicsModel& undistorted_intrinsics =
        *undistorted_camera->CameraIntrinsics();

    // Undisort all features seen by the view.
    const auto track_ids = view->TrackIds();
    for (const TrackId track_id : track_ids) {
      Track* track = reconstruction->MutableTrack(track_id);
      if (track == nullptr || !track->IsEstimated()) {
        continue;
      }

      // Get the undistorted feature.
      const Feature* distorted_feature = view->GetFeature(track_id);
      const Eigen::Vector3d distorted_point =
          distorted_intrinsics.ImageToCameraCoordinates(*distorted_feature);
      const Eigen::Vector2d undistorted_feature =
          undistorted_intrinsics.CameraToImageCoordinates(distorted_point);

      // Add the new undistorted feature to the undistorted view. First, remove
      // the feature since it is not mutable.
      CHECK(view->RemoveFeature(track_id));

      // Only add the feature back if it is within the new image
      // boundaries. Otherwise, remove the feature from the view and the track.
      if (undistorted_feature.x() < 0 ||
          undistorted_feature.x() > undistorted_camera->ImageWidth() - 1 ||
          undistorted_feature.y() < 0 ||
          undistorted_feature.y() > undistorted_camera->ImageHeight() - 1) {
        track->RemoveView(view_id);
        // Remove the track if there are no more observations.
        if (track->NumViews() == 0) {
          reconstruction->RemoveTrack(track_id);
        }
      } else {
        view->AddFeature(track_id, undistorted_feature);
      }
    }
  }

  return true;
}

}  // namespace theia


#include "PiecewisePolynomial.h"
#include <Eigen/Core>
#include <random>
#include <vector>
#include "testUtil.h"
#include <iostream>

using namespace std;
using namespace Eigen;

default_random_engine generator;
uniform_real_distribution<double> uniform;

template <typename CoefficientType>
void testIntegralAndDerivative() {
  int num_coefficients = 5;
  int num_segments = 3;
  int rows = 3;
  int cols = 5;

  typedef PiecewisePolynomial<CoefficientType> PiecewisePolynomialType;
  typedef typename PiecewisePolynomialType::CoefficientMatrix CoefficientMatrix;

  vector<double> segment_times = PiecewiseFunction::randomSegmentTimes(num_segments, generator);
  PiecewisePolynomialType piecewise = PiecewisePolynomial<CoefficientType>::random(rows, cols, num_coefficients, segment_times);

  // differentiate integral, get original back
  PiecewisePolynomialType piecewise_back = piecewise.integral().derivative();
  if (!piecewise.isApprox(piecewise_back, 1e-10))
    throw runtime_error("wrong");

  // check value at start time
  CoefficientMatrix desired_value_at_t0 = PiecewisePolynomialType::CoefficientMatrix::Random(piecewise.rows(), piecewise.cols());
  PiecewisePolynomialType integral = piecewise.integral(desired_value_at_t0);
  auto value_at_t0 = integral.value(piecewise.getStartTime());
  valuecheckMatrix(desired_value_at_t0, value_at_t0, 1e-10);

  // check continuity at knot points
  for (int i = 0; i < piecewise.getNumberOfSegments() - 1; ++i) {
    valuecheck(integral.getPolynomial(i).value(integral.getDuration(i)), integral.getPolynomial(i + 1).value(0.0));
  }
}

template <typename CoefficientType>
void testBasicFunctionality() {
  int max_num_coefficients = 6;
  int num_tests = 100;
  default_random_engine generator;
  uniform_int_distribution<> int_distribution(1, max_num_coefficients);

  typedef PiecewisePolynomial<CoefficientType> PiecewisePolynomialType;
  typedef typename PiecewisePolynomialType::CoefficientMatrix CoefficientMatrix;

  for (int i = 0; i < num_tests; ++i) {
    int num_coefficients = int_distribution(generator);
    int num_segments = int_distribution(generator);
    int rows = int_distribution(generator);
    int cols = int_distribution(generator);

    vector<double> segment_times = PiecewiseFunction::randomSegmentTimes(num_segments, generator);
    PiecewisePolynomialType piecewise1 = PiecewisePolynomial<CoefficientType>::random(rows, cols, num_coefficients, segment_times);
    PiecewisePolynomialType piecewise2 = PiecewisePolynomial<CoefficientType>::random(rows, cols, num_coefficients, segment_times);

    normal_distribution<double> normal;
    double shift = normal(generator);
    CoefficientMatrix offset = CoefficientMatrix::Random(piecewise1.rows(), piecewise1.cols());

    PiecewisePolynomialType sum = piecewise1 + piecewise2;
    PiecewisePolynomialType difference = piecewise2 - piecewise1;
    PiecewisePolynomialType piecewise1_plus_offset = piecewise1 + offset;
    PiecewisePolynomialType piecewise1_minus_offset = piecewise1 - offset;
    PiecewisePolynomialType piecewise1_shifted = piecewise1;
    piecewise1_shifted.shiftRight(shift);

    uniform_real_distribution<double> uniform(piecewise1.getStartTime(), piecewise1.getEndTime());
    double t = uniform(generator);

    valuecheckMatrix(sum.value(t), piecewise1.value(t) + piecewise2.value(t), 1e-8);
    valuecheckMatrix(difference.value(t), piecewise2.value(t) - piecewise1.value(t), 1e-8);
    valuecheckMatrix(piecewise1_plus_offset.value(t), piecewise1.value(t) + offset, 1e-8);
    valuecheckMatrix(piecewise1_minus_offset.value(t), piecewise1.value(t) - offset, 1e-8);
    valuecheckMatrix(piecewise1_shifted.value(t), piecewise1.value(t - shift), 1e-8);
  }
}

template <typename CoefficientType>
void testValueOutsideOfRange() {
  typedef PiecewisePolynomial<CoefficientType> PiecewisePolynomialType;

  default_random_engine generator;
  vector<double> segment_times = PiecewiseFunction::randomSegmentTimes(6, generator);
  PiecewisePolynomialType piecewise = PiecewisePolynomial<CoefficientType>::random(3, 4, 5, segment_times);
  valuecheckMatrix(piecewise.value(piecewise.getStartTime()), piecewise.value(piecewise.getStartTime() - 1.0), 1e-10);
  valuecheckMatrix(piecewise.value(piecewise.getEndTime()), piecewise.value(piecewise.getEndTime() + 1.0), 1e-10);
}

int main(int argc, char **argv) {
  testIntegralAndDerivative<double>();
  testBasicFunctionality<double>();
  testValueOutsideOfRange<double>();

  std::cout << "test passed";

  return 0;
}


#include <iostream>
#include <vector>
#include <unistd.h>

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

using namespace std;

long bw(long w)
{
    long k= 0;
    //for (long long i0= 0; i0 < w; i0++)
      for (long long i= 0; i < w; i++)
	for (long j= 0; j < 100000000; j++)
	    k+= j;
      std::cout << k << endl;
      return k;
}

int main()
{
    const int s= 1000000, n= 4, z= 32, t= 15;
    long w= 100, r;

#if 0
    std::vector<double> v(10000000);
    std::cout << "Vector 10M created. Capacity == " << v.capacity() << endl;
    r= bw(w);

    v.resize(20000000);
    std::cout << "Vector auf 20M vergroessert. Capacity == " << v.capacity() << endl;
    r= bw(w);

    v.resize(5000000);
    std::cout << "Vector auf 5M verkleinert. Capacity == " << v.capacity() << endl;
    r= bw(w);

    {
	std::vector<double> w(v.begin(), v.end());
	swap(v, w);
    }
    std::cout << "Mit 5M geswappt. Capacity == " << v.capacity() << endl;
    r= bw(w);
#endif

    mtl::compressed2D<double> A(s, s);
    std::cout << "Matrix created." << endl;
    r= bw(w);

    {
	mtl::mat::inserter<mtl::compressed2D<double> > ins(A, z);
	std::cout << "Inserter created." << endl;
	r= bw(w);

	std::cout << "start insertion." << endl;
	for (int i= n; i < s; i++)
	    for (int j= i - n; j < i; j++)
		ins[i][j] << 9.0;
	std::cout << "Matrix filled." << endl;
	r= bw(w);
    }
    std::cout << "Instructor destroyed." << endl;
    r= bw(w);

    A.shrink();
    std::cout << "After A.shrink()." << endl;
    r= bw(w);

    return 0;
}


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

  @copyright 2017 NumScale SAS

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

#include <boost/simd/algorithm/transform.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/pack.hpp>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Replaces all elements that are equal to old_value with new_value in the range [first, last).

    @param first      Beginning of the range of elements
    @param last       End of the range of elements
    @param old_val    Value to be replaced
    @param new_val    Value to replace with

    @par Requirement

      - @c first, @c last and @c out must be pointer to Vectorizable type.

      - @c new_val and old_val must be a scalar values convertible to  pointee type of first.

    @par Example:

       @snippet replace.cpp replace

    @par possible output:

       @snippet replace.txt replace
  **/
  template<typename T>
  void replace(T * first, T * last, T const & old_val, T const & new_val)
  {
    struct local
    {
      using p_t = boost::simd::pack<T>;

      local(const T & ov, const T & nv) : nv_(nv), ov_(ov), pnv_(nv), pov_(ov) {}

      T   operator()(T    const& x) const { return x == ov_ ? nv_ : x;          }
      p_t operator()(p_t  const& x) const { return if_else(x == pov_, pnv_, x); }

      T nv_, ov_;
      p_t pnv_, pov_;
    };

    boost::simd::transform(first, last, first, local(old_val, new_val));
  }
} }

#endif


#define BOOST_TEST_MODULE MyTest
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include "pose.h"
#include "Criteria/criteriaName.h"
#include "Criteria/weightmatrix.h"
#include "Criteria/sensingtimecriterion.h"
#include "Criteria/traveldistancecriterion.h"
#include "Criteria/informationgaincriterion.h"
#include <cmath>
#include <iostream>

using namespace std;

BOOST_AUTO_TEST_CASE( test_weightmatrix )
{
    //create a weightmatrix for 3 criteria
    WeightMatrix matrix(3);
    matrix.insertSingleCriterion(INFORMATION_GAIN,0.5,true);
    matrix.insertSingleCriterion(TRAVEL_DISTANCE,0.2, true);
    matrix.insertSingleCriterion(SENSING_TIME,0.3, true);
    string str1(INFORMATION_GAIN);
    string str2(TRAVEL_DISTANCE);
    string str3(SENSING_TIME);
    list<string> list1 ;
    list<string> list2 ;
    list<string> list3 ;
    list<string> list4;
    list<string> listCasual;
    list1.push_back(str1);
    list1.push_back(str2);
    list2.push_back(str1);
    list2.push_back(str3);
    list3.push_back(str2);
    list3.push_back(str3);
    list4.push_back(str1);
    list4.push_back(str2);
    list4.push_back(str3);
    listCasual.push_back(str2);
    listCasual.push_back(str1);
    matrix.insertCombinationWeight(list1,0.8);
    matrix.insertCombinationWeight(list2,0.6);
    matrix.insertCombinationWeight(list3,0.6);
    matrix.insertCombinationWeight(list4,1);

    cout << "1) check the encoding of single criteria" << endl;
    string encInforGain = matrix.getNameEncoding(INFORMATION_GAIN);
    string encTravDist = matrix.getNameEncoding(TRAVEL_DISTANCE);
    string encSensTime = matrix.getNameEncoding(SENSING_TIME);
    BOOST_CHECK_EQUAL(encInforGain,"A");
    BOOST_CHECK_EQUAL(encTravDist,"B");
    BOOST_CHECK_EQUAL(encSensTime,"C");

    cout << "2) check the encoding of combined criteria " << endl;
    string encList1 = matrix.computeNamesEncoding(list1);
    string encList2 = matrix.computeNamesEncoding(list2);
    string encList3 = matrix.computeNamesEncoding(list3);
    string encList4 = matrix.computeNamesEncoding(list4);
    BOOST_CHECK_EQUAL(encList1,"AB");
    BOOST_CHECK_EQUAL(encList2,"AC");
    BOOST_CHECK_EQUAL(encList3, "BC");
    BOOST_CHECK_EQUAL(encList4, "ABC");

    cout << "3) check the encoding of combined criteria in casual order" << endl;
    string encListCasual = matrix.computeNamesEncoding(listCasual);
    BOOST_CHECK_EQUAL(encListCasual,"AB");

    cout << "4) chech the activation of criteria before and after a changing state" << endl;
    vector<string> activeCriteria = matrix.getActiveCriteria();

    for(int it = 0; it < activeCriteria.size(); it++){
	if(it ==0){
	    string tmp = activeCriteria.at(it);
	    //cout << tmp << endl;
	    BOOST_CHECK_EQUAL(tmp, "informationGain");
	}else if (it == 1){
	    string tmp = activeCriteria.at(it);
	    //cout << tmp << endl;
	    BOOST_CHECK_EQUAL(tmp, "travelDistance");
	}else {
	    string tmp = activeCriteria.at(it);
	    //cout << tmp << endl;
	    BOOST_CHECK_EQUAL(tmp, "sensingTime");
	}
    }

    int sizeActiveCriteria = matrix.getNumOfActiveCriteria();
    BOOST_CHECK_EQUAL(sizeActiveCriteria,3);
    matrix.changeCriteriaActivation(str1,false);
    sizeActiveCriteria = matrix.getNumOfActiveCriteria();
    BOOST_CHECK_EQUAL(sizeActiveCriteria,2);

    cout << "5) check the insertion of criteria" << endl;
    list<string> knownCriteria = matrix.getKnownCriteria();
    BOOST_CHECK_EQUAL(knownCriteria.size(),3);
    list<string>::iterator it = knownCriteria.begin();

    cout << "6) check the correct insertion of weights" << endl;
    double weightTravel = matrix.getWeight(encTravDist);
    double weightInfo = matrix.getWeight(encInforGain);
    double weightSensing = matrix.getWeight(encSensTime);
    double weightInfoTravel = matrix.getWeight(list1);
    cout << "weightInfoTravel = " << weightInfoTravel << endl;
    double weightInfoSensing = matrix.getWeight(list2);
    cout << "weightInfoSensing = " << weightInfoSensing << endl;
    double weightTravelSensing = matrix.getWeight(list3);
    cout << "weightTravelSensing = " << weightTravelSensing << endl;
    double weightAll = matrix.getWeight(list4);
    cout << "weightAll = " << weightAll << endl;

    BOOST_CHECK_EQUAL(weightInfo,0.5);
    BOOST_CHECK_EQUAL(weightTravel,0.2);
    BOOST_CHECK_EQUAL(weightSensing, 0.3);
    BOOST_CHECK_EQUAL(weightInfoTravel,0.8);
    BOOST_CHECK_EQUAL(weightInfoSensing,0.6);
    BOOST_CHECK_EQUAL(weightTravelSensing, 0.6);
    BOOST_CHECK_EQUAL(weightAll, 1);

}

//EVERYTHING WORK LIKE A CHARM


#if COMPILATION_INSTRUCTIONS
(echo "#include<"$0">" > $0x.cpp) && time clang++ -O3 -std=c++1z -Wall `#-Wfatal-errors` -I.. -D_TEST_SPARSE_COO_MATRIX $0x.cpp -lstdc++fs -lboost_system -lboost_timer -o $0x.x && time $0x.x $@ && rm -f $0x.cpp; exit
#endif

////////////////////////////////////////////////////////////////////////////////
// File developed by:
// Alfredo Correa, correaa@llnl.gov
//    Lawrence Livermore National Laboratory
//
// File created by:
// Alfredo Correa, correaa@llnl.gov
//    Lawrence Livermore National Laboratory
////////////////////////////////////////////////////////////////////////////////

#ifndef SPARSE_COO_MATRIX_HPP
#define SPARSE_COO_MATRIX_HPP

#include<array>
#include<cassert>
#include<cstddef>  // ptrdiff_t
#include<vector>
#include<tuple>

namespace ma{
namespace sparse{

using size_type           = std::size_t;

template<class T, class index, class Alloc = std::allocator<T>>
class coo_matrix{
	protected:
	using alloc_ts = std::allocator_traits<Alloc>;
	using index_allocator = typename alloc_ts::template rebind_alloc<index>;
	std::vector<index, index_allocator> is_;
	std::vector<index, index_allocator> js_;
	std::vector<T, Alloc> vs_;
	size_type cols_;
	size_type rows_;
	public:
	using element = T;
	coo_matrix(coo_matrix const&) = default;
	coo_matrix(coo_matrix&&     ) = default;
	bool operator==(coo_matrix const&) const = delete;
	coo_matrix(
		std::tuple<index, index> const& arr = std::tuple<index,index>{0,0},
		std::initializer_list<std::pair<std::tuple<index, index>, element>> il = {}
	) :
		cols_(std::get<0>(arr)),
		rows_(std::get<1>(arr))
	{
		for(auto e: il) emplace(e.first, e.second);
	}
	void reserve(size_type s){
		is_.reserve(s);
		js_.reserve(s);
		vs_.reserve(s);
	}
	auto size() const{return rows_;}
	auto num_elements() const{return size()*cols_;}
	std::array<index, 2> shape() const{return {{size(), cols_}};}
	auto num_non_zero_elements() const{return vs_.size();}
	T* non_zero_values_data(){return vs_.data();}
	index* non_zero_indices1_data(){return (index*)is_.data();}
	index* non_zero_indices2_data() const{return (index*)js_.data();}
	decltype(auto) move_non_zero_values()&&{return std::move(vs_);}
	decltype(auto) move_non_zero_indices1()&&{return std::move(is_);}
	decltype(auto) move_non_zero_indices2()&&{return std::move(js_);}
	void clear(){is_.clear(); js_.clear(); vs_.clear(); cols_ = 0; rows_ = 0;}
	template<class Pair = std::array<index, 2>, class TT>
	void emplace(Pair&& indices, TT&& tt){
		using std::get;
		is_.emplace_back(get<0>(std::forward<Pair>(indices)));
		js_.emplace_back(get<1>(std::forward<Pair>(indices)));
		vs_.emplace_back(std::forward<TT>(tt));
	}
	protected:
	struct row_reference{
		coo_matrix& self_;
		index i_;
		struct element_reference{
			row_reference& self_;
			index j_;
			template<class TT>
			element_reference&&
			operator=(TT&& tt)&&{
				self_.self_.emplace({{self_.i_, j_}}, std::forward<TT>(tt));
				return std::move(*this);
			}
		};
		using reference = element_reference;
		reference operator[](index i)&&{return reference{*this, i};}
	};
	public:
	using reference = row_reference;
	reference operator[](index i){return reference{*this, i};}
	friend decltype(auto) size(coo_matrix const& s){return s.size();}
	friend decltype(auto) shape(coo_matrix const& s){return s.shape();}
	friend decltype(auto) clear(coo_matrix& s){s.clear();}
};
/*
template<class... Ts>
std::array<index, 2> index_bases(coo_matrix<Ts...> const&){return {{0,0}};}
template<class... Ts>
auto num_non_zero_elements(coo_matrix<Ts...> const& s){
	return s.num_non_zero_elements();
}
template<class... Ts>
auto non_zero_values_data(coo_matrix<Ts...>& s){return s.non_zero_values_data();}
template<class... Ts>
auto non_zero_indices1_data(coo_matrix<Ts...>& s){
	return s.non_zero_indices1_data();
}
template<class... Ts>
auto non_zero_indices2_data(coo_matrix<Ts...>& s){
	return s.non_zero_indices2_data();
}
*/
}
}

#ifdef _TEST_SPARSE_COO_MATRIX

#include "iterator/zipper.hpp"
#include "timer/timed.hpp"

#include <boost/timer/timer.hpp>

#include<algorithm> // std::sort
#include<cassert>
#include<iostream>
#include<random>

using std::cout;
using std::cerr;
using std::get;

int main(){

	using ma::sparse::coo_matrix;

	auto const M = 40000;
	auto const N = 40000;
	double const sparsity = 0.01;

	// generate tuples for reference
	auto const csource = [&](){
		std::vector<std::tuple<int, int, double>> source;
		source.reserve(M*N*sparsity);
		std::default_random_engine gen;
		std::uniform_real_distribution<double> dist;
		std::uniform_real_distribution<double> dist2(0, 10);
		for(auto i = M; i != 0; --i)
			for(auto j = N; j != 0; --j)
				if(dist(gen) < sparsity) source.emplace_back(i - 1, j - 1, dist2(gen));
		return source;
	}();

	cerr << "value size " << csource.size()*sizeof(double)/1000000 << " MB\n";
	{
		coo_matrix<double> coom({M, N});
		{
			boost::timer::auto_cpu_timer t("ugly syntax: %t seconds\n");
			for(auto& s : csource)
				coom.emplace({{get<0>(s), get<1>(s)}}, get<2>(s));
		}
	}
	{
		coo_matrix<double> coom({M, N});
		{
			boost::timer::auto_cpu_timer t("generic syntax: %t seconds\n");
			for(auto& s : csource)
				coom[get<0>(s)][get<1>(s)] = get<2>(s);
		}
	}

	coo_matrix<double> small(
		{4,4},
		{
			{{3,3}, 1},
			{{2,1}, 3},
			{{0,1}, 9}
		}
	);

}

#endif
#endif


// Copyright 2017 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/list.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>

int main()
{
    using boost::mp11::mp_list_c;
    using boost::mp11::mp_list;
    using boost::mp11::mp_int;

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_list_c<int>, mp_list<>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_list_c<int, 1>, mp_list<mp_int<1>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_list_c<int, 1, 3>, mp_list<mp_int<1>, mp_int<3>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_list_c<int, 1, 3, 5>, mp_list<mp_int<1>, mp_int<3>, mp_int<5>>>));

    return boost::report_errors();
}


/* Copyright (c) 2020 vargaconsulting, Toronto,ON Canada
 * Author: Varga, Steven <steven@vargaconsulting.ca>
 */

#include <cstdint>
#include <iostream>
#include <limits>
#include <random>
#include <algorithm>
#include <armadillo>
#include <Eigen/Dense>
#include <h5cpp/core> // include this before custom type definition
	#include "n-bit.hpp"
#include <h5cpp/io> // IO operators become aware of your custom type

namespace ei {
	template <class T>
	using Matrix   = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
}

// in all cases when filtering used chunk must be set as well, no contiguous
int main(){
	namespace bs = bitstring;

	h5::fd_t fd = h5::create("example.h5",H5F_ACC_TRUNC);
	// prints out type info, eases on debugging
	std::cout << h5::dt_t<bs::n_bit>() << std::endl;

	std::random_device rd;
    std::mt19937 random_int(rd());
    std::uniform_int_distribution<> sample(0, 3);

	{
	// method 1: use C++ conversion CTOR to convert from fundamental type to your custom one
	// notice the armadillo can only hold arithmetic types, you have to do some forcing
	// also that colmajor, coordinates are swapped
		arma::Mat<unsigned char> M(12,8);
		std::generate(M.begin(), M.end(), [&](){ return sample(random_int);} );

		h5::ds_t ds = h5::create<bs::n_bit>(fd, "arma",
			   h5::current_dims{8,12,1}, h5::max_dims{8,12,H5S_UNLIMITED}, h5::chunk{4,3,1} | h5::nbit);
		// force conversion for zero copy:
		h5::write<bs::n_bit>(fd,"arma", (bs::n_bit*)M.memptr(), h5::offset{0,0,0}, h5::count{8,12,1});

		arma::Mat<unsigned char> data(12,8);
	   	h5::read<bs::n_bit>(fd, "arma", (bs::n_bit*)data.memptr(), h5::offset{0,0,0}, h5::count{8,12,1});
		data.print();
		std::cout <<"\n\n";
		M.print();
	}
	{
	// method 2: eigen allows native type handling
		ei::Matrix<bs::n_bit> M(12,8);
		for(int i=0; i<12; i++) for( int j=0; j<8; j++)
	   		M(i,j) = static_cast<bs::n_bit>( sample(random_int));

		h5::ds_t ds = h5::create<bs::n_bit>(fd, "eigen", // chunk must be used with nbit
			   h5::current_dims{12,8}, h5::max_dims{12,H5S_UNLIMITED}, h5::chunk{3,4} | h5::nbit);
		h5::write(ds, M, h5::chunk{3,4} | h5::nbit);

		ei::Matrix<bs::n_bit> data(12,8);
	  	h5::read(fd, "eigen", data, h5::offset{0,0});
		std::cout << data << std::endl <<std::endl << M <<std::endl;
	}
	{ //method 3, use STL
		std::vector<bs::n_bit> V(12*8);
		std::generate(V.begin(), V.end(), [&](){
			return static_cast<bs::n_bit>(sample(random_int));
		});

		h5::ds_t ds = h5::create<bs::n_bit>(fd, "stl", // chunk must be used with nbit
			   h5::current_dims{12,8}, h5::max_dims{12,H5S_UNLIMITED}, h5::chunk{3,4} | h5::nbit);
		// from typed memory pointer to different shape
		h5::write<bs::n_bit>(ds, V.data(), h5::count{12,8}); // single shot write

		auto data = h5::read<std::vector<bs::n_bit>>(fd, "stl");
		for( int i=0; i<V.size(); i++ )
			std::cout << static_cast<unsigned int>( data[i] ) << " ";
		std::cout << "\n\ncomputing difference ||saved - read|| expecting norm to be zero:\n";
		for( int i=0; i<V.size(); i++ )
			std::cout << abs(V[i].value - data[i].value) <<" ";
	}

}


/*
 * eigen_numpy_list.cpp
 *
 *  Created on: Mar 19, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 */

//standard
#include <vector>

//libraries
#include <boost/python.hpp>
#include <Eigen/Eigen>
#include <Python.h>

#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <numpy/arrayobject.h>

//local
#include "eigen_numpy.hpp"
#include "numpy_conversions_shared.hpp"
#include "../math/typedefs.hpp"

// These macros were renamed in NumPy 1.7.1.
#if !defined(NPY_ARRAY_C_CONTIGUOUS) && defined(NPY_C_CONTIGUOUS)
#define NPY_ARRAY_C_CONTIGUOUS NPY_C_CONTIGUOUS
#endif

#if !defined(NPY_ARRAY_ALIGNED) && defined(NPY_ALIGNED)
#define NPY_ARRAY_ALIGNED NPY_ALIGNED
#endif

namespace bp = boost::python;

using namespace Eigen;

template<class MatType> // MatrixXf or MatrixXd
struct EigenMatrixVectorToPython {
	static PyObject* convert(const std::vector<MatType>& vec) {
		PyObject* py_list = PyList_New(vec.size());
		Py_ssize_t index = 0;
		for (MatType mat : vec) {
			npy_intp shape[2] = { mat.rows(), mat.cols() };
			PyArrayObject* python_array = (PyArrayObject*) PyArray_SimpleNew(
					2, shape, NumpyEquivalentType<typename MatType::Scalar>::type_code);
			copy_array(mat.data(),
					(typename MatType::Scalar*) PyArray_DATA(python_array),
					mat.rows(),
					mat.cols(),
					false,
					true,
					MatType::Flags & Eigen::RowMajorBit);
			PyObject* py_mat = (PyObject*) python_array;
			PyList_SET_ITEM(py_list, index, py_mat);
			index += 1;
		}
		return py_list;
	}
};

template<> // MatrixXf or MatrixXd
struct EigenMatrixVectorToPython<math::MatrixXv2f> {
	static PyObject* convert(const std::vector<math::MatrixXv2f>& vec) {
		PyObject* py_list = PyList_New(vec.size());
		Py_ssize_t index = 0;
		for (math::MatrixXv2f mat : vec) {
			npy_intp shape[3] = { mat.rows(), mat.cols(), 2 };
			PyArrayObject* python_array = (PyArrayObject*) PyArray_SimpleNew(
					3, shape, NumpyEquivalentType<math::MatrixXv2f::Scalar>::type_code);
			copy_array(mat.data(),
					(math::MatrixXv2f::Scalar*) PyArray_DATA(python_array),
					mat.rows(),
					mat.cols(),
					false,
					true,
					math::MatrixXv2f::Flags & Eigen::RowMajorBit);
			PyObject* py_mat = (PyObject*) python_array;
			PyList_SET_ITEM(py_list, index, py_mat);
			index += 1;
		}
		return py_list;
	}
};

#define EIGEN_MATRIX_LIST_CONVERTER(Type) \
  bp::to_python_converter<std::vector<Type>, EigenMatrixVectorToPython<Type> >();

#if PY_VERSION_HEX >= 0x03000000
void*
#else
void
#endif
setup_Eigen_list_converters() {
	static bool is_setup = false;
	if (is_setup)
		return NUMPY_IMPORT_ARRAY_RETVAL;
	is_setup = true;
	import_array();

	EIGEN_MATRIX_LIST_CONVERTER(Eigen::MatrixXf);
	EIGEN_MATRIX_LIST_CONVERTER(math::MatrixXv2f);
#if PY_VERSION_HEX >= 0x03000000
	return 0;
#endif
}


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

/*!
 * \ingroup boost_simd_reduction
 * \defgroup boost_simd_reduction_maximum maximum
 *
 * \par Description
 * Returns the greatest element of the SIMD vector
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/maximum.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::maximum_(A0)>::type
 *     maximum(const A0 & a0);
 * }
 * \endcode
 *
 * \param a0 the unique parameter of maximum
 *
 * \return always a scalar value
 *
 * \par Notes
 * \par
 * This is a reduction operation. As such it has not real interest outside
 * SIMD mode.
 * \par
 * Such an operation always has a scalar result which translate a property
 * of the whole SIMD vector.
 * \par
 * If usable and used in scalar mode, it reduces to the operation as acting
 * on a one element vector.
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    struct max_;
    struct Maxinit;

    /*!
     * \brief Define the tag maximum_ of functor maximum
     *        in namespace boost::simd::tag for toolbox boost.simd.reduction
    **/
    struct maximum_ : ext::reduction_<maximum_, tag::max_, tag::Maxinit>
    {
      typedef ext::reduction_<maximum_, tag::max_, tag::Maxinit> parent;
    };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::maximum_, maximum, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::maximum_, maximum, 2)
} }

#endif


#include <boost/math/constants/constants.hpp>


#include <swizzle/parser/detail/ExtractBitValueFromToken.hpp>

#include <swizzle/Exceptions.hpp>
#include <swizzle/lexer/TokenInfo.hpp>
#include <swizzle/types/ReadAs.hpp>

#include <boost/numeric/conversion/cast.hpp>
#include <cstddef>
#include <cstdint>

namespace swizzle { namespace parser { namespace detail {

    std::intmax_t extractBitValueFromToken(const boost::string_view& underlying, const lexer::TokenInfo& info)
    {
        try
        {
            if(underlying == "u8")
            {
                const auto value = boost::numeric_cast<std::intmax_t>(types::readAs<std::uint8_t>(info.token().value()));
                if(value > 7)
                {
                    throw SyntaxError("Bitfield bit value exceeds range supported by underlying type (" + underlying.to_string() + ")", info);
                }

                return value;
            }

            if(underlying == "u16")
            {
                const auto value = boost::numeric_cast<std::intmax_t>(types::readAs<std::uint16_t>(info.token().value()));
                if(value > 15)
                {
                    throw SyntaxError("Bitfield bit value exceeds range supported by underlying type (" + underlying.to_string() + ")", info);
                }

                return value;
            }

            if(underlying == "u32")
            {
                const auto value = boost::numeric_cast<std::intmax_t>(types::readAs<std::uint32_t>(info.token().value()));
                if(value > 31)
                {
                    throw SyntaxError("Bitfield bit value exceeds range supported by underlying type (" + underlying.to_string() + ")", info);
                }

                return value;
            }

            if(underlying == "u64")
            {
                const auto value = boost::numeric_cast<std::intmax_t>(types::readAs<std::uint64_t>(info.token().value()));
                if(value > 63)
                {
                    throw SyntaxError("Bitfield bit value exceeds range supported by underlying type (" + underlying.to_string() + ")", info);
                }

                return value;
            }
        }
        catch(const boost::bad_numeric_cast&)
        {
            throw SyntaxError("Internal parser error, unexpected value overflow.", info);
        }
        catch(const StreamInputCausesOverflow&)
        {
            throw SyntaxError("Bitfield bit value exceeds range supported by underlying type (" + underlying.to_string() + ")", info);
        }
        catch(const StreamInputCausesUnderflow&)
        {
            throw ParserError("Internal parser error, unexpected value underflow.");
        }
        catch(const InvalidStreamInput&)
        {
            throw SyntaxError("Bitfield bit value contains invalid character.", info);
        }

        throw ParserError("Internal parser error, unexpected underlying type found parsing bitfield");
    }

}}}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.

#include "popops/NaN.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/ElementWise.hpp"
#include "popops/codelets.hpp"
#include "poputil/TileMapping.hpp"
#include "poputil/Util.hpp"
#include <boost/multi_array.hpp>
#include <boost/program_options.hpp>
#include <boost/random.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <random>

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_test::util;
using namespace poplibs_support;

static constexpr std::size_t D0 = 6;
static constexpr std::size_t D1 = 5;
static constexpr std::size_t D2 = 4;
static constexpr std::size_t D3 = 3;
static constexpr std::size_t totalSize = D0 * D1 * D2 * D3;

static bool hasNaNTest(DeviceType deviceType, const bool introduceNaN,
                       const bool introduceInf, const Type &type,
                       std::size_t testSize, unsigned numTiles,
                       bool twoDimensionalVertices, bool checkNaNOnly) {
  std::mt19937 randomEngine;
  boost::random::uniform_real_distribution<double> dist(0., 10.);

  boost::multi_array<double, 4> input(boost::extents[D0][D1][D2][D3]);

  for (unsigned i = 0; i < input.num_elements(); ++i) {
    *(input.data() + i) = dist(randomEngine);
  }

  // Fill last element
  const std::vector<std::size_t> shape = {D0, D1, D2, D3};

  if (introduceNaN) {
    auto indices = poputil::unflattenIndex(shape, testSize - 1);
    input[indices[0]][indices[1]][indices[2]][indices[3]] = NAN;
  }

  if ((testSize > 1 || !introduceNaN) && introduceInf) {
    auto off = 1 + (testSize > 1);
    auto indices = poputil::unflattenIndex(shape, testSize - off);
    input[indices[0]][indices[1]][indices[2]][indices[3]] =
        std::numeric_limits<double>::infinity();
  }

  // fill NANs outside
  std::fill(input.data() + testSize, input.data() + input.num_elements(), NAN);

  auto device = createTestDevice(deviceType, 1, numTiles, true);
  const auto &target = device.getTarget();
  Graph graph(target);

  popops::addCodelets(graph);

  Tensor inputT;
  if (twoDimensionalVertices) {
    static_assert(D0 % 2 == 0);
    auto var1 = graph.addVariable(type, {D0 / 2, D1, D2, D3}, "input1");
    auto var2 = graph.addVariable(type, {D0 / 2, D1, D2, D3}, "input2");
    poputil::mapTensorLinearly(graph, var1);
    poputil::mapTensorLinearly(graph, var2);

    inputT = concat(var1, var2);
  } else {
    inputT = graph.addVariable(type, {D0, D1, D2, D3}, "input");
    poputil::mapTensorLinearly(graph, inputT);
  }

  Sequence uploadProg, downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;

  auto rawHostInput = allocateHostMemoryForTensor(
      inputT, "input", graph, uploadProg, downloadProg, tmap);
  copy(target, input, type, rawHostInput.get());

  Sequence prog;
  const auto out =
      checkNaNOnly
          ? popops::hasNaN(graph, inputT.flatten().slice(0, testSize), prog)
          : popops::hasNaNOrInf(graph, inputT.flatten().slice(0, testSize),
                                prog);
  auto rawHostOutput = allocateHostMemoryForTensor(
      out, "out", graph, uploadProg, downloadProg, tmap);

  const poplar::OptionFlags options{{"debug.floatPointOpException", "false"}};
  Engine engine(graph, Sequence{uploadProg, prog, downloadProg}, options);
  device.bind([&](const Device &d) {
    engine.load(d);
    attachStreams(engine, tmap);

    engine.run(0);
  });

  boost::multi_array<bool, 1> result(boost::extents[1]);
  copy(target, BOOL, rawHostOutput.get(), result);
  return checkNaNOnly ? result[0] == introduceNaN
                      : result[0] == (introduceNaN || introduceInf);
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;
  DeviceType deviceType;

  std::size_t numElements;
  bool fullSize, addInf, addNaN, twoD = false, checkNaNOnly;
  Type dType;
  unsigned numTiles = 1;

  const std::string sizeString = "Number of elements in tensor (max is " +
                                 std::to_string(totalSize) +
                                 ") "
                                 " ignored if fullSize is set to true";

  po::options_description desc("Options");
  // clang-format off
  desc.add_options() ("help", "Print help")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("full-size",
     po::value<bool>(&fullSize)->required(),
     "Test full size tensor(if set tests full tensor size else only part of it "
     "as given by numElements)")
    ("size",
     po::value<std::size_t>(&numElements)->default_value(totalSize),
     sizeString.data())
    ("two-d",
      po::value<bool>(&twoD)->default_value(twoD),
      "Use 2D vertex (only allowed with fullSize)")
    ("add-inf",
     po::value<bool>(&addInf)->required(),
     "Add inf to the tensor")
    ("add-nan",
     po::value<bool>(&addNaN)->required(),
     "Add NaN to the tensor")
    ("check-nan-only",
     po::value<bool>(&checkNaNOnly)->required(),
     "Check for NaNs only even if there may be infs, else checks for both")
    ("num-tiles",
     po::value<unsigned>(&numTiles)->default_value(numTiles),
     "Number of tiles to use")
    ("data-type",
     po::value<Type>(&dType)->required(),
     "data type used in the test (half/float)");

  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }
  if (numElements > totalSize) {
    std::cerr << "size exceed maximum allowed\n";
    return 1;
  }
  if (!fullSize && twoD) {
    std::cerr << "2D vertices are only restricted with full sized tensors\n";
    return 1;
  }

  auto result = hasNaNTest(deviceType, addNaN, addInf, dType, numElements,
                           numTiles, twoD, checkNaNOnly);

  return !result;
}