#ifndef CAFFE_VISION_LAYERS_HPP_
#define CAFFE_VISION_LAYERS_HPP_

#include <string>
#include <utility>
#include <vector>

#include <boost/unordered_map.hpp>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/common_layers.hpp"
#include "caffe/data_layers.hpp"
#include "caffe/layer.hpp"
#include "caffe/loss_layers.hpp"
#include "caffe/neuron_layers.hpp"
#include "caffe/proto/caffe.pb.h"

namespace caffe {

/**
 * @brief Abstract base class that factors out the BLAS code common to
 *        ConvolutionLayer and DeconvolutionLayer.
 */
template <typename Dtype>
class BaseConvolutionLayer : public Layer<Dtype> {
 public:
  explicit BaseConvolutionLayer(const LayerParameter& param)
      : Layer<Dtype>(param) {}
  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline int MinBottomBlobs() const { return 1; }
  virtual inline int MinTopBlobs() const { return 1; }
  virtual inline bool EqualNumBottomTopBlobs() const { return true; }

 protected:
  // Helper functions that abstract away the column buffer and gemm arguments.
  // The last argument in forward_cpu_gemm is so that we can skip the im2col if
  // we just called weight_cpu_gemm with the same input.
  void forward_cpu_gemm(const Dtype* input, const Dtype* weights,
      Dtype* output, bool skip_im2col = false);
  void forward_cpu_bias(Dtype* output, const Dtype* bias);
  void backward_cpu_gemm(const Dtype* input, const Dtype* weights,
      Dtype* output);
  void weight_cpu_gemm(const Dtype* input, const Dtype* output, Dtype*
      weights);
  void backward_cpu_bias(Dtype* bias, const Dtype* input);

#ifndef CPU_ONLY
  void forward_gpu_gemm(const Dtype* col_input, const Dtype* weights,
      Dtype* output, bool skip_im2col = false);
  void forward_gpu_bias(Dtype* output, const Dtype* bias);
  void backward_gpu_gemm(const Dtype* input, const Dtype* weights,
      Dtype* col_output);
  void weight_gpu_gemm(const Dtype* col_input, const Dtype* output, Dtype*
      weights);
  void backward_gpu_bias(Dtype* bias, const Dtype* input);
#endif

  // reverse_dimensions should return true iff we are implementing deconv, so
  // that conv helpers know which dimensions are which.
  virtual bool reverse_dimensions() = 0;
  // Compute height_out_ and width_out_ from other parameters.
  virtual void compute_output_shape() = 0;

  int kernel_h_, kernel_w_;
  int stride_h_, stride_w_;
  int num_;
  int channels_;
  int pad_h_, pad_w_;
  int height_, width_;
  int group_;
  int num_output_;
  int height_out_, width_out_;
  bool bias_term_;
  bool is_1x1_;

 private:
  // wrap im2col/col2im so we don't have to remember the (long) argument lists
  inline void conv_im2col_cpu(const Dtype* data, Dtype* col_buff) {
    im2col_cpu(data, conv_in_channels_, conv_in_height_, conv_in_width_,
        kernel_h_, kernel_w_, pad_h_, pad_w_, stride_h_, stride_w_, col_buff);
  }
  inline void conv_col2im_cpu(const Dtype* col_buff, Dtype* data) {
    col2im_cpu(col_buff, conv_in_channels_, conv_in_height_, conv_in_width_,
        kernel_h_, kernel_w_, pad_h_, pad_w_, stride_h_, stride_w_, data);
  }
#ifndef CPU_ONLY
  inline void conv_im2col_gpu(const Dtype* data, Dtype* col_buff) {
    im2col_gpu(data, conv_in_channels_, conv_in_height_, conv_in_width_,
        kernel_h_, kernel_w_, pad_h_, pad_w_, stride_h_, stride_w_, col_buff);
  }
  inline void conv_col2im_gpu(const Dtype* col_buff, Dtype* data) {
    col2im_gpu(col_buff, conv_in_channels_, conv_in_height_, conv_in_width_,
        kernel_h_, kernel_w_, pad_h_, pad_w_, stride_h_, stride_w_, data);
  }
#endif

  int conv_out_channels_;
  int conv_in_channels_;
  int conv_out_spatial_dim_;
  int conv_in_height_;
  int conv_in_width_;
  int kernel_dim_;
  int weight_offset_;
  int col_offset_;
  int output_offset_;

  Blob<Dtype> col_buffer_;
  Blob<Dtype> bias_multiplier_;
};

/**
 * @brief Convolves the input image with a bank of learned filters,
 *        and (optionally) adds biases.
 *
 *   Caffe convolves by reduction to matrix multiplication. This achieves
 *   high-throughput and generality of input and filter dimensions but comes at
 *   the cost of memory for matrices. This makes use of efficiency in BLAS.
 *
 *   The input is "im2col" transformed to a channel K' x H x W data matrix
 *   for multiplication with the N x K' x H x W filter matrix to yield a
 *   N' x H x W output matrix that is then "col2im" restored. K' is the
 *   input channel * kernel height * kernel width dimension of the unrolled
 *   inputs so that the im2col matrix has a column for each input region to
 *   be filtered. col2im restores the output spatial structure by rolling up
 *   the output channel N' columns of the output matrix.
 */
template <typename Dtype>
class ConvolutionLayer : public BaseConvolutionLayer<Dtype> {
 public:
  /**
   * @param param provides ConvolutionParameter convolution_param,
   *    with ConvolutionLayer options:
   *  - num_output. The number of filters.
   *  - kernel_size / kernel_h / kernel_w. The filter dimensions, given by
   *  kernel_size for square filters or kernel_h and kernel_w for rectangular
   *  filters.
   *  - stride / stride_h / stride_w (\b optional, default 1). The filter
   *  stride, given by stride_size for equal dimensions or stride_h and stride_w
   *  for different strides. By default the convolution is dense with stride 1.
   *  - pad / pad_h / pad_w (\b optional, default 0). The zero-padding for
   *  convolution, given by pad for equal dimensions or pad_h and pad_w for
   *  different padding. Input padding is computed implicitly instead of
   *  actually padding.
   *  - group (\b optional, default 1). The number of filter groups. Group
   *  convolution is a method for reducing parameterization by selectively
   *  connecting input and output channels. The input and output channel dimensions must be divisible
   *  by the number of groups. For group @f$ \geq 1 @f$, the
   *  convolutional filters' input and output channels are separated s.t. each
   *  group takes 1 / group of the input channels and makes 1 / group of the
   *  output channels. Concretely 4 input channels, 8 output channels, and
   *  2 groups separate input channels 1-2 and output channels 1-4 into the
   *  first group and input channels 3-4 and output channels 5-8 into the second
   *  group.
   *  - bias_term (\b optional, default true). Whether to have a bias.
   *  - engine: convolution has CAFFE (matrix multiplication) and CUDNN (library
   *    kernels + stream parallelism) engines.
   */
  explicit ConvolutionLayer(const LayerParameter& param)
      : BaseConvolutionLayer<Dtype>(param) {}

  virtual inline const char* type() const { return "Convolution"; }

 protected:
  virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  virtual inline bool reverse_dimensions() { return false; }
  virtual void compute_output_shape();
};

/**
 * @brief Convolve the input with a bank of learned filters, and (optionally)
 *        add biases, treating filters and convolution parameters in the
 *        opposite sense as ConvolutionLayer.
 *
 *   ConvolutionLayer computes each output value by dotting an input window with
 *   a filter; DeconvolutionLayer multiplies each input value by a filter
 *   elementwise, and sums over the resulting output windows. In other words,
 *   DeconvolutionLayer is ConvolutionLayer with the forward and backward passes
 *   reversed. DeconvolutionLayer reuses ConvolutionParameter for its
 *   parameters, but they take the opposite sense as in ConvolutionLayer (so
 *   padding is removed from the output rather than added to the input, and
 *   stride results in upsampling rather than downsampling).
 */
template <typename Dtype>
class DeconvolutionLayer : public BaseConvolutionLayer<Dtype> {
 public:
  explicit DeconvolutionLayer(const LayerParameter& param)
      : BaseConvolutionLayer<Dtype>(param) {}

  virtual inline const char* type() const { return "Deconvolution"; }

 protected:
  virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  virtual inline bool reverse_dimensions() { return true; }
  virtual void compute_output_shape();
};

#ifdef USE_CUDNN
/*
 * @brief cuDNN implementation of ConvolutionLayer.
 *        Fallback to ConvolutionLayer for CPU mode.
 *
 * cuDNN accelerates convolution through forward kernels for filtering and bias
 * plus backward kernels for the gradient w.r.t. the filters, biases, and
 * inputs. Caffe + cuDNN further speeds up the computation through forward
 * parallelism across groups and backward parallelism across gradients.
 *
 * The CUDNN engine does not have memory overhead for matrix buffers. For many
 * input and filter regimes the CUDNN engine is faster than the CAFFE engine,
 * but for fully-convolutional models and large inputs the CAFFE engine can be
 * faster as long as it fits in memory.
*/
template <typename Dtype>
class CuDNNConvolutionLayer : public ConvolutionLayer<Dtype> {
 public:
  explicit CuDNNConvolutionLayer(const LayerParameter& param)
      : ConvolutionLayer<Dtype>(param), handles_setup_(false) {}
  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual ~CuDNNConvolutionLayer();

  static void RuntimeOptimize(size_t mem_limit);

 protected:
  virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

  void AdjustWorkSpaces();

  bool handles_setup_;
  cudnnHandle_t* handle_;
  cudaStream_t*  stream_;
  vector<cudnnTensorDescriptor_t> bottom_descs_, top_descs_;
  cudnnTensorDescriptor_t    bias_desc_;
  cudnnFilterDescriptor_t      filter_desc_;
  vector<cudnnConvolutionDescriptor_t> conv_descs_;
  int bottom_offset_, top_offset_, weight_offset_, bias_offset_;

  // algorithms for forward and backwards convolutions
  cudnnConvolutionFwdAlgo_t *fwd_algo_;
  cudnnConvolutionBwdFilterAlgo_t *bwd_filter_algo_;
  cudnnConvolutionBwdDataAlgo_t *bwd_data_algo_;

  size_t *workspace_fwd_sizes_;
  size_t *workspace_bwd_data_sizes_;
  size_t *workspace_bwd_filter_sizes_;

  /** We prefer using a series of managed memory blocks to a single memory pool
   *  The latter approach is prone to problem and has issues in memory alignment
   **/
  size_t workspaceSizeInBytes_fwd;  // size of underlying storage
  static vector<shared_ptr<SyncedMemory> > workspaceData_fwd;  // underlying storage

  size_t workspaceSizeInBytes_bwd;  // size of underlying storage
  static vector<shared_ptr<SyncedMemory> > workspaceData_bwd_filter;  // underlying storage
  static vector<shared_ptr<SyncedMemory> > workspaceData_bwd_data;  // underlying storage

  vector<vector<int> > prev_bottom_shapes_;

  struct PerfReg {
      vector<vector<cudnnConvolutionFwdAlgoPerf_t> > fwd_perf;
      vector<vector<cudnnConvolutionBwdFilterAlgoPerf_t> > bwd_filter_perf;
      vector<vector<cudnnConvolutionBwdDataAlgoPerf_t> > bwd_data_perf;
      vector<int> fwd_algo;
      vector<int> bwd_filter_algo;
      vector<int> bwd_data_algo;
  };

  PerfReg layer_perf_;

  static boost::unordered_map<CuDNNConvolutionLayer*, PerfReg*>  perf_reg;
  static bool need_optimize_;
};
#endif

template <typename Dtype>
class ExtractLayer : public Layer<Dtype> {
 public:
  explicit ExtractLayer(const LayerParameter& param)
      : Layer<Dtype>(param) {}
  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline const char* type() const { return "Extract"; }
  virtual inline int ExactNumBottomBlobs() const { return 1; }
  virtual inline int ExactNumTopBlobs() const { return 1; }

 protected:
  virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

 private:
  int wstart_;
  int hstart_;
  int wend_;
  int hend_;
};

/**
 * @brief A helper for image operations that rearranges image regions into
 *        column vectors.  Used by ConvolutionLayer to perform convolution
 *        by matrix multiplication.
 *
 * TODO(dox): thorough documentation for Forward, Backward, and proto params.
 */
template <typename Dtype>
class Im2colLayer : public Layer<Dtype> {
 public:
  explicit Im2colLayer(const LayerParameter& param)
      : Layer<Dtype>(param) {}
  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline const char* type() const { return "Im2col"; }
  virtual inline int ExactNumBottomBlobs() const { return 1; }
  virtual inline int ExactNumTopBlobs() const { return 1; }

