#include <iostream>
#include <fstream>
#include <vector>
//include ros dep.
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <image_transport/image_transport.h>
#include <dynamic_reconfigure/server.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Bool.h>

#include "tf/tf.h"
#include "tf/transform_datatypes.h"
#include <tf/transform_broadcaster.h>
//include pcl dep
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
//include opencv and eigen
// #include <eigen3/Eigen/Dense>
#include <Eigen/Eigen>
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <cv_bridge/cv_bridge.h>

//#include <cloud_banchmark/cloud_banchmarkConfig.h>
#include "depth_render.cuh"
#include "quadrotor_msgs/PositionCommand.h"
using namespace cv;
using namespace std;
using namespace Eigen;

int *depth_hostptr;
cv::Mat depth_mat;

//camera param
int width, height;
double fx,fy,cx,cy;

DepthRender depthrender;
ros::Publisher pub_depth;
ros::Publisher pub_color;
ros::Publisher pub_pose;
ros::Publisher pub_pcl_wolrd;

sensor_msgs::PointCloud2 local_map_pcl;
sensor_msgs::PointCloud2 local_depth_pcl;

ros::Subscriber odom_sub;
ros::Subscriber global_map_sub, local_map_sub;

ros::Timer local_sensing_timer, estimation_timer;

bool has_global_map(false);
bool has_local_map(false);
bool has_odom(false);

Matrix4d cam02body;
Matrix4d cam2world;
Eigen::Quaterniond cam2world_quat;
nav_msgs::Odometry _odom;

double sensing_horizon, sensing_rate, estimation_rate;
double _x_size, _y_size, _z_size;
double _gl_xl, _gl_yl, _gl_zl;
double _resolution, _inv_resolution;
int _GLX_SIZE, _GLY_SIZE, _GLZ_SIZE;

ros::Time last_odom_stamp = ros::TIME_MAX;
Eigen::Vector3d last_pose_world;

void render_currentpose();
void render_pcl_world();

inline Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i & index)
{
    Eigen::Vector3d pt;
    pt(0) = ((double)index(0) + 0.5) * _resolution + _gl_xl;
    pt(1) = ((double)index(1) + 0.5) * _resolution + _gl_yl;
    pt(2) = ((double)index(2) + 0.5) * _resolution + _gl_zl;

    return pt;
};

inline Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d & pt)
{
    Eigen::Vector3i idx;
    idx(0) = std::min( std::max( int( (pt(0) - _gl_xl) * _inv_resolution), 0), _GLX_SIZE - 1);
    idx(1) = std::min( std::max( int( (pt(1) - _gl_yl) * _inv_resolution), 0), _GLY_SIZE - 1);
    idx(2) = std::min( std::max( int( (pt(2) - _gl_zl) * _inv_resolution), 0), _GLZ_SIZE - 1);

    return idx;
};

void rcvOdometryCallbck(const nav_msgs::Odometry& odom)
{
  /*if(!has_global_map)
    return;*/
  has_odom = true;
  _odom = odom;
  Matrix4d Pose_receive = Matrix4d::Identity();

  Eigen::Vector3d request_position;
  Eigen::Quaterniond request_pose;
  request_position.x() = odom.pose.pose.position.x;
  request_position.y() = odom.pose.pose.position.y;
  request_position.z() = odom.pose.pose.position.z;
  request_pose.x() = odom.pose.pose.orientation.x;
  request_pose.y() = odom.pose.pose.orientation.y;
  request_pose.z() = odom.pose.pose.orientation.z;
  request_pose.w() = odom.pose.pose.orientation.w;
  Pose_receive.block<3,3>(0,0) = request_pose.toRotationMatrix();
  Pose_receive(0,3) = request_position(0);
  Pose_receive(1,3) = request_position(1);
  Pose_receive(2,3) = request_position(2);

  Matrix4d body_pose = Pose_receive;
  //convert to cam pose
  cam2world = body_pose * cam02body;
  cam2world_quat = cam2world.block<3,3>(0,0);

  last_odom_stamp = odom.header.stamp;

  last_pose_world(0) = odom.pose.pose.position.x;
  last_pose_world(1) = odom.pose.pose.position.y;
  last_pose_world(2) = odom.pose.pose.position.z;

  //publish tf
  /*static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(cam2world(0,3), cam2world(1,3), cam2world(2,3) ));
  transform.setRotation(tf::Quaternion(cam2world_quat.x(), cam2world_quat.y(), cam2world_quat.z(), cam2world_quat.w()));
  br.sendTransform(tf::StampedTransform(transform, last_odom_stamp, "world", "camera")); //publish transform from world frame to quadrotor frame.*/
}

void pubCameraPose(const ros::TimerEvent & event)
{
  //cout<<"pub cam pose"
  geometry_msgs::PoseStamped camera_pose;
  camera_pose.header = _odom.header;
  camera_pose.header.frame_id = "world";
  camera_pose.pose.position.x = cam2world(0,3);
  camera_pose.pose.position.y = cam2world(1,3);
  camera_pose.pose.position.z = cam2world(2,3);
  camera_pose.pose.orientation.w = cam2world_quat.w();
  camera_pose.pose.orientation.x = cam2world_quat.x();
  camera_pose.pose.orientation.y = cam2world_quat.y();
  camera_pose.pose.orientation.z = cam2world_quat.z();
  pub_pose.publish(camera_pose);
}

void renderSensedPoints(const ros::TimerEvent & event)
{
  if(! has_global_map || ! has_odom) return;
  // if( !has_global_map && !has_local_map) return;
  // if( !has_odom ) return;

  render_currentpose();
//   render_pcl_world();
}

vector<float> cloud_data;
void rcvGlobalPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map )
{
  if(has_global_map)
    return;

  ROS_WARN("[Depth Simulator] Global Pointcloud received..");
  //load global map
  pcl::PointCloud<pcl::PointXYZ> cloudIn;
  pcl::PointXYZ pt_in;
  //transform map to point cloud format
  pcl::fromROSMsg(pointcloud_map, cloudIn);
  for(int i = 0; i < int(cloudIn.points.size()); i++){
    if (isnan(cloudIn.points[i].x) || isnan(cloudIn.points[i].y) || isnan(cloudIn.points[i].z) ||
      isinf(cloudIn.points[i].x) || isinf(cloudIn.points[i].y) || isinf(cloudIn.points[i].z)) {
      continue;
    }
    pt_in = cloudIn.points[i];
    cloud_data.push_back(pt_in.x);
    cloud_data.push_back(pt_in.y);
    cloud_data.push_back(pt_in.z);
  }
  printf("global map has points: %d.\n", (int)cloud_data.size() / 3 );
  //pass cloud_data to depth render
  depthrender.set_data(cloud_data);
  depth_hostptr = (int*) malloc(width * height * sizeof(int));

  has_global_map = true;
  global_map_sub.shutdown();
}

void rcvLocalPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map )
{
  //ROS_WARN("Local Pointcloud received..");
  //load local map
  pcl::PointCloud<pcl::PointXYZ> cloudIn;
  pcl::PointXYZ pt_in;
  //transform map to point cloud format
  pcl::fromROSMsg(pointcloud_map, cloudIn);

  if(cloudIn.points.size() == 0) return;
  for(int i = 0; i < int(cloudIn.points.size()); i++){
    pt_in = cloudIn.points[i];
    Eigen::Vector3d pose_pt(pt_in.x, pt_in.y, pt_in.z);
    //pose_pt = gridIndex2coord(coord2gridIndex(pose_pt));
    cloud_data.push_back(pose_pt(0));
    cloud_data.push_back(pose_pt(1));
    cloud_data.push_back(pose_pt(2));
  }
  //printf("local map has points: %d.\n", (int)cloud_data.size() / 3 );
  //pass cloud_data to depth render
  depthrender.set_data(cloud_data);
  depth_hostptr = (int*) malloc(width * height * sizeof(int));

  has_local_map = true;
}

void render_pcl_world()
{
  //for debug purpose
  pcl::PointCloud<pcl::PointXYZ> localMap;
  pcl::PointXYZ pt_in;

  Eigen::Vector4d pose_in_camera;
  Eigen::Vector4d pose_in_world;
  Eigen::Vector3d pose_pt;

  for(int u = 0; u < width; u++)
    for(int v = 0; v < height; v++){
      float depth = depth_mat.at<float>(v,u);

      if(depth == 0.0)
        continue;

      pose_in_camera(0) = (u - cx) * depth / fx;
      pose_in_camera(1) = (v - cy) * depth / fy;
      pose_in_camera(2) = depth;
      pose_in_camera(3) = 1.0;

      pose_in_world = cam2world * pose_in_camera;

      if( (pose_in_world.segment(0,3) - last_pose_world).norm() > sensing_horizon )
          continue;

      pose_pt = pose_in_world.head(3);
      //pose_pt = gridIndex2coord(coord2gridIndex(pose_pt));
      pt_in.x = pose_pt(0);
      pt_in.y = pose_pt(1);
      pt_in.z = pose_pt(2);

      localMap.points.push_back(pt_in);
    }

  localMap.width = localMap.points.size();
  localMap.height = 1;
  localMap.is_dense = true;

  pcl::toROSMsg(localMap, local_map_pcl);
  local_map_pcl.header.frame_id  = "world";
  local_map_pcl.header.stamp     = last_odom_stamp;

  pub_pcl_wolrd.publish(local_map_pcl);
}

