#pragma once

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <cstdint>
#include <functional>
#include <limits>
#include <stdexcept>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/openmp_helpers.hpp>

namespace common_robotics_utilities
{
/// Compute Hausdorff distance between two distributions.
/// "The Hausdorff distance is the longest distance you can be forced to travel
/// by an adversary who chooses a point in one of the two sets, from where you
/// then must travel to the other set. In other words, it is the greatest of all
/// the distances from a point in one set to the closest point in the other
/// set." - from Wikipedia
namespace simple_hausdorff_distance
{
/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_function to compute distance
/// between an element in @param first_distribution and an element in @param
/// second_distribution. @return distance between distributions.
/// Computation is performed in parallel, and the larger of the two input
/// distributions is automatically selected to be the outer (parallelized) loop.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistanceParallel(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const std::function<double(const FirstDatatype&,
                               const SecondDatatype&)>& distance_fn)
{
  if (first_distribution.empty())
  {
    throw std::invalid_argument("first_distribution is empty");
  }
  if (second_distribution.empty())
  {
    throw std::invalid_argument("second_distribution is empty");
  }
  // Swap if needed to make the outer parallel loop be larger
  const bool swap = (first_distribution.size() < second_distribution.size());
  const auto& outer_distribution
      = (swap) ? second_distribution : first_distribution;
  const auto& inner_distribution
      = (swap) ? first_distribution : second_distribution;
  // Make per-thread storage
  std::vector<double> per_thread_storage(
      openmp_helpers::GetNumOmpThreads(), 0.0);
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t idx = 0; idx < outer_distribution.size(); idx++)
  {
    const FirstDatatype& first = outer_distribution[idx];
    double minimum_distance = std::numeric_limits<double>::infinity();
    for (size_t jdx = 0; jdx < inner_distribution.size(); jdx++)
    {
      const SecondDatatype& second = inner_distribution[jdx];
      const double current_distance = distance_fn(first, second);
      if (current_distance < minimum_distance)
      {
        minimum_distance = current_distance;
      }
    }
    const auto current_thread_id = openmp_helpers::GetContextOmpThreadNum();
    if (minimum_distance > per_thread_storage.at(current_thread_id))
    {
      per_thread_storage.at(current_thread_id) = minimum_distance;
    }
  }
  double maximum_minimum_distance = 0.0;
  for (const double& temp_minimum_distance : per_thread_storage)
  {
    if (temp_minimum_distance > maximum_minimum_distance)
    {
      maximum_minimum_distance = temp_minimum_distance;
    }
  }
  return maximum_minimum_distance;
}

/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_function to compute distance
/// between an element in @param first_distribution and an element in @param
/// second_distribution. @return distance between distributions.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistanceSerial(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const std::function<double(const FirstDatatype&,
                               const SecondDatatype&)>& distance_fn)
{
  if (first_distribution.empty())
  {
    throw std::invalid_argument("first_distribution is empty");
  }
  if (second_distribution.empty())
  {
    throw std::invalid_argument("second_distribution is empty");
  }
  double maximum_minimum_distance = 0.0;
  for (size_t idx = 0; idx < first_distribution.size(); idx++)
  {
    const FirstDatatype& first = first_distribution[idx];
    double minimum_distance = std::numeric_limits<double>::infinity();
    for (size_t jdx = 0; jdx < second_distribution.size(); jdx++)
    {
      const SecondDatatype& second = second_distribution[jdx];
      const double current_distance = distance_fn(first, second);
      if (current_distance < minimum_distance)
      {
        minimum_distance = current_distance;
      }
    }
    if (minimum_distance > maximum_minimum_distance)
    {
      maximum_minimum_distance = minimum_distance;
    }
  }
  return maximum_minimum_distance;
}

/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_matrix which stores the pairwise
/// distance between all elements in @param first_distribution and @param
/// second_distribution. @return distance between distributions.
/// Computation is performed in parallel, and the larger of the two input
/// distributions is automatically selected to be the outer (parallelized) loop.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistanceParallel(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const Eigen::MatrixXd& distance_matrix)
{
  if (first_distribution.empty())
  {
    throw std::invalid_argument("first_distribution is empty");
  }
  if (second_distribution.empty())
  {
    throw std::invalid_argument("second_distribution is empty");
  }
  if (static_cast<size_t>(distance_matrix.rows()) != first_distribution.size()
      || static_cast<size_t>(distance_matrix.cols())
         != second_distribution.size())
  {
    throw std::invalid_argument("distance_matrix is the wrong size");
  }
  // Swap if needed to make the outer parallel loop be larger
  const bool swap = (first_distribution.size() < second_distribution.size());
  const auto& outer_distribution
      = (swap) ? second_distribution : first_distribution;
  const auto& inner_distribution
      = (swap) ? first_distribution : second_distribution;
  // Make per-thread storage
  std::vector<double> per_thread_storage(
      openmp_helpers::GetNumOmpThreads(), 0.0);
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t idx = 0; idx < outer_distribution.size(); idx++)
  {
    double minimum_distance = std::numeric_limits<double>::infinity();
    for (size_t jdx = 0; jdx < inner_distribution.size(); jdx++)
    {
      // Swap the lookup if we've swapped the loops
      const ssize_t row = (swap) ? static_cast<ssize_t>(jdx)
                                 : static_cast<ssize_t>(idx);
      const ssize_t col = (swap) ? static_cast<ssize_t>(idx)
                                 : static_cast<ssize_t>(jdx);
      const double current_distance = distance_matrix(row, col);
      if (current_distance < minimum_distance)
      {
        minimum_distance = current_distance;
      }
    }
    const auto current_thread_id = openmp_helpers::GetContextOmpThreadNum();
    if (minimum_distance > per_thread_storage.at(current_thread_id))
    {
      per_thread_storage.at(current_thread_id) = minimum_distance;
    }
  }
  double maximum_minimum_distance = 0.0;
  for (const double& temp_minimum_distance : per_thread_storage)
  {
    if (temp_minimum_distance > maximum_minimum_distance)
    {
      maximum_minimum_distance = temp_minimum_distance;
    }
  }
  return maximum_minimum_distance;
}

/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_matrix which stores the pairwise
/// distance between all elements in @param first_distribution and @param
/// second_distribution. @return distance between distributions.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistanceSerial(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const Eigen::MatrixXd& distance_matrix)
{
  if (first_distribution.empty())
  {
    throw std::invalid_argument("first_distribution is empty");
  }
  if (second_distribution.empty())
  {
    throw std::invalid_argument("second_distribution is empty");
  }
  if (static_cast<size_t>(distance_matrix.rows()) != first_distribution.size()
      || static_cast<size_t>(distance_matrix.cols())
         != second_distribution.size())
  {
    throw std::invalid_argument("distance_matrix is the wrong size");
  }
  double maximum_minimum_distance = 0.0;
  for (size_t idx = 0; idx < first_distribution.size(); idx++)
  {
    double minimum_distance = std::numeric_limits<double>::infinity();
    for (size_t jdx = 0; jdx < second_distribution.size(); jdx++)
    {
      const double current_distance
          = distance_matrix(static_cast<ssize_t>(idx),
                            static_cast<ssize_t>(jdx));
      if (current_distance < minimum_distance)
      {
        minimum_distance = current_distance;
      }
    }
    if (minimum_distance > maximum_minimum_distance)
    {
      maximum_minimum_distance = minimum_distance;
    }
  }
  return maximum_minimum_distance;
}

/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_function to compute distance
/// between an element in @param first_distribution and an element in @param
/// second_distribution. @return distance between distributions.
/// @param use_parallel selects if the computation should be performed in
/// parallel. If computation is performed in parallel, the larger of the two
/// input distributions is automatically selected to be the outer (parallelized)
/// loop.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistance(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const std::function<double(const FirstDatatype&,
                               const SecondDatatype&)>& distance_fn,
    const bool use_parallel = false)
{
  if (use_parallel)
  {
    return ComputeDistanceParallel
        <FirstDatatype, SecondDatatype, FirstContainer, SecondContainer>(
            first_distribution, second_distribution, distance_fn);
  }
  else
  {
    return ComputeDistanceSerial
        <FirstDatatype, SecondDatatype, FirstContainer, SecondContainer>(
            first_distribution, second_distribution, distance_fn);
  }
}

/// Compute Hausdorff distance between @param first_distribution and @param
/// second_distribution using @param distance_matrix which stores the pairwise
/// distance between all elements in @param first_distribution and @param
/// second_distribution. @return distance between distributions.
/// @param use_parallel selects if the computation should be performed in
/// parallel. If computation is performed in parallel, the larger of the two
/// input distributions is automatically selected to be the outer (parallelized)
/// loop.
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double ComputeDistance(
    const FirstContainer& first_distribution,
    const SecondContainer& second_distribution,
    const Eigen::MatrixXd& distance_matrix,
    const bool use_parallel = false)
{
  if (use_parallel)
  {
    return ComputeDistanceParallel
        <FirstDatatype, SecondDatatype, FirstContainer, SecondContainer>(
            first_distribution, second_distribution, distance_matrix);
  }
  else
  {
    return ComputeDistanceSerial
        <FirstDatatype, SecondDatatype, FirstContainer, SecondContainer>(
            first_distribution, second_distribution, distance_matrix);
  }
}
}  // namespace simple_hausdorff_distance
}  // namespace common_robotics_utilities


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

#ifndef DART_MATH_GEOMETRY_HPP_
#define DART_MATH_GEOMETRY_HPP_

#include <Eigen/Dense>

#include "dart/common/Deprecated.hpp"
#include "dart/math/Constants.hpp"
#include "dart/math/MathTypes.hpp"