 protected:
  virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

  int kernel_h_, kernel_w_;
  int stride_h_, stride_w_;
  int channels_;
  int height_, width_;
  int pad_h_, pad_w_;
};

// Forward declare PoolingLayer and SplitLayer for use in LRNLayer.
template <typename Dtype> class PoolingLayer;
template <typename Dtype> class SplitLayer;

/**
 * @brief Normalize the input in a local region across or within feature maps.
 *
 * TODO(dox): thorough documentation for Forward, Backward, and proto params.
 */
template <typename Dtype>
class LRNLayer : public Layer<Dtype> {
 public:
  explicit LRNLayer(const LayerParameter& param)
      : Layer<Dtype>(param) {}
  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline const char* type() const { return "LRN"; }
  virtual inline int ExactNumBottomBlobs() const { return 1; }
  virtual inline int ExactNumTopBlobs() const { return 1; }

 protected:
  virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

  virtual void CrossChannelForward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void CrossChannelForward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void WithinChannelForward(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void CrossChannelBackward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  virtual void CrossChannelBackward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  virtual void WithinChannelBackward(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

  int size_;
  int pre_pad_;
  Dtype alpha_;
  Dtype beta_;
  Dtype k_;
  int num_;
  int channels_;
  int height_;
  int width_;

  // Fields used for normalization ACROSS_CHANNELS
  // scale_ stores the intermediate summing results
  Blob<Dtype> scale_;

  // Fields used for normalization WITHIN_CHANNEL
  shared_ptr<SplitLayer<Dtype> > split_layer_;
  vector<Blob<Dtype>*> split_top_vec_;
  shared_ptr<PowerLayer<Dtype> > square_layer_;
  Blob<Dtype> square_input_;
  Blob<Dtype> square_output_;
  vector<Blob<Dtype>*> square_bottom_vec_;
  vector<Blob<Dtype>*> square_top_vec_;
  shared_ptr<PoolingLayer<Dtype> > pool_layer_;
  Blob<Dtype> pool_output_;
  vector<Blob<Dtype>*> pool_top_vec_;
  shared_ptr<PowerLayer<Dtype> > power_layer_;
  Blob<Dtype> power_output_;
  vector<Blob<Dtype>*> power_top_vec_;
  shared_ptr<EltwiseLayer<Dtype> > product_layer_;
  Blob<Dtype> product_input_;
  vector<Blob<Dtype>*> product_bottom_vec_;
};

/**
 * @brief Pools the input image by taking the max, average, etc. within regions.
 *
 * TODO(dox): thorough documentation for Forward, Backward, and proto params.
 */
template <typename Dtype>
class PoolingLayer : public Layer<Dtype> {
 public:
  explicit PoolingLayer(const LayerParameter& param)
      : Layer<Dtype>(param) {}
  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline const char* type() const { return "Pooling"; }
  virtual inline int ExactNumBottomBlobs() const { return 1; }
  virtual inline int MinTopBlobs() const { return 1; }
  // MAX POOL layers can output an extra top blob for the mask;
  // others can only output the pooled inputs.
  virtual inline int MaxTopBlobs() const {
    return (this->layer_param_.pooling_param().pool() ==
            PoolingParameter_PoolMethod_MAX) ? 2 : 1;
  }

 protected:
  virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

  int kernel_h_, kernel_w_;
  int stride_h_, stride_w_;
  int pad_h_, pad_w_;
  int channels_;
  int height_, width_;
  int pooled_height_, pooled_width_;
  bool global_pooling_;
  Blob<Dtype> rand_idx_;
  Blob<int> max_idx_;
};

#ifdef USE_CUDNN
/*
 * @brief cuDNN implementation of PoolingLayer.
 *        Fallback to PoolingLayer for CPU mode.
*/
template <typename Dtype>
class CuDNNPoolingLayer : public PoolingLayer<Dtype> {
 public:
  explicit CuDNNPoolingLayer(const LayerParameter& param)
      : PoolingLayer<Dtype>(param), handles_setup_(false) {}
  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual ~CuDNNPoolingLayer();
  // Currently, cuDNN does not support the extra top blob.
  virtual inline int MinTopBlobs() const { return -1; }
  virtual inline int ExactNumTopBlobs() const { return 1; }

 protected:
  virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

  bool handles_setup_;
  cudnnHandle_t             handle_;
  cudnnTensorDescriptor_t bottom_desc_, top_desc_;
  cudnnPoolingDescriptor_t  pooling_desc_;
  cudnnPoolingMode_t        mode_;
};
#endif

/**
 * @brief Does spatial pyramid pooling on the input image
 *        by taking the max, average, etc. within regions
 *        so that the result vector of different sized
 *        images are of the same size.
 */
template <typename Dtype>
class SPPLayer : public Layer<Dtype> {
 public:
  explicit SPPLayer(const LayerParameter& param)
      : Layer<Dtype>(param) {}
  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline const char* type() const { return "SPP"; }
  virtual inline int ExactNumBottomBlobs() const { return 1; }
  virtual inline int MinTopBlobs() const { return 1; }
  // MAX POOL layers can output an extra top blob for the mask;
  // others can only output the pooled inputs.
  virtual inline int MaxTopBlobs() const {
    return (this->layer_param_.pooling_param().pool() ==
            PoolingParameter_PoolMethod_MAX) ? 2 : 1;
  }

 protected:
  virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  // calculates the kernel and stride dimensions for the pooling layer,
  // returns a correctly configured LayerParameter for a PoolingLayer
  virtual LayerParameter GetPoolingParam(const int pyramid_level,
      const int bottom_h, const int bottom_w, const SPPParameter spp_param);

  int pyramid_height_;
  int bottom_h_, bottom_w_;
  int channels_;
  int kernel_h_, kernel_w_;
  int pad_h_, pad_w_;

  /// the internal Split layer that feeds the pooling layers
  shared_ptr<SplitLayer<Dtype> > split_layer_;
  /// top vector holder used in call to the underlying SplitLayer::Forward
  vector<Blob<Dtype>*> split_top_vec_;
  /// bottom vector holder used in call to the underlying PoolingLayer::Forward
  vector<vector<Blob<Dtype>*>*> pooling_bottom_vecs_;
  /// the internal Pooling layers of different kernel sizes
  vector<shared_ptr<PoolingLayer<Dtype> > > pooling_layers_;
  /// top vector holders used in call to the underlying PoolingLayer::Forward
  vector<vector<Blob<Dtype>*>*> pooling_top_vecs_;
  /// pooling_outputs stores the outputs of the PoolingLayers
  vector<Blob<Dtype>*> pooling_outputs_;
  /// the internal Flatten layers that the Pooling layers feed into
  vector<FlattenLayer<Dtype>*> flatten_layers_;
  /// top vector holders used in call to the underlying FlattenLayer::Forward
  vector<vector<Blob<Dtype>*>*> flatten_top_vecs_;
  /// flatten_outputs stores the outputs of the FlattenLayers
  vector<Blob<Dtype>*> flatten_outputs_;
  /// bottom vector holder used in call to the underlying ConcatLayer::Forward
  vector<Blob<Dtype>*> concat_bottom_vec_;
  /// the internal Concat layers that the Flatten layers feed into
  shared_ptr<ConcatLayer<Dtype> > concat_layer_;
};

/**
 * @brief ROIPoolingLayer - Region of Interest Pooling Layer
 */
    template <typename Dtype>
    class ROIPoolingLayer : public Layer<Dtype> {
    public:
        explicit ROIPoolingLayer(const LayerParameter& param)
                : Layer<Dtype>(param) {}
        virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
                                const vector<Blob<Dtype>*>& top);
        virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
                             const vector<Blob<Dtype>*>& top);

        virtual inline const char* type() const { return "ROIPooling"; }

        virtual inline int MinBottomBlobs() const { return 2; }
        virtual inline int MaxBottomBlobs() const { return 2; }
        virtual inline int MinTopBlobs() const { return 1; }
        virtual inline int MaxTopBlobs() const { return 1; }

    protected:
        virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
                                 const vector<Blob<Dtype>*>& top);
        virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
                                 const vector<Blob<Dtype>*>& top);
        virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
                                  const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
        virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
                                  const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

        int channels_;
        int height_;
        int width_;
        int pooled_height_;
        int pooled_width_;
        Dtype spatial_scale_;
        Dtype spatial_shift_;
        Blob<int> max_idx_;
    };

/* ROIMaskPoolingLayer - Region of Interest Pooling Layer With Mask
*/
template <typename Dtype>
class ROIMaskPoolingLayer : public Layer<Dtype> {
 public:
  explicit ROIMaskPoolingLayer(const LayerParameter& param)
      : Layer<Dtype>(param) {}
  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline const char* type() const { return "ROIMaskPooling"; }

  virtual inline int MinBottomBlobs() const { return 2; }
  virtual inline int MaxBottomBlobs() const { return 2; }
  virtual inline int MinTopBlobs() const { return 1; }
  virtual inline int MaxTopBlobs() const { return 1; }

 protected:
  virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
  virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

  int channels_;
  int height_;
  int width_;
  int pooled_height_;
  int pooled_width_;
  Dtype spatial_scale_;
  Dtype spatial_shift_;
  int half_part_;
  Dtype roi_scale_;
  Dtype mask_scale_;

  Blob<int> max_idx_;
};

}  // namespace caffe

#endif  // CAFFE_VISION_LAYERS_HPP_


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Regression

#include <fstream>
#include <iostream>

#include <boost/test/unit_test.hpp>

#include <xolotl/core/diffusion/Diffusion2DHandler.h>
#include <xolotl/core/network/PSIReactionNetwork.h>
#include <xolotl/options/Options.h>
#include <xolotl/test/CommandLine.h>
#include <xolotl/util/MPIUtils.h>

using namespace std;
using namespace xolotl;
using namespace core;
using namespace diffusion;

using Kokkos::ScopeGuard;
BOOST_GLOBAL_FIXTURE(ScopeGuard);

/**
 * This suite is responsible for testing the Diffusion2DHandler.
 */
BOOST_AUTO_TEST_SUITE(Diffusion2DHandler_testSuite)

/**
 * Method checking the initialization of the off-diagonal part of the Jacobian,
 * and the compute diffusion methods.
 */
