#include <ros/ros.h>

#include <string>
#include <fstream>

// ros includes
#include <ros/ros.h>
#include <ros/package.h>
#include <ros/console.h>
#include <std_srvs/Trigger.h>

// ros actionlib includes
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>

// intrinsic_cal includes
#include <industrial_extrinsic_cal/cal_srv_solve.h>

// industrial_extrinsic_cal includes
#include <industrial_extrinsic_cal/basic_types.h>
#include <industrial_extrinsic_cal/camera_observer.hpp>
#include <industrial_extrinsic_cal/camera_definition.h>
#include <industrial_extrinsic_cal/observation_scene.h>
#include <industrial_extrinsic_cal/observation_data_point.h>
#include <industrial_extrinsic_cal/ceres_blocks.h>
#include <industrial_extrinsic_cal/ros_camera_observer.h>
#include <industrial_extrinsic_cal/ceres_costs_utils.hpp>
#include <industrial_extrinsic_cal/ceres_costs_utils.h>
#include <industrial_extrinsic_cal/circle_cost_utils.hpp>
#include <industrial_extrinsic_cal/calibration_job_definition.h>
#include <industrial_extrinsic_cal/calibrationAction.h>
#include <industrial_extrinsic_cal/calibrate.h>
#include <industrial_extrinsic_cal/covariance.h>
#include <industrial_extrinsic_cal/ros_target_display.hpp>
#include <industrial_extrinsic_cal/camera_yaml_parser.h>
#include <industrial_extrinsic_cal/targets_yaml_parser.h>

// boost includes
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>

using boost::shared_ptr;
using ceres::Solver;
using ceres::CostFunction;
using industrial_extrinsic_cal::Camera;
using industrial_extrinsic_cal::Target;
using industrial_extrinsic_cal::CeresBlocks;
using industrial_extrinsic_cal::Roi;
using industrial_extrinsic_cal::CameraObservations;
using industrial_extrinsic_cal::Point3d;
using industrial_extrinsic_cal::Pose6d;
using industrial_extrinsic_cal::P_BLOCK;
using std::string;

class icalServiceNode
{
public:
  icalServiceNode(const  ros::NodeHandle& nh)
    :nh_(nh), P_(NULL), problem_initialized_(false), total_observations_(0), scene_(0)
  {
    string nn = ros::this_node::getName();
    ros::NodeHandle priv_nh("~");

    // load cameras and targets
    priv_nh.getParam("yaml_file_path", yaml_file_path_);
    string camera_file, target_file;
    priv_nh.getParam("camera_file", camera_file);
    priv_nh.getParam("target_file", target_file);
    priv_nh.getParam("save_data", save_data_);
    camera_file_ = yaml_file_path_ + camera_file ;
    target_file_ = yaml_file_path_ + target_file ;
    ROS_INFO("yaml_file_path: %s", yaml_file_path_.c_str());
    ROS_INFO("camera_file: %s",  camera_file_.c_str());
    ROS_INFO("target_file: %s",  target_file_.c_str());
    if(save_data_) {
      ROS_INFO("saving data");
    }
    else{
      ROS_INFO("NOT saving data");
    }
    if(!load_camera()){
      ROS_ERROR("can't load the camera from %s", (yaml_file_path_+camera_file_).c_str());
      exit(1);
    }

    if(!load_target()){
      ROS_ERROR("can't load the target from %s", (yaml_file_path_+target_file_).c_str());
      exit(1);
    }

    // set all the data directories to the first camera's image_directory_
    for(int i=1; i<all_cameras_.size(); i++){
      all_cameras_[i]->camera_observer_->set_image_directory(all_cameras_[0]->camera_observer_->get_image_directory());
    }
    for(int i=0; i<all_targets_.size(); i++){
      all_targets_[i]->transform_interface_->setDataDirectory(all_cameras_[0]->camera_observer_->get_image_directory());
    }
    ROS_ERROR("image_directory = %s", all_cameras_[0]->camera_observer_->get_image_directory().c_str());

    // load cameras, targets and intiialize ceres blocks
    init_blocks();

    // advertise services
    start_server_       = nh_.advertiseService( "ICalSrvStart",       &icalServiceNode::startCallBack, this);
    observation_server_ = nh_.advertiseService( "ICalSrvObs",         &icalServiceNode::observationCallBack, this);
    run_server_         = nh_.advertiseService( "ICalSrvRun",         &icalServiceNode::runCallBack, this);
    save_server_        = nh_.advertiseService( "ICalSrvSave",        &icalServiceNode::saveCallBack, this);
    load_server_        = nh_.advertiseService( "StereoCalSrvImport", &icalServiceNode::loadCallBack, this);
    covariance_server_  = nh_.advertiseService( "ICalSrvCov",         &icalServiceNode::covCallBack, this);

  };// end of constructor

  void init_blocks()
  {
    // adds the intial block for each camera and target
    for (int i = 0; i < (int)all_cameras_.size(); i++)
      {
	if (all_cameras_[i]->is_moving_)
	  {
	    scene_ = 0;
	    ceres_blocks_.addMovingCamera(all_cameras_[i], scene_);
	  }
	else
	  {
	    ceres_blocks_.addStaticCamera(all_cameras_[i]);
	  }
      }

    for (int i = 0; i < (int)all_targets_.size(); i++)
      {
	if (all_targets_[i]->pub_rviz_vis_){ // use rviz visualization marker to display the target, currently must be modified circle grid
	  displayRvizTarget(all_targets_[i]);
	}
	if (all_targets_[i]->is_moving_)
	  {
	    scene_ = 0;
	    ceres_blocks_.addMovingTarget(all_targets_[i], scene_);
	  }
	else
	  {
	    ceres_blocks_.addStaticTarget(all_targets_[i]);
	  }
      }  // end for every target found

    // if loaded camera is the right side of a stereo pair, it contains a pointer to the left one.
    for (int i = 0; i < (int)all_cameras_.size(); i++)
      {
	if(all_cameras_[i]->is_right_stereo_camera_)
	  {
	    all_cameras_[i]->left_stereo_camera_ = ceres_blocks_.getCameraByName(all_cameras_[i]->left_stereo_camera_name_);
	  }
      }

    // set reference frame for all transform interfaces
    ceres_blocks_.setReferenceFrame(all_cameras_[0]->transform_interface_->getTransformFrame());
  }; // end init_blocks()

  // read the camera yaml file
  bool load_camera()
  {
    bool rtn = true;
    if (!parseCameras(camera_file_, all_cameras_))
      {
	ROS_ERROR("failed to parse cameras from %s", camera_file_.c_str());
	rtn = false;
      }
    return rtn;
  };// end of load_camera()

  // read the target yaml file
  bool load_target()
  {
    bool rtn = true;

    if (!parseTargets(target_file_, all_targets_))
      {
	ROS_ERROR("failed to parse targets from %s", target_file_.c_str());
	rtn = false;
      }
    return rtn;
  }; // end load_target()