namespace dart {
namespace math {

/// \brief
Eigen::Matrix3s makeSkewSymmetric(const Eigen::Vector3s& _v);

/// \brief
Eigen::Vector3s fromSkewSymmetric(const Eigen::Matrix3s& _m);

//------------------------------------------------------------------------------
/// \brief
Eigen::Quaternion_s expToQuat(const Eigen::Vector3s& _v);

/// \brief
Eigen::Vector3s quatToExp(const Eigen::Quaternion_s& _q);

/// \brief
Eigen::Vector3s rotatePoint(
    const Eigen::Quaternion_s& _q, const Eigen::Vector3s& _pt);

/// \brief
Eigen::Vector3s rotatePoint(
    const Eigen::Quaternion_s& _q, s_t _x, s_t _y, s_t _z);

/// \brief
Eigen::Matrix3s quatDeriv(const Eigen::Quaternion_s& _q, int _el);

/// \brief
Eigen::Matrix3s quatSecondDeriv(
    const Eigen::Quaternion_s& _q, int _el1, int _el2);

//------------------------------------------------------------------------------
/// \brief Given Euler XYX angles, return a 3x3 rotation matrix, which is
/// equivalent to RotX(angle(0)) * RotY(angle(1)) * RotX(angle(2)).
Eigen::Matrix3s eulerXYXToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerXYZ angles, return a 3x3 rotation matrix, which is
/// equivalent to RotX(angle(0)) * RotY(angle(1)) * RotZ(angle(2)).
Eigen::Matrix3s eulerXYZToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerXZX angles, return a 3x3 rotation matrix, which is
/// equivalent to RotX(angle(0)) * RotZ(angle(1)) * RotX(angle(2)).
Eigen::Matrix3s eulerXZXToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerXZY angles, return a 3x3 rotation matrix, which is
/// equivalent to RotX(angle(0)) * RotZ(angle(1)) * RotY(angle(2)).
Eigen::Matrix3s eulerXZYToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerYXY angles, return a 3x3 rotation matrix, which is
/// equivalent to RotY(angle(0)) * RotX(angle(1)) * RotY(angle(2)).
Eigen::Matrix3s eulerYXYToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerYXZ angles, return a 3x3 rotation matrix, which is
/// equivalent to RotY(angle(0)) * RotX(angle(1)) * RotZ(angle(2)).
Eigen::Matrix3s eulerYXZToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerYZX angles, return a 3x3 rotation matrix, which is
/// equivalent to RotY(angle(0)) * RotZ(angle(1)) * RotX(angle(2)).
Eigen::Matrix3s eulerYZXToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerYZY angles, return a 3x3 rotation matrix, which is
/// equivalent to RotY(angle(0)) * RotZ(angle(1)) * RotY(angle(2)).
Eigen::Matrix3s eulerYZYToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerZXY angles, return a 3x3 rotation matrix, which is
/// equivalent to RotZ(angle(0)) * RotX(angle(1)) * RotY(angle(2)).
Eigen::Matrix3s eulerZXYToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerZYX angles, return a 3x3 rotation matrix, which is
/// equivalent to RotZ(angle(0)) * RotY(angle(1)) * RotX(angle(2)).
/// singularity : angle[1] = -+ 0.5*PI
Eigen::Matrix3s eulerZYXToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerZXZ angles, return a 3x3 rotation matrix, which is
/// equivalent to RotZ(angle(0)) * RotX(angle(1)) * RotZ(angle(2)).
Eigen::Matrix3s eulerZXZToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerZYZ angles, return a 3x3 rotation matrix, which is
/// equivalent to RotZ(angle(0)) * RotY(angle(1)) * RotZ(angle(2)).
/// singularity : angle[1] = 0, PI
Eigen::Matrix3s eulerZYZToMatrix(const Eigen::Vector3s& _angle);

//------------------------------------------------------------------------------
/// \brief get the Euler XYX angle from R
Eigen::Vector3s matrixToEulerXYX(const Eigen::Matrix3s& _R);

/// \brief get the Euler XYZ angle from R
Eigen::Vector3s matrixToEulerXYZ(const Eigen::Matrix3s& _R);

///// \brief get the Euler XZX angle from R
// Eigen::Vector3s matrixToEulerXZX(const Eigen::Matrix3s& R);

/// \brief get the Euler XZY angle from R
Eigen::Vector3s matrixToEulerXZY(const Eigen::Matrix3s& _R);

///// \brief get the Euler YXY angle from R
// Eigen::Vector3s matrixToEulerYXY(const Eigen::Matrix3s& R);

/// \brief get the Euler YXZ angle from R
Eigen::Vector3s matrixToEulerYXZ(const Eigen::Matrix3s& _R);

/// \brief get the Euler YZX angle from R
Eigen::Vector3s matrixToEulerYZX(const Eigen::Matrix3s& _R);

///// \brief get the Euler YZY angle from R
// Eigen::Vector3s matrixToEulerYZY(const Eigen::Matrix3s& R);

/// \brief get the Euler ZXY angle from R
Eigen::Vector3s matrixToEulerZXY(const Eigen::Matrix3s& _R);

/// \brief get the Euler ZYX angle from R
Eigen::Vector3s matrixToEulerZYX(const Eigen::Matrix3s& _R);

///// \brief get the Euler ZXZ angle from R
// Eigen::Vector3s matrixToEulerZXZ(const Eigen::Matrix3s& R);

///// \brief get the Euler ZYZ angle from R
// Eigen::Vector3s matrixToEulerZYZ(const Eigen::Matrix3s& R);

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

/// Returns the Jacobian of an SO(3) element w.r.t. its exponential
/// coordinates where the Jacobian maps the time derivative of the exponential
/// coordinates to the angular velocity in the world frame.
Eigen::Matrix3s so3LeftJacobian(const Eigen::Vector3s& w);

/// Returns the Jacobian of an SO(3) element w.r.t. its exponential
/// coordinates where the Jacobian maps the time derivative of the exponential
/// coordinates to the angular velocity in the body frame.
Eigen::Matrix3s so3RightJacobian(const Eigen::Vector3s& w);

/// Returns the time derivative of the left Jacobian of SO(3).
Eigen::Matrix3s so3LeftJacobianTimeDeriv(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq);

/// Returns the time derivative of the right Jacobian of SO(3).
Eigen::Matrix3s so3RightJacobianTimeDeriv(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq);

/// \brief Exponential mapping
Eigen::Isometry3s expMap(const Eigen::Vector6s& _S);

/// \brief Exponential mapping, DART style. This treats the exponentiation
/// operation as a rotation, and then a translation, rather than an integration
/// of a screw.
Eigen::Isometry3s expMapDart(const Eigen::Vector6s& _S);

/// \brief fast version of Exp(se3(s, 0))
/// \todo This expAngular() can be replaced by Eigen::AngleAxis() but we need
/// to verify that they have exactly same functionality.
/// See: https://github.com/dartsim/dart/issues/88
Eigen::Isometry3s expAngular(const Eigen::Vector3s& _s);

/// \brief Computes the Rotation matrix from a given expmap vector.
Eigen::Matrix3s expMapRot(const Eigen::Vector3s& _expmap);

/// \brief Computes the Jacobian of the expmap
Eigen::Matrix3s expMapJac(const Eigen::Vector3s& _expmap);

/// Returns the Jacobian of an SO(3) element w.r.t. its exponential
/// coordinates where the Jacobian maps the time derivative of the exponential
/// coordinates to the angular velocity in the world frame.
Eigen::Matrix3s so3LeftJacobian(const Eigen::Vector3s& w);

/// Returns the Jacobian of an SO(3) element w.r.t. its exponential
/// coordinates where the Jacobian maps the time derivative of the exponential
/// coordinates to the angular velocity in the body frame.
Eigen::Matrix3s so3RightJacobian(const Eigen::Vector3s& w);

/// \brief Computes the Jacobian of the logMap(R * expMapRot(expMap))
Eigen::Matrix3s expMapJacAt(
    const Eigen::Vector3s& _expmap, const Eigen::Matrix3s& R);

/// \brief Computes the time derivative of the expmap Jacobian.
Eigen::Matrix3s expMapJacDot(
    const Eigen::Vector3s& _expmap, const Eigen::Vector3s& _qdot);

Eigen::Matrix3s so3LeftJacobianTimeDeriv(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq);

Eigen::Matrix3s so3RightJacobianTimeDeriv(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq);

Eigen::Matrix3s so3RightJacobianTimeDerivDeriv(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq, int index);

Eigen::Matrix3s so3RightJacobianTimeDerivDeriv2(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq, int index);

/// \brief computes the derivative of the Jacobian of the expmap wrt to _qi
/// indexed dof; _qi \f$ \in \f$ {0,1,2}
Eigen::Matrix3s expMapJacDeriv(const Eigen::Vector3s& _expmap, int _qi);

/// \brief computes the gradient of logMap(expMapRot()) wrt to _qi
/// indexed dof; _qi \f$ \in \f$ {0,1,2}
Eigen::Vector3s expMapGradient(const Eigen::Vector3s& pos, int _qi);

/// \brief computes the gradient of logMap(expMapRot(screw * eps) *
/// expMapRot(original)) wrt to eps
Eigen::Vector3s expMapNestedGradient(
    const Eigen::Vector3s& original, const Eigen::Vector3s& screw);

Eigen::Vector3s finiteDifferenceExpMapNestedGradient(
    const Eigen::Vector3s& original,
    const Eigen::Vector3s& screw,
    bool useRidders = true);

Eigen::Vector3s finiteDifferenceRiddersExpMapNestedGradient(
    const Eigen::Vector3s& original, const Eigen::Vector3s& screw);

/// \brief Log mapping
/// \note When @f$|Log(R)| = @pi@f$, Exp(LogR(R) = Exp(-Log(R)).
/// The implementation returns only the positive one.
Eigen::Vector3s logMap(const Eigen::Matrix3s& _R);

/// \brief Log mapping
Eigen::Vector6s logMap(const Eigen::Isometry3s& _T);

/// This takes a screw axis and a point, and gives us the direction that the
/// point will move if we increase theta by an infinitesimal amount.
Eigen::Vector3s gradientWrtTheta(
    const Eigen::Vector6s& screwAxis, const Eigen::Vector3s& point, s_t theta);

/// This takes a rotation axis and a point, and gives us the direction that the
/// point will move if we increase the theta by an infinitesimal amount.
Eigen::Vector3s gradientWrtThetaPureRotation(
    const Eigen::Vector3s& omega, const Eigen::Vector3s& point, s_t theta);

/// This returns the average of the points on edge A and edge B closest to each
/// other.
Eigen::Vector3s getContactPoint(
    const Eigen::Vector3s& edgeAPoint,
    const Eigen::Vector3s& edgeADir,
    const Eigen::Vector3s& edgeBPoint,
    const Eigen::Vector3s& edgeBDir,
    s_t radiusA = 1.0,
    s_t radiusB = 1.0);

/// This returns gradient of the average of the points on edge A and edge B
/// closest to each other, allowing all the inputs to change.
Eigen::Vector3s getContactPointGradient(
    const Eigen::Vector3s& edgeAPoint,
    const Eigen::Vector3s& edgeAPointGradient,
    const Eigen::Vector3s& edgeADir,
    const Eigen::Vector3s& edgeADirGradient,
    const Eigen::Vector3s& edgeBPoint,
    const Eigen::Vector3s& edgeBPointGradient,
    const Eigen::Vector3s& edgeBDir,
    const Eigen::Vector3s& edgeBDirGradient,
    s_t radiusA = 1.0,
    s_t radiusB = 1.0);

Eigen::VectorXs dampedPInv(
    const Eigen::MatrixXs& J, const Eigen::VectorXs& x, s_t damping = 0.05);

bool hasTinySingularValues(
    const Eigen::MatrixXs& J, s_t clippingThreshold = 1e-4);

Eigen::MatrixXs clippedSingularsPinv(
    const Eigen::MatrixXs& J, s_t clippingThreshold = 1e-4);

//------------------------------------------------------------------------------
/// \brief Rectify the rotation part so as that it satifies the orthogonality
/// condition.
///
/// It is one step of @f$R_{i_1}=1/2(R_i + R_i^{-T})@f$.
/// Hence by calling this function iterativley, you can make the rotation part
/// closer to SO(3).
// SE3 Normalize(const SE3& T);

/// \brief reparameterize such as ||s'|| < M_PI and Exp(s) == Epx(s')
// Axis Reparameterize(const Axis& s);

//------------------------------------------------------------------------------
/// \brief adjoint mapping
/// \note @f$Ad_TV = ( Rw@,, ~p @times Rw + Rv)@f$,
/// where @f$T=(R,p)@in SE(3), @quad V=(w,v)@in se(3) @f$.
Eigen::Vector6s AdT(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _V);

/// \brief Get linear transformation matrix of Adjoint mapping
Eigen::Matrix6s getAdTMatrix(const Eigen::Isometry3s& T);

// TODO(JS): Rename and add documentation
Eigen::Matrix6s AdTMatrix(const Eigen::Isometry3s& T);
Eigen::Matrix6s AdInvTMatrix(const Eigen::Isometry3s& T);

// TODO(JS): Rename and add documentation
Eigen::Matrix6s dAdTMatrix(const Eigen::Isometry3s& T);
Eigen::Matrix6s dAdInvTMatrix(const Eigen::Isometry3s& T);

/// Adjoint mapping for dynamic size Jacobian
template <typename Derived>
typename Derived::PlainObject AdTJac(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  // Compute AdT column by column
  for (int i = 0; i < _J.cols(); ++i)
    ret.col(i) = AdT(_T, _J.col(i));

  return ret;
}

/// Adjoint mapping for fixed size Jacobian
template <typename Derived>
typename Derived::PlainObject AdTJacFixed(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  // Check if _J is fixed size Jacobian
  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);

  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  // Compute AdT
  ret.template topRows<3>().noalias() = _T.linear() * _J.template topRows<3>();
  ret.template bottomRows<3>().noalias()
      = -ret.template topRows<3>().colwise().cross(_T.translation())
        + _T.linear() * _J.template bottomRows<3>();