BOOST_AUTO_TEST_CASE(checkDiffusion)
{
	// Create the option to create a network
	xolotl::options::Options opts;
	// Create a good parameter file
	std::string parameterFile = "param.txt";
	std::ofstream paramFile(parameterFile);
	paramFile << "netParam=8 0 0 1 0" << std::endl;
	paramFile.close();

	// Create a fake command line to read the options
	test::CommandLine<2> cl{{"fakeXolotlAppNameForTests", parameterFile}};
	util::mpiInit(cl.argc, cl.argv);
	opts.readParams(cl.argc, cl.argv);

	std::remove(parameterFile.c_str());

	// Create a grid
	std::vector<double> grid;
	std::vector<double> temperatures;
	for (int l = 0; l < 5; l++) {
		grid.push_back((double)l);
		temperatures.push_back(1000.0);
	}

	// Create the network
	using NetworkType =
		network::PSIReactionNetwork<network::PSIFullSpeciesList>;
	NetworkType::AmountType maxV = opts.getMaxV();
	NetworkType::AmountType maxI = opts.getMaxI();
	NetworkType::AmountType maxHe = opts.getMaxImpurity();
	NetworkType::AmountType maxD = opts.getMaxD();
	NetworkType::AmountType maxT = opts.getMaxT();
	NetworkType network({maxHe, maxD, maxT, maxV, maxI}, grid.size(), opts);
	network.syncClusterDataOnHost();
	network.getSubpaving().syncZones(plsm::onHost);
	// Get its size
	const int dof = network.getDOF();

	// Create the diffusion handler
	Diffusion2DHandler diffusionHandler(opts.getMigrationThreshold());

	// Create a collection of advection handlers
	std::vector<advection::IAdvectionHandler*> advectionHandlers;

	// Create ofill
	network::IReactionNetwork::SparseFillMap ofill;

	// Initialize it
	diffusionHandler.initializeOFill(network, ofill);
	diffusionHandler.initializeDiffusionGrid(
		advectionHandlers, grid, 5, 0, 3, 1.0, 0);

	// Check the total number of diffusing clusters
	BOOST_REQUIRE_EQUAL(diffusionHandler.getNumberOfDiffusing(), 8);

	// The step size in the x direction
	double hx = 1.0;
	// The size parameter in the y direction
	double sy = 1.0;

	// The arrays of concentration
	double concentration[9 * dof];
	double newConcentration[9 * dof];

	// Initialize their values
	for (int i = 0; i < 9 * dof; i++) {
		concentration[i] = (double)i * i;
		newConcentration[i] = 0.0;
	}

	// Set the temperature to 1000K to initialize the diffusion coefficients
	network.setTemperatures(temperatures);
	network.syncClusterDataOnHost();

	// Get pointers
	double* conc = &concentration[0];
	double* updatedConc = &newConcentration[0];

	// Get the offset for the grid point in the middle
	// Supposing the 9 grid points are laid-out as follow:
	// 6 | 7 | 8
	// 3 | 4 | 5
	// 0 | 1 | 2
	double* concOffset = conc + 4 * dof;
	double* updatedConcOffset = updatedConc + 4 * dof;

	// Fill the concVector with the pointer to the middle, left, right, bottom,
	// and top grid points
	double* concVector[5]{};
	concVector[0] = concOffset; // middle
	concVector[1] = conc + 3 * dof; // left
	concVector[2] = conc + 5 * dof; // right
	concVector[3] = conc + 1 * dof; // bottom
	concVector[4] = conc + 7 * dof; // top

	// Compute the diffusion at this grid point
	diffusionHandler.computeDiffusion(
		network, concVector, updatedConcOffset, hx, hx, 0, sy, 1);

	// Check the new values of updatedConcOffset
	BOOST_REQUIRE_CLOSE(updatedConcOffset[1], 3.7081e+13, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[3], 1.8160e+13, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[5], 7.3065e+12, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[7], 9.6476e+12, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[9], 7.1800e+12, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[11], 1.7783e+11, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[13], 2.7860e+10, 0.01);
	BOOST_REQUIRE_CLOSE(
		updatedConcOffset[15], 0.0, 0.01); // He_8 does not diffuse
	BOOST_REQUIRE_CLOSE(updatedConcOffset[0], 2.9207e+09, 0.01);

	// Initialize the indices and values to set in the Jacobian
	int nDiff = diffusionHandler.getNumberOfDiffusing();
	IdType indices[nDiff];
	double val[5 * nDiff];
	// Get the pointer on them for the compute diffusion method
	IdType* indicesPointer = &indices[0];
	double* valPointer = &val[0];

	// Compute the partial derivatives for the diffusion a the grid point 1
	diffusionHandler.computePartialsForDiffusion(
		network, valPointer, indicesPointer, hx, hx, 0, sy, 1);

	// Check the values for the indices
	BOOST_REQUIRE_EQUAL(indices[0], 0);
	BOOST_REQUIRE_EQUAL(indices[1], 1);
	BOOST_REQUIRE_EQUAL(indices[2], 3);
	BOOST_REQUIRE_EQUAL(indices[3], 5);
	BOOST_REQUIRE_EQUAL(indices[4], 7);
	BOOST_REQUIRE_EQUAL(indices[5], 9);
	BOOST_REQUIRE_EQUAL(indices[6], 11);
	BOOST_REQUIRE_EQUAL(indices[7], 13);

	// Check some values
	BOOST_REQUIRE_CLOSE(val[0], -2021250, 0.01);
	BOOST_REQUIRE_CLOSE(val[3], 505312, 0.01);
	BOOST_REQUIRE_CLOSE(val[8], 6415444736, 0.01);
	BOOST_REQUIRE_CLOSE(val[12], 3141913616, 0.01);
	BOOST_REQUIRE_CLOSE(val[15], -5056420168, 0.01);

	// Finalize MPI
	MPI_Finalize();
}

BOOST_AUTO_TEST_SUITE_END()


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

/**
 * \file libs/numeric/ublasx/test/sign.cpp
 *
 * \brief Test suite for the \c sign operation.
 *
 * \author comcon1 based on code of Marco Guazzone
 *
 * <hr/>
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/sign.hpp>
#include <cmath>
#include <complex>
#include <cstddef>
#include "libs/numeric/ublasx/test/utils.hpp"
#include <limits>

namespace ublas = boost::numeric::ublas;
namespace ublasx = boost::numeric::ublasx;

static const double tol = std::numeric_limits<double>::epsilon()*2.0;

namespace detail { namespace /*<unnamed>*/ {

template <typename T>
inline
static T sign_impl(T x)
{
    if (std::isnan(x))
    {
        return std::numeric_limits<T>::quiet_NaN();
    }
    return (x > 0) ? 1 : ((x < 0) ? -1 : 0);
}

template <typename T>
inline
static std::complex<T> sign_impl(const std::complex<T>& x)
{
    T a = std::abs(x);
    return (a == 0) ? std::complex<T>(0,0) : x/std::abs(x);
}

template <typename T>
inline
static bool isnan_impl(T x)
{
    return std::isnan(x);
}

template <typename T>
inline
static bool isnan_impl(const std::complex<T>& x)
{
    return std::isnan(std::real(x)) || std::isnan(std::imag(x));
}

}} // Namespace detail::<unnamed>

BOOST_UBLASX_TEST_DEF( test_real_vector )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vector" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::vector<value_type> vector_type;

    const size_type n(4);

    vector_type v(n);

    v(0) =  0.0;
    v(1) = -2.0;
    v(2) = -3.0;
    v(3) =  4.0;

    vector_type res;
    vector_type expect_res(n);

    res = ublasx::sign(v);

    BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
    BOOST_UBLASX_DEBUG_TRACE( "sign(v) = " << res );

    for (size_type i = 0; i < n; ++i)
    {
        expect_res(i) = detail::sign_impl(v(i));
    }

    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_complex_vector )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Vector" );

    typedef double real_type;
    typedef std::complex<real_type> value_type;
    typedef std::size_t size_type;
    typedef ublas::vector<value_type> vector_type;

    const size_type n(4);

    vector_type v(n);

    v(0) = value_type( 0.0,  0.0);
    v(1) = value_type(-2.0,  2.0);
    v(2) = value_type(-2.0, -2.0);
    v(3) = value_type( 2.0, -2.0);

    vector_type res;
    vector_type expect_res(n);

    res = ublasx::sign(v);

    BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
    BOOST_UBLASX_DEBUG_TRACE( "sign(v) = " << res );

    for (size_type i = 0; i < n; ++i)
    {
        expect_res(i) = detail::sign_impl(v(i));
    }

    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_real_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Matrix" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type> matrix_type;

    const size_type nr(2);
    const size_type nc(3);

    matrix_type A(nr,nc);

    A(0,0) =  0; A(0,1) = -2; A(0,2) = -3;
    A(1,0) = -4; A(1,1) =  5; A(1,2) =  6;

    matrix_type R;
    matrix_type expect_R(nr,nc);

    R = ublasx::sign(A);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "sign(A) = " << R );

    for (size_type r = 0; r < nr; ++r)
    {
        for (size_type c = 0; c < nc; ++c)
        {
            expect_R(r,c) = detail::sign_impl(A(r,c));
        }
    }

    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( test_complex_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Matrix" );

    typedef double real_type;
    typedef std::complex<real_type> value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type> matrix_type;

    const size_type nr(2);
    const size_type nc(3);

    matrix_type A(nr,nc);

    A(0,0) = value_type( 0,-6); A(0,1) = value_type(-2, 4); A(0,2) = value_type(-3,-3);
    A(1,0) = value_type(-4, 2); A(1,1) = value_type( 5, 7); A(1,2) = value_type( 6,-1);

    matrix_type R;
    matrix_type expect_R(nr,nc);

    R = ublasx::sign(A);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "sign(A) = " << R );

    for (size_type r = 0; r < nr; ++r)
    {
        for (size_type c = 0; c < nc; ++c)
        {
            expect_R(r,c) = detail::sign_impl(A(r,c));
        }
    }

    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( test_real_special_vector )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Special - Vector" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::vector<value_type> vector_type;

    const size_type n(3);

    vector_type v(n);

    v(0) =  std::numeric_limits<value_type>::quiet_NaN();
    v(1) =  std::numeric_limits<value_type>::infinity();
    v(2) = -std::numeric_limits<value_type>::infinity();

    vector_type res;
    vector_type expect_res(n);

    res = ublasx::sign(v);

    BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
    BOOST_UBLASX_DEBUG_TRACE( "sign(v) = " << res );

    // We don't use BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE() since by definition NaN != NaN
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res(0)) );
    BOOST_UBLASX_TEST_CHECK_CLOSE( res(1),  1, tol );
    BOOST_UBLASX_TEST_CHECK_CLOSE( res(2), -1, tol );
}

BOOST_UBLASX_TEST_DEF( test_complex_special_vector )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Special - Vector" );

    typedef double real_type;
    typedef std::complex<real_type> value_type;
    typedef std::size_t size_type;
    typedef ublas::vector<value_type> vector_type;

    const size_type n(13);

    vector_type v(n);

    v( 0) = value_type( 1, std::numeric_limits<real_type>::quiet_NaN());
    v( 1) = value_type(-1, std::numeric_limits<real_type>::quiet_NaN());
    v( 2) = value_type( std::numeric_limits<real_type>::quiet_NaN(),  1);
    v( 3) = value_type( std::numeric_limits<real_type>::quiet_NaN(),-1);
    v( 4) = value_type( std::numeric_limits<real_type>::quiet_NaN(), std::numeric_limits<real_type>::quiet_NaN());
    v( 5) = value_type( std::numeric_limits<real_type>::infinity(), std::numeric_limits<real_type>::quiet_NaN());
    v( 6) = value_type(-std::numeric_limits<real_type>::infinity(), std::numeric_limits<real_type>::quiet_NaN());
    v( 7) = value_type( std::numeric_limits<real_type>::quiet_NaN(), std::numeric_limits<real_type>::infinity());
    v( 8) = value_type( std::numeric_limits<real_type>::quiet_NaN(),-std::numeric_limits<real_type>::infinity());
    v( 9) = value_type( std::numeric_limits<real_type>::infinity(), std::numeric_limits<real_type>::infinity());
    v(10) = value_type( std::numeric_limits<real_type>::infinity(),-std::numeric_limits<real_type>::infinity());
    v(11) = value_type(-std::numeric_limits<real_type>::infinity(), std::numeric_limits<real_type>::infinity());
    v(12) = value_type(-std::numeric_limits<real_type>::infinity(),-std::numeric_limits<real_type>::infinity());

    vector_type res;
    vector_type expect_res(n);

    res = ublasx::sign(v);

    BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
    BOOST_UBLASX_DEBUG_TRACE( "sign(v) = " << res );

    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res( 0)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res( 1)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res( 2)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res( 3)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res( 4)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res( 5)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res( 6)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res( 7)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res( 8)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res( 9)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res(10)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res(11)) );
    BOOST_UBLASX_TEST_CHECK( detail::isnan_impl(res(12)) );
}

int main()
{

    BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'sign' operation");

    BOOST_UBLASX_TEST_BEGIN();

    BOOST_UBLASX_TEST_DO( test_real_vector );
    BOOST_UBLASX_TEST_DO( test_complex_vector );
    BOOST_UBLASX_TEST_DO( test_real_matrix );
    BOOST_UBLASX_TEST_DO( test_complex_matrix );
    BOOST_UBLASX_TEST_DO( test_real_special_vector );
    BOOST_UBLASX_TEST_DO( test_complex_special_vector );

    BOOST_UBLASX_TEST_END();
}


#pragma once

#include <algorithm>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/dominator_tree.hpp>
#include <boost/version.hpp>
#include <unordered_map>

#include <crab/common/stats.hpp>

/*

  - A node u dominates v if all paths from entry to v pass through
    u.

  - A node u strictly dominates v if u dominates v and u!=v.

  - A node u is the immediate dominator of v (idom) if u is the unique
    node that strictly dominates v but does not strictly dominates any
    other node that strictly dominates v.

  - The dominance frontier of a node n is the set of nodes such that
    they are NOT strictly dominated by n but their predecessors are.

  The concepts of post-dominator and post-dominance frontier are dual
  and can be computed by computing dominators and dominance frontiers
  on the reverse graph.

*/

namespace crab {
  namespace analyzer {
   namespace graph_algo {

