#pragma once

#ifdef USE_OPENCV
#include <boost/generator_iterator.hpp>
#include <boost/random.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>

#include "caffe/data_transformer.hpp"
#include "caffe/util/io.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {
namespace ultinous {

class UltinousTransformer
{
private:
  typedef boost::mt19937 RNGType;

public:
  UltinousTransformer( UltinousTransformationParameter const& params, Phase phase )
    : m_params( params )
    , m_phase( phase )
  {
    CHECK_LT(m_params.uniformnoisestrength(), 128)
      << "uniformNoiseStrength must not exceed 127!";

    CHECK_LE(m_params.minocclusionradius(), m_params.maxocclusionradius())
      << "minOcclusionRadius must be less than maxOcclusionRadius!";

    CHECK_LE(m_params.minblursigma(), m_params.maxblursigma())
      << "minBlurSigma must be less then or equal to maxBlurSigma";

    m_uniformNoiseStrength = static_cast<int16_t>( m_params.uniformnoisestrength() );

    xorshf96_x=123456789;
    xorshf96_y=362436069;
    xorshf96_z=521288629;
  }

  void transform( cv::Mat& cv_img )
  {
    if( m_phase != TRAIN ) return;

    // Apply affine transformation
    cv::Mat cv_affine = cv_img.clone();
    applyAffine( cv_img, cv_affine );
    cv_img = cv_affine;

    uint32_t cropHeight = m_params.cropheight();
    uint32_t cropWidth = m_params.cropwidth();
    if( cropHeight > 0 || cropWidth > 0 )
    {
      CHECK( cropHeight > 0 && cropWidth > 0 );
      CHECK( cropHeight <= cv_img.rows && cropWidth <= cv_img.cols );

      uint32_t offX = rand() % (cv_img.cols-cropWidth+1);
      uint32_t offY = rand() % (cv_img.rows-cropHeight+1);

      cv_img = cv_img( cv::Rect(offX, offY, cropWidth, cropHeight ) ).clone();
    }

    // Apply color transformation
    if( m_params.luminanceminscale() != 1.0f
      || m_params.luminancemaxscale() != 1.0f
      || m_params.saturationminscale() != 1.0f
      || m_params.saturationmaxscale() != 1.0f )
    {

      float lumCoef = m_params.luminanceminscale()
        + (m_params.luminancemaxscale()-m_params.luminanceminscale())
          *static_cast <float> (rand()) / static_cast <float> (RAND_MAX);

      float satCoef = m_params.saturationminscale()
        + (m_params.saturationmaxscale()-m_params.saturationminscale())
          *static_cast <float> (rand()) / static_cast <float> (RAND_MAX);

      cv::cvtColor( cv_img, cv_img, CV_BGR2HLS );

      CHECK(cv_img.depth() == CV_8U) << "Image data type must be unsigned byte";
      CHECK( cv_img.isContinuous() ) << "OpenCV image matrix must be contiuously stored in memory!";

      cv::Mat hls[3];
      cv::split( cv_img, hls );
      hls[1] *= lumCoef;
      hls[2] *= satCoef;
      cv::merge( hls, 3, cv_img );

      cv::cvtColor( cv_img, cv_img, CV_HLS2BGR );
    }

    // Gaussian Blur
    if( m_params.maxblursigma() != 0 )
    {
      float sigma = m_params.minblursigma()
        + (m_params.maxblursigma()-m_params.minblursigma())
          *static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
      cv::Mat cv_dst;
      GaussianBlur( cv_img, cv_dst, cv::Size( 0, 0 ), sigma, sigma );
      cv_img = cv_dst;
    }

    // Occlusion
    if( m_params.numocclusions() != 0 && m_params.maxocclusionradius() != 0 )
    {

      for( size_t i = 0; i < m_params.numocclusions(); ++ i )
      {
        size_t r = m_params.minocclusionradius()
        + (m_params.maxocclusionradius()-m_params.minocclusionradius())
          *static_cast <float> (rand()) / static_cast <float> (RAND_MAX);

        size_t cx = r + std::rand() % (cv_img.rows - 2*r);
        size_t cy = r + std::rand() % (cv_img.cols - 2*r);

        cv::circle( cv_img, cv::Point(cx,cy), r, cv::Scalar(0), -1 );
      }
    }

    // Uniform noise
    if( m_uniformNoiseStrength != 0 )
    {
      CHECK(cv_img.depth() == CV_8U) << "Image data type must be unsigned byte";
      CHECK( cv_img.isContinuous() ) << "OpenCV image matrix must be contiuously stored in memory!";

      //boost::uniform_int<int16_t> distribution( -m_uniformNoiseStrength, m_uniformNoiseStrength );
      //boost::variate_generator< RNGType, boost::uniform_int<int16_t> > noiseGenerator(m_rng, distribution);

      size_t N = cv_img.channels() * cv_img.rows * cv_img.cols;

      //vector<int16_t> rndVec(N);
      //generate(rndVec.begin(), rndVec.end(), noiseGenerator);

      uint8_t * p = cv_img.data;
      int16_t MIN = static_cast<int16_t>(0);
      int16_t MAX = static_cast<int16_t>(std::numeric_limits<uint8_t>::max());
      for( size_t i = 0; i < N; ++i, ++p) {
        *p = static_cast<uint8_t>(
          std::max( MIN, std::min( MAX,
            static_cast<int16_t>(static_cast<int16_t>(*p)
              + static_cast<int16_t>(myRandom(1+2*m_uniformNoiseStrength))-m_uniformNoiseStrength )
          ) )
        );
      }
    }

    // Padding
    if( m_params.padtop() != 0 || m_params.padbottom() != 0
      || m_params.padleft() != 0 || m_params.padright() != 0 )
    {
      cv::Mat dst;

      cv::copyMakeBorder( cv_img, dst, m_params.padtop(), m_params.padbottom()
                    , m_params.padleft(), m_params.padright(), cv::BORDER_CONSTANT );

      cv_img = dst;
    }

/*    static int ccc = 0;
    std::string fname = boost::lexical_cast<std::string>(ccc) + ".png";
    cv::imwrite( fname, cv_img );
    ++ccc;*/

  }

private:
  void applyAffine( cv::Mat& src, cv::Mat& dst )
  {
    const double pi = std::acos(-1);

    float sx=1.0f, sy=1.0f, ex=0.0f, ey=0.0f, ca=1.0f, sa=0.0f, tx=0.0f, ty=0.0f;

    float centery = static_cast<float>(src.rows)/2.0f;
    float centerx = static_cast<float>(src.cols)/2.0f;

    if( m_params.verticalminscale() != 1.0f || m_params.verticalmaxscale() != 1.0f)
    {
      sy = m_params.verticalminscale()
        + (m_params.verticalmaxscale()-m_params.verticalminscale())
          *static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
    }
    if( m_params.horizontalminscale() != 1.0f || m_params.horizontalmaxscale() != 1.0f )
    {
      sx = m_params.horizontalminscale()
        + (m_params.horizontalmaxscale()-m_params.horizontalminscale())
          *static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
    }

    if( m_params.verticalminshear() != 0.0f || m_params.verticalmaxshear() != 0.0f)
    {
      ey = m_params.verticalminshear()
        + (m_params.verticalmaxshear()-m_params.verticalminshear())
          *static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
    }

    if( m_params.horizontalminshear() != 0.0f || m_params.horizontalmaxshear() != 0.0f)
    {
      ex = m_params.horizontalminshear()
        + (m_params.horizontalmaxshear()-m_params.horizontalminshear())
          *static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
    }

    if( m_params.minrotation() != 0.0f || m_params.maxrotation() != 0.0f)
    {
      float deg = m_params.minrotation()
        + (m_params.maxrotation()-m_params.minrotation())
          *static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
      float rad = deg / 180.0f * pi;

      ca = std::cos(rad);
      sa = std::sin(rad);
    }

    if( m_params.horizontaltranslation() != 0.0f )
    {
      tx = m_params.horizontaltranslation()
          *(2.0f*static_cast<float>(rand()) / static_cast<float>(RAND_MAX)-1);
    }
    if( m_params.verticaltranslation() != 0.0f )
    {
      ty = m_params.verticaltranslation()
          *(2.0f*static_cast<float>(rand()) / static_cast<float>(RAND_MAX)-1);
    }

    cv::Mat affine( 2, 3, CV_32FC1 );

    affine.at<float>( 0, 0 ) = sx*(ca+sa*ex);
    affine.at<float>( 0, 1 ) = sx*(-sa+ca*ex);

    affine.at<float>( 1, 0 ) = sy*(ca*ey+sa);
    affine.at<float>( 1, 1 ) = sy*(-sa*ey+ca);

    affine.at<float>( 0, 2 ) = affine.at<float>(0,0)*(-centerx)
              + affine.at<float>(0,1)*(-centery)
              + centerx + tx;
    affine.at<float>( 1, 2 ) = affine.at<float>(1,0)*(-centerx)
              + affine.at<float>(1,1)*(-centery)
              + centery + ty;

    cv::warpAffine( src, dst, affine, src.size(), cv::INTER_CUBIC, cv::BORDER_TRANSPARENT);
  }

  uint32_t xorshf96(void) {          //period 2^96-1
    xorshf96_x ^= xorshf96_x << 16;
    xorshf96_x ^= xorshf96_x >> 5;
    xorshf96_x ^= xorshf96_x << 1;
    xorshf96_t = xorshf96_x;
    xorshf96_x = xorshf96_y;
    xorshf96_y = xorshf96_z;
    xorshf96_z = xorshf96_t ^ xorshf96_x ^ xorshf96_y;
    return xorshf96_z;
  }

  uint64_t myRandom( uint64_t N )
  {
    if( N-1 <= std::numeric_limits<uint32_t>::max() )
    {
      return uint64_t(xorshf96()) % N;
    }
    uint64_t r = 0;
    r += uint64_t(xorshf96());
    r += uint64_t(xorshf96())<<32;
    return r % N;
  }
private:
  UltinousTransformationParameter const& m_params;
  Phase m_phase;

  int16_t m_uniformNoiseStrength;
  RNGType m_rng;

    /* Random generator variables */
  uint32_t xorshf96_x, xorshf96_y, xorshf96_z;
  uint32_t xorshf96_t;

};

}  // namespace ultinous
}  // namespace caffe
#endif  // USE_OPENCV


/*
    This file is part of Nori, a simple educational ray tracer

    Copyright (c) 2015 by Wenzel Jakob

    Nori is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Nori is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <nori/camera.h>
#include <nori/rfilter.h>
#include <nori/warp.h>
#include <Eigen/Geometry>

NORI_NAMESPACE_BEGIN

/**
 * \brief Perspective camera with depth of field
 *
 * This class implements a simple perspective camera model. It uses an
 * infinitesimally small aperture, creating an infinite depth of field.
 */
class PerspectiveCamera : public Camera {
public:
    PerspectiveCamera(const PropertyList &propList) {
        /* Width and height in pixels. Default: 720p */
        m_outputSize.x() = propList.getInteger("width", 1280);
        m_outputSize.y() = propList.getInteger("height", 720);
        m_invOutputSize = m_outputSize.cast<float>().cwiseInverse();

        /* Specifies an optional camera-to-world transformation. Default: none */
        m_cameraToWorld = propList.getTransform("toWorld", Transform());

        /* Horizontal field of view in degrees */
        m_fov = propList.getFloat("fov", 30.0f);

        /* Near and far clipping planes in world-space units */
        m_nearClip = propList.getFloat("nearClip", 1e-4f);
        m_farClip = propList.getFloat("farClip", 1e4f);

        m_rfilter = NULL;
    }