  // callback functions
  bool startCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    if(problem_initialized_)	delete(P_);
    P_ = new ceres::Problem;
    problem_initialized_ = true;
    total_observations_  = 0;
    scene_=0;
    ceres_blocks_.clearCamerasTargets();
    init_blocks();
    res.message = string("Intrinsic calibration service started");
    res.success = true;
    return(true);
  }

  // called to collect observations for the current pose of the scene
  bool observationCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    char msg[100];
    string null_string("");
    if(problem_initialized_ != true ){
      ROS_INFO("calling start");
      std_srvs::TriggerRequest  sreq;
      std_srvs::TriggerResponse sres;
      startCallBack(sreq,sres);
    }

    for(int i=0; i<all_targets_.size(); i++){
      all_targets_[i]->pullTransform();
      if(save_data_){
	all_targets_[i]->transform_interface_->saveCurrentPose(scene_, null_string);
      }
    }

    for(int i=0; i<all_cameras_.size(); i++){
      // set the roi to the whole image
      Roi roi;
      roi.x_min = 0;
      roi.y_min = 0;
      roi.x_max = all_cameras_[i]->camera_parameters_.width;
      roi.y_max = all_cameras_[i]->camera_parameters_.height;

      // get observations
      all_cameras_[i]->camera_observer_->clearTargets();
      all_cameras_[i]->camera_observer_->clearObservations();
      industrial_extrinsic_cal::Cost_function cost_type = industrial_extrinsic_cal::cost_functions::CameraReprjErrorWithDistortionPK;
      int total_pts=0;
      for(int j=0;j<all_targets_.size();j++){ // add all targets to the camera
	all_cameras_[i]->camera_observer_->addTarget(all_targets_[j], roi, cost_type);
	total_pts += all_targets_[j]->num_points_;
      }
      all_cameras_[i]->camera_observer_->triggerCamera();

      if(save_data_){
	all_cameras_[i]->camera_observer_->save_current_image(scene_, null_string);
      }

      while (!all_cameras_[i]->camera_observer_->observationsDone());
      CameraObservations camera_observations;
      all_cameras_[i]->camera_observer_->getObservations(camera_observations);
      int num_observations = (int)camera_observations.size();
      ROS_INFO("Found %d observations", (int)camera_observations.size());

      // add observations to problem
      num_observations = (int)camera_observations.size();
      if (num_observations != total_pts)
	{
	  ROS_ERROR("Target Locator could not find all targets found %d out of %d", num_observations, total_pts);
	}
      else
	{	 // add a new cost to the problem for each observation
	  CostFunction* cost_function[num_observations];
	  total_observations_ += num_observations;
	  ceres_blocks_.addMovingTarget(camera_observations[0].target, scene_);
	  for (int k = 0; k < num_observations; k++)
	    {
	      shared_ptr<Target> target = camera_observations[k].target;
	      double image_x = camera_observations[k].image_loc_x;
	      double image_y = camera_observations[k].image_loc_y;
	      Point3d point  = target->pts_[k];
	      P_BLOCK intrinsics = ceres_blocks_.getStaticCameraParameterBlockIntrinsics(all_cameras_[i]->camera_name_);
	      P_BLOCK target_pb  = ceres_blocks_.getMovingTargetPoseParameterBlock(target->target_name_,scene_);
	      if(k==0){
		Pose6d P(target_pb[3],target_pb[4],target_pb[5],target_pb[0],target_pb[1],target_pb[2]);
		P.show("Pose of Target");
	      }
	      cost_function[k] = industrial_extrinsic_cal::CameraReprjErrorWithDistortionPK::Create(image_x, image_y, point);
	      P_->AddResidualBlock(cost_function[k], NULL, target_pb, intrinsics);
	    }  // for each observation at this camera_location
	} // end of else (there are some observations to add)
    }// for each camera

    ROS_INFO("now have %d observations after scene %d",total_observations_, scene_);
    scene_++;

    sprintf(msg, "Ical_srv now has %d observations after scene %d",total_observations_, scene_);
    res.message = string(msg);
    res.success = true;
    return(true);

  }; // end observation service};

  /**
   * \brief Load and set the calibration data from a previous job.
   */
  bool loadCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    string null_string("");
    if(problem_initialized_ != true ) // scene_id=0, problem re-initialized
      {
	ROS_INFO("calling start");
	std_srvs::TriggerRequest  sreq;
	std_srvs::TriggerResponse sres;
	startCallBack(sreq,sres);
      }

    // for each scene data includes
    //   a. image from each camera
    //   b. pose of each target
    // Note that the target's transform interface should provide the pose between the target and the camera that observes it
    // usually there is only one camera, and one target

    bool data_read_ok=true;
    while(data_read_ok){ //read all the scene data from image_directory
      for(int i=0; i<all_cameras_.size(); i++){
	data_read_ok &= all_cameras_[0]->camera_observer_->load_current_image(scene_, null_string);
      }
      for(int i=0; i<all_targets_.size(); i++){
	data_read_ok &= all_targets_[0]->transform_interface_->loadPose(scene_, null_string);
      }

      scene_++;

      if(data_read_ok)
	{
	  for(int i=0; i<all_cameras_.size(); i++){
	    // add the target to each camera's observer
	    Roi roi;
	    roi.x_min = 0;
	    roi.y_min = 0;

	    roi.x_max = all_cameras_[i]->camera_parameters_.width;
	    roi.y_max = all_cameras_[i]->camera_parameters_.height;
	    all_cameras_[i]->camera_observer_->clearTargets();
	    all_cameras_[i]->camera_observer_->clearObservations();
	    for(int j=0; j<all_targets_.size(); j++){
	      all_cameras_[i]->camera_observer_->addTarget(all_targets_[j], roi, industrial_extrinsic_cal::cost_functions::CameraReprjErrorWithDistortionPK);
	    }

	    // wait for observers to do its work
	    while (!all_cameras_[i]->camera_observer_->observationsDone());

	    // get the observations from left and right cameras
	    CameraObservations camera_observations;
	    all_cameras_[i]->camera_observer_->getObservations(camera_observations);

	    // only add if there are enough observations in both
	    int num_observations = (int)camera_observations.size();

	    if(camera_observations.size() == all_targets_[0]->num_points_);
	    ROS_INFO("Found %d camera observations", (int)camera_observations.size());

	    // add the moving target to the blocks
	    shared_ptr<Target> target = camera_observations[0].target;
	    ceres_blocks_.addMovingTarget(all_targets_[0], scene_);
	    P_BLOCK intrinsics  = ceres_blocks_.getStaticCameraParameterBlockIntrinsics(all_cameras_[i]->camera_name_);
	    P_BLOCK target_pb   = ceres_blocks_.getMovingTargetPoseParameterBlock(target->target_name_,scene_);

	    CostFunction* cost_function[num_observations];
	    for (int k = 0; k < num_observations; k++)
	      {
		shared_ptr<Target> target = camera_observations[k].target;
		double image_x = camera_observations[k].image_loc_x;
		double image_y = camera_observations[k].image_loc_y;
		Point3d point  = target->pts_[k];
		P_BLOCK intrinsics = ceres_blocks_.getStaticCameraParameterBlockIntrinsics(all_cameras_[i]->camera_name_);
		P_BLOCK target_pb  = ceres_blocks_.getMovingTargetPoseParameterBlock(target->target_name_,scene_);
		if(k==0){
		  ROS_ERROR("target_pb = %ld intrinsics = %ld",(long int ) &(target_pb[0]), (long int ) &intrinsics[0]);
		  Pose6d P(target_pb[3],target_pb[4],target_pb[5],target_pb[0],target_pb[1],target_pb[2]);
		  P.show("Pose of Target");
		}
		cost_function[k] = industrial_extrinsic_cal::CameraReprjErrorWithDistortionPK::Create(image_x, image_y, point);
		P_->AddResidualBlock(cost_function[k], NULL, target_pb, intrinsics);
	      }  // for each observation at this camera_location
	  }// end for each camera
	}// end if data_read_ok
    }
  }

  bool covCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    string covariance_file_name("/home/clewis/junk.txt");
    if(!computeCovariance(covariance_file_name)){
      ROS_ERROR("could not compute covariance");
      res.success = false;
      res.message = "failure";
    }
    else{
      res.message = "good job";
      res.success = true;
    }
    return(true);
  };

  bool computeCovariance(string& covariance_file_name)
  {
    FILE* fp;
    if ((fp = fopen(covariance_file_name.c_str(), "w")) != NULL)
    {
      ceres::Covariance::Options covariance_options;
      covariance_options.algorithm_type = ceres::DENSE_SVD;
      ceres::Covariance covariance(covariance_options);

      P_BLOCK extrinsics = ceres_blocks_.getStaticCameraParameterBlockExtrinsics(all_cameras_[1]->camera_name_);

      std::vector<std::pair<const double*, const double*> > covariance_pairs;
      covariance_pairs.push_back(std::make_pair(extrinsics, extrinsics));

      if(covariance.Compute(covariance_pairs, P_))
      {
        fprintf(fp, "stereo covariance block:\n");
        double cov[6*6];
        if(covariance.GetCovarianceBlock(extrinsics, extrinsics, cov)){
          fprintf(fp, "cov is 6x6\n");
          for(int i=0;i<6;i++){
            double sigma_i = sqrt(fabs(cov[i*6+i]));
            for(int j=0;j<6;j++){
              double sigma_j = sqrt(fabs(cov[j*6+j]));
              double value;
              if(i==j){
                value = sigma_i;
              }
              else{
                if(sigma_i==0) sigma_i = 1;
                if(sigma_j==0) sigma_j = 1;
                value = cov[i * 6 + j]/(sigma_i*sigma_j);
              }
              fprintf(fp, "%16.5f ", value);
            }  // end of j loop
            fprintf(fp, "\n");
          }  // end of i loop
        }// end if success getting covariance
        fclose(fp);
        return(true);
      }// end if covariances could be computed
      else{
        ROS_ERROR("could not compute covariance");
      }
    }// end if file opens
    ROS_ERROR("could not open covariance file %s", covariance_file_name.c_str());
    return (false);
  };  // end computeCovariance()

  bool runCallBack( industrial_extrinsic_cal::cal_srv_solveRequest &req, industrial_extrinsic_cal::cal_srv_solveResponse &res)
  {
    char msg[100];
    ROS_INFO("initial values");
    for(int i=0; i<all_cameras_.size(); i++){
      ROS_INFO("camera_matrix data: [ %lf, 0.0, %lf, 0.0, %lf, %lf, 0.0, 0.0, 1.0]",
	       all_cameras_[i]->camera_parameters_.focal_length_x, all_cameras_[i]->camera_parameters_.center_x,
	       all_cameras_[i]->camera_parameters_.focal_length_y, all_cameras_[i]->camera_parameters_.center_y);
      ROS_INFO("distortion data: [ %lf,  %lf,  %lf,  %lf,  %lf]", all_cameras_[i]->camera_parameters_.distortion_k1,
	       all_cameras_[i]->camera_parameters_.distortion_k2, all_cameras_[i]->camera_parameters_.distortion_p1,
	       all_cameras_[i]->camera_parameters_.distortion_p2, all_cameras_[i]->camera_parameters_.distortion_k3);
    }

    // check for obvious errors
    if(problem_initialized_==false){
      ROS_ERROR("must call start service");
      sprintf(msg, "must call start service to initialized ical service");
      res.message = string(msg);
      res.success = false;
      return(true);
    }
    if(total_observations_ == 0){
      ROS_ERROR("must call observations service at least once");
      sprintf(msg, "must call observations service at least once");
      res.message = string(msg);
      res.success = false;
      return(true);
    }

    Solver::Options options;
    Solver::Summary summary;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 1000;
    ceres::Solve(options, P_, &summary);
    if (summary.termination_type != ceres::NO_CONVERGENCE)
      {
	double initial_cost = summary.initial_cost / total_observations_;
	double final_cost = sqrt( 2*summary.final_cost   / total_observations_);
	res.final_cost_per_observation = final_cost;

	ROS_INFO("Problem solved, initial cost = %lf, final cost = %lf", initial_cost, final_cost);
	for(int i=0; i<all_cameras_.size(); i++){
	  ROS_INFO("camera_matrix data: [ %lf, 0.0, %lf, 0.0, %lf, %lf, 0.0, 0.0, 1.0]",
		   all_cameras_[i]->camera_parameters_.focal_length_x, all_cameras_[i]->camera_parameters_.center_x,
		   all_cameras_[i]->camera_parameters_.focal_length_y, all_cameras_[i]->camera_parameters_.center_y);
	  ROS_INFO("distortion data: [ %lf,  %lf,  %lf,  %lf,  %lf]", all_cameras_[i]->camera_parameters_.distortion_k1,
		   all_cameras_[i]->camera_parameters_.distortion_k2, all_cameras_[i]->camera_parameters_.distortion_p1,
		   all_cameras_[i]->camera_parameters_.distortion_p2, all_cameras_[i]->camera_parameters_.distortion_k3);
	  ROS_INFO("projection_matrix data: [ %lf, 0.0, %lf, 0.0, 0.0, %lf, %lf, 0.0, 0.0, 0.0, 1.0, 0.0]",
		   all_cameras_[i]->camera_parameters_.focal_length_x, all_cameras_[i]->camera_parameters_.center_x,
		   all_cameras_[i]->camera_parameters_.focal_length_y, all_cameras_[i]->camera_parameters_.center_y);
	}
	if (final_cost <= req.allowable_cost_per_observation)
	  {
	    ROS_INFO("calibration was successful");
	  }
	else
	  {
	    ROS_ERROR("allowable cost exceeded %f > %f", final_cost, req.allowable_cost_per_observation);
	    sprintf(msg, "allowable cost exceeded %f > %f", final_cost, req.allowable_cost_per_observation);
	    res.message = string(msg);
	    res.success = false;
	    return(true);
	  }
	sprintf(msg, "final cost %f", final_cost);
	res.message = string(msg);
	res.success = true;
	return(true);
      }
    ROS_ERROR("NO CONVERGENCE");
    sprintf(msg, "NO CONVERGENCE");
    res.message = string(msg);
    res.success = false;
    return(true);
  }; // end runCallBack()

  bool saveCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    char msg[100];
    ROS_ERROR("inside saveCallBack()");
    for(int i=0; i<all_cameras_.size(); i++){
      all_cameras_[i]->camera_observer_->pushCameraInfo(
							all_cameras_[i]->camera_parameters_.focal_length_x, all_cameras_[i]->camera_parameters_.focal_length_y,
							all_cameras_[i]->camera_parameters_.center_x, all_cameras_[i]->camera_parameters_.center_y,
							all_cameras_[i]->camera_parameters_.distortion_k1, all_cameras_[i]->camera_parameters_.distortion_k2,
							all_cameras_[i]->camera_parameters_.distortion_k3, all_cameras_[i]->camera_parameters_.distortion_p1,
							all_cameras_[i]->camera_parameters_.distortion_p2);
    }
    sprintf(msg, "intrinsics: %8.2lf %8.2lf %8.2lf %8.2lf, %5.3lf %5.3lf %5.3lf, %5.3lf %5.3lf",
	    all_cameras_[0]->camera_parameters_.focal_length_x, all_cameras_[0]->camera_parameters_.focal_length_y,
	    all_cameras_[0]->camera_parameters_.center_x, all_cameras_[0]->camera_parameters_.center_y,
	    all_cameras_[0]->camera_parameters_.distortion_k1, all_cameras_[0]->camera_parameters_.distortion_k2,
	    all_cameras_[0]->camera_parameters_.distortion_k3, all_cameras_[0]->camera_parameters_.distortion_p1,
	    all_cameras_[0]->camera_parameters_.distortion_p2);

    res.message = string(msg);
    res.success = true;
    return(true);

  }// end saveCallback()

private:
  ros::NodeHandle nh_;
  std::vector<shared_ptr<Camera> > all_cameras_;
  std::vector<shared_ptr<Target> > all_targets_;
  string yaml_file_path_;
  string camera_file_;
  string target_file_;
  CeresBlocks ceres_blocks_;                 /*!< This structure maintains the parameter sets for ceres */
  ros::ServiceServer start_server_;
  ros::ServiceServer observation_server_;
  ros::ServiceServer run_server_;
  ros::ServiceServer save_server_;
  ros::ServiceServer load_server_;
  ros::ServiceServer covariance_server_;
  ceres::Problem *P_;
  bool problem_initialized_;
  int total_observations_;
  int scene_;
  bool save_data_;
  string image_directory_;
};// end of class icalServiceNode