void render_currentpose()
{
  double this_time = ros::Time::now().toSec();

  Matrix4d cam_pose = cam2world.inverse();

  double pose[4 * 4];

  for(int i = 0; i < 4; i ++)
    for(int j = 0; j < 4; j ++)
      pose[j + 4 * i] = cam_pose(i, j);

  depthrender.render_pose(pose, depth_hostptr);
  //depthrender.render_pose(cam_pose, depth_hostptr);

  depth_mat = cv::Mat::zeros(height, width, CV_32FC1);
  double min = 0.5;
  double max = 1.0f;
  for(int i = 0; i < height; i++)
  	for(int j = 0; j < width; j++)
  	{
  		float depth = (float)depth_hostptr[i * width + j] / 1000.0f;
  		depth = depth < 500.0f ? depth : 0;
  		max = depth > max ? depth : max;
  		depth_mat.at<float>(i,j) = depth;
  	}
  //ROS_INFO("render cost %lf ms.", (ros::Time::now().toSec() - this_time) * 1000.0f);
  //printf("max_depth %lf.\n", max);

  cv_bridge::CvImage out_msg;
  out_msg.header.stamp = last_odom_stamp;
  out_msg.header.frame_id = "camera";
  out_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
  out_msg.image = depth_mat.clone();
  pub_depth.publish(out_msg.toImageMsg());

  cv::Mat adjMap;
  // depth_mat.convertTo(adjMap,CV_8UC1, 255 / (max-min), -min);
  depth_mat.convertTo(adjMap,CV_8UC1, 255 /13.0, -min);
  cv::Mat falseColorsMap;
  cv::applyColorMap(adjMap, falseColorsMap, cv::COLORMAP_RAINBOW);
  cv_bridge::CvImage cv_image_colored;
  cv_image_colored.header.frame_id = "depthmap";
  cv_image_colored.header.stamp = last_odom_stamp;
  cv_image_colored.encoding = sensor_msgs::image_encodings::BGR8;
  cv_image_colored.image = falseColorsMap;
  pub_color.publish(cv_image_colored.toImageMsg());
  //cv::imshow("depth_image", adjMap);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pcl_render");
  ros::NodeHandle nh("~");

  nh.getParam("cam_width", width);
  nh.getParam("cam_height", height);
  nh.getParam("cam_fx", fx);
  nh.getParam("cam_fy", fy);
  nh.getParam("cam_cx", cx);
  nh.getParam("cam_cy", cy);
  nh.getParam("sensing_horizon", sensing_horizon);
  nh.getParam("sensing_rate",    sensing_rate);
  nh.getParam("estimation_rate", estimation_rate);

  nh.getParam("map/x_size",     _x_size);
  nh.getParam("map/y_size",     _y_size);
  nh.getParam("map/z_size",     _z_size);

  depthrender.set_para(fx, fy, cx, cy, width, height);

  // cam02body <<  0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
  //               0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
  //               -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
  //               0.0, 0.0, 0.0, 1.0;

  cam02body << 0.0, 0.0, 1.0, 0.0,
              -1.0, 0.0, 0.0, 0.0,
               0.0,-1.0, 0.0, 0.0,
               0.0, 0.0, 0.0, 1.0;

  //init cam2world transformation
  cam2world = Matrix4d::Identity();
  //subscribe point cloud
  global_map_sub = nh.subscribe( "global_map", 1,  rcvGlobalPointCloudCallBack);
  local_map_sub  = nh.subscribe( "local_map",  1,  rcvLocalPointCloudCallBack);
  odom_sub       = nh.subscribe( "odometry",   50, rcvOdometryCallbck   );

  //publisher depth image and color image
  pub_depth = nh.advertise<sensor_msgs::Image>("depth",1000);
  pub_color = nh.advertise<sensor_msgs::Image>("colordepth",1000);
  pub_pose  = nh.advertise<geometry_msgs::PoseStamped>("camera_pose",1000);
  pub_pcl_wolrd = nh.advertise<sensor_msgs::PointCloud2>("rendered_pcl",1);

  double sensing_duration  = 1.0 / sensing_rate;
  double estimate_duration = 1.0 / estimation_rate;

  local_sensing_timer = nh.createTimer(ros::Duration(sensing_duration),  renderSensedPoints);
  estimation_timer    = nh.createTimer(ros::Duration(estimate_duration), pubCameraPose);
  //cv::namedWindow("depth_image",1);

  _inv_resolution = 1.0 / _resolution;

  _gl_xl = -_x_size/2.0;
  _gl_yl = -_y_size/2.0;
  _gl_zl =   0.0;

  _GLX_SIZE = (int)(_x_size * _inv_resolution);
  _GLY_SIZE = (int)(_y_size * _inv_resolution);
  _GLZ_SIZE = (int)(_z_size * _inv_resolution);

  ros::Rate rate(100);
  bool status = ros::ok();
  while(status)
  {
    ros::spinOnce();
    status = ros::ok();
    rate.sleep();
  }
}


#pragma once

#include <Eigen/Dense>

namespace fastEnvelope {
#ifdef FAST_ENVELOPE_USE_FLOAT
    typedef float Scalar;
#define SCALAR_ZERO 1e-6
#define SCALAR_ZERO_2 1e-12
#define SCALAR_ZERO_3 1e-18
#else
    typedef double Scalar;
#define SCALAR_ZERO 1e-8
#define SCALAR_ZERO_2 1e-16
#define SCALAR_ZERO_3 1e-24

#endif

	static const bool OUT_PRISM = 1;
	static const bool IN_PRISM = 0;

	static const int FE_CUT_COPLANAR = 4;
	static const int FE_CUT_EMPTY = -1;
	static const int FE_CUT_FACE = 3;

	static const int NOT_DEGENERATED = 0;
	static const int NERLY_DEGENERATED = 1;
	static const int DEGENERATED_SEGMENT = 2;
	static const int DEGENERATED_POINT = 3;

    typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;

    typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
    typedef Eigen::Matrix<Scalar, 2, 1> Vector2;

    typedef Eigen::Matrix<int, 4, 1> Vector4i;
    typedef Eigen::Matrix<int, 3, 1> Vector3i;
    typedef Eigen::Matrix<int, 2, 1> Vector2i;

}


#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <boost/filesystem.hpp>
#include "RobotMPC.h"
using namespace std;
using namespace Eigen;
using namespace boost;

int main(){
  RobotPathFollowMPC pf = RobotPathFollowMPC();
  // mpc.printRobot();
  pf.clkDiv = 4;

  // "Test"

  // Init MPC
  float T = 0.01 * pf.clkDiv;
  float v_des = 0.3;
  pf.a_des = 0.3;
  float w = 0.26;
  float a = 0.1;

  pf.initRobot(w,a);
  pf.init(T, v_des);
  pf.currentLine=0;

  // Initialize Constraints
  float vw_max =  0.6;
  float vw_min = -vw_max;
  float omega_max = 2*PI/8;
  float omega_min = -omega_max;
  float v_max =  1;
  float v_min = -v_max;
  float acc_max =  0.5;
  float acc_min = -0.1;
  pf.setConstraints(vw_min,vw_max,omega_min,omega_max,v_min,v_max,acc_min,acc_max);

  // Design MPC - Compute controller gains for the horizon
  int N = 100;
  float Qth = 0.05;
  float Qdu = 0.001;
  float Qu =  0.001;
  pf.initMPC(N);
  pf.design(Qth, Qdu, Qu);
  pf.ks = 0*v_des/omega_max;
  pf.ka = 0;

  pf.useNLScaling = 1;

  // Print That Shit !
  pf.printRobot();
  pf.sys.printSys();
  pf.printConstraints();
  pf.printParams();

  //pf.mpc.printCondenseStateSpace();
  //pf.mpc.printControllerGains();
  // cout << "Matrix P:\n" << pf.P << endl;
  // cout << "Vector q:\n" << pf.q << endl;
  int track = 1;

  switch(track){
    case 0:{
      pf.addWaypoint(0,    0);
      pf.addWaypoint(4,    0);
    }
    break;
    case 1:{
      int n=2;
      pf.addWaypoint(0,    0);
      pf.addWaypoint(n,    0);
      pf.addWaypoint(n,   -n);
      pf.addWaypoint(0,   -n);
      pf.addWaypoint(0,    0);
    }
    break;
    case 2:{
      pf.addWaypoint(0,    0);
      pf.addWaypoint(1.5,  0);
      pf.addWaypoint(1.5, -0.5);
      pf.addWaypoint(3,   -0.5);
    }
    break;
    case 3:{
      pf.addWaypoint(0,  0 );
      pf.addWaypoint(1.5,  0);
      pf.addWaypoint(1.5, -0.2);
      pf.addWaypoint(3,   -0.2);
    }
    break;
    case 4:{
      float Nstep = 20;
      for(int i=0; i<=Nstep; i++){
        float amp = 0.5;
        float x = i/Nstep;
        float y = amp*sin(2 * M_PI * x);
        pf.addWaypoint(3*x,y);
      }
    }
    break;
    case 5:{
      float len = 2.0;
      float angleDeg = 150;
      float angleRad = angleDeg*M_PI/180.0f;
      pf.addWaypoint(0,    0);
      pf.addWaypoint(len,    0);
      float x = len+len*cos(angleRad);
      float y = len*sin(angleRad);
      pf.addWaypoint(x,y);
    }
    break;
    case 6:{
      pf.addWaypoint(0,    0);
      pf.addWaypoint(2,    0);
      pf.addWaypoint(2,    2);
    }
  }
  pf.makeLineDefs();
  // Define waypoints
  // n-by-n square

  // pf.insertWaypoint(2, 3.5,0.5);
  cout << "Line Definitions:\n" << pf.lineDefs << endl;

  // cout << "Static Kalman Gain:\n" << pf.Kfx << endl;

  //Vector2f ur = pf.compute(-0.3,-0.6,-0.2);
  //cout << "ur:\n" << ur << endl;

  bool runSim = true;
  if(runSim){
    pf.runSimulation(1,0.5,0,10000);
    // cout << "Here is the simdata:\n\ttime\t      x\t\t  y\t   theta\t  v\t\t  omega\t     vl\t\t vr\t      s\t\t  d\t th_err\n" << mpc.simData << endl;
    cout << "Simulation Complete!" << endl;

    ofstream file("simData/simData.txt");
    if (file.is_open()){
      file << pf.simData << '\n';
      file.close();
    }

    ofstream file2("simData/simWaypoints.txt");
    if (file2.is_open()){
      file2 << pf.lineDefs << '\n';
      file2.close();
    }

    ofstream file3("simData/simConstraints.txt");
    if (file3.is_open()){
      file3 << "T\tv_des\ta_des\tw\ta\tN\tQth\tQdu\tQu\tks\tvw_min\tvw_max\tomega_min\tomega_max\tv_min\tv_max\tacc_min\tacc_max\ttrack\t" << endl;
      file3 << pf.T << '\t' << pf.v_des << '\t' << pf.a_des << '\t' << pf.w << '\t' << pf.a << '\t';
      file3 << pf.mpc.N << '\t' << Qth << '\t' << Qdu << '\t' << Qu  << '\t' << pf.ks << '\t';
      file3 << -pf.q(2) << '\t' << pf.q(0) << '\t' << -pf.q(5) << '\t' << pf.q(4) << '\t' << -pf.q(7) << '\t' << pf.q(6) << '\t' << pf.acc_min << '\t' << pf.acc_max << '\t';
      file3 << track << '\t';
      file3 << endl;
      file3.close();
    }
  }

  // cout << "Rk:\n" << mpc.Rk << endl;
  // cout << "Uk:\n" << mpc.Uk << endl;

}


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

#include "mathext.hpp"

BOOST_AUTO_TEST_SUITE( suite1 );

BOOST_AUTO_TEST_CASE( test1_isPowerOf2 )
{
  BOOST_CHECK(MathExt::isPowerOf2(3) == false);
  BOOST_CHECK(MathExt::isPowerOf2(3) == false);
}

BOOST_AUTO_TEST_CASE( test1_rand_width )
{
  BOOST_CHECK(MathExt::rand_width(4) < 16); // 2^4 = 16
  BOOST_CHECK(MathExt::rand_width(4) >= 0); // 2^4 = 16
}

BOOST_AUTO_TEST_SUITE_END();


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

#ifndef BOOST_MATH_MODF_HPP
#define BOOST_MATH_MODF_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/trunc.hpp>

namespace boost{ namespace math{

template <class T, class Policy>
inline T modf(const T& v, T* ipart, const Policy& pol)
{
   *ipart = trunc(v, pol);
   return v - *ipart;
}
template <class T>
inline T modf(const T& v, T* ipart)
{
   return modf(v, ipart, policies::policy<>());
}

template <class T, class Policy>
inline T modf(const T& v, int* ipart, const Policy& pol)
{
   *ipart = itrunc(v, pol);
   return v - *ipart;
}
template <class T>
inline T modf(const T& v, int* ipart)
{
   return modf(v, ipart, policies::policy<>());
}

template <class T, class Policy>
inline T modf(const T& v, long* ipart, const Policy& pol)
{
   *ipart = ltrunc(v, pol);
   return v - *ipart;
}
template <class T>
inline T modf(const T& v, long* ipart)
{
   return modf(v, ipart, policies::policy<>());
}

#ifdef BOOST_HAS_LONG_LONG
template <class T, class Policy>
inline T modf(const T& v, boost::long_long_type* ipart, const Policy& pol)
{
   *ipart = lltrunc(v, pol);
   return v - *ipart;
}
template <class T>
inline T modf(const T& v, boost::long_long_type* ipart)
{
   return modf(v, ipart, policies::policy<>());
}
#endif

}} // namespaces