    void activate() {
        float aspect = m_outputSize.x() / (float) m_outputSize.y();

        /* Project vectors in camera space onto a plane at z=1:
         *
         *  xProj = cot * x / z
         *  yProj = cot * y / z
         *  zProj = (far * (z - near)) / (z * (far-near))
         *  The cotangent factor ensures that the field of view is
         *  mapped to the interval [-1, 1].
         */
        float recip = 1.0f / (m_farClip - m_nearClip),
              cot = 1.0f / std::tan(degToRad(m_fov / 2.0f));

        Eigen::Matrix4f perspective;
        perspective <<
            cot, 0,   0,   0,
            0, cot,   0,   0,
            0,   0,   m_farClip * recip, -m_nearClip * m_farClip * recip,
            0,   0,   1,   0;

        /**
         * Translation and scaling to shift the clip coordinates into the
         * range from zero to one. Also takes the aspect ratio into account.
         */
        m_sampleToCamera = Transform(
            Eigen::DiagonalMatrix<float, 3>(Vector3f(-0.5f, -0.5f * aspect, 1.0f)) *
            Eigen::Translation<float, 3>(-1.0f, -1.0f/aspect, 0.0f) * perspective).inverse();

        /* If no reconstruction filter was assigned, instantiate a Gaussian filter */
        if (!m_rfilter)
            m_rfilter = static_cast<ReconstructionFilter *>(
                NoriObjectFactory::createInstance("gaussian", PropertyList()));
    }

    Color3f sampleRay(Ray3f &ray,
            const Point2f &samplePosition,
            const Point2f &apertureSample) const {
        /* Compute the corresponding position on the
           near plane (in local camera space) */
        Point3f nearP = m_sampleToCamera * Point3f(
            samplePosition.x() * m_invOutputSize.x(),
            samplePosition.y() * m_invOutputSize.y(), 0.0f);

        /* Turn into a normalized ray direction, and
           adjust the ray interval accordingly */
        Vector3f d = nearP.normalized();
        float invZ = 1.0f / d.z();

        ray.o = m_cameraToWorld * Point3f(0, 0, 0);
        ray.d = m_cameraToWorld * d;
        ray.mint = m_nearClip * invZ;
        ray.maxt = m_farClip * invZ;
        ray.update();

        return Color3f(1.0f);
    }

    void addChild(NoriObject *obj) {
        switch (obj->getClassType()) {
            case EReconstructionFilter:
                if (m_rfilter)
                    throw NoriException("Camera: tried to register multiple reconstruction filters!");
                m_rfilter = static_cast<ReconstructionFilter *>(obj);
                break;

            default:
                throw NoriException("Camera::addChild(<%s>) is not supported!",
                    classTypeName(obj->getClassType()));
        }
    }

    /// Return a human-readable summary
    std::string toString() const {
        return tfm::format(
            "PerspectiveCamera[\n"
            "  cameraToWorld = %s,\n"
            "  outputSize = %s,\n"
            "  fov = %f,\n"
            "  clip = [%f, %f],\n"
            "  rfilter = %s\n"
            "]",
            indent(m_cameraToWorld.toString(), 18),
            m_outputSize.toString(),
            m_fov,
            m_nearClip,
            m_farClip,
            indent(m_rfilter->toString())
        );
    }
private:
    Vector2f m_invOutputSize;
    Transform m_sampleToCamera;
    Transform m_cameraToWorld;
    float m_fov;
    float m_nearClip;
    float m_farClip;
};

NORI_REGISTER_CLASS(PerspectiveCamera, "perspective");
NORI_NAMESPACE_END


/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright notice,
 *       this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice,
 *       this list of conditions and the following disclaimer in the documentation
 *       and/or other materials provided with the distribution.
 *     * Neither the name of the Politecnico di Milano nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
 */

#ifndef AMA_TENSOR_TENSOR_HPP
#define AMA_TENSOR_TENSOR_HPP 1

#include <ama/tensor/config.hpp>
#include <ama/tensor/copy.hpp>
#include <ama/tensor/detail/tensor_base.hpp>
#include <ama/tensor/iexp/iexp.hpp>
#include <ama/tensor/iexp/iexp_calculator.hpp>
#include <ama/tensor/iexp/iexp_factory.hpp>
#include <ama/tensor/iexp/indices.hpp>
#include <ama/common/size_t.hpp>
#include <ama/multi_array/multi_array.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/size_t.hpp>

namespace ama
{

  template <typename S, /* scalar value used to represent scalar value */
            size_t D,   /* the dimension of vector space               */
            size_t CT,  /* the dimension of controvariant part         */
            size_t CO>  /* the dimension of covariant part             */
  class tensor;

  /* tensor traits specialization */
  namespace tensor_
  {

    template <typename S, size_t D, size_t CT, size_t CO>
    struct tensor_traits< tensor<S,D,CT,CO> >
    {
      typedef S value_type;

      typedef ::boost::mpl::size_t<D> dimension_type;

      typedef ::boost::mpl::size_t<CT> controvariant_type;
      typedef ::boost::mpl::size_t<CO> covariant_type;

      typedef ::boost::mpl::true_ is_assignable;
      typedef ::boost::mpl::false_ is_temporary;
    };

  }

  /* class declaration */
  template <typename S, size_t D, size_t CT, size_t CO>
  class tensor:
        public tensor_::tensor_base< tensor<S,D,CT,CO> >
      , public multi_array<S,D,CT+CO>
#ifdef AMA_MULTI_ARRAY_USE_LINEAR_ACCESS
      , public tensor_::iexp< tensor<S,D,CT,CO>, CT+CO >
#endif /* AMA_MULTI_ARRAY_USE_LINEAR_ACCESS */
  {
  public:
    /* resolv the ambiguity */
    typedef typename tensor_::tensor_base< tensor<S,D,CT,CO> >::value_type value_type;
    typedef typename tensor_::tensor_base< tensor<S,D,CT,CO> >::dimension_type dimension_type;

    /* needed */
    typedef typename tensor_::tensor_base< tensor<S,D,CT,CO> >::controvariant_type controvariant_type;
    typedef typename tensor_::tensor_base< tensor<S,D,CT,CO> >::covariant_type covariant_type;

  public:
    /* default constructor */
    tensor():
        tensor_::tensor_base< tensor<S,D,CT,CO> >(),
        multi_array<S,D,CT+CO>() { }

    /* copy constructor */
    tensor(tensor const & t):
        tensor_::tensor_base< tensor<S,D,CT,CO> >(),
        multi_array<S,D,CT+CO>(t) { }

    template <typename DERIVED>
    explicit
    tensor(tensor_::tensor_base<DERIVED> const & t):
        tensor_::tensor_base< tensor<S,D,CT,CO> >(),
        multi_array<S,D,CT+CO>()
    {
      ama::copy(t.derived(), *this);
    }

  public:
    /* copy operator */
    template <typename DERIVED>
    tensor & operator=(tensor const & t)
    {
      ama::copy(t.derived(), *this);
      return *this;
    }

    template <typename DERIVED>
    tensor & operator=(tensor_::tensor_base<DERIVED> const & t)
    {
      ama::copy(t.derived(), *this);
      return *this;
    }

#ifdef AMA_MULTI_ARRAY_USE_LINEAR_ACCESS
  public:
    using multi_array<S,D,CT+CO>::operator();
    using tensor_::iexp< tensor<S,D,CT,CO> , CT+CO >::operator();
#endif /* AMA_MULTI_ARRAY_USE_LINEAR_ACCESS */

/* ============================ INDEX EXPRESSION ============================ */
  public:
    /* index expression contruction */
    template <typename ILIST>
    typename tensor_::iexp_calculator<tensor, ILIST, ::boost::mpl::false_>::type
    idx()
    {
      namespace mpl = ::boost::mpl;

      typedef mpl::equal_to<
            mpl::size<typename tensor_::controvariant<tensor<S,D,CT,CO>, ILIST>::type>
          , controvariant_type
          > ct;
      typedef mpl::equal_to<
            mpl::size<typename tensor_::covariant<tensor<S,D,CT,CO>, ILIST>::type>
          , covariant_type
          > co;

      BOOST_MPL_ASSERT_MSG(
            (ct::value && co::value)
          , THE_LENGTH_OF_LIST_OF_INDICES_MUST_BE_EQUAL_TO_THE_TENSOR_ORDER
          , (ILIST));

      typedef typename tensor_::iexp_calculator<tensor, ILIST, ::boost::mpl::false_>::type what;

      return tensor_::iexp_factory<what>::template apply<ILIST>(*this);
    }

    template <typename ILIST>
    typename tensor_::iexp_calculator<tensor, ILIST, ::boost::mpl::true_>::type
    idx() const
    {
      namespace mpl = ::boost::mpl;

      typedef mpl::equal_to<
            mpl::size<typename tensor_::controvariant<tensor<S,D,CT,CO>, ILIST>::type>
          , controvariant_type
          > ct;
      typedef mpl::equal_to<
            mpl::size<typename tensor_::covariant<tensor<S,D,CT,CO>, ILIST>::type>
          , covariant_type
          > co;

      BOOST_MPL_ASSERT_MSG(
            (ct::value && co::value)
          , THE_LENGTH_OF_LIST_OF_INDICES_MUST_BE_EQUAL_TO_THE_TENSOR_ORDER
          , (ILIST));

      typedef typename tensor_::iexp_calculator<tensor, ILIST, ::boost::mpl::true_>::type what;

      return tensor_::iexp_factory<what>::template apply<ILIST>(*this);
    }
/* ============================ INDEX EXPRESSION ============================ */
  };

  /* partial specialization for 0-order tensor */
  template <typename S, size_t D>
  class tensor<S,D,0,0>:
      public tensor_::tensor_base< tensor<S,D,0,0> >,
      public multi_array<S,D,0>
  {
  public:
    /* resolv the ambiguity */
    typedef typename tensor_::tensor_base< tensor<S,D,0,0> >::value_type value_type;
    typedef typename tensor_::tensor_base< tensor<S,D,0,0> >::dimension_type dimension_type;

  public:
    /* default constructor */
    explicit
    tensor(S const & value = S()):
        tensor_::tensor_base< tensor<S,D,0,0> >(),
        multi_array<S,D,0>(value) { }

    /* copy constructor */
    tensor(tensor const & t):
        tensor_::tensor_base< tensor<S,D,0,0> >(),
        multi_array<S,D,0>(t) { }

    template <typename DERIVED>
    explicit
    tensor(tensor_::tensor_base<DERIVED> const & t):
        tensor_::tensor_base< tensor<S,D,0,0> >(),
        multi_array<S,D,0>()
    {
      ama::copy(t.derived(), *this);
    }

  public:
    /* copy operator */
    template <typename DERIVED>
    tensor & operator=(tensor_::tensor_base<DERIVED> const & t)
    {
      ama::copy(t.derived(), *this);
      return *this;
    }

  public:
    /* cast operator */
    operator value_type() { return this->template at<void>(); }
    operator value_type() const { return this->template at<void>(); }
  };

  /* partial specialization for 0-dimension tensor */
  template <typename S, size_t CT, size_t CO>
  class tensor<S,0,CT,CO>:
      public tensor_::tensor_base< tensor<S,0,CT,CO> >,
      public multi_array<S,0,CT+CO>
  {
  public:
    /* resolv the ambiguity */
    typedef typename tensor_::tensor_base< tensor<S,0,CT,CO> >::value_type value_type;
    typedef typename tensor_::tensor_base< tensor<S,0,CT,CO> >::dimension_type dimension_type;
  };

  /* partial specialization for 0-dimension tensor */
  template <typename S>
  class tensor<S,0,0,0>:
      public tensor_::tensor_base< tensor<S,0,0,0> >,
      public multi_array<S,0,0>
  {
  public:
    /* resolv the ambiguity */
    typedef typename tensor_::tensor_base< tensor<S,0,0,0> >::value_type value_type;
    typedef typename tensor_::tensor_base< tensor<S,0,0,0> >::dimension_type dimension_type;
  };

}

#endif /* AMA_TENSOR_TENSOR_HPP */


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include <geometry_test_common.hpp>

#include <boost/type_traits/is_same.hpp>

#include <boost/geometry/core/tag.hpp>

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

#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

#include <boost/geometry/geometries/register/linestring.hpp>

#include <vector>
#include <deque>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(std::vector)
BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(std::deque)

template <typename G, typename Expected>
void test_geometry()
{
    BOOST_CHECK_EQUAL(typeid(typename bg::tag<G>::type).name(),
        typeid(Expected).name());

    static const bool is_same = boost::is_same<typename bg::tag<G>::type, Expected>::value;
    BOOST_CHECK(is_same);
}