int main(int argc, char** argv)
{
  ros::init(argc, argv, "ical_cal_service");
  ros::NodeHandle node_handle;
  icalServiceNode ISN(node_handle);
  ros::spin();
  ros::waitForShutdown();
  return 0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_ARCH_COMMON_SIMD_FUNCTION_SIGNNZ_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SIGNNZ_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/function/simd/bitwise_and.hpp>
#include <boost/simd/function/simd/bitwise_or.hpp>
#include <boost/simd/function/simd/shift_right.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/simd/if_allbits_else.hpp>
#include <boost/simd/function/simd/is_nan.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(signnz_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::integer_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using sA0 = bd::scalar_of_t<A0>;
        return bitwise_or(shift_right(a0, (sizeof(sA0)*8-1)), One<A0>());
      }
   };

   BOOST_DISPATCH_OVERLOAD(signnz_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::unsigned_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(const A0&) const
      {
        return One<A0>();
      }
   };

   BOOST_DISPATCH_OVERLOAD(signnz_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        #ifndef BOOST_SIMD_NO_NANS
        return if_allbits_else(is_nan(a0), bitwise_or(One<A0>(), bitwise_and(Signmask<A0>(), a0)));
        #else
        return bitwise_or(One<A0>(), bitwise_and(Signmask<A0>(), a0));
        #endif
        ;
      }
   };

} } }

#endif


/******************************************************************
 *
 * uEcho for C
 *
 * Copyright (C) Satoshi Konno 2015
 *
 * This is licensed under BSD-style license, see file COPYING.
 *
 ******************************************************************/

#include <boost/test/unit_test.hpp>

#include <uecho/misc.h>

BOOST_AUTO_TEST_CASE(ObjCode2ClsCode)
{
  BOOST_CHECK_EQUAL(uecho_objectcode2classcode(0x0EF001), 0x0EF0);
  BOOST_CHECK_EQUAL(uecho_objectcode2classcode(0x001102), 0x0011);
  BOOST_CHECK_EQUAL(uecho_objectcode2classcode(0x001102), 0x0011);
}

BOOST_AUTO_TEST_CASE(Byte2Integer)
{
  byte int_bytes[4];

  for (int n = 0; n <= 0xFF; n++) {
    uecho_integer2byte(n, int_bytes, 1);
    BOOST_CHECK_EQUAL(n, uecho_byte2integer(int_bytes, 1));
  }

  for (int n = 0; n <= 0xFFFF; n += (0xFFFF / 0xFF)) {
    uecho_integer2byte(n, int_bytes, 2);
    BOOST_CHECK_EQUAL(n, uecho_byte2integer(int_bytes, 2));
  }

  for (int n = 0; n <= 0xFFFFFF; n += (0xFFFFFF / 0xFF)) {
    uecho_integer2byte(n, int_bytes, 3);
    BOOST_CHECK_EQUAL(n, uecho_byte2integer(int_bytes, 3));
  }

  for (int n = 0; n < 0xFFFFFFFF; n += (0xFFFFFFFF / 0xFF)) {
    uecho_integer2byte(n, int_bytes, 4);
    BOOST_CHECK_EQUAL(n, uecho_byte2integer(int_bytes, 4));
  }
}

BOOST_AUTO_TEST_CASE(ClassGroupCode)
{
  for (int n = uEchoClassGroupDeviceMin; n <= uEchoClassGroupDeviceMax; n++) {
    BOOST_CHECK(uecho_isdeviceclassgroupcode(n));
    BOOST_CHECK(!uecho_isprofileclassgroupcode(n));
  }

  BOOST_CHECK(!uecho_isdeviceclassgroupcode(uEchoClassGroupProfile));
  BOOST_CHECK(uecho_isprofileclassgroupcode(uEchoClassGroupProfile));
}


/*
Copyright 2018 Sahaj Software Solutions, Inc.

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.
*/

#include <boost/python.hpp>
#include <Eigen/Dense>

using Eigen::MatrixXd;

double greet(int N)
{
  MatrixXd m = MatrixXd::Random(N,N);
  return m.norm();
}

BOOST_PYTHON_MODULE(hello)
{
    using namespace boost::python;
    def("greet", greet);
}


#pragma once
#include "std140.hpp"

#include <glm/glm.hpp>

#include <boost/fusion/include/for_each.hpp>
#include <boost/range/counting_range.hpp>

#include <vector>

struct std140_data
{
    std::vector<char> data;
    size_t align;
};

inline std140_data to_std140(bool v)
{
    std140_bool sv = v;
    auto* p = reinterpret_cast<char*>(&sv);
    return {std::vector<char>(p, p + sizeof(sv)), sizeof(sv)};
}

inline std140_data to_std140(int v)
{
    std140_int sv = v;
    auto* p = reinterpret_cast<char*>(&sv);
    return {std::vector<char>(p, p + sizeof(sv)), sizeof(sv)};
}

inline std140_data to_std140(float v)
{
    std140_float sv = v;
    auto* p = reinterpret_cast<char*>(&sv);
    return {std::vector<char>(p, p + sizeof(sv)), sizeof(sv)};
}

namespace glm
{
    inline std140_data to_std140(vec2 v)
    {
        std140_vector<float, 2> sv;
        for (auto i : boost::counting_range(0, v.length()))
            sv[i] = v[i];
        auto* p = reinterpret_cast<char*>(&sv);
        return {std::vector<char>(p, p + sizeof(sv)), sv.align};
    }

    inline std140_data to_std140(vec3 v)
    {
        std140_vector<float, 3> sv;
        for (auto i : boost::counting_range(0, v.length()))
            sv[i] = v[i];
        auto* p = reinterpret_cast<char*>(&sv);
        return {std::vector<char>(p, p + sizeof(sv)), sv.align};
    }

    inline std140_data to_std140(vec4 v)
    {
        std140_vector<float, 4> sv;
        for (auto i : boost::counting_range(0, v.length()))
            sv[i] = v[i];
        auto* p = reinterpret_cast<char*>(&sv);
        return {std::vector<char>(p, p + sizeof(sv)), sv.align};
    }

    inline std140_data to_std140(mat3 m)
    {
        std140_matrix<float, 3, 3> sm;
        for (auto i : boost::counting_range(0, m.length()))
            for (auto j : boost::counting_range(0, m[0].length()))
                sm[i][j] = m[i][j];
        auto* p = reinterpret_cast<char*>(&sm);
        return {std::vector<char>(p, p + sizeof(sm)), sm.align};
    }

    inline std140_data to_std140(mat4 m)
    {
        std140_matrix<float, 4, 4> sm;
        for (auto i : boost::counting_range(0, m.length()))
            for (auto j : boost::counting_range(0, m[0].length()))
                sm[i][j] = m[i][j];
        auto* p = reinterpret_cast<char*>(&sm);
        return {std::vector<char>(p, p + sizeof(sm)), sm.align};
    }
}

template<typename struct_t>
inline std140_data to_std140(struct_t the_struct)
{
    std::vector<char> result;
    boost::fusion::for_each(
        the_struct,
        [&](auto v){
            auto data = to_std140(v);
            size_t aligned_size = roundedup(result.size(), data.align);
            if (aligned_size > result.size())
                result.resize(aligned_size);
            result.insert(result.end(), data.data.begin(), data.data.end());
        }
    );
    return {result, 16};
}


// Copyright 2020 Peter Dimov
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt

#if !defined(__GNUC__)

#include <boost/config/pragma_message.hpp>
BOOST_PRAGMA_MESSAGE( "Skipping test because __GNUC__ is not defined" )
int main() {}

#else

#define BOOST_ENDIAN_FORCE_PODNESS
#define BOOST_ENDIAN_NO_CTORS
#include <boost/endian/arithmetic.hpp>
#include <boost/core/lightweight_test.hpp>

using namespace boost::endian;

struct X
{
    big_uint16_t a;
    native_float64_t b;
    little_uint16_t c;
} __attribute__((packed));

int main()
{
    BOOST_TEST_EQ( sizeof(big_uint16_t), 2 );
    BOOST_TEST_EQ( sizeof(native_float64_t), 8 );
    BOOST_TEST_EQ( sizeof(little_uint16_t), 2 );

    BOOST_TEST_EQ( sizeof(X), 12 );

    return boost::report_errors();
}

#endif


#include <boost/function_output_iterator.hpp>
#include <boost/geometry/geometry.hpp>
#include <chrono>
#include <iostream>
#include <random>
#include <vector>
#include <string>
#include <fstream>

// using cxxopts for CLI argument parsing
#include "packages/cxxopts.hpp"

// using prakhar1989's ProgressBar for CLI progress indicator
#include "packages/ProgressBar.hpp"

// using tinyobjloader for OBJ file parsing
#define TINYOBJLOADER_IMPLEMENTATION
#define EPSILON 0.00000001
#include "packages/tiny_obj_loader.h"

using namespace std;

// full 3D model to inhibit growth
string fullModelFilename = "";
tinyobj::attrib_t fullModelAttrib;
vector<tinyobj::shape_t> fullModelShapes;
vector<tinyobj::material_t> fullModelMaterials;

// 3D model of selected faces to seed growth
string seedModelFilename = "";
tinyobj::attrib_t seedModelAttrib;
vector<tinyobj::shape_t> seedModelShapes;
vector<tinyobj::material_t> seedModelMaterials;

// number of particles
const int DefaultNumberOfParticles = 1000000;
int numParticles = DefaultNumberOfParticles;

// progress bar
ProgressBar progressBar(numParticles, 40);

// output file
string filename = "points";
ofstream file;

// interval (in iterations) that point data is outputted
int lastOutputIteration = 0;
int interval = numParticles;

// number of dimensions (must be 2 or 3)
const int D = 3;

// default parameters (documented below)
const double DefaultParticleSpacing = 1;
const double DefaultAttractionDistance = 3;
const double DefaultMinMoveDistance = 1;
const int DefaultStubbornness = 0;
const double DefaultStickiness = 1;
const double DefaultBoundingRadius = 0;

// boost is used for its spatial index
using BoostPoint = boost::geometry::model::point<double, D, boost::geometry::cs::cartesian>;
using IndexValue = pair<BoostPoint, int>;
using Index = boost::geometry::index::rtree<IndexValue, boost::geometry::index::linear<4>>;

// Vector represents a point or a vector
class Vector {
public:
    Vector() :
        m_X(0), m_Y(0), m_Z(0) {}

    Vector(double x, double y) :
        m_X(x), m_Y(y), m_Z(0) {}

    Vector(double x, double y, double z) :
        m_X(x), m_Y(y), m_Z(z) {}

    double X() const {
        return m_X;
    }

    double Y() const {
        return m_Y;
    }

    double Z() const {
        return m_Z;
    }

    BoostPoint ToBoost() const {
        return BoostPoint(m_X, m_Y, m_Z);
    }

    double Length() const {
        return sqrt(m_X * m_X + m_Y * m_Y + m_Z * m_Z);
    }

    double LengthSquared() const {
        return m_X * m_X + m_Y * m_Y + m_Z * m_Z;
    }

    double Distance(const Vector &v) const {
        const double dx = m_X - v.m_X;
        const double dy = m_Y - v.m_Y;
        const double dz = m_Z - v.m_Z;
        return sqrt(dx * dx + dy * dy + dz * dz);
    }

    Vector Normalized() const {
        const double m = 1 / Length();
        return Vector(m_X * m, m_Y * m, m_Z * m);
    }

    Vector GetCrossed(const Vector &v) const {
        return Vector(m_Y * v.m_Z - m_Z * v.m_Y,
                      m_Z * v.m_X - m_X * v.m_Z,
                      m_X * v.m_Y - m_Y * v.m_X);
    }

    double GetDot(const Vector &v) const {
        return m_X * v.m_X +
               m_Y * v.m_Y +
               m_Z * v.m_Z;
    }

    Vector operator+(const Vector &v) const {
        return Vector(m_X + v.m_X, m_Y + v.m_Y, m_Z + v.m_Z);
    }