#endif // BOOST_MATH_MODF_HPP


// Boost.Polygon library voronoi_benchmark.cpp file

//          Copyright Andrii Sydorchuk 2010-2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#include <algorithm>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <numeric>
#include <vector>
#include <utility>

#include <boost/random/mersenne_twister.hpp>
#include <boost/timer/timer.hpp>

typedef boost::int32_t int32;
using boost::timer::cpu_times;
using boost::timer::nanosecond_type;

// Include for the Boost.Polygon Voronoi library.
#include <boost/polygon/voronoi.hpp>
typedef boost::polygon::default_voronoi_builder VB_BOOST;
typedef boost::polygon::voronoi_diagram<double> VD_BOOST;

// Includes for the CGAL library.
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
typedef CGAL::Exact_predicates_inexact_constructions_kernel CGAL_KERNEL;
typedef CGAL::Delaunay_triangulation_2<CGAL_KERNEL> DT_CGAL;
typedef CGAL_KERNEL::Point_2 POINT_CGAL;

// Includes for the S-Hull library.
#include <s_hull.h>

const int RANDOM_SEED = 27;
const int NUM_TESTS = 6;
const int NUM_POINTS[] = {10, 100, 1000, 10000, 100000, 1000000};
const int NUM_RUNS[] = {100000, 10000, 1000, 100, 10, 1};
std::ofstream bf("benchmark_points.txt",
                 std::ios_base::out | std::ios_base::app);
boost::timer::cpu_timer timer;

void format_line(int num_points, int num_tests, double time_per_test) {
  bf << "| " << std::setw(16) << num_points << " ";
  bf << "| " << std::setw(15) << num_tests << " ";
  bf << "| " << std::setw(17) << time_per_test << " ";
  bf << "|" << std::endl;
}

double get_elapsed_secs() {
  cpu_times elapsed_times(timer.elapsed());
  return 1E-9 * static_cast<double>(
      elapsed_times.system + elapsed_times.user);
}

void run_boost_voronoi_test() {
  boost::mt19937 gen(RANDOM_SEED);
  bf << "Boost.Polygon Voronoi Diagram of Points:\n";
  for (int i = 0; i < NUM_TESTS; ++i) {
    timer.start();
    for (int j = 0; j < NUM_RUNS[i]; ++j) {
      VB_BOOST vb;
      VD_BOOST vd;
      for (int k = 0; k < NUM_POINTS[i]; ++k) {
        int32 x = static_cast<int32>(gen());
        int32 y = static_cast<int32>(gen());
        vb.insert_point(x, y);
      }
      vb.construct(&vd);
    }
    double time_per_test = get_elapsed_secs() / NUM_RUNS[i];
    format_line(NUM_POINTS[i], NUM_RUNS[i], time_per_test);
  }
  bf << "\n";
}

void run_cgal_delaunay_test() {
  boost::mt19937 gen(RANDOM_SEED);
  bf << "CGAL Delaunay Triangulation of Points:\n";
  for (int i = 0; i < NUM_TESTS; ++i) {
    timer.start();
    for (int j = 0; j < NUM_RUNS[i]; ++j) {
      DT_CGAL dt;
      std::vector<POINT_CGAL> points;
      for (int k = 0; k < NUM_POINTS[i]; ++k) {
        int32 x = static_cast<int32>(gen());
        int32 y = static_cast<int32>(gen());
        points.push_back(POINT_CGAL(x, y));
      }
      // CGAL's implementation sorts points along
      // the Hilbert curve implicitly to improve
      // spatial ordering of the input geometries.
      dt.insert(points.begin(), points.end());
    }
    double time_per_test = get_elapsed_secs() / NUM_RUNS[i];
    format_line(NUM_POINTS[i], NUM_RUNS[i], time_per_test);
  }
  bf << "\n";
}

void run_shull_delaunay_test() {
  boost::mt19937 gen(RANDOM_SEED);
  bf << "S-Hull Delaunay Triangulation of Points:\n";
  // This value is required by S-Hull as it doesn't seem to support properly
  // coordinates with the absolute value higher than 100.
  float koef = 100.0 / (1 << 16) / (1 << 15);
  for (int i = 0; i < NUM_TESTS; ++i) {
    timer.start();
    for (int j = 0; j < NUM_RUNS[i]; ++j) {
      // S-Hull doesn't deal properly with duplicates so we need
      // to eliminate them before running the algorithm.
      std::vector< pair<int32, int32> > upts;
      std::vector<Shx> pts;
      std::vector<Triad> triads;
      Shx pt;
      for (int k = 0; k < NUM_POINTS[i]; ++k) {
        int32 x = static_cast<int32>(gen());
        int32 y = static_cast<int32>(gen());
        upts.push_back(std::make_pair(x, y));
      }
      // Absolutely the same code is used by the Boost.Polygon Voronoi library.
      std::sort(upts.begin(), upts.end());
      upts.erase(std::unique(upts.begin(), upts.end()), upts.end());
      for (int k = 0; k < upts.size(); ++k) {
        pt.r = koef * upts[k].first;
        pt.c = koef * upts[k].second;
        pt.id = k;
        pts.push_back(pt);
      }
      s_hull_del_ray2(pts, triads);
    }
    double time_per_test = get_elapsed_secs() / NUM_RUNS[i];
    format_line(NUM_POINTS[i], NUM_RUNS[i], time_per_test);
  }
  bf << "\n";
}

int main() {
  bf << std::setiosflags(std::ios::right | std::ios::fixed)
     << std::setprecision(6);
  run_boost_voronoi_test();
  run_cgal_delaunay_test();
  run_shull_delaunay_test();
  bf.close();
  return 0;
}


#include <boost/rational.hpp>


// file: tracking_class.cpp, style: README.md
//
// License http://opensource.org/licenses/BSD-3-Clause
// Copyright (c) 2016 14U2g4ocMy5aB2cY4cmCtbXD6qyNQzujuA (serves donations as well)
// All rights reserved.
//
// Get input matrix, filter it_ and do minimum entry search (MES)
// Then project found MES-pixel via intrinsics to 3D point and publish
// Also paint found MES-pixel into image and publish it
//
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
// ros
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
// boost
#include <boost/thread/mutex.hpp>
// opencv
#include <opencv2/core/mat.hpp>
// local
#include "data_container.h"
#include "tracking_class.h"
#include "custom_defines.h"

// Subscribe to video topic, advertise 3D point/marker, set kinect intrinsics
TrackingClass::TrackingClass(DataContainer *container) : container_(container), it_(nh_)
{
  verbose_ = false;
  point_f1_.resize(6);
  point_f1_[1] = 15;
  // kinect1 intrinsics, adjust fx_,fy_,cx_,cy_ to your camera
  fx_ = 5.9421480358642339e+02;
  fy_ = 5.9104092248505947e+02;
  cx_ = 3.3930546187516956e+02;
  cy_ = 2.4273843891390746e+02;
  // publish image with point position
  pub_img_transport_ = it_.advertise("/dotnect_platform/paint_img", 1);
}

TrackingClass::~TrackingClass()
{
}

// change topic
void TrackingClass::subscribeImgSrc(std::string topic_name)
{
  sub_img_transport_ = it_.subscribe(topic_name, 1, &TrackingClass::callbImg, this);
}

// flag to printf pixel position and depth_ value
bool TrackingClass::setVerbose(bool &verbose)
{
  this->verbose_ = verbose;
}

// image callback: convert, prepare then MES
void TrackingClass::callbImg(const sensor_msgs::ImageConstPtr &img_ptr)
{
  boost::mutex::scoped_lock lock(mtx_);
  cv_bridge::CvImagePtr cv_ptr;
  try
  {
    // mutable matrix is needed, so copy
    cv_ptr = cv_bridge::toCvCopy(img_ptr, sensor_msgs::image_encodings::TYPE_16UC1);
  }
  catch (cv_bridge::Exception &e)
  {
    printf(ACRED NNAME ": %s" ACRESETN, e.what());
    return;
  }

  // get pointer directly to cv::Mat member
  cv::Mat *mat_ptr = &(cv_ptr->image);

  // push 0-entries into background having 4000mm along z_-axis (camera depth maximum)
  // layout for img_ptr, cv_ptr, mat_ptr and mat_CV8UC1_;
  // mat_CV8UC1_flip_ inverts the horizontal direction
  //              ________ j,u
  //              |
  //              |  x__.
  //          i,v |     |
  //              |     y
  ushort *p_row;
  // speed: first row, then column
  for (int i = 0; i < mat_ptr->rows; ++i)
  {
    p_row = mat_ptr->ptr<ushort>(i);
    for (int j = 0; j < mat_ptr->cols; ++j)
    {
      if (p_row[j] == 0)
        p_row[j] = 4000;
    }
  }
  // precaution blur
  blur(*mat_ptr, *mat_ptr, cv::Size(3, 3));

  // find minimum = closest point, MES
  depth_ = mat_ptr->at<ushort>(0, 0);
  for (int i = 0; i < mat_ptr->rows; ++i)
  {
    p_row = mat_ptr->ptr<ushort>(i);
    for (int j = 0; j < mat_ptr->cols; ++j)
    {
      if (p_row[j] < depth_)
      {
        u_ = j;
        v_ = i;
        depth_ = p_row[j];
      }
    }
  }

  // pinhole model to project into camera-kosy [mm] (plumb_bob)
  // http://wiki.ros.org/image_pipeline/CameraInfo
  x_ = (u_ - cx_) / fx_ * depth_;
  y_ = (v_ - cy_) / fy_ * depth_;
  z_ = double(depth_);

  // send to container
  point_f1_[0] = ros::Time::now().toNSec();
  point_f1_[2] = cv_ptr->header.seq;
  point_f1_[3] = x_;
  point_f1_[4] = y_;
  point_f1_[5] = z_;
  container_->setVec(point_f1_);

  // option to output pixel position
  if (verbose_)
  {
    printf(NNAME ": x:%f y:%f z:%f; px u:%d v:%d d:%d\n", x_, y_, z_, u_, v_, depth_);
  }

  // convert to greyscale cv::Mat with 8-Bit depth_, (max - min)
  (*mat_ptr).convertTo(mat_CV8UC1_, CV_8UC1, 255. / (2500 - 0), 0);
  cv::flip(mat_CV8UC1_, mat_CV8UC1_flip_, 1);

  // paint black and white point into image
  cv::circle(mat_CV8UC1_flip_, cv::Point(mat_CV8UC1_flip_.cols - 1 - u_, v_), 9, cv::Scalar(0, 0, 0), CV_FILLED, 8, 0);
  cv::circle(mat_CV8UC1_flip_, cv::Point(mat_CV8UC1_flip_.cols - 1 - u_, v_), 5, cv::Scalar(255, 255, 255), 3, 8, 0);

  // output modified video stream
  pub_img_transport_.publish(
      cv_bridge::CvImage((*cv_ptr).header, sensor_msgs::image_encodings::TYPE_8UC1, mat_CV8UC1_flip_).toImageMsg());
}
// EOF