  return ret;
}

/// \brief Fast version of Ad([R 0; 0 1], V)
Eigen::Vector6s AdR(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _V);

/// \brief fast version of Ad(T, se3(w, 0))
Eigen::Vector6s AdTAngular(
    const Eigen::Isometry3s& _T, const Eigen::Vector3s& _w);

/// \brief fast version of Ad(T, se3(0, v))
Eigen::Vector6s AdTLinear(
    const Eigen::Isometry3s& _T, const Eigen::Vector3s& _v);

///// \brief fast version of Ad([I p; 0 1], V)
// se3 AdP(const Vec3& p, const se3& s);

/// \brief Change coordinate Frame of a Jacobian
template <typename Derived>
typename Derived::PlainObject AdRJac(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  ret.template topRows<3>().noalias() = _T.linear() * _J.template topRows<3>();

  ret.template bottomRows<3>().noalias()
      = _T.linear() * _J.template bottomRows<3>();

  return ret;
}

template <typename Derived>
typename Derived::PlainObject AdRInvJac(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  ret.template topRows<3>().noalias()
      = _T.linear().transpose() * _J.template topRows<3>();

  ret.template bottomRows<3>().noalias()
      = _T.linear().transpose() * _J.template bottomRows<3>();

  return ret;
}

template <typename Derived>
typename Derived::PlainObject adJac(
    const Eigen::Vector6s& _V, const Eigen::MatrixBase<Derived>& _J)
{
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  ret.template topRows<3>().noalias()
      = -_J.template topRows<3>().colwise().cross(_V.head<3>());

  ret.template bottomRows<3>().noalias()
      = -_J.template bottomRows<3>().colwise().cross(_V.head<3>())
        - _J.template topRows<3>().colwise().cross(_V.tail<3>());

  return ret;
}

/// \brief fast version of Ad(Inv(T), V)
Eigen::Vector6s AdInvT(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _V);

/// Adjoint mapping for dynamic size Jacobian
template <typename Derived>
typename Derived::PlainObject AdInvTJac(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  // Compute AdInvT column by column
  for (int i = 0; i < _J.cols(); ++i)
    ret.col(i) = AdInvT(_T, _J.col(i));

  return ret;
}

/// Adjoint mapping for fixed size Jacobian
template <typename Derived>
typename Derived::PlainObject AdInvTJacFixed(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  // Check if _J is fixed size Jacobian
  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);

  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  // Compute AdInvT
  ret.template topRows<3>().noalias()
      = _T.linear().transpose() * _J.template topRows<3>();
  ret.template bottomRows<3>().noalias()
      = _T.linear().transpose()
        * (_J.template bottomRows<3>()
           + _J.template topRows<3>().colwise().cross(_T.translation()));

  return ret;
}

///// \brief fast version of Ad(Inv(T), se3(Eigen_Vec3(0), v))
// Eigen::Vector3s AdInvTLinear(const Eigen::Isometry3s& T,
//                             const Eigen::Vector3s& v);

///// \brief fast version of Ad(Inv(T), se3(w, Eigen_Vec3(0)))
// Axis AdInvTAngular(const SE3& T, const Axis& w);

///// \brief Fast version of Ad(Inv([R 0; 0 1]), V)
// se3 AdInvR(const SE3& T, const se3& V);

/// \brief Fast version of Ad(Inv([R 0; 0 1]), se3(0, v))
Eigen::Vector6s AdInvRLinear(
    const Eigen::Isometry3s& _T, const Eigen::Vector3s& _v);

/// \brief dual adjoint mapping
/// \note @f$Ad^{@,*}_TF = ( R^T (m - p@times f)@,,~ R^T f)@f$,
/// where @f$T=(R,p)@in SE(3), F=(m,f)@in se(3)^*@f$.
Eigen::Vector6s dAdT(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _F);

///// \brief fast version of Ad(Inv(T), dse3(Eigen_Vec3(0), F))
// dse3 dAdTLinear(const SE3& T, const Vec3& F);

/// \brief fast version of dAd(Inv(T), F)
Eigen::Vector6s dAdInvT(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _F);

/// \brief fast version of dAd(Inv([R 0; 0 1]), F)
Eigen::Vector6s dAdInvR(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _F);

///// \brief fast version of dAd(Inv(SE3(p)), dse3(Eigen_Vec3(0), F))
// dse3 dAdInvPLinear(const Vec3& p, const Vec3& F);

/// \brief adjoint mapping
/// \note @f$ad_X Y = ( w_X @times w_Y@,,~w_X @times v_Y - w_Y @times v_X),@f$,
/// where @f$X=(w_X,v_X)@in se(3), @quad Y=(w_Y,v_Y)@in se(3) @f$.
Eigen::Vector6s ad(const Eigen::Vector6s& _X, const Eigen::Vector6s& _Y);

// TODO(JS): Rename and add documentation
Eigen::Matrix6s adMatrix(const Eigen::Vector6s& X);

/// \brief fast version of ad(se3(Eigen_Vec3(0), v), S)
// Vec3 ad_Vec3_se3(const Vec3& v, const se3& S);

/// \brief fast version of ad(se3(w, 0), se3(v, 0)) -> check
// Axis ad_Axis_Axis(const Axis& w, const Axis& v);

/// \brief dual adjoint mapping
/// \note @f$ad^{@,*}_V F = (m @times w + f @times v@,,~ f @times w),@f$
/// , where @f$F=(m,f)@in se^{@,*}(3), @quad V=(w,v)@in se(3) @f$.
Eigen::Vector6s dad(const Eigen::Vector6s& _s, const Eigen::Vector6s& _t);

/// \brief
Inertia transformInertia(const Eigen::Isometry3s& _T, const Inertia& _AI);

/// Use the Parallel Axis Theorem to compute the moment of inertia of a body
/// whose center of mass has been shifted from the origin
Eigen::Matrix3s parallelAxisTheorem(
    const Eigen::Matrix3s& _original,
    const Eigen::Vector3s& _comShift,
    s_t _mass);

enum AxisType
{
  AXIS_X = 0,
  AXIS_Y = 1,
  AXIS_Z = 2
};

/// Compute a rotation matrix from a vector. One axis of the rotated coordinates
/// by the rotation matrix matches the input axis where the axis is specified
/// by axisType.
Eigen::Matrix3s computeRotation(
    const Eigen::Vector3s& axis, AxisType axisType = AxisType::AXIS_X);

/// Compute a transform from a vector and a position. The rotation of the result
/// transform is computed by computeRotationMatrix(), and the translation is
/// just the input translation.
Eigen::Isometry3s computeTransform(
    const Eigen::Vector3s& axis,
    const Eigen::Vector3s& translation,
    AxisType axisType = AxisType::AXIS_X);

/// Generate frame given origin and z-axis
DART_DEPRECATED(6.0)
Eigen::Isometry3s getFrameOriginAxisZ(
    const Eigen::Vector3s& _origin, const Eigen::Vector3s& _axisZ);

/// \brief Check if determinant of _R is equat to 1 and all the elements are not
/// NaN values.
bool verifyRotation(const Eigen::Matrix3s& _R);

/// \brief Check if determinant of the rotational part of _T is equat to 1 and
/// all the elements are not NaN values.
bool verifyTransform(const Eigen::Isometry3s& _T);

/// Compute the angle (in the range of -pi to +pi) which ignores any full
/// rotations
#ifdef DART_USE_ARBITRARY_PRECISION
inline s_t wrapToPi(s_t angle)
{
  s_t pi = constantsd::pi();
  return fmod(angle + pi, 2 * pi) - pi;
}
#else
inline s_t wrapToPi(s_t angle)
{
  constexpr auto pi = constantsd::pi();

  return std::fmod(angle + pi, 2 * pi) - pi;
}
#endif

template <typename MatrixType, typename ReturnType>
void extractNullSpace(const Eigen::JacobiSVD<MatrixType>& _SVD, ReturnType& _NS)
{
  int rank = 0;
  // TODO(MXG): Replace this with _SVD.rank() once the latest Eigen is released
  if (_SVD.nonzeroSingularValues() > 0)
  {
    s_t thresh = max(
        _SVD.singularValues().coeff(0) * 1e-10,
        std::numeric_limits<s_t>::min());
    int i = _SVD.nonzeroSingularValues() - 1;
    while (i >= 0 && _SVD.singularValues().coeff(i) < thresh)
      --i;
    rank = i + 1;
  }

  int cols = _SVD.matrixV().cols(), rows = _SVD.matrixV().rows();
  _NS = _SVD.matrixV().block(0, rank, rows, cols - rank);
}

template <typename MatrixType, typename ReturnType>
void computeNullSpace(const MatrixType& _M, ReturnType& _NS)
{
  Eigen::JacobiSVD<MatrixType> svd(_M, Eigen::ComputeFullV);
  extractNullSpace(svd, _NS);
}

typedef std::vector<Eigen::Vector3s> SupportGeometry;

typedef common::aligned_vector<Eigen::Vector2s> SupportPolygon;

/// Project the support geometry points onto a plane with the given axes
/// and then compute their convex hull, which will take the form of a polgyon.
/// _axis1 and _axis2 must both have unit length for this function to work
/// correctly.
SupportPolygon computeSupportPolgyon(
    const SupportGeometry& _geometry,
    const Eigen::Vector3s& _axis1 = Eigen::Vector3s::UnitX(),
    const Eigen::Vector3s& _axis2 = Eigen::Vector3s::UnitY());

/// Same as computeSupportPolgyon, except you can pass in a
/// std::vector<std::size_t> which will have the same size as the returned
/// SupportPolygon, and each entry will contain the original index of each point
/// in the SupportPolygon
SupportPolygon computeSupportPolgyon(
    std::vector<std::size_t>& _originalIndices,
    const SupportGeometry& _geometry,
    const Eigen::Vector3s& _axis1 = Eigen::Vector3s::UnitX(),
    const Eigen::Vector3s& _axis2 = Eigen::Vector3s::UnitY());