    Vector operator-(const Vector &v) const {
        return Vector(m_X - v.m_X, m_Y - v.m_Y, m_Z - v.m_Z);
    }

    Vector operator*(const double a) const {
        return Vector(m_X * a, m_Y * a, m_Z * a);
    }

    Vector &operator+=(const Vector &v) {
        m_X += v.m_X; m_Y += v.m_Y; m_Z += v.m_Z;
        return *this;
    }

    bool operator==(const Vector &v) const {
        return m_X == v.m_X && m_Y == v.m_Y && m_Z == v.m_Z;
    }

private:
    double m_X;
    double m_Y;
    double m_Z;
};

// Lerp linearly interpolates from a to b by distance.
Vector Lerp(const Vector &a, const Vector &b, const double d) {
    return a + (b - a).Normalized() * d;
}

// Random returns a uniformly distributed random number between lo and hi
double Random(const double lo = 0, const double hi = 1) {
    static thread_local mt19937 gen(chrono::high_resolution_clock::now().time_since_epoch().count());
    uniform_real_distribution<double> dist(lo, hi);
    return dist(gen);
}

// RandomInUnitSphere returns a random, uniformly distributed point inside the
// unit sphere (radius = 1)
Vector RandomInUnitSphere() {
    while (true) {
        const Vector p = Vector(
            Random(-1, 1),
            Random(-1, 1),
            D == 2 ? 0 : Random(-1, 1));
        if (p.LengthSquared() < 1) {
            return p;
        }
    }
}

// IsIntersectingFace checks for intersection of a ray (O) with random direction (D) and a triangle face defined by vertices V1, V2, and V3
// Adapted from Amnon Owed's ofxPointInMesh::triangleIntersection: https://github.com/AmnonOwed/ofxPointInMesh
// Uses Möller–Trumbore: http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
bool IsIntersectingFace(const Vector &V1, const Vector &V2, const Vector &V3, const Vector &O, const Vector &D, Vector &R) {
	Vector e1, e2; // Edge1, Edge2
	Vector P, Q, T;
	float det, inv_det, u, v;
	float t;

	// Find vectors for two edges sharing V1
	e1 = V2 - V1;
	e2 = V3 - V1;

	// Begin calculating determinant - also used to calculate u parameter
	P = D.GetCrossed(e2);

	// if determinant is near zero, ray lies in plane of triangle
	det = e1.GetDot(P);

	// NOT CULLING
	if(det > -EPSILON && det < EPSILON){
		return false;
	}

	inv_det = 1.f / det;

	// calculate distance from V1 to ray origin
	T = O - V1;

	// calculate u parameter and test bound
	u = T.GetDot(P) * inv_det;

	// the intersection lies outside of the triangle
	if(u < 0.f || u > 1.f){
		return false;
	}

	// prepare to test v parameter
	Q = T.GetCrossed(e1);

	// calculate V parameter and test bound
	v = D.GetDot(Q) * inv_det;

	// the intersection lies outside of the triangle
	if(v < 0.f || u + v  > 1.f){
		return false;
	}

	t = e2.GetDot(Q) * inv_det;

	if(t > EPSILON){ // ray intersection
		R = O + D * t; // store intersection point
		return true;
	}

	// no hit, no win
	return false;
}

// IsInsideMesh determines if a given point is inside of the base model mesh
// Adapted from Amnon Owed's ofxPointInMesh::isInside: https://github.com/AmnonOwed/ofxPointInMesh
bool IsInsideMesh(Vector &p) {
    // if no mesh was provided, skip this test
    if(fullModelFilename.empty()) {
        return false;
    }

    Vector foundIntersection; // variable to store a single found intersection
	vector<Vector> results;  // vector to store all found intersections
	Vector randomDirection = Vector(0.1, 0.2, 0.3); // a random direction

    // go over all the shapes in the mesh
    for (size_t s = 0; s < fullModelShapes.size(); s++) {
        size_t index_offset = 0;

        // go over all the faces in the shape
        for (size_t f = 0; f < fullModelShapes[s].mesh.num_face_vertices.size(); f++) {
            unsigned int fv = fullModelShapes[s].mesh.num_face_vertices[f];
            vector<Vector> vertices;

            // collect the vertices
            for (size_t v = 0; v < fv; v++) {
                tinyobj::index_t idx = fullModelShapes[s].mesh.indices[index_offset + v];
                tinyobj::real_t vx = fullModelAttrib.vertices[3*idx.vertex_index+0];
                tinyobj::real_t vy = fullModelAttrib.vertices[3*idx.vertex_index+1];
                tinyobj::real_t vz = fullModelAttrib.vertices[3*idx.vertex_index+2];
                vertices.push_back(Vector(vx, vy, vz));
            }

            index_offset += fv;

            // do a triangle-ray intersection on each face in the mesh
            // store the intersection (if any) in the variable foundIntersection
            if(IsIntersectingFace(vertices[0], vertices[1], vertices[2], p, randomDirection, foundIntersection)) {
                // store all found intersections
                results.push_back(foundIntersection);
            }
        }
    }

    // handle multiple mesh intersections at the same point (by removing duplicates)
    vector<Vector> unique_results;
    unique_copy(results.begin(), results.end(), back_inserter(unique_results));

    // // determine if the point is inside or outside the mesh, based on the number of unique intersections
    if(unique_results.size() % 2 == 1) {
        return true;
    } else {
        return false;
    }
}

// GetDot2 calculates the dot product of a vector with itself
double GetDot2(const Vector &p) {
    return p.GetDot(p);
}

// GetSign returns -1 for negative numbers, 1 for positive numbers, and 0 for 0
int GetSign(const double &v) {
    int result = 0;
    if(v < 0) {
        result = -1;
    } else if(v > 0) {
        result = 1;
    }
    return result;
}

// clamp ensures that a value (x) is within the range defined by [lower] and [upper]
double clamp(double x, double lower, double upper) {
    return min(upper, max(x, lower));
}

// DistanceToTriangle calculates the shortest distance between a point (p) and a 3D triangle defined by vertices v1, v2, and v3
// Adapted from: https://iquilezles.org/www/articles/triangledistance/triangledistance.htm
double DistanceToTriangle(const Vector &v1, const Vector &v2, const Vector &v3, const Vector &p) {
    Vector v21 = v2 - v1;
    Vector v32 = v3 - v2;
    Vector v13 = v1 - v3;
    Vector p1 = p - v1;
    Vector p2 = p - v2;
    Vector p3 = p - p3;
    Vector nor = v21.GetCrossed(v13);

    return sqrt(
        // inside/outside test
        (GetSign(p1.GetDot(v21.GetCrossed(nor))) +
         GetSign(p2.GetDot(v32.GetCrossed(nor))) +
         GetSign(p3.GetDot(v13.GetCrossed(nor))) < 2.0)
        ?
        // 3 edges
        min(
            min(
                GetDot2(v21 * clamp(p1.GetDot(v21) / GetDot2(v21), 0.0, 1.0) - p1),
                GetDot2(v32 * clamp(p2.GetDot(v32) / GetDot2(v32), 0.0, 1.0) - p2)
            ),
            GetDot2(v13 * clamp(p3.GetDot(v13) / GetDot2(v13), 0.0, 1.0) - p3)
        )
        :
        // 1 face
        p1.GetDot(nor) * p1.GetDot(nor) / GetDot2(nor)
    );
}

// Model holds all of the particles and defines their behavior.
class Model {
public:
    Model() :
        m_ParticleSpacing(DefaultParticleSpacing),
        m_AttractionDistance(DefaultAttractionDistance),
        m_MinMoveDistance(DefaultMinMoveDistance),
        m_Stubbornness(DefaultStubbornness),
        m_Stickiness(DefaultStickiness),
        m_BoundingRadius(DefaultBoundingRadius) {}

    void SetParticleSpacing(const double a) {
        m_ParticleSpacing = a;
    }

    void SetAttractionDistance(const double a) {
        m_AttractionDistance = a;
    }

    void SetMinMoveDistance(const double a) {
        m_MinMoveDistance = a;
    }

    void SetStubbornness(const int a) {
        m_Stubbornness = a;
    }

    void SetStickiness(const double a) {
        m_Stickiness = a;
    }

    void SetBoundingRadius(const double a) {
        m_BoundingRadius = a;
    }

    // Add adds a new particle with the specified parent particle
    void Add(const Vector &p, const int parent = -1) {
        const int id = m_Points.size();
        m_Index.insert(make_pair(p.ToBoost(), id));
        m_Points.push_back(p);
        m_Parents.push_back(parent);
        m_JoinAttempts.push_back(0);
        m_BoundingRadius = max(m_BoundingRadius, p.Length() + m_AttractionDistance);

        // update and display progress bar
        ++progressBar;
        progressBar.display();

        // wrap up the progress bar when last particle is placed
        if(id == numParticles) {
            progressBar.done();
        }
    }

    // Nearest returns the index of the particle nearest the specified point
    int Nearest(const Vector &point) const {
        int result = -1;
        m_Index.query(
            boost::geometry::index::nearest(point.ToBoost(), 1),
            boost::make_function_output_iterator([&result](const auto &value) {
                result = value.second;
            }));
        return result;
    }

    // DistanceToNearestFace calculates the shortest distance from a particle (p) and
    // TODO: if needed, consider putting mesh vertices in spatial index, then doing knn search search within radius defined by largest edge found in mesh (precomputed)
    double DistanceToNearestFace(const Vector &p) {
        double distance = m_AttractionDistance;

        // go over all the shapes in the mesh
        for (size_t s = 0; s < seedModelShapes.size(); s++) {
            size_t index_offset = 0;

            // go over all the faces in the shape
            for (size_t f = 0; f < seedModelShapes[s].mesh.num_face_vertices.size(); f++) {
                unsigned int fv = seedModelShapes[s].mesh.num_face_vertices[f];
                vector<Vector> vertices;

                // collect the vertices
                for (size_t v = 0; v < fv; v++) {
                    tinyobj::index_t idx = seedModelShapes[s].mesh.indices[index_offset + v];
                    tinyobj::real_t vx = seedModelAttrib.vertices[3*idx.vertex_index+0];
                    tinyobj::real_t vy = seedModelAttrib.vertices[3*idx.vertex_index+1];
                    tinyobj::real_t vz = seedModelAttrib.vertices[3*idx.vertex_index+2];
                    vertices.push_back(Vector(vx, vy, vz));
                }

                index_offset += fv;

                // calculate distance from point to face
                distance = min(distance, DistanceToTriangle(vertices[0], vertices[1], vertices[2], p));
            }
        }

        return distance;
    }

    // RandomStartingPosition returns a random point to start a new particle
    Vector RandomStartingPosition() const {
        const double d = m_BoundingRadius;
        return RandomInUnitSphere().Normalized() * d;
    }

    // ShouldReset returns true if the particle has gone too far away and
    // should be reset to a new random starting position
    bool ShouldReset(const Vector &p) const {
        return p.Length() > m_BoundingRadius * 2;
    }

    // ShouldJoin returns true if the point should attach to the specified
    // parent particle. This is only called when the point is already within
    // the required attraction distance.
    bool ShouldJoin(const Vector &p, const int parent) {
        m_JoinAttempts[parent]++;
        if (m_JoinAttempts[parent] < m_Stubbornness) {
            return false;
        }
        return Random() <= m_Stickiness;
    }

    // PlaceParticle computes the final placement of the particle.
    Vector PlaceParticle(const Vector &p, const int parent) const {
        return Lerp(m_Points[parent], p, m_ParticleSpacing);
    }

    // MotionVector returns a vector specifying the direction that the
    // particle should move for one iteration. The distance that it will move
    // is determined by the algorithm.
    Vector MotionVector(const Vector &p) const {
        return RandomInUnitSphere();
    }