   struct dominator_tree_traits {
#if BOOST_VERSION /100 % 100 < 62
       // XXX: the boost dominator_tree class used by
       //      lengauer_tarjan_dominator_tree ignores completely
       //      index_map in versions older than 1.62 so calls to
       //      get(vertex_index, g) will fail.
       // FIXME: need to get around in older versions.
     enum { can_compute_dominator_tree = 0};
#else
     enum { can_compute_dominator_tree = 1};
#endif
   };

     // Compute the dominator tree of a graph.
     // Out: idom is an associative container of pairs (u,v) where v
     //      is the immediate dominator of u.
     template <typename G, typename Map>
     void dominator_tree(G g, typename G::node_t entry, Map &idom) {
#if BOOST_VERSION /100 % 100 < 62
       return;
#else
       typedef typename G::node_t node_t;
       typedef typename boost::graph_traits<G>::vertices_size_type vertices_size_type_t;
       typedef boost::associative_property_map<std::map <node_t, int> > index_map_t;
       typedef boost::iterator_property_map
	       <typename std::vector<vertices_size_type_t>::iterator,
		index_map_t> time_map_t;
       typedef boost::iterator_property_map
	        <typename std::vector<node_t>::iterator, index_map_t> pred_map_t;

       // build a map that maps graph vertices to indexes
       std::map <node_t, int> imap;
       index_map_t index_map(imap);
       typename boost::graph_traits<G>::vertex_iterator It, Et;
       int j = 0;
       for (tie(It, Et) = vertices(g); It != Et; ++It, ++j) {
       	 put(index_map, *It, j);
       }

       std::vector<vertices_size_type_t> df_num(num_vertices(g), 0);
       time_map_t df_num_map(make_iterator_property_map(df_num.begin(), index_map));
       std::vector<node_t> parent(num_vertices(g),boost::graph_traits<G>::null_vertex());
       pred_map_t parent_map(make_iterator_property_map(parent.begin(), index_map));
       std::vector<node_t> vertices_by_df_num(parent);
       std::vector<node_t> dom_tree_pred_vector(num_vertices(g),
						boost::graph_traits<G>::null_vertex());
       pred_map_t dom_tree_pred_map(make_iterator_property_map(dom_tree_pred_vector.begin(),
							       index_map));

       boost::lengauer_tarjan_dominator_tree(g,entry ,
					     index_map, df_num_map, parent_map,
					     vertices_by_df_num, dom_tree_pred_map);

       for (auto v: boost::make_iterator_range(vertices(g))) {
	 if (get(dom_tree_pred_map, v) != boost::graph_traits<G>::null_vertex()) {
	   idom.insert(typename Map::value_type(v, get(dom_tree_pred_map, v)));
	   CRAB_LOG("dominator",
		     crab::outs() << crab::cfg_impl::get_label_str(get(dom_tree_pred_map, v))
		                   << " is the immediate dominator of "
 		                   << crab::cfg_impl::get_label_str(v)
		                   << "\n";);

	 } else {
	   idom.insert(typename Map::value_type(v, boost::graph_traits<G>::null_vertex()));
	   CRAB_LOG("dominator",
		     crab::outs() << crab::cfg_impl::get_label_str(v)
		                   << " is not dominated by anyone!\n");
	 }
       }
#endif
     }

     namespace graph_algo_impl {
       // OUT: df is a map from nodes to their dominance frontier
       template<typename G, typename VectorMap>
       void dominance(G g, typename G::node_t entry, VectorMap &df) {

	 if (!dominator_tree_traits::can_compute_dominator_tree) {
	   CRAB_WARN("Cannot compute dominator tree. Use boost >= 1.62");
	   return;
	 }

	 typedef typename G::node_t node_t;
	 // map node to its idom node
	 std::unordered_map<node_t, node_t> idom;
	 crab::CrabStats::resume("Dominator Tree");
	 dominator_tree(g, entry, idom);
	 crab::CrabStats::stop("Dominator Tree");
	 // // map node n to all its immediate dominated nodes
	 // std::unordered_map<node_t, std::vector<node_t> > dominated;
	 // for (auto v: boost::make_iterator_range(vertices(g))) {
	 //   if (idom[v] != boost::graph_traits<G>::null_vertex()) {
	 //     auto &dom_vector = dominated[idom[v]];
	 //     dom_vector.push_back(v);
	 //   }
	 // }

	 crab::CrabStats::resume("Dominance Frontier");
	 // computer dominance frontier
	 // use the iterative solution from Cooper/Torczon
	 for (auto n: boost::make_iterator_range(vertices(g))) {
	   for (auto e: boost::make_iterator_range(in_edges(n,g))) {
	     // Traverse backwards the dominator tree starting from
	     // n's predecessors until found a node that immediate
	     // dominates n.
	     node_t runner = source(e, g);
	     while (runner != boost::graph_traits<G>::null_vertex() &&
		    runner != idom[n] && runner != n) {
	       if (std::find(df[runner].begin(),df[runner].end(),n) == df[runner].end())
		 df[runner].push_back(n);
	       runner = idom[runner];
	     }
	   }
	 } // end outer for
	 crab::CrabStats::stop("Dominance Frontier");

	 CRAB_LOG("dominance",
		   for(auto &kv: df) {
		     crab::outs() <<  crab::cfg_impl::get_label_str(kv.first)
				   << "={";
		     for (auto v: kv.second) {
		       crab::outs() << crab::cfg_impl::get_label_str(v) << ";";
		     }
		     crab::outs() << "}\n";
		   });
       }

     } // end namespace

     // OUT: a map that for each node returns its dominance frontier.
     template<typename G, typename VectorMap>
     void dominance(G g, VectorMap &fdf) {
       CRAB_LOG("dominance", crab::outs() << "Dominance Frontiers\n");
       if (dominator_tree_traits::can_compute_dominator_tree) {
	 graph_algo_impl::dominance(g, g.entry(), fdf);
       } else {
	 CRAB_WARN("Cannot compute dominator tree. Use boost >= 1.62");
       }
     }

     // OUT: a map that for each node returns its post-dominance
     // (reverse/inverse)frontier.
     template<typename G, typename VectorMap>
     void post_dominance(G g, VectorMap &rdf) {
       if (!g.has_exit()) return;

       if (dominator_tree_traits::can_compute_dominator_tree) {
	 crab::cfg::cfg_rev<G> rev_g(g);
	 CRAB_LOG("dominance", crab::outs() << "Post-Dominance Frontiers\n");
	 graph_algo_impl::dominance(rev_g, rev_g.entry(), rdf);
       } else {
	 CRAB_WARN("Cannot compute dominator tree. Use boost >= 1.62");
       }
     }

   } // end namespace
  } // end namespace
} // end namespace