/// Computes the convex hull of a set of 2D points
SupportPolygon computeConvexHull(const SupportPolygon& _points);

/// Computes the convex hull of a set of 2D points and fills in _originalIndices
/// with the original index of each entry in the returned SupportPolygon
SupportPolygon computeConvexHull(
    std::vector<std::size_t>& _originalIndices, const SupportPolygon& _points);

/// Compute the centroid of a polygon, assuming the polygon is a convex hull
Eigen::Vector2s computeCentroidOfHull(const SupportPolygon& _convexHull);

/// Intersection_t is returned by the computeIntersection() function to indicate
/// whether there was a valid intersection between the two line segments
enum IntersectionResult
{

  INTERSECTING = 0, ///< An intersection was found
  PARALLEL,         ///< The line segments are parallel
  BEYOND_ENDPOINTS  ///< There is no intersection because the end points do not
                    ///< expand far enough

};

/// Compute the intersection between a line segment that goes from a1 -> a2 and
/// a line segment that goes from b1 -> b2.
IntersectionResult computeIntersection(
    Eigen::Vector2s& _intersectionPoint,
    const Eigen::Vector2s& a1,
    const Eigen::Vector2s& a2,
    const Eigen::Vector2s& b1,
    const Eigen::Vector2s& b2);

/// Compute a 2D cross product
s_t cross(const Eigen::Vector2s& _v1, const Eigen::Vector2s& _v2);

/// Returns true if the point _p is inside the support polygon
bool isInsideSupportPolygon(
    const Eigen::Vector2s& _p,
    const SupportPolygon& _support,
    bool _includeEdge = true);

/// Returns the point which is closest to _p that also lays on the line segment
/// that goes from _s1 -> _s2
Eigen::Vector2s computeClosestPointOnLineSegment(
    const Eigen::Vector2s& _p,
    const Eigen::Vector2s& _s1,
    const Eigen::Vector2s& _s2);

/// Returns the point which is closest to _p that also lays on the edge of the
/// support polygon
Eigen::Vector2s computeClosestPointOnSupportPolygon(
    const Eigen::Vector2s& _p, const SupportPolygon& _support);

/// Same as closestPointOnSupportPolygon, but also fills in _index1 and _index2
/// with the indices of the line segment
Eigen::Vector2s computeClosestPointOnSupportPolygon(
    std::size_t& _index1,
    std::size_t& _index2,
    const Eigen::Vector2s& _p,
    const SupportPolygon& _support);

/// This computes and returns the closest point on a line, given by a point and
/// a direction, to a goal point
Eigen::Vector3s closestPointOnLine(
    const Eigen::Vector3s& pointOnLine,
    const Eigen::Vector3s& lineDirection,
    const Eigen::Vector3s& goalPoint);

/// This computes and returns the gradient of closestPointOnLine(), given the
/// gradients of its inputs
Eigen::Vector3s closestPointOnLineGradient(
    const Eigen::Vector3s& pointOnLine,
    const Eigen::Vector3s& pointOnLineGradient,
    const Eigen::Vector3s& lineDirection,
    const Eigen::Vector3s& lineDirectionGradient,
    const Eigen::Vector3s& goalPoint,
    const Eigen::Vector3s& goalPointGradient);

// Represents a bounding box with minimum and maximum coordinates.
class BoundingBox
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  BoundingBox();
  BoundingBox(const Eigen::Vector3s& min, const Eigen::Vector3s& max);

  inline const Eigen::Vector3s& getMin() const
  {
    return mMin;
  }
  inline const Eigen::Vector3s& getMax() const
  {
    return mMax;
  }

  inline void setMin(const Eigen::Vector3s& min)
  {
    mMin = min;
  }
  inline void setMax(const Eigen::Vector3s& max)
  {
    mMax = max;
  }

  // \brief Centroid of the bounding box (i.e average of min and max)
  inline Eigen::Vector3s computeCenter() const
  {
    return (mMax + mMin) * 0.5;
  }
  // \brief Coordinates of the maximum corner with respect to the centroid.
  inline Eigen::Vector3s computeHalfExtents() const
  {
    return (mMax - mMin) * 0.5;
  }
  // \brief Length of each of the sides of the bounding box.
  inline Eigen::Vector3s computeFullExtents() const
  {
    return (mMax - mMin);
  }

protected:
  // \brief minimum coordinates of the bounding box
  Eigen::Vector3s mMin;
  // \brief maximum coordinates of the bounding box
  Eigen::Vector3s mMax;
};

} // namespace math
} // namespace dart

#endif // DART_MATH_GEOMETRY_HPP_


#ifndef LBP_XCSLBP_HPP
#define LBP_XCSLBP_HPP

#include <lbp/defs.hpp>
#include <lbp/utils.hpp>
#include <lbp/detail/neighborhoods.hpp>
#include <lbp/detail/sampling.hpp>

#include <opencv2/core.hpp>

#include <boost/hana/fold.hpp>
#include <boost/hana/tuple.hpp>

#include <boost/integer.hpp>

//
// @inproceedings{silva2015extended,
//   title={An eXtended center-symmetric local binary pattern for background modeling and subtraction in videos},
//   author={Silva, Caroline and Bouwmans, Thierry and Fr{\'e}licot, Carl},
//   booktitle={International Joint Conference on Computer Vision, Imaging and Computer Graphics Theory and Applications, VISAPP 2015},
//   year={2015}
// }
//

namespace lbp {
namespace xcslbp_detail {

template< typename T >
auto xcslbp = [](auto N, auto S) {
    return [=](const cv::Mat& src, size_t i, size_t j, const T& epsilon) {
        using namespace cv;
        using namespace hana::literals;

        const auto c = S (src, i, j);

        return boost::hana::fold (
            N, 0, [&, shift = 0](auto accum, auto x) mutable {
                const auto a = S (src, i + x [0_c], j + x [1_c]);
                const auto b = S (src, i - x [0_c], j - x [1_c]);

                const auto g1 = a - b + c;
                const auto g2 = (a - c) * (b - c);

                return accum | (((g1 + g2) >= epsilon) << shift++);
            });
    };
};

} // namespace xcslbp_detail

template< typename T, size_t R, size_t P >
auto xcslbp = [](const cv::Mat& src, const T& epsilon = T { }) {
    using value_type = typename boost::uint_t< P/2 >::least;

    cv::Mat dst (src.size (), opencv_type< value_type >, cv::Scalar (0));

    auto op = xcslbp_detail::xcslbp< T > (
        detail::semicircular_neighborhood< R, P >,
        detail::nearest_sampler< T >);

#pragma omp parallel for
    for (size_t i = R; i < src.rows - R; ++i) {
        for (size_t j = R; j < src.cols - R; ++j) {
            dst.at< value_type > (i, j) = op (src, i, j, epsilon);
        }
    }

    return dst;
};

} // namespace lbp

#endif // LBP_XCSLBP_HPP


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  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)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>
using std::cout;  using std::endl;
#include <cerrno> // for ::errno

//[policy_eg_6

/*`
Suppose we want a set of distributions to behave as follows:

* Return infinity on overflow, rather than throwing an exception.
* Don't perform any promotion from double to long double internally.
* Return the closest integer result from the quantiles of discrete
distributions.

We'll begin by including the needed header for all the distributions:
*/

#include <boost/math/distributions.hpp>

/*`

Open up an appropriate namespace, calling it `my_distributions`,
for our distributions, and define the policy type we want.
Any policies we don't specify here will inherit the defaults:

*/

namespace my_distributions
{
  using namespace boost::math::policies;
  // using boost::math::policies::errno_on_error; // etc.

  typedef policy<
     // return infinity and set errno rather than throw:
     overflow_error<errno_on_error>,
     // Don't promote double -> long double internally:
     promote_double<false>,
     // Return the closest integer result for discrete quantiles:
     discrete_quantile<integer_round_nearest>
  > my_policy;

/*`

All we need do now is invoke the BOOST_MATH_DECLARE_DISTRIBUTIONS
macro passing the floating point type `double` and policy types `my_policy` as arguments:

*/

BOOST_MATH_DECLARE_DISTRIBUTIONS(double, my_policy)

} // close namespace my_namespace

/*`

We now have a set of typedefs defined in namespace my_distributions
that all look something like this:

``
typedef boost::math::normal_distribution<double, my_policy> normal;
typedef boost::math::cauchy_distribution<double, my_policy> cauchy;
typedef boost::math::gamma_distribution<double, my_policy> gamma;
// etc
``

So that when we use my_distributions::normal we really end up using
`boost::math::normal_distribution<double, my_policy>`:

*/

int main()
{
   // Construct distribution with something we know will overflow
  // (using double rather than if promoted to long double):
   my_distributions::normal norm(10, 2);

   errno = 0;
   cout << "Result of quantile(norm, 0) is: "
      << quantile(norm, 0) << endl; // -infinity.
   cout << "errno = " << errno << endl;
   errno = 0;
   cout << "Result of quantile(norm, 1) is: "
      << quantile(norm, 1) << endl; // +infinity.
   cout << "errno = " << errno << endl;

   // Now try a discrete distribution.
   my_distributions::binomial binom(20, 0.25);
   cout << "Result of quantile(binom, 0.05) is: "
      << quantile(binom, 0.05) << endl; // To check we get integer results.
   cout << "Result of quantile(complement(binom, 0.05)) is: "
      << quantile(complement(binom, 0.05)) << endl;
}

/*`

Which outputs:

[pre
Result of quantile(norm, 0) is: -1.#INF
errno = 34
Result of quantile(norm, 1) is: 1.#INF
errno = 34
Result of quantile(binom, 0.05) is: 1
Result of quantile(complement(binom, 0.05)) is: 8
]

This mechanism is particularly useful when we want to define a
project-wide policy, and don't want to modify the Boost source
or set  project wide build macros (possibly fragile and easy to forget).

*/
//] //[/policy_eg_6]


// Copyright Stephan T. Lavavej, http://nuwen.net .
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt .

#ifndef PHAM_ARITH_HH
#define PHAM_ARITH_HH

#include "compiler.hh"

#ifdef NUWEN_PLATFORM_MSVC
    #pragma once
#endif

// Based on the ANSI C Arithmetic Coding Library by Fred Wheeler,
// which was adapted from the program in "Arithmetic Coding For Data
// Compression", by Ian H. Witten, Radford M. Neal, and John G. Cleary,
// Communications Of The ACM, June 1987, Vol. 30, No. 6.

#include "typedef.hh"
#include "vector.hh"

#include "external_begin.hh"
    #include <boost/utility.hpp>
#include "external_end.hh"

namespace nuwen {
    inline vuc_t   arith(const vuc_t& v);
    inline vuc_t unarith(const vuc_t& v);
}