template <typename P, size_t D>
void test_all()
{
    test_geometry<P, bg::point_tag>();
    test_geometry<P const, bg::point_tag>();
    test_geometry<P*, bg::point_tag>();
    test_geometry<P&, bg::point_tag>();
    test_geometry<P*&, bg::point_tag>();
    test_geometry<const P *, bg::point_tag>();
    test_geometry<bg::model::linestring<P> , bg::linestring_tag>();
    test_geometry<bg::model::linestring<P> *&, bg::linestring_tag>();
    test_geometry<bg::model::ring<P> , bg::ring_tag>();
    test_geometry<bg::model::polygon<P> , bg::polygon_tag>();
    test_geometry<bg::model::box<P> , bg::box_tag>();
    test_geometry<bg::model::segment<P> , bg::segment_tag>();
    test_geometry<bg::model::referring_segment<P const> , bg::segment_tag>();

    test_geometry<std::vector<P>, bg::linestring_tag>();
    test_geometry<std::deque<P>, bg::linestring_tag>();

}

int test_main(int, char* [])
{
    test_geometry<int[2], bg::point_tag>();
    test_geometry<float[2], bg::point_tag>();
    test_geometry<double[2], bg::point_tag>();

    test_geometry<int[3], bg::point_tag>();
    test_geometry<float[3], bg::point_tag>();
    test_geometry<double[3], bg::point_tag>();

    test_geometry<boost::tuple<double, double>, bg::point_tag>();
    test_geometry<boost::tuple<double, double, double>, bg::point_tag>();

    test_all<bg::model::point<int, 2, bg::cs::cartesian>, 2 >();
    test_all<bg::model::point<float, 2, bg::cs::cartesian>, 2 >();
    test_all<bg::model::point<double, 2, bg::cs::cartesian>, 2 >();

    return 0;
}


/*
 * $Id: vecnorm1.cc,v 1.3 2003/12/11 03:44:22 julianc Exp $
 *
 * Copyright (C) 1997 Todd Veldhuizen <tveldhui@oonumerics.org>
 * All rights reserved.  Please see <blitz/blitz.h> for terms and
 * conditions of use.
 *
 */

#ifndef BZ_VECNORM1_CC
#define BZ_VECNORM1_CC

#ifndef BZ_VECGLOBS_H
 #error <blitz/vecnorm1.cc> must be included via <blitz/vecglobs.h>
#endif

#include <blitz/applics.h>

BZ_NAMESPACE(blitz)

template<typename P_expr>
inline
BZ_SUMTYPE(_bz_typename P_expr::T_numtype)
_bz_vec_norm1(P_expr vector)
{
    typedef _bz_typename P_expr::T_numtype T_numtype;
    typedef BZ_SUMTYPE(T_numtype)          T_sumtype;

    T_sumtype sum = 0;
    int length = vector._bz_suggestLength();

    if (vector._bz_hasFastAccess())
    {
        for (int i=0; i < length; ++i)
            sum += _bz_abs<T_numtype>::apply(vector._bz_fastAccess(i));
    }
    else {
        for (int i=0; i < length; ++i)
            sum += _bz_abs<T_numtype>::apply(vector(i));
    }

    return sum;
}

// norm1(vector)
template<typename P_numtype>
BZ_SUMTYPE(P_numtype) norm1(const Vector<P_numtype>& x)
{
    return _bz_vec_norm1(x._bz_asVecExpr());
}

// norm1(expr)
template<typename P_expr>
BZ_SUMTYPE(_bz_typename P_expr::T_numtype) norm1(_bz_VecExpr<P_expr> expr)
{
    return _bz_vec_norm1(expr);
}

// norm1(vecpick)
template<typename P_numtype>
BZ_SUMTYPE(P_numtype) norm1(const VectorPick<P_numtype>& x)
{
    return _bz_vec_norm1(x._bz_asVecExpr());
}

// norm1(TinyVector)
template<typename P_numtype, int N_dimensions>
BZ_SUMTYPE(P_numtype) norm1(const TinyVector<P_numtype, N_dimensions>& x)
{
    return _bz_vec_norm1(x._bz_asVecExpr());
}

BZ_NAMESPACE_END

#endif // BZ_VECNORM1_CC


#include <cassert>
#include <cmath>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <random>
#include <ctime>              // required for "struct timezone" and
                              // "gettimeofday()" used to set the randomseed
using namespace std;

#ifndef DISABLE_OPENMP
#include <omp.h>       // (OpenMP-specific)
#endif

//#include <boost/math/special_functions/gamma.hpp>

#include <mrc_simple.hpp>
#include <visfd.hpp>
using namespace visfd;