/*
 *  testGeometry.cpp
 *  Trogdor6
 *
 *  Created by Paul Hansen on 4/7/10.
 *  Copyright 2010 Stanford University. All rights reserved.
 *
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Test3D

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

#include <iostream>
#include <cstdlib>
#include "geometry.h"
#include "geometry2.h"

using namespace std;

//static Vector3d randomVector()
//{
//    return Vector3d(drand48(), drand48(), drand48());
//}
//
//static Triangle3d randomTriangle()
//{
//    return Triangle3d(randomVector(), randomVector(), randomVector());
//}

BOOST_AUTO_TEST_CASE( TestSegmentIntersection )
{
    Vector2d p0(0,0), p1(1,0);
    Vector2d q0(0,-1), q1(1,1);
    Vector2d r0(0,1e-4), r1(1, 1e-4);
    Vector2d s0(2, -1), s1(1, 1);

    BOOST_CHECK(segmentIntersects(p0, p1, q0, q1, 0.0));
    BOOST_CHECK(segmentIntersects(r0, r1, q0, q1, 0.0));
    BOOST_CHECK(segmentIntersects(p0, p1, s0, s1, 0.0) == false);
    BOOST_CHECK(segmentIntersects(p0, p1, r0, r1, 0.0) == false);
}

BOOST_AUTO_TEST_CASE( TestPlane )
{
    Plane3d x_1(Vector3d(1,0,0), -1.0);
    Plane3d y_3(Vector3d(0,1,0), -3.0);
    Plane3d z_2(Vector3d(0,0,-1), 2.0);

    Vector3d rayX(1,0,0);
    Vector3d rayY(0,-13,0);
    Vector3d rayZ(0,0,200);

    BOOST_CHECK(intersection(x_1, Vector3d(0,0,0), rayX) == Vector3d(1,0,0));
    BOOST_CHECK(intersection(x_1, Vector3d(0,1,5), rayX) == Vector3d(1,1,5));

    BOOST_CHECK(intersection(y_3, Vector3d(0,0,0), rayY) == Vector3d(0,3,0));
    BOOST_CHECK(intersection(y_3, Vector3d(1,1,1), rayY) == Vector3d(1,3,1));

    BOOST_CHECK(intersection(z_2, Vector3d(0,0,0), rayZ) == Vector3d(0,0,2));
    BOOST_CHECK(intersection(z_2, Vector3d(-1,1,1), rayZ) == Vector3d(-1,1,2));

    // plane parallel to x=y.
//    Plane3d xy45(Vector3d(-1,1,0), -1.0); // should run through (-.707, .707)
//    Vector3d rayXY(unitVector(Vector3d(-1,1,0)));
}

BOOST_AUTO_TEST_CASE( TestTriPlane )
{
    Triangle3d xy(Vector3d(1,0,0), Vector3d(0,1,0), Vector3d(0,0,0));

    Vector3d normalVector(xy.normal());
    Plane3d plane(xy.plane());

    BOOST_CHECK(normalVector == plane.normal());
    BOOST_CHECK_EQUAL(normalVector[0], 0.0);
    BOOST_CHECK_EQUAL(normalVector[1], 0.0);
    BOOST_CHECK_EQUAL(normalVector[2], 1.0); // only if two sides are length 1
}

BOOST_AUTO_TEST_CASE( TestTriPlanePerturbation_Z )
{
    Triangle3d tri(Vector3d(0,0,1),
        Vector3d(1,0,1),
        Vector3d(0,1,1));

//    BOOST_TEST_MESSAGE("Plane = " << tri.plane());
    double epsilon = 1e-1;
    for (int nn = 0; nn < 3; nn++)
    {
//        BOOST_TEST_MESSAGE("dcdv" << nn << " = " << tri.dcdv(nn) );
//        BOOST_TEST_MESSAGE("dndv" << nn << " =\n" << tri.dndv(nn) );
//        BOOST_TEST_MESSAGE("dndv" << nn << " * v0 = " << tri.dndv(nn)*tri[0]);
        for (int xyz = 0; xyz < 3; xyz++)
        {
            Vector3d perturbation = epsilon*Vector3d::unit(xyz);
            Triangle3d tri2(tri);
            tri2[nn] += perturbation;

//            double diffc = tri2.plane().constant() - tri.plane().constant();
//            double dc = tri.dcdv(nn)[xyz];

//            BOOST_TEST_MESSAGE("dcdv" << nn << "" << char('x'+xyz)
//                << " = " << diffc/epsilon
//                << " expected " << dc);
        }
    }
}

BOOST_AUTO_TEST_CASE( TestTriPlanePerturbation_Angled )
{
    Triangle3d tri(Vector3d(0,0,0), Vector3d(1,1,1), Vector3d(0,1,2));
    double epsilon = 1e-5;
    double tolerance = 1e-3;

    for (int vertex = 0; vertex < 3; vertex++)
    for (int direction = 0; direction < 3; direction++)
    {
        Vector3d dv[] = { Vector3d(0,0,0), Vector3d(0,0,0), Vector3d(0,0,0) };
        dv[vertex][direction] = epsilon;

        Triangle3d tri2(tri[0] + dv[0], tri[1]+dv[1], tri[2]+dv[2]);

        Vector3d dNormal = tri.dndv(0)*dv[0] +
            tri.dndv(1)*dv[1] +
            tri.dndv(2)*dv[2];
        Vector3d dUnitNormal = tri.dudv(0)*dv[0] +
            tri.dudv(1)*dv[1] +
            tri.dudv(2)*dv[2];

        // A plane is defined by dot(a,x) + b = 0.  b is the constant.  Here is
        // its sensitivity, dConstant.
        double dConstant = dot(tri.dcdv(0), dv[0]) +
            dot(tri.dcdv(1), dv[1]) +
            dot(tri.dcdv(2), dv[2]);

        // Relative error < tolerance:
        // |delta normal - dNormal| < tolerance * |dNormal|
        BOOST_CHECK_SMALL(
            norm(tri2.plane().normal() - tri.plane().normal() - dNormal),
            tolerance*norm(dNormal));

        // Error < tolerance*epsilon.
        // Loosely speaking, the constant is expected to be as big as the
        // perturbation for a triangle with O(1) dimensions.  If I change
        // the triangle dimensions I'll need to rethink the test tolerance.
        BOOST_CHECK_SMALL(
            tri2.plane().constant() - tri.plane().constant() - dConstant,
            tolerance*epsilon);

        // The (separate) calculation of dUnitNormal needs to be tested too.
        // The finite difference is O(epsilon), so try to keep the error down
        // to O(tolerance*epsilon).  In this case, using a larger triangle
        // we'd be able to push the tolerance lower.
        BOOST_CHECK_SMALL(
            norm(tri2.normal()/norm(tri2.normal()) -
                tri.normal()/norm(tri.normal()) -
                dUnitNormal),
            tolerance*epsilon);
    }
}

BOOST_AUTO_TEST_CASE( TestTriJacobianSimple )
{
    Triangle3d t(Vector3d(0,0,0), Vector3d::unit(0), Vector3d::unit(1));

    for (int vnum = 0; vnum < 3; vnum++)
    {
//        BOOST_TEST_MESSAGE("Sensitivity to " << vnum << ":\n"
//            << t.dndv(vnum));

        for (int xyz = 0; xyz < 3; xyz++)
        {
            Vector3d perturbation(Vector3d::unit(xyz));
            double epsilon = 1e-5;
            Triangle3d t2(t);
            t2[vnum] += epsilon*perturbation;

            Vector3d predictedNormal = t.normal() +
                t.dndv(vnum)*epsilon*perturbation;

            BOOST_CHECK_SMALL(norm(predictedNormal - t2.normal()), 1e-8);
//            BOOST_TEST_MESSAGE("Perturb v[" << vnum << "] along "
//                << perturbation << ": expected " << predictedNormal
//                << " got " << t2.normal() );
        }
    }
}

BOOST_AUTO_TEST_CASE( TestTriJacobian )
{
    Triangle3d tri(Vector3d(0,0,0), Vector3d(1,1,1), Vector3d(0,1,2));
    double epsilon = 1e-5;
    double tolerance = 1e-3;

    for (int vertex = 0; vertex < 3; vertex++)
    for (int direction = 0; direction < 3; direction++)
    {
        Vector3d perturbation(epsilon*Vector3d::unit(direction));
        Triangle3d tri2(tri);
        tri2[vertex] += perturbation;

        Vector3d predictedNormal = tri.normal() +
            tri.dndv(vertex)*perturbation;

//        Vector3d diffN = tri2.normal() - tri.normal();
//        Vector3d dn = tri.dndv(vertex)*perturbation;
//            BOOST_TEST_MESSAGE("Perturb v[" << vnum << "] along "
//                << perturbation << ": dn " << dn
//                << " got " << diffN );

        // For a triangle of roughly unit dimensions, the unit normal changes
        // by roughly epsilon.  Multiply by tolerance.
        BOOST_CHECK_SMALL(norm(predictedNormal - tri2.normal()),
            epsilon*tolerance*norm(tri.normal()));
//            BOOST_TEST_MESSAGE("Perturb v[" << vnum << "] along "
//                << perturbation << ": expected " << predictedNormal
//                << " got " << t2.normal() );
    }
}

BOOST_AUTO_TEST_CASE( TestRayTriangleDecomposition )
{
    Triangle3d t(Vector3d(0,0,0), Vector3d(1,1,1), Vector3d(0,1,2));
    double tolerance = 1e-5;

    Vector3d rayOrigin(-1,-2,-3);
    Vector3d rayDir(3, 2, -1);

    // Coefficient vector (a, b, t) for ray-triangle intersection
    Vector3d abt = t.intersectionCoefficients(rayOrigin, rayDir);
    Vector3d intersectPt = (t[1]-t[0])*abt[0] + (t[2]-t[0])*abt[1] + t[0];
    Vector3d intersectionCheck = intersection(t.plane(), rayOrigin, rayDir);

    BOOST_CHECK_SMALL(norm(intersectPt - intersectionCheck), tolerance);
}

// This test was written while debugging the test below.  It is not terribly
// enlightening.  (-:
BOOST_AUTO_TEST_CASE ( RayPlaneIntersectionPerturbationZ )
{
    Plane3d p(Vector3d(0,0,1), -1.0); // z = 1

    Vector3d rayOrigin(0,0,0);
    Vector3d rayDir(0,0,1);

    double t = intersectionTime(p, rayOrigin, rayDir);
    BOOST_CHECK_EQUAL(t, 1.0);

    double epsilon = 1e-1;
    Plane3d pPerturbedNormal(Vector3d(0,0,1+epsilon), -1.0);
    Plane3d pPerturbedConstant(Vector3d(0,0,1), -1.0+epsilon);

    Plane3d dtdp = intersectionTimeJacobian(p, rayOrigin, rayDir);
//    BOOST_TEST_MESSAGE(dtdp);

//    double tN = intersectionTime(pPerturbedNormal, rayOrigin, rayDir);
//    double tC = intersectionTime(pPerturbedConstant, rayOrigin, rayDir);

    double dtN = dtdp.normal()[2]*epsilon;
    double dtC = dtdp.constant()*epsilon;

//    BOOST_TEST_MESSAGE("t = " << t);
//    BOOST_TEST_MESSAGE("tN = " << tN << ", dtN = " << dtN);
//    BOOST_TEST_MESSAGE("tC = " << tC << ", dtC = " << dtC);

    double dtN2 = dot(Vector3d(0,0,epsilon), dtdp.normal());
    double dtC2 = epsilon*dtdp.constant();

    BOOST_CHECK_EQUAL(dtN, dtN2);
    BOOST_CHECK_EQUAL(dtC, dtC2);
}

BOOST_AUTO_TEST_CASE( RayPlaneIntersectionPerturbation )
{
    Plane3d p(Vector3d(1,2,3), 4);
    Vector3d rayOrigin(-1,-2,-3);
    Vector3d rayDir(3, 2, -1);

    double epsilon = 1e-5;
    double tolerance = 1e-5;

    double t = intersectionTime(p, rayOrigin, rayDir);

    // These should be identical values.
    BOOST_CHECK_SMALL(norm(intersection(p, rayOrigin, rayDir) -
        rayOrigin - t*rayDir), 1e-10);

    Plane3d dtdp = intersectionTimeJacobian(p, rayOrigin, rayDir);

    for (int xyz = 0; xyz < 4; xyz++) // change normal AND constant
    {
        Vector3d perturbNormal(0,0,0);
        double perturbConstant(0);

        if (xyz < 3)
            perturbNormal[xyz] = epsilon;
        else
            perturbConstant = epsilon;

        Plane3d perturbation(perturbNormal, perturbConstant);
        Plane3d p2(p.normal() + perturbation.normal(),
            p.constant() + perturbation.constant());
        double t2 = intersectionTime(p2, rayOrigin, rayDir);

        double dt = dot(perturbation.normal(), dtdp.normal())
            + perturbation.constant()*dtdp.constant();
        double tExpected = t + dt;

    //        BOOST_TEST_MESSAGE("delta t " << t2-t << " dt " << dt );
        // Calculate the relative error in dt.
        BOOST_CHECK_SMALL(t2 - tExpected, tolerance*fabs(dt));
    }
}

BOOST_AUTO_TEST_CASE( TriangleIntersectionPerturbation )
{
    vector<Triangle3d> testTriangles;
    vector<Vector3d> testRayOrigins;
    vector<Vector3d> testRayDirections;

    double epsilon = 1e-5;
    double tolerance = 1e-5;

    testTriangles.push_back(Triangle3d(
        Vector3d(0,0,0),
        Vector3d(1,0,0),
        Vector3d(0,1,0)));
    testTriangles.push_back(Triangle3d(
        Vector3d(0,0,0),
        Vector3d(0,1,0),
        Vector3d(0,0,1)));
    testTriangles.push_back(Triangle3d(
        Vector3d(10,0,0),
        Vector3d(20,0,40),
        Vector3d(0,0,40)));
    testRayOrigins.push_back(Vector3d(0.25,0.25,10));
    testRayOrigins.push_back(Vector3d(-20,0.3, 0.3));
    testRayOrigins.push_back(Vector3d(25,100,120));
    testRayDirections.push_back(Vector3d(0,0,-1));
    testRayDirections.push_back(Vector3d(100,0,0));
    testRayDirections.push_back(Vector3d(-1,-50,-10));

    for (unsigned int tt = 0; tt < testTriangles.size(); tt++)
    {
        Triangle3d & t = testTriangles.at(tt);
        Vector3d & rayOrigin = testRayOrigins.at(tt);
        Vector3d & rayDir = testRayDirections.at(tt);

//        Vector3d abt = t.intersectionCoefficients(rayOrigin, rayDir);
//        Vector3d intersectPt = (t[1]-t[0])*abt[0] + (t[2]-t[0])*abt[1] + t[0];
        Vector3d intersectPt = intersection(t.plane(), rayOrigin, rayDir);
        for (int vertex = 0; vertex < 3; vertex++)
        for (int direction = 0; direction < 3; direction++)
        {
            Vector3d perturbation(0,0,0);
            perturbation[direction] = epsilon;
            Triangle3d perturbed(t);
            perturbed[vertex] += perturbation;

            Vector3d perturbedIntersect = intersection(perturbed.plane(),
                rayOrigin, rayDir);
            Vector3d perturbedExpect = intersectPt +
                t.intersectionJacobian(rayOrigin, rayDir, vertex)*perturbation;

            // The intersection point should move by O(epsilon).
            // Check that it's accurate to tolerance.
            BOOST_CHECK_SMALL(norm(perturbedIntersect - perturbedExpect),
                tolerance*epsilon*norm(intersectPt));
        }
    }
}

BOOST_AUTO_TEST_CASE( TestClosedInterval )
{
    ClosedInterval<double> i1(3, 4), iLeft, iLeftOverlap, iRight, iRightOverlap,
        iBigger;

    iLeft[0] = 1;
    iLeft[1] = 2;
    BOOST_CHECK(!i1.intersects(iLeft));
    BOOST_CHECK(!iLeft.intersects(i1));

    iRight[0] = 5;
    iRight[1] = 6;
    BOOST_CHECK(!i1.intersects(iRight));
    BOOST_CHECK(!iRight.intersects(i1));

    iLeftOverlap[0] = -1;
    iLeftOverlap[1] = 3.5;
    BOOST_CHECK(i1.intersects(iLeftOverlap));
    BOOST_CHECK(iLeftOverlap.intersects(i1));

    iRightOverlap[0] = 3.5;
    iRightOverlap[1] = 7;
    BOOST_CHECK(i1.intersects(iRightOverlap));
    BOOST_CHECK(iRightOverlap.intersects(i1));

    iBigger[0] = 0;
    iBigger[1] = 10;
    BOOST_CHECK(i1.intersects(iBigger));
    BOOST_CHECK(iBigger.intersects(i1));

    ClosedInterval<double> backwardsInterval(1, -1);
    ClosedInterval<double> onePoint(0,0);

    BOOST_CHECK(!backwardsInterval.intersects(onePoint));
    BOOST_CHECK(!onePoint.intersects(backwardsInterval));

    iBigger[0] = -2;
    iBigger[1] = 2;
    BOOST_CHECK(iBigger.intersects(backwardsInterval));
    BOOST_CHECK(backwardsInterval.intersects(iBigger));
}

BOOST_AUTO_TEST_CASE( TestRectProjection )
{
    Rect3d r(0,0,0,1,1,1);

    ClosedInterval<double> interval;

    interval = r.projectionAlong(Vector3d::unit(0));
    BOOST_CHECK_CLOSE(interval[0], 0, 1e-6);
    BOOST_CHECK_CLOSE(interval[1], 1, 1e-6);

    interval = r.projectionAlong(-Vector3d::unit(0));
    BOOST_CHECK_CLOSE(interval[0], -1, 1e-6);
    BOOST_CHECK_CLOSE(interval[1], 0, 1e-6);

    interval = r.projectionAlong(Vector3d(1,1,1));
    BOOST_CHECK_CLOSE(interval[0], 0, 1e-6);
    BOOST_CHECK_CLOSE(interval[1], 3.0, 1e-6);

    interval = r.projectionAlong(Vector3d(1,-1,0));
    BOOST_CHECK_CLOSE(interval[0], -1.0, 1e-6);
    BOOST_CHECK_CLOSE(interval[1], 1.0, 1e-6);
}

BOOST_AUTO_TEST_CASE( TestTriangleProjection )
{
    Triangle3d tri(Vector3d(0,0,0), Vector3d(1,0,0), Vector3d(0,1,1));

    ClosedInterval<double> interval;

    interval = tri.projectionAlong(Vector3d::unit(1));
    BOOST_CHECK_CLOSE(interval[0], 0, 1e-6);
    BOOST_CHECK_CLOSE(interval[1], 1, 1e-6);

    interval = tri.projectionAlong(-Vector3d::unit(1));
    BOOST_CHECK_CLOSE(interval[0], -1, 1e-6);
    BOOST_CHECK_CLOSE(interval[1], 0, 1e-6);

    interval = tri.projectionAlong(Vector3d(1,1,1));
    BOOST_CHECK_CLOSE(interval[0], 0, 1e-6);
    BOOST_CHECK_CLOSE(interval[1], 2, 1e-6);

    interval = tri.projectionAlong(Vector3d(-1,1,0));
    BOOST_CHECK_CLOSE(interval[0], -1, 1e-6);
    BOOST_CHECK_CLOSE(interval[1], 1, 1e-6);
}

BOOST_AUTO_TEST_CASE( TestRectTriangleIntersection )
{
    Triangle3d tri(Vector3d(0,0,0), Vector3d(1,0,0), Vector3d(0,1,1));

    Rect3d r(0,0,0,1,1,1);
    BOOST_CHECK(r.intersects(tri));

    r = Rect3d(0,0,1.001,1,1,2); // too high
    BOOST_CHECK(!r.intersects(tri));

    r = Rect3d(1.001, 0, 0, 2, 1, 1); // too far right
    BOOST_CHECK(!r.intersects(tri));

    r = Rect3d(0.8, 0.8, -1, 1.5, 1.5, 2);
    BOOST_CHECK(!r.intersects(tri));

    r = Rect3d(-10, -10, -10, 10, 10, 10);
    BOOST_CHECK(r.intersects(tri));

    r = Rect3d(-0.5, -0.5, 0.5, 0.5, 0.5, 1);
    BOOST_CHECK(r.intersects(tri));

    // this is the only case, I think, which will depend on the last nine axis
    // checks.
    tri = Triangle3d(Vector3d(0,0,0), Vector3d(1,1,0.5), Vector3d(0,1,0.5));
    r = Rect3d(0.5, -0.5, -1, 1, 0.49, 1);
    BOOST_CHECK(!r.intersects(tri));

    r = Rect3d(0.5, -0.5, -1, 1, 0.51, 1);
    BOOST_CHECK(r.intersects(tri));
}

BOOST_AUTO_TEST_CASE( TestTriangleIntersection )
{
    Triangle2d t1(Vector2d(0,0), Vector2d(1,0), Vector2d(0,1));
    Triangle2d t2(Vector2d(0,-0.5), Vector2d(1,-0.5), Vector2d(0,0.5));
    Triangle2d t3(Vector2d(0,-1.5), Vector2d(1,-1.5), Vector2d(0,-0.5));

    BOOST_CHECK(intersects(t1, t2, 0.0));
    BOOST_CHECK(intersects(t1, t3, 0.0) == false);
}

BOOST_AUTO_TEST_CASE( TestTriangleCoincidence )
{
    // Coincident = coplanar + intersecting.
    Triangle3d t1(Vector3d(0,0,0), Vector3d(1,0,0), Vector3d(0,1,0));
    Triangle3d t2(Vector3d(0,0,-0.1), Vector3d(1,0,0.1), Vector3d(0,1,0.1));
    Triangle3d t3(Vector3d(0,0,-1e-6), Vector3d(1,0,-1e-6), Vector3d(0,1,-1e-6));
    Triangle3d t4(Vector3d(0,0,0), Vector3d(0.5,-1,0), Vector3d(1,1,0));
    Triangle3d t5(Vector3d(0.1,0.1,0), Vector3d(0.9,0.1,0), Vector3d(0.1,0.9,0));
    Triangle3d t6(Vector3d(1.1,0.1,0), Vector3d(1.9,0.1,0), Vector3d(1.1,0.9,0));

    BOOST_CHECK(coincident(t1, t2, 0.0) == false); // intersecting but not coplanar
    BOOST_CHECK(coincident(t1, t3, 0.0) == false); // parallel and overlapping in the XY plane but not coplanar
    BOOST_CHECK(coincident(t1, t4, 0.0) == true); // coplanar with intersecting edges
    BOOST_CHECK(coincident(t1, t5, 0.0) == true); // coplanar without intersecting edges
    BOOST_CHECK(coincident(t1, t6, 0.0) == false); // coplanar and non-intersecting
}


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <iostream>
#include <memory>
#include <stdexcept>
#include <boost/lexical_cast.hpp>
#include <rl/mdl/Kinematic.h>
#include <rl/mdl/UrdfFactory.h>
#include <rl/mdl/XmlFactory.h>

int
main(int argc, char** argv)
{
	if (argc < 2)
	{
		std::cout << "Usage: rlJacobianDemo KINEMATICSFILE Q1 ... Qn QD1 ... QDn" << std::endl;
		return EXIT_FAILURE;
	}

	try
	{
		std::string filename(argv[1]);
		std::shared_ptr<rl::mdl::Kinematic> kinematic;

		if ("urdf" == filename.substr(filename.length() - 4, 4))
		{
			rl::mdl::UrdfFactory factory;
			kinematic = std::dynamic_pointer_cast<rl::mdl::Kinematic>(factory.create(filename));
		}
		else
		{
			rl::mdl::XmlFactory factory;
			kinematic = std::dynamic_pointer_cast<rl::mdl::Kinematic>(factory.create(filename));
		}

		rl::math::Vector q(kinematic->getDofPosition());

		for (std::ptrdiff_t i = 0; i < q.size(); ++i)
		{
			q(i) = boost::lexical_cast<rl::math::Real>(argv[i + 2]);
		}

		rl::math::Vector qdot(kinematic->getDof());

		for (std::ptrdiff_t i = 0; i < qdot.size(); ++i)
		{
			qdot(i) = boost::lexical_cast<rl::math::Real>(argv[q.size() + i + 2]);
		}

		kinematic->setPosition(q);
		kinematic->forwardPosition();
		kinematic->calculateJacobian();

		std::cout << "J=" << std::endl << kinematic->getJacobian() << std::endl;

		rl::math::Vector xdot = kinematic->getJacobian() * qdot;

		std::cout << "xdot=" << std::endl;

		for (std::size_t i = 0; i < kinematic->getOperationalDof(); ++i)
		{
			std::cout << i << " " << xdot.transpose() << std::endl;
		}

		kinematic->calculateJacobianInverse();

		std::cout << "invJ=" << std::endl << kinematic->getJacobianInverse() << std::endl;

		rl::math::Vector qdot2 = kinematic->getJacobianInverse() * xdot;

		std::cout << "qdot=" << std::endl << qdot2.transpose() << std::endl;
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


/*
    Authors: Darya Filippova, Geet Duggal, Rob Patro
    dfilippo | geet | robp @cs.cmu.edu
*/