namespace pham {
    namespace arith {

// The triple of (CODE_VALUE_BITS, MAX_FREQUENCY, INCREMENT_VALUE) is deeply magic.
// It controls how much compression is achieved by BWT/MTF-2/ZLE/Arith.
// The CACM paper explains that when frequency counts are represented in F bits,
// code values are represented in C bits, and arithmetic is performed with
// P bits of precision, the following must hold: F <= C - 2 and F + C <= P.
// For sl_t arithmetic, P = 31, so F = 14 and C = 16 are possible.
// For ul_t arithmetic, P = 32, so F = 15 and C = 17 are possible.
// CODE_VALUE_BITS is C, while MAX_FREQUENCY may be as large as 2^F - 1.
// As MAX_FREQUENCY becomes larger, the arithmetic coder gains a more precise
// memory (its model of the frequencies becomes closer to the true, unscaled
// frequencies), but the arithmetic coder also becomes less adaptive, as
// rescalings are forced less frequently. Adaptiveness is good for compressing
// BWT/MTF-2/ZLE data, which consists of lengthy stretches of "boring" low bytes,
// interspersed with bursts of "exciting" high bytes. When the arithmetic coder
// adapts quickly, better compression is achieved. A generalization of the
// CACM model can achieve the best of both worlds: use the largest possible
// MAX_FREQUENCY to give the coder a good memory, but increment the frequency counts
// by more than 1. This forces more frequent rescaling.
// (16, 16383, 1): Used by the CACM code (as well as Fred Wheeler's). As they used
// sl_t arithmetic, this was "maximally" precise but not very adaptive.
// (16, 8192, 16): Suggested by Fenwick, this forces rescaling 32x more often,
// at the cost of precision.
// (16, 16383, 32): Used by the first libnuwen implementation, 1.0.22.0. Combines
// Fenwick's rescaling with CACM's precision.
// (17, 32767, 1): Used by libnuwen 1.0.23.0 through 1.0.28.0. I reread the CACM
// paper and realized that ul_t arithmetic allowed higher precision.
// (17, 32767, 64): Used by libnuwen 2.0.0.0 and beyond. Combines maximum precision
// with Fenwick's frequent rescaling.

// Here is how BWT/MTF-2/ZLE/Arith behaves on suall10.txt (9873495 bytes)
// with the various triples:
//     CACM (16, 16383,  1): 2073812 bytes, 1.680 bits/byte: The untuned baseline.
//  Fenwick (16,  8192, 16): 2050015 bytes, 1.661 bits/byte: A clear improvement.
// 1.0.22.0 (16, 16383, 32): 2033592 bytes, 1.648 bits/byte: Even better.
// 1.0.23.0 (17, 32767,  1): 2078591 bytes, 1.684 bits/byte: Crippling regression.
//  2.0.0.0 (17, 32767, 64): 2026462 bytes, 1.642 bits/byte: Best of all worlds.

        const nuwen::ul_t CODE_VALUE_BITS = 17;
        const nuwen::ul_t MAX_FREQUENCY   = 32767;
        const nuwen::ul_t INCREMENT_VALUE = 64;

        const nuwen::ul_t TOP_VALUE       = (1 << CODE_VALUE_BITS) - 1;
        const nuwen::ul_t FIRST_QTR       = TOP_VALUE / 4 + 1;
        const nuwen::ul_t HALF            = 2 * FIRST_QTR;
        const nuwen::ul_t THIRD_QTR       = 3 * FIRST_QTR;

        typedef nuwen::us_t symbol_t;

        const symbol_t    SENTINEL        = 256;
        const symbol_t    NUM_SYMBOLS     = 257;

        class model : public boost::noncopyable {
        public:
            model() {
                for (nuwen::ul_t i = 0; i < NUM_SYMBOLS; ++i) {
                    m_freq[i] = 1;
                }

                for (nuwen::ul_t i = 0; i < NUM_SYMBOLS + 1; ++i) {
                    m_cfreq[i] = NUM_SYMBOLS - i;
                }
            }

            nuwen::ul_t operator[](const symbol_t i) const {
                return m_cfreq[i];
            }

            void update(const symbol_t sym) {
                // The CACM code used a hardcoded increment value of 1 and wrote this test
                // (using current terminology) as m_cfreq[0] == MAX_FREQUENCY, which was correct.
                // libnuwen 1.0.28.0 and earlier used a generalized INCREMENT_VALUE and wrote this test
                // (using current terminology) as m_cfreq[0] >= MAX_FREQUENCY, which was WRONG.
                // The correct generalization is below; an invariant of the model is that
                // m_cfreq[0] is ALWAYS <= MAX_FREQUENCY. If we detect that adding INCREMENT_VALUE
                // will destroy this invariant, we must rescale so that the invariant will be
                // maintained.

                if (m_cfreq[0] + INCREMENT_VALUE > MAX_FREQUENCY) {
                    nuwen::ul_t cum = 0;
                    m_cfreq[NUM_SYMBOLS] = 0;

                    for (int i = NUM_SYMBOLS - 1; i >= 0; --i) {
                        m_freq[i] = (m_freq[i] + 1) / 2;
                        cum += m_freq[i];
                        m_cfreq[i] = cum;
                    }
                }

                m_freq[sym] += INCREMENT_VALUE;

                for (int i = 0; i < sym + 1; ++i) {
                    m_cfreq[i] += INCREMENT_VALUE;
                }
            }

        private:
            nuwen::ul_t m_freq[NUM_SYMBOLS];
            nuwen::ul_t m_cfreq[NUM_SYMBOLS + 1];
        };

        class encoder : public boost::noncopyable {
        public:
            encoder() : m_low(0), m_high(TOP_VALUE), m_fbits(0), m_out(), m_acm() { }

            void encode(const symbol_t sym) {
                const nuwen::ul_t range = m_high - m_low + 1;
                m_high = m_low + range * m_acm[sym] / m_acm[0] - 1;
                m_low  = m_low + range * m_acm[static_cast<symbol_t>(sym + 1)] / m_acm[0];

                while (true) {
                    if (m_high < HALF) {
                        bit_plus_follow(0);
                    } else if (m_low >= HALF) {
                        bit_plus_follow(1);
                        m_low  -= HALF;
                        m_high -= HALF;
                    } else if (m_low >= FIRST_QTR && m_high < THIRD_QTR) {
                        ++m_fbits;
                        m_low  -= FIRST_QTR;
                        m_high -= FIRST_QTR;
                    } else {
                        break;
                    }

                    m_low  = 2 * m_low;
                    m_high = 2 * m_high + 1;
                }

                m_acm.update(sym);
            }

            nuwen::vuc_t finalize() {
                ++m_fbits;

                bit_plus_follow(m_low >= FIRST_QTR);

                return m_out.vuc();
            }

        private:
            void bit_plus_follow(const bool bit) {
                m_out.push_back(bit);

                while (m_fbits > 0) {
                    m_out.push_back(!bit);
                    --m_fbits;
                }
            }

            nuwen::ul_t              m_low;
            nuwen::ul_t              m_high;
            nuwen::ul_t              m_fbits;
            nuwen::pack::packed_bits m_out;
            model                    m_acm;
        };

        class decoder : public boost::noncopyable {
        public:
            decoder(const nuwen::vuc_ci_t start, const nuwen::vuc_ci_t finish)
                : m_curr(start), m_shift(7), m_end(finish), m_value(0), m_low(0), m_high(TOP_VALUE), m_acm() {

                for (nuwen::ul_t i = 0; i < CODE_VALUE_BITS; ++i) {
                    m_value <<= 1;
                    m_value += input_bit();
                }
            }

            symbol_t decode() {
                const nuwen::ul_t range = m_high - m_low + 1;
                const nuwen::ul_t cum = ((m_value - m_low + 1) * m_acm[0] - 1) / range;

                symbol_t sym;

                for (sym = 0; m_acm[static_cast<symbol_t>(sym + 1)] > cum; ++sym) { }

                m_high = m_low + range * m_acm[sym] / m_acm[0] - 1;
                m_low  = m_low + range * m_acm[static_cast<symbol_t>(sym + 1)] / m_acm[0];

                while (true) {
                    if (m_high < HALF) {
                    } else if (m_low >= HALF) {
                        m_value -= HALF;
                        m_low   -= HALF;
                        m_high  -= HALF;
                    } else if (m_low >= FIRST_QTR && m_high < THIRD_QTR) {
                        m_value -= FIRST_QTR;
                        m_low   -= FIRST_QTR;
                        m_high  -= FIRST_QTR;
                    } else {
                        break;
                    }

                    m_low   = 2 * m_low;
                    m_high  = 2 * m_high + 1;
                    m_value = 2 * m_value + input_bit();
                }

                m_acm.update(sym);

                return sym;
            }

        private:
            bool input_bit() {
                if (m_curr != m_end) {
                    const bool ret = *m_curr >> m_shift & 1;

                    if (m_shift == 0) {
                        m_shift = 7;
                        ++m_curr;
                    } else {
                        --m_shift;
                    }

                    return ret;
                } else {
                    return 0;
                }
            }

            nuwen::vuc_ci_t m_curr;
            nuwen::uc_t     m_shift;
            nuwen::vuc_ci_t m_end;
            nuwen::ul_t     m_value;
            nuwen::ul_t     m_low;
            nuwen::ul_t     m_high;
            model           m_acm;
        };
    }
}

inline nuwen::vuc_t nuwen::arith(const vuc_t& v) {
    pham::arith::encoder ae;

    for (vuc_ci_t i = v.begin(); i != v.end(); ++i) {
        ae.encode(*i);
    }

    ae.encode(pham::arith::SENTINEL);

    return ae.finalize();
}

inline nuwen::vuc_t nuwen::unarith(const vuc_t& v) {
    pham::arith::decoder ad(v.begin(), v.end());

    vuc_t ret;

    while (true) {
        const pham::arith::symbol_t decoded = ad.decode();

        if (decoded != pham::arith::SENTINEL) {
            ret.push_back(static_cast<uc_t>(decoded));
        } else {
            return ret;
        }
    }
}

#endif // Idempotency


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/arch/common/detail/generic/tanh_kernel.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/tanh.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF( tanh_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        //////////////////////////////////////////////////////////////////////////////
        // if x = abs(a0) is less than 5/8 tanh is computed using a polynomial(float)
        // (respectively rational(double)) approx from cephes.
        // else
        // tanh(a0) is  sign(a0)*(1 - 2/(exp(2*x)+1))
        //////////////////////////////////////////////////////////////////////////////
        A0 x = bs::abs(a0);
        auto test0= is_less(x, Ratio<A0, 5, 8>());
        A0 bts = bitofsign(a0);
        std::size_t nb = nbtrue(test0);
        A0 z = One<A0>();
        if(nb > 0)
        {
          A0 x2 = sqr(x);
          z = detail::tanh_kernel<A0>::tanh(x, x2);
          if(nb >= A0::static_size) return  bitwise_xor(z, bts);
        }
        A0 r = fma(Mtwo<A0>(), rec(inc(exp(x+x))), One<A0>());
        return bitwise_xor(if_else(test0, z, r), bts);
      }
   };

} } }

#endif


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

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

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