#include "err.hpp"
#include "settings.hpp"

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

    Settings settings; // parse the command-line argument list from the shell
    settings.ParseArgs(argc, argv);

    #ifndef DISABLE_OPENMP
    #pragma omp parallel
    {
      int rank, nthr;
      rank = omp_get_thread_num();
      //cerr << "rank=" << rank << endl;
      if (rank == 0) {
        nthr = omp_get_num_threads();
        cerr << "   (Using " << nthr << " threads (cpu cores).  You can change this using the \"-np n\"\n"
             << "    argument, or by setting the OMP_NUM_THREADS environment variable.)" << endl;
      }
    }
    #else
    cerr << " (Serial version)" << endl;
    #endif //#ifndef DISABLE_OPENMP

    // Read the input tomogram
    MrcSimple tomo_in;

    if (settings.in_file_name != "") {
      cerr << "Reading tomogram \""<<settings.in_file_name<<"\"" << endl;
      tomo_in.Read(settings.in_file_name, false);
      // (Note: You can also use "tomo_in.Read(cin);" or "cin >> tomo;")
      tomo_in.PrintStats(cerr);      //Optional (display the tomogram size & format)
      WarnMRCSignedBytes(tomo_in, settings.in_file_name, cerr);
    }
    else {
      assert((settings.image_size[0] > 0) &&
             (settings.image_size[1] > 0) &&
             (settings.image_size[2] > 0));
      tomo_in.Resize(settings.image_size);
    }

    // ---- mask ----

    // Optional: if there is a "mask", read that too
    MrcSimple mask;
    if (settings.mask_file_name != "") {
      cerr << "Reading mask \""<<settings.mask_file_name<<"\"" << endl;
      mask.Read(settings.mask_file_name, false);
      if ((mask.header.nvoxels[0] != tomo_in.header.nvoxels[0]) ||
          (mask.header.nvoxels[1] != tomo_in.header.nvoxels[1]) ||
          (mask.header.nvoxels[2] != tomo_in.header.nvoxels[2]))
        throw InputErr("Error: The size of the mask image does not match the size of the input image.\n");
    }

    // ---- Voxel size? ----

    float voxel_width[3] = {1.0, 1.0, 1.0};
    if (settings.voxel_width > 0.0) {
      // Did the user manually specify the width of each voxel?
      voxel_width[0] = settings.voxel_width;
      voxel_width[1] = settings.voxel_width;
      voxel_width[2] = settings.voxel_width;
    }
    else {
      // Otherwise, infer it from the header of the MRC file
      voxel_width[0] = tomo_in.header.cellA[0]/tomo_in.header.nvoxels[0];
      voxel_width[1] = tomo_in.header.cellA[1]/tomo_in.header.nvoxels[1];
      voxel_width[2] = tomo_in.header.cellA[2]/tomo_in.header.nvoxels[2];
      if (settings.voxel_width_divide_by_10) {
        voxel_width[0] *= 0.1;
        voxel_width[1] *= 0.1;
        voxel_width[2] *= 0.1;
      }
      cerr << "voxel width in physical units = ("
           << voxel_width[0] << ", "
           << voxel_width[1] << ", "
           << voxel_width[2] << ")\n";
    }

    if ((voxel_width[0] <= 0.0) ||
        (voxel_width[1] <= 0.0) ||
        (voxel_width[2] <= 0.0))
      throw InputErr("Error in tomogram header: Invalid voxel width(s).\n"
                     "Use the -w argument to specify the voxel width.");

    if ((voxel_width[0] != voxel_width[1]) ||
        (voxel_width[1] != voxel_width[2]))
      throw InputErr("Error in tomogram header: Unequal voxel widths in the x, y and directions.\n"
                     "Use the -w argument to specify the voxel width.");

    if (settings.in_coords_file_name != "") {
      for (int iz = 0; iz < tomo_in.header.nvoxels[2]; iz++)
        for (int iy = 0; iy < tomo_in.header.nvoxels[1]; iy++)
          for (int ix = 0; ix < tomo_in.header.nvoxels[0]; ix++)
            tomo_in.aaafI[iz][iy][ix] = 0.0;
      fstream coords_file;
      coords_file.open(settings.in_coords_file_name, ios::in);
      if (! coords_file)
        throw InputErr("Error: unable to open \""+
                       settings.in_coords_file_name +"\" for reading.\n");
      while (coords_file) {
        float x, y, z;
        coords_file >> x;
        coords_file >> y;
        coords_file >> z;
        int ix, iy, iz;
        ix = static_cast<int>(x / voxel_width[0]);
        iy = static_cast<int>(y / voxel_width[1]);
        iz = static_cast<int>(z / voxel_width[2]);
        if (((0 <= ix) && (ix < tomo_in.header.nvoxels[0])) &&
            ((0 <= iy) && (iy < tomo_in.header.nvoxels[1])) &&
            ((0 <= iz) && (iz < tomo_in.header.nvoxels[2])))
          tomo_in.aaafI[iz][iy][ix] = 1.0;
      }
    } //if (settings.in_coords_file_name != "") {

    double vol_total = settings.compartment_volume;
    if (settings.compartment_volume < 0) {
      if (mask.aaafI) {
        for (int iz=0; iz<tomo_in.header.nvoxels[2]; iz++)
          for (int iy=0; iy<tomo_in.header.nvoxels[1]; iy++)
            for (int ix=0; ix<tomo_in.header.nvoxels[0]; ix++)
              vol_total += mask.aaafI[iz][iy][ix];
      }
      else {
        vol_total = (tomo_in.header.nvoxels[0] *
                     tomo_in.header.nvoxels[1] *
                     tomo_in.header.nvoxels[2]);
      }
      vol_total *= (voxel_width[0]*voxel_width[1]*voxel_width[2]);
    }

    if (settings.num_particles < 0) {
      // If the user did not explicitly specify the number of particles
      // (ie "ribosomes") present in the original image, we can attempt
      // to infer it from the image itself.  To do this, I assume that
      // the image is filled with voxels containing 0s everywhere except
      // at the center of where each particle is located, where the voxel is 1.
      // Consequently, the number of non-zero voxels (not excluded by the mask)
      // should equal the sum of all of the voxel entries.
      // (This should also equal the number of non-zero voxels.  However
      //  sometimes these images are themselves slightly blurred to make it
      //  easier to see each particle.  This blurring should not effect the sum)
      double sum = 0.0;
      for (int iz=0; iz<tomo_in.header.nvoxels[2]; iz++) {
        for (int iy=0; iy<tomo_in.header.nvoxels[1]; iy++) {
          for (int ix=0; ix<tomo_in.header.nvoxels[0]; ix++) {
            if (mask.aaafI)
              sum += tomo_in.aaafI[iz][iy][ix] * mask.aaafI[iz][iy][ix];
            else
              sum += tomo_in.aaafI[iz][iy][ix];
          }
        }
      }
      settings.num_particles = sum;
    }

    // DEBUG: The next if-then statement is for debugging only.
    if (settings.randomize_input_image) {
      size_t nparticles = floor(settings.num_particles);
      size_t nvoxels;
      if (mask.aaafI) {
        for (int iz=0; iz<tomo_in.header.nvoxels[2]; iz++)
          for (int iy=0; iy<tomo_in.header.nvoxels[1]; iy++)
            for (int ix=0; ix<tomo_in.header.nvoxels[0]; ix++)
              if (mask.aaafI[iz][iy][ix] != 0.0)
                nvoxels += 1;
      }
      else {
        nvoxels = (tomo_in.header.nvoxels[0] *
                   tomo_in.header.nvoxels[1] *
                   tomo_in.header.nvoxels[2]);
      }
      vector<bool> random_bit_list(nvoxels, false);
      for (size_t i=0; i < nparticles; i++)
        random_bit_list[i] = true;

      long random_seed = settings.random_seed;
      if (random_seed <= 0) {
        random_seed = time(nullptr);
        cerr << "(random_seed = " << random_seed << ")" << endl;
      }
      shuffle(random_bit_list.begin(), random_bit_list.end(),
              default_random_engine(random_seed));
      size_t i=0;
      for (int iz = 0; iz < tomo_in.header.nvoxels[2]; iz++) {
        for (int iy = 0; iy < tomo_in.header.nvoxels[1]; iy++) {
          for (int ix = 0; ix < tomo_in.header.nvoxels[0]; ix++) {
            tomo_in.aaafI[iz][iy][ix] = 0.0;
            if ((! mask.aaafI) || mask.aaafI[iz][iy][ix] != 0.0) {
              tomo_in.aaafI[iz][iy][ix] = random_bit_list[i];
              i++;
            }
          }
        }
      }
    } //if (settings.randomize_input_image)

    // First we must allocate a new array to store the filtered image.
    MrcSimple tomo_out = tomo_in;

    for (int i_sig = 0; i_sig < settings.vfSigma.size(); ++i_sig) {

      // ---- main calculation: -----

      //A Gaussian filter is a (weighted) average over a volume of nearby voxels
      //Below we estimate the effective volume of those nearby voxels, by taking
      //advantage of the fact that Gaussian is normalized (it's integral = 1).
      //Normalization effectively reduces the height of the filter by the number
      //of voxels which effectively belong to the filter.
      // Example: a cube filter
      // If we were averaging the brightness of the 27 voxels
      // belonging to a 3*3*3 cube surrounding the voxel of interest
      // then the effective volume of the filter is is 3*3*3=27, and
      // weight of each voxel's contribution to the average is 1/27.  Similarly,
      // for a more general filter, we can interpret the peak height of the
      // filter as 1/volume. Below, we calculate the height of the 3D Gaussian's
      // peak after normalization.  Normally the 3D Gaussian is:
      //   (2*pi*sigma^2)^(-3/2) * exp(-0.5*(x^2+y^2+z^2)/sigma^2)
      // and the peak height is (2*pi*sigma^2)^(-3/2),
      // However this formula does not work for small sigma << 1.
      // For small sigma, you have to normalize by computing a discrete sum
      // (over a finite range).  We do this below (in a non-intuitive way).

      float sigma = settings.vfSigma[i_sig];

      sigma /= voxel_width[0]; //scale by voxel_width (if specified)

      // How wide is the filter window (over what size window do we integrate)?
      if (settings.filter_truncate_ratio <= 0) {
        assert(settings.filter_truncate_threshold > 0.0);
        settings.filter_truncate_ratio = sqrt(-2*log(settings.filter_truncate_threshold));
      }
      int filter_truncate_halfwidth = floor(sigma *
                                            settings.filter_truncate_ratio);

      float gauss_peak_height_3D;

      {
        // GenFilterGauss1D generates a 1D discrete normalized Gaussian (located
        // in the afH[]) member. afH[0] is the height at the central peak.
        // However, for now I just use them to estimate the height of the
        // Gaussian peak.  (See below)
        Filter1D<float, int> filter1D =
          GenFilterGauss1D(sigma, filter_truncate_halfwidth);

        // A 3D Gaussian blur can be performed by expoiting the fact that
        // multidimensional Gaussians are "seperable" filters:  You can blur
        // in the X direction, then in the Y direcion, then in the Z direction.
        // The result is the same that you would get from a 3D filter Gaussian
        // which is the product of Gaussians in the X,Y,Z directions.
        // So the peak height is the product of the 3 1D Gaussian peak heights.
        gauss_peak_height_3D =
          (filter1D.afH[0] *
           filter1D.afH[0] *
           filter1D.afH[0]);
      }

      // The Gaussian peak's height is 1/volume.

      float volume_gaussian_bin = 1.0 / gauss_peak_height_3D;
      volume_gaussian_bin *= (voxel_width[0]*voxel_width[1]*voxel_width[2]);

      float num_bins = vol_total / volume_gaussian_bin;

      // Now blur the original image (if the user did not do it already)
      if ((! settings.precomputed_gaussian_blur) ||
          (settings.vfSigma.size() > 1)) {

        ApplyGauss(tomo_in.header.nvoxels,
                   tomo_in.aaafI,
                   tomo_out.aaafI, //<-store resulting image here
                   mask.aaafI,
                   sigma,
                   filter_truncate_halfwidth,
                   true,
                   &cerr);

        // Densities everywhere are assumed to be in physical units
        // (ie. of 1/Angstroms^3),  NOT   1/voxels^3
        // The Gaussian blur computes densities in 1/voxels^3
        // To compensate for this, divide the densities by voxel_width^3
        MultiplyScalarArr(static_cast<float>(
                          1.0/(voxel_width[0]*voxel_width[1]*voxel_width[2])),
                          tomo_out.header.nvoxels,
                          tomo_out.aaafI);
      }

      // After bluring the image, find the lowest density:

      float extreme_density;
      int afXextreme[3] = {-1, -1, -1};
      float global_minima;
      float global_maxima;

      // Find the voxels with the minima and maxima intensities.
      // Discard global minima or maxima which lie on the boundary
      // The safe, careful way to do this is to only consider
      // voxels which are surrounded by other voxels and are
      // local minima or maxima.
      for (int iz=0; iz < tomo_out.header.nvoxels[2]; iz++) {
        for (int iy=0; iy < tomo_out.header.nvoxels[1]; iy++) {
          for (int ix=0; ix < tomo_out.header.nvoxels[0]; ix++) {

            bool is_local_minima = true;
            bool is_local_maxima = true;

            if (! settings.extrema_on_boundary) {
              if ((ix == 0) || (ix ==  tomo_out.header.nvoxels[0]-1) ||
                  (ix == 0) || (ix ==  tomo_out.header.nvoxels[0]-1) ||
                  (ix == 0) || (ix ==  tomo_out.header.nvoxels[0]-1)) {
                is_local_minima = false;
                is_local_maxima = false;
              }
            }

            float center_val = tomo_out.aaafI[iz][iy][ix];
            for (int jz=-1; jz<=1; jz++) {
              for (int jy=-1; jy<=1; jy++) {
                for (int jx=-1; jx<=1; jx++) {
                  if (! settings.extrema_on_boundary) {
                    if (mask.aaafI && (mask.aaafI[iz+jz][iy+jy][ix+jx] == 0.0))
                    {
                      is_local_minima = false;
                      is_local_maxima = false;
                    }
                  }
                  if (tomo_out.aaafI[iz+jz][iy+jy][ix+jx] <= center_val)
                    is_local_minima = false;
                  if (tomo_out.aaafI[iz+jz][iy+jy][ix+jx] >= center_val)
                    is_local_maxima = false;
                }
              }
            }
            if ((center_val < global_minima) || (afXextreme[0] == -1)) {
              global_minima = center_val;
              if (settings.use_min_density) {
                extreme_density = global_minima;
                afXextreme[0] = ix;
                afXextreme[1] = iy;
                afXextreme[2] = iz;
              }
            }
            if ((center_val > global_maxima) || (afXextreme[0] == -1)) {
              global_maxima = center_val;
              if (! settings.use_min_density) {
                extreme_density = global_maxima;
                afXextreme[0] = ix;
                afXextreme[1] = iy;
                afXextreme[2] = iz;
              }
            }
          } //for (int ix=1; ix < tomo_out.header.nvoxels[0]-1; ix++)
        } //for (int iy=1; iy < tomo_out.header.nvoxels[1]-1; iy++)
      } //for (int iz=1; iz < tomo_out.header.nvoxels[2]-1; iz++)

      // Did we fail to find any local minima or maxima densities?
      if (afXextreme[0] == -1) {
        string extrema_type = "minina";
        if (! settings.use_min_density)
          extrema_type = "maxima";
        stringstream msg_ss;
        msg_ss << "Error: There are no local density " << extrema_type << "\n"
               << "       in the image (at scale sigma = " << sigma << ")\n"
               << "       Aborting...\n";
        throw InputErr(msg_ss.str());
      }

      float ave_density = settings.num_particles / vol_total;

      // Suppose you divide the volume of the region (ie. cell, compartment...)
      // into equal size "bins" of equal size ("volume_gaussian_bin").
      // A fixed number of particles lie within this volume.
      // ...What is the probability that the number of particles in a bin
      //    does not exceed "extreme_density"?
      //  This is given by the (cumulative) Poisson distribution:
      //     prob = lambda^k * exp(-lambda) / k!
      //          = lambda^k * exp(-lambda) / Gamma(k+1)  <-continuous version
      // "k" is the number of particles in this Gaussian-shaped "bin"
      long double k = extreme_density * volume_gaussian_bin;
      // "lambda" is the expected number of particles in this "bin"
      long double lambda = ave_density * volume_gaussian_bin;

      long double prob_cdf_obvious_way = 0.0;
      if (settings.use_min_density) {
        // Calculate the culmulative probability of seeing this many
        // particles in the bin or less.  First try it the obvious way:

        for (long i=0; i <= floor(k); i++)
          prob_cdf_obvious_way += pow(lambda, i) * exp(-lambda) / tgamma(i+1.0);
      }
      else {
      // Calculate the culmulative probability of seeing this many
      // particles in the bin or more.  First try it the obvious way:
        for (long i=0; i < floor(k); i++)
          prob_cdf_obvious_way += pow(lambda, i) * exp(-lambda) / tgamma(i+1.0);
        prob_cdf_obvious_way = 1.0 - prob_cdf_obvious_way;
      }

      cerr << "####################################" << endl;
      cerr << "## DEBUG MESSAGES (PLEASE IGNORE) ##" << endl;
      cerr << "## location (in voxels) = ("
           << afXextreme[0] << ","
           << afXextreme[1] << ","
           << afXextreme[2] << ")" << endl;
      cerr << "## num_in_bin = " << k << endl;
      cerr << "## num_expected_in_bin = " << lambda << endl;
      cerr << "## prob_cdf_obvious_way = " << prob_cdf_obvious_way << endl;
      cerr << "####################################" << endl;

      long double prob_cdf;

      // However, in our case the number of particles in this "bin" is not (k)
      // necessarily an integer.  In that case use the continuous version of
      // (the cumulative distribution) of the Poisson distributio.
      // COMMENTING OUT: This requires the BOOST libraries
      // COMMENTING OUT: if (use_min_density)
      // COMMENTING OUT:   prob_cdf = gamma_q(k+1,lambda);
      // COMMENTING OUT: else
      // COMMENTING OUT:   prob_cdf = 1.0 - gamma_q(k,lambda);
      //    (gamma_q() is defined in boost)

      // The cumulative distribution of the Poisson distribution
      // is also given by the upper incomplete Gamma function
      // COMMENTING OUT: USE THE BOOST LIBRARY
      //double prob_cdf = gamma_q(floor(k)+1, lambda);
      // Unfortunately the  BOOST library can not be easily distributed with
      // this code.  (Even if I throw away most ofthe BOOST code using the
      // "bcp" utility, the remaining BOOST code exceeds the size of this
      // the code for this project by a factor of 50.)
      // Hence we will use the integer approximation ("obvious_way"):

      prob_cdf = prob_cdf_obvious_way;

      // ...Now what is the probability that the number of particles
      //    in ANY OF THE BINS does not exceed "extreme_density"?

      long double prob_total = 1.0 - pow((1.0 - prob_cdf), num_bins);

      double effective_bin_size = (pow(volume_gaussian_bin, 1.0/3) *
                                   voxel_width[0]);
      cout << prob_total
           << " " << extreme_density
           << " " << afXextreme[0]
           << " " << afXextreme[1]
           << " " << afXextreme[2]
           << " " << effective_bin_size
           << endl;

      // Discussion:
      //      What do we expect this number to be?
      // What is the expected value of "prob_total" for a randomly distributed
      // particles in the same number of bins.
      //
      // Answer: 0.5
      //
      // The proof is not specific to the kind of distribution we are using.
      // (a Poisson distribution)
      //
      // Proof:
      //
      // Let "c" be the cumulative probability distrubution for the entire
      // system (in this "prob_total", but it could also be "prob_cdf")
      //
      // Let p(x) be the probability density of measuring x.
      //
      // Let C(x) be the cumulative distribution of measuring something <= x.
      // (in this case the probability that none of the bins have
      //  a density exceeding x)  This means that:
      //
      // C(X) = \int_{-\infty}^X  dx  p(x)
      //
      // Let C^{-1}(c)  denote the inverse of C(x)
      // ==> C^{-1}(c) = x   if   C(x) = c
      //
      // What is the average value of c?
      //
      // <c> = \int_{-infty}^{\infty} * c(x) * p(x) * dx
      //
      //     = \int_0^1         c *  p(x(c)) * dx(c)/dc * dc
      //
      //     = \int_0^1 c * p( C^{-1}(c) ) * (d/dc) C^{-1}(c)  * dc
      //
      //     = \int_0^1 c * p( C^{-1}(c) ) * ( 1 / (d/dx) C( C^{-1}(c) ) ) * dc
      //
      //     = \int_0^1 c * p( C^{-1}(c) ) * ( 1 / p( C^{-1}(c) ) )  * dc
      //
      //     = \int_0^1 c * dc
      //
      //     = 0.5
      //
      // However this number will be lower if you repeat this multiple times
      // (using different bin-sizes ("sigma" values), for example)
      // because by repeating, you give it more opportunities to find
      // a more extreme value.  (Here we scan over a range of "sigma" values.)

    } // for (int i_sig = 0; i_sig < settings.vfSigma.size(); i_sig++) {...

    if ((settings.out_file_name != "") && (settings.vfSigma.size() == 1))
    {
      // Write the image file containing the filtered version for that sigma
      cerr << "writing a tomogram containing the most recently calculated density cloud"
           << endl;

      tomo_out.Write(settings.out_file_name);
      // (You can also use "file_stream << tomo_out;")
    }

  } //try {
  catch (const std::exception& e) {
    cerr << "\n" << e.what() << endl;
    exit(1);
  }

} // main()