    // AddParticle diffuses one new particle and adds it to the model
    void AddParticle() {
        // compute particle starting location
        Vector p = RandomStartingPosition();

        // do the random walk
        while (true) {
            // get distance to nearest other particle
            const int parent = Nearest(p);
            const double d = p.Distance(m_Points[parent]);

            // do not allow particle to stick when its inside of the base model mesh
            if(!IsInsideMesh(p)) {
                // check for particle-particle collisions
                if (d < m_AttractionDistance) {
                    if (!ShouldJoin(p, parent)) {
                        // push particle away a bit
                        p = Lerp(m_Points[parent], p, m_AttractionDistance + m_MinMoveDistance);
                        continue;
                    }

                    // adjust particle position in relation to its parent
                    p = PlaceParticle(p, parent);

                    // add the point
                    Add(p);
                    return;
                }

                // get distance to the nearest seed face
                const double df = DistanceToNearestFace(p);

                // check for particle-face collisions
                if (df < m_AttractionDistance) {
                    Add(p, -1);
                    return;
                }
            }

            // move randomly
            const double m = max(m_MinMoveDistance, d - m_AttractionDistance);
            p += MotionVector(p).Normalized() * m;

            // check if particle is too far away, reset if so
            if (ShouldReset(p)) {
                p = RandomStartingPosition();
            }
        }
    }

    // OutputPointData creates a new point data file and fills it with most current point data
    void OutputPointData(const int iteration) const {
        file.open("data/" + filename + "-" + to_string(iteration) + ".csv");

        for(unsigned int id = 0; id < m_Points.size(); id++) {
            file << id << "," << m_Parents[id] << "," << m_Points[id].X() << "," << m_Points[id].Y() << "," << m_Points[id].Z() << endl;
        }

        file.close();
    }

private:
    // m_ParticleSpacing defines the distance between particles that are
    // joined together
    double m_ParticleSpacing;

    // m_AttractionDistance defines how close together particles must be in
    // order to join together
    double m_AttractionDistance;

    // m_MinMoveDistance defines the minimum distance that a particle will move
    // during its random walk
    double m_MinMoveDistance;

    // m_Stubbornness defines how many interactions must occur before a
    // particle will allow another particle to join to it.
    int m_Stubbornness;

    // m_Stickiness defines the probability that a particle will allow another
    // particle to join to it.
    double m_Stickiness;

    // m_BoundingRadius defines the radius of the bounding sphere that bounds
    // all of the particles
    double m_BoundingRadius;

    // m_Points stores the final particle positions
    vector<Vector> m_Points;

    // m_Parents stores the parent IDs of each clustered particle
    vector<int> m_Parents;

    // m_JoinAttempts tracks how many times other particles have attempted to
    // join with each finalized particle
    vector<int> m_JoinAttempts;

    // m_Index is the spatial index used to accelerate nearest neighbor queries
    Index m_Index;
};

// create the model in global scope so that parseArgs can configure it
Model model;

// Parses CLI arguments and configures simulation with what is passed
void ParseArgs(int argc, char* argv[]) {
    try {
        cxxopts::Options options(argv[0]);

        options
        .allow_unrecognised_options()
        .add_options()
            ("p,particles", "Number of walker particles", cxxopts::value<int>())
            ("i,input", "Full 3D model filename (.obj)", cxxopts::value<string>())
            ("f,faces", "Seed faces 3D model filename (.obj)", cxxopts::value<string>())
            ("o,output", "Point data output filename", cxxopts::value<string>())
            ("n,interval", "Point data capture interval", cxxopts::value<int>())
            ("s,spacing", "Particle spacing", cxxopts::value<double>())
            ("a,attraction", "Attraction distance", cxxopts::value<double>())
            ("m,move", "Minimum move distance", cxxopts::value<double>())
            ("b,stubbornness", "Stubbornness", cxxopts::value<int>())
            ("k,stickiness", "Stickiness", cxxopts::value<double>())
            ("r,radius", "Initial bounding radius", cxxopts::value<double>())
        ;

        auto result = options.parse(argc, argv);

        if(result.count("particles")) {
            numParticles = result["particles"].as<int>();
            interval = numParticles;
            progressBar.setTotal(numParticles);
        }

        if(result.count("input")) {
            fullModelFilename = result["input"].as<string>();

            // throw error when filename doesn't end in `.obj`
            if(fullModelFilename.substr(fullModelFilename.length() - 4, fullModelFilename.length() - 1).compare(".obj") != 0) {
                cerr << "Base model file must be an OBJ file" << endl;
                exit(1);
            }
        }

        if(result.count("faces")) {
            seedModelFilename = result["faces"].as<string>();

            // throw error when filename doesn't end in `.obj`
            if(seedModelFilename.substr(seedModelFilename.length() - 4, seedModelFilename.length() - 1).compare(".obj") != 0) {
                cerr << "Seed model file must be an OBJ file" << endl;
                exit(1);
            }
        }

        if(result.count("output")) {
            filename = result["output"].as<string>();
        }

        if(result.count("interval")) {
            interval = result["interval"].as<int>();
        }

        if(result.count("spacing")) {
            model.SetParticleSpacing(result["spacing"].as<double>());
        }

        if(result.count("attraction")) {
            model.SetAttractionDistance(result["attraction"].as<double>());
        }

        if(result.count("move")) {
            model.SetMinMoveDistance(result["move"].as<double>());
        }

        if(result.count("stubbornness")) {
            model.SetStubbornness(result["stubbornness"].as<int>());
        }

        if(result.count("stickiness")) {
            model.SetStickiness(result["stickiness"].as<double>());
        }

        if(result.count("radius")) {
            model.SetBoundingRadius(result["radius"].as<double>());
        }
    } catch(const cxxopts::OptionException& e) {
        cout << "Error parsing options: " << e.what() << endl;
        exit(1);
    }
}

// LoadFullModel loads the 3D model passed with the `-i` option
void LoadFullModel() {
    string warn;
    string err;

    bool ret = tinyobj::LoadObj(&fullModelAttrib, &fullModelShapes, &fullModelMaterials, &warn, &err, fullModelFilename.c_str());

    if (!warn.empty()) {
        cout << warn << endl;
    }

    if (!err.empty()) {
        cerr << err << endl;
    }

    if (!ret) {
        exit(1);
    }
}

// LoadSeedModel loads the 3D model passed with the `-f` option
void LoadSeedModel() {
    string warn;
    string err;

    bool ret = tinyobj::LoadObj(&seedModelAttrib, &seedModelShapes, &seedModelMaterials, &warn, &err, seedModelFilename.c_str());

    if (!warn.empty()) {
        cout << warn << endl;
    }

    if (!err.empty()) {
        cerr << err << endl;
    }

    if (!ret) {
        exit(1);
    }
}

int main(int argc, char* argv[]) {
    // parse the CLI arguments
    ParseArgs(argc, argv);

    // load the full 3D model for inhibiting growth
    if(!fullModelFilename.empty()) {
        LoadFullModel();
    }

    // load the seed faces 3D model
    if(!seedModelFilename.empty()) {
        LoadSeedModel();
    }

    // add seed point at origin (a single point is necessary for now)
    model.Add(Vector());

    // {
    //     const int n = 3600;
    //     const double r = 1000;
    //     for (int i = 0; i < n; i++) {
    //         const double t = (double)i / n;
    //         const double a = t * 2 * M_PI;
    //         const double x = cos(a) * r;
    //         const double y = sin(a) * r;
    //         model.Add(Vector(x, y, 0));
    //     }
    // }

    // run diffusion-limited aggregation
    for (int i = 1; i <= numParticles; i++) {
        model.AddParticle();

        // output current point data based on interval
        if(i - lastOutputIteration >= interval) {
            model.OutputPointData(i);
            lastOutputIteration = i;
        }
    }

    return 0;
}


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

#include "transparent_arithmetic.hpp"

#include <chrono>
#include <cmath>
#include <random>
#include <sstream>

#include <boost/algorithm/string/join.hpp>
#include <boost/format.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/algorithm_ext/push_back.hpp>

#include <core/utils/range_utils.hpp>
#include <core/utils/timer.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