//
// Copyright Fabien Dekeyser, Quoc-Cuong Pham 2008
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

/**
 *
 * \brief Generates an M-by-N real matrix Q with orthonormal columns,
 * which is defined as the first N columns of a product of K elementary
 *  reflectors of order M
 *        Q  =  H(1) H(2) . . . H(k)
 *
 * \warning
 * \todo
 * \date 2005
 * \author CEA/DRT/DTSI/SARC
 * \author Q.C. PHAM
 *
 **/

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_ORGQR_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_ORGQR_HPP

#include <complex>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    /*
     * Generates an M-by-N real matrix Q with orthonormal columns,
     * which is defined as the first N columns of a product of K elementary
     *      reflectors of order M
     *        Q  =  H(1) H(2) . . . H(k)
     * as returned by geqrf().
     * The init value of matrix Q is the matrix A returned by geqrf()
     */
     ///////////////////////////////////////////////////////////////////
    namespace detail {

      inline
      void orgqr(integer_t const m, integer_t const n, integer_t const k,
                 float* a, integer_t const lda,
                 float* tau, float* work, integer_t const lwork, integer_t& info)
      {
        LAPACK_SORGQR (&m, &n, &k, a, &lda, tau, work, &lwork, &info);
      }

      inline
      void orgqr(integer_t const m, integer_t const n, integer_t const k,
                 double* a, integer_t const lda,
                 double* tau, double* work, integer_t const lwork, integer_t& info)
      {
        LAPACK_DORGQR (&m, &n, &k, a, &lda, tau, work, &lwork, &info);
      }

      inline
      void orgqr(integer_t const m, integer_t const n, integer_t const k,
                 traits::complex_f* a, integer_t const lda,
                 traits::complex_f* tau, traits::complex_f* work, integer_t const lwork, integer_t& info)
      {
        LAPACK_CUNGQR (&m, &n, &k, traits::complex_ptr(a), &lda, traits::complex_ptr(tau),
                       traits::complex_ptr(work), &lwork, &info);
      }

      inline
      void orgqr(integer_t const m, integer_t const n, integer_t const k,
                 traits::complex_d* a, integer_t const lda,
                 traits::complex_d* tau, traits::complex_d* work, integer_t const lwork, integer_t& info)
      {
        LAPACK_ZUNGQR (&m, &n, &k, traits::complex_ptr(a), &lda, traits::complex_ptr(tau),
                       traits::complex_ptr(work), &lwork, &info);
      }

    } // fin namespace detail

    //--------------------------------------------

   template <typename A, typename Tau, typename Work>
   int orgqr(A& a, Tau& tau, Work &work) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<A>::matrix_structure,
        traits::general_t
      >::value));
#endif

      const integer_t m = traits::matrix_size1 (a);
      const integer_t n = traits::matrix_size2 (a);
      const integer_t k = n;

      assert (std::min<integer_t>(m,n) <= traits::vector_size (tau));
      assert (n <= traits::vector_size (work));

      integer_t info;
      detail::orgqr (m, n, k,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (tau),
                     traits::vector_storage (work),
                     traits::vector_size (work),
                     info);
      return info;
    }

    // Computation of Q.
    // Workspace is allocated dynamically so that the optimization of
    // blas 3 calls is optimal.
    template <typename A, typename Tau>
    int orgqr (A& a, Tau& tau, optimal_workspace ) {
       typedef typename A::value_type value_type ;
       const std::ptrdiff_t n = traits::matrix_size2 (a);
       traits::detail::array<value_type> work(std::max<std::ptrdiff_t>(1, n*32));
       return orgqr( a, tau, work );

    }

    // Computation of Q
    // Workspace is allocated dynamically to its minimum size.
    // Blas 3 calls are not optimal.
    template <typename A, typename Tau>
    int orgqr (A& a, Tau& tau, minimal_workspace ) {
       typedef typename A::value_type value_type ;
       const std::ptrdiff_t n = traits::matrix_size2 (a);
       traits::detail::array<value_type> work(std::max<std::ptrdiff_t>(1, n));
       return orgqr( a, tau, work );
    }

    // Computation of the Q
    // Workspace is taken from the array in workspace.

    template <typename A, typename Tau, typename Work>
    int orgqr (A& a, Tau& tau, detail::workspace1<Work> workspace ) {
       typedef typename A::value_type value_type ;
       const std::ptrdiff_t n = traits::matrix_size2 (a);
       traits::detail::array<value_type> work(std::max<std::ptrdiff_t>(1, n));
       return orgqr( a, tau, workspace.select(value_type()) );
    }

    // Function without workarray as argument
    template <typename A, typename Tau>
    inline
    int orgqr (A& a, Tau& tau) {
       return orgqr( a, tau, optimal_workspace() );
    }

  }

}}}

#endif


//Implementation files to build necessary libraries
#include "ETL.h"

#include <vector>
#include <stdlib.h>
#include <cmath>
#include <boost/algorithm/string.hpp>

//Parse content of CSV file line by line  and store data as vector of strings
std::vector<std::vector<std::string>> ETL::readCSV(){

    // use ifstream to open file for reading purposes only
    std::ifstream file(dataset);
    //vector of vector of strings to store and return data
    std::vector<std::vector<std::string>> dataString;

    std::string line = "";

    //split line using limiter provided from constructor
    while(getline(file, line)){
        std::vector<std::string> vec;
        boost::algorithm::split(vec, line, boost::is_any_of(delimiter));
        dataString.push_back(vec);
    }

    //close file and return vector of strings
    file.close();
    return dataString;
}

Eigen::MatrixXd ETL::CSVtoEigen(std::vector<std::vector<std::string>> dataset, int rows, int cols){
    //Function will store output of readCSV() as eigen matrix
    if(header==true){
        rows= rows-1;
        //we dont need header files from data
    }

    Eigen::MatrixXd mat(cols, rows); // create empty matrix
    for(int i=0; i<rows; i++){ // iterate over cols and rows and fill with numbers
        for(int j=0; j<cols; ++j){
            mat(j,i)= atof(dataset[i][j].c_str()); //convert strings to floats
        }
    }

    return mat.transpose();
}

auto ETL::Mean(Eigen::MatrixXd data) -> decltype(data.colwise().mean()){
    return data.colwise().mean();
}
Eigen::MatrixXd ETL::Normalize(Eigen::MatrixXd data){

}


/*
  [auto_generated]
  libs/numeric/odeint/test/dummy_odes.hpp

  [begin_description]
  tba.
  [end_description]

  Copyright 2009-2012 Karsten Ahnert
  Copyright 2009-2012 Mario Mulansky

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE_1_0.txt or
  copy at http://www.boost.org/LICENSE_1_0.txt)
*/

#ifndef LIBS_NUMERIC_ODEINT_TEST_DUMMY_ODES_HPP_DEFINED
#define LIBS_NUMERIC_ODEINT_TEST_DUMMY_ODES_HPP_DEFINED

#include "vector_space_1d.hpp"

#include <boost/fusion/include/at_c.hpp>

/*
 * rhs functors/functions for different state types
 */
struct constant_system_functor_standard
{
    template< class State , class Deriv , class Time >
    void operator()( const State &x , Deriv &dxdt , const Time t ) const
    {
        dxdt[0] = 1.0;
    }
};

struct constant_system_functor_vector_space
{
    template< class State , class Deriv , class Time >
    void operator()( const State &x , Deriv &dxdt , const Time t  ) const
    {
        dxdt.m_x = 1.0;
    }
};

struct constant_system_functor_fusion
{
    template< class State , class Deriv , class Time >
    void operator()( const State &x , Deriv &dxdt , const Time t ) const
    {
        boost::fusion::at_c< 0 >( dxdt ) = boost::fusion::at_c< 0 >( x ) / Time( 1.0 );
    }
};

template< class State , class Deriv , class Time >
void constant_system_standard( const State &x , Deriv &dxdt , const Time t )
{
    dxdt[0] = 1.0;
}

template< class State , class Deriv , class Time >
void constant_system_vector_space( const State &x , Deriv &dxdt , const Time t )
{
    dxdt.m_x = 1.0;
}

template< class State , class Deriv , class Time >
void constant_system_fusion( const State &x , Deriv &dxdt , const Time t )
{
    boost::fusion::at_c< 0 >( dxdt ) = boost::fusion::at_c< 0 >( x ) / Time( 1.0 );
}

/*
 * rhs functors for symplectic steppers
 */
struct constant_mom_func
{
    template< class StateIn , class StateOut >
    void operator()( const StateIn &q , StateOut &dp ) const
    {
        dp[0] = 1.0;
    }
};

struct default_coor_func
{
    template< class StateIn , class StateOut >
    void operator()( const StateIn &p , StateOut &dq ) const
    {
        dq[0] = p[0];
    }
};

struct constant_mom_func_vector_space_1d
{
    template< class T >
    void operator()( const vector_space_1d< T > &q , vector_space_1d< T > &dp ) const
    {
        dp.m_x = 1.0;
    }
};

struct default_coor_func_vector_space_1d
{
    template< class T >
    void operator()( const vector_space_1d< T > &p , vector_space_1d< T > &dq ) const
    {
        dq.m_x = p.m_x;
    }
};

#endif // LIBS_NUMERIC_ODEINT_TEST_DUMMY_ODES_HPP_DEFINED