/*!
@file
Forward declares `boost::hana::difference`.

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

#ifndef BOOST_HANA_FWD_DIFFERENCE_HPP
#define BOOST_HANA_FWD_DIFFERENCE_HPP

#include <boost/hana/config.hpp>
#include <boost/hana/core/when.hpp>

BOOST_HANA_NAMESPACE_BEGIN
    //! Returns the set-theoretic difference of two sets.
    //! @relates hana::set
    //!
    //! Given two sets `xs` and `ys`, `difference(xs, ys)` is a new set
    //! containing all the elements of `xs` that are _not_ contained in `ys`.
    //! For any object `x`, the following holds:
    //! @code
    //!     x ^in^ difference(xs, ys) if and only if x ^in^ xs && !(x ^in^ ys)
    //! @endcode
    //!
    //!
    //! @note
    //! This operation is not commutative, i.e. `difference(xs, ys)` is not
    //! necessarily the same as `difference(ys, xs)`. Indeed, consider the
    //! case where `xs` is empty and `ys` isn't. Then, `difference(xs, ys)`
    //! is empty but `difference(ys, xs)` is equal to `ys`. For the symmetric
    //! version of this operation, see `symmetric_difference`.
    //!
    //!
    //! @param xs
    //! A set to remove values from.
    //!
    //! @param ys
    //! The set whose values are removed from `xs`.
    //!
    //!
    //! Example
    //! -------
    //! @include example/difference.cpp
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto difference = [](auto&& xs, auto&& ys) {
        return tag-dispatched;
    };
#else
    template <typename S, typename = void>
    struct difference_impl : difference_impl<S, when<true>> { };

    struct difference_t {
        template <typename Xs, typename Ys>
        constexpr auto operator()(Xs&& xs, Ys&& ys) const;
    };

    constexpr difference_t difference{};
#endif
BOOST_HANA_NAMESPACE_END

#endif // !BOOST_HANA_FWD_DIFFERENCE_HPP


/*Copyright (c) 2020 James Gayvert

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include "Shell.h"
#include <iostream>
#include <vector>
#include <map>
#include <Eigen/Dense>

static map<string, int> angmom_map = {{"S", 0}, {"P", 1}, {"D", 2},{"F",3},{"G",4}};
double fact2(int n)
{
    int res = 1;
    for (int i=n; i>=0; i=i-2)
    {
        if (i==0 || i==1)
            return res;
        else
            res *= i;
    }
    return static_cast<double>(res);
}

int parity(int i)
{
  return i%2 ? -1 : 1;
}

double fact(int n)
{
    int res = 1;
    for(int i = 1; i <= n; ++i)
        res *= i;
    return static_cast<double>(res);
}

double binom(int p, int q)
{
	if(q<0 || q>p)
		return 0;
	return fact(p)/fact(q)/fact(p-q);
}

void fill_mat(std::vector<double> &matrix_elements, Eigen::MatrixXd &opdm)
{
	size_t vec_idx = 0;
	for (size_t row_idx=0;row_idx<opdm.rows();row_idx++)
	{
		for (size_t col_idx=0;col_idx<opdm.cols();col_idx++)
		{
			opdm(row_idx,col_idx) = matrix_elements[vec_idx];
			vec_idx++;
		}
	}
}

void fill_LT(std::vector<double> matrix_elements, Eigen::MatrixXd &opdm)
{
	size_t vec_idx = 0;
	size_t row_idx = 0;
	while(row_idx<opdm.rows() && vec_idx<matrix_elements.size())
	{
		//elements are added to each column <= row index
		for (size_t col_idx=0;col_idx<=row_idx;col_idx++)
		{
			opdm(row_idx,col_idx) = matrix_elements[vec_idx];
			opdm(col_idx,row_idx) = matrix_elements[vec_idx];
			vec_idx++;
		}
		row_idx++;
	}
}

std::vector<std::string> split(const std::string& s, char delimiter)
{
   std::vector<std::string> tokens;
   std::string token;
   std::istringstream tokenStream(s);
   while (std::getline(tokenStream, token, delimiter))
   {
	  if (!token.empty())
		  tokens.push_back(token);
   }
   return tokens;
}

void fortran_dfloats_to_efloats(std::string& str)
{
	for(auto& ch: str)
	{
		if (ch == 'd') ch = 'e';
		if (ch == 'D') ch = 'E';
	}
}

int shell2angmom(std::string shell_label)
{
	return angmom_map[shell_label];
}

bool compare_strings(std::string s1, std::string s2)
{
	std::transform(s1.begin(), s1.end(), s1.begin(), ::tolower);
	std::transform(s2.begin(), s2.end(), s2.begin(), ::tolower);
	return s1==s2;
}

bool is_number(const std::string &s)
{
  return !s.empty() && std::all_of(s.begin(), s.end(), ::isdigit);
}

bool is_letter(const std::string &s)
{
  return !s.empty() && std::all_of(s.begin(), s.end(), ::isalpha);
}


#include <algorithm>

#include <iostream>
#include <vector>

#include <boost/sort/common/time_measure.hpp>

uint64_t llcnt_partitionings=0;
uint64_t llcnt_partitions=0;

extern "C" {
  #include "introsort.h"
}

size_t NELEM = 32;

namespace bsc = boost::sort::common;

using bsc::time_point;
using bsc::now;
using bsc::subtract_time;

template <typename A> void print_range(A begin, A end) {

  uint64_t last=0;
  bool dots=false;
  for (auto i=begin;i<end;++i) {
      bool print = i==begin || last+1 != *i;

      if (!print && !dots) {std::cout<<"..."; dots=true; }

      if (print && dots) {std::cout<<last; dots=false; }

      if (print && i!=begin) std::cout<<" ";
      if (print) {std::cout<<*i; dots=false; }

      last=*i;
  }

  if (dots) {std::cout<<last; dots=false; }

  std::cout<<std::endl;
}

void prn(int x) {
  if (x<10) std::cout<<" "; std::cout<<x;
}

template <typename A> void print_range2(A l0, A begin, A end) {

  size_t n = end-begin;
  size_t base = begin-l0;

  for (size_t i=0;i<n;++i) {prn(base+i); std::cout<<" ";}
  std::cout<<std::endl;
  for (size_t i=0;i<n;++i) {prn(*(begin+i)); std::cout<<" ";}
  std::cout<<std::endl;
}

/*
template <typename A> void print_range(A begin, A end) {
  for (auto i=begin;i<end;++i) std::cout<<*i<<" ";
  std::cout<<std::endl;
}
*/

extern "C" {
  void incr_partitionings() {++llcnt_partitionings;}
  void incr_partitions() {++llcnt_partitions;}

  void print_partition(uint64_t *a, int64_t l, int64_t h) {
    print_range(a+l,a+h);
  }

}

template<typename RandomAccessIterator>
  inline void
  std_heapsort(RandomAccessIterator first, RandomAccessIterator last)
  {
    std::make_heap(first,last);
    std::sort_heap(first,last);
  }

namespace annot {

  uint64_t cnt_dlimit=0;
  uint64_t cnt_intro_loops=0;
  uint64_t cnt_partitionings=0;
  uint64_t cnt_partitions=0;

  void print_stats() {
    std::cout<<"**** STL ***"<<std::endl;
    std::cout<<"hit depth limit: "<<cnt_dlimit<<std::endl;
    std::cout<<"intro loops: "<<cnt_intro_loops<<std::endl;
    std::cout<<"partitionings: "<<cnt_partitionings<<std::endl;
    std::cout<<"partitions: "<<cnt_partitions<<std::endl;
    std::cout<<std::endl;
    std::cout<<"**** Isabelle-LLVM ***"<<std::endl;
    std::cout<<"partitionings: "<<llcnt_partitionings<<std::endl;
    std::cout<<"partitions: "<<llcnt_partitions<<std::endl;

  }

  /// This is a helper function for the sort routine.
  template<typename _RandomAccessIterator, typename _Size, typename _Compare>
    void
    __introsort_loop(_RandomAccessIterator __first,
		     _RandomAccessIterator __last,
		     _Size __depth_limit, _Compare __comp)
    {
      while (__last - __first > int(std::_S_threshold))
	{
          ++cnt_intro_loops;
	  if (__depth_limit == 0)
	    {
              ++cnt_dlimit;
	      std::__partial_sort(__first, __last, __last, __comp);
	      return;
	    }
	  --__depth_limit;
          print_range(__first,__last);
	  _RandomAccessIterator __cut =
	    std::__unguarded_partition_pivot(__first, __last, __comp);
          print_range(__first,__last);
          ++cnt_partitionings;
	  annot::__introsort_loop(__cut, __last, __depth_limit, __comp);
	  __last = __cut;
	}

      ++cnt_partitions;
    }

  // sort

  template<typename _RandomAccessIterator, typename _Compare>
    inline void
    __sort(_RandomAccessIterator __first, _RandomAccessIterator __last,
	   _Compare __comp)
    {
      if (__first != __last)
	{
	  annot::__introsort_loop(__first, __last,
				std::__lg(__last - __first) * 2,
				__comp);
	  std::__final_insertion_sort(__first, __last, __comp);
	}
    }

  template<typename _RandomAccessIterator>
    inline void
    sort(_RandomAccessIterator __first, _RandomAccessIterator __last)
    {
      // concept requirements
      __glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
	    _RandomAccessIterator>)
      __glibcxx_function_requires(_LessThanComparableConcept<
	    typename iterator_traits<_RandomAccessIterator>::value_type>)
      __glibcxx_requires_valid_range(__first, __last);
      __glibcxx_requires_irreflexive(__first, __last);

      annot::__sort(__first, __last, __gnu_cxx::__ops::__iter_less_iter());
    }

}

namespace test2 {

  std::vector<uint64_t>::iterator l0;