void generate_transparent_arithmetic_circuit( std::ostream& os,
                                              const properties::ptr& settings,
                                              const properties::ptr& statistics )
{
  /* settings */
  const auto seed            = get( settings, "seed",            static_cast<unsigned>( std::chrono::system_clock::now().time_since_epoch().count() ) );
  const auto bitwidth        = get( settings, "bitwidth",        8u );
  const auto min_words       = get( settings, "min_words",       3u );
  const auto max_words       = get( settings, "max_words",       10u );
  const auto max_fanout      = get( settings, "max_fanout",      4u );
  const auto max_rounds      = get( settings, "max_rounds",      4u );
  const auto operators       = get( settings, "operators",       std::vector<std::string>( {"+", "-", "*", "/"} ) );
  const auto mux_types       = get( settings, "mux_types",       std::string( "MO" ) );
  const auto mux_prob        = get( settings, "mux_prob",        40u );
  const auto new_ctrl_prob   = get( settings, "new_ctrl_prob",   30u );
  const auto word_pattern    = get( settings, "word_pattern",    std::string( "w%d" ) );
  const auto control_pattern = get( settings, "control_pattern", std::string( "c%d" ) );
  const auto module_name     = get( settings, "module_name",     std::string( "trans_arith" ) );

  /* timing */
  properties_timer t( statistics );

  /* random number generation */
  std::default_random_engine generator( seed );
  std::exponential_distribution<> num_fanouts( 1 );

  /* some helper functions */
  const auto get_fanout = [&generator, &num_fanouts, max_fanout]() {
    return std::min( static_cast<unsigned>( ceil( num_fanouts( generator ) ) ), max_fanout );
  };

  const auto dice = [&generator]( unsigned from, unsigned to ) {
    return std::uniform_int_distribution<unsigned>( from, to )( generator );
  };

  const auto pick_two_and_remove = [&generator, &dice]( std::vector<std::string>& words ) {
    const auto first_index = dice( 0u, words.size() - 1u );
    const auto first_word  = words[first_index];
    words.erase( words.begin() + first_index );

    const auto second_index = dice( 0u, words.size() - 1u );
    const auto second_word  = words[second_index];
    words.erase( words.begin() + second_index );

    return std::make_pair( first_word, second_word );
  };

  /* initialize */
  const auto num_words = dice( min_words, max_words );

  auto words = create_name_list( word_pattern, num_words, 1u );
  auto inputs = words;
  auto next_word_id = num_words + 1u;
  std::vector<std::string> new_words, wires, controls;

  /* helper function to create a new word */
  const auto new_word = [&new_words, word_pattern, &next_word_id]() {
    new_words.push_back( boost::str( boost::format( word_pattern ) % next_word_id++ ) );
    return new_words.back();
  };

  const auto get_control = [&controls, new_ctrl_prob, control_pattern, &dice]( bool force_new = false ) {
    if ( force_new || controls.empty() || dice( 0u, 100u ) <= new_ctrl_prob )
    {
      std::string c = boost::str( boost::format( control_pattern ) % ( controls.size() + 1u ) );
      controls.push_back( c );
      return c;
    }
    else
    {
      return controls[dice( 0u, controls.size() - 1u )];
    }
  };

  /* create */
  std::stringstream assigns;
  auto round = 0u;
  while ( round++ < max_rounds && words.size() >= 2u )
  {
    boost::push_back( wires, new_words );
    new_words.clear();

    while ( words.size() >= 2u )
    {
      std::string w1, w2;
      std::tie( w1, w2 ) = pick_two_and_remove( words );

      const auto nw = new_word();

      /* mux or operator */
      if ( dice( 0u, 100u ) <= mux_prob )
      {
        const auto c = get_control();
        const auto mux_type = mux_types[dice( 0, mux_types.size() - 1 )];

        switch ( mux_type )
        {
        case 'M': /* MUX */
          assigns << boost::format( "  assign %s = %s ? %s : %s;" ) % nw % c % w1 % w2 << std::endl;
          break;

        case 'O': /* ONE-HOT */
          {
            /* we need one more control and two more words */
            const auto c2 = get_control( true );
            const auto cw1 = new_word();
            const auto cw2 = new_word();
            const auto nw1 = new_word();
            const auto nw2 = new_word();

            assigns << boost::format( "  assign %s = {%s};" ) % cw1 % boost::join( std::vector<std::string>( bitwidth, c ), "," ) << std::endl;
            assigns << boost::format( "  assign %s = {%s};" ) % cw2 % boost::join( std::vector<std::string>( bitwidth, c2 ), "," ) << std::endl;
            assigns << boost::format( "  assign %s = %s & %s;" ) % nw1 % cw1 % w1 << std::endl;
            assigns << boost::format( "  assign %s = %s & %s;" ) % nw2 % cw2 % w2 << std::endl;
            assigns << boost::format( "  assign %s = %s | %s;" ) % nw % nw1 % nw2 << std::endl;
          }
          break;

        default:
          std::cout << "[e] unsupported mux type " << mux_type << std::endl;
          break;
        }
      }
      else
      {
        const auto op = operators[dice( 0u, operators.size() - 1 )];
        assigns << boost::format( "  assign %s = %s %s %s;" ) % nw % w1 % op % w2 << std::endl;
      }

      /* fanout? */
      for ( auto i = 1u; i < get_fanout(); ++i )
      {
        const auto nw_copy = new_word();

        assigns << boost::format( "  assign %s = %s;" ) % nw_copy % nw << std::endl;
      }
    }

    boost::push_back( words, new_words );
  }

  for ( const auto& w : words )
  {
    const auto it = boost::find( wires, w );
    if ( it == wires.end() ) { continue; }
    wires.erase( it );
  }
  auto outputs = words;

  /* dump file */
  os << "// this file has been generated with CirKit using the command:" << std::endl
     << boost::format( "//   gen_trans_arith --seed %d --bitwidth %d --min_words %d --max_words %d --max_fanout %d --max_rounds %d --operators \"%s\" --mux_types %s --mux_prob %d --new_ctrl_prob %d --word_pattern \"%s\" --control_pattern \"%s\" --module_name \"%s\"" )
        % seed
        % bitwidth
        % min_words
        % max_words
        % max_fanout
        % max_rounds
        % boost::join( operators, " " )
        % mux_types
        % mux_prob
        % new_ctrl_prob
        % word_pattern
        % control_pattern
        % module_name
     << std::endl << std::endl
     << boost::format( "module %s( %s" ) % module_name % boost::join( inputs, ", " );

  if ( !controls.empty() )
  {
    os << boost::format( ", %s" ) % boost::join( controls, ", " );
  }

  os << boost::format( ", %s );" )  % boost::join( outputs, ", " ) << std::endl
     << boost::format( "  input [%d:0] %s;" ) % ( bitwidth - 1 ) % boost::join( inputs, ", " ) << std::endl;

  if ( !controls.empty() )
  {
    os << boost::format( "  input %s;" ) % boost::join( controls, ", " ) << std::endl;
  }
  os << boost::format( "  output [%d:0] %s;" ) % ( bitwidth - 1 ) % boost::join( outputs, ", " ) << std::endl
     << boost::format( "  wire [%d:0] %s;" ) % ( bitwidth - 1 ) % boost::join( wires, ", " ) << std::endl << std::endl
     << assigns.str()
     << "endmodule" << std::endl;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-bitwise
    Function object implementing clz capabilities

    Returns  the bit count of leading zeros.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    as_integer_t<T> r = clz(x);
    @endcode

    @see  ctz, popcnt

  **/
  const boost::dispatch::functor<tag::clz_> clz = {};
} }
#endif

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

#endif


#include <boost/graph/adjacency_list.hpp>


/*

Copyright (c) 2013, Project OSRM, Dennis Luxen, others
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.

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 HOLDER 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.

*/

#include <osrm/Coordinate.h>
#include "../Util/MercatorUtil.h"
#ifndef NDEBUG
#include "../Util/simple_logger.hpp"
#endif
#include "../Util/string_util.hpp"

#include <boost/assert.hpp>

#ifndef NDEBUG
#include <bitset>
#endif
#include <iostream>
#include <limits>

FixedPointCoordinate::FixedPointCoordinate()
    : lat(std::numeric_limits<int>::min()), lon(std::numeric_limits<int>::min())
{
}

FixedPointCoordinate::FixedPointCoordinate(int lat, int lon) : lat(lat), lon(lon)
{
#ifndef NDEBUG
    if (0 != (std::abs(lat) >> 30))
    {
        std::bitset<32> y_coordinate_vector(lat);
        SimpleLogger().Write(logDEBUG) << "broken lat: " << lat
                                       << ", bits: " << y_coordinate_vector;
    }
    if (0 != (std::abs(lon) >> 30))
    {
        std::bitset<32> x_coordinate_vector(lon);
        SimpleLogger().Write(logDEBUG) << "broken lon: " << lon
                                       << ", bits: " << x_coordinate_vector;
    }
#endif
}

void FixedPointCoordinate::Reset()
{
    lat = std::numeric_limits<int>::min();
    lon = std::numeric_limits<int>::min();
}
bool FixedPointCoordinate::isSet() const
{
    return (std::numeric_limits<int>::min() != lat) && (std::numeric_limits<int>::min() != lon);
}
bool FixedPointCoordinate::is_valid() const
{
    if (lat > 90 * COORDINATE_PRECISION || lat < -90 * COORDINATE_PRECISION ||
        lon > 180 * COORDINATE_PRECISION || lon < -180 * COORDINATE_PRECISION)
    {
        return false;
    }
    return true;
}
bool FixedPointCoordinate::operator==(const FixedPointCoordinate &other) const
{
    return lat == other.lat && lon == other.lon;
}

double FixedPointCoordinate::ApproximateDistance(const int lat1,
                                                 const int lon1,
                                                 const int lat2,
                                                 const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
    double RAD = 0.017453292519943295769236907684886;
    double lt1 = lat1 / COORDINATE_PRECISION;
    double ln1 = lon1 / COORDINATE_PRECISION;
    double lt2 = lat2 / COORDINATE_PRECISION;
    double ln2 = lon2 / COORDINATE_PRECISION;
    double dlat1 = lt1 * (RAD);

    double dlong1 = ln1 * (RAD);
    double dlat2 = lt2 * (RAD);
    double dlong2 = ln2 * (RAD);

    double dLong = dlong1 - dlong2;
    double dLat = dlat1 - dlat2;

    double aHarv = pow(sin(dLat / 2.0), 2.0) + cos(dlat1) * cos(dlat2) * pow(sin(dLong / 2.), 2);
    double cHarv = 2. * atan2(sqrt(aHarv), sqrt(1.0 - aHarv));
    // earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
    // The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
    const double earth = 6372797.560856;
    return earth * cHarv;
}

double FixedPointCoordinate::ApproximateDistance(const FixedPointCoordinate &coordinate_1,
                                                 const FixedPointCoordinate &coordinate_2)
{
    return ApproximateDistance(
        coordinate_1.lat, coordinate_1.lon, coordinate_2.lat, coordinate_2.lon);
}

float FixedPointCoordinate::ApproximateEuclideanDistance(const FixedPointCoordinate &coordinate_1,
                                                         const FixedPointCoordinate &coordinate_2)
{
    return ApproximateEuclideanDistance(
        coordinate_1.lat, coordinate_1.lon, coordinate_2.lat, coordinate_2.lon);
}

float FixedPointCoordinate::ApproximateEuclideanDistance(const int lat1,
                                                         const int lon1,
                                                         const int lat2,
                                                         const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());

    const float RAD = 0.017453292519943295769236907684886f;
    const float float_lat1 = (lat1 / COORDINATE_PRECISION) * RAD;
    const float float_lon1 = (lon1 / COORDINATE_PRECISION) * RAD;
    const float float_lat2 = (lat2 / COORDINATE_PRECISION) * RAD;
    const float float_lon2 = (lon2 / COORDINATE_PRECISION) * RAD;

    const float x_value = (float_lon2 - float_lon1) * cos((float_lat1 + float_lat2) / 2.f);
    const float y_value = float_lat2 - float_lat1;
    const float earth_radius = 6372797.560856f;
    return sqrt(x_value * x_value + y_value * y_value) * earth_radius;
}

float
FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &source_coordinate,
                                                   const FixedPointCoordinate &target_coordinate,
                                                   const FixedPointCoordinate &point)
{
    // initialize values
    const float x_value = static_cast<float>(lat2y(point.lat / COORDINATE_PRECISION));
    const float y_value = point.lon / COORDINATE_PRECISION;
    float a = static_cast<float>(lat2y(source_coordinate.lat / COORDINATE_PRECISION));
    float b = source_coordinate.lon / COORDINATE_PRECISION;
    float c = static_cast<float>(lat2y(target_coordinate.lat / COORDINATE_PRECISION));
    float d = target_coordinate.lon / COORDINATE_PRECISION;
    float p, q;
    if (std::abs(a - c) > std::numeric_limits<float>::epsilon())
    {
        const float slope = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x_value + (slope * y_value)) + (slope * slope * a - slope * b)) /
            (1.f + slope * slope);
        q = b + slope * (p - a);
    }
    else
    {
        p = c;
        q = y_value;
    }

    float ratio;
    bool inverse_ratio = false;

    // straight line segment on equator
    if (std::abs(c) < std::numeric_limits<float>::epsilon() &&
        std::abs(a) < std::numeric_limits<float>::epsilon())
    {
        ratio = (q - b) / (d - b);
    }
    else
    {
        if (std::abs(c) < std::numeric_limits<float>::epsilon())
        {
            // swap start/end
            std::swap(a, c);
            std::swap(b, d);
            inverse_ratio = true;
        }

        float nY = (d * p - c * q) / (a * d - b * c);
        // discretize the result to coordinate precision. it's a hack!
        if (std::abs(nY) < (1.f / COORDINATE_PRECISION))
        {
            nY = 0.f;
        }

        // compute ratio
        ratio = (p - nY * a) / c;
    }

    if (std::isnan(ratio))
    {
        ratio = (target_coordinate == point ? 1.f : 0.f);
    }
    else if (std::abs(ratio) <= std::numeric_limits<float>::epsilon())
    {
        ratio = 0.f;
    }
    else if (std::abs(ratio - 1.f) <= std::numeric_limits<float>::epsilon())
    {
        ratio = 1.f;
    }

    // we need to do this, if we switched start/end coordinates
    if (inverse_ratio)
    {
        ratio = 1.0f - ratio;
    }

    // compute the nearest location
    FixedPointCoordinate nearest_location;
    BOOST_ASSERT(!std::isnan(ratio));
    if (ratio <= 0.f)
    { // point is "left" of edge
        nearest_location = source_coordinate;
    }
    else if (ratio >= 1.f)
    { // point is "right" of edge
        nearest_location = target_coordinate;
    }
    else
    { // point lies in between
        nearest_location.lat = static_cast<int>(y2lat(p) * COORDINATE_PRECISION);
        nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION);
    }

    BOOST_ASSERT(nearest_location.is_valid());
    return FixedPointCoordinate::ApproximateEuclideanDistance(point, nearest_location);
}

float FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &segment_source,
                                                         const FixedPointCoordinate &segment_target,
                                                         const FixedPointCoordinate &query_location,
                                                         FixedPointCoordinate &nearest_location,
                                                         float &ratio)
{
    BOOST_ASSERT(query_location.is_valid());

    // initialize values
    const double x = lat2y(query_location.lat / COORDINATE_PRECISION);
    const double y = query_location.lon / COORDINATE_PRECISION;
    const double a = lat2y(segment_source.lat / COORDINATE_PRECISION);
    const double b = segment_source.lon / COORDINATE_PRECISION;
    const double c = lat2y(segment_target.lat / COORDINATE_PRECISION);
    const double d = segment_target.lon / COORDINATE_PRECISION;
    double p, q /*,mX*/, nY;
    if (std::abs(a - c) > std::numeric_limits<double>::epsilon())
    {
        const double m = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m);
        q = b + m * (p - a);
    }
    else
    {
        p = c;
        q = y;
    }
    nY = (d * p - c * q) / (a * d - b * c);

    // discretize the result to coordinate precision. it's a hack!
    if (std::abs(nY) < (1.f / COORDINATE_PRECISION))
    {
        nY = 0.f;
    }

    // compute ratio
    ratio = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need
    // not calculate the explicit values of m an n as we
    // are just interested in the ratio
    if (std::isnan(ratio))
    {
        ratio = (segment_target == query_location ? 1.f : 0.f);
    }
    else if (std::abs(ratio) <= std::numeric_limits<double>::epsilon())
    {
        ratio = 0.f;
    }
    else if (std::abs(ratio - 1.f) <= std::numeric_limits<double>::epsilon())
    {
        ratio = 1.f;
    }

    // compute nearest location
    BOOST_ASSERT(!std::isnan(ratio));
    if (ratio <= 0.f)
    {
        nearest_location = segment_source;
    }
    else if (ratio >= 1.f)
    {
        nearest_location = segment_target;
    }
    else
    {
        // point lies in between
        nearest_location.lat = static_cast<int>(y2lat(p) * COORDINATE_PRECISION);
        nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION);
    }
    BOOST_ASSERT(nearest_location.is_valid());

    const float approximate_distance =
        FixedPointCoordinate::ApproximateEuclideanDistance(query_location, nearest_location);
    BOOST_ASSERT(0. <= approximate_distance);
    return approximate_distance;
}

void FixedPointCoordinate::convertInternalLatLonToString(const int value, std::string &output)
{
    char buffer[12];
    buffer[11] = 0; // zero termination
    output = printInt<11, 6>(buffer, value);
}

void FixedPointCoordinate::convertInternalCoordinateToString(const FixedPointCoordinate &coord,
                                                             std::string &output)
{
    std::string tmp;
    tmp.reserve(23);
    convertInternalLatLonToString(coord.lon, tmp);
    output = tmp;
    output += ",";
    convertInternalLatLonToString(coord.lat, tmp);
    output += tmp;
}

void
FixedPointCoordinate::convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord,
                                                                std::string &output)
{
    std::string tmp;
    tmp.reserve(23);
    convertInternalLatLonToString(coord.lat, tmp);
    output = tmp;
    output += ",";
    convertInternalLatLonToString(coord.lon, tmp);
    output += tmp;
}

void FixedPointCoordinate::Output(std::ostream &out) const
{
    out << "(" << lat / COORDINATE_PRECISION << "," << lon / COORDINATE_PRECISION << ")";
}

float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &first_coordinate,
                                       const FixedPointCoordinate &second_coordinate)
{
    const float lon_diff =
        second_coordinate.lon / COORDINATE_PRECISION - first_coordinate.lon / COORDINATE_PRECISION;
    const float lon_delta = DegreeToRadian(lon_diff);
    const float lat1 = DegreeToRadian(first_coordinate.lat / COORDINATE_PRECISION);
    const float lat2 = DegreeToRadian(second_coordinate.lat / COORDINATE_PRECISION);
    const float y = sin(lon_delta) * cos(lat2);
    const float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon_delta);
    float result = RadianToDegree(std::atan2(y, x));
    while (result < 0.f)
    {
        result += 360.f;
    }

    while (result >= 360.f)
    {
        result -= 360.f;
    }
    return result;
}

float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &other) const
{
    const float lon_delta =
        DegreeToRadian(lon / COORDINATE_PRECISION - other.lon / COORDINATE_PRECISION);
    const float lat1 = DegreeToRadian(other.lat / COORDINATE_PRECISION);
    const float lat2 = DegreeToRadian(lat / COORDINATE_PRECISION);
    const float y_value = std::sin(lon_delta) * std::cos(lat2);
    const float x_value =
        std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
    float result = RadianToDegree(std::atan2(y_value, x_value));

    while (result < 0.f)
    {
        result += 360.f;
    }

    while (result >= 360.f)
    {
        result -= 360.f;
    }
    return result;
}

float FixedPointCoordinate::DegreeToRadian(const float degree)
{
    return degree * (static_cast<float>(M_PI) / 180.f);
}

float FixedPointCoordinate::RadianToDegree(const float radian)
{
    return radian * (180.f * static_cast<float>(M_1_PI));
}

// This distance computation does integer arithmetic only and is a lot faster than
// the other distance function which are numerically correct('ish).
// It preserves some order among the elements that make it useful for certain purposes
int FixedPointCoordinate::OrderedPerpendicularDistanceApproximation(
    const FixedPointCoordinate &input_point,
    const FixedPointCoordinate &segment_source,
    const FixedPointCoordinate &segment_target)
{
    // initialize values
    const float x = static_cast<float>(lat2y(input_point.lat / COORDINATE_PRECISION));
    const float y = input_point.lon / COORDINATE_PRECISION;
    const float a = static_cast<float>(lat2y(segment_source.lat / COORDINATE_PRECISION));
    const float b = segment_source.lon / COORDINATE_PRECISION;
    const float c = static_cast<float>(lat2y(segment_target.lat / COORDINATE_PRECISION));
    const float d = segment_target.lon / COORDINATE_PRECISION;

    float p, q;
    if (a == c)
    {
        p = c;
        q = y;
    }
    else
    {
        const float m = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m);
        q = b + m * (p - a);
    }

    const float nY = (d * p - c * q) / (a * d - b * c);
    float ratio = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need
    // not calculate the explicit values of m an n as we
    // are just interested in the ratio
    if (std::isnan(ratio))
    {
        ratio = (segment_target == input_point) ? 1.f : 0.f;
    }

    // compute target quasi-location
    int dx, dy;
    if (ratio < 0.f)
    {
        dx = input_point.lon - segment_source.lon;
        dy = input_point.lat - segment_source.lat;
    }
    else if (ratio > 1.f)
    {
        dx = input_point.lon - segment_target.lon;
        dy = input_point.lat - segment_target.lat;
    }
    else
    {
        // point lies in between
        dx = input_point.lon - static_cast<int>(q * COORDINATE_PRECISION);
        dy = input_point.lat - static_cast<int>(y2lat(p) * COORDINATE_PRECISION);
    }

    // return an approximation in the plane
    return static_cast<int>(sqrt(dx * dx + dy * dy));
}


#ifndef TRANSMISSION_INTERFACE_EXTENSIONS_SLIDER_CRANK_TRANSMISSION_LOADER_HPP
#define TRANSMISSION_INTERFACE_EXTENSIONS_SLIDER_CRANK_TRANSMISSION_LOADER_HPP

#include <stdexcept>
#include <string>
#include <vector>

#include <ros/console.h>
#include <transmission_interface/transmission_interface_exception.h>
#include <transmission_interface/transmission_loader.h>
#include <transmission_interface_extensions/common_namespaces.hpp>
#include <transmission_interface_extensions/slider_crank_transmission.hpp>

#include <boost/lexical_cast.hpp>

namespace transmission_interface_extensions {

class SliderCrankTransmissionLoader : public ti::TransmissionLoader {
public:
  virtual ti::TransmissionSharedPtr load(const ti::TransmissionInfo &src_info) {
    try {
      SliderCrankTransmissionInfo dst_info;
      parseActuator(src_info, &dst_info);
      parseJoints(src_info, &dst_info);
      return ti::TransmissionSharedPtr(new SliderCrankTransmission(dst_info));
    } catch (const std::exception &ex) {
      ROS_ERROR_STREAM("Exception caught on loading SliderCrankTransmission named "
                       << src_info.name_ << ": " << ex.what());
      return ti::TransmissionSharedPtr();
    }
  }

private:
  //
  // helper functions to parse xml
  //

  // raw values in a xml element

  static double parseOptionalValue(const TiXmlElement &xml, const std::string &child_name,
                                   const double default_value) {
    const TiXmlElement *const child(xml.FirstChildElement(child_name));
    return child ? boost::lexical_cast< double >(child->GetText()) : default_value;
  }

  static double parseRequiredValue(const TiXmlElement &xml, const std::string &child_name) {
    const TiXmlElement *const child(xml.FirstChildElement(child_name));
    if (!child) {
      throw ti::TransmissionInterfaceException("Missing required element <" + child_name + ">");
    }
    return boost::lexical_cast< double >(child->GetText());
  }

  // <actuator> element in xml

  static void parseActuator(const ti::TransmissionInfo &src_info,
                            SliderCrankTransmissionInfo *dst_info) {
    // make sure exactly 1 actuator is given
    if (src_info.actuators_.size() != 1) {
      throw ti::TransmissionInterfaceException(
          "Invalid number of actuators: " +
          boost::lexical_cast< std::string >(src_info.actuators_.size()));
    }

    // parse params from the <actuator> element
    const TiXmlElement actuator_el(loadXmlElement(src_info.actuators_[0].xml_element_));
    dst_info->actuator_reduction = parseOptionalValue(actuator_el, "mechanicalReduction", 1.);
  }

  // <joint> element in xml

  static std::size_t findJointByRole(const std::vector< ti::JointInfo > &joints,
                                     const std::string &role) {
    for (std::size_t i = 0; i < joints.size(); ++i) {
      if (joints[i].role_ == role) {
        return i;
      }
    }
    throw ti::TransmissionInterfaceException("No joint with the role of " + role);
  }

  static void parseJoints(const ti::TransmissionInfo &src_info,
                          SliderCrankTransmissionInfo *dst_info) {
    // makes sure exactly 3 joint are given
    if (src_info.joints_.size() != 3) {
      throw ti::TransmissionInterfaceException(
          "Invalid number of joints: " +
          boost::lexical_cast< std::string >(src_info.joints_.size()));
    }

    // parse params from the <joint> element with "crank" role
    dst_info->crank_joint_id = findJointByRole(src_info.joints_, "crank");
    const TiXmlElement crank_joint_el(
        loadXmlElement(src_info.joints_[dst_info->crank_joint_id].xml_element_));
    dst_info->crank_offset = parseOptionalValue(crank_joint_el, "offset", 0.);
    dst_info->crank_length = parseRequiredValue(crank_joint_el, "length");

    // parse params from the <joint> element with "bar" role
    dst_info->bar_joint_id = findJointByRole(src_info.joints_, "bar");
    const TiXmlElement bar_joint_el(
        loadXmlElement(src_info.joints_[dst_info->bar_joint_id].xml_element_));
    dst_info->bar_length = parseRequiredValue(bar_joint_el, "length");

    // parse params from the <joint> element with "slider" role
    dst_info->slider_joint_id = findJointByRole(src_info.joints_, "slider");
    const TiXmlElement slider_joint_el(
        loadXmlElement(src_info.joints_[dst_info->slider_joint_id].xml_element_));
    dst_info->slider_offset_x = parseOptionalValue(slider_joint_el, "offsetX", 0.);
    dst_info->slider_offset_y = parseOptionalValue(slider_joint_el, "offsetY", 0.);
  }
};

} // namespace transmission_interface_extensions

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2014 Barend Gehrels, Amsterdam, the Netherlands.

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