/***************************************************************************
 * Copyright 1998-2018 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxCoreRender.                                   *
 *                                                                         *
 * 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/lexical_cast.hpp>
#include <boost/serialization/export.hpp>

#include "luxrays/kernels/kernels.h"
#include "slg/kernels/kernels.h"
#include "slg/film/film.h"
#include "slg/film/imagepipeline/plugins/tonemaps/reinhard02.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// Reinhard02 tone mapping
//------------------------------------------------------------------------------

BOOST_CLASS_EXPORT_IMPLEMENT(slg::Reinhard02ToneMap)

Reinhard02ToneMap::Reinhard02ToneMap() {
	preScale = 1.f;
	postScale = 1.2f;
	burn = 3.75f;

#if !defined(LUXRAYS_DISABLE_OPENCL)
	oclIntersectionDevice = NULL;
	oclAccumBuffer = NULL;

	opRGBValuesReduceKernel = NULL;
	opRGBValueAccumulateKernel = NULL;
	applyKernel = NULL;
#endif
}

Reinhard02ToneMap::Reinhard02ToneMap(const float preS, const float postS, const float b) {
	preScale = preS;
	postScale = postS;
	burn = b;

#if !defined(LUXRAYS_DISABLE_OPENCL)
	oclIntersectionDevice = NULL;
	oclAccumBuffer = NULL;

	opRGBValuesReduceKernel = NULL;
	opRGBValueAccumulateKernel = NULL;
	applyKernel = NULL;
#endif
}

Reinhard02ToneMap::~Reinhard02ToneMap() {
#if !defined(LUXRAYS_DISABLE_OPENCL)
	delete opRGBValuesReduceKernel;
	delete opRGBValueAccumulateKernel;
	delete applyKernel;

	delete oclAccumBuffer;
#endif
}

//------------------------------------------------------------------------------
// CPU version
//------------------------------------------------------------------------------

void Reinhard02ToneMap::Apply(Film &film, const u_int index) {
	Spectrum *pixels = (Spectrum *)film.channel_IMAGEPIPELINEs[index]->GetPixels();
	RGBColor *rgbPixels = (RGBColor *)pixels;

	const float alpha = .1f;
	const u_int pixelCount = film.GetWidth() * film.GetHeight();

	float Ywa = 0.f;
	for (u_int i = 0; i < pixelCount; ++i) {
		if (*(film.channel_FRAMEBUFFER_MASK->GetPixel(i)) && !rgbPixels[i].IsInf())
			Ywa += logf(Max(rgbPixels[i].Y(), 1e-6f));
	}

	if (pixelCount > 0)
		Ywa = expf(Ywa / pixelCount);

	// Avoid division by zero
	if (Ywa == 0.f)
		Ywa = 1.f;

	const float invB2 = (burn > 0.f) ? 1.f / (burn * burn) : 1e5f;
	const float scale = alpha / Ywa;
	const float preS = scale / preScale;
	const float postS = scale * postScale;

	#pragma omp parallel for
	for (
			// Visual C++ 2013 supports only OpenMP 2.5
#if _OPENMP >= 200805
			unsigned
#endif
			int i = 0; i < pixelCount; ++i) {
		if (*(film.channel_FRAMEBUFFER_MASK->GetPixel(i))) {
			const float ys = rgbPixels[i].Y() * preS;
			// Note: I don't need to convert to XYZ and back because I'm only
			// scaling the value.
			rgbPixels[i] *= postS * (1.f + ys * invB2) / (1.f + ys);
		}
	}
}

//------------------------------------------------------------------------------
// OpenCL version
//------------------------------------------------------------------------------

#if !defined(LUXRAYS_DISABLE_OPENCL)
void Reinhard02ToneMap::ApplyOCL(Film &film, const u_int index) {
	const u_int pixelCount = film.GetWidth() * film.GetHeight();
	const u_int workSize = RoundUp((pixelCount + 1) / 2, 64u);

	if (!applyKernel) {
		// Allocate buffers
		oclIntersectionDevice = film.oclIntersectionDevice;
		film.ctx->SetVerbose(true);
		oclIntersectionDevice->AllocBufferRW(&oclAccumBuffer, (workSize / 64) * sizeof(float) * 3, "Accumulation buffer");
		film.ctx->SetVerbose(false);

		// Compile sources
		const double tStart = WallClockTime();

		cl::Program *program = ImagePipelinePlugin::CompileProgram(
				film,
				"-D LUXRAYS_OPENCL_KERNEL -D SLG_OPENCL_KERNEL",
				slg::ocl::KernelSource_luxrays_types +
				slg::ocl::KernelSource_color_types +
				slg::ocl::KernelSource_color_funcs +
				slg::ocl::KernelSource_tonemap_reinhard02_funcs +
				slg::ocl::KernelSource_tonemap_reduce_funcs,
				"Reinhard02ToneMap");

		SLG_LOG("[Reinhard02ToneMap] Compiling OpRGBValuesReduce Kernel");
		opRGBValuesReduceKernel = new cl::Kernel(*program, "OpRGBValuesReduce");
		SLG_LOG("[Reinhard02ToneMap] Compiling OpRGBValueAccumulate Kernel");
		opRGBValueAccumulateKernel = new cl::Kernel(*program, "OpRGBValueAccumulate");
		SLG_LOG("[Reinhard02ToneMap] Compiling Reinhard02ToneMap_Apply Kernel");
		applyKernel = new cl::Kernel(*program, "Reinhard02ToneMap_Apply");

		delete program;

		// Set kernel arguments
		u_int argIndex = 0;
		opRGBValuesReduceKernel->setArg(argIndex++, film.GetWidth());
		opRGBValuesReduceKernel->setArg(argIndex++, film.GetHeight());
		opRGBValuesReduceKernel->setArg(argIndex++, *(film.ocl_IMAGEPIPELINE));
		opRGBValuesReduceKernel->setArg(argIndex++, *(film.ocl_FRAMEBUFFER_MASK));
		opRGBValuesReduceKernel->setArg(argIndex++, *oclAccumBuffer);

		argIndex = 0;
		opRGBValueAccumulateKernel->setArg(argIndex++, workSize / 64);
		opRGBValueAccumulateKernel->setArg(argIndex++, *oclAccumBuffer);

		argIndex = 0;
		applyKernel->setArg(argIndex++, film.GetWidth());
		applyKernel->setArg(argIndex++, film.GetHeight());
		applyKernel->setArg(argIndex++, *(film.ocl_IMAGEPIPELINE));
		applyKernel->setArg(argIndex++, *(film.ocl_FRAMEBUFFER_MASK));
		const float gamma = GetGammaCorrectionValue(film, index);
		applyKernel->setArg(argIndex++, gamma);
		applyKernel->setArg(argIndex++, preScale);
		applyKernel->setArg(argIndex++, postScale);
		applyKernel->setArg(argIndex++, burn);
		applyKernel->setArg(argIndex++, *oclAccumBuffer);

		const double tEnd = WallClockTime();
		SLG_LOG("[Reinhard02ToneMap] Kernels compilation time: " << int((tEnd - tStart) * 1000.0) << "ms");
	}

	film.oclIntersectionDevice->GetOpenCLQueue().enqueueNDRangeKernel(*opRGBValuesReduceKernel,
			cl::NullRange, cl::NDRange(workSize), cl::NDRange(64));
	film.oclIntersectionDevice->GetOpenCLQueue().enqueueNDRangeKernel(*opRGBValueAccumulateKernel,
			cl::NullRange, cl::NDRange(64), cl::NDRange(64));

	film.oclIntersectionDevice->GetOpenCLQueue().enqueueNDRangeKernel(*applyKernel,
			cl::NullRange, cl::NDRange(RoundUp(pixelCount, 256u)), cl::NDRange(256));
}
#endif


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

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  fma generic tag

      Represents the fma function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct fma_ : ext::elementwise_<fma_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<fma_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_fma_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::fma_, Site> dispatching_fma_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::fma_, Site>();
    }
    template<class... Args>
    struct impl_fma_;
  }
  /*!
    Computes the (fused) multiply add of the three parameters.

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

    @code
    T r = fma(x, y, z);
    @endcode

    The code is similar to:

    @code
    T r = x*y+z;
    @endcode

    @par Note
    Correct fused multiply/add implies

    1) only one rounding

    2) no "intermediate" overflow

    fma provides this each time it is reasonable
    in terms of performance (mainly if the system has the hard
    wired capability).
    If you need fma in all circumstances in your own
    code use correct_fma.

    @par Alias

    @c madd

    @param  a0
    @param  a1
    @param  a2

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::fma_, fma, 3)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::fma_, madd, 3)

  BOOST_DISPATCH_FUNCTION_INTERFACE(tag::fma_, fam, 3)
  {
    typename boost::dispatch::make_functor<tag::fma_, A0>::type callee;
    return callee(a1, a2, a0);
  }

  BOOST_DISPATCH_FUNCTION_INTERFACE(tag::fma_, amul, 3)
  {
    typename boost::dispatch::make_functor<tag::fma_, A0>::type callee;
    return callee(a1, a2, a0);
  }

} }

#endif


/**
 * @file rnn.hpp
 * @author Marcus Edel
 *
 * Definition of the RNN class, which implements recurrent neural networks.
 */
#ifndef __MLPACK_METHODS_ANN_RNN_HPP
#define __MLPACK_METHODS_ANN_RNN_HPP

#include <mlpack/core.hpp>

#include <boost/ptr_container/ptr_vector.hpp>

#include <mlpack/methods/ann/network_traits.hpp>
#include <mlpack/methods/ann/performance_functions/cee_function.hpp>
#include <mlpack/methods/ann/layer/layer_traits.hpp>
#include <mlpack/methods/ann/connections/connection_traits.hpp>

namespace mlpack {
namespace ann /** Artificial Neural Network. */ {

/**
 * An implementation of a recurrent neural network.
 *
 * @tparam ConnectionTypes Tuple that contains all connection module which will
 * be used to construct the network.
 * @tparam OutputLayerType The outputlayer type used to evaluate the network.
 * @tparam PerformanceFunction Performance strategy used to claculate the error.
 * @tparam MaType of data (arma::mat or arma::sp_mat).
 * @tparam VecTypeDelta Type of the delta (arma::colvec, arma::mat or
 * arma::sp_mat).
 */
template <
  typename ConnectionTypes,
  typename OutputLayerType,
  class PerformanceFunction = CrossEntropyErrorFunction<>,
  typename MatType = arma::mat,
  typename VecTypeDelta = arma::colvec
>
class RNN
{
  public:
    /**
     * Construct the RNN object, which will construct a recurrent neural
     * network with the specified layers.
     *
     * @param network The network modules used to construct the network.
     * @param outputLayer The outputlayer used to evaluate the network.
     */
    RNN(const ConnectionTypes& network, OutputLayerType& outputLayer) :
        network(network), err(0),  trainError(0), seqNum(0),
        outputLayer(outputLayer)
    {
      // Nothing to do here.
    }

    /**
     * Run a single iteration of the feed forward algorithm, using the given
     * input and target vector, updating the resulting error into the error
     * vector.
     *
     * @param input Input data used for evaluating the network.
     * @param target Target data used to calculate the network error.
     * @param error The calulated error of the output layer.
     * @tparam VecType Type of target data (arma::colvec, arma::mat or
     * arma::sp_mat).
     */
    template <typename VecType>
    void FeedForward(const MatType& input,
                     const VecType& target,
                     MatType& error)
    {
      // Initialize the activation storage only once.
      if (!activations.size())
      {
        InitLayer(network, input);
      }
      else
      {
        // Expand the activation storage to handle sequences of
        // different length.
        if (activations[0].n_cols < input.n_elem)
        {
          for (size_t i = 0; i < activations.size(); i++)
          {
            activations[i].insert_cols(activations[i].n_cols,
                arma::zeros<MatType>(activations[i].n_rows,
                input.n_elem - activations[i].n_cols));
          }
        }
      }

      seqLen = input.n_rows / inputSize;
      seqOutput = outputSize < target.n_elem ? true : false;
      error = MatType(outputSize, outputSize < target.n_elem ? seqLen : 1);

      // Iterate through the input sequence and perform the feed forward pass.
      for (seqNum = 0; seqNum < seqLen; seqNum++)
      {
        // Reset the network by zeroing the layer activations.
        ResetActivations(network);

        // Set the current input activation.
        std::get<0>(std::get<0>(
            network)).InputLayer().InputActivation() = input.submat(
            seqNum * inputSize, 0, (seqNum + 1) * inputSize - 1, 0);

        // Perform the forward pass and calculate the output error.
        LayerForward(network);
        if (seqOutput)
        {
          arma::colvec seqError = error.unsafe_col(seqNum);
          arma::colvec seqTarget = target.subvec(seqNum * outputSize,
              (seqNum + 1) * outputSize - 1);

          OutputError(network, seqTarget, seqError);
        }

        // Save the network activation for the backward/forward pass and update
        // the recurrent connections.
        if (seqNum < (input.n_rows / inputSize - 1))
        {
          layerNum = 0;
          SaveActivations(network);
        }
      }

      // Calculate the error only once for a non-sequence input.
      if (!seqOutput)
      {
        seqNum = 0;
        arma::colvec seqError = error.unsafe_col(seqNum);
        OutputError(network, target, seqError);
      }
    }

