/**
* 2D NDT-MCL Node.
* This application runs the ndt-mcl localization based on a map and laser scanner and odometry.
*
* The initialization is based now on ground truth pose information (should be replaced with manual input).
*
* The visualization depends on mrpt-gui
*
* More details about the algorithm:
* Jari Saarinen, Henrik Andreasson, Todor Stoyanov and Achim J. Lilienthal, Normal Distributions Transform Monte-Carlo Localization (NDT-MCL)
* IEEE/RSJ International Conference on Intelligent Robots and Systems November 3-8, 2013, Tokyo Big Sight, Japan
*
* @author Jari Saarinen (jari.p.saarinen@gmail.com)
*
*	@TODO Initialization from GUI
* @TODO Global initialization possibility
* Known issues: in launch file (or hard coded parameters) you have to set the same resolution for NDT map as is saved -- otherwise it wont work
*/

#define USE_VISUALIZATION_DEBUG ///< Enable / Disable visualization

#ifdef USE_VISUALIZATION_DEBUG
    #include <ndt_visualisation/ndt_viz.h>
#endif

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
#include <sensor_msgs/LaserScan.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_broadcaster.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/Pose.h"

#include "ndt_mcl/ndt_mcl.h"
#include <ndt_map/ndt_map.h>

/////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Visualization stuff
/////////////////////////////////////////////////////////////////////////////////////////////////////////
#ifdef USE_VISUALIZATION_DEBUG
NDTViz ndt_viz;
#endif
ros::Publisher ndtmap_pub;

/////////////////////////////////////////////////////////////////////////////////////////////////////////
/// RVIZ NDT-MAP Visualization stuff
/////////////////////////////////////////////////////////////////////////////////////////////////////////
void sendMapToRviz(perception_oru::NDTMap &map){

	std::vector<perception_oru::NDTCell*> ndts;
	ndts = map.getAllCells();
	fprintf(stderr,"SENDING MARKER ARRAY MESSAGE (%zu components)\n",ndts.size());
	visualization_msgs::MarkerArray marray;

	for(unsigned int i=0;i<ndts.size();i++){
	    if(!ndts[i]->hasGaussian_) continue;
	    if(ndts[i]->getOccupancy() <= 0) continue;
		Eigen::Matrix3d cov = ndts[i]->getCov();
		Eigen::Vector3d m = ndts[i]->getMean();

		Eigen::Matrix3d evecs;
		Eigen::Vector3d evals;

		const double d=cov.determinant();
		if (d<=0 || d!=d) // Note: "d!=d" is a great test for invalid numbers, don't remove!
		{
		    continue;
		    // All zeros:
		    //evecs = Eigen::Matrix3d::Zero(3,3);
		    //evals = Eigen::Vector3d::Zero(3,3);
		}
		else
		{
		    Eigen::EigenSolver<Eigen::Matrix3d> es(cov);

		    evals = es.pseudoEigenvalueMatrix().diagonal();
		    evecs = es.pseudoEigenvectors();
		    evals = evals.cwiseSqrt();
		}
	/*
		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> Sol (cov);

    Eigen::Matrix3d evecs;
    Eigen::Vector3d evals;

    evecs = Sol.eigenvectors().real();
    evals = Sol.eigenvalues().real().cwiseSqrt();
    */
		Eigen::AngleAxisd orient;
		orient.fromRotationMatrix(evecs);

		Eigen::Quaternion<double> q(orient);

		visualization_msgs::Marker marker;
		marker.header.frame_id = "world";
		marker.header.stamp = ros::Time();
		marker.ns = "NDT";
		marker.id = i;
		marker.type = visualization_msgs::Marker::SPHERE;
		marker.action = visualization_msgs::Marker::ADD;
		marker.pose.position.x = m[0];
		marker.pose.position.y = m[1];
		marker.pose.position.z = m[2];

		marker.pose.orientation.x = q.x();
		marker.pose.orientation.y = q.y();
		marker.pose.orientation.z = q.z();
		marker.pose.orientation.w = q.w();

		marker.scale.x = 3.0*evals(0);
		marker.scale.y = 3.0*evals(1);
		marker.scale.z = 3.0*evals(2);
		/*
		marker.pose.orientation.x = 0;
		marker.pose.orientation.y = 0;
		marker.pose.orientation.z = 0;
		marker.pose.orientation.w = 1;

		marker.scale.x = 1;
		marker.scale.y = 1;
		marker.scale.z = 1;
	*/

		marker.color.a = 1.0;
		marker.color.r = 0.0;
		marker.color.g = 1.0;
		marker.color.b = 0.0;

		marray.markers.push_back(marker);
	}

	ndtmap_pub.publish(marray);

	for(unsigned int i=0;i<ndts.size();i++){
		delete ndts[i];
	}

}

/*
 * enter code here
(eigValues,eigVectors) = numpy.linalg.eig (covMat)

eigx_n=-PyKDL.Vector(eigVectors[0,0],eigVectors[1,0],eigVectors[2,0])
eigy_n=-PyKDL.Vector(eigVectors[0,1],eigVectors[1,1],eigVectors[2,1])
eigz_n=-PyKDL.Vector(eigVectors[0,2],eigVectors[1,2],eigVectors[2,2])

rot = PyKDL.Rotation (eigx_n,eigy_n,eigz_n)
quat = rot.GetQuaternion ()

#painting the Gaussian Ellipsoid Marker
marker.pose.orientation.x =quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = eigValues[0]
marker.scale.y = eigValues[1]
marker.scale.z =eigValues[2]
*/

/////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Initial pose stuff (quick and dirty as always)
/////////////////////////////////////////////////////////////////////////////////////////////////////////
bool userInitialPose = false;
bool hasNewInitialPose = false;

double ipos_x=0,ipos_y=0,ipos_yaw=0;
double ivar_x=0,ivar_y=0,ivar_yaw=0;

ros::Subscriber initial_pose_sub_;