#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/inv2pi.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pix2_1.hpp>
#include <boost/simd/constant/pix2_2.hpp>
#include <boost/simd/constant/pix2_3.hpp>
#include <boost/simd/constant/threeeps.hpp>
#include <boost/simd/constant/twopi.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/rem_pio2.hpp>
#include <boost/simd/function/rem_pio2_medium.hpp>
#include <boost/simd/function/round2even.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD (rem_2pi_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_ < bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0>;
      A0 xr;
      i_t n = rem_pio2(a0, xr);
      xr = xr+tofloat(n)*Pio_2<A0>();
      return if_else((xr > Pi<A0>()), xr-Twopi<A0>(), xr);
    }

  };

  BOOST_DISPATCH_OVERLOAD (rem_2pi_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_ <bd::floating_<A0> >
                          , bd::target_ <bd::unspecified_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A1 const&) const BOOST_NOEXCEPT
    {
      typedef typename A1::type selector;
      return rem2pi<selector, void>::rem(a0);
    }

    template < class T, class dummy = void> struct rem2pi
    {
      static BOOST_FORCEINLINE A0 rem( A0 const&, A0 &, A0&) BOOST_NOEXCEPT
      {
        BOOST_ASSERT_MSG(false, "wrong target for rem_2pi");
      }
    };
    template < class dummy> struct rem2pi < tag::big_tag, dummy>
    {
      static BOOST_FORCEINLINE A0 rem( A0 const& x) BOOST_NOEXCEPT
      {
        return rem_2pi(x);
      }
    };
    template < class dummy> struct rem2pi < tag::very_small_tag, dummy > // |a0| <2*pi
    {
      static BOOST_FORCEINLINE A0 rem( A0 const& x) BOOST_NOEXCEPT
      {
        return if_else(gt(x, Pi<A0>()), x-Twopi<A0>(),
                       if_else(lt(x, -Pi<A0>()), x+Twopi<A0>(), x));
      }
    };
    template < class dummy> struct rem2pi < tag::small_tag, dummy >// |a0| <= 20*pi
    {
      static BOOST_FORCEINLINE A0 rem( A0 const& x) BOOST_NOEXCEPT
      {
        A0 xi =  round2even(x*Inv2pi<A0>());
        A0 xr = x-xi*Pix2_1<A0>();
        xr -= xi*Pix2_2<A0>();
        xr -= xi*Pix2_3<A0>();
        return xr;
      }
    };

    template < class dummy> struct rem2pi < tag::medium_tag, dummy >
    {
      static BOOST_FORCEINLINE A0 rem( A0 const& x) BOOST_NOEXCEPT
      {
        using i_t = bd::as_integer_t<A0>;
        A0 xr;
        i_t n = rem_pio2_medium(x, xr);
        xr += tofloat(n)*Pio_2<A0>();
        xr = if_else(gt(xr, Pi<A0>()), xr-Twopi<A0>(), xr);
        return xr;
      }
    };
  };
} } }

#endif


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

  This source file is part of the Avogadro project.

  Copyright 2010 Eric C. Brown

  This source code is released under the New BSD License, (the "License").

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.

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

#include "qtaimcriticalpointlocator.h"
#include "qtaimlsodaintegrator.h"
#include "qtaimmathutilities.h"
#include "qtaimodeintegrator.h"
#include "qtaimwavefunction.h"

#include <Eigen/Core>

#include <QList>

#include <QtConcurrent/QtConcurrentMap>

#include <QDataStream>
#include <QDir>
#include <QFile>
#include <QTemporaryFile>

#include <QVariant>

#include <QFuture>
#include <QFutureWatcher>
#include <QProgressDialog>

using namespace std;
using namespace Eigen;

#define HUGE_REAL_NUMBER 1.e20
#define SMALL_GRADIENT_NORM 1.e-4

namespace Avogadro {
namespace QtPlugins {

QList<QVariant> QTAIMLocateNuclearCriticalPoint(QList<QVariant> input)
{
  const QString fileName = input.at(0).toString();
  const qint64 nucleus = input.at(1).toInt();
  const QVector3D x0y0z0(input.at(2).toReal(), input.at(3).toReal(),
                         input.at(4).toReal());

  QTAIMWavefunction wfn;
  wfn.loadFromBinaryFile(fileName);

  QTAIMWavefunctionEvaluator eval(wfn);

  QVector3D result;

  if (wfn.nuclearCharge(nucleus) < 4) {
    //      QTAIMODEIntegrator
    //      ode(eval,QTAIMODEIntegrator::CMBPMinusThreeGradientInElectronDensity);
    QTAIMLSODAIntegrator ode(
      eval, QTAIMLSODAIntegrator::CMBPMinusThreeGradientInElectronDensity);
    result = ode.integrate(x0y0z0);
  } else {
    result = x0y0z0;
  }

  bool correctSignature;
  Matrix<qreal, 3, 1> xyz;
  xyz << result.x(), result.y(), result.z();

  if (QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
        eval.hessianOfElectronDensity(xyz)) == -3) {
    correctSignature = true;
  } else {
    correctSignature = false;
  }

  QList<QVariant> value;

  if (correctSignature) {
    value.append(correctSignature);
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
  } else {
    value.append(false);
  }

  return value;
}

QList<QVariant> QTAIMLocateBondCriticalPoint(QList<QVariant> input)
{

  QList<QVariant> value;
  value.clear();

  const QString wfnFileName = input.at(0).toString();
  const QString nuclearCriticalPointsFileName = input.at(1).toString();
  const qint64 nucleusA = input.at(2).toInt();
  const qint64 nucleusB = input.at(3).toInt();
  const QVector3D x0y0z0(input.at(4).toReal(), input.at(5).toReal(),
                         input.at(6).toReal());

  QTAIMWavefunction wfn;
  wfn.loadFromBinaryFile(wfnFileName);

  QList<QVector3D> nuclearCriticalPoints;
  QFile nuclearCriticalPointsFile(nuclearCriticalPointsFileName);
  nuclearCriticalPointsFile.open(QIODevice::ReadOnly);
  QDataStream nuclearCriticalPointsFileIn(&nuclearCriticalPointsFile);
  nuclearCriticalPointsFileIn >> nuclearCriticalPoints;
  nuclearCriticalPointsFile.close();

  QList<QPair<QVector3D, qreal>> betaSpheres;
  for (qint64 i = 0; i < nuclearCriticalPoints.length(); ++i) {
    QPair<QVector3D, qreal> thisBetaSphere;
    thisBetaSphere.first = nuclearCriticalPoints.at(i);
    thisBetaSphere.second = 0.1;
    betaSpheres.append(thisBetaSphere);
  }

  QTAIMWavefunctionEvaluator eval(wfn);

  QList<QVector3D> ncpList;

  QVector3D result;
  //    QTAIMODEIntegrator
  //    ode(eval,QTAIMODEIntegrator::CMBPMinusOneGradientInElectronDensity);
  QTAIMLSODAIntegrator ode(
    eval, QTAIMLSODAIntegrator::CMBPMinusOneGradientInElectronDensity);
  result = ode.integrate(x0y0z0);
  Matrix<qreal, 3, 1> xyz;
  xyz << result.x(), result.y(), result.z();

  if (!(QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
          eval.hessianOfElectronDensity(xyz)) == -1) ||
      (eval.gradientOfElectronDensity(xyz)).norm() > SMALL_GRADIENT_NORM) {
    value.append(false);
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
    return value;
  }

  Matrix<qreal, 3, 3> eigenvectorsOfHessian;
  eigenvectorsOfHessian =
    QTAIMMathUtilities::eigenvectorsOfASymmetricThreeByThreeMatrix(
      eval.hessianOfElectronDensity(xyz));
  Matrix<qreal, 3, 1> highestEigenvectorOfHessian;
  highestEigenvectorOfHessian << eigenvectorsOfHessian(0, 2),
    eigenvectorsOfHessian(1, 2), eigenvectorsOfHessian(2, 2);

  const qreal smallStep = 0.01;

  QVector3D forwardStartingPoint(
    result.x() + smallStep * highestEigenvectorOfHessian(0),
    result.y() + smallStep * highestEigenvectorOfHessian(1),
    result.z() + smallStep * highestEigenvectorOfHessian(2));

  QVector3D backwardStartingPoint(
    result.x() - smallStep * highestEigenvectorOfHessian(0),
    result.y() - smallStep * highestEigenvectorOfHessian(1),
    result.z() - smallStep * highestEigenvectorOfHessian(2));

  //    QTAIMODEIntegrator
  //    forwardODE(eval,QTAIMODEIntegrator::SteepestAscentPathInElectronDensity);
  QTAIMLSODAIntegrator forwardODE(
    eval, QTAIMLSODAIntegrator::SteepestAscentPathInElectronDensity);
  forwardODE.setBetaSpheres(betaSpheres);
  QVector3D forwardEndpoint = forwardODE.integrate(forwardStartingPoint);
  QList<QVector3D> forwardPath = forwardODE.path();

  //    QTAIMODEIntegrator
  //    backwardODE(eval,QTAIMODEIntegrator::SteepestAscentPathInElectronDensity);
  QTAIMLSODAIntegrator backwardODE(
    eval, QTAIMLSODAIntegrator::SteepestAscentPathInElectronDensity);
  backwardODE.setBetaSpheres(betaSpheres);
  QVector3D backwardEndpoint = backwardODE.integrate(backwardStartingPoint);
  QList<QVector3D> backwardPath = backwardODE.path();

  qreal smallestDistance = HUGE_REAL_NUMBER;
  qint64 smallestDistanceIndex = 0;

  for (qint64 n = 0; n < wfn.numberOfNuclei(); ++n) {
    Matrix<qreal, 3, 1> a(forwardEndpoint.x(), forwardEndpoint.y(),
                          forwardEndpoint.z());
    Matrix<qreal, 3, 1> b(wfn.xNuclearCoordinate(n), wfn.yNuclearCoordinate(n),
                          wfn.zNuclearCoordinate(n));

    qreal distance = QTAIMMathUtilities::distance(a, b);

    if (distance < smallestDistance) {
      smallestDistance = distance;
      smallestDistanceIndex = n;
    }
  }
  qint64 forwardNucleusIndex = smallestDistanceIndex;

  smallestDistance = HUGE_REAL_NUMBER;
  smallestDistanceIndex = 0;

  for (qint64 n = 0; n < wfn.numberOfNuclei(); ++n) {
    Matrix<qreal, 3, 1> a(backwardEndpoint.x(), backwardEndpoint.y(),
                          backwardEndpoint.z());
    Matrix<qreal, 3, 1> b(wfn.xNuclearCoordinate(n), wfn.yNuclearCoordinate(n),
                          wfn.zNuclearCoordinate(n));

    qreal distance = QTAIMMathUtilities::distance(a, b);

    if (distance < smallestDistance) {
      smallestDistance = distance;
      smallestDistanceIndex = n;
    }
  }
  qint64 backwardNucleusIndex = smallestDistanceIndex;

  bool bondPathConnectsPair;
  if ((forwardNucleusIndex == nucleusA && backwardNucleusIndex == nucleusB) ||
      (forwardNucleusIndex == nucleusB && backwardNucleusIndex == nucleusA)) {
    bondPathConnectsPair = true;
  } else {
    bondPathConnectsPair = false;
  }