#ifndef __ARMATUS_UTIL_HPP__
#define __ARMATUS_UTIL_HPP__

#include <memory>
#include <string>
#include <vector>

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/symmetric.hpp>

using namespace std;

//using SparseMatrix = boost::numeric::ublas::compressed_matrix<double>;
using SparseMatrix = boost::numeric::ublas::matrix<double>;

class MatrixProperties {
    public:
    std::shared_ptr<SparseMatrix> matrix;
    std::string chrom;
    int resolution;
};

MatrixProperties parseGZipMatrix(string path, int resolution, string chrom);
MatrixProperties parseRaoMatrix(string path, int resolution, string chrom, bool noNormalization);
MatrixProperties parseSparseMatrix(string path, int resolution, string chrom);

double d(size_t const & i, size_t const & j);

class ArmatusParams;
class Domain {
    public:
    Domain(size_t s, size_t e);
    size_t start;
    size_t end;
    double score(ArmatusParams& p);
    bool operator< (const Domain &other) const {
        return std::make_tuple(start, end) < std::make_tuple(other.start, other.end);
    }
};

using DomainSet = vector<Domain>;
using DomainEnsemble = vector<DomainSet>;
using Weights = vector<double>;
using Resolutions = vector<double>;
using OptIndex = vector<size_t>;

struct WeightedDomainEnsemble {
    DomainEnsemble domainSets;
    Weights weights;
    Resolutions resolutions;
    OptIndex optidx;
};

// void optimalDomains(std::shared_ptr<SparseMatrix> A, float gamma);

WeightedDomainEnsemble multiscaleDomains(std::shared_ptr<SparseMatrix> A, float gammaMax, double stepSize, int k, int minMeanSamples, bool justThisGamma);
DomainSet consensusDomains(WeightedDomainEnsemble& dEnsemble);

void outputDomains(DomainSet dSet, string fname, MatrixProperties matProp);

#endif // __ARMATUS_UTIL_HPP__