  /// This is a helper function...
  template<typename _RandomAccessIterator, typename _Compare>
    _RandomAccessIterator
    __unguarded_partition(_RandomAccessIterator __first,
			  _RandomAccessIterator __last,
			  _RandomAccessIterator __pivot, _Compare __comp)
    {
      while (true)
	{
	  while (__first!=__last && __comp(__first, __pivot))
	    ++__first;
	  --__last;
	  while (__first!=__last && __comp(__pivot, __last))
	    --__last;
	  if (!(__first < __last))
	    return __first;
	  std::iter_swap(__first, __last);
	  ++__first;
	}
    }

  /// This is a helper function...
  template<typename _RandomAccessIterator, typename _Compare>
    inline _RandomAccessIterator
    __unguarded_partition_pivot(_RandomAccessIterator __first,
				_RandomAccessIterator __last, _Compare __comp)
    {
      _RandomAccessIterator __mid = __first + (__last - __first) / 2;

      std::cout<<"mid = "<<__mid-l0<<std::endl;

      std::__move_median_to_first(__first, __first + 1, __mid, __last - 1,
				  __comp);
      return test2::__unguarded_partition(__first + 1, __last, __first, __comp);
    }

  /// This is a helper function for the sort routine.
  template<typename _RandomAccessIterator, typename _Size, typename _Compare>
    void
    __introsort_loop(_RandomAccessIterator __first,
		     _RandomAccessIterator __last,
		     _Size __depth_limit, _Compare __comp)
    {
      while (__last - __first > int(std::_S_threshold))
	{
          std::cout<<"Sorting ["<<(__first-l0)<<"..."<<(__last-l0)<<"]"<<std::endl;

	  if (__depth_limit == 0)
	    {
              std::cout<<"HIT DEPTH LIMIT"<<std::endl;
	      std::__partial_sort(__first, __last, __last, __comp);
	      return;
	    }
	  --__depth_limit;
	  _RandomAccessIterator __cut =
	    test2::__unguarded_partition_pivot(__first, __last, __comp);

          std::cout<<"cut at "<<(__cut-l0)<<std::endl;
          print_range2(l0,__first,__last);

	  test2::__introsort_loop(__cut, __last, __depth_limit, __comp);
	  __last = __cut;
	}
    }

  template<typename _RandomAccessIterator, typename _Compare>
    inline void
    __sort(_RandomAccessIterator __first, _RandomAccessIterator __last,
	   _Compare __comp)
    {
      if (__first != __last)
	{
	  test2::__introsort_loop(__first, __last,
				std::__lg(__last - __first) * 2,
				__comp);

          std::cout<<"partially sorted array"<<std::endl;
          print_range2(l0,__first,__last);

	  std::__final_insertion_sort(__first, __last, __comp);

          std::cout<<"RESULT"<<std::endl;
          print_range2(l0,__first,__last);

	}
    }

  template<typename _RandomAccessIterator>
    inline void
    sort(_RandomAccessIterator __first, _RandomAccessIterator __last)
    {
      // concept requirements
      __glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
	    _RandomAccessIterator>)
      __glibcxx_function_requires(_LessThanComparableConcept<
	    typename iterator_traits<_RandomAccessIterator>::value_type>)
      __glibcxx_requires_valid_range(__first, __last);
      __glibcxx_requires_irreflexive(__first, __last);

      test2::__sort(__first, __last, __gnu_cxx::__ops::__iter_less_iter());
    }

}

template<typename A> void time_op(std::string name, A op) {
  auto start = now ();
  op();
  auto finish = now ();
  auto duration = subtract_time (finish, start);

  std::cout<<name<<": "<<duration<<std::endl;
}

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

  std::cout<<sizeof(llstring)<<std::endl;
  std::cout<<sizeof(std::vector<char>)<<std::endl;
  std::cout<<sizeof(std::string)<<std::endl;

  return 1;

  std::vector<uint64_t> A,B;

  A.reserve (NELEM);
  A.clear ();
  for (size_t i = 0; i < NELEM; ++i)
      A.push_back (i);

  A[0]=9;
  A[15]=10;

  B=A;

  test2::l0=B.begin();

  time_op("test2::sort", [&B]{test2::sort(B.begin(),B.end());});

//   B=A;
//   time_op("std::sort", [&B]{std::sort(B.begin(),B.end());});

//   B=A;
//   time_op("std::heapsort", [&B]{std_heapsort(B.begin(),B.end());});
//
//   B=A;
//   time_op("isabelle-llvm::heapsort", [&B]{heapsort(B.data(),0,NELEM);});

//   annot::print_stats();

/*
  B=A;
  time_op("std::heapsort", [&B]{std::make_heap(B.begin(),B.end()); std::sort_heap(B.begin(),B.end()); });*/

//   xxx, ctd here: try to confirm influence of insertion-sort (measure time with omitted final-sort)
//   try to tune threshold factor?

}


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

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/exceptions.hpp>

NT2_TEST_CASE_TPL ( ror_integer, BOOST_SIMD_INTEGRAL_TYPES)
{
  using boost::simd::ror;
  using boost::simd::tag::ror_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<ror_(T,T)>::type
                  , T
                  );

  T w = sizeof(T)*CHAR_BIT;

  for(T i=0;i<w;++i)
  {
    NT2_TEST_EQUAL(ror(T(1),i), T(T(1)<<((w-i) & (w-1))));
  }

  NT2_TEST_ASSERT(ror(T(1),T(-1)));
  NT2_TEST_ASSERT(ror(T(1),T(w+1)));
}

NT2_TEST_CASE_TPL ( ror_real, BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::ror;
  using boost::simd::tag::ror_;
  using boost::simd::bitwise_cast;

  typedef typename boost::dispatch::meta::as_integer<T>::type iT;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<ror_(T,iT)>::type
                  , T
                  );

  iT w = sizeof(T)*CHAR_BIT;

  for(iT i=0;i<w;++i)
    NT2_TEST_EQUAL( ror(bitwise_cast<T>(iT(1)),i)
                  , bitwise_cast<T>(iT(1)<<((w-i) & (w-1)))
                  );

  NT2_TEST_ASSERT(ror(T(1),iT(-1)));
  NT2_TEST_ASSERT(ror(T(1),iT(w+1)));
}


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

#ifndef BOOST_MATH_SPECIAL_LEGENDRE_STIELTJES_HPP
#define BOOST_MATH_SPECIAL_LEGENDRE_STIELTJES_HPP

/*
 * Constructs the Legendre-Stieltjes polynomial of degree m.
 * The Legendre-Stieltjes polynomials are used to create extensions for Gaussian quadratures,
 * commonly called "Gauss-Konrod" quadratures.
 *
 * References:
 * Patterson, TNL. "The optimum addition of points to quadrature formulae." Mathematics of Computation 22.104 (1968): 847-856.
 */

#include <iostream>
#include <vector>
#include <boost/math/tools/roots.hpp>
#include <boost/math/special_functions/legendre.hpp>

namespace boost{
namespace math{

template<class Real>
class legendre_stieltjes
{
public:
    legendre_stieltjes(size_t m)
    {
        if (m == 0)
        {
           throw std::domain_error("The Legendre-Stieltjes polynomial is defined for order m > 0.\n");
        }
        m_m = static_cast<int>(m);
        std::ptrdiff_t n = m - 1;
        std::ptrdiff_t q;
        std::ptrdiff_t r;
        bool odd = n & 1;
        if (odd)
        {
           q = 1;
           r = (n-1)/2 + 2;
        }
        else
        {
           q = 0;
           r = n/2 + 1;
        }
        m_a.resize(r + 1);
        // We'll keep the ones-based indexing at the cost of storing a superfluous element
        // so that we can follow Patterson's notation exactly.
        m_a[r] = static_cast<Real>(1);
        // Make sure using the zero index is a bug:
        m_a[0] = std::numeric_limits<Real>::quiet_NaN();

        for (std::ptrdiff_t k = 1; k < r; ++k)
        {
            Real ratio = 1;
            m_a[r - k] = 0;
            for (std::ptrdiff_t i = r + 1 - k; i <= r; ++i)
            {
                // See Patterson, equation 12
                std::ptrdiff_t num = (n - q + 2*(i + k - 1))*(n + q + 2*(k - i + 1))*(n-1-q+2*(i-k))*(2*(k+i-1) -1 -q -n);
                std::ptrdiff_t den = (n - q + 2*(i - k))*(2*(k + i - 1) - q - n)*(n + 1 + q + 2*(k - i))*(n - 1 - q + 2*(i + k));
                ratio *= static_cast<Real>(num)/static_cast<Real>(den);
                m_a[r - k] -= ratio*m_a[i];
            }
        }
    }

    Real norm_sq() const
    {
        Real t = 0;
        bool odd = m_m & 1;
        for (size_t i = 1; i < m_a.size(); ++i)
        {
            if(odd)
            {
                t += 2*m_a[i]*m_a[i]/static_cast<Real>(4*i-1);
            }
            else
            {
                t += 2*m_a[i]*m_a[i]/static_cast<Real>(4*i-3);
            }
        }
        return t;
    }

    Real operator()(Real x) const
    {
        // Trivial implementation:
        // Em += m_a[i]*legendre_p(2*i - 1, x);  m odd
        // Em += m_a[i]*legendre_p(2*i - 2, x);  m even
        size_t r = m_a.size() - 1;
        Real p0 = 1;
        Real p1 = x;

        Real Em;
        bool odd = m_m & 1;
        if (odd)
        {
            Em = m_a[1]*p1;
        }
        else
        {
            Em = m_a[1]*p0;
        }

        unsigned n = 1;
        for (size_t i = 2; i <= r; ++i)
        {
            std::swap(p0, p1);
            p1 = boost::math::legendre_next(n, x, p0, p1);
            ++n;
            if (!odd)
            {
               Em += m_a[i]*p1;
            }
            std::swap(p0, p1);
            p1 = boost::math::legendre_next(n, x, p0, p1);
            ++n;
            if(odd)
            {
                Em += m_a[i]*p1;
            }
        }
        return Em;
    }

    Real prime(Real x) const
    {
        Real Em_prime = 0;

        for (size_t i = 1; i < m_a.size(); ++i)
        {
            if(m_m & 1)
            {
                Em_prime += m_a[i]*detail::legendre_p_prime_imp(static_cast<unsigned>(2*i - 1), x, policies::policy<>());
            }
            else
            {
                Em_prime += m_a[i]*detail::legendre_p_prime_imp(static_cast<unsigned>(2*i - 2), x, policies::policy<>());
            }
        }
        return Em_prime;
    }

    std::vector<Real> zeros() const
    {
        using boost::math::constants::half;

        std::vector<Real> stieltjes_zeros;
        std::vector<Real> legendre_zeros = legendre_p_zeros<Real>(m_m - 1);
        int k;
        if (m_m & 1)
        {
            stieltjes_zeros.resize(legendre_zeros.size() + 1, std::numeric_limits<Real>::quiet_NaN());
            stieltjes_zeros[0] = 0;
            k = 1;
        }
        else
        {
            stieltjes_zeros.resize(legendre_zeros.size(), std::numeric_limits<Real>::quiet_NaN());
            k = 0;
        }

        while (k < (int)stieltjes_zeros.size())
        {
            Real lower_bound;
            Real upper_bound;
            if (m_m & 1)
            {
                lower_bound = legendre_zeros[k - 1];
                if (k == (int)legendre_zeros.size())
                {
                    upper_bound = 1;
                }
                else
                {
                    upper_bound = legendre_zeros[k];
                }
            }
            else
            {
                lower_bound = legendre_zeros[k];
                if (k == (int)legendre_zeros.size() - 1)
                {
                    upper_bound = 1;
                }
                else
                {
                    upper_bound = legendre_zeros[k+1];
                }
            }

            // The root bracketing is not very tight; to keep weird stuff from happening
            // in the Newton's method, let's tighten up the tolerance using a few bisections.
            boost::math::tools::eps_tolerance<Real> tol(6);
            auto g = [&](Real t) { return this->operator()(t); };
            auto p = boost::math::tools::bisect(g, lower_bound, upper_bound, tol);

            Real x_nk_guess = p.first + (p.second - p.first)*half<Real>();
            boost::uintmax_t number_of_iterations = 500;

            auto f = [&] (Real x) { Real Pn = this->operator()(x);
                                    Real Pn_prime = this->prime(x);
                                    return std::pair<Real, Real>(Pn, Pn_prime); };

            const Real x_nk = boost::math::tools::newton_raphson_iterate(f, x_nk_guess,
                                                  p.first, p.second,
                                                  2*std::numeric_limits<Real>::digits10,
                                                  number_of_iterations);

            BOOST_ASSERT(p.first < x_nk);
            BOOST_ASSERT(x_nk < p.second);
            stieltjes_zeros[k] = x_nk;
            ++k;
        }
        return stieltjes_zeros;
    }

private:
    // Coefficients of Legendre expansion
    std::vector<Real> m_a;
    int m_m;
};

}}
#endif