    /**
     * Run a single iteration of the feed backward algorithm, using the given
     * error of the output layer.
     *
     * @param error The calulated error of the output layer.
     */
    void FeedBackward(const MatType& error)
    {
      // Reset the network deltas by zeroing the storage.
      for (size_t i = 0; i < delta.size(); i++)
          delta[i].zeros();

      // Iterate through the input sequence and perform the feed backward pass.
      for (seqNum = seqLen - 1; seqNum >= 0; seqNum--)
      {
        deltaNum = 0;

        // Perform the backward pass and update the gradient storage.
        arma::colvec seqError = error.unsafe_col(seqOutput ? seqNum : 0);
        LayerBackward(network, seqError);
        UpdateGradients(network);

        // Load the network activation for the upcoming backward pass.
        if (seqNum > 0)
        {
          layerNum = 0;
          LoadActivations(network);
        }
        else if (seqNum == 0) break;
      }
    }

    /**
     * Updating the weights using the specified optimizer.
     *
     */
    void ApplyGradients()
    {
      ApplyGradients(network);

      // Reset the overall error.
      err = 0;
      trainError = 0;
      seqNum = 0;
    }

    /**
     * Evaluate the network using the given input. The output activation is
     * stored into the output parameter.
     *
     * @param input Input data used to evaluate the network.
     * @param output Output data used to store the output activation
     * @tparam VecType Type of data (arma::colvec, arma::mat or arma::sp_mat).
     */
    template <typename VecType>
    void Predict(const VecType& input, VecType& output)
    {
      seqLen = input.n_rows / inputSize;

      // Iterate through the input sequence and perform the feed forward pass.
      for (seqNum = 0; seqNum < seqLen; seqNum++)
      {
        // Reset the network by zeroing the layer activations.
        ResetActivations(network);

        // Set the current input activation.
        std::get<0>(std::get<0>(
            network)).InputLayer().InputActivation() = input.submat(
            seqNum * inputSize, 0, (seqNum + 1) * inputSize - 1, 0);

        // Perform the forward pass and calculate the output error.
        LayerForward(network);
        if (seqOutput)
        {
          arma::colvec targetCol;
          OutputPrediction(network, targetCol);
          output = arma::join_cols(output, targetCol);
        }

        // Save the network activation for the backward/forward pass and update
        // the recurrent connections.
        if (seqNum < (input.n_rows / inputSize - 1))
        {
          layerNum = 0;
          SaveActivations(network);
        }
      }

      if (!seqOutput)
        OutputPrediction(network, output);
    }

    /**
     * Evaluate the trained network using the given input and compare the output
     * with the given target vector.
     *
     * @param input Input data used to evaluate the trained network.
     * @param target Target data used to calculate the network error.
     * @param error The calulated error of the output layer.
     * @tparam VecType Type of data (arma::colvec, arma::mat or arma::sp_mat).
     */
    template <typename VecType>
    double Evaluate(const MatType& input,
                     const VecType& target,
                     MatType& error)
    {
      FeedForward(input, target, error);
      return err;
    }

    //! Get the error of the network.
    double Error() const { return trainError; }

  private:
    /**
     * Helper function to reset the network by zeroing the layer activations.
     *
     * enable_if (SFINAE) is used to iterate through the network connection
     * modules. The general case peels off the first type and recurses, as usual
     * with variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    ResetActivations(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    ResetActivations(std::tuple<Tp...>& t)
    {
      Reset(std::get<I>(t));
      ResetActivations<I + 1, Tp...>(t);
    }

    /**
     * Reset the network by zeroing the layer activations.
     *
     * enable_if (SFINAE) is used to iterate through the network connections.
     * The general case peels off the first type and recurses, as usual with
     * variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    Reset(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    Reset(std::tuple<Tp...>& t)
    {
      Parameter<I, typename std::remove_reference<
          decltype(std::get<I>(t).InputLayer())>::type, Tp...>(t);

      std::get<I>(t).OutputLayer().InputActivation().zeros(
          std::get<I>(t).OutputLayer().InputSize());

      // Reset the recurrent connection only at the beginning of a new sequence.
      if (seqNum == 0 && (ConnectionTraits<typename std::remove_reference<
          decltype(std::get<I>(t))>::type>::IsSelfConnection ||
          ConnectionTraits<typename std::remove_reference<decltype(
          std::get<I>(t))>::type>::IsFullselfConnection))
      {
        std::get<I>(t).InputLayer().InputActivation().zeros(
          std::get<I>(t).InputLayer().InputSize());
      }

      Reset<I + 1, Tp...>(t);
    }

    /**
     * Update the sequence length for a specific layer.
     *
     * enable_if (SFINAE) is used to determine if classes passed contains the
     * SeqLen function.
     */
    template<size_t I, typename LayerType, typename... Tp>
    typename std::enable_if<
        LayerTraits<LayerType>::IsLSTMLayer == false, void>::type
    Parameter(std::tuple<Tp...>& /* unused */) { }

    /**
     * Update the sequence length for a specific layer.
     *
     * enable_if (SFINAE) is used to determine if classes passed contains the
     * SeqLen function.
     */
    template<size_t I, typename LayerType, typename... Tp>
    typename std::enable_if<
        LayerTraits<LayerType>::IsLSTMLayer == true, void>::type
    Parameter(std::tuple<Tp...>& t)
    {
      std::get<I>(t).InputLayer().SeqLen() = seqLen;
    }

    /**
     * Run a single iteration of the feed forward algorithm, using the given
     * input and target vector, updating the resulting error into the error
     * vector.
     *
     * enable_if (SFINAE) is used to select between two template overloads of
     * the get function - one for when I is equal the size of the tuple of
     * connections, and one for the general case which peels off the first type
     * and recurses, as usual with variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    LayerForward(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    LayerForward(std::tuple<Tp...>& t)
    {
      ConnectionForward(std::get<I>(t));

      // Use the first connection to perform the feed forward algorithm.
      std::get<0>(std::get<I>(t)).OutputLayer().FeedForward(
          std::get<0>(std::get<I>(t)).OutputLayer().InputActivation(),
          std::get<0>(std::get<I>(t)).OutputLayer().InputActivation());

      LayerForward<I + 1, Tp...>(t);
    }

    /**
     * Sum up all layer activations by evaluating all connections.
     *
     * enable_if (SFINAE) is used to iterate through the network connections.
     * The general case peels off the first type and recurses, as usual with
     * variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    ConnectionForward(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    ConnectionForward(std::tuple<Tp...>& t)
    {
      std::get<I>(t).FeedForward(std::get<I>(t).InputLayer().InputActivation());
      ConnectionForward<I + 1, Tp...>(t);
    }

    /*
     * Calculate the output error and update the overall error.
     */
    template<typename VecType, typename... Tp>
    void OutputError(std::tuple<Tp...>& t,
                     const VecType& target,
                     VecType& error)
    {
       // Calculate and store the output error.
      outputLayer.CalculateError(std::get<0>(
          std::get<sizeof...(Tp) - 1>(t)).OutputLayer().InputActivation(),
          target, error);

      // Save the output activation for the upcoming feed backward pass.
      activations.back().unsafe_col(seqNum) = std::get<0>(
          std::get<sizeof...(Tp) - 1>(t)).OutputLayer().InputActivation();

      // Masures the network's performance with the specified performance
      // function.
      err = PerformanceFunction::Error(std::get<0>(
          std::get<sizeof...(Tp) - 1>(t)).OutputLayer().InputActivation(),
          target);

      // Update the overall training error.
      trainError += err;
    }

    /*
     * Calculate and store the output activation.
     */
    template<typename VecType, typename... Tp>
    void OutputPrediction(std::tuple<Tp...>& t, VecType& output)
    {
       // Calculate and store the output prediction.
      outputLayer.OutputClass(std::get<0>(
          std::get<sizeof...(Tp) - 1>(t)).OutputLayer().InputActivation(),
          output);
    }

    /**
     * Run a single iteration of the feed backward algorithm, using the given
     * error of the output layer. Note that we iterate backward through the
     * connection modules.
     *
     * enable_if (SFINAE) is used to select between two template overloads of
     * the get function - one for when I is equal the size of the tuple of
     * connections, and one for the general case which peels off the first type
     * and recurses, as usual with variadic function templates.
     */
    template<size_t I = 0, typename VecType, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp) + 1, void>::type
    LayerBackward(std::tuple<Tp...>& /* unused */, VecType& /* unused */)
    { }

    template<size_t I = 1, typename VecType, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp) + 1, void>::type
    LayerBackward(std::tuple<Tp...>& t, VecType& error)
    {
      // Distinguish between the output layer and the other layer. In case of
      // the output layer use the specified error vector to store the error and
      // to perform the feed backward pass.
      if (I == 1)
      {
        // Use the first connection from the last connection module to
        // calculate the error.
        std::get<0>(std::get<sizeof...(Tp) - I>(t)).OutputLayer().FeedBackward(
            activations.back().unsafe_col(seqNum), error,
            std::get<0>(std::get<sizeof...(Tp) - I>(t)).OutputLayer().Delta());
      }

      ConnectionBackward(std::get<sizeof...(Tp) - I>(t), std::get<0>(std::get<
          sizeof...(Tp) - I>(t)).OutputLayer().Delta(), I, sizeof...(Tp));

      LayerBackward<I + 1, VecType, Tp...>(t, error);
    }

    /**
     * Back propagate the given error and store the delta in the connection
     * between the corresponding layer.
     *
     * enable_if (SFINAE) is used to iterate through the network connections.
     * The general case peels off the first type and recurses, as usual with
     * variadic function templates.
     */
    template<size_t I = 0, typename VecType, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    ConnectionBackward(std::tuple<Tp...>& /* unused */,
                       VecType& /* unused */,
                       const size_t /* unused */,
                       const size_t /* unused */) { }

    template<size_t I = 0, typename VecType, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    ConnectionBackward(std::tuple<Tp...>& t,
                       VecType& error,
                       const size_t layer,
                       const size_t layerNum)
    {
      std::get<I>(t).FeedBackward(error);

      // Update the recurrent delta.
      if (ConnectionTraits<typename std::remove_reference<decltype(
          std::get<I>(t))>::type>::IsSelfConnection ||
          ConnectionTraits<typename std::remove_reference<decltype(
          std::get<I>(t))>::type>::IsFullselfConnection)
      {
        std::get<I>(t).FeedBackward(delta[deltaNum]);
        delta[deltaNum++] = std::get<I>(t).Delta();
      }

      // We calculate the delta only for non bias layer and self connections.
      if (!(ConnectionTraits<typename std::remove_reference<decltype(
            std::get<I>(t))>::type>::IsSelfConnection ||
        LayerTraits<typename std::remove_reference<decltype(
            std::get<I>(t).InputLayer())>::type>::IsBiasLayer ||
        ConnectionTraits<typename std::remove_reference<decltype(
            std::get<I>(t))>::type>::IsFullselfConnection) && layer < layerNum)
      {
        // Sum up the stored delta for recurrent connections.
        if (recurrentLayer[layer])
        {
          std::get<I>(t).Delta() += delta[deltaNum].subvec(
              0, std::get<I>(t).InputLayer().OutputSize() - 1);
        }

        // Perform the backward pass.
        std::get<I>(t).InputLayer().FeedBackward(
            std::get<I>(t).InputLayer().InputActivation(),
            std::get<I>(t).Delta(), std::get<I>(t).InputLayer().Delta());

        // Update the delta storage for the next backward pass.
        if (recurrentLayer[layer])
          delta[deltaNum] = std::get<I>(t).InputLayer().Delta();
      }

      ConnectionBackward<I + 1, VecType, Tp...>(t, error, layer, layerNum);
    }