/**
 * @file cosine_tree.hpp
 * @author Siddharth Agrawal
 *
 * Definition of Cosine Tree.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_CORE_TREE_COSINE_TREE_COSINE_TREE_HPP
#define MLPACK_CORE_TREE_COSINE_TREE_COSINE_TREE_HPP

#include <mlpack/prereqs.hpp>
#include <boost/heap/priority_queue.hpp>

namespace mlpack {
namespace tree {

// Predeclare classes for CosineNodeQueue typedef.
class CompareCosineNode;
class CosineTree;

// CosineNodeQueue typedef.
typedef boost::heap::priority_queue<CosineTree*,
    boost::heap::compare<CompareCosineNode> > CosineNodeQueue;

class CosineTree
{
 public:
  /**
   * CosineTree constructor for the root node of the tree. It initializes the
   * necessary variables required for splitting of the node, and building the
   * tree further. It takes a pointer to the input matrix and calculates the
   * relevant variables using it.
   *
   * @param dataset Matrix for which cosine tree is constructed.
   */
  CosineTree(const arma::mat& dataset);

  /**
   * CosineTree constructor for nodes other than the root node of the tree. It
   * takes in a pointer to the parent node and a list of column indices which
   * mentions the columns to be included in the node. The function calculate the
   * relevant variables just like the constructor above.
   *
   * @param parentNode Pointer to the parent cosine node.
   * @param subIndices Pointer to vector of column indices to be included.
   */
  CosineTree(CosineTree& parentNode, const std::vector<size_t>& subIndices);

  /**
   * Construct the CosineTree and the basis for the given matrix, and passed
   * 'epsilon' and 'delta' parameters. The CosineTree is constructed by
   * splitting nodes in the direction of maximum error, stored using a priority
   * queue. Basis vectors are added from the left and right children of the
   * split node. The basis vector from a node is the orthonormalized centroid of
   * its columns. The splitting continues till the Monte Carlo estimate of the
   * input matrix's projection on the obtained subspace is less than a fraction
   * of the norm of the input matrix.
   *
   * @param dataset Matrix for which the CosineTree is constructed.
   * @param epsilon Error tolerance fraction for calculated subspace.
   * @param delta Cumulative probability for Monte Carlo error lower bound.
   */
  CosineTree(const arma::mat& dataset,
             const double epsilon,
             const double delta);

  /**
   * Clean up the CosineTree: release allocated memory (including children).
   */
  ~CosineTree();

  /**
   * Calculates the orthonormalization of the passed centroid, with respect to
   * the current vector subspace.
   *
   * @param treeQueue Priority queue of cosine nodes.
   * @param centroid Centroid of the node being added to the basis.
   * @param newBasisVector Orthonormalized centroid of the node.
   * @param addBasisVector Address to additional basis vector.
   */
  void ModifiedGramSchmidt(CosineNodeQueue& treeQueue,
                           arma::vec& centroid,
                           arma::vec& newBasisVector,
                           arma::vec* addBasisVector = NULL);

  /**
   * Estimates the squared error of the projection of the input node's matrix
   * onto the current vector subspace. A normal distribution is fit using
   * weighted norms of projections of samples drawn from the input node's matrix
   * columns. The error is calculated as the difference between the Frobenius
   * norm of the input node's matrix and lower bound of the normal distribution.
   *
   * @param node Node for which Monte Carlo estimate is calculated.
   * @param treeQueue Priority queue of cosine nodes.
   * @param addBasisVector1 Address to first additional basis vector.
   * @param addBasisVector2 Address to second additional basis vector.
   */
  double MonteCarloError(CosineTree* node,
                         CosineNodeQueue& treeQueue,
                         arma::vec* addBasisVector1 = NULL,
                         arma::vec* addBasisVector2 = NULL);

  /**
   * Constructs the final basis matrix, after the cosine tree construction.
   *
   * @param treeQueue Priority queue of cosine nodes.
   */
  void ConstructBasis(CosineNodeQueue& treeQueue);

  /**
   * This function splits the cosine node into two children based on the cosines
   * of the columns contained in the node, with respect to the sampled splitting
   * point. The function also calls the CosineTree constructor for the children.
   */
  void CosineNodeSplit();

  /**
   * Sample 'numSamples' points from the Length-Squared distribution of the
   * cosine node. The function uses 'l2NormsSquared' to calculate the cumulative
   * probability distribution of the column vectors. The sampling is based on a
   * randomly generated values in the range [0, 1].
   */
  void ColumnSamplesLS(std::vector<size_t>& sampledIndices,
                       arma::vec& probabilities, size_t numSamples);

  /**
   * Sample a point from the Length-Squared distribution of the cosine node. The
   * function uses 'l2NormsSquared' to calculate the cumulative probability
   * distribution of the column vectors. The sampling is based on a randomly
   * generated value in the range [0, 1].
   */
  size_t ColumnSampleLS();

  /**
   * Sample a column based on the cumulative Length-Squared distribution of the
   * cosine node, and a randomly generated value in the range [0, 1]. Binary
   * search is more efficient than searching linearly for the same. This leads
   * a significant speedup when there are large number of columns to choose from
   * and when a number of samples are to be drawn from the distribution.
   *
   * @param cDistribution Cumulative LS distribution of columns in the node.
   * @param value Randomly generated value in the range [0, 1].
   * @param start Starting index of the distribution interval to search in.
   * @param end Ending index of the distribution interval to search in.
   */
  size_t BinarySearch(arma::vec& cDistribution, double value, size_t start,
                      size_t end);

  /**
   * Calculate cosines of the columns present in the node, with respect to the
   * sampled splitting point. The calculated cosine values are useful for
   * splitting the node into its children.
   *
   * @param cosines Vector to store the cosine values in.
   */
  void CalculateCosines(arma::vec& cosines);

  /**
   * Calculate centroid of the columns present in the node. The calculated
   * centroid is used as a basis vector for the cosine tree being constructed.
   */
  void CalculateCentroid();

  //! Returns the basis of the constructed subspace.
  void GetFinalBasis(arma::mat& finalBasis) { finalBasis = basis; }

  //! Get pointer to the dataset matrix.
  const arma::mat& GetDataset() const { return dataset; }

  //! Get the indices of columns in the node.
  std::vector<size_t>& VectorIndices() { return indices; }

  //! Set the Monte Carlo error.
  void L2Error(const double error) { this->l2Error = error; }
  //! Get the Monte Carlo error.
  double L2Error() const { return l2Error; }

  //! Get pointer to the centroid vector.
  arma::vec& Centroid() { return centroid; }

  //! Set the basis vector of the node.
  void BasisVector(arma::vec& bVector) { this->basisVector = bVector; }

  //! Get the basis vector of the node.
  arma::vec& BasisVector() { return basisVector; }

  //! Get pointer to the parent node.
  CosineTree* Parent() const { return parent; }
  //! Modify the pointer to the parent node.
  CosineTree*& Parent() { return parent; }

  //! Get pointer to the left child of the node.
  CosineTree* Left() const { return left; }
  //! Modify the pointer to the left child of the node.
  CosineTree*& Left() { return left; }

  //! Get pointer to the right child of the node.
  CosineTree* Right() const { return right; }
  //! Modify the pointer to the left child of the node.
  CosineTree*& Right() { return right; }

  //! Get number of columns of input matrix in the node.
  size_t NumColumns() const { return numColumns; }

  //! Get the Frobenius norm squared of columns in the node.
  double FrobNormSquared() const { return frobNormSquared; }

  //! Get the column index of split point of the node.
  size_t SplitPointIndex() const { return indices[splitPointIndex]; }

 private:
  //! Matrix for which cosine tree is constructed.
  const arma::mat& dataset;
  //! Cumulative probability for Monte Carlo error lower bound.
  double delta;
  //! Subspace basis of the input dataset.
  arma::mat basis;
  //! Parent of the node.
  CosineTree* parent;
  //! Left child of the node.
  CosineTree* left;
  //! Right child of the node.
  CosineTree* right;
  //! Indices of columns of input matrix in the node.
  std::vector<size_t> indices;
  //! L2-norm squared of columns in the node.
  arma::vec l2NormsSquared;
  //! Centroid of columns of input matrix in the node.
  arma::vec centroid;
  //! Orthonormalized basis vector of the node.
  arma::vec basisVector;
  //! Index of split point of cosine node.
  size_t splitPointIndex;
  //! Number of columns of input matrix in the node.
  size_t numColumns;
  //! Monte Carlo error for this node.
  double l2Error;
  //! Frobenius norm squared of columns in the node.
  double frobNormSquared;
};

class CompareCosineNode
{
 public:
  // Comparison function for construction of priority queue.
  bool operator() (const CosineTree* a, const CosineTree* b) const
  {
    return a->L2Error() < b->L2Error();
  }
};

} // namespace tree
} // namespace mlpack

#endif


#include <iostream>
#include <array>
#include <armadillo>

#include "../helper/utility.h"
#include "free_flight.h"

namespace mc
{

// check for collision to boundaries
void free_flight::check_boundary(particle* p, const double& dt, const std::pair<arma::vec, arma::vec>& domain) const {
  if (arma::any(p->pos() < domain.first) || arma::any(p->pos() > domain.second)) {
    p->set_pos(p->old_pos());
  }
};

} // end namespace mc


#include "polygons_to_triangles.h"
#include "ear_clipping.h"
#include "../sort.h"
#include "../slice.h"
#include <Eigen/Eigenvalues>

template <
  typename DerivedV,
  typename DerivedI,
  typename DerivedC,
  typename DerivedF,
  typename DerivedJ>