  if (bondPathConnectsPair) {
    value.append(true);
    value.append(nucleusA);
    value.append(nucleusB);
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
    Matrix<qreal, 3, 1> xyz_;
    xyz_ << result.x(), result.y(), result.z();
    value.append(eval.laplacianOfElectronDensity(xyz_));
    value.append(QTAIMMathUtilities::ellipticityOfASymmetricThreeByThreeMatrix(
      eval.hessianOfElectronDensity(xyz_)));
    value.append(1 + forwardPath.length() + 1 + backwardPath.length() + 1);
    value.append(forwardEndpoint.x());
    for (qint64 i = forwardPath.length() - 1; i >= 0; --i) {
      value.append(forwardPath.at(i).x());
    }
    value.append(result.x());
    for (qint64 i = 0; i < backwardPath.length(); ++i) {
      value.append(backwardPath.at(i).x());
    }
    value.append(backwardEndpoint.x());
    value.append(forwardEndpoint.y());
    for (qint64 i = forwardPath.length() - 1; i >= 0; --i) {
      value.append(forwardPath.at(i).y());
    }
    value.append(result.y());
    for (qint64 i = 0; i < backwardPath.length(); ++i) {
      value.append(backwardPath.at(i).y());
    }
    value.append(backwardEndpoint.y());
    value.append(forwardEndpoint.z());
    for (qint64 i = forwardPath.length() - 1; i >= 0; --i) {
      value.append(forwardPath.at(i).z());
    }
    value.append(result.z());
    for (qint64 i = 0; i < backwardPath.length(); ++i) {
      value.append(backwardPath.at(i).z());
    }
    value.append(backwardEndpoint.z());

  } else {
    value.append(false);
    // for debugging
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
  }

  return value;
}

QList<QVariant> QTAIMLocateElectronDensitySink(QList<QVariant> input)
{
  qint64 counter = 0;
  const QString fileName = input.at(counter).toString();
  counter++;
  //    const qint64 nucleus=input.at(counter).toInt(); counter++
  qreal x0 = input.at(counter).toReal();
  counter++;
  qreal y0 = input.at(counter).toReal();
  counter++;
  qreal z0 = input.at(counter).toReal();
  counter++;

  const QVector3D x0y0z0(x0, y0, z0);

  QTAIMWavefunction wfn;
  wfn.loadFromBinaryFile(fileName);

  QTAIMWavefunctionEvaluator eval(wfn);

  bool correctSignature;
  QVector3D result;

  Matrix<qreal, 3, 1> xyz;
  xyz << x0, y0, z0;
  if (eval.electronDensity(xyz) < 1.e-1) {
    correctSignature = false;
  } else {
    //      QTAIMODEIntegrator
    //      ode(eval,QTAIMODEIntegrator::CMBPMinusThreeGradientInElectronDensityLaplacian);
    QTAIMLSODAIntegrator ode(
      eval,
      QTAIMLSODAIntegrator::CMBPMinusThreeGradientInElectronDensityLaplacian);
    result = ode.integrate(x0y0z0);

    Matrix<qreal, 3, 1> xyz_;
    xyz_ << result.x(), result.y(), result.z();

    if (eval.electronDensity(xyz_) > 1.e-1 &&
        eval.gradientOfElectronDensityLaplacian(xyz_).norm() < 1.e-3) {
      if (QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
            eval.hessianOfElectronDensityLaplacian(xyz_)) == -3) {
        correctSignature = true;
      } else {
        correctSignature = false;
      }
    } else {
      correctSignature = false;
    }
  }

  QList<QVariant> value;
  if (correctSignature) {
    value.append(correctSignature);
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
  } else {
    value.append(false);
  }

  return value;
}

QList<QVariant> QTAIMLocateElectronDensitySource(QList<QVariant> input)
{
  qint64 counter = 0;
  const QString fileName = input.at(counter).toString();
  counter++;
  //    const qint64 nucleus=input.at(counter).toInt(); counter++
  qreal x0 = input.at(counter).toReal();
  counter++;
  qreal y0 = input.at(counter).toReal();
  counter++;
  qreal z0 = input.at(counter).toReal();
  counter++;

  const QVector3D x0y0z0(x0, y0, z0);

  QTAIMWavefunction wfn;
  wfn.loadFromBinaryFile(fileName);

  QTAIMWavefunctionEvaluator eval(wfn);

  bool correctSignature;
  QVector3D result;

  Matrix<qreal, 3, 1> xyz;
  xyz << x0, y0, z0;
  if (eval.electronDensity(xyz) < 1.e-1) {
    correctSignature = false;
  } else {
    //      QTAIMODEIntegrator
    //      ode(eval,QTAIMODEIntegrator::CMBPPlusThreeGradientInElectronDensityLaplacian);
    QTAIMLSODAIntegrator ode(
      eval,
      QTAIMLSODAIntegrator::CMBPPlusThreeGradientInElectronDensityLaplacian);
    result = ode.integrate(x0y0z0);

    Matrix<qreal, 3, 1> xyz_;
    xyz_ << result.x(), result.y(), result.z();

    if (eval.electronDensity(xyz_) > 1.e-1 &&
        eval.gradientOfElectronDensityLaplacian(xyz_).norm() < 1.e-3) {
      if (QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
            eval.hessianOfElectronDensityLaplacian(xyz_)) == 3) {
        correctSignature = true;
      } else {
        correctSignature = false;
      }
    } else {
      correctSignature = false;
    }
  }

  QList<QVariant> value;
  if (correctSignature) {
    value.append(correctSignature);
    value.append(result.x());
    value.append(result.y());
    value.append(result.z());
  } else {
    value.append(false);
  }

  return value;
}

QTAIMCriticalPointLocator::QTAIMCriticalPointLocator(QTAIMWavefunction& wfn)
{
  m_wfn = &wfn;

  m_nuclearCriticalPoints.empty();
  m_bondCriticalPoints.empty();
  m_ringCriticalPoints.empty();
  m_cageCriticalPoints.empty();

  m_laplacianAtBondCriticalPoints.empty();
  m_ellipticityAtBondCriticalPoints.empty();

  m_bondPaths.empty();
  m_bondedAtoms.empty();

  m_electronDensitySources.empty();
  m_electronDensitySinks.empty();
}

void QTAIMCriticalPointLocator::locateNuclearCriticalPoints()
{

  QString tempFileName = QTAIMCriticalPointLocator::temporaryFileName();

  QList<QList<QVariant>> inputList;

  const qint64 numberOfNuclei = m_wfn->numberOfNuclei();

  for (qint64 n = 0; n < numberOfNuclei; ++n) {
    QList<QVariant> input;
    input.append(tempFileName);
    input.append(n);
    input.append(m_wfn->xNuclearCoordinate(n));
    input.append(m_wfn->yNuclearCoordinate(n));
    input.append(m_wfn->zNuclearCoordinate(n));

    inputList.append(input);
  }

  m_wfn->saveToBinaryFile(tempFileName);

  QProgressDialog dialog;
  dialog.setWindowTitle("QTAIM");
  dialog.setLabelText(QString("Nuclear Critical Points Search"));

  QFutureWatcher<void> futureWatcher;
  QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
  QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
  QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int, int)),
                   &dialog, SLOT(setRange(int, int)));
  QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog,
                   SLOT(setValue(int)));

  QFuture<QList<QVariant>> future =
    QtConcurrent::mapped(inputList, QTAIMLocateNuclearCriticalPoint);
  futureWatcher.setFuture(future);
  dialog.exec();
  futureWatcher.waitForFinished();

  QList<QList<QVariant>> results;
  if (futureWatcher.future().isCanceled()) {
    results.clear();
  } else {
    results = future.results();
  }

  QFile file;
  file.remove(tempFileName);

  for (qint64 n = 0; n < results.length(); ++n) {

    bool correctSignature = results.at(n).at(0).toBool();

    if (correctSignature) {

      QVector3D result(results.at(n).at(1).toReal(),
                       results.at(n).at(2).toReal(),
                       results.at(n).at(3).toReal());

      m_nuclearCriticalPoints.append(result);
    }
  }
}

void QTAIMCriticalPointLocator::locateBondCriticalPoints()
{

  if (m_nuclearCriticalPoints.length() < 1) {
    return;
  }

  const qint64 numberOfNuclei = m_wfn->numberOfNuclei();

  if (numberOfNuclei < 2) {
    return;
  }

  QString tempFileName = QTAIMCriticalPointLocator::temporaryFileName();

  QString nuclearCriticalPointsFileName =
    QTAIMCriticalPointLocator::temporaryFileName();
  QFile nuclearCriticalPointsFile(nuclearCriticalPointsFileName);
  nuclearCriticalPointsFile.open(QIODevice::WriteOnly);
  QDataStream nuclearCriticalPointsOut(&nuclearCriticalPointsFile);
  nuclearCriticalPointsOut << m_nuclearCriticalPoints;
  nuclearCriticalPointsFile.close();

  QList<QList<QVariant>> inputList;

  for (qint64 M = 0; M < numberOfNuclei - 1; ++M) {
    for (qint64 N = M + 1; N < numberOfNuclei; ++N) {

      const qreal distanceCutoff = 8.0;

      Matrix<qreal, 3, 1> a;
      Matrix<qreal, 3, 1> b;

      a << m_wfn->xNuclearCoordinate(M), m_wfn->yNuclearCoordinate(M),
        m_wfn->zNuclearCoordinate(M);
      b << m_wfn->xNuclearCoordinate(N), m_wfn->yNuclearCoordinate(N),
        m_wfn->zNuclearCoordinate(N);

      if (QTAIMMathUtilities::distance(a, b) < distanceCutoff) {
        QVector3D x0y0z0(
          (m_wfn->xNuclearCoordinate(M) + m_wfn->xNuclearCoordinate(N)) / 2.0,
          (m_wfn->yNuclearCoordinate(M) + m_wfn->yNuclearCoordinate(N)) / 2.0,
          (m_wfn->zNuclearCoordinate(M) + m_wfn->zNuclearCoordinate(N)) / 2.0);

        QList<QVariant> input;
        input.append(tempFileName);
        input.append(nuclearCriticalPointsFileName);
        input.append(M);
        input.append(N);
        input.append(x0y0z0.x());
        input.append(x0y0z0.y());
        input.append(x0y0z0.z());

        inputList.append(input);
      }
    } // end N
  }   // end M

  m_wfn->saveToBinaryFile(tempFileName);

  QProgressDialog dialog;
  dialog.setWindowTitle("QTAIM");
  dialog.setLabelText(QString("Bond Critical Points Search"));

  QFutureWatcher<void> futureWatcher;
  QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
  QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
  QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int, int)),
                   &dialog, SLOT(setRange(int, int)));
  QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog,
                   SLOT(setValue(int)));

  QFuture<QList<QVariant>> future =
    QtConcurrent::mapped(inputList, QTAIMLocateBondCriticalPoint);
  ;
  futureWatcher.setFuture(future);
  dialog.exec();
  futureWatcher.waitForFinished();

  QList<QList<QVariant>> results;
  if (futureWatcher.future().isCanceled()) {
    results.clear();
  } else {
    results = future.results();
  }

  QFile file;
  file.remove(tempFileName);
  file.remove(nuclearCriticalPointsFileName);

  for (qint64 i = 0; i < results.length(); ++i) {
    QList<QVariant> thisCriticalPoint = results.at(i);

    bool success = thisCriticalPoint.at(0).toBool();

    if (success) {
      QPair<qint64, qint64> bondedAtoms_;
      bondedAtoms_.first = thisCriticalPoint.at(1).toInt();
      bondedAtoms_.second = thisCriticalPoint.at(2).toInt();
      m_bondedAtoms.append(bondedAtoms_);

      QVector3D coordinates(thisCriticalPoint.at(3).toReal(),
                            thisCriticalPoint.at(4).toReal(),
                            thisCriticalPoint.at(5).toReal());

      m_bondCriticalPoints.append(coordinates);

      m_laplacianAtBondCriticalPoints.append(thisCriticalPoint.at(6).toReal());
      m_ellipticityAtBondCriticalPoints.append(
        thisCriticalPoint.at(7).toReal());
      qint64 pathLength = thisCriticalPoint.at(8).toInt();

      QList<QVector3D> bondPath;
      for (qint64 j = 0; j < pathLength; ++j) {
        QVector3D pathPoint(
          thisCriticalPoint.at(9 + j).toReal(),
          thisCriticalPoint.at(9 + j + pathLength).toReal(),
          thisCriticalPoint.at(9 + j + 2 * pathLength).toReal());

        bondPath.append(pathPoint);
      }

      m_bondPaths.append(bondPath);
    }
  }
}