    /**
     * Helper function to update the gradient storage.
     *
     * enable_if (SFINAE) is used to select between two template overloads of
     * the get function - one for when I is equal the size of the tuple of
     * connections, and one for the general case which peels off the first type
     * and recurses, as usual with variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    UpdateGradients(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    UpdateGradients(std::tuple<Tp...>& t)
    {
      Gradients(std::get<I>(t));
      UpdateGradients<I + 1, Tp...>(t);
    }

    /**
     * Sum up all gradients and store the results in the gradients storage.
     *
     * enable_if (SFINAE) is used to iterate through the network connections.
     * The general case peels off the first type and recurses, as usual with
     * variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    Gradients(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    Gradients(std::tuple<Tp...>& t)
    {
      std::get<I>(t).Optimizer().Update();
      Gradients<I + 1, Tp...>(t);
    }

    /**
     * Helper function to update the weights using the specified optimizer and
     * the given input.
     *
     * enable_if (SFINAE) is used to select between two template overloads of
     * the get function - one for when I is equal the size of the tuple of
     * connections, and one for the general case which peels off the first type
     * and recurses, as usual with variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    ApplyGradients(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    ApplyGradients(std::tuple<Tp...>& t)
    {
      Apply(std::get<I>(t));
      ApplyGradients<I + 1, Tp...>(t);
    }

    /**
     * Update the weights using the specified optimizer,the given input and the
     * calculated delta.
     *
     * enable_if (SFINAE) is used to select between two template overloads of
     * the get function - one for when I is equal the size of the tuple of
     * connections, and one for the general case which peels off the first type
     * and recurses, as usual with variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    Apply(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    Apply(std::tuple<Tp...>& t)
    {
      std::get<I>(t).Optimizer().Optimize();
      std::get<I>(t).Optimizer().Reset();

      Apply<I + 1, Tp...>(t);
    }

    /**
     * Helper function to iterate through all connection modules and to build
     * the activation, gradients and delta storage.
     *
     * enable_if (SFINAE) is used to select between two template overloads of
     * the get function - one for when I is equal the size of the tuple of
     * connections, and one for the general case which peels off the first type
     * and recurses, as usual with variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    InitLayer(std::tuple<Tp...>& t, const MatType& input)
    {
      recurrentLayer.push_back(false);
      outputSize = std::get<0>(std::get<I - 1>(t)).OutputLayer().OutputSize();
      activations.push_back(new MatType(outputSize, input.n_elem));
    }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    InitLayer(std::tuple<Tp...>& t, const MatType& input)
    {
      if (I == 0)
        inputSize = std::get<0>(std::get<I>(t)).InputLayer().InputSize();

      recurrentLayer.push_back(false);
      Recurrent(std::get<sizeof...(Tp) - I - 1>(t));

      Layer(std::get<I>(t), input, I);
      InitLayer<I + 1, Tp...>(t, input);
    }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    Recurrent(std::tuple<Tp...>& /* unusded */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    Recurrent(std::tuple<Tp...>& t)
    {
      if (ConnectionTraits<typename std::remove_reference<decltype(
              std::get<I>(t))>::type>::IsSelfConnection ||
          ConnectionTraits<typename std::remove_reference<decltype(
            std::get<I>(t))>::type>::IsFullselfConnection)
      {
        recurrentLayer.back() = true;
        delta.push_back(new VecTypeDelta(std::get<I>(t).Weights().n_rows));
      }
      else
      {
        Recurrent<I + 1, Tp...>(t);
      }
    }

    /**
     * Iterate through all connections and build the the activation, gradients
     * and delta storage.
     *
     * enable_if (SFINAE) is used to select between two template overloads of
     * the get function - one for when I is equal the size of the tuple of
     * connections, and one for the general case which peels off the first type
     * and recurses, as usual with variadic function templates.
     */
    template<size_t I = 0, typename VecType, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    Layer(std::tuple<Tp...>& /* unusded */,
          const VecType& /* unused */,
          const size_t /* unsued */) { }

    template<size_t I = 0, typename VecType, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    Layer(std::tuple<Tp...>& t, const VecType& input, const size_t layer)
    {
      activations.push_back(new MatType(
        std::get<I>(t).InputLayer().OutputSize(), input.n_elem));

      Layer<I + 1, VecType, Tp...>(t, input, layer);
    }

    /**
     * Helper function to iterate through all connection modules and to load
     * the layer activations.
     *
     * enable_if (SFINAE) is used to iterate through the network connection
     * modules. The general case peels off the first type and recurses, as usual
     * with variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    LoadActivations(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    LoadActivations(std::tuple<Tp...>& t)
    {
      Load(std::get<I>(t));
      LoadActivations<I + 1, Tp...>(t);
    }

    /**
     * Load and set the network layer activations.
     *
     * enable_if (SFINAE) is used to iterate through the network connections.
     * The general case peels off the first type and recurses, as usual with
     * variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    Load(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    Load(std::tuple<Tp...>& t)
    {
      std::get<I>(t).InputLayer().InputActivation() =
          activations[layerNum++].unsafe_col(seqNum - 1);
      Load<I + 1, Tp...>(t);
    }

    /**
     * Helper function to iterate through all connection modules and to save
     * the layer activations.
     *
     * enable_if (SFINAE) is used to iterate through the network connection
     * modules. The general case peels off the first type and recurses, as usual
     * with variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    SaveActivations(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    SaveActivations(std::tuple<Tp...>& t)
    {
      Save(std::get<I>(t));
      SaveActivations<I + 1, Tp...>(t);
    }

    /**
     * Save the network layer activations.
     *
     * enable_if (SFINAE) is used to iterate through the network connections.
     * The general case peels off the first type and recurses, as usual with
     * variadic function templates.
     */
    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I == sizeof...(Tp), void>::type
    Save(std::tuple<Tp...>& /* unused */) { }

    template<size_t I = 0, typename... Tp>
    typename std::enable_if<I < sizeof...(Tp), void>::type
    Save(std::tuple<Tp...>& t)
    {
      activations[layerNum++].unsafe_col(seqNum) =
          std::get<I>(t).InputLayer().InputActivation();

      // Use the activation from the corresponding outputlayer for
      // self connections.
      if (ConnectionTraits<typename std::remove_reference<decltype(
              std::get<I>(t))>::type>::IsSelfConnection ||
          ConnectionTraits<typename std::remove_reference<decltype(
              std::get<I>(t))>::type>::IsFullselfConnection)
      {
        std::get<I>(t).InputLayer().InputActivation() =
            std::get<I>(t).OutputLayer().InputActivation();
      }

      Save<I + 1, Tp...>(t);
    }

    //! The layer we are using to build the network.
    ConnectionTypes network;

    //! The current error of the network.
    double err;

    //! The current training error of the network.
    double trainError;

    //! The activation storage we are using to perform the feed backward pass.
    boost::ptr_vector<MatType> activations;

    //! The index of the current sequence number.
    size_t seqNum;

    //! The index of the currently activate layer.
    size_t layerNum;

    //! The index of the currently activate delta.
    size_t deltaNum;

    //! Locally stored network output size.
    size_t outputSize;

    //! Locally stored network input size.
    size_t inputSize;

    //! Locally stored parameter that indicates if the input is a sequence.
    bool seqOutput;

    //! The outputlayer used to evaluate the network
    OutputLayerType& outputLayer;

    //! Locally stored number of samples in one input sequence.
    size_t seqLen;

    //! The recurrentLayer storage we are using to perform the backward pass.
    std::vector<bool> recurrentLayer;

    //! The detla storage we are using to perform the feed backward pass.
    boost::ptr_vector<VecTypeDelta> delta;
}; // class RNN

//! Network traits for the FFNN network.
template <
  typename ConnectionTypes,
  typename OutputLayerType,
  class PerformanceFunction
>
class NetworkTraits<RNN<ConnectionTypes, OutputLayerType, PerformanceFunction> >
{
 public:
  static const bool IsFNN = false;
  static const bool IsRNN = true;
  static const bool IsCNN = false;
};

}; // namespace ann
}; // namespace mlpack

#endif


/** $Id: NaturalRateInjector.cxx 165001 2018-08-27 09:22:30Z jvansanten $
 * @file
 * @author Jakob van Santen <vansanten@wisc.edu>
 *
 * $Revision: 165001 $
 * $Date: 2018-08-27 03:22:30 -0600 (Mon, 27 Aug 2018) $
 */

#include <MuonGun/I3MuonGun.h>
#include <MuonGun/NaturalRateInjector.h>
#include <MuonGun/Cylinder.h>
#include <dataclasses/I3Constants.h>

#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>

#include <icetray/I3Module.h>
#include <dataclasses/I3Double.h>
#include <dataclasses/physics/I3MCTree.h>
#include <dataclasses/physics/I3MCTreeUtils.h>
#include <phys-services/I3RandomService.h>