IGL_INLINE void igl::predicates::polygons_to_triangles(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedI> & I,
  const Eigen::MatrixBase<DerivedC> & C,
  Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedJ> & J)
{
  typedef Eigen::Index Index;
  // Each polygon results in #sides-2 triangles. So ∑#sides-2
  F.resize(C(C.size()-1) - (C.size()-1)*2,3);
  J.resize(F.rows());
  {
    Index f = 0;
    for(Index p = 0;p<C.size()-1;p++)
    {
      const Index np = C(p+1)-C(p);
      Eigen::MatrixXi pF;
      if(np == 3)
      {
        pF = (Eigen::MatrixXi(1,3)<<0,1,2).finished();
      }else
      {
        // Make little copy of this polygon with an initial fan
        DerivedV pV(np,V.cols());
        for(Index c = 0;c<np;c++)
        {
          pV.row(c) = V.row(I(C(p)+c));
        }
        // Use PCA to project to 2D
        Eigen::MatrixXd S;
        switch(V.cols())
        {
          case 2:
            S = V.template cast<double>();
            break;
          case 3:
          {
            Eigen::MatrixXd P = (pV.rowwise() - pV.colwise().mean()).template cast<double>();
            Eigen::Matrix3d O = P.transpose() * P;
            Eigen::EigenSolver<Eigen::Matrix3d> es(O);
            Eigen::Matrix3d C = es.eigenvectors().real();
            {
              Eigen::Vector3d _1;
              Eigen::Vector3i I;
              igl::sort(es.eigenvalues().real().eval(),1,false,_1,I);
              igl::slice(Eigen::Matrix3d(C),I,2,C);
            }
            S = P*C.leftCols(2);
            break;
          }
          default: assert(false && "dim>3 not supported");
        }

        Eigen::VectorXi RT = Eigen::VectorXi::Zero(S.rows(),1);
        Eigen::VectorXi _I;
        Eigen::MatrixXd _nS;

        // compute signed area
        {
          double area = 0;
          for(Index c = 0;c<np;c++)
          {
            area += S((c+0)%np,0)*S((c+1)%np,1) - S((c+1)%np,0)*S((c+0)%np,1);
          }
          //prIndexf("area: %g\n",area);
          if(area<0)
          {
            S.col(0) *= -1;
          }
        }

        // This is a really low quality triangulator and will contain nearly
        // degenerate elements which become degenerate or worse when unprojected
        // back to 3D.
        igl::predicates::ear_clipping(S,RT,_I,pF,_nS);
        // igl::predicates::ear_clipping does not gracefully fail when the input
        // is not simple. Instead it (tends?) to output too few triangles.
        if(pF.rows() < np-2)
        {
          // Fallback, use a fan
          //std::cout<<igl::matlab_format(S,"S")<<std::endl;
          //std::cout<<igl::matlab_format(RT,"RT")<<std::endl;
          //std::cout<<igl::matlab_format(_I,"I")<<std::endl;
          //std::cout<<igl::matlab_format(pF,"pF")<<std::endl;
          //std::cout<<igl::matlab_format(_nS,"nS")<<std::endl;
          //std::cout<<std::endl;

          pF.resize(np-2,3);
          for(Index c = 0;c<np;c++)
          {
            if(c>0 && c<np-1)
            {
              pF(c-1,0) = 0;
              pF(c-1,1) = c;
              pF(c-1,2) = c+1;
            }
          }
        }
        assert(pF.rows() == np-2);

        // Could at least flip edges of degenerate edges

        //if(pF.rows()>1)
        //{
        //  // Delaunay-ize
        //  Eigen::MatrixXd pl;
        //  igl::edge_lengths(pV,pF,pl);

        //  typedef Eigen::Matrix<Index,Eigen::Dynamic,2> MatrixX2I;
        //  typedef Eigen::Matrix<Index,Eigen::Dynamic,1> VectorXI;
        //  MatrixX2I E,uE;
        //  VectorXI EMAP;
        //  std::vector<std::vector<Index> > uE2E;
        //  igl::unique_edge_map(pF, E, uE, EMAP, uE2E);
        //  typedef Index Index;
        //  typedef double Scalar;
        //  const Index num_faces = pF.rows();
        //  std::vector<Index> Q;
        //  Q.reserve(uE2E.size());
        //  for (size_t uei=0; uei<uE2E.size(); uei++)
        //  {
        //    Q.push_back(uei);
        //  }
        //  while(!Q.empty())
        //  {
        //    const Index uei = Q.back();
        //    Q.pop_back();
        //    if (uE2E[uei].size() == 2)
        //    {
        //      double w;
        //      igl::is_Indexrinsic_delaunay(pl,uE2E,num_faces,uei,w);
        //      prIndexf("%d : %0.17f\n",uei,w);
        //      if(w<-1e-7)
        //      {
        //        prIndexf("  flippin'\n");
        //        //
        //        //          v1                 v1
        //        //          /|\                / \
        //        //        c/ | \b            c/f1 \b
        //        //     v3 /f2|f1\ v4  =>  v3 /__f__\ v4
        //        //        \  e  /            \ f2  /
        //        //        d\ | /a            d\   /a
        //        //          \|/                \ /
        //        //          v2                 v2
        //        //
        //        // hmm... is the flip actually in the other direction?
        //        const Index f1 = uE2E[uei][0]%num_faces;
        //        const Index f2 = uE2E[uei][1]%num_faces;
        //        const Index c1 = uE2E[uei][0]/num_faces;
        //        const Index c2 = uE2E[uei][1]/num_faces;
        //        const size_t e_24 = f1 + ((c1 + 1) % 3) * num_faces;
        //        const size_t e_41 = f1 + ((c1 + 2) % 3) * num_faces;
        //        const size_t e_13 = f2 + ((c2 + 1) % 3) * num_faces;
        //        const size_t e_32 = f2 + ((c2 + 2) % 3) * num_faces;
        //        const size_t ue_24 = EMAP(e_24);
        //        const size_t ue_41 = EMAP(e_41);
        //        const size_t ue_13 = EMAP(e_13);
        //        const size_t ue_32 = EMAP(e_32);
        //        // new edge lengths
        //        const Index v1 = pF(f1, (c1+1)%3);
        //        const Index v2 = pF(f1, (c1+2)%3);
        //        const Index v4 = pF(f1, c1);
        //        const Index v3 = pF(f2, c2);
        //        {
        //          const Scalar e = pl(f1,c1);
        //          const Scalar a = pl(f1,(c1+1)%3);
        //          const Scalar b = pl(f1,(c1+2)%3);
        //          const Scalar c = pl(f2,(c2+1)%3);
        //          const Scalar d = pl(f2,(c2+2)%3);
        //          const double f = (pV.row(v3)-pV.row(v4)).norm();
        //          // New order
        //          pl(f1,0) = f;
        //          pl(f1,1) = b;
        //          pl(f1,2) = c;
        //          pl(f2,0) = f;
        //          pl(f2,1) = d;
        //          pl(f2,2) = a;
        //        }
        //        prIndexf("%d,%d %d,%d -> %d,%d\n",uE(uei,0),uE(uei,1),v1,v2,v3,v4);
        //        igl::flip_edge(pF, E, uE, EMAP, uE2E, uei);
        //        std::cout<<"  "<<pl.row(f1)<<std::endl;
        //        std::cout<<"  "<<pl.row(f2)<<std::endl;
        //        //// new edge lengths, slow!
        //        //igl::edge_lengths(pV,pF,pl);
        //        // recompute edge lengths of two faces. (extra work on untouched
        //        // edges)
        //        for(Index f : {f1,f2})
        //        {
        //          for(Index c=0;c<3;c++)
        //          {
        //            pl(f,c) =
        //              (pV.row(pF(f,(c+1)%3))-pV.row(pF(f,(c+2)%3))).norm();
        //          }
        //        }
        //        std::cout<<"  "<<pl.row(f1)<<std::endl;
        //        std::cout<<"  "<<pl.row(f2)<<std::endl;
        //        std::cout<<std::endl;

        //        Q.push_back(ue_24);
        //        Q.push_back(ue_41);
        //        Q.push_back(ue_13);
        //        Q.push_back(ue_32);
        //      }
        //    }
        //  }

        //  // check for self-loops (I claim these cannot happen)
        //  for(Index f = 0;f<pF.rows();f++)
        //  {
        //    for(Index c =0;c<3;c++)
        //    {
        //      assert(pF(f,c) != pF(f,(c+1)%3) && "self loops should not exist");
        //    }
        //  }
        //}
      }
      // Copy Indexo global list
      for(Index i = 0;i<pF.rows();i++)
      {
        for(Index c =0;c<3;c++)
        {
          F(f,c) = I(C(p)+pF(i,c));
        }
        J(f) = p;
        f++;
      }

    }
    assert(f == F.rows());
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
#endif


/*
    Copyright 2005-2007 Adobe Systems Incorporated

    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_GIL_EXTENSION_NUMERIC_CONVOLVE_HPP
#define BOOST_GIL_EXTENSION_NUMERIC_CONVOLVE_HPP

/*!
/// \file
/// \brief 2D seperable convolutions and correlations
///
/// \author Hailin Jin and Lubomir Bourdev \n
///         Adobe Systems Incorporated
/// \date   2005-2007 \n
*/

#include <cstddef>
#include <cassert>
#include <algorithm>
#include <vector>
#include <functional>

#include <boost/gil/gil_config.hpp>
#include <boost/gil/image_view_factory.hpp>
#include <boost/gil/algorithm.hpp>
#include <boost/gil/metafunctions.hpp>
#include <boost/gil/extension/numeric/pixel_numeric_operations.hpp>
#include <boost/gil/extension/numeric/algorithm.hpp>

namespace boost { namespace gil {

/// \ingroup ImageAlgorithms
/// Boundary options for 1D correlations/convolutions
enum convolve_boundary_option  {
    convolve_option_output_ignore,  /// do nothing to the output
    convolve_option_output_zero,    /// set the output to zero
    convolve_option_extend_padded,  /// assume the source boundaries to be padded already
    convolve_option_extend_zero,    /// assume the source boundaries to be zero
    convolve_option_extend_constant /// assume the source boundaries to be the boundary value
};

namespace detail {
/// compute the correlation of 1D kernel with the rows of an image
template <typename PixelAccum,typename SrcView,typename Kernel,typename DstView,typename Correlator>
void correlate_rows_imp(const SrcView& src, const Kernel& ker, const DstView& dst,
                        convolve_boundary_option option,
                        Correlator correlator) {
    assert(src.dimensions()==dst.dimensions());
    assert(ker.size()!=0);

    typedef typename pixel_proxy<typename SrcView::value_type>::type PIXEL_SRC_REF;
    typedef typename pixel_proxy<typename DstView::value_type>::type PIXEL_DST_REF;
    typedef typename Kernel::value_type kernel_type;

    if(ker.size()==1) {//reduces to a multiplication
        view_multiplies_scalar<PixelAccum>(src,*ker.begin(),dst);
        return;
    }

    int width=src.width(),height=src.height();
    PixelAccum acc_zero; pixel_zeros_t<PixelAccum>()(acc_zero);
    if (width==0) return;
    if (option==convolve_option_output_ignore || option==convolve_option_output_zero) {
        typename DstView::value_type dst_zero; pixel_assigns_t<PixelAccum,PIXEL_DST_REF>()(acc_zero,dst_zero);
        if (width<(int)ker.size()) {
            if (option==convolve_option_output_zero)
                fill_pixels(dst,dst_zero);
        } else {
            std::vector<PixelAccum> buffer(width);
            for(int rr=0;rr<height;++rr) {
                assign_pixels(src.row_begin(rr),src.row_end(rr),&buffer.front());
                typename DstView::x_iterator it_dst=dst.row_begin(rr);
                if (option==convolve_option_output_zero)
                    std::fill_n(it_dst,ker.left_size(),dst_zero);
                it_dst+=ker.left_size();
                correlator(&buffer.front(),&buffer.front()+width+1-ker.size(),
                           ker.begin(),it_dst);
                it_dst+=width+1-ker.size();
                if (option==convolve_option_output_zero)
                    std::fill_n(it_dst,ker.right_size(),dst_zero);
            }
        }
    } else {
        std::vector<PixelAccum> buffer(width+ker.size()-1);
        for(int rr=0;rr<height;++rr) {
            PixelAccum* it_buffer=&buffer.front();
            if        (option==convolve_option_extend_padded) {
                assign_pixels(src.row_begin(rr)-ker.left_size(),
                              src.row_end(rr)+ker.right_size(),
                              it_buffer);
            } else if (option==convolve_option_extend_zero) {
                std::fill_n(it_buffer,ker.left_size(),acc_zero);
                it_buffer+=ker.left_size();
                assign_pixels(src.row_begin(rr),src.row_end(rr),it_buffer);
                it_buffer+=width;
                std::fill_n(it_buffer,ker.right_size(),acc_zero);
            } else if (option==convolve_option_extend_constant) {
                PixelAccum filler;
                pixel_assigns_t<PIXEL_SRC_REF,PixelAccum>()(*src.row_begin(rr),filler);
                std::fill_n(it_buffer,ker.left_size(),filler);
                it_buffer+=ker.left_size();
                assign_pixels(src.row_begin(rr),src.row_end(rr),it_buffer);
                it_buffer+=width;
                pixel_assigns_t<PIXEL_SRC_REF,PixelAccum>()(src.row_end(rr)[-1],filler);
                std::fill_n(it_buffer,ker.right_size(),filler);
            }
            correlator(&buffer.front(),&buffer.front()+width,
                       ker.begin(),
                       dst.row_begin(rr));
        }
    }
}
template <typename PixelAccum>
class correlator_n {
private:
    std::size_t _size;
public:
    correlator_n(std::size_t size_in) : _size(size_in) {}
    template <typename SrcIterator,typename KernelIterator,typename DstIterator>
    void operator()(SrcIterator src_begin,SrcIterator src_end,
                    KernelIterator ker_begin,
                    DstIterator dst_begin) {
        correlate_pixels_n<PixelAccum>(src_begin,src_end,ker_begin,_size,dst_begin);
    }
};
template <std::size_t Size,typename PixelAccum>
struct correlator_k {
public:
    template <typename SrcIterator,typename KernelIterator,typename DstIterator>
    void operator()(SrcIterator src_begin,SrcIterator src_end,
                    KernelIterator ker_begin,
                    DstIterator dst_begin){
        correlate_pixels_k<Size,PixelAccum>(src_begin,src_end,ker_begin,dst_begin);
    }
};
} // namespace detail

/// \ingroup ImageAlgorithms
///correlate a 1D variable-size kernel along the rows of an image
template <typename PixelAccum,typename SrcView,typename Kernel,typename DstView>
BOOST_FORCEINLINE
void correlate_rows(const SrcView& src, const Kernel& ker, const DstView& dst,
                    convolve_boundary_option option=convolve_option_extend_zero) {
    detail::correlate_rows_imp<PixelAccum>(src,ker,dst,option,detail::correlator_n<PixelAccum>(ker.size()));
}

/// \ingroup ImageAlgorithms
///correlate a 1D variable-size kernel along the columns of an image
template <typename PixelAccum,typename SrcView,typename Kernel,typename DstView>
BOOST_FORCEINLINE
void correlate_cols(const SrcView& src, const Kernel& ker, const DstView& dst,
                    convolve_boundary_option option=convolve_option_extend_zero) {
    correlate_rows<PixelAccum>(transposed_view(src),ker,transposed_view(dst),option);
}

/// \ingroup ImageAlgorithms
///convolve a 1D variable-size kernel along the rows of an image
template <typename PixelAccum,typename SrcView,typename Kernel,typename DstView>
BOOST_FORCEINLINE
void convolve_rows(const SrcView& src, const Kernel& ker, const DstView& dst,
                   convolve_boundary_option option=convolve_option_extend_zero) {
    correlate_rows<PixelAccum>(src,reverse_kernel(ker),dst,option);
}

/// \ingroup ImageAlgorithms
///convolve a 1D variable-size kernel along the columns of an image
template <typename PixelAccum,typename SrcView,typename Kernel,typename DstView>
BOOST_FORCEINLINE
void convolve_cols(const SrcView& src, const Kernel& ker, const DstView& dst,
                   convolve_boundary_option option=convolve_option_extend_zero) {
    convolve_rows<PixelAccum>(transposed_view(src),ker,transposed_view(dst),option);
}

/// \ingroup ImageAlgorithms
///correlate a 1D fixed-size kernel along the rows of an image
template <typename PixelAccum,typename SrcView,typename Kernel,typename DstView>
BOOST_FORCEINLINE
void correlate_rows_fixed(const SrcView& src, const Kernel& ker, const DstView& dst,
                          convolve_boundary_option option=convolve_option_extend_zero) {
    detail::correlate_rows_imp<PixelAccum>(src,ker,dst,option,detail::correlator_k<Kernel::static_size,PixelAccum>());
}

/// \ingroup ImageAlgorithms
///correlate a 1D fixed-size kernel along the columns of an image
template <typename PixelAccum,typename SrcView,typename Kernel,typename DstView>
BOOST_FORCEINLINE
void correlate_cols_fixed(const SrcView& src, const Kernel& ker, const DstView& dst,
                          convolve_boundary_option option=convolve_option_extend_zero) {
    correlate_rows_fixed<PixelAccum>(transposed_view(src),ker,transposed_view(dst),option);
}

/// \ingroup ImageAlgorithms
///convolve a 1D fixed-size kernel along the rows of an image
template <typename PixelAccum,typename SrcView,typename Kernel,typename DstView>
BOOST_FORCEINLINE
void convolve_rows_fixed(const SrcView& src, const Kernel& ker, const DstView& dst,
                         convolve_boundary_option option=convolve_option_extend_zero) {
    correlate_rows_fixed<PixelAccum>(src,reverse_kernel(ker),dst,option);
}

/// \ingroup ImageAlgorithms
///convolve a 1D fixed-size kernel along the columns of an image
template <typename PixelAccum,typename SrcView,typename Kernel,typename DstView>
BOOST_FORCEINLINE
void convolve_cols_fixed(const SrcView& src, const Kernel& ker, const DstView& dst,
                         convolve_boundary_option option=convolve_option_extend_zero) {
    convolve_rows_fixed<PixelAccum>(transposed_view(src),ker,transposed_view(dst),option);
}

} }  // namespace boost::gil

#endif // BOOST_GIL_EXTENSION_NUMERIC_CONVOLVE_HPP