#ifndef HYPSYS1D_RECONSTRUCTION_HPP
#define HYPSYS1D_RECONSTRUCTION_HPP

#include <Eigen/Dense>
#include <cmath>
#include <memory>

#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/limiters.hpp>
#include <ancse/rate_of_change.hpp>
#include <ancse/simulation_time.hpp>

class PWConstantReconstruction {
  public:
    void set(const Eigen::MatrixXd &u) const {
        up.resize(u.rows(),u.cols());
        up = u;
    }

    /// Compute the left and right trace at the interface i + 1/2.
    /** Note: This API is agnostic to the number of cell-averages required
     *        by the method. Therefore, reconstructions with different stencil
     *        sizes can implement this API; and this call can be used in parts
     *        of the code that do not need to know about the details of the
     *        reconstruction.
     */
    std::pair<Eigen::VectorXd, Eigen::VectorXd>
    operator()(int i) const {
        return (*this)(up.col(i), up.col(i+1));
    }

    /// Compute the left and right trace at the interface.
    /** Piecewise constant reconstruction of the left and right trace only
     *  requires the cell-average to the left and right of the interface.
     *
     *  Note: Compared to the other overload this reduces the assumption on
     *        how the cell-averages are stored. This is useful when testing and
     *        generally makes the function useful in more situations.
     */
    inline
    std::pair<Eigen::VectorXd, Eigen::VectorXd>
    operator()(Eigen::VectorXd ua, Eigen::VectorXd ub) const {
        return {std::move(ua), std::move(ub)};
    }

private:
  mutable Eigen::MatrixXd up;
};

#endif // HYPSYS1D_RATE_OF_CHANGE_HPP


#define BOOST_TEST_MODULE VectorPointer
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/vector_pointer.hpp>
#include <vexcl/element_index.hpp>
#include <vexcl/tagged_terminal.hpp>
#include <vexcl/temporary.hpp>
#include <vexcl/constants.hpp>
#include <vexcl/function.hpp>
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(nbody)
{
    const size_t n = 1024;

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    std::vector<double> X = random_vector<double>(n);

    vex::vector<double> x(queue, X);
    vex::vector<double> y(queue, n);

    VEX_FUNCTION(double, nbody, (size_t, n)(size_t, j)(double*, x),
            double sum = 0;
            for(size_t i = 0; i < n; ++i)
                if (i != j) sum += x[i];
                    return sum;
            );

    y = nbody(n, vex::element_index(), vex::raw_pointer(x));

    check_sample(y, [&](size_t idx, double v) {
            double sum = 0;
            for(size_t i = 0; i < n; ++i)
                if (i != idx) sum += X[i];
            BOOST_CHECK_CLOSE(v, sum, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(manual_stencil)
{
    const size_t n = 1024;

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    std::vector<double> X = random_vector<double>(n);

    vex::vector<double> x(queue, X);
    vex::vector<double> y(queue, n);

    VEX_CONSTANT(nil, 0);
    VEX_CONSTANT(one, 1);
    VEX_CONSTANT(two, 2);

    auto N = vex::tag<1>( x.size() );
    auto p = vex::raw_pointer(x);

    auto i     = vex::make_temp<1>( vex::element_index() );
    auto left  = vex::make_temp<2>( if_else(i > nil(), i - one(), i ) );
    auto right = vex::make_temp<3>( if_else(i + one() < N, i + one(), i ) );

    // Use pointer arithmetics
    y = *(p + i) * two() - *(p + left) - *(p + right);

    check_sample(y, [&](size_t idx, double v) {
            double xc = X[idx];
            double xl = X[idx > 0 ? idx - 1 : idx];
            double xr = X[idx + 1 < n ? idx + 1 : idx];
            BOOST_CHECK_CLOSE(v, 2 * xc - xr - xl, 1e-8);
            });

    // Same thing with index operators
    y = p[i] * two() - p[left] - p[right];

    check_sample(y, [&](size_t idx, double v) {
            double xc = X[idx];
            double xl = X[idx > 0 ? idx - 1 : idx];
            double xr = X[idx + 1 < n ? idx + 1 : idx];
            BOOST_CHECK_CLOSE(v, 2 * xc - xr - xl, 1e-8);
            });
}

#ifndef VEXCL_BACKEND_CUDA
BOOST_AUTO_TEST_CASE(constant_pointer)
{
    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    const size_t n = 1024;

    int host_data[] = {1, 2, 3, 4};
    vex::vector<int> x(queue, 4, host_data, vex::backend::MEM_READ_ONLY);
    vex::vector<int> y(queue, n);

    auto X = vex::constant_pointer(x);
    y = X[vex::element_index() % 4];

    check_sample(y, [&](size_t idx, int v) {
            BOOST_CHECK_EQUAL(v, host_data[idx % 4]);
            });
}

BOOST_AUTO_TEST_CASE(pass_constant_pointer_to_function)
{
    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    const size_t N = 1024;

    int host_data[] = {1, 2, 3, 4};

    vex::vector<int> x(queue, 4, host_data, vex::backend::MEM_READ_ONLY);
    vex::vector<int> y(queue, N);

    VEX_FUNCTION(int, foo, (int, idx)(vex::constant_ptr<int>, x),
            return x[idx % 4];
            );

    y = foo(vex::element_index(), vex::constant_pointer(x));

    check_sample(y, [&](size_t idx, int v) {
            BOOST_CHECK_EQUAL(v, host_data[idx % 4]);
            });
}
#endif

BOOST_AUTO_TEST_SUITE_END()


#include "CameraConfig.h"
#include <boost/concept_check.hpp>

using namespace A2O;

CameraConfig::CameraConfig(const std::string& name,
			   const Eigen::Vector3d& position,
			   const Eigen::AngleAxisd& orientation,
			   const unsigned int& widht,
			   const unsigned int& height,
			   const float& focalLengthX,
			   const float& focalLengthY)
      : CameraConfig(name, name, position, orientation, widht, height, focalLengthX, focalLengthY)
{
}

CameraConfig::CameraConfig(const std::string& name,
			   const std::string& perceptorName,
			   const Eigen::Vector3d& position,
			   const Eigen::AngleAxisd& orientation,
			   const unsigned int& widht,
			   const unsigned int& height,
			   const float& focalLengthX,
			   const float& focalLengthY)
      : SensorConfig(name, perceptorName, position, orientation),
        _imageWidth(widht),
        _imageHeight(height),
        _focalLengthX(focalLengthX),
        _focalLengthY(focalLengthY)
{
}

CameraConfig::~CameraConfig()
{
}

const unsigned int& CameraConfig::getFrameWidth() const
{
  return _imageWidth;
}

const unsigned int& CameraConfig::getFrameHeight() const
{
  return _imageHeight;
}

const float& CameraConfig::getFocalLengthX() const
{
  return _focalLengthX;
}

const float& CameraConfig::getFocalLengthY() const
{
  return _focalLengthY;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <g.gael@free.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "sparse_solver.h"

#include <Eigen/UmfPackSupport>

template<typename T> void test_umfpack_support_T()
{
  UmfPackLU<SparseMatrix<T, ColMajor> > umfpack_colmajor;
  UmfPackLU<SparseMatrix<T, RowMajor> > umfpack_rowmajor;

  check_sparse_square_solving(umfpack_colmajor);
  check_sparse_square_solving(umfpack_rowmajor);

  check_sparse_square_determinant(umfpack_colmajor);
  check_sparse_square_determinant(umfpack_rowmajor);
}

void test_umfpack_support()
{
  CALL_SUBTEST_1(test_umfpack_support_T<double>());
  CALL_SUBTEST_2(test_umfpack_support_T<std::complex<double> >());
}


/*
 *  Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014-2016, CNRS-LAAS.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER 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.
 */

/** \author Florent Lamiraux <florent@laas.fr> */

#define BOOST_TEST_MODULE FCL_BOX_BOX
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))

#include <cmath>
#include <iostream>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/shape/geometric_shapes.h>

#include "utility.h"

typedef boost::shared_ptr <hpp::fcl::CollisionGeometry> CollisionGeometryPtr_t;

using hpp::fcl::Transform3f;
using hpp::fcl::Vec3f;
using hpp::fcl::CollisionObject;
using hpp::fcl::DistanceResult;
using hpp::fcl::DistanceRequest;

BOOST_AUTO_TEST_CASE(distance_box_box_1)
{
  CollisionGeometryPtr_t s1 (new hpp::fcl::Box (6, 10, 2));
  CollisionGeometryPtr_t s2 (new hpp::fcl::Box (2, 2, 2));

  Transform3f tf1;
  Transform3f tf2 (Vec3f(25, 20, 5));

  CollisionObject o1 (s1, tf1);
  CollisionObject o2 (s2, tf2);

  // Enable computation of nearest points
  DistanceRequest distanceRequest (true, 0, 0);
  DistanceResult distanceResult;

  hpp::fcl::distance (&o1, &o2, distanceRequest, distanceResult);

  std::cerr << "Applied transformations on two boxes" << std::endl;
  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
	    << " R1 = " << tf1.getRotation () << std::endl
	    << " T2 = " << tf2.getTranslation() << std::endl
	    << " R2 = " << tf2.getRotation () << std::endl;
  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
	    << ", p2 = " << distanceResult.nearest_points [1]
	    << ", distance = " << distanceResult.min_distance << std::endl;
  double dx = 25 - 3 - 1;
  double dy = 20 - 5 - 1;
  double dz = 5 - 1 - 1;

  const Vec3f& p1 = distanceResult.nearest_points [0];
  const Vec3f& p2 = distanceResult.nearest_points [1];
  BOOST_CHECK_CLOSE(distanceResult.min_distance,
		    sqrt (dx*dx + dy*dy + dz*dz), 1e-4);

  BOOST_CHECK_CLOSE (p1 [0], 3, 1e-6);
  BOOST_CHECK_CLOSE (p1 [1], 5, 1e-6);
  BOOST_CHECK_CLOSE (p1 [2], 1, 1e-6);
  BOOST_CHECK_CLOSE (p2 [0], 24, 1e-6);
  BOOST_CHECK_CLOSE (p2 [1], 19, 1e-6);
  BOOST_CHECK_CLOSE (p2 [2], 4, 1e-6);
}

BOOST_AUTO_TEST_CASE(distance_box_box_2)
{
  CollisionGeometryPtr_t s1 (new hpp::fcl::Box (6, 10, 2));
  CollisionGeometryPtr_t s2 (new hpp::fcl::Box (2, 2, 2));
  static double pi = M_PI;
  Transform3f tf1;
  Transform3f tf2 (hpp::fcl::makeQuat (cos (pi/8), sin(pi/8)/sqrt(3),
				 sin(pi/8)/sqrt(3), sin(pi/8)/sqrt(3)),
		   Vec3f(0, 0, 10));

  CollisionObject o1 (s1, tf1);
  CollisionObject o2 (s2, tf2);

  // Enable computation of nearest points
  DistanceRequest distanceRequest (true, 0, 0);
  DistanceResult distanceResult;

  hpp::fcl::distance (&o1, &o2, distanceRequest, distanceResult);

  std::cerr << "Applied transformations on two boxes" << std::endl;
  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
	    << " R1 = " << tf1.getRotation () << std::endl
	    << " T2 = " << tf2.getTranslation() << std::endl
	    << " R2 = " << tf2.getRotation () << std::endl;
  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
	    << ", p2 = " << distanceResult.nearest_points [1]
	    << ", distance = " << distanceResult.min_distance << std::endl;

  const Vec3f& p1 = distanceResult.nearest_points [0];
  const Vec3f& p2 = distanceResult.nearest_points [1];
  double distance = -1.62123444 + 10 - 1;
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);

  BOOST_CHECK_CLOSE (p1 [0], 0.60947571, 1e-4);
  BOOST_CHECK_CLOSE (p1 [1], 0.01175873, 1e-4);
  BOOST_CHECK_CLOSE (p1 [2], 1, 1e-6);
  BOOST_CHECK_CLOSE (p2 [0], 0.60947571, 1e-4);
  BOOST_CHECK_CLOSE (p2 [1], 0.01175873, 1e-4);
  BOOST_CHECK_CLOSE (p2 [2], -1.62123444 + 10, 1e-4);
}

BOOST_AUTO_TEST_CASE(distance_box_box_3)
{
  CollisionGeometryPtr_t s1 (new hpp::fcl::Box (1, 1, 1));
  CollisionGeometryPtr_t s2 (new hpp::fcl::Box (1, 1, 1));
  static double pi = M_PI;
  Transform3f tf1 (hpp::fcl::makeQuat (cos (pi/8), 0, 0, sin (pi/8)),
		   Vec3f (-2, 1, .5));
  Transform3f tf2 (hpp::fcl::makeQuat (cos (pi/8), 0, sin(pi/8),0),
		   Vec3f (2, .5, .5));

  CollisionObject o1 (s1, tf1);
  CollisionObject o2 (s2, tf2);

  // Enable computation of nearest points
  DistanceRequest distanceRequest (true, 0, 0);
  DistanceResult distanceResult;

  hpp::fcl::distance (&o1, &o2, distanceRequest, distanceResult);

  std::cerr << "Applied transformations on two boxes" << std::endl;
  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
	    << " R1 = " << tf1.getRotation () << std::endl
	    << " T2 = " << tf2.getTranslation() << std::endl
	    << " R2 = " << tf2.getRotation () << std::endl;
  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
	    << ", p2 = " << distanceResult.nearest_points [1]
	    << ", distance = " << distanceResult.min_distance << std::endl;

  const Vec3f& p1 = distanceResult.nearest_points [0];
  const Vec3f& p2 = distanceResult.nearest_points [1];
  double distance = 4 - sqrt (2);
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);

  const Vec3f p1Ref (sqrt(2)/2 - 2, 1, .5);
  const Vec3f p2Ref (2 - sqrt(2)/2, 1, .5);
  BOOST_CHECK_CLOSE (p1 [0], p1Ref [0], 1e-4);
  BOOST_CHECK_CLOSE (p1 [1], p1Ref [1], 1e-4);
  BOOST_CHECK_CLOSE (p1 [2], p1Ref [2], 1e-4);
  BOOST_CHECK_CLOSE (p2 [0], p2Ref [0], 1e-4);
  BOOST_CHECK_CLOSE (p2 [1], p2Ref [1], 1e-4);
  BOOST_CHECK_CLOSE (p2 [2], p2Ref [2], 1e-4);

  // Apply the same global transform to both objects and recompute
  Transform3f tf3 (hpp::fcl::makeQuat (0.435952844074,-0.718287018243,
				 0.310622451066, 0.444435113443),
		   Vec3f (4, 5, 6));
  tf1 = tf3*tf1;
  tf2 = tf3*tf2;
  o1 = CollisionObject (s1, tf1);
  o2 = CollisionObject (s2, tf2);

  distanceResult.clear ();
  hpp::fcl::distance (&o1, &o2, distanceRequest, distanceResult);

  std::cerr << "Applied transformations on two boxes" << std::endl;
  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
	    << " R1 = " << tf1.getRotation () << std::endl
	    << " T2 = " << tf2.getTranslation() << std::endl
	    << " R2 = " << tf2.getRotation () << std::endl;
  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
	    << ", p2 = " << distanceResult.nearest_points [1]
	    << ", distance = " << distanceResult.min_distance << std::endl;

  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);

  const Vec3f p1Moved = tf3.transform (p1Ref);
  const Vec3f p2Moved = tf3.transform (p2Ref);
  BOOST_CHECK_CLOSE (p1 [0], p1Moved [0], 1e-4);
  BOOST_CHECK_CLOSE (p1 [1], p1Moved [1], 1e-4);
  BOOST_CHECK_CLOSE (p1 [2], p1Moved [2], 1e-4);
  BOOST_CHECK_CLOSE (p2 [0], p2Moved [0], 1e-4);
  BOOST_CHECK_CLOSE (p2 [1], p2Moved [1], 1e-4);
  BOOST_CHECK_CLOSE (p2 [2], p2Moved [2], 1e-4);

}