//[buffer_distance_asymmetric
//` Shows how the distance_asymmetric strategy can be used as a DistanceStrategy to create asymmetric buffers

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/geometries.hpp>
/*<-*/ #include "../examples_utils/create_svg_buffer.hpp" /*->*/

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point;
    typedef boost::geometry::model::linestring<point> linestring;
    typedef boost::geometry::model::polygon<point> polygon;

    // Declare the asymmetric distance strategy
    boost::geometry::strategy::buffer::distance_asymmetric<double> distance_strategy(1.0, 0.5);

    // Declare other strategies
    boost::geometry::strategy::buffer::side_straight side_strategy;
    boost::geometry::strategy::buffer::join_round join_strategy;
    boost::geometry::strategy::buffer::end_round end_strategy;
    boost::geometry::strategy::buffer::point_circle point_strategy;

    // Declare/fill a multi linestring
    boost::geometry::model::multi_linestring<linestring> ml;
    boost::geometry::read_wkt("MULTILINESTRING((3 5,5 10,7 5),(7 7,11 10,15 7,19 10))", ml);

    // Create the buffered geometry with left/right a different distance
    boost::geometry::model::multi_polygon<polygon> result;
    boost::geometry::buffer(ml, result,
                distance_strategy, side_strategy,
                join_strategy, end_strategy, point_strategy);
    /*<-*/ create_svg_buffer("buffer_distance_asymmetric.svg", ml, result); /*->*/

    return 0;
}

//]


// Boost.Geometry Index
// Rtree tests generator

// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.

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

#include <fstream>
#include <vector>
#include <string>
#include <boost/foreach.hpp>
#include <boost/assert.hpp>
#include <boost/tuple/tuple.hpp>

int main()
{
    typedef boost::tuple<std::string, std::string> CT;
    std::vector<CT> coordinate_types;
    coordinate_types.push_back(boost::make_tuple("double", "d"));
    //coordinate_types.push_back(boost::make_tuple("int", "i"));
    //coordinate_types.push_back(boost::make_tuple("float", "f"));

    std::vector<std::string> dimensions;
    dimensions.push_back("2");
    dimensions.push_back("3");

    typedef boost::tuple<std::string, std::string> P;
    std::vector<P> parameters;
    parameters.push_back(boost::make_tuple("bgi::linear<5, 2>()", "lin"));
    parameters.push_back(boost::make_tuple("bgi::dynamic_linear(5, 2)", "dlin"));
    parameters.push_back(boost::make_tuple("bgi::quadratic<5, 2>()", "qua"));
    parameters.push_back(boost::make_tuple("bgi::dynamic_quadratic(5, 2)", "dqua"));
    parameters.push_back(boost::make_tuple("bgi::rstar<5, 2>()", "rst"));
    parameters.push_back(boost::make_tuple("bgi::dynamic_rstar(5, 2)","drst"));

    std::vector<std::string> indexables;
    indexables.push_back("p");
    indexables.push_back("b");
    indexables.push_back("s");

    typedef std::pair<std::string, std::string> TS;
    std::vector<TS> testsets;
    testsets.push_back(std::make_pair("testset::modifiers", "mod"));
    testsets.push_back(std::make_pair("testset::queries", "que"));
    testsets.push_back(std::make_pair("testset::additional", "add"));

    BOOST_FOREACH(P const& p, parameters)
    {
        BOOST_FOREACH(TS const& ts, testsets)
        {
            BOOST_FOREACH(std::string const& i, indexables)
            {
                BOOST_FOREACH(std::string const& d, dimensions)
                {
                    // If the I is Segment, generate only for 2d
                    if ( i == "s" && d != "2" )
                    {
                        continue;
                    }

                    BOOST_FOREACH(CT const& c, coordinate_types)
                    {
                        std::string filename = std::string() +
                            "rtree_" + boost::get<1>(p) + '_' + ts.second + '_' + i + d + boost::get<1>(c) + ".cpp";

                        std::ofstream f(filename.c_str(), std::ios::trunc);

                        f <<
                            "// Boost.Geometry Index\n" <<
                            "// Unit Test\n" <<
                            "\n" <<
                            "// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.\n" <<
                            "\n" <<
                            "// Use, modification and distribution is subject to the Boost Software License,\n" <<
                            "// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at\n" <<
                            "// http://www.boost.org/LICENSE_1_0.txt)\n" <<
                            "\n";

                        f <<
                            "#include <rtree/test_rtree.hpp>\n" <<
                            "\n";

                        std::string indexable_type;
                        std::string point_type = std::string("bg::model::point<") + boost::get<0>(c) + ", " + d + ", bg::cs::cartesian>";
                        if ( i == "p" )
                            indexable_type = point_type;
                        else if ( i == "b" )
                            indexable_type = std::string("bg::model::box< ") + point_type + " >";
                        else if ( i == "s" )
                            indexable_type = std::string("bg::model::segment< ") + point_type + " >";
                        else
                            BOOST_ASSERT(false);

                        f <<
                            "int test_main(int, char* [])\n" <<
                            "{\n" <<
                            "    typedef " << indexable_type << " Indexable;\n" <<
                            "    " << ts.first << "<Indexable>(" << boost::get<0>(p) << ", std::allocator<int>());\n" <<
                            "    return 0;\n" <<
                            "}\n";
                    }
                }
            }

        }
    }

    return 0;
}


/*
 * Copyright 2015 David A. Boyuka II
 *
 * 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.
 */

/*
 * test-index-io.cpp
 *
 *  Created on: May 10, 2014
 *      Author: David A. Boyuka II
 */

#include <cstdint>
#include <cassert>
#include <cstdlib>
#include <vector>
#include <boost/smart_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/iterator/function_input_iterator.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/timer/timer.hpp>

#include "pique/region/region-encoding.hpp"
#include "pique/region/cblq/cblq.hpp"
#include "pique/region/cblq/cblq-encode.hpp"
#include "pique/region/bitmap/bitmap.hpp"

#include "pique/convert/cblq/cblq-to-bitmap-convert.hpp"

using boost::timer::cpu_timer;
using boost::timer::cpu_times;
static double walltime(const cpu_timer &timer) { return (double)timer.elapsed().wall * 1e-9; }

typedef boost::dynamic_bitset< BitmapRegionEncoding::block_t > Bitset;

static Bitset make_large_bitmap(uint64_t nbits, uint64_t seed = boost::mt19937_64::default_seed);
static Bitset make_large_sparse_bitmap(uint64_t nbits, int iters);

std::string small_bitmap_str = "0001110100001111"; // written in big-endian: bits are 1111 0000 1011 1000 in little-endian
const Bitset small_bitmap(small_bitmap_str);
const Bitset large_bitmap = make_large_bitmap(1ULL<<14);
const Bitset uneven_bitmap = make_large_bitmap(27 + (1ULL<<14));
const Bitset very_uneven_bitmap = make_large_bitmap(27 + (1ULL<<13));
const Bitset large_sparse_bitmap = make_large_sparse_bitmap(27 + (1ULL<<13), 7); // 4 -> density ~= 2^-7

static Bitset make_large_bitmap(uint64_t nbits, uint64_t seed) {
	boost::mt19937_64 rand; // default seed is fine

	Bitset b(
		boost::make_function_input_iterator(rand, (uint64_t)0),
		boost::make_function_input_iterator(rand, nbits / std::numeric_limits< Bitset::block_type >::digits));
	b.resize(nbits);
	return b;
}

static Bitset make_large_sparse_bitmap(uint64_t nbits, int iters) {
	uint64_t seed = boost::mt19937_64::default_seed;
	Bitset b = make_large_bitmap(nbits, seed);
	for (int i = 1; i < iters; ++i) {
		seed *= seed;
		Bitset b2 = make_large_bitmap(nbits, seed);
		b &= b2;
	}
	return b;
}

template<typename RegionEncoderT>
static boost::shared_ptr< typename RegionEncoderT::RegionEncodingOutT > make_region(typename RegionEncoderT::RegionEncoderConfig conf, const Bitset &bitmap) {
	RegionEncoderT encoder(conf, bitmap.size());

	for (size_t i = 0; i < bitmap.size(); i++) {
		encoder.push_bits(1, bitmap[i]);
	}

	encoder.finalize();
	return encoder.to_region_encoding();
}

template<typename RegionEncoderT, typename RegionConverterT>
static void do_bitmap_test(const std::string testname, typename RegionEncoderT::RegionEncoderConfig conf, const Bitset &bitmap) {
	typedef typename RegionEncoderT::RegionEncodingOutT RegionEncodingT;

	RegionConverterT converter;
	boost::dynamic_bitset< BitmapRegionEncoding::block_t > actual_bitmap;

	// Generate the input region (non-bitmap RE)
	boost::shared_ptr< RegionEncodingT > region = make_region< RegionEncoderT >(conf, bitmap);

	// Convert to bitmap (timed)
	cpu_timer convert_timer;
	boost::shared_ptr< BitmapRegionEncoding > bitmap_region = converter.convert(region);
	convert_timer.stop();

	// Check correctness
	bitmap_region->to_bitset(actual_bitmap);
	assert(actual_bitmap == bitmap);

	// Clear
	bitmap_region->zero();

	// In-place union to bitmap (timed)
	cpu_timer ipunion_timer;
	converter.inplace_convert(region, bitmap_region, NArySetOperation::UNION);
	ipunion_timer.stop();

	// Check correctness
	bitmap_region->to_bitset(actual_bitmap);
	assert(actual_bitmap == bitmap);

	// Clear
	bitmap_region->fill();

	// In-place union to bitmap (timed)
	cpu_timer ipinter_timer;
	converter.inplace_convert(region, bitmap_region, NArySetOperation::INTERSECTION);
	ipinter_timer.stop();

	// Check correctness
	bitmap_region->to_bitset(actual_bitmap);
	assert(actual_bitmap == bitmap);

#ifdef VERBOSE_TESTS
	printf("Test \"%s\" passed! Time convert/ip-union/ip-inter: %lf/%lf/%lf\n", testname.c_str(), walltime(convert_timer), walltime(ipunion_timer), walltime(ipinter_timer));
#endif
}

int main(int argc, char **argv) {
	do_bitmap_test< CBLQRegionEncoder<2>, CBLQToBitmapConverter<2> >("small-cblq", CBLQRegionEncoderConfig(true), small_bitmap);
	do_bitmap_test< CBLQRegionEncoder<2>, CBLQToBitmapConverter<2> >("large-cblq", CBLQRegionEncoderConfig(true), large_bitmap);
	do_bitmap_test< CBLQRegionEncoder<2>, CBLQToBitmapConverter<2> >("uneven-cblq", CBLQRegionEncoderConfig(true), uneven_bitmap);
	do_bitmap_test< CBLQRegionEncoder<2>, CBLQToBitmapConverter<2> >("very-uneven-cblq", CBLQRegionEncoderConfig(true), very_uneven_bitmap);
	do_bitmap_test< CBLQRegionEncoder<2>, CBLQToBitmapConverter<2> >("large-sparse-cblq", CBLQRegionEncoderConfig(true), large_sparse_bitmap);

	do_bitmap_test< CBLQRegionEncoder<2>, CBLQToBitmapDFConverter<2> >("small-cblq-df", CBLQRegionEncoderConfig(true), small_bitmap);
	do_bitmap_test< CBLQRegionEncoder<2>, CBLQToBitmapDFConverter<2> >("large-cblq-df", CBLQRegionEncoderConfig(true), large_bitmap);
	do_bitmap_test< CBLQRegionEncoder<2>, CBLQToBitmapDFConverter<2> >("uneven-cblq-df", CBLQRegionEncoderConfig(true), uneven_bitmap);
	do_bitmap_test< CBLQRegionEncoder<2>, CBLQToBitmapDFConverter<2> >("very-uneven-cblq-df", CBLQRegionEncoderConfig(true), very_uneven_bitmap);
	do_bitmap_test< CBLQRegionEncoder<2>, CBLQToBitmapDFConverter<2> >("large-sparse-cblq-df", CBLQRegionEncoderConfig(true), large_sparse_bitmap);
}