void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg){
	tf::Pose ipose;
  tf::poseMsgToTF(msg->pose.pose, ipose);
  ipos_x = ipose.getOrigin().x();
  ipos_y = ipose.getOrigin().y();
  double pitch, roll;
  ipose.getBasis().getEulerYPR(ipos_yaw,pitch,roll);

  ivar_x = msg->pose.covariance[0];
  ivar_x = msg->pose.covariance[6];
  ivar_x = msg->pose.covariance[35];

  hasNewInitialPose=true;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * Convert x,y,yaw to Eigen::Affine3d
 */
Eigen::Affine3d getAsAffine(float x, float y, float yaw ){
	Eigen::Matrix3d m;
	m = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
			* Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
			* Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
	Eigen::Translation3d v(x,y,0);
	Eigen::Affine3d T = v*m;

	return T;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Update measurement
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Globals
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
NDTMCL *ndtmcl;
///Laser sensor offset
float offx = 0;
float offy = 0;
float offa = 0;
static bool has_sensor_offset_set = false;
static bool isFirstLoad=true;
Eigen::Affine3d Told,Todo;

ros::Publisher mcl_pub; ///< The output of MCL is published with this!
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
bool sendROSOdoMessage(Eigen::Vector3d mean,Eigen::Matrix3d cov, ros::Time ts){
	nav_msgs::Odometry O;
	static int seq = 0;
	O.header.stamp = ts;
	O.header.seq = seq;
	O.header.frame_id = "/world";
	O.child_frame_id = "/mcl_pose";

	O.pose.pose.position.x = mean[0];
	O.pose.pose.position.y = mean[1];
	tf::Quaternion q;
	q.setRPY(0,0,mean[2]);
	O.pose.pose.orientation.x = q.getX();
	O.pose.pose.orientation.y = q.getY();
	O.pose.pose.orientation.z = q.getZ();
	O.pose.pose.orientation.w = q.getW();

	/**
	O.pose.covariance = { cov(0,0), cov(0,1), 0 , 0 , 0 , 0,
											 cov(1,0), cov(1,1), 0 , 0 , 0 , 0,
											 0       ,     0   , 0 , 0 , 0 , 0,
											 0       ,     0   , 0 , 0 , 0 , 0,
											 0       ,     0   , 0 , 0 , 0 , 0,
											 0       ,     0   , 0 , 0 , 0 , cov(2,2)};*/

	O.pose.covariance[0] = cov(0,0);
	O.pose.covariance[1] = cov(0,1);
	O.pose.covariance[2] = 0;
	O.pose.covariance[3] = 0;
	O.pose.covariance[4] = 0;
	O.pose.covariance[5] = 0;

	O.pose.covariance[6] = cov(1,0);
	O.pose.covariance[7] = cov(1,1);
	O.pose.covariance[8] = 0;
	O.pose.covariance[9] = 0;
	O.pose.covariance[10] = 0;
	O.pose.covariance[11] = 0;

	O.pose.covariance[12] = 0;
	O.pose.covariance[13] = 0;
	O.pose.covariance[14] = 0;
	O.pose.covariance[15] = 0;
	O.pose.covariance[16] = 0;
	O.pose.covariance[17] = 0;

	O.pose.covariance[18] = 0;
	O.pose.covariance[19] = 0;
	O.pose.covariance[20] = 0;
	O.pose.covariance[21] = 0;
	O.pose.covariance[22] = 0;
	O.pose.covariance[23] = 0;

	O.pose.covariance[24] = 0;
	O.pose.covariance[25] = 0;
	O.pose.covariance[26] = 0;
	O.pose.covariance[27] = 0;
	O.pose.covariance[28] = 0;
	O.pose.covariance[29] = 0;

	O.pose.covariance[30] = 0;
	O.pose.covariance[31] = 0;
	O.pose.covariance[32] = 0;
	O.pose.covariance[33] = 0;
	O.pose.covariance[34] = 0;
	O.pose.covariance[35] = cov(2,2);

	seq++;
	mcl_pub.publish(O);

	static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(mean[0],mean[1], 0.0) );

	transform.setRotation( q );
  br.sendTransform(tf::StampedTransform(transform, ts, "world", "mcl_pose"));

	return true;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
double time_end;
std::string tf_odo_topic =   "odom_base_link";
std::string tf_state_topic = "base_link";
std::string tf_laser_link =  "base_laser_link";

double getDoubleTime()
{
    struct timeval time;
    gettimeofday(&time,NULL);
    return time.tv_sec + time.tv_usec * 1e-6;
}
/**
 * Callback for laser scan messages
 */
void callback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
	static int counter = 0;
	counter++;

	static tf::TransformListener tf_listener;
	double time_now = getDoubleTime();
	double looptime = time_end - time_now;
	fprintf(stderr,"Lt( %.1lfms %.1lfHz seq:%d) -",looptime*1000,1.0/looptime,scan->header.seq);

	if(has_sensor_offset_set == false) return;
	double gx,gy,gyaw,x,y,yaw;

	///Get state information
	tf::StampedTransform transform;
	tf_listener.waitForTransform("world", tf_state_topic, scan->header.stamp,ros::Duration(1.0));
	///Ground truth --- Not generally available so should be changed to the manual initialization
	try{
		tf_listener.lookupTransform("world", tf_state_topic, scan->header.stamp, transform);
		gyaw = tf::getYaw(transform.getRotation());
	  gx = transform.getOrigin().x();
	  gy = transform.getOrigin().y();
	}
	catch (tf::TransformException ex){
		ROS_ERROR("%s",ex.what());
		return;
	}

	///Odometry
	try{
		tf_listener.lookupTransform("world", tf_odo_topic, scan->header.stamp, transform);
		yaw = tf::getYaw(transform.getRotation());
	  x = transform.getOrigin().x();
	  y = transform.getOrigin().y();
	}
	catch (tf::TransformException ex){
		ROS_ERROR("%s",ex.what());
		return;
	}

	///Number of scans
	int N =(scan->angle_max - scan->angle_min)/scan->angle_increment;
	/////
	/// Pose conversions
	////

	Eigen::Affine3d T = getAsAffine(x,y,yaw);
	Eigen::Affine3d Tgt = getAsAffine(gx,gy,gyaw);

	if(userInitialPose && hasNewInitialPose){
		gx = ipos_x;
		gy = ipos_y;
		gyaw = ipos_yaw;
	}

	if(isFirstLoad || hasNewInitialPose){
		fprintf(stderr,"Initializing to (%lf, %lf, %lf)\n",gx,gy,gyaw);
		/// Initialize the particle filter
		ndtmcl->initializeFilter(gx, gy,gyaw,0.2, 0.2, 2.0*M_PI/180.0, 150);
		Told = T;
		Todo = Tgt;
		hasNewInitialPose = false;
	}

	///Calculate the differential motion from the last frame
	Eigen::Affine3d Tmotion = Told.inverse() * T;
	Todo = Todo*Tmotion; ///< just integrates odometry for the visualization

	if(isFirstLoad==false){
		if( (Tmotion.translation().norm()<0.005 && fabs(Tmotion.rotation().eulerAngles(0,1,2)[2])<(0.2*M_PI/180.0))){
			Eigen::Vector3d dm = ndtmcl->getMean();
			Eigen::Matrix3d cov = ndtmcl->pf.getDistributionVariances();
			sendROSOdoMessage(dm,cov, scan->header.stamp);
			return;
		}
	}

	Told = T;

	///Calculate the laser pose with respect to the base
	float dy =offy;
	float dx = offx;
	float alpha = atan2(dy,dx);
	float L = sqrt(dx*dx+dy*dy);

	///Laser pose in base frame
	float lpx = L * cos(alpha);
	float lpy = L * sin(alpha);
	float lpa = offa;

	///Laser scan to PointCloud expressed in the base frame
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	for (int j=0;j<N;j++){
		double r  = scan->ranges[j];
		if(r>=scan->range_min && r<scan->range_max && r>0.3 && r<20.0){
			double a  = scan->angle_min + j*scan->angle_increment;
			pcl::PointXYZ pt;
			pt.x = r*cos(a+lpa)+lpx;
			pt.y = r*sin(a+lpa)+lpy;
			pt.z = 0.1+0.02 * (double)rand()/(double)RAND_MAX;
			cloud->push_back(pt);
		}
	}
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	/// Now we have the sensor origin and pointcloud -- Lets do MCL
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	ndtmcl->updateAndPredict(Tmotion, *cloud); ///<predicts, updates and resamples if necessary (ndt_mcl.hpp)

	Eigen::Vector3d dm = ndtmcl->getMean(); ///Maximum aposteriori pose
	Eigen::Matrix3d cov = ndtmcl->pf.getDistributionVariances(); ///Pose covariance
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	time_end = getDoubleTime();
	fprintf(stderr,"Time elapsed %.1lfms (%lf %lf %lf) \n",time_end-time_now,dm[0],dm[1],dm[2]);
	isFirstLoad = false;
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	sendROSOdoMessage(dm,cov, scan->header.stamp); ///Spit out the pose estimate

	if(counter%50==0){
		sendMapToRviz(ndtmcl->map);
	}

	///This is all for visualization
#ifdef USE_VISUALIZATION_DEBUG
	if(counter%500==0){
	    ndt_viz.clear();
	    ndt_viz.plotNDTSAccordingToOccupancy(-1,&ndtmcl->map);

	}
	ndt_viz.clearParticles();
	for(int i=0;i<ndtmcl->pf.NumOfParticles;i++){
	    ndt_viz.addParticle(ndtmcl->pf.Particles[i].x, ndtmcl->pf.Particles[i].y, 0.5, 1.0, 1.0, 1.0);
	}

	ndt_viz.addTrajectoryPoint(dm[0],dm[1],0.5,1.0,0,0);
	ndt_viz.addTrajectoryPoint(Tgt.translation()(0), Tgt.translation()(1),0.5,1.0,1.0,1.0);
	ndt_viz.addTrajectoryPoint(Todo.translation()(0), Todo.translation()(1),0.5,0.0,1.0,0.0);

	ndt_viz.displayParticles();
	ndt_viz.displayTrajectory();
	ndt_viz.win3D->setCameraPointingToPoint(dm[0],dm[1],3.0);
	ndt_viz.repaint();

#endif
}

int main(int argc, char **argv){
	ros::init(argc, argv, "NDT-MCL");
	double resolution=0.4;

	ros::NodeHandle n;
	ros::NodeHandle nh;
	ros::NodeHandle paramHandle ("~");
	time_end = getDoubleTime();

	std::string tf_base_link,input_laser_topic;

	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	/// Parameters for the mapper
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	bool loadMap = false; ///< flag to indicate that we want to load a map
	std::string mapName("basement.ndmap"); ///<name and the path to the map

	bool saveMap = true;						///< indicates if we want to save the map in a regular intervals
	std::string output_map_name = std::string("ndt_mapper_output.ndmap");

	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	/// Set the values from a config file
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////

	paramHandle.param<std::string>("input_laser_topic", input_laser_topic, std::string("/base_scan"));
	paramHandle.param<std::string>("tf_base_link", tf_state_topic, std::string("/base_link"));
	paramHandle.param<std::string>("tf_laser_link", tf_laser_link, std::string("/hokuyo1_link"));

	bool use_sensor_pose;
	paramHandle.param<bool>("use_sensor_pose", use_sensor_pose, false);
	double sensor_pose_x, sensor_pose_y, sensor_pose_th;
	paramHandle.param<double>("sensor_pose_x", sensor_pose_x, 0.);
	paramHandle.param<double>("sensor_pose_y", sensor_pose_y, 0.);
	paramHandle.param<double>("sensor_pose_th", sensor_pose_th, 0.);

	paramHandle.param<bool>("load_map_from_file", loadMap, false);
	paramHandle.param<std::string>("map_file_name", mapName, std::string("basement.ndmap"));

	paramHandle.param<bool>("save_output_map", saveMap, true);
	paramHandle.param<std::string>("output_map_file_name", output_map_name, std::string("ndt_mapper_output.ndmap"));

	paramHandle.param<double>("map_resolution", resolution , 0.2);
	bool forceSIR=false;
	paramHandle.param<bool>("forceSIR", forceSIR, false);

	paramHandle.param<bool>("set_initial_pose", userInitialPose, false);
	paramHandle.param<double>("initial_pose_x", ipos_x, 0.);
	paramHandle.param<double>("initial_pose_y", ipos_y, 0.);
	paramHandle.param<double>("initial_pose_yaw", ipos_yaw, 0.);

	if(userInitialPose==true) hasNewInitialPose=true;

	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	/// Prepare the map
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	fprintf(stderr,"USING RESOLUTION %lf\n",resolution);
	perception_oru::NDTMap ndmap(new perception_oru::LazyGrid(resolution));

	ndmap.setMapSize(80.0, 80.0, 1.0);

	if(loadMap){
		fprintf(stderr,"Loading Map from '%s'\n",mapName.c_str());
		ndmap.loadFromJFF(mapName.c_str());
	}

	ndtmcl = new NDTMCL(resolution,ndmap,-0.5);
	if(forceSIR) ndtmcl->forceSIR=true;

	fprintf(stderr,"*** FORCE SIR = %d****",forceSIR);
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	///Set up our output
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	mcl_pub = nh.advertise<nav_msgs::Odometry>("ndt_mcl",10);
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	/// Set the subscribers and setup callbacks and message filters
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	ros::Subscriber scansub=nh.subscribe(input_laser_topic, 1, callback);

	ndtmap_pub = nh.advertise<visualization_msgs::MarkerArray>( "NDTMAP", 0 );

	initial_pose_sub_ = nh.subscribe("initialpose", 1, initialPoseReceived);

	offa = sensor_pose_th;
	offx = sensor_pose_x;
	offy = sensor_pose_y;

	has_sensor_offset_set = true;

	fprintf(stderr,"Sensor Pose = (%lf %lf %lf)\n",offx, offy, offa);

	ros::spin();

	return 0;
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

// Alain 15.08.2015: workaround after VS2013 & intel XE 2015
#ifndef MKL_BLAS
#  define MKL_BLAS MKL_DOMAIN_BLAS
#endif

#include <Utils/DataStructures/MolecularOrbitals.h>
#include <Utils/DataStructures/OccupiedMolecularOrbitals.h>
#include <Utils/Scf/LcaoUtils/ElectronicOccupation.h>
#include <Utils/Scf/LcaoUtils/HFWaveFunctionOverlap.h>
#include <gmock/gmock.h>
#include <Eigen/Eigenvalues>
#include <cmath>

using namespace testing;
namespace Scine {
namespace Utils {
namespace LcaoUtils {
namespace Tests {

class AHFWaveFunctionOverlap : public Test {
 protected:
  void SetUp() override;

 public:
  MolecularOrbitals randomOrthonormalRhfOrbitals;
  MolecularOrbitals randomNonOrthonormalRhfOrbitals;
  MolecularOrbitals randomOrthonormalUhfOrbitals;
  MolecularOrbitals randomNonOrthonormalUhfOrbitals;
  Eigen::MatrixXd randomOverlapMatrixForRhf;
  Eigen::MatrixXd randomOverlapMatrixForUhf;
  const int nOrbitals = 8;

  Eigen::MatrixXd generateOrthogonalCoefficientMatrix();
  Eigen::MatrixXd generateNonOrthogonalCoefficientMatrix(const Eigen::MatrixXd& overlap);
  Eigen::MatrixXd generateRandomOverlapMatrix();
};

void AHFWaveFunctionOverlap::SetUp() {
  randomOverlapMatrixForRhf = generateRandomOverlapMatrix();
  randomOverlapMatrixForUhf = generateRandomOverlapMatrix();
  auto randomOrthogonalCoefficientMatrix1 = generateOrthogonalCoefficientMatrix();
  auto randomOrthogonalCoefficientMatrix2 = generateOrthogonalCoefficientMatrix();
  auto randomOrthogonalCoefficientMatrix3 = generateOrthogonalCoefficientMatrix();
  auto randomNonOrthogonalCoefficientMatrix1 = generateNonOrthogonalCoefficientMatrix(randomOverlapMatrixForRhf);
  auto randomNonOrthogonalCoefficientMatrix2 = generateNonOrthogonalCoefficientMatrix(randomOverlapMatrixForUhf);
  auto randomNonOrthogonalCoefficientMatrix3 = generateNonOrthogonalCoefficientMatrix(randomOverlapMatrixForUhf);
  randomOrthonormalRhfOrbitals = MolecularOrbitals::createFromRestrictedCoefficients(randomOrthogonalCoefficientMatrix1);
  randomNonOrthonormalRhfOrbitals = MolecularOrbitals::createFromRestrictedCoefficients(randomNonOrthogonalCoefficientMatrix1);
  randomOrthonormalUhfOrbitals = MolecularOrbitals::createFromUnrestrictedCoefficients(randomOrthogonalCoefficientMatrix2,
                                                                                randomOrthogonalCoefficientMatrix3);
  randomNonOrthonormalUhfOrbitals = MolecularOrbitals::createFromUnrestrictedCoefficients(
      randomNonOrthogonalCoefficientMatrix2, randomNonOrthogonalCoefficientMatrix3);
}

Eigen::MatrixXd AHFWaveFunctionOverlap::generateRandomOverlapMatrix() {
  Eigen::MatrixXd random = Eigen::MatrixXd::Random(nOrbitals, nOrbitals);
  random /= 10;
  for (int i = 0; i < nOrbitals; ++i)
    random(i, i) = 1;
  return .5 * (random + random.transpose());
}

Eigen::MatrixXd AHFWaveFunctionOverlap::generateOrthogonalCoefficientMatrix() {
  Eigen::MatrixXd matrixToDiagonalize = Eigen::MatrixXd::Random(nOrbitals, nOrbitals);
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es;
  es.compute(matrixToDiagonalize);
  return es.eigenvectors();
}

Eigen::MatrixXd AHFWaveFunctionOverlap::generateNonOrthogonalCoefficientMatrix(const Eigen::MatrixXd& overlap) {
  Eigen::MatrixXd matrixToDiagonalize = 20 * Eigen::MatrixXd::Random(nOrbitals, nOrbitals);
  Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> es;
  es.compute(matrixToDiagonalize, overlap);
  return es.eigenvectors();
}

TEST_F(AHFWaveFunctionOverlap, returnsOneForIdenticalRhfWaveFunctions) {
  LcaoUtils::ElectronicOccupation rhfOccupation;
  rhfOccupation.fillSpecifiedRestrictedOrbitals({0, 2, 3});
  OccupiedMolecularOrbitals orthonormalRhfOrbitals{randomOrthonormalRhfOrbitals, rhfOccupation};
  OccupiedMolecularOrbitals nonOrthonormalRhfOrbitals{randomNonOrthonormalRhfOrbitals, rhfOccupation};

  double selfRhfOrthonormal =
      HFWaveFunctionOverlap::calculateOrthonormalOverlap(orthonormalRhfOrbitals, orthonormalRhfOrbitals);
  double selfRhfNonOrthonormal = HFWaveFunctionOverlap::calculateNonOrthonormalOverlap(
      nonOrthonormalRhfOrbitals, nonOrthonormalRhfOrbitals, randomOverlapMatrixForRhf);
  ASSERT_THAT(selfRhfOrthonormal, DoubleNear(1, 1e-12));
  ASSERT_THAT(selfRhfNonOrthonormal, DoubleNear(1, 1e-12));
}

TEST_F(AHFWaveFunctionOverlap, returnsOneForIdenticalUhfWaveFunctions) {
  LcaoUtils::ElectronicOccupation uhfOccupation;
  uhfOccupation.fillSpecifiedUnrestrictedOrbitals({1, 2, 4}, {5, 6});
  OccupiedMolecularOrbitals orthonormalUhfOrbitals{randomOrthonormalUhfOrbitals, uhfOccupation};
  OccupiedMolecularOrbitals nonOrthonormalUhfOrbitals{randomNonOrthonormalUhfOrbitals, uhfOccupation};

  double selfUhfOrthonormal =
      HFWaveFunctionOverlap::calculateOrthonormalOverlap(orthonormalUhfOrbitals, orthonormalUhfOrbitals);
  double selfUhfNonOrthonormal = HFWaveFunctionOverlap::calculateNonOrthonormalOverlap(
      nonOrthonormalUhfOrbitals, nonOrthonormalUhfOrbitals, randomOverlapMatrixForUhf);
  ASSERT_THAT(selfUhfOrthonormal, DoubleNear(1, 1e-12));
  ASSERT_THAT(selfUhfNonOrthonormal, DoubleNear(1, 1e-12));
}

TEST_F(AHFWaveFunctionOverlap, returnsZeroIfOneOrbitalIsSwitched) {
  LcaoUtils::ElectronicOccupation uhfOccupation1;
  LcaoUtils::ElectronicOccupation uhfOccupation2;
  uhfOccupation1.fillSpecifiedUnrestrictedOrbitals({1, 2, 4}, {5, 6});
  uhfOccupation2.fillSpecifiedUnrestrictedOrbitals({1, 2, 4}, {5, 7});
  OccupiedMolecularOrbitals orthonormalUhfOrbitals1{randomOrthonormalUhfOrbitals, uhfOccupation1};
  OccupiedMolecularOrbitals nonOrthonormalUhfOrbitals1{randomNonOrthonormalUhfOrbitals, uhfOccupation1};
  OccupiedMolecularOrbitals orthonormalUhfOrbitals2{randomOrthonormalUhfOrbitals, uhfOccupation2};
  OccupiedMolecularOrbitals nonOrthonormalUhfOrbitals2{randomNonOrthonormalUhfOrbitals, uhfOccupation2};

  double selfUhfOrthonormal =
      HFWaveFunctionOverlap::calculateOrthonormalOverlap(orthonormalUhfOrbitals1, orthonormalUhfOrbitals2);
  double selfUhfNonOrthonormal = HFWaveFunctionOverlap::calculateNonOrthonormalOverlap(
      nonOrthonormalUhfOrbitals1, nonOrthonormalUhfOrbitals2, randomOverlapMatrixForUhf);
  ASSERT_THAT(selfUhfOrthonormal, DoubleNear(0, 1e-12));
  ASSERT_THAT(selfUhfNonOrthonormal, DoubleNear(0, 1e-12));
}

TEST_F(AHFWaveFunctionOverlap, ReturnsOneForOverlapBetweenRhfAndCorrespondingUhfWaveFunctions) {
  LcaoUtils::ElectronicOccupation rhfOccupation;
  LcaoUtils::ElectronicOccupation uhfOccupation;
  rhfOccupation.fillSpecifiedRestrictedOrbitals({0, 2, 3});
  uhfOccupation.fillSpecifiedUnrestrictedOrbitals({0, 2, 3}, {0, 2, 3});

  OccupiedMolecularOrbitals orthonormalRhfOrbitals{randomOrthonormalRhfOrbitals, rhfOccupation};
  OccupiedMolecularOrbitals nonOrthonormalRhfOrbitals{randomNonOrthonormalRhfOrbitals, rhfOccupation};
  auto orthonormalUhfOrbitals = orthonormalRhfOrbitals.toUnrestricted();
  auto nonOrthonormalUhfOrbitals = nonOrthonormalRhfOrbitals.toUnrestricted();

  double orthonormalDistance =
      HFWaveFunctionOverlap::calculateOrthonormalOverlap(orthonormalRhfOrbitals, orthonormalUhfOrbitals);
  double nonOrthonormalDistance = HFWaveFunctionOverlap::calculateNonOrthonormalOverlap(
      nonOrthonormalRhfOrbitals, nonOrthonormalUhfOrbitals, randomOverlapMatrixForRhf);
  ASSERT_THAT(orthonormalDistance, DoubleNear(1, 1e-12));
  ASSERT_THAT(nonOrthonormalDistance, DoubleNear(1, 1e-12));
}

TEST_F(AHFWaveFunctionOverlap, returnsZeroIfDifferentNumberOfElectrons) {
  LcaoUtils::ElectronicOccupation uhfOccupation1;
  LcaoUtils::ElectronicOccupation uhfOccupation2;
  uhfOccupation1.fillSpecifiedUnrestrictedOrbitals({1, 2, 4}, {1, 2});
  uhfOccupation2.fillSpecifiedUnrestrictedOrbitals({1, 2}, {1, 2, 4});
  OccupiedMolecularOrbitals orthonormalUhfOrbitals1{randomOrthonormalUhfOrbitals, uhfOccupation1};
  OccupiedMolecularOrbitals nonOrthonormalUhfOrbitals1{randomNonOrthonormalUhfOrbitals, uhfOccupation1};
  OccupiedMolecularOrbitals orthonormalUhfOrbitals2{randomOrthonormalUhfOrbitals, uhfOccupation2};
  OccupiedMolecularOrbitals nonOrthonormalUhfOrbitals2{randomNonOrthonormalUhfOrbitals, uhfOccupation2};

  ASSERT_ANY_THROW(HFWaveFunctionOverlap::calculateOrthonormalOverlap(orthonormalUhfOrbitals1, orthonormalUhfOrbitals2));
  ASSERT_ANY_THROW(HFWaveFunctionOverlap::calculateNonOrthonormalOverlap(
      nonOrthonormalUhfOrbitals1, nonOrthonormalUhfOrbitals2, randomOverlapMatrixForUhf));
}
} // namespace Tests
} // namespace LcaoUtils
} // namespace Utils
} // namespace Scine


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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.
*/

#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
// All the boost bindings required includes ...
#include <boost/numeric/bindings/lapack.hpp>
//#include <boost/numeric/bindings/noop.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

#include "SiconosConfig.h"

// Typedef to define interface to boost ublas.
//#include "SiconosAlgebraTypeDef.hpp"
#include "SiconosMatrix.hpp"
#include "SiconosVector.hpp"

#include "EigenProblems.hpp"
// Some utilities (print ...)

namespace lapack = boost::numeric::bindings::lapack;

namespace Siconos {
  namespace eigenproblems {

    int syev(SiconosVector& eigenval, SiconosMatrix& eigenvec, bool withVect)
    {
      int info = 0;
      // Eigenvec must contains the values of the matrix from which we want
      // to compute eigenvalues and vectors. It must be a symmetric matrix.
      // It will be overwritten with eigenvectors.

      // Adaptor to symmetric_mat. Warning : no copy, eigenvec will be modified
      // by syev.
      ublas::symmetric_adaptor<DenseMat, ublas::lower> s_a(*eigenvec.dense());

      char jobz;
      if(withVect)
        jobz = 'V';
      else
        jobz = 'N';

#ifdef USE_OPTIMAL_WORKSPACE
      info += lapack::syev(jobz, s_a, *eigenval.dense(), lapack::optimal_workspace());
#endif
#ifdef USE_MINIMAL_WORKSPACE
      info += lapack::syev(jobz, s_a, *eigenval.dense(), lapack::minimal_workspace());
#endif
      std::cout << "Compute eigenvalues ..." << std::endl;
      return info;
    }

    int geev(SiconosMatrix& input_mat, complex_vector& eigenval, complex_matrix& left_eigenvec, complex_matrix& right_eigenvec, bool withLeft, bool withRight)
    {
      int info = 0;
      complex_matrix tmp(*input_mat.dense());
      // tmp must contains the values of the matrix from which we want
      // to compute eigenvalues and vectors. It must be a complex matrix.
      // It will be overwritten with temp results.

      char jobvl, jobvr;
      if(withLeft)
        jobvl = 'V';
      else
        jobvl = 'N';

      if(withRight)
        jobvr = 'V';
      else
        jobvr = 'N';

#ifdef USE_OPTIMAL_WORKSPACE
      info += lapack::geev(jobvl, jobvr, tmp, eigenval, left_eigenvec, right_eigenvec, lapack::optimal_workspace());
#endif
#ifdef USE_MINIMAL_WORKSPACE
      info += lapack::geev(jobvl, jobvr, tmp, eigenval, left_eigenvec, right_eigenvec, lapack::minimal_workspace());
#endif
      return info;
    }

  } // namespace eigenproblems
} // namespace Siconos


/* Copyright (C) 2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

#include <iostream>
#include <fstream>

#include <helib/helib.h>
#include <helib/ArgMap.h>

#include <NTL/BasicThreadPool.h>

#include "Reader.h"
#include "common.h"

struct CmdLineOpts
{
  std::string skFilePath;
  std::string ctxtFilePath;
  std::string outFilePath;
  long batchSize = 0;
  long nthreads = 0; // Default is 0 for number of cpus.
};

void writeDimsHeader(std::ostream& os, std::pair<long, long>& dims)
{
  if (dims.second == 1) {
    os << dims.first << std::endl;
  } else {
    os << dims.first << " " << dims.second << std::endl;
  }
}

template <typename SCHEME>
void decryptFromTo(const CmdLineOpts& cmdLineOpts,
                   const helib::Context& context,
                   const helib::SecKey& sk)
{
  // Read in data file header
  std::ifstream dataFile(cmdLineOpts.ctxtFilePath);
  if (!dataFile.is_open()) {
    throw std::runtime_error("Could not open file '" +
                             cmdLineOpts.ctxtFilePath + "'.");
  }

  // Read in a batch
  std::vector<helib::Ptxt<SCHEME>> ptxts;
  std::vector<helib::Ctxt> ctxts;
  helib::Ctxt zero_ctxt(sk);
  helib::Ptxt<SCHEME> zero_ptxt(context);

  Reader<helib::Ctxt> reader(cmdLineOpts.ctxtFilePath, zero_ctxt);

  std::pair<long, long> dims = {reader.getTOC().getRows(),
                                reader.getTOC().getCols()};

  std::ofstream outFile;
  std::ostream* out;

  if (!cmdLineOpts.outFilePath.empty()) {
    outFile.open(cmdLineOpts.outFilePath);
    if (!outFile.is_open()) {
      throw std::runtime_error("Could not open file '" +
                               cmdLineOpts.outFilePath + "'.");
    }
    out = &outFile;
  } else {
    out = &std::cout;
  }

  writeDimsHeader(*out, dims);

  for (long remaining = dims.first * dims.second, readBatches = 0;
       remaining > 0;
       remaining -= cmdLineOpts.batchSize, ++readBatches) {

    // Read in a batch
    long bsz =
        (remaining > cmdLineOpts.batchSize) ? cmdLineOpts.batchSize : remaining;
    ptxts.resize(bsz, zero_ptxt);
    ctxts.resize(bsz, zero_ctxt);

    NTL_EXEC_RANGE(ctxts.size(), first, last)
    Reader<helib::Ctxt> threadReader(reader);
    for (long i = first; i < last; ++i) {
      if (dims.second == 1) {
        threadReader.readDatum(ctxts[i],
                               readBatches * cmdLineOpts.batchSize + i,
                               0);
      } else {
        ldiv_t qr = ldiv(readBatches * cmdLineOpts.batchSize + i, dims.second);
        threadReader.readDatum(ctxts[i], qr.quot, qr.rem);
      }
    }

    // Decrypt using NTL threads
    for (long i = first; i < last; ++i) {
      sk.Decrypt(ptxts[i], ctxts[i]);
    }
    NTL_EXEC_RANGE_END

    // Write out to stream
    for (const auto& ptxt : ptxts)
      *out << ptxt << std::endl;
  }
}

int main(int argc, char* argv[])
{
  CmdLineOpts cmdLineOpts;

  // clang-format off
  helib::ArgMap()
    .required()
    .positional()
      .arg("<sk>", cmdLineOpts.skFilePath,
           "secret key.", nullptr)
      .arg("<ctxt>", cmdLineOpts.ctxtFilePath,
           "path of the ciphertext to read in.", nullptr)
    .separator(helib::ArgMap::Separator::WHITESPACE)
    .named()
    .optional()
      .arg("-o", cmdLineOpts.outFilePath,
           "the output file name.", nullptr)
      .arg("-b", cmdLineOpts.batchSize,
           "batch size, how many ctxts in memory. If not set or 0 defaults to the number of threads used.")
      .arg("-n", cmdLineOpts.nthreads,
           "number of threads to use. If not set or 0 defaults to the number of concurrent threads supported.", "num. of cores")
    .parse(argc, argv);
  // clang-format on

  // Set NTL nthreads
  if (cmdLineOpts.nthreads == 0) {
    cmdLineOpts.nthreads = std::thread::hardware_concurrency();
    // hardware_concurrency may still return 0 if not supported on
    // implementation.
    if (cmdLineOpts.nthreads == 0) {
      std::cerr << "C++ `hardware_concurrency` call not available on this"
                   "platform.\nnthreads must be explicitly provided."
                << std::endl;
      return EXIT_FAILURE;
    }
  }

  if (cmdLineOpts.nthreads > 0)
    NTL::SetNumThreads(cmdLineOpts.nthreads);
  else {
    std::cerr << "Number of threads must a be positive integer." << std::endl;
    return EXIT_FAILURE;
  }

  // Set default batch size.
  if (cmdLineOpts.batchSize == 0) {
    cmdLineOpts.batchSize = cmdLineOpts.nthreads;
  }

  // Check batch size above 1.
  if (cmdLineOpts.batchSize < 1) {
    std::cerr << "Batch size must be a positive integer." << std::endl;
    return EXIT_FAILURE;
  }

  // Warn if batch size is less than thread count
  if (cmdLineOpts.batchSize < cmdLineOpts.nthreads) {
    std::cerr << "WARNING: Not enough elements in the batch ("
              << cmdLineOpts.batchSize << ") to run with "
              << cmdLineOpts.nthreads << " threads." << std::endl;
    std::cerr << "To achieve better performance set batch size (-b) to be "
                 "equal to or more than the number of threads (-n). Ideally "
                 "the batch size should be a multiple of the number of threads."
              << std::endl;
  }

  // Load Context and SecKey
  std::unique_ptr<helib::Context> contextp;
  std::unique_ptr<helib::SecKey> skp;

  std::tie(contextp, skp) =
      loadContextAndKey<helib::SecKey>(cmdLineOpts.skFilePath);

  // Read in, decrypt, output.
  try {
    if (contextp->getP() == -1) {
      decryptFromTo<helib::CKKS>(cmdLineOpts, *contextp, *skp);
    } else if (contextp->getP() > 0) {
      decryptFromTo<helib::BGV>(cmdLineOpts, *contextp, *skp);
    } else {
      std::cerr << "Unrecognized scheme from context." << std::endl;
      return EXIT_FAILURE;
    }
  } catch (const std::invalid_argument& e) {
    std::cerr << "Exit due to invalid argument thrown:\n"
              << e.what() << std::endl;
    return EXIT_FAILURE;
  } catch (const helib::IOError& e) {
    std::cerr << "Exit due to IOError thrown:\n" << e.what() << std::endl;
    return EXIT_FAILURE;
  } catch (const std::runtime_error& e) {
    std::cerr << "Exit due to runtime error thrown:\n" << e.what() << std::endl;
    return EXIT_FAILURE;
  } catch (const std::logic_error& e) {
    std::cerr << "Exit due to logic error thrown:\n" << e.what() << std::endl;
    return EXIT_FAILURE;
  } catch (const std::exception& e) {
    std::cerr << "Exit due to unknown exception thrown:\n"
              << e.what() << std::endl;
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file dcs/fog/random.hpp
 *
 * \brief Utility for random number generation.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2017 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * 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.
 */

#ifndef DCS_FOG_RANDOM_HPP
#define DCS_FOG_RANDOM_HPP

#include <boost/random/beta_distribution.hpp>
#include <random>

namespace dcs { namespace fog {

class random_number_engine_t
{
private:
    //typedef std::defatul_random_engine eng_impl_type;
    typedef std::mt19937 eng_impl_type;

public:
    typedef typename eng_impl_type::result_type result_type;

    constexpr static result_type default_seed = eng_impl_type::default_seed;

public:
    explicit random_number_engine_t(result_type seed = default_seed)
    : eng_(seed)
    {
    }

    virtual ~random_number_engine_t()
    {
        // Empty
    }

    /// Initialize the random engine with the given seed
    void seed(result_type value = default_seed)
    {
        eng_.seed(value);
    }

    /// Generate a new random number
    result_type operator()()
    {
        return eng_();
    }

    static result_type constexpr min()
    {
        return eng_impl_type::min();
    }

    static result_type constexpr max()
    {
        return eng_impl_type::max();
    }

private:
    eng_impl_type eng_;
}; // random_number_engine_t

template <typename T>
class degenerate_distribution
{
public:
	typedef T result_type;

	struct param_type
	{
		public:
			typedef degenerate_distribution<result_type> distribution_type;
			explicit param_type(result_type v = result_type())
			: v_(v)
			{
			}

			result_type value() const
			{
				return v_;
			}

			friend bool operator==(const param_type& lhs, const param_type& rhs)
			{
				return lhs.v_ == rhs.v_;
			}

			friend bool operator!=(const param_type& lhs, const param_type& rhs)
			{
				return !(lhs == rhs);
			}

		private:
			T v_;
	};

public:
	explicit degenerate_distribution(T v = T())
	: p_(v)
	{
	}

	explicit degenerate_distribution(const param_type& p)
	: p_(p)
	{
	}

	void reset() { }

	result_type value() const { return p_.value(); }

	param_type param() const { return p_; }

	void param(const param_type& p) { p_ = p; }

	result_type min() const { return p_.value(); }

	result_type max() const { return p_.value(); }

	template <typename URNGT>
	result_type operator()(URNGT& rng)
	{
		(void) rng;
		return p_.value();
	}

	friend bool operator==(const degenerate_distribution& lhs, const degenerate_distribution& rhs)
	{
		return lhs.p_ == rhs.p_;
	}

	friend bool operator!=(const degenerate_distribution& lhs, const degenerate_distribution& rhs)
	{
		return !(lhs == rhs);
	}

private:
	param_type p_;
}; // degenerate_distribution

/**
 * \brief Beta distribution with four parameteres.
 *
 * This is the general form of the Beta distribution with 4 parameters: two shape parameters, lower bound and upper bound.
 */
template <typename T>
class beta_distribution
{
public:
	typedef T result_type;

	struct param_type
	{
		public:
			typedef beta_distribution<result_type> distribution_type;
			explicit param_type(result_type alpha = 1, result_type beta = 1, result_type lower = 0, result_type upper = 1)
			: alpha_(alpha),
              beta_(beta),
              lower_(lower),
              upper_(upper)
			{
			}

			result_type alpha() const
			{
				return alpha_;
			}

			result_type beta() const
			{
				return beta;
			}

			result_type lower() const
			{
				return lower_;
			}

			result_type upper() const
			{
				return upper_;
			}

			friend bool operator==(const param_type& lhs, const param_type& rhs)
			{
				return  lhs.alpha_ == rhs.alpha_
                     && lhs.beta_ == rhs.beta_
                     && lhs.lower_ == rhs.lower_
                     && lhs.upper_ == rhs.upper_;
			}

			friend bool operator!=(const param_type& lhs, const param_type& rhs)
			{
				return !(lhs == rhs);
			}

		private:
			T alpha_;
			T beta_;
			T lower_;
			T upper_;
	};

public:
	explicit beta_distribution(T alpha = 1, T beta = 1, T lower = 0, T upper = 1)
	: p_(alpha, beta, lower, upper),
      beta01_(alpha, beta)
	{
	}

	explicit beta_distribution(const param_type& p)
	: p_(p),
      beta01_(p.alpha(), p.beta())
	{
	}

	void reset() { }

	result_type alpha() const { return p_.alpha(); }

	result_type beta() const { return p_.beta(); }

	result_type lower() const { return p_.lower(); }

	result_type upper() const { return p_.upper(); }

	param_type param() const { return p_; }

	void param(const param_type& p) { p_ = p; }

	result_type min() const { return p_.lower(); }

	result_type max() const { return p_.upper(); }

	template <typename URNGT>
	result_type operator()(URNGT& rng)
	{
        // If Y~Beta(p,q,a,b) and X~Beta(p,q,0,1), then:
        //   y = x(b-a) + a
        // or, equivalently:
        //   x = \frac{y-a}{b-a}.
        //
        // References:
        // - https://en.wikipedia.org/wiki/Beta_distribution#Four_parameters_2
        // - http://www.itl.nist.gov/div898/handbook/eda/section3/eda364.htm#FORMULAS
        //   where location=a, scale=(b-a)
        //

        result_type x = beta01_(rng);
		return x*(p_.upper()-p_.lower()) + p_.lower();
	}

	friend bool operator==(const beta_distribution& lhs, const beta_distribution& rhs)
	{
		return lhs.p_ == rhs.p_;
	}

	friend bool operator!=(const beta_distribution& lhs, const beta_distribution& rhs)
	{
		return !(lhs == rhs);
	}

private:
	param_type p_;
    boost::random::beta_distribution<T> beta01_;
}; // beta_distribution

}} // Namespace dcs::fog

#endif // DCS_FOG_RANDOM_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Roshan <thisisroshansmail@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestStablePartition
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/stable_partition.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(partition_int)
{
    int dataset[] = {1, 1, -2, 0, 5, -1, 2, 4, 0, -1};
    bc::vector<bc::int_> vector(dataset, dataset + 10, queue);

    bc::vector<bc::int_>::iterator iter =
        bc::stable_partition(vector.begin(), vector.begin() + 10,
                             bc::_1 > 0, queue);

    CHECK_RANGE_EQUAL(int, 10, vector, (1, 1, 5, 2, 4, -2, 0, -1, 0, -1));
    BOOST_VERIFY(iter == vector.begin()+5);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Louis Dionne 2013-2017
// 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/bool.hpp>
#include <boost/hana/find_if.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/tuple.hpp>

struct is_last {
    template <typename N>
    constexpr auto operator()(N) const {
        return boost::hana::bool_c<N::value == <%= input_size %>>;
    }
};

int main() {
    constexpr auto tuple = boost::hana::make_tuple(
        <%= (1..input_size).map { |n| "boost::hana::int_c<#{n}>" }.join(', ') %>
    );
    constexpr auto result = boost::hana::find_if(tuple, is_last{});
    (void)result;
}


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <iostream>
#include <graphblas/graphblas.hpp>

using namespace grb;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE views_test_suite

#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxm)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double, DirectedMatrixTag> mA(A, 0);

    std::vector<std::vector<double> > B = {{5, 8, 1, 2},
                                           {6, 7, 3, 0},
                                           {4, 5, 9, 1}};
    Matrix<double, DirectedMatrixTag> mB(B, 0);

    Matrix<double, DirectedMatrixTag> result(3, 4);

    std::vector<std::vector<double> > ans = {{114, 160,  60,  27},
                                             { 74,  97,  73,  14},
                                             {119, 157, 112,  23}};
    Matrix<double, DirectedMatrixTag> answer(ans, 0);

    mxm(result,
        grb::NoMask(), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxm_masked)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double, DirectedMatrixTag> mA(A, 0);

    std::vector<std::vector<double> > B = {{5, 8, 1, 2},
                                           {6, 7, 3, 0},
                                           {4, 5, 9, 1}};
    Matrix<double, DirectedMatrixTag> mB(B, 0);

    Matrix<double, DirectedMatrixTag> result(3, 4);

    std::vector<std::vector<double> > ans = {{114,   0,   0,   0},
                                             { 74,  97,   0,   0},
                                             {119, 157, 112,   0}};
    Matrix<double, DirectedMatrixTag> answer(ans, 0);

    std::vector<std::vector<uint8_t> > M = {{1, 0, 0, 0},
                                            {1, 1, 0, 0},
                                            {1, 1, 1, 0}};
    Matrix<uint8_t, DirectedMatrixTag> mask(M, 0);

    mxm(result,
        mask, grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxm_complemented_mask)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double, DirectedMatrixTag> mA(A, 0);

    std::vector<std::vector<double> > B = {{5, 8, 1, 2},
                                           {6, 7, 3, 0},
                                           {4, 5, 9, 1}};
    Matrix<double, DirectedMatrixTag> mB(B, 0);

    Matrix<double, DirectedMatrixTag> result(3, 4);

    std::vector<std::vector<double> > ans = {{114,   0,   0,   0},
                                             { 74,  97,   0,   0},
                                             {119, 157, 112,   0}};
    Matrix<double, DirectedMatrixTag> answer(ans, 0);

    std::vector<std::vector<uint8_t> > M = {{0, 1, 1, 1},
                                            {0, 0, 1, 1},
                                            {0, 0, 0, 1}};
    Matrix<uint8_t, DirectedMatrixTag> mask(M, 0);

    mxm(result,
        grb::complement(mask), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxm_structure_mask)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double, DirectedMatrixTag> mA(A, 0);

    std::vector<std::vector<double> > B = {{5, 8, 1, 2},
                                           {6, 7, 3, 0},
                                           {4, 5, 9, 1}};
    Matrix<double, DirectedMatrixTag> mB(B, 0);

    Matrix<double, DirectedMatrixTag> result(3, 4);

    std::vector<std::vector<double> > ans = {{114,   0,   0,   0},
                                             { 74,  97,   0,   0},
                                             {119, 157, 112,   0}};
    Matrix<double, DirectedMatrixTag> answer(ans, 0);

    std::vector<std::vector<uint8_t> > M = {{1,99,99,99},
                                            {0, 1,99,99},
                                            {0, 0, 1,99}};
    Matrix<uint8_t, DirectedMatrixTag> mask(M, 99);

    mxm(result,
        grb::structure(mask), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxm_complemented_structure_mask)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double, DirectedMatrixTag> mA(A, 0);

    std::vector<std::vector<double> > B = {{5, 8, 1, 2},
                                           {6, 7, 3, 0},
                                           {4, 5, 9, 1}};
    Matrix<double, DirectedMatrixTag> mB(B, 0);

    Matrix<double, DirectedMatrixTag> result(3, 4);

    std::vector<std::vector<double> > ans = {{114,   0,   0,   0},
                                             { 74,  97,   0,   0},
                                             {119, 157, 112,   0}};
    Matrix<double, DirectedMatrixTag> answer(ans, 0);

    std::vector<std::vector<uint8_t> > M = {{99, 1, 0, 1},
                                            {99,99, 0, 1},
                                            {99,99,99, 0}};
    Matrix<uint8_t, DirectedMatrixTag> mask(M, 99);

    mxm(result,
        grb::complement(grb::structure(mask)),
        grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxm_a_transpose)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double, DirectedMatrixTag> mA(A, 0);

    std::vector<std::vector<double> > B = {{5, 8, 1, 2},
                                           {6, 7, 3, 0},
                                           {4, 5, 9, 1}};
    Matrix<double, DirectedMatrixTag> mB(B, 0);

    Matrix<double, DirectedMatrixTag> result(3, 4);

    IndexArrayType i_answer = {0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2};
    IndexArrayType j_answer = {0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3};
    std::vector<double> v_answer = {112, 159, 87, 31, 97, 131,
                                    94, 22, 87, 111, 102, 15};
    Matrix<double, DirectedMatrixTag> answer(3, 4);
    answer.build(i_answer, j_answer, v_answer);

    mxm(result,
        grb::NoMask(), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        transpose(mA), mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxm_b_transpose)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double, DirectedMatrixTag> mA(A, 0);

    std::vector<std::vector<double> > B = {{5, 8, 1},
                                           {2, 6, 7},
                                           {3, 4, 5}};
    Matrix<double, DirectedMatrixTag> mB(B, 0);

    Matrix<double, DirectedMatrixTag> result(3, 3);

    IndexArrayType i_answer = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_answer = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_answer = {119, 87, 79, 66, 80, 62, 108, 125, 98};
    Matrix<double, DirectedMatrixTag> answer(3, 3);
    answer.build(i_answer, j_answer, v_answer);

    mxm(result,
        grb::NoMask(), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, transpose(mB));

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxm_a_and_b_transpose)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double, DirectedMatrixTag> mA(A, 0);

    std::vector<std::vector<double> > B = {{5, 8, 1},
                                           {2, 6, 7},
                                           {3, 4, 5}};
    Matrix<double, DirectedMatrixTag> mB(B, 0);

    Matrix<double, DirectedMatrixTag> result(3, 3);

    IndexArrayType i_answer = {0, 0, 0, 1, 1, 1, 2, 2, 2};
    IndexArrayType j_answer = {0, 1, 2, 0, 1, 2, 0, 1, 2};
    std::vector<double> v_answer = {99, 97, 87, 83, 100, 81, 72, 105, 78};
    Matrix<double, DirectedMatrixTag> answer(3, 3);
    answer.build(i_answer, j_answer, v_answer);

    mxm(result,
        grb::NoMask(), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        transpose(mA), transpose(mB));

    BOOST_CHECK_EQUAL(result, answer);
}

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

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double> mA(A, 0);

    std::vector<double> B = {5, 6, 4};
    Vector<double> mB(B, 0);

    Vector<double> result(3);

    std::vector<double> ans = {114,  74, 119};
    Vector<double> answer(ans, 0);

    mxv(result,
        grb::NoMask(), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_masked)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double> mA(A, 0);

    std::vector<double> B = {5, 6, 4};
    Vector<double> mB(B, 0);

    Vector<double> result(3);

    std::vector<double> ans = {114,  74, 0};
    Vector<double> answer(ans, 0);

    std::vector<uint8_t> M = {1, 1, 0};
    Vector<uint8_t> mask(M, 0);

    mxv(result,
        mask, grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_complemented_mask)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double> mA(A, 0);

    std::vector<double> B = {5, 6, 4};
    Vector<double> mB(B, 0);

    Vector<double> result(3);

    std::vector<double> ans = {114,  74, 0};
    Vector<double> answer(ans, 0);

    std::vector<uint8_t> M = {0, 0, 1};
    Vector<uint8_t> mask(M, 0);

    mxv(result,
        grb::complement(mask), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_structure_mask)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double> mA(A, 0);

    std::vector<double> B = {5, 6, 4};
    Vector<double> mB(B, 0);

    Vector<double> result(3);

    std::vector<double> ans = {114,  74, 0};
    Vector<double> answer(ans, 0);

    std::vector<uint8_t> M = {1, 0, 99};
    Vector<uint8_t> mask(M, 99);

    mxv(result,
        grb::structure(mask), grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_complemented_structure_mask)
{
    std::vector<std::vector<double> > A ={{12, 7, 3},
                                          { 4, 5, 6},
                                          { 7, 8, 9}};
    Matrix<double> mA(A, 0);

    std::vector<double> B = {5, 6, 4};
    Vector<double> mB(B, 0);

    Vector<double> result(3);

    std::vector<double> ans = {114,  74, 0};
    Vector<double> answer(ans, 0);

    std::vector<uint8_t> M = {99, 99, 0};
    Vector<uint8_t> mask(M, 99);

    mxv(result,
        grb::complement(grb::structure(mask)),
        grb::NoAccumulate(),
        grb::ArithmeticSemiring<double>(),
        mA, mB);

    BOOST_CHECK_EQUAL(result, answer);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Louis Dionne 2013-2017
// 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/ext/boost/tuple.hpp>

#include <boost/hana/bool.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/monad_plus.hpp>

#include <boost/tuple/tuple.hpp>
namespace hana = boost::hana;

template <int i>
using eq = hana::test::ct_eq<i>;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Setup for the laws below
    //////////////////////////////////////////////////////////////////////////
    auto eq_tuples = hana::make_tuple(
          ::boost::make_tuple(eq<0>{})
        , ::boost::make_tuple(eq<0>{}, eq<1>{})
        , ::boost::make_tuple(eq<0>{}, eq<1>{}, eq<2>{})
    );

    auto eq_values = hana::make_tuple(eq<0>{}, eq<1>{}, eq<2>{});
    auto predicates = hana::make_tuple(
        hana::equal.to(eq<0>{}), hana::equal.to(eq<1>{}), hana::equal.to(eq<2>{}),
        hana::always(hana::false_c), hana::always(hana::true_c)
    );

    //////////////////////////////////////////////////////////////////////////
    // MonadPlus
    //////////////////////////////////////////////////////////////////////////
    hana::test::TestMonadPlus<hana::ext::boost::tuple_tag>{eq_tuples, predicates, eq_values};
}


//
// Copyright 2020 Debabrata Mandal <mandaldebabrata123@gmail.com>
//
// 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/gil/histogram.hpp>

#include <boost/core/lightweight_test.hpp>

#include <string>

namespace gil = boost::gil;

void check_sub_histogram_without_tuple()
{
    gil::histogram<int, int, std::string, int> h;
    h(1, 1, "A", 1) = 1;
    h(1, 2, "B", 1) = 1;
    h(2, 1, "C", 1) = 1;
    h(2, 1, "D", 1) = 1;
    h(2, 3, "E", 4) = 1;
    auto h1 = h.sub_histogram<0,3>();
    BOOST_TEST(h1(1, 1) == 2);
    BOOST_TEST(h1(2, 1) == 2);
    BOOST_TEST(h1(2, 4) == 1);
    BOOST_TEST(h1(5, 5) == 0);
}

void check_sub_histogram_with_tuple()
{
    gil::histogram<int, int, std::string, int> h;
    h(1, 1, "A", 1) = 3;
    h(1, 2, "C", 1) = 1;
    h(2, 1, "C", 1) = 1;
    h(2, 1, "A", 1) = 1;
    h(2, 3, "E", 4) = 1;
    h(1, 3, "A", 1) = 2;
    std::tuple<double, int, std::string, int> t(1.0, 1000, "A", 1);
    // This means 1st dimension is useless for matching.
    auto h1 = h.sub_histogram<0,2,3>(t, t);
    BOOST_TEST(h1(1, 1, "A", 1) == 3);
    BOOST_TEST(h1(1, 2, "C", 1) == 0);
    BOOST_TEST(h1(2, 1, "C", 1) == 0);
    BOOST_TEST(h1(2, 1, "A", 1) == 0);
    BOOST_TEST(h1(2, 1, "A", 1) == 0);
    BOOST_TEST(h1(1, 3, "A", 1) == 2);
}

int main() {

    check_sub_histogram_without_tuple();
    check_sub_histogram_with_tuple();

    return boost::report_errors();
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file subset_iterator_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-02-04
 */

#include "paal/data_structures/subset_iterator.hpp"

#include <boost/test/unit_test.hpp>

#include <vector>
#include <string>

using std::vector;

template <typename T> void print_tuple3(T t) {
    LOGLN(std::get<0>(t) << "," << std::get<1>(t) << "," << std::get<2>(t));
}

typedef std::vector<int>::iterator It;
vector<int> v{ 1, 2, 3, 4, 5, 6, 7 };

BOOST_AUTO_TEST_CASE(subset_iterator_engine_test) {
    auto engine = paal::data_structures::make_subsets_iterator_engine<3>(
        v.begin(), v.begin() + 4);

    BOOST_CHECK(engine.call(paal::make_tuple{}) == std::make_tuple(1, 2, 3));
    BOOST_CHECK(engine.next());
    BOOST_CHECK(engine.call(paal::make_tuple{}) == std::make_tuple(1, 2, 4));
    BOOST_CHECK(engine.next());
    BOOST_CHECK(engine.call(paal::make_tuple{}) == std::make_tuple(1, 3, 4));
    BOOST_CHECK(engine.next());
    BOOST_CHECK(engine.call(paal::make_tuple{}) == std::make_tuple(2, 3, 4));
    BOOST_CHECK(!engine.next());
}

BOOST_AUTO_TEST_CASE(subset_iterator_test) {
    auto r = paal::data_structures::make_subsets_iterator_range<3>(v.begin(),
                                                                   v.end());
    typedef std::tuple<int, int, int> tup;
    BOOST_CHECK_EQUAL(std::get<0>(*std::begin(r)), 1);
    BOOST_CHECK_EQUAL(std::get<1>(*std::begin(r)), 2);
    BOOST_CHECK_EQUAL(std::get<2>(*std::begin(r)), 3);
    BOOST_CHECK_EQUAL(boost::distance(r), 7 * 6 * 5 / 6);
}

void test_dist(int i, int res) {
    auto r = paal::data_structures::make_subsets_iterator_range<3>(
        v.begin(), v.begin() + i);
    BOOST_CHECK_EQUAL(boost::distance(r), res);
}

BOOST_AUTO_TEST_CASE(subset_iterator_corner_test) {
    test_dist(6, 6 * 5 * 4 / 6);
    test_dist(5, 5 * 4 * 3 / 6);
    test_dist(4, 4);
    test_dist(3, 1);
    test_dist(2, 0);
    test_dist(1, 0);
    test_dist(0, 0);
}


#include "hsm/hsm.h"

#include <gtest/gtest.h>

#include <boost/hana.hpp>

using namespace ::testing;

class IndexMapTests : public Test {
};

class S1 {
};
class S2 {
};

TEST_F(IndexMapTests, should_make_index_map)
{
    auto T1 = boost::hana::typeid_(S1 {});
    auto T2 = boost::hana::typeid_(S2 {});

    auto tuple = boost::hana::make_tuple(T1, T2);
    auto indexMap = hsm::make_index_map(tuple);

    ASSERT_EQ(boost::hana::int_c<0>, boost::hana::find(indexMap, T1).value());
    ASSERT_EQ(boost::hana::int_c<1>, boost::hana::find(indexMap, T2).value());
}


#include "drake/common/is_approx_equal_abstol.h"

#include <Eigen/Dense>
#include <gtest/gtest.h>

using Eigen::MatrixXd;
using Eigen::VectorXd;

namespace drake {
namespace {

GTEST_TEST(IsApproxEqualMatrixTest, BasicTest) {
  const VectorXd ones1 = VectorXd::Ones(1);
  const VectorXd ones2 = VectorXd::Ones(2);
  const MatrixXd id22 = MatrixXd::Identity(2, 2);
  const MatrixXd id33 = MatrixXd::Identity(3, 3);

  const double lit_nudge = 1e-12;
  const double big_nudge = 1e-4;
  const VectorXd ones1_lit = ones1 + VectorXd::Constant(1, lit_nudge);
  const VectorXd ones1_big = ones1 + VectorXd::Constant(1, big_nudge);
  const VectorXd ones2_lit = ones2 + VectorXd::Constant(2, lit_nudge);
  const VectorXd ones2_big = ones2 + VectorXd::Constant(2, big_nudge);
  const MatrixXd id22_lit = id22 + MatrixXd::Constant(2, 2, lit_nudge);
  const MatrixXd id22_big = id22 + MatrixXd::Constant(2, 2, big_nudge);
  const MatrixXd id33_lit = id33 + MatrixXd::Constant(3, 3, lit_nudge);
  const MatrixXd id33_big = id33 + MatrixXd::Constant(3, 3, big_nudge);

  // Compare same-size matrices, within tolerances.
  const double tolerance = 1e-8;
  EXPECT_TRUE(is_approx_equal_abstol(ones1, ones1, tolerance));
  EXPECT_TRUE(is_approx_equal_abstol(ones1, ones1_lit, tolerance));
  EXPECT_FALSE(is_approx_equal_abstol(ones1, ones1_big, tolerance));
  EXPECT_TRUE(is_approx_equal_abstol(ones2, ones2, tolerance));
  EXPECT_TRUE(is_approx_equal_abstol(ones2, ones2_lit, tolerance));
  EXPECT_FALSE(is_approx_equal_abstol(ones2, ones2_big, tolerance));
  EXPECT_TRUE(is_approx_equal_abstol(id22, id22, tolerance));
  EXPECT_TRUE(is_approx_equal_abstol(id22, id22_lit, tolerance));
  EXPECT_FALSE(is_approx_equal_abstol(id22, id22_big, tolerance));
  EXPECT_TRUE(is_approx_equal_abstol(id33, id33, tolerance));
  EXPECT_TRUE(is_approx_equal_abstol(id33, id33_lit, tolerance));
  EXPECT_FALSE(is_approx_equal_abstol(id33, id33_big, tolerance));

  // Compare different-size matrices.
  EXPECT_FALSE(is_approx_equal_abstol(ones1, ones2, tolerance));
  EXPECT_FALSE(is_approx_equal_abstol(id22, id33, tolerance));

  // Special values do not compare equal.
  const double inf = std::numeric_limits<double>::infinity();
  const VectorXd inf2 = VectorXd::Constant(2, inf);
  EXPECT_FALSE(is_approx_equal_abstol(inf2, inf2, tolerance));
  EXPECT_FALSE(is_approx_equal_abstol(inf2, ones2, tolerance));
  const double nan = std::numeric_limits<double>::quiet_NaN();
  const VectorXd nan2 = VectorXd::Constant(2, nan);
  EXPECT_FALSE(is_approx_equal_abstol(nan2, nan2, tolerance));
  EXPECT_FALSE(is_approx_equal_abstol(nan2, inf2, tolerance));
  EXPECT_FALSE(is_approx_equal_abstol(nan2, ones2, tolerance));
}

GTEST_TEST(IsApproxEqualAbstolPermutationTest, PermutationTest) {
  MatrixXd test(2, 3);
  // clang-format off
  test << 1, 2, 3,
          4, 5, 6;
  // clang-format on

  const double tol = 1e-8;
  EXPECT_TRUE(IsApproxEqualAbsTolWithPermutedColumns(test, test, tol));
  EXPECT_FALSE(
      IsApproxEqualAbsTolWithPermutedColumns(test, test.leftCols<2>(), tol));
  EXPECT_FALSE(
      IsApproxEqualAbsTolWithPermutedColumns(test.leftCols<2>(), test, tol));

  MatrixXd test2(2, 3);

  // Switch cols 2 and 3.
  // clang-format off
  test2 << 1, 3, 2,
           4, 6, 5;
  // clang-format on
  EXPECT_TRUE(IsApproxEqualAbsTolWithPermutedColumns(test, test2, tol));

  // All columns in test2 are in test1, but one is repeated.
  // clang-format off
  test2 << 1, 1, 2,
           4, 4, 5;
  // clang-format on
  EXPECT_FALSE(IsApproxEqualAbsTolWithPermutedColumns(test, test2, tol));
  EXPECT_FALSE(IsApproxEqualAbsTolWithPermutedColumns(test2, test, tol));

  // Matching but with one duplicated columns.
  test2.resize(2, 4);
  // clang-format off
  test2 << 1, 1, 2, 3,
           4, 4, 5, 6;
  // clang-format on
  EXPECT_FALSE(IsApproxEqualAbsTolWithPermutedColumns(test, test2, tol));
  EXPECT_FALSE(IsApproxEqualAbsTolWithPermutedColumns(test2, test, tol));
}

}  // namespace
}  // namespace drake


#include <igl/read_triangle_mesh.h>
#include <igl/copyleft/cgal/CSGTree.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/jet.h>
#include <Eigen/Core>

#include "tutorial_shared_path.h"

int main(int argc, char * argv[])
{
  using namespace Eigen;
  using namespace igl::copyleft::cgal;
  using namespace std;
  using namespace igl;
  cout<<R"(
[,]  Toggle between boolean sub-tree operations
)";

  MatrixXi FA,FB,FC,FD,FE;
  MatrixXd VA,VB,VC,VD,VE;
  // Read in inputs as double precision floating point meshes
  read_triangle_mesh(TUTORIAL_SHARED_PATH "/cube.obj"     ,VA,FA);
  read_triangle_mesh(TUTORIAL_SHARED_PATH "/sphere.obj"   ,VB,FB);
  read_triangle_mesh(TUTORIAL_SHARED_PATH "/xcylinder.obj",VC,FC);
  read_triangle_mesh(TUTORIAL_SHARED_PATH "/ycylinder.obj",VD,FD);
  read_triangle_mesh(TUTORIAL_SHARED_PATH "/zcylinder.obj",VE,FE);
  igl::opengl::glfw::Viewer viewer;

  int num_views = 5+4;
  int view_id = num_views-1;
  const auto & update = [&]()
  {
    viewer.data().clear();
    // CSGTree templated on type of F
    VectorXd I;
    const auto & set_mesh =
      [&](const MatrixXd & V, const MatrixXi & F, const int i)
    {
      viewer.data().set_mesh(V,F);
      I = VectorXd::Constant(F.rows(),1,i);
    };
    switch(view_id)
    {
      case 0:
        set_mesh(VA,FA,5);
        break;
      case 1:
        set_mesh(VB,FB,4);
        break;
      case 2:
        set_mesh(VC,FC,3);
        break;
      case 3:
        set_mesh(VD,FD,2);
        break;
      case 4:
        set_mesh(VE,FE,1);
        break;
      default:
      {
        CSGTree M;
        Matrix<MatrixXi::Index,Dynamic,1> J;
        switch(view_id)
        {
          case 5:
            // Compute result of (A ∩ B)
            M = {{VA,FA},{VB,FB},"i"};
            J = M.J().array()+0;
            break;
          case 6:
            // Compute result of (C ∪ D)
            M = {{VC,FC},{VD,FD},"u"};
            J = M.J().array()+FA.rows()+FB.rows();
            break;
          case 7:
            // Compute result of (C ∪ D) ∪ E
            M = {{{VC,FC},{VD,FD},"u"},{VE,FE},"u"};
            J = M.J().array()+FA.rows()+FB.rows();
            break;
          case 8:
            // Compute result of (A ∩ B) \ ((C ∪ D) ∪ E)
            M = {{{VA,FA},{VB,FB},"i"},{{{VC,FC},{VD,FD},"u"},{VE,FE},"u"},"m"};
            J = M.J().array()+0;
            break;
          default:
            assert(false && "unknown view id");
        }
        viewer.data().set_mesh(M.cast_V<MatrixXd>(),M.F());
        I.resize(M.F().rows(),1);
        // Compute colors based on original facets
        for(int f = 0;f<M.F().rows();f++)
        {
          const int j = J(f);
          I(f) =
            (int)(j<FA.rows())+
            (int)(j<FA.rows()+FB.rows())+
            (int)(j<FA.rows()+FB.rows()+FC.rows())+
            (int)(j<FA.rows()+FB.rows()+FC.rows()+FD.rows())+
            (int)(j<FA.rows()+FB.rows()+FC.rows()+FD.rows()+FE.rows());
        }
      }
    }

    MatrixXd C;
    jet(I,1,5,C);
    viewer.data().set_colors(C);
  };
  update();

  viewer.callback_key_down =
    [&](igl::opengl::glfw::Viewer &viewer, unsigned char key, int mods)->bool
    {
      switch(key)
      {
        case ']':
          view_id = (view_id+1)%num_views;
          break;
        case '[':
          view_id = (view_id+num_views-1)%num_views;
          break;
        default:
          return false;
      }
      update();
      return true;
    };
  viewer.launch();
}


#pragma once

#include <limits>

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

namespace bold
{
  class AgentPosition
  {
  public:
    AgentPosition()
    : d_x(std::numeric_limits<double>::signaling_NaN()),
      d_y(std::numeric_limits<double>::signaling_NaN()),
      d_theta(std::numeric_limits<double>::signaling_NaN())
    {}

    AgentPosition(double x, double y, double theta)
      : d_x(x),
        d_y(y),
        d_theta(theta)
    {}

    AgentPosition(Eigen::Vector3d const& vec)
      : d_x(vec.x()),
        d_y(vec.y()),
        d_theta(vec(2))
    {}

    AgentPosition(Eigen::Vector4d const& vec)
      : d_x(vec.x()),
        d_y(vec.y()),
        d_theta(atan2(-vec(2), vec(3)))
    {}

    double x() const { return d_x; }
    double y() const { return d_y; }
    double theta() const { return d_theta; }

    Eigen::Affine3d worldAgentTransform() const;
    Eigen::Affine3d agentWorldTransform() const;

    Eigen::Vector2d pos2d() const { return Eigen::Vector2d(d_x, d_y); }
    Eigen::Vector3d pos3d(double z = 0) const { return Eigen::Vector3d(d_x, d_y, z); }

    AgentPosition withX(double x) const { return AgentPosition(x, d_y, d_theta); }
    AgentPosition withY(double y) const { return AgentPosition(d_x, y, d_theta); }
    AgentPosition withTheta(double theta) const { return AgentPosition(d_x, d_y, theta); }

  private:
    double d_x;
    double d_y;
    double d_theta;
  };
}