void QTAIMCriticalPointLocator::locateElectronDensitySources()
{

  QString tempFileName = QTAIMCriticalPointLocator::temporaryFileName();

  QList<QList<QVariant>> inputList;

  qreal xmin, ymin, zmin;
  qreal xmax, ymax, zmax;
  qreal xstep, ystep, zstep;

  // TODO: if only we were using Eigen data structures...
  QList<qreal> xNuclearCoordinates;
  QList<qreal> yNuclearCoordinates;
  QList<qreal> zNuclearCoordinates;

  for (qint64 i = 0; i < m_wfn->numberOfNuclei(); ++i) {
    xNuclearCoordinates.append(m_wfn->xNuclearCoordinate(i));
    yNuclearCoordinates.append(m_wfn->yNuclearCoordinate(i));
    zNuclearCoordinates.append(m_wfn->zNuclearCoordinate(i));
  }

  xmin = xNuclearCoordinates.first();
  xmax = xNuclearCoordinates.first();
  for (qint64 i = 1; i < m_wfn->numberOfNuclei(); ++i) {
    if (xNuclearCoordinates.at(i) < xmin) {
      xmin = xNuclearCoordinates.at(i);
    }
    if (xNuclearCoordinates.at(i) > xmax) {
      xmax = xNuclearCoordinates.at(i);
    }
  }

  ymin = yNuclearCoordinates.first();
  ymax = yNuclearCoordinates.first();
  for (qint64 i = 1; i < yNuclearCoordinates.count(); ++i) {
    if (yNuclearCoordinates.at(i) < ymin) {
      ymin = yNuclearCoordinates.at(i);
    }
    if (yNuclearCoordinates.at(i) > ymax) {
      ymax = yNuclearCoordinates.at(i);
    }
  }

  zmin = zNuclearCoordinates.first();
  zmax = zNuclearCoordinates.first();
  for (qint64 i = 1; i < zNuclearCoordinates.count(); ++i) {
    if (zNuclearCoordinates.at(i) < zmin) {
      zmin = zNuclearCoordinates.at(i);
    }
    if (zNuclearCoordinates.at(i) > zmax) {
      zmax = zNuclearCoordinates.at(i);
    }
  }

  xmin = -2.0 + xmin;
  ymin = -2.0 + ymin;
  zmin = -2.0 + zmin;

  xmax = 2.0 + xmax;
  ymax = 2.0 + ymax;
  zmax = 2.0 + zmax;

  xstep = ystep = zstep = 0.5;

  for (qreal x = xmin; x < xmax + xstep; x = x + xstep) {
    for (qreal y = ymin; y < ymax + ystep; y = y + ystep) {
      for (qreal z = zmin; z < zmax + zstep; z = z + zstep) {
        QList<QVariant> input;
        input.append(tempFileName);
        //          input.append( n );
        input.append(x);
        input.append(y);
        input.append(z);

        inputList.append(input);
      }
    }
  }

  m_wfn->saveToBinaryFile(tempFileName);

  QProgressDialog dialog;
  dialog.setWindowTitle("QTAIM");
  dialog.setLabelText(QString("Electron Density Sources Search"));

  QFutureWatcher<void> futureWatcher;
  QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
  QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
  QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int, int)),
                   &dialog, SLOT(setRange(int, int)));
  QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog,
                   SLOT(setValue(int)));

  QFuture<QList<QVariant>> future =
    QtConcurrent::mapped(inputList, QTAIMLocateElectronDensitySource);
  futureWatcher.setFuture(future);
  dialog.exec();
  futureWatcher.waitForFinished();

  QList<QList<QVariant>> results;
  if (futureWatcher.future().isCanceled()) {
    results.clear();
  } else {
    results = future.results();
  }

  QFile file;
  file.remove(tempFileName);

  for (qint64 n = 0; n < results.length(); ++n) {

    qint64 counter = 0;
    bool correctSignature = results.at(n).at(counter).toBool();
    counter++;

    if (correctSignature) {
      qreal x = results.at(n).at(counter).toReal();
      counter++;
      qreal y = results.at(n).at(counter).toReal();
      counter++;
      qreal z = results.at(n).at(counter).toReal();
      counter++;

      if ((xmin < x && x < xmax) && (ymin < y && y < ymax) &&
          (zmin < z && z < zmax)) {
        QVector3D result(x, y, z);

        qreal smallestDistance = HUGE_REAL_NUMBER;

        for (qint64 i = 0; i < m_electronDensitySources.length(); ++i) {

          Matrix<qreal, 3, 1> a(x, y, z);
          Matrix<qreal, 3, 1> b(m_electronDensitySources.at(i).x(),
                                m_electronDensitySources.at(i).y(),
                                m_electronDensitySources.at(i).z());

          qreal distance = QTAIMMathUtilities::distance(a, b);

          if (distance < smallestDistance) {
            smallestDistance = distance;
          }
        }

        if (smallestDistance > 1.e-2) {
          m_electronDensitySources.append(result);
        }
      }
    }
  }
  //    qDebug() << "SOURCES" << m_electronDensitySources;
}

void QTAIMCriticalPointLocator::locateElectronDensitySinks()
{

  QString tempFileName = QTAIMCriticalPointLocator::temporaryFileName();

  QList<QList<QVariant>> inputList;

  qreal xmin, ymin, zmin;
  qreal xmax, ymax, zmax;
  qreal xstep, ystep, zstep;

  // TODO: if only we were using Eigen data structures...
  QList<qreal> xNuclearCoordinates;
  QList<qreal> yNuclearCoordinates;
  QList<qreal> zNuclearCoordinates;

  for (qint64 i = 0; i < m_wfn->numberOfNuclei(); ++i) {
    xNuclearCoordinates.append(m_wfn->xNuclearCoordinate(i));
    yNuclearCoordinates.append(m_wfn->yNuclearCoordinate(i));
    zNuclearCoordinates.append(m_wfn->zNuclearCoordinate(i));
  }

  xmin = xNuclearCoordinates.first();
  xmax = xNuclearCoordinates.first();
  for (qint64 i = 1; i < m_wfn->numberOfNuclei(); ++i) {
    if (xNuclearCoordinates.at(i) < xmin) {
      xmin = xNuclearCoordinates.at(i);
    }
    if (xNuclearCoordinates.at(i) > xmax) {
      xmax = xNuclearCoordinates.at(i);
    }
  }

  ymin = yNuclearCoordinates.first();
  ymax = yNuclearCoordinates.first();
  for (qint64 i = 1; i < yNuclearCoordinates.count(); ++i) {
    if (yNuclearCoordinates.at(i) < ymin) {
      ymin = yNuclearCoordinates.at(i);
    }
    if (yNuclearCoordinates.at(i) > ymax) {
      ymax = yNuclearCoordinates.at(i);
    }
  }

  zmin = zNuclearCoordinates.first();
  zmax = zNuclearCoordinates.first();
  for (qint64 i = 1; i < zNuclearCoordinates.count(); ++i) {
    if (zNuclearCoordinates.at(i) < zmin) {
      zmin = zNuclearCoordinates.at(i);
    }
    if (zNuclearCoordinates.at(i) > zmax) {
      zmax = zNuclearCoordinates.at(i);
    }
  }

  xmin = -2.0 + xmin;
  ymin = -2.0 + ymin;
  zmin = -2.0 + zmin;

  xmax = 2.0 + xmax;
  ymax = 2.0 + ymax;
  zmax = 2.0 + zmax;

  xstep = ystep = zstep = 0.5;

  for (qreal x = xmin; x < xmax + xstep; x = x + xstep) {
    for (qreal y = ymin; y < ymax + ystep; y = y + ystep) {
      for (qreal z = zmin; z < zmax + zstep; z = z + zstep) {
        QList<QVariant> input;
        input.append(tempFileName);
        //          input.append( n );
        input.append(x);
        input.append(y);
        input.append(z);

        inputList.append(input);
      }
    }
  }

  m_wfn->saveToBinaryFile(tempFileName);

  QProgressDialog dialog;
  dialog.setWindowTitle("QTAIM");
  dialog.setLabelText(QString("Electron Density Sinks Search"));

  QFutureWatcher<void> futureWatcher;
  QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
  QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
  QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int, int)),
                   &dialog, SLOT(setRange(int, int)));
  QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog,
                   SLOT(setValue(int)));

  QFuture<QList<QVariant>> future =
    QtConcurrent::mapped(inputList, QTAIMLocateElectronDensitySink);
  futureWatcher.setFuture(future);
  dialog.exec();
  futureWatcher.waitForFinished();

  QList<QList<QVariant>> results;
  if (futureWatcher.future().isCanceled()) {
    results.clear();
  } else {
    results = future.results();
  }

  QFile file;
  file.remove(tempFileName);

  for (qint64 n = 0; n < results.length(); ++n) {

    qint64 counter = 0;
    bool correctSignature = results.at(n).at(counter).toBool();
    counter++;

    if (correctSignature) {
      qreal x = results.at(n).at(counter).toReal();
      counter++;
      qreal y = results.at(n).at(counter).toReal();
      counter++;
      qreal z = results.at(n).at(counter).toReal();
      counter++;

      if ((xmin < x && x < xmax) && (ymin < y && y < ymax) &&
          (zmin < z && z < zmax)) {
        QVector3D result(x, y, z);

        qreal smallestDistance = HUGE_REAL_NUMBER;

        for (qint64 i = 0; i < m_electronDensitySinks.length(); ++i) {

          Matrix<qreal, 3, 1> a(x, y, z);
          Matrix<qreal, 3, 1> b(m_electronDensitySinks.at(i).x(),
                                m_electronDensitySinks.at(i).y(),
                                m_electronDensitySinks.at(i).z());

          qreal distance = QTAIMMathUtilities::distance(a, b);

          if (distance < smallestDistance) {
            smallestDistance = distance;
          }
        }

        if (smallestDistance > 1.e-2) {
          m_electronDensitySinks.append(result);
        }
      }
    }
  }
  //    qDebug() << "SINKS" << m_electronDensitySinks;
}

QString QTAIMCriticalPointLocator::temporaryFileName()
{
  QTemporaryFile temporaryFile;
  temporaryFile.open();
  QString tempFileName = temporaryFile.fileName();
  temporaryFile.close();
  temporaryFile.remove();

  // wait for temporary file to be deleted
  QDir dir;
  do {
    // Nothing
  } while (dir.exists(tempFileName));

  return tempFileName;
}

} // namespace QtPlugins
} // namespace Avogadro