namespace I3MuonGun {

namespace {

std::string
GetTablePath(const std::string &subpath)
{
	std::ostringstream tablePath;
	tablePath << getenv("I3_BUILD") << "/MuonGun/resources/tables/" << subpath;
	return tablePath.str();
}

}

template <typename Archive>
void
NaturalRateInjector::serialize(Archive &ar, unsigned version)
{
	if (version > 0)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("Generator", base_object<Generator>(*this));
	ar & make_nvp("Surface", surface_);

	ar & make_nvp("Flux", flux_);
	ar & make_nvp("EnergyRadiusDistribution", energyDistribution_);
	ar & make_nvp("TotalRate", totalRate_);
}

NaturalRateInjector::NaturalRateInjector()
{
	SetSurface(boost::make_shared<Cylinder>(1600, 800));

	SetFlux(boost::make_shared<SplineFlux>(
	    GetTablePath("Hoerandel5_atmod12_SIBYLL.single_flux.fits"),
	    GetTablePath("Hoerandel5_atmod12_SIBYLL.bundle_flux.fits")));

	SetEnergyDistribution(boost::make_shared<SplineEnergyDistribution>(
	    GetTablePath("Hoerandel5_atmod12_SIBYLL.single_energy.fits"),
	    GetTablePath("Hoerandel5_atmod12_SIBYLL.bundle_energy.fits")));
}

NaturalRateInjector::NaturalRateInjector(SamplingSurfacePtr surface, FluxPtr flux,
    EnergyDistributionPtr edist)
{
	SetSurface(surface);
	SetFlux(flux);
	SetEnergyDistribution(edist);
}

GenerationProbabilityPtr
NaturalRateInjector::Clone() const
{
	return boost::make_shared<NaturalRateInjector>(*this);
}

bool
NaturalRateInjector::IsCompatible(GenerationProbabilityConstPtr o) const
{
	boost::shared_ptr<const NaturalRateInjector> other
	    = boost::dynamic_pointer_cast<const NaturalRateInjector>(o);
	if (!other)
		return false;
	return (*surface_ == *(other->surface_) && *flux_ == *(other->flux_)
	    && *energyDistribution_ == *(other->energyDistribution_));
}

void
NaturalRateInjector::SetSurface(SamplingSurfacePtr p)
{
	assert(p);
	surface_ = p;
	totalRate_ = NAN;
}

void
NaturalRateInjector::SetFlux(FluxPtr p)
{
	assert(p);
	flux_ = p;
	totalRate_ = NAN;
}

void
NaturalRateInjector::SetEnergyDistribution(EnergyDistributionPtr e)
{
	assert(e);

	double norm = e->Integrate(surface_->GetMinDepth(), 1., flux_->GetMinMultiplicity(), 0, 300, e->GetMin(), e->GetMax());
	if (std::abs(norm-1) > 1e-1)
		log_fatal_stream("The provided energy distribution is not normalized "
		    "(integrates to "<<norm<<"). This could be because the minimum and "
		    "maximum energies were changed from their defaults.");
	energyDistribution_ = e;
}

double
NaturalRateInjector::GetTotalRate() const
{
	if (std::isnan(totalRate_) && surface_ && flux_) {
		totalRate_ = 0;
		for (unsigned m = flux_->GetMinMultiplicity(); m <= flux_->GetMaxMultiplicity(); m++) {
			totalRate_ += surface_->IntegrateFlux(boost::bind(boost::cref(*flux_), _1, _2, m));
		}
	}
	return totalRate_;
}

void
NaturalRateInjector::GenerateAxis(I3RandomService &rng, std::pair<I3Particle, unsigned> &axis) const
{
	// Choose a direction and impact position from a uniform flux through
	// the sampling surface, then pick a multiplicity. Accept zenith angles
	// and multiplicities at a rate proportional to the flux at the chosen
	// depth.
	I3Direction dir;
	I3Position pos;
	unsigned m;
	double flux;
	// For any sane distribution the maximum flux is for single vertical muons
	// at minimum depth
	double maxflux = (*flux_)(surface_->GetMinDepth(), 1., flux_->GetMinMultiplicity());
	do {
		surface_->SampleImpactRay(pos, dir, rng);
		m = rng.Integer(flux_->GetMaxMultiplicity() - flux_->GetMinMultiplicity())
		    + flux_->GetMinMultiplicity();
		flux = (*flux_)(GetDepth(pos.GetZ()), cos(dir.GetZenith()), m);
	} while (rng.Uniform(0., maxflux) > flux);

	axis.first.SetPos(pos);
	axis.first.SetDir(dir);
	axis.first.SetShape(I3Particle::Primary);
	axis.first.SetLocationType(I3Particle::Anywhere);
	axis.first.SetType(I3Particle::unknown);
	axis.first.SetTime(0.);
	axis.second = m;
}

void
NaturalRateInjector::FillMCTree(I3RandomService &rng,
    const std::pair<I3Particle, unsigned> &axis,
    I3MCTree &mctree, BundleConfiguration &bundlespec) const
{
	const I3Particle &primary = axis.first;
	I3MCTreeUtils::AddPrimary(mctree, primary);
	double h = GetDepth(primary.GetPos().GetZ());
	double coszen = cos(primary.GetDir().GetZenith());

	unsigned m = axis.second;
	auto tracks = energyDistribution_->Generate(rng, h, coszen, m, m);
	for (auto &radius_energy : tracks) {
		I3Particle track = CreateParallelTrack(radius_energy.first, rng.Uniform(0., 2*M_PI), *surface_, primary);
		track.SetEnergy(radius_energy.second);
		I3MCTreeUtils::AppendChild(mctree, primary, track);
		bundlespec.push_back(BundleEntry(radius_energy.first, radius_energy.second));
	}
}

void
NaturalRateInjector::Generate(I3RandomService &rng, I3MCTree &mctree, BundleConfiguration &bundlespec) const
{
	std::pair<I3Particle, unsigned> axis;
	GenerateAxis(rng, axis);
	FillMCTree(rng, axis, mctree, bundlespec);
}

double
NaturalRateInjector::GetLogGenerationProbability(const I3Particle &axis,
    const BundleConfiguration &bundlespec) const
{
	std::pair<double, double> steps = surface_->GetIntersection(axis.GetPos(), axis.GetDir());
	// This shower axis doesn't intersect the sampling surface. Bail.
	if (!std::isfinite(steps.first))
		return -std::numeric_limits<double>::infinity();

	double h = GetDepth(axis.GetPos().GetZ() + steps.first*axis.GetDir().GetZ());
	double coszen = cos(axis.GetDir().GetZenith());
	unsigned m = static_cast<unsigned>(bundlespec.size());

	// We used the flux to do rejection sampling in depth, zenith, and
	// multiplicity. Evaluate the properly-normalized PDF here.
	double logprob = flux_->GetLog(h, coszen, m) - std::log(GetTotalRate());
	BOOST_FOREACH(const BundleEntry &track, bundlespec) {
		logprob += energyDistribution_->GetLog(h, coszen, m, track.radius,
		    EnergyDistribution::log_value(std::log(track.energy)));
	}

	return logprob;
}

}

I3_SERIALIZABLE(I3MuonGun::NaturalRateInjector);


// Boost.Geometry Index
//
// Quickbook Examples
//
// Copyright (c) 2011-2013 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)

//[rtree_value_shared_ptr

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>

#include <boost/geometry/index/rtree.hpp>

#include <cmath>
#include <vector>
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

namespace boost { namespace geometry { namespace index {

template <typename Box>
struct indexable< boost::shared_ptr<Box> >
{
    typedef boost::shared_ptr<Box> V;

    typedef Box const& result_type;
    result_type operator()(V const& v) const { return *v; }
};

}}} // namespace boost::geometry::index

int main()
{
    typedef bg::model::point<float, 2, bg::cs::cartesian> point;
    typedef bg::model::box<point> box;
    typedef boost::shared_ptr<box> shp;
    typedef shp value;

    // create the rtree using default constructor
    bgi::rtree< value, bgi::linear<16, 4> > rtree;

    std::cout << "filling index with boxes shared pointers:" << std::endl;

    // fill the spatial index
    for ( unsigned i = 0 ; i < 10 ; ++i )
    {
        // create a box
        shp b(new box(point(i+0.0f, i+0.0f), point(i+0.5f, i+0.5f)));

        // display new box
        std::cout << bg::wkt<box>(*b) << std::endl;

        // insert new value
        rtree.insert(b);
    }

    // find values intersecting some area defined by a box
    box query_box(point(0, 0), point(5, 5));
    std::vector<value> result_s;
    rtree.query(bgi::intersects(query_box), std::back_inserter(result_s));

    // find 5 nearest values to a point
    std::vector<value> result_n;
    rtree.query(bgi::nearest(point(0, 0), 5), std::back_inserter(result_n));

    // note: in Boost.Geometry the WKT representation of a box is polygon

    // display results
    std::cout << "spatial query box:" << std::endl;
    std::cout << bg::wkt<box>(query_box) << std::endl;
    std::cout << "spatial query result:" << std::endl;
    BOOST_FOREACH(value const& v, result_s)
        std::cout << bg::wkt<box>(*v) << std::endl;

    std::cout << "knn query point:" << std::endl;
    std::cout << bg::wkt<point>(point(0, 0)) << std::endl;
    std::cout << "knn query result:" << std::endl;
    BOOST_FOREACH(value const& v, result_n)
        std::cout << bg::wkt<box>(*v) << std::endl;

    return 0;
}

//]


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//
#include "pxr/base/gf/lineSeg.h"

#include "pxr/base/tf/pyUtils.h"
#include "pxr/base/tf/wrapTypeHelpers.h"

#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/operators.hpp>
#include <boost/python/return_arg.hpp>
#include <boost/python/tuple.hpp>

#include <string>

using namespace boost::python;

using std::string;

static string _Repr(GfLineSeg const &self) {
    return TF_PY_REPR_PREFIX + "LineSeg(" + TfPyRepr(self.GetPoint(0.0)) + ", " +
        TfPyRepr(self.GetPoint(1.0)) + ")";
}

static tuple
FindClosestPointsHelper1( const GfLine &l1, const GfLineSeg &l2 )
{
    GfVec3d p1(0), p2(0);
    double t1 = 0, t2 = 0;
    bool result = GfFindClosestPoints( l1, l2, &p1, &p2, &t1, &t2 );
    return boost::python::make_tuple( result, p1, p2, t1, t2 );
}

static tuple
FindClosestPointsHelper2( const GfLineSeg &l1, const GfLineSeg &l2 )
{
    GfVec3d p1(0), p2(0);
    double t1 = 0, t2 = 0;
    bool result = GfFindClosestPoints( l1, l2, &p1, &p2, &t1, &t2 );
    return boost::python::make_tuple( result, p1, p2, t1, t2 );
}

static tuple
FindClosestPointHelper( const GfLineSeg &self, const GfVec3d &point )
{
    double t;
    GfVec3d p1 = self.FindClosestPoint( point, &t );
    return boost::python::make_tuple( p1, t );
}

void wrapLineSeg()
{
    typedef GfLineSeg This;

    def("FindClosestPoints", FindClosestPointsHelper1,
        "FindClosestPoints( l1, s2 ) -> tuple< intersects = bool, "
        "p1 = GfVec3d, p2 = GfVec3d, t1 = double, t2 = double>"
        "\n\n"
        "l1 : GfLine\n"
        "s2 : GfLineSeg"
        "\n\n"
        "Computes the closest points between a line and a line "
        "segment, returning a tuple. The first item in the tuple "
        "is true if they intersect. The two points are returned in "
        "p1 and p2.  The parametric distance of each point on the "
        "line and line segment is returned in t1 and t2.\n"
        "----------------------------------------------------------------------");
    def("FindClosestPoints", FindClosestPointsHelper2,
        "FindClosestPoints( s1, s2 ) -> tuple<result = bool,"
        "p1 = GfVec3d, p2 = GfVec3d, t1 = double, t2 = double>"
        "\n\n"
        "l1 : GfLineSeg\n"
        "l2 : GfLineSeg"
        "\n\n"
        "Computes the closest points between two line segments, "
        "returning a tuple.  The first item in the tuple "
        "is true if they intersect.  The two points are returned in "
        "p1 and p2.  The parametric distance of each point on the "
        "line and line segment is returned in t1 and t2.\n"
        "----------------------------------------------------------------------");

    object getDirection = make_function(&This::GetDirection,
                                        return_value_policy<return_by_value>());

    class_<This>( "LineSeg", "Line segment class", init<>() )
        .def(init< const GfVec3d &, const GfVec3d & >())

        .def( TfTypePythonClass() )

        .def("GetDirection", getDirection)
        .def("GetLength", &This::GetLength)
        .def("GetPoint", &This::GetPoint)

        .add_property("direction", getDirection)
        .add_property("length", &This::GetLength)

        .def( "FindClosestPoint", FindClosestPointHelper )

        .def( str(self) )
        .def( self == self )
        .def( self != self )

        .def("__repr__", _Repr)
        ;

}