BOOST_AUTO_TEST_CASE(distance_box_box_4)
{
  hpp::fcl::Box s1 (1, 1, 1);
  hpp::fcl::Box s2 (1, 1, 1);

  // Enable computation of nearest points
  DistanceRequest distanceRequest (true, 0, 0);
  DistanceResult distanceResult;
  double distance;

  Transform3f tf1 (Vec3f (2, 0, 0));
  Transform3f tf2;
  hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);

  distance = 1.;
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);

  tf1.setTranslation(Vec3f (1.01, 0, 0));
  distanceResult.clear();
  hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);

  distance = 0.01;
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);

  tf1.setTranslation(Vec3f (0.99, 0, 0));
  distanceResult.clear();
  hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);

  distance = -0.01;
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);

  tf1.setTranslation(Vec3f (0, 0, 0));
  distanceResult.clear();
  hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);

  distance = -1;
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2013-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef ERROREST_HH
#define ERROREST_HH

#include <vector>

#include <boost/fusion/include/for_each.hpp>
#include <boost/utility.hpp>
#include <boost/multi_array.hpp>

namespace Kaskade
{
  /**
   * \ingroup refcrit
   * \brief Base class for refinement criteria.
   */
  class RefinementCriterion
  {
  public:
    /**
     * \brief Computes the thresholds for refinement.
     * \param normalizedErrors a twodimensional array containing the normalized error contribution for each cell and
     *                         each variable, i.e. normalizedErrors[i][j] contains the error contribution of cell i
     *                         to the error in variable j, divided by the tolerance, such that the sum over i
     *                         should not exceed one (for acceptance)
     * \returns an array containing a threshold value such that any cell i for which normalizedErrors[i][j] > return[j]
     *          for any j is to be refined, or an empty array in case all variables are accurate enough
     */
    std::vector<double> threshold(boost::multi_array<double,2> const& normalizedErrors) const;

  private:
    virtual double computeThreshold(std::vector<double>& normalizedErrors, double totalError, int j) const = 0;
  };

  /**
   * \ingroup refcrit
   * \brief Refines the grid according to the normalized errors and the given thresholds.
   */
  template <class GridManager, class GridView>
  void markAndRefine(GridManager& gridManager, GridView const& gridView, boost::multi_array<double,2> const& normalizedErrors,
                     std::vector<double> const& threshold)
  {
    auto end = gridView.template end<0>();
    for (auto ci=gridView.template begin<0>(); ci!=end; ++ci)
    {
      auto idx = gridView.indexSet().index(*ci);
      for (int j=0; j<threshold.size(); ++j)
        if (normalizedErrors[idx][j] >= threshold[j])
        {
          gridManager.mark(1,*ci);
          break;
        }
    }
    gridManager.adaptAtOnce();
  }

  /**
   * \ingroup refcrit
   * \brief Fixed fraction refinement criterion.
   * Determines the refinement thresholds such that at least the specified fraction of the cells are
   * marked for refinement. This ensures that not too few cells are marked, which could lead to very many
   * refinement iterations, but may lead to erratic refinement if there are indeed only very local refinements
   * necessary.
   *
   * In very rare circumstances (namely when there are many cells with exactly the same local error estimate),
   * much more cells than the required fraction can be marked for refinement.
   */
  class FixedFractionCriterion: public RefinementCriterion
  {
  public:
    FixedFractionCriterion(double fraction = 0.2);

  private:
    double fraction;
    virtual double computeThreshold(std::vector<double>& normalizedErrors, double totalError, int j) const;
  };

  /**
   * \ingroup refcrit
   * \brief Bulk refinement criterion.
   * Determines the refinement thresholds such that approximately the specified fraction of the total error
   * is removed by the refinement (under the unrealistically optimistic assumption that refinement eliminates
   * the error completely in that cell - in reality, it's only reduced by a certain factor, so the total
   * error is reduced somewhat less).
   */
  class BulkCriterion: public RefinementCriterion
  {
  public:
    BulkCriterion(double fraction = 0.2);

  private:
    double fraction;
    virtual double computeThreshold(std::vector<double>& normalizedErrors, double totalError, int j) const;
  };

  /**
   * \ingroup refcrit
   * \brief Max value refinement criterion.
   * Determines the refinement thresholds such that all cells with an error contribution exceeding a certain
   * fraction of the maximum error contribution are refined.
   */
  class MaxValueCriterion: public RefinementCriterion
  {
  public:
    MaxValueCriterion(double fraction = 0.2);

  private:
    double fraction;
    virtual double computeThreshold(std::vector<double>& normalizedErrors, double totalError, int j) const;
  };

  /**
   * \ingroup refcrit
   * \brief Babuska-Rheinboldt refinement criterion.
   * Determines the refinement thresholds such that all cells with an error contribution exceeding the
   * expected error contribution of the worst cell \em after refinement will be marked. This tends to
   * equilibrate the error contributions quite fast.
   *
   * Note that the local convergence order should be estimated (important in the vicinity of singularities)
   * but is here assumed to be the fixed specified order.
   */
  class BabuskaRheinboldtCriterion: public MaxValueCriterion
  {
  public:
    BabuskaRheinboldtCriterion(std::vector<int> const& order);

  private:
    std::vector<int> order;
    virtual double computeThreshold(std::vector<double>& normalizedErrors, double totalError, int j) const;
  };

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

  namespace ErrorestDetail
  {
    template <class GroupByCell>
    struct GroupedSummationCollector
    {
      GroupedSummationCollector(GroupByCell const& group_):
        group(group_)
      {}

      template <class Cell>
      int integrationOrder(Cell const& , int shapeFunctionOrder) const
      {
        return shapeFunctionOrder;
      }

      // need to define this because boost::multi_array does not perform assignment if the arrays have
      // different shape.
      GroupedSummationCollector<GroupByCell>& operator=(GroupedSummationCollector<GroupByCell> const& c)
      {
        auto shape = c.sums.shape();

        sums.resize(boost::extents[shape[0]][shape[1]]);
        sums = c.sums;
        group = c.group;

        return *this;
      }

      template <class CellPointer, class Index, class Sequence>
      void operator()(CellPointer const& ci, Index idx, double weight, Sequence const& x)
      {
        if (sums.num_elements()==0)
          sums.resize(boost::extents[group.nGroups][boost::fusion::size(x)]);
        int i = 0; // for_each needs constant functor...
        boost::fusion::for_each(x,Add(sums,group[idx],i,weight));
      }

      void join(GroupedSummationCollector<GroupByCell> const& c)
      {
        auto shape = c.sums.shape();
        auto myshape = sums.shape();

        if(c.sums.num_elements()==0) return;
        if (sums.num_elements()==0 || myshape[0]!=shape[0] || myshape[1]!=shape[1]) // not yet initialized
        {
          sums.resize(boost::extents[shape[0]][shape[1]]);
          sums = c.sums;                                  // -> do a simple copy
        }
        else
          for (size_t i=0; i<shape[0]; ++i)               // otherwise add up
            for (size_t j=0; j<shape[1]; ++j)
              sums[i][j] += c.sums[i][j];
      }

      boost::multi_array<double,2> sums;

    private:
      GroupByCell group;

      struct Add
      {
        Add(boost::multi_array<double,2>& sums_, int idx_, int& i_, double w_):
          sums(sums_), idx(idx_), i(i_) , w(w_)
        {}

        template <class T>
        void operator()(T const& t) const { sums[idx][i++] += w*t; }

      private:
        boost::multi_array<double,2>& sums;
        int idx;
        int& i;
        double w;
      };
    };

  }
}

#endif