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

#ifndef CAMERA_MODEL_UTILS_HPP
#define CAMERA_MODEL_UTILS_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <unsupported/Eigen/AutoDiff>

#include <sophus/so2.hpp>
#include <sophus/se2.hpp>
#include <sophus/so3.hpp>
#include <sophus/se3.hpp>

// For future Eigen + CUDA
#ifndef EIGEN_DEVICE_FUNC
#define EIGEN_DEVICE_FUNC
#endif // EIGEN_DEVICE_FUNC

// If Cereal serializer is preferred
#ifdef CAMERA_MODELS_SERIALIZER_CEREAL
#define CAMERA_MODELS_HAVE_SERIALIZER
#define CAMERA_MODELS_SERIALIZE(ARCHIVE,NAME,VAR) ARCHIVE(cereal::make_nvp(NAME,VAR))
#endif // CAMERA_MODELS_SERIALIZER_CEREAL

// If Boost serializer is preferred
#ifdef CAMERA_MODELS_SERIALIZER_BOOST
#define CAMERA_MODELS_HAVE_SERIALIZER
#define CAMERA_MODELS_SERIALIZE(ARCHIVE,NAME,VAR) ARCHIVE & boost::serialization::make_nvp(NAME,VAR)
#endif // CAMERA_MODELS_SERIALIZER_BOOST

#ifndef VISIONCORE_EIGEN_MISSING_BITS_HPP
#define VISIONCORE_EIGEN_MISSING_BITS_HPP
namespace Eigen
{

namespace numext
{

template<typename T>
EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
T atan2(const T& y, const T &x) {
  EIGEN_USING_STD_MATH(atan2);
  return atan2(y,x);
}

#ifdef __CUDACC__
template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
float atan2(const float& y, const float &x) { return ::atan2f(y,x); }

template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
double atan2(const double& y, const double &x) { return ::atan2(y,x); }
#endif

}

template<typename DerType>
inline const Eigen::AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all<DerType>::type, typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar, product)>
atan(const Eigen::AutoDiffScalar<DerType>& x)
{
  using namespace Eigen;
  EIGEN_UNUSED typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar;
  using numext::atan;
  return Eigen::MakeAutoDiffScalar(atan(x.value()),x.derivatives() * ( Scalar(1) / (Scalar(1) + x.value() * x.value()) ));
}

}
#endif // VISIONCORE_EIGEN_MISSING_BITS_HPP

namespace cammod
{

/**
 * Camera Models
 */
enum class CameraModelType
{
    PinholeDistance = 0,
    PinholeDistanceDistorted,
    PinholeDisparity,
    PinholeDisparityDistorted,
    Generic,
    GenericDistorted,
    Spherical,
    SphericalPovRay,
    Fisheye,
    FisheyeDistorted,
    PinholeDisparityBrownConrady
};

template<CameraModelType cmt>
struct CameraModelToTypeAndName;

/**
 * Collection of common 2D/3D types.
 */
template<typename T>
struct ComplexTypes
{
    typedef Sophus::SO3<T> RotationT;
    typedef Sophus::SE3<T> TransformT;
    typedef Eigen::Map<TransformT> TransformMapT;
    typedef Eigen::Map<const TransformT> ConstTransformMapT;
    typedef Eigen::Map<RotationT> RotationMapT;
    typedef Eigen::Map<const RotationT> ConstRotationMapT;

    typedef Sophus::SO2<T> Rotation2DT;
    typedef Sophus::SE2<T> Transform2DT;
    typedef Eigen::Map<Transform2DT> Transform2DMapT;
    typedef Eigen::Map<const Transform2DT> ConstTransform2DMapT;
    typedef Eigen::Map<Rotation2DT> Rotation2DMapT;
    typedef Eigen::Map<const Rotation2DT> ConstRotation2DMapT;

    typedef typename Sophus::SE3<T>::Tangent TangentTransformT;
    typedef Eigen::Map<typename Sophus::SE3<T>::Tangent> TangentTransformMapT;
    typedef Eigen::Map<const typename Sophus::SE3<T>::Tangent> ConstTangentTransformMapT;

    typedef typename Sophus::SE2<T>::Tangent TangentTransform2DT;
    typedef Eigen::Map<typename Sophus::SE2<T>::Tangent> TangentTransform2DMapT;
    typedef Eigen::Map<const typename Sophus::SE2<T>::Tangent> ConstTangentTransform2DMapT;

    typedef typename Sophus::SO3<T>::Tangent TangentRotationT;
    typedef Eigen::Map<typename Sophus::SO3<T>::Tangent> TangentRotationMapT;
    typedef Eigen::Map<const typename Sophus::SO3<T>::Tangent> ConstTangentRotationMapT;

    typedef typename Sophus::SO2<T>::Tangent TangentRotation2DT;
    typedef Eigen::Map<typename Sophus::SO2<T>::Tangent> TangentRotation2DMapT;
    typedef Eigen::Map<const typename Sophus::SO2<T>::Tangent> ConstTangentRotation2DMapT;

    typedef Eigen::Matrix<T,2,1> PixelT;
    typedef Eigen::Map<PixelT> PixelMapT;
    typedef Eigen::Map<const PixelT> ConstPixelMapT;

    typedef typename TransformT::Point PointT;
    typedef Eigen::Map<PointT> PointMapT;
    typedef Eigen::Map<const PointT> ConstPointMapT;

    typedef typename Transform2DT::Point Point2DT;
    typedef Eigen::Map<Point2DT> Point2DMapT;
    typedef Eigen::Map<const Point2DT> ConstPoint2DMapT;

    typedef Eigen::Quaternion<T> QuaternionT;
    typedef Eigen::Map<QuaternionT> QuaternionMapT;
    typedef Eigen::Map<const QuaternionT> ConstQuaternionMapT;

    typedef Eigen::Matrix<T,2,3> ForwardPointJacobianT;
    typedef Eigen::Map<ForwardPointJacobianT> ForwardPointJacobianMapT;
    typedef Eigen::Map<const ForwardPointJacobianT> ConstForwardPointJacobianMapT;

    typedef Eigen::Matrix<T,3,2> InversePointJacobianT;
    typedef Eigen::Map<InversePointJacobianT> InversePointJacobianMapT;
    typedef Eigen::Map<const InversePointJacobianT> ConstInversePointJacobianMapT;

    typedef Eigen::Matrix<T,2,2> DistortionJacobianT;
    typedef Eigen::Map<DistortionJacobianT> DistortionJacobianMapT;
    typedef Eigen::Map<const DistortionJacobianT> ConstDistortionJacobianMapT;

    template<int ParametersToOptimize>
    using ForwardParametersJacobianT = Eigen::Matrix<T,2,ParametersToOptimize>;
    template<int ParametersToOptimize>
    using ForwardParametersJacobianMapT = Eigen::Map<ForwardParametersJacobianT<ParametersToOptimize>>;
    template<int ParametersToOptimize>
    using ConstForwardParametersJacobianMapT = Eigen::Map<const ForwardParametersJacobianT<ParametersToOptimize>>;
};

template<typename T>
EIGEN_DEVICE_FUNC static inline T getFieldOfView(T focal, T width)
{
    using Eigen::numext::atan;
    return T(2.0) * atan(width / (T(2.0) * focal));
}

template<typename T>
EIGEN_DEVICE_FUNC static inline T getFocalLength(T fov, T width)
{
    using Eigen::numext::tan;
    return width / (T(2.0) * tan(fov / T(2.0)));
}

/**
 * Runtime polymorphic interface if one prefers that.
 */
template<typename T>
class CameraInterface
{
public:
    virtual ~CameraInterface() { }

    virtual CameraModelType getModelType() const = 0;
    virtual const char* getModelName() const = 0;
    virtual bool pointValid(const typename ComplexTypes<T>::PointT& tmp_pt) const = 0;
    virtual bool pixelValid(T x, T y) const = 0;
    virtual bool pixelValidSquare(T x, T y) const = 0;
    virtual bool pixelValidSquare(const typename ComplexTypes<T>::PixelT& pt) const = 0;
    virtual bool pixelValidCircular(T x, T y) const = 0;
    virtual bool pixelValidCircular(const typename ComplexTypes<T>::PixelT& pt) const = 0;
    virtual typename ComplexTypes<T>::PixelT forward(const typename ComplexTypes<T>::PointT& tmp_pt) const = 0;
    virtual typename ComplexTypes<T>::PointT inverse(T x, T y) const = 0;
    virtual typename ComplexTypes<T>::PixelT forward(const typename ComplexTypes<T>::TransformT& pose,
                                                     const typename ComplexTypes<T>::PointT& pt) const = 0;
    virtual typename ComplexTypes<T>::PixelT forward(const typename ComplexTypes<T>::RotationT& pose,
                                                    const typename ComplexTypes<T>::PointT& pt) const = 0;
    virtual typename ComplexTypes<T>::PointT inverse(const typename ComplexTypes<T>::PixelT& pix) const = 0;
    virtual typename ComplexTypes<T>::PointT inverseAtDistance(const typename ComplexTypes<T>::PixelT& pix, T dist) const = 0;
    virtual typename ComplexTypes<T>::PointT inverseAtDistance(T x, T y, T dist) const = 0;
    virtual typename ComplexTypes<T>::PointT inverseAtDistance(const typename ComplexTypes<T>::TransformT& pose,
                                                               const typename ComplexTypes<T>::PixelT& pix, T dist) const = 0;
    virtual typename ComplexTypes<T>::PointT inverseAtDistance(const typename ComplexTypes<T>::TransformT& pose,
                                                               T x, T y, T dist) const = 0;
    virtual typename ComplexTypes<T>::PixelT twoFrameProject(const typename ComplexTypes<T>::TransformT& pose1,
                                                             const typename ComplexTypes<T>::PixelT& pix, T dist,
                                                             const typename ComplexTypes<T>::TransformT& pose2) const = 0;
    virtual typename ComplexTypes<T>::PixelT twoFrameProject(const typename ComplexTypes<T>::TransformT& pose1,
                                                             T x, T y, T dist, const typename ComplexTypes<T>::TransformT& pose2) const = 0;
};

/**
 * Functions / methods shared by all the camera models.
 */
template<typename Derived>
class CameraFunctions
{
    static constexpr unsigned int ParametersToOptimize = Eigen::internal::traits<Derived>::ParametersToOptimize;
    static constexpr bool HasForwardPointJacobian = Eigen::internal::traits<Derived>::HasForwardPointJacobian;
    static constexpr bool HasForwardParametersJacobian = Eigen::internal::traits<Derived>::HasForwardParametersJacobian;
    static constexpr bool HasInversePointJacobian = Eigen::internal::traits<Derived>::HasInversePointJacobian;
    static constexpr bool HasInverseParametersJacobian = Eigen::internal::traits<Derived>::HasInverseParametersJacobian;
public:
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;

    // ------------------- statics ---------------------
    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT worldToCamera(const typename ComplexTypes<T>::TransformT& pose, const typename ComplexTypes<T>::PointT& pt)
    {
        return pose.inverse() * pt; // why!
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT cameraToWorld(const typename ComplexTypes<T>::TransformT& pose, const typename ComplexTypes<T>::PointT& pt)
    {
        return pose * pt; // why!
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT worldToCamera(const typename ComplexTypes<T>::RotationT& pose, const typename ComplexTypes<T>::PointT& pt)
    {
        return pose.inverse() * pt; // why!
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT cameraToWorld(const typename ComplexTypes<T>::RotationT& pose, const typename ComplexTypes<T>::PointT& pt)
    {
        return pose * pt; // why!
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool pixelValid(const Derived& ccd, const typename ComplexTypes<T>::PixelT& pt)
    {
        return Derived::template pixelValidSquare<T>(ccd, pt(0), pt(1));
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool pixelValidCircular(const Derived& ccd, const typename ComplexTypes<T>::PixelT& pt)
    {
        return Derived::template pixelValidCircular<T>(ccd, pt(0), pt(1));
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool pixelValidSquare(const Derived& ccd, const typename ComplexTypes<T>::PixelT& pt)
    {
        return Derived::template pixelValidSquare<T>(ccd, pt(0), pt(1));
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resizeViewport(Derived& ccd, const typename ComplexTypes<T>::PixelT& pt)
    {
        Derived::template resizeViewport<T>(ccd, pt(0), pt(1));
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PixelT forward(const Derived& ccd,
                                                                                const typename ComplexTypes<T>::TransformT& pose,
                                                                                const typename ComplexTypes<T>::PointT& pt)
    {
        return Derived::template forward<T>(ccd, worldToCamera<T>(pose, pt));
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PixelT forward(const Derived& ccd,
                                                                                const typename ComplexTypes<T>::RotationT& pose,
                                                                                const typename ComplexTypes<T>::PointT& pt)
    {
        return Derived::template forward<T>(ccd, worldToCamera<T>(pose, pt));
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverse(const Derived& ccd,
                                                                                const typename ComplexTypes<T>::PixelT& pix)
    {
        return Derived::template inverse<T>(ccd, pix(0), pix(1));
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverseAtDistance(const Derived& ccd, const typename ComplexTypes<T>::PixelT& pix, T dist)
    {
        return Derived::template inverse<T>(ccd, pix(0), pix(1)) * dist;
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverseAtDistance(const Derived& ccd, T x, T y, T dist)
    {
        return Derived::template inverse<T>(ccd, x, y) * dist;
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverseAtDistance(const Derived& ccd,
                                                                                const typename ComplexTypes<T>::TransformT& pose,
                                                                                const typename ComplexTypes<T>::PixelT& pix, T dist)
    {
        return cameraToWorld<T>(pose, ((Derived::template inverse<T>(ccd,pix)) * dist));
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverseAtDistance(const Derived& ccd,
                                                                                const typename ComplexTypes<T>::TransformT& pose,
                                                                                T x, T y, T dist)
    {
        return cameraToWorld<T>(pose, ((Derived::template inverse<T>(ccd,x,y)) * dist));
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PixelT twoFrameProject(const Derived& ccd,
                                                                                const typename ComplexTypes<T>::TransformT& pose1,
                                                                                const typename ComplexTypes<T>::PixelT& pix,
                                                                                T dist,
                                                                                const typename ComplexTypes<T>::TransformT& pose2)
    {
        return forward<T>(ccd, pose2, inverseAtDistance<T>(ccd, pose1, pix, dist));
    }

    template<typename T = Scalar>
    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PixelT twoFrameProject(const Derived& ccd,
                                                                                const typename ComplexTypes<T>::TransformT& pose1,
                                                                                T x, T y, T dist,
                                                                                const typename ComplexTypes<T>::TransformT& pose2)
    {
        return forward<T>(ccd, pose2, inverseAtDistance<T>(ccd, pose1, x, y, dist));
    }

    // ------------------- non statics ---------------------

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resizeViewport(T x, T y)
    {
        Derived::template resizeViewport<T>(*static_cast<Derived*>(this), x, y);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resizeViewport(const typename ComplexTypes<T>::PixelT& pt)
    {
        Derived::template resizeViewport<T>(*static_cast<Derived*>(this), pt(0), pt(1));
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool pointValid(const typename ComplexTypes<T>::PointT& pt) const
    {
        return Derived::template pointValid<T>(*static_cast<const Derived*>(this), pt);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool pixelValid(T x, T y) const
    {
        return Derived::template pixelValidSquare<T>(*static_cast<const Derived*>(this), x, y);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool pixelValidSquare(T x, T y) const
    {
        return Derived::template pixelValidSquare<T>(*static_cast<const Derived*>(this), x, y);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool pixelValidSquare(const typename ComplexTypes<T>::PixelT& pt) const
    {
        return Derived::template pixelValidSquare<T>(*static_cast<const Derived*>(this), pt(0), pt(1));
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool pixelValidCircular(T x, T y) const
    {
        return Derived::template pixelValidCircular<T>(*static_cast<const Derived*>(this), x, y);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool pixelValidCircular(const typename ComplexTypes<T>::PixelT& pt) const
    {
        return Derived::template pixelValidCircular<T>(*static_cast<const Derived*>(this), pt(0), pt(1));
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PixelT forward(const typename ComplexTypes<T>::PointT& tmp_pt) const
    {
        return Derived::template forward<T>(*static_cast<const Derived*>(this), tmp_pt);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverse(T x, T y) const
    {
        return Derived::template inverse<T>(*static_cast<const Derived*>(this), x, y);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PixelT forward(const typename ComplexTypes<T>::TransformT& pose, const typename ComplexTypes<T>::PointT& pt) const
    {
        return CameraFunctions::forward<T>(*static_cast<const Derived*>(this), pose, pt);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PixelT forward(const typename ComplexTypes<T>::RotationT& pose, const typename ComplexTypes<T>::PointT& pt) const
    {
        return CameraFunctions::forward<T>(*static_cast<const Derived*>(this), pose, pt);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename std::enable_if<HasForwardPointJacobian, typename ComplexTypes<T>::ForwardPointJacobianT>::type forwardPointJacobian(const typename ComplexTypes<T>::PointT& pt) const
    {
        return Derived::template forwardPointJacobian<T>(*static_cast<const Derived*>(this), pt);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename std::enable_if<HasForwardParametersJacobian, typename ComplexTypes<T>::template ForwardParametersJacobianT<ParametersToOptimize> >::type forwardParametersJacobian(const typename ComplexTypes<T>::PointT& pt) const
    {
        return Derived::template forwardParametersJacobian<T>(*static_cast<const Derived*>(this), pt);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverse(const typename ComplexTypes<T>::PixelT& pix) const
    {
        return CameraFunctions::inverse<T>(*static_cast<const Derived*>(this), pix);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverseAtDistance(const typename ComplexTypes<T>::PixelT& pix, T dist) const
    {
        return CameraFunctions::inverseAtDistance<T>(*static_cast<const Derived*>(this), pix, dist);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverseAtDistance(T x, T y, T dist) const
    {
        return CameraFunctions::inverseAtDistance<T>(*static_cast<const Derived*>(this), x, y, dist);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverseAtDistance(const typename ComplexTypes<T>::TransformT& pose,
                                                                                const typename ComplexTypes<T>::PixelT& pix, T dist) const
    {
        return CameraFunctions::inverseAtDistance<T>(*static_cast<const Derived*>(this), pose, pix, dist);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PointT inverseAtDistance(const typename ComplexTypes<T>::TransformT& pose, T x, T y, T dist) const
    {
        return CameraFunctions::inverseAtDistance<T>(*static_cast<const Derived*>(this), pose, x, y, dist);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PixelT twoFrameProject(const typename ComplexTypes<T>::TransformT& pose1, const typename ComplexTypes<T>::PixelT& pix, T dist,
                                                                                const typename ComplexTypes<T>::TransformT& pose2) const
    {
        return CameraFunctions::twoFrameProject<T>(*static_cast<const Derived*>(this), pose1, pix, dist, pose2);
    }

    template<typename T = Scalar>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename ComplexTypes<T>::PixelT twoFrameProject(const typename ComplexTypes<T>::TransformT& pose1, T x, T y, T dist,
                                                                                const typename ComplexTypes<T>::TransformT& pose2) const
    {
        return CameraFunctions::twoFrameProject<T>(*static_cast<const Derived*>(this), pose1, x, y, dist, pose2);
    }
};

/**
 * Wraps templated typed camera model into a polymorphic class.
 */
template <typename ModelT>
class CameraFromCRTP : public ModelT, public CameraInterface<typename ModelT::Scalar>
{
public:
    typedef ModelT Derived;
    typedef typename Derived::Scalar Scalar;

    CameraFromCRTP() = default;
    CameraFromCRTP(const Derived& d) : Derived(d) { }
    CameraFromCRTP(const CameraFromCRTP& other) = default;

    virtual ~CameraFromCRTP() { }

    virtual CameraModelType getModelType() const
    {
        return Derived::ModelType;
    }

    virtual const char* getModelName() const
    {
        return CameraModelToTypeAndName<Derived::ModelType>::Name;
    }

    virtual bool pointValid(const typename ComplexTypes<Scalar>::PointT& tmp_pt) const
    {
        return Derived::template pointValid<Scalar>(tmp_pt);
    }

    virtual bool pixelValid(Scalar x, Scalar y) const
    {
        return Derived::template pixelValid<Scalar>(x,y);
    }

    virtual bool pixelValidSquare(Scalar x, Scalar y) const
    {
        return Derived::template pixelValidSquare<Scalar>(x,y);
    }

    virtual bool pixelValidSquare(const typename ComplexTypes<Scalar>::PixelT& pt) const
    {
        return Derived::template pixelValidSquare<Scalar>(pt);
    }

    virtual bool pixelValidCircular(Scalar x, Scalar y) const
    {
        return Derived::template pixelValidCircular<Scalar>(x,y);
    }

    virtual bool pixelValidCircular(const typename ComplexTypes<Scalar>::PixelT& pt) const
    {
        return Derived::template pixelValidCircular<Scalar>(pt);
    }

    virtual typename ComplexTypes<Scalar>::PixelT forward(const typename ComplexTypes<Scalar>::PointT& tmp_pt) const
    {
        return Derived::template forward<Scalar>(tmp_pt);
    }

    virtual typename ComplexTypes<Scalar>::PointT inverse(Scalar x, Scalar y) const
    {
        return Derived::template inverse<Scalar>(x,y);
    }

    virtual typename ComplexTypes<Scalar>::PixelT forward(const typename ComplexTypes<Scalar>::TransformT& pose,
                                                          const typename ComplexTypes<Scalar>::PointT& pt) const
    {
        return Derived::template forward<Scalar>(pose, pt);
    }

    virtual typename ComplexTypes<Scalar>::PixelT forward(const typename ComplexTypes<Scalar>::RotationT& pose,
                                                          const typename ComplexTypes<Scalar>::PointT& pt) const
    {
        return Derived::template forward<Scalar>(pose, pt);
    }

    virtual typename ComplexTypes<Scalar>::PointT inverse(const typename ComplexTypes<Scalar>::PixelT& pix) const
    {
        return Derived::template inverse<Scalar>(pix);
    }

    virtual typename ComplexTypes<Scalar>::PointT inverseAtDistance(const typename ComplexTypes<Scalar>::PixelT& pix,
                                                                    Scalar dist) const
    {
        return Derived::template inverseAtDistance<Scalar>(pix,dist);
    }

    virtual typename ComplexTypes<Scalar>::PointT inverseAtDistance(Scalar x, Scalar y, Scalar dist) const
    {
        return Derived::template inverseAtDistance<Scalar>(x,y,dist);
    }

    virtual typename ComplexTypes<Scalar>::PointT inverseAtDistance(const typename ComplexTypes<Scalar>::TransformT& pose,
                                                                    const typename ComplexTypes<Scalar>::PixelT& pix,
                                                                    Scalar dist) const
    {
        return Derived::template inverseAtDistance<Scalar>(pose,pix,dist);
    }

    virtual typename ComplexTypes<Scalar>::PointT inverseAtDistance(const typename ComplexTypes<Scalar>::TransformT& pose,
                                                                    Scalar x, Scalar y, Scalar dist) const
    {
        return Derived::template inverseAtDistance<Scalar>(pose,x,y,dist);
    }

    virtual typename ComplexTypes<Scalar>::PixelT twoFrameProject(const typename ComplexTypes<Scalar>::TransformT& pose1,
                                                                  const typename ComplexTypes<Scalar>::PixelT& pix,
                                                                  Scalar dist,
                                                                  const typename ComplexTypes<Scalar>::TransformT& pose2) const
    {
        return Derived::template twoFrameProject<Scalar>(pose1,pix,dist,pose2);
    }

    virtual typename ComplexTypes<Scalar>::PixelT twoFrameProject(const typename ComplexTypes<Scalar>::TransformT& pose1,
                                                                  Scalar x, Scalar y, Scalar dist,
                                                                  const typename ComplexTypes<Scalar>::TransformT& pose2) const
    {
        return Derived::template twoFrameProject<Scalar>(pose1,x,y,dist,pose2);
    }
};

}

#endif // CAMERA_MODEL_UTILS_HPP


// Copyright (C) 2018 Thejaka Amila Kanewala, Marcin Zalewski, Andrew Lumsdaine.

// Boost Software License - Version 1.0 - August 17th, 2003

// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:

// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.

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

//  Authors: Thejaka Kanewala
//           Andrew Lumsdaine

#ifndef __AGM_SSSP_HPP
#define __AGM_SSSP_HPP

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/iteration_macros.hpp>

#include <boost/graph/agm/util/stat.hpp>
#include <boost/graph/agm/model/agm.hpp>
#include <boost/graph/agm/runtime/runtime.hpp>
#include <boost/graph/agm/model/general_orderings.hpp>

#define VISITED_THRESHOLD 100

namespace boost { namespace graph { namespace agm {

// definition of the SSSP work item set
typedef uint32_t Distance;
typedef uint32_t Weight;

template<typename Graph>
class sssp_family {

  typedef typename boost::graph_traits < Graph >::vertex_descriptor Vertex;

  // work item set definition
  // Every work item must have the time
  // destination, source, distance
  typedef std::tuple<Vertex, Distance> WorkItem;

  // For level ordering, redifining the work item
  typedef int Level;
  // work item set definition
  // Every work item must have the time
  // destination, distance
  typedef std::tuple<Vertex, Distance, Level> LevelWorkItem;

  //===================================================================================
  // Pair of processing functions : post order pf and pre-order pf
  //===================================================================================
  // the processing function
  template<typename WeightState,
           typename DistanceState>
  struct post_order_sssp_pf {

  public:
    post_order_sssp_pf(const Graph& _rg,
                       WeightState& _vweight,
                       DistanceState& _dist,
                       agm_work_stats& _sr) : g(_rg),
                                              vdistance(_dist),
                                              vweight(_vweight),
                                              stats(_sr){}

  private:
    const Graph& g;
    DistanceState& vdistance;
    WeightState& vweight;
    agm_work_stats& stats;

  public:
    template<typename outset>
    void operator()(const WorkItem& w,
                    int tid,
                    outset& out) {
      Vertex v = std::get<0>(w);
      Distance d = std::get<1>(w);

      Distance vd;
      __atomic_load(&vdistance[v], &vd, __ATOMIC_SEQ_CST);
      if (vd == d) {
        BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
          Vertex u = boost::target(e, g);
          Distance we = boost::get(vweight, e);
#ifdef PBGL2_PRINT_WORK_STATS
          stats.increment_edges(tid);
#endif
          WorkItem generated(u, (d+we));
          out.push(generated, tid);
        }
      }
#ifdef PBGL2_PRINT_WORK_STATS
      else {
        stats.increment_invalidated_cancels(tid);
      }
#endif
    }

    // for level work items
    template<typename outset>
    void operator()(const LevelWorkItem& w,
                    int tid,
                    outset& out) {
      Vertex v = std::get<0>(w);
      Distance d = std::get<1>(w);
      Level level = std::get<2>(w);

      if (vdistance[v] == d) {
        BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
          Vertex u = boost::target(e, g);
          Distance we = boost::get(vweight, e);
#ifdef PBGL2_PRINT_WORK_STATS
          stats.increment_edges(tid);
#endif
          LevelWorkItem generated(u, (d+we), (level+1));
          out.push(generated, tid);
        }
      }
#ifdef PBGL2_PRINT_WORK_STATS
      else {
        stats.increment_invalidated_cancels(tid);
      }
#endif
    }
  };

  // the pre-order processing function
  template<typename WeightState,
           typename DistanceState>
  struct pre_order_sssp_pf {

  public:
    pre_order_sssp_pf(const Graph& _rg,
            WeightState& _weight,
            DistanceState& _dist,
            agm_work_stats& _sr) : g(_rg),
                                   vweight(_weight),
                                   vdistance(_dist),
                                   stats(_sr){}

  private:
    const Graph& g;
    WeightState& vweight;
    DistanceState& vdistance;
    agm_work_stats& stats;

  public:

    template<typename work_item, typename buckets>
    void operator()(const work_item& wi,
                    int tid,
		    buckets& outset) {
      Vertex v = std::get<0>(wi);
      Distance d = std::get<1>(wi);

      Distance old_dist = vdistance[v], last_old_dist;

      while (d < old_dist) {
        last_old_dist = old_dist;
        old_dist = boost::parallel::val_compare_and_swap(&vdistance[v], old_dist, d);
        if (last_old_dist == old_dist) {
#ifdef PBGL2_PRINT_WORK_STATS
          if (old_dist < std::numeric_limits<weight_type>::max()) {
            stats.increment_invalidated(tid);
          } else
            stats.increment_useful(tid);
#endif
          outset.push(wi, tid);
          return;
        }
      }

#ifdef PBGL2_PRINT_WORK_STATS
      stats.increment_rejected(tid);
#endif
    }
  };

  //=====================================================================
  // The general processing function that can be used as the
  // pre-order processing function or post-order processing
  // function.
  //=====================================================================
  // the processing function
  template<typename WeightState,
           typename DistanceState>
  struct sssp_pf {

  public:
    sssp_pf(const Graph& _rg,
            WeightState& _weight,
            DistanceState& _dist,
            agm_work_stats& _sr) : g(_rg),
                                   vweight(_weight),
                                   vdistance(_dist),
                                   stats(_sr){}

  private:
    const Graph& g;
    WeightState& vweight;
    DistanceState& vdistance;
    agm_work_stats& stats;

  public:

    template<typename buckets>
    void operator()(const WorkItem& wi,
                    int tid,
		    buckets& outset) {

      Vertex v = std::get<0>(wi);
      Distance d = std::get<1>(wi);

      Distance old_dist = vdistance[v], last_old_dist;

      while (d < old_dist) {
        last_old_dist = old_dist;
        old_dist = boost::parallel::val_compare_and_swap(&vdistance[v], old_dist, d);
        if (last_old_dist == old_dist) {
#ifdef PBGL2_PRINT_WORK_STATS
          if (old_dist < std::numeric_limits<weight_type>::max()) {
            stats.increment_invalidated(tid);
          } else
            stats.increment_useful(tid);
#endif

          BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
            Vertex u = boost::target(e, g);
            Distance we = boost::get(vweight, e);
#ifdef PBGL2_PRINT_WORK_STATS
            stats.increment_edges(tid);
#endif
            WorkItem generated(u, (d+we));
            outset.push(generated, tid);
          }

          return;
        }
      }

#ifdef PBGL2_PRINT_WORK_STATS
      stats.increment_rejected(tid);
#endif
    }

    // For level ordering
    template<typename buckets>
    void operator()(const LevelWorkItem& wi,
                    int tid,
		    buckets& outset) {

      Vertex v = std::get<0>(wi);
      Distance d = std::get<1>(wi);
      Level l = std::get<2>(wi);

      Distance old_dist = vdistance[v], last_old_dist;

      while (d < old_dist) {
        last_old_dist = old_dist;
        old_dist = boost::parallel::val_compare_and_swap(&vdistance[v], old_dist, d);
        if (last_old_dist == old_dist) {
#ifdef PBGL2_PRINT_WORK_STATS
          if (old_dist < std::numeric_limits<weight_type>::max()) {
            stats.increment_invalidated(tid);
          } else
            stats.increment_useful(tid);
#endif

          BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
            Vertex u = boost::target(e, g);
            Distance we = boost::get(vweight, e);
#ifdef PBGL2_PRINT_WORK_STATS
            stats.increment_edges(tid);
#endif
            LevelWorkItem generated(u, (d+we), (l+1));
            outset.push(generated, tid);
          }

          return;
        }
      }

#ifdef PBGL2_PRINT_WORK_STATS
      stats.increment_rejected(tid);
#endif
    }
  };

public:

  template <typename DistMap, typename WeightMap>
  bool verify(const Graph& g, DistMap& distance, WeightMap& weight) {
    distance.set_consistency_model(boost::parallel::cm_forward);
    distance.set_max_ghost_cells(0);

    { amplusplus::scoped_epoch epoch(g.transport()); }

    if (g.transport().rank() == 0) std::cout<<"[INFO] Verifying results......";

    {
      amplusplus::scoped_epoch epoch(g.transport());

      BGL_FORALL_VERTICES_T(v, g, Graph) {
	BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
	  get(distance, target(e, g));
	}
      }
    }

    BGL_FORALL_VERTICES_T(v, g, Graph) {
      BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
        //#ifdef 1 //PRINT_DEBUG
	if (get(distance, target(e, g)) >
	    boost::closed_plus<Distance>()(get(distance, v), get(weight, e))) {
	  std::cout << get(get(vertex_local, g), source(e, g)) << "@"
		    << get(get(vertex_owner, g), source(e, g)) << "->"
		    << get(get(vertex_local, g), target(e, g)) << "@"
		    << get(get(vertex_owner, g), target(e, g)) << "  weight = "
		    << get(weight, e)
		    << "  distance(" << get(get(vertex_local, g), source(e, g)) << "@"
		    << get(get(vertex_owner, g), source(e, g))
		    << ") = " << get(distance, source(e, g)) << "  distance("
		    << get(get(vertex_local, g), target(e, g)) << "@"
		    << get(get(vertex_owner, g), target(e, g)) << ") = "
		    << get(distance, target(e, g)) << std::endl;

          std::cout << "[ERROR] Verification failed." << std::endl;
          assert(false);
        }
      }
    }

    if (g.transport().rank() == 0) std::cout << "Verified." << std::endl;
    distance.clear(); // Clear memory used by ghost cells

    return true;
  }

  template <typename DistanceMap>
  uint64_t count_visited_vertices(Graph& g, DistanceMap& distance) {
    uint64_t vvertices = 0;
    weight_type dist;
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      dist = boost::get(distance, v);
      if(dist < std::numeric_limits<Distance>::max()) {
	vvertices+=1;
      }
    }

    uint64_t totalverts = 0;
    MPI_Allreduce(&vvertices, &totalverts, 1, MPI_UINT64_T, MPI_SUM, MPI_COMM_WORLD);
    return totalverts;
  }

  template<typename WeightMap,
           typename DistMap,
           typename RuntimeModelGen,
           typename EAGMConfig,
           typename WorkItemType>
  time_type execute_split_pf(Graph& g,
                             WeightMap& weights,
                             DistMap& distance_state,
                             RuntimeModelGen rtmodelgen,
                             EAGMConfig& config,
                             WorkItemType sw,
                             instance_params& runtime_params,
                             agm_work_stats& sr,
                             bool _verify=true) {

    info("Setting the initial work item set ...");
    // Initial work item set
    typedef append_buffer<WorkItemType, 10u> InitialWorkItems;
    InitialWorkItems initial;
    auto s = std::get<0>(sw);
    if (get(get(vertex_owner, g), s) == _RANK) {
      distance_state[s] = 0;
      initial.push_back(sw);

#ifdef PBGL2_PRINT_WORK_STATS
      sr.increment_useful(0);
#endif

    }

    // Processing funcion
    typedef pre_order_sssp_pf<WeightMap, DistMap> ProcessingFunction;
    ProcessingFunction pf(g, weights, distance_state, sr);

    //Send predicate
    typedef post_order_sssp_pf<WeightMap, DistMap> PostOrderProcessingFunction;
    PostOrderProcessingFunction sendpf(g, weights, distance_state, sr);

    // SSSP algorithm
    typedef eagm<Graph,
                 WorkItemType,
                 ProcessingFunction,
                 EAGMConfig,
                 RuntimeModelGen,
                 PostOrderProcessingFunction> sssp_eagm_t;

    sssp_eagm_t ssspalgo(rtmodelgen,
                         config,
                         pf,
                         sendpf,
                         initial);

    info("Invoking SSSP algorithm with split processing functions ...");
    time_type elapsed = ssspalgo(runtime_params);

#ifdef PBGL2_PRINT_WORK_STATS
    ssspalgo.print_stats();
#endif

    return elapsed;
  }

  template<typename WeightMap,
           typename DistMap,
           typename RuntimeModelGen,
           typename EAGMConfig,
           typename WorkItemType>
  time_type execute_pre_order_pf(Graph& g,
                                 WeightMap& weights,
                                 DistMap& distance_state,
                                 RuntimeModelGen rtmodelgen,
                                 EAGMConfig& config,
                                 WorkItemType sw,
                                 instance_params& runtime_params,
                                 agm_work_stats& sr,
                                 bool _verify=true) {

    info("Setting the initial work item set ...");
    // Initial work item set
    typedef append_buffer<WorkItemType, 10u> InitialWorkItems;
    InitialWorkItems initial;
    auto s = std::get<0>(sw);
    if (get(get(vertex_owner, g), s) == _RANK) {
      initial.push_back(sw);
    }

    // Processing funcion
    typedef sssp_pf<WeightMap, DistMap> ProcessingFunction;
    ProcessingFunction pf(g, weights, distance_state, sr);

    // SSSP algorithm
    typedef eagm<Graph,
                 WorkItemType,
                 ProcessingFunction,
                 EAGMConfig,
                 RuntimeModelGen,
                 EMPTY_PF> sssp_eagm_t;

    sssp_eagm_t ssspalgo(rtmodelgen,
                         config,
                         pf,
                         initial);

    info("Invoking SSSP algorithm with preorder processing functions ...");
    time_type elapsed = ssspalgo(runtime_params);

#ifdef PBGL2_PRINT_WORK_STATS
    ssspalgo.print_stats();
#endif

    return elapsed;
  }

  template<typename WeightMap,
           typename DistMap,
           typename RuntimeModelGen,
           typename EAGMConfig,
           typename WorkItemType>
  time_type execute_post_order_pf(Graph& g,
                                 WeightMap& weights,
                                 DistMap& distance_state,
                                 RuntimeModelGen rtmodelgen,
                                 EAGMConfig& config,
                                 WorkItemType sw,
                                 instance_params& runtime_params,
                                 agm_work_stats& sr,
                                 bool _verify=true) {

    info("Setting the initial work item set ...");
    // Initial work item set
    typedef append_buffer<WorkItemType, 10u> InitialWorkItems;
    InitialWorkItems initial;
    auto s = std::get<0>(sw);
    if (get(get(vertex_owner, g), s) == _RANK) {
      initial.push_back(sw);
    }

    // Processing funcion
    typedef sssp_pf<WeightMap, DistMap> ProcessingFunction;
    ProcessingFunction pf(g, weights, distance_state, sr);

    // SSSP algorithm
    typedef eagm<Graph,
                 WorkItemType,
                 EMPTY_PF,
                 EAGMConfig,
                 RuntimeModelGen,
                 ProcessingFunction> sssp_eagm_t;

    sssp_eagm_t ssspalgo(rtmodelgen,
                         config,
                         pf,
                         initial);

    info("Invoking SSSP algorithm with post-processing functions ...");
    time_type elapsed = ssspalgo(runtime_params);

#ifdef PBGL2_PRINT_WORK_STATS
    ssspalgo.print_stats();
#endif

    return elapsed;
  }

  template<typename WeightMap,
           typename RuntimeModelGen,
           typename EAGMConfig,
           typename WorkItemType,
           typename agm_param_type>
  time_type execute_eagm_common(Graph& g,
                                WeightMap& weights,
                                RuntimeModelGen rtmodelgen,
                                EAGMConfig& config,
                                WorkItemType sw,
                                agm_param_type& agm_params,
                                instance_params& runtime_params,
                                agm_work_stats& sr,
                                bool _verify=true) {

    info("Creating the state ...");
    // State
    typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMap;
    std::vector<Distance> distmap(num_vertices(g), std::numeric_limits<Distance>::max());
    typedef boost::iterator_property_map<typename std::vector<Distance>::iterator, VertexIndexMap> DistMap;
    DistMap distance_state(distmap.begin(), get(boost::vertex_index, g));
    time_type elapsed = -1;

    if (agm_params.pf_mode == agm_pf_splitted) {
      elapsed = execute_split_pf(g,
                                 weights,
                                 distance_state,
                                 rtmodelgen,
                                 config,
                                 sw,
                                 runtime_params,
                                 sr,
                                 _verify);
    } else if (agm_params.pf_mode == agm_pf_preorder) {
      elapsed = execute_pre_order_pf(g,
                                     weights,
                                     distance_state,
                                     rtmodelgen,
                                     config,
                                     sw,
                                     runtime_params,
                                     sr,
                                     _verify);
    } else if (agm_params.pf_mode == agm_pf_postorder) {
      elapsed = execute_post_order_pf(g,
                                     weights,
                                     distance_state,
                                     rtmodelgen,
                                     config,
                                     sw,
                                     runtime_params,
                                     sr,
                                     _verify);

    } else {
      error("Invalid processing function invocation mode!");
      assert(false);
    }

    if (_verify) {
      verify(g, distance_state, weights);
    }

    //visited vertices threshold
    // if the number if visited vertices are less than
    // visited threshold repeat the run
    auto visited = count_visited_vertices(g, distance_state);
    if (boost::num_vertices(g) > VISITED_THRESHOLD) {
      if (visited < VISITED_THRESHOLD)
        elapsed = -1;
    }

    return elapsed;
  }

  template<typename WeightMap,
           typename RuntimeModelGen,
           typename EAGMConfig,
           typename agm_param_type>
  time_type execute_eagm(Graph& g,
                         WeightMap& weights,
                         RuntimeModelGen rtmodelgen,
                         EAGMConfig& config,
                         agm_param_type& agm_param,
                         Vertex source,
                         instance_params& runtime_params,
                         agm_work_stats& sr,
                         bool _verify=true) {

    WorkItem w(source, 0);

    return execute_eagm_common(g,
                               weights,
                               rtmodelgen,
                               config,
                               w,
                               agm_param,
                               runtime_params,
                               sr,
                               _verify);
  }

  template<typename WeightMap,
           typename RuntimeModelGen,
           typename EAGMConfig,
           typename agm_param_type>
  time_type execute_klevel_eagm(Graph& g,
                                WeightMap& weights,
                                RuntimeModelGen rtmodelgen,
                                EAGMConfig& config,
                                agm_param_type& agm_param,
                                Vertex source,
                                instance_params& runtime_params,
                                agm_work_stats& sr,
                                bool _verify=true) {

    LevelWorkItem w(source, 0, 0);

    return execute_eagm_common(g,
                               weights,
                               rtmodelgen,
                               config,
                               w,
                               agm_param,
                               runtime_params,
                               sr,
                               _verify);
  }

};

}}}
#endif


/*

	This file is part of PEST++.

	PEST++ is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

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

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

#include "RunManagerPanther.h" //needs to be first because it includes winsock2.h
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iomanip>
#include "config_os.h"
#include "MorrisMethod.h"
#include "sobol.h"
//#include "TornadoPlot.h"
#include "Pest.h"
#include "Transformable.h"
#include "Transformation.h"
#include "ParamTransformSeq.h"
#include "utilities.h"
#include "pest_error.h"
#include "ModelRunPP.h"
#include "FileManager.h"
#include "RunManagerSerial.h"
#include "OutputFileWriter.h"
#include "PantherAgent.h"
#include "Serialization.h"
#include "system_variables.h"

using namespace std;
using namespace pest_utils;
using Eigen::MatrixXd;
using Eigen::VectorXd;

int main(int argc, char* argv[])
{
#ifndef _DEBUG
	try
	{
#endif
	string version = PESTPP_VERSION;
	cout << endl << endl;
	cout << "             pestpp-sen: a tool for global sensitivity analysis" << endl << endl;
	cout << "                       by The PEST++ Development Team" << endl;
	cout << endl << endl << "version: " << version << endl;
	cout << "binary compiled on " << __DATE__ << " at " << __TIME__ << endl << endl;

	// build commandline
	string commandline = "";
	for(int i=0; i<argc; ++i)
	{
		commandline.append(" ");
		commandline.append(argv[i]);
	}

	vector<string> cmd_arg_vec(argc);
	copy(argv, argv + argc, cmd_arg_vec.begin());
	for (vector<string>::iterator it = cmd_arg_vec.begin(); it != cmd_arg_vec.end(); ++it)
	{
		transform(it->begin(), it->end(), it->begin(), ::tolower);
	}

	string complete_path;
	enum class RunManagerType {SERIAL, PANTHER, GENIE};

	if (argc >=2) {
		complete_path = argv[1];
	}
	else {
		cerr << "--------------------------------------------------------" << endl;
		cerr << "usage:" << endl << endl;
		cerr << "    serial run manager:" << endl;
		cerr << "        gsa pest_ctl_file.pst" << endl << endl;
		cerr << "    PANTHER master:" << endl;
		cerr << "        gsa control_file.pst /H :port" << endl;
		cerr << "    PANTHER worker:" << endl;
		cerr << "        gsa control_file.pst /H hostname:port " << endl << endl;
		cerr << " additional options can be found in the PEST++ manual" << endl;
		cerr << "--------------------------------------------------------" << endl;
		exit(0);
	}

	FileManager file_manager;
	string filename = complete_path;
	string pathname = ".";
	file_manager.initialize_path(get_filename_without_ext(filename), pathname);

	//by default use the serial run manager.  This will be changed later if another
	//run manger is specified on the command line.
	RunManagerType run_manager_type = RunManagerType::SERIAL;
	//Check for PANTHER worker
	vector<string>::const_iterator it_find, it_find_next;
	string next_item;
	string socket_str = "";
	it_find = find(cmd_arg_vec.begin(), cmd_arg_vec.end(), "/h");
	next_item.clear();
	if (it_find != cmd_arg_vec.end() && it_find + 1 != cmd_arg_vec.end())
	{
		next_item = *(it_find + 1);
		strip_ip(next_item);
	}
	if (it_find != cmd_arg_vec.end() && !next_item.empty() && next_item[0] != ':')
	{
		// This is a PANTHER worker, start PEST++ as a PANTHER worker
		vector<string> sock_parts;
		vector<string>::const_iterator it_find_yamr_ctl;
		string file_ext = get_filename_ext(filename);
		tokenize(next_item, sock_parts, ":");
		try
		{
			if (sock_parts.size() != 2)
			{
				cerr << "PANTHER worker requires the master be specified as /H hostname:port" << endl << endl;
				throw(PestCommandlineError(commandline));
			}
			PANTHERAgent yam_agent;
			string ctl_file = "";
			try {
				string ctl_file;
				if (upper_cp(file_ext) == "YMR")
				{
					ctl_file = file_manager.build_filename("ymr");
					yam_agent.process_panther_ctl_file(ctl_file);
				}
				else
				{
					// process traditional PEST control file
					ctl_file = file_manager.build_filename("pst");
					yam_agent.process_ctl_file(ctl_file);
				}
			}
			catch (PestError e)
			{
				cerr << "Error prococessing control file: " << ctl_file << endl << endl;
				cerr << e.what() << endl << endl;
				throw(e);
			}

			yam_agent.start(sock_parts[0], sock_parts[1]);
		}
		catch (PestError &perr)
		{
			cerr << perr.what();
			throw(perr);
		}
		cout << endl << "Simulation Complete..." << endl;
		exit(0);
	}
	//Check for PANTHER master
	else if (it_find != cmd_arg_vec.end())
	{
		// using PANTHER run manager
		run_manager_type = RunManagerType::PANTHER;
		socket_str = next_item;
	}

	it_find = find(cmd_arg_vec.begin(), cmd_arg_vec.end(), "/g");
	next_item.clear();
	it_find = find(cmd_arg_vec.begin(), cmd_arg_vec.end(), "/g");
	next_item.clear();
	if (it_find != cmd_arg_vec.end())
	{
		cerr << "Genie run manager ('/g') no longer supported, please use PANTHER instead" << endl;
		return 1;

	}

	ofstream &fout_rec = file_manager.open_ofile_ext("rec");
	fout_rec << "             pestpp-sen: a tool for global sensitivity analysis" << endl << endl;
	fout_rec << "                         by The PEST++ Development Team" << endl << endl;
	fout_rec << endl << endl << "version: " << version << endl;
	fout_rec << "binary compiled on " << __DATE__ << " at " << __TIME__ << endl << endl;
	fout_rec << "using control file: \"" << complete_path << "\"" << endl;
	fout_rec << "in directory: \"" << OperSys::getcwd() << "\"" << endl << endl;

	cout << endl;
	cout << "using control file: \"" << complete_path << "\"" << endl;
	cout << "in directory: \"" << OperSys::getcwd() << "\"" << endl << endl;

	// create pest run and process control file to initialize it
	Pest pest_scenario;
	pest_scenario.set_defaults();
	try {
		pest_scenario.process_ctl_file(file_manager.open_ifile_ext("pst"), file_manager.build_filename("pst"),fout_rec);
		file_manager.close_file("pst");
		pest_scenario.check_inputs(fout_rec);
	}
	catch(PestError e)
	{
		cerr << "Error prococessing control file: " << filename << endl << endl;
		cerr << e.what() << endl << endl;
		fout_rec << "Error prococessing control file: " << filename << endl << endl;
		fout_rec << e.what() << endl;
		fout_rec.close();
		//throw(e);
		return 1;
	}

	RunManagerAbstract *run_manager_ptr;
	if (run_manager_type == RunManagerType::PANTHER)
	{
		string port = argv[3];
		strip_ip(port);
		strip_ip(port, "front", ":");
		const ModelExecInfo &exi = pest_scenario.get_model_exec_info();
		run_manager_ptr = new RunManagerPanther (
			file_manager.build_filename("rns"), port,
			file_manager.open_ofile_ext("rmr"),
			pest_scenario.get_pestpp_options().get_max_run_fail(),
			pest_scenario.get_pestpp_options().get_overdue_reched_fac(),
			pest_scenario.get_pestpp_options().get_overdue_giveup_fac(),
			pest_scenario.get_pestpp_options().get_overdue_giveup_minutes());
	}
	else
	{
		const ModelExecInfo &exi = pest_scenario.get_model_exec_info();
		run_manager_ptr = new RunManagerSerial(exi.comline_vec,
		exi.tplfile_vec, exi.inpfile_vec, exi.insfile_vec, exi.outfile_vec,
		file_manager.build_filename("rns"), pathname);
	}

	cout << endl;
	fout_rec << endl;
	cout << "using control file: \"" <<  complete_path << "\"" << endl;
	fout_rec << "using control file: \"" <<  complete_path << "\"" << endl;

	enum class GSA_RESTART { NONE, RESTART };
	GSA_RESTART gsa_restart = GSA_RESTART::NONE;
	//process restart and  reuse jacibian directives
	vector<string>::const_iterator it_find_r = find(cmd_arg_vec.begin(), cmd_arg_vec.end(), "/r");
	if (it_find_r != cmd_arg_vec.end())
	{
		gsa_restart = GSA_RESTART::RESTART;
	}
	else
	{
		gsa_restart = GSA_RESTART::NONE;
	}
	//OutputFileWriter(FileManager &_file_manager, Pest &_pest_scenario, bool restart_flag = false, bool _save_rei = true, int _eigenwrite = 0);
	OutputFileWriter output_writer(file_manager, pest_scenario, false, false);
	ofstream &frec = file_manager.rec_ofstream();
	output_writer.scenario_report(frec);
	output_writer.scenario_io_report(frec);
	output_writer.scenario_par_report(frec);
	output_writer.scenario_obs_report(frec);

	pest_scenario.check_inputs(frec);
	pest_scenario.check_io();

	//map<string, string> gsa_opt_map;
	//process .gsa file
	string gsa_filename = file_manager.get_base_filename() + ".gsa";
	/*if (!check_exist_in(gsa_filename))
	{
		cout << "WARNING: " << gsa_filename << " not found, using standard settings and Method of Morris:" << endl;
		cout << "     MORRIS_P: 4" << endl;
		cout << "     MORRIS_R: 4" << endl;
		cout << "     MORRIS_DELTA: 0.666" << endl;
		gsa_opt_map["METHOD"] = "MORRIS";
		gsa_opt_map["MORRIS_DELTA"] = ".666666";
		gsa_opt_map["MORRIS_P"] = "4";
		gsa_opt_map["MORRIS_R"] = "4";
		gsa_opt_map["RAND_SEED"] = "2";
	}
	else
	{
		try
		{
			gsa_opt_map = GsaAbstractBase::process_gsa_file(file_manager.open_ifile_ext("gsa"), file_manager);
			file_manager.close_file("gsa");
		}
		catch (PestError e)
		{
			cerr << "Error prococessing .gsa file: " << file_manager.build_filename("gsa") << endl << endl;
			cerr << e.what() << endl << endl;
			throw(e);
		}
	}*/
	if (check_exist_in(gsa_filename))
	{
		cout << "WARNING: use of .gsa files is deprecated - .gsa file '" << gsa_filename << "' is being ignored, please use '++' args";
		return 1;
	}

	PestppOptions *pp_ptr = pest_scenario.get_pestpp_options_ptr();
	map<string, string> gsa_opt_map = pp_ptr->get_arg_map();
	/*gsa_opt_map["METHOD"] = pp_ptr->get_gsa_method();
	gsa_opt_map["MORRIS_DELTA"] = pp_ptr->get_gsa_morris_delta();
	gsa_opt_map["MORRIS_P"] = pp_ptr->get_gsa_morris_p();
	gsa_opt_map["MORRIS_R"] = pp_ptr->get_gsa_morris_r;
	gsa_opt_map["MORRIS_OBS_SEN"] = pp_ptr->get_gsa_morris_obs_sen();
*/
	//Build Transformation with ctl_2_numberic
	ParamTransformSeq base_partran_seq(pest_scenario.get_base_par_tran_seq());
	Parameters ctl_par = pest_scenario.get_ctl_parameters();

	//Build Transformation with ctl_2_numberic
	ObjectiveFunc obj_func(&(pest_scenario.get_ctl_observations()), &(pest_scenario.get_ctl_observation_info()), &(pest_scenario.get_prior_info()));
	ModelRun model_run(&obj_func, pest_scenario.get_ctl_observations());
	const set<string> &log_trans_pars = base_partran_seq.get_log10_ptr()->get_items();
	auto method = gsa_opt_map.find("GSA_METHOD");

	GsaAbstractBase* gsa_method = nullptr;
	if (method == gsa_opt_map.end() || method->second == "MORRIS")
	{
		int morris_r = 4;
		int morris_p = 4;

		double morris_delta = .666;
		bool default_delta = true;
		bool calc_pooled_obs = false;
		bool calc_morris_obs_sen = true;
		auto morris_r_it = gsa_opt_map.find("GSA_MORRIS_R");
		if (morris_r_it != gsa_opt_map.end())
		{
			convert_ip(morris_r_it->second, morris_r);
		}
		auto morris_p_it = gsa_opt_map.find("GSA_MORRIS_P");
		if (morris_p_it != gsa_opt_map.end())
		{
			convert_ip(morris_p_it->second, morris_p);
		}
		auto morris_d_it = gsa_opt_map.find("GSA_MORRIS_DELTA");
		if (morris_d_it != gsa_opt_map.end())
		{

			convert_ip(morris_d_it->second, morris_delta);
			default_delta = false;
		}
		auto morris_pool_it = gsa_opt_map.find("GSA_MORRIS_POOLED_OBS");
		if (morris_pool_it != gsa_opt_map.end())
		{
			string pooled_obs_flag = morris_pool_it->second;
			upper_ip(pooled_obs_flag);
			if (pooled_obs_flag == "TRUE") calc_pooled_obs = true;
		}

		auto morris_obs_sen_it = gsa_opt_map.find("GSA_MORRIS_OBS_SEN");
		if (morris_obs_sen_it != gsa_opt_map.end())
		{
			string obs_sen_flag = morris_obs_sen_it->second;
			upper_ip(obs_sen_flag);
			if (obs_sen_flag == "FALSE") calc_morris_obs_sen = false;
		}

		if (default_delta) morris_delta = morris_p / (2.0 * (morris_p - 1));

		MorrisMethod *m_ptr = new MorrisMethod(pest_scenario, file_manager, &obj_func,
			base_partran_seq, morris_p, morris_r, morris_delta, calc_pooled_obs,
			calc_morris_obs_sen, GsaAbstractBase::PARAM_DIST::uniform, 1.0);
		gsa_method = m_ptr;
		m_ptr->process_pooled_var_file();

		frec << endl << endl << endl << "Method of Morris settings:" << endl;

		frec << scientific << left << setw(30) << " morris_r " << morris_r << endl;
		frec << scientific << left << setw(30) << " morris_p " << morris_p << endl;
		frec << scientific << left << setw(30) << " morris_delta " << morris_delta << endl << endl;

	}

	else if (method != gsa_opt_map.end() && method->second == "SOBOL")
	{
		GsaAbstractBase::PARAM_DIST par_dist = GsaAbstractBase::PARAM_DIST::uniform;
		string par_dist_str = "UNIFORM";
		int n_sample = 100;
		auto sob_n_sam_it = gsa_opt_map.find("GSA_SOBOL_SAMPLES");
		if (sob_n_sam_it != gsa_opt_map.end())
		{
			convert_ip(sob_n_sam_it->second, n_sample);
		}
		auto sob_p_dist_it = gsa_opt_map.find("GSA_SOBOL_PAR_DIST");
		if (sob_p_dist_it != gsa_opt_map.end())
		{
			par_dist_str = sob_p_dist_it->second;
			upper_ip(par_dist_str);
			if (par_dist_str == "NORM") par_dist = GsaAbstractBase::PARAM_DIST::normal;
			else if (par_dist_str == "UNIF") par_dist = GsaAbstractBase::PARAM_DIST::uniform;
			else
			{
				ostringstream str;
				str << "SOBOL_PAR_DIST(" << par_dist_str << "):  \"" << par_dist_str << "\" is an invalid distribuation type";
				throw PestError(str.str());
			}
		}

		gsa_method = new Sobol(pest_scenario, file_manager, &obj_func,
			base_partran_seq, n_sample, par_dist, 1.0);

		frec << endl << endl << endl << "Method of Sobol settings:" << endl;

		frec << scientific << left << setw(30) << " n_sample " << n_sample << endl;
		frec << scientific << left << setw(30) << " sobol par dist " <<par_dist_str << endl;

	}
	else
	{
		throw PestError("A valid method for computing the sensitivity must be specified in the control file");
	}

	auto morris_r_it = gsa_opt_map.find("RAND_SEED");
	if (morris_r_it != gsa_opt_map.end())
	{
		unsigned int seed = convert_cp<unsigned int>(morris_r_it->second);
		gsa_method->set_seed(seed);
	}
	frec << scientific << left << setw(30) << " gsa random seed " << gsa_method->get_seed() << endl;
	// make model runs
	if (gsa_restart == GSA_RESTART::NONE)
	{
		//Allocates Space for Run Manager.  This initializes the model parameter names and observations names.
		//Neither of these will change over the course of the simulation
		cout << endl;
		cout << "Building model run parameter sets..." << endl;
		run_manager_ptr->initialize(base_partran_seq.ctl2model_cp(ctl_par), pest_scenario.get_ctl_observations());

		Parameters model_pars = base_partran_seq.ctl2model_cp(ctl_par);
		run_manager_ptr->reinitialize();
		gsa_method->assemble_runs(*run_manager_ptr);
	}
	else
	{
		run_manager_ptr->initialize_restart(file_manager.build_filename("rns"));
	}
	cout << endl;
	cout << "Performing model runs..." << endl;
	run_manager_ptr->run();

	cout << "Calculating sensitivities..." << endl;
	gsa_method->calc_sen(*run_manager_ptr, model_run);
	file_manager.close_file("srw");
	file_manager.close_file("msn");
	file_manager.close_file("orw");
	delete run_manager_ptr;
	cout << endl << endl << "Simulation Complete..." << endl;
	//cout << endl << "Simulation Complete - Press RETURN to close window" << endl;
	//char buf[256];
	//OperSys::gets_s(buf, sizeof(buf));
#ifndef _DEBUG
	}
	catch (exception &e)
	{
		cout << "Error condition prevents further execution: " << endl << e.what() << endl;
		//cout << "press enter to continue" << endl;
		//char buf[256];
		//OperSys::gets_s(buf, sizeof(buf));
	}
#endif
}


// Copyright 2015-2022 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

#pragma once

/// @file
/// Definitions of the basic data-handling classes in ALMA.

#include <map>
#include <array>
#include <vector>
#include <utility>
#include <numeric>
#include <limits>
#include <boost/math/special_functions/pow.hpp>
#include <boost/mpi.hpp>
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <cmakevars.hpp>
#include <constants.hpp>
#include <utilities.hpp>
#include <exceptions.hpp>
#include <periodic_table.hpp>

namespace alma {
// Forward declarations placed here for convenience.
class Symmetry_operations;
class Gamma_grid;
class Threeph_process;

/// Convenient shorthand for an array of three ints.
using Triple_int = std::array<int, 3>;

/// Specialized version of map whose keys are of class Triple_int.
template <typename T>
using Triple_int_map =
    std::map<Triple_int, T, Container_comparator<Triple_int>>;

/// Hold information about a crystal structure.
class Crystal_structure {
public:
    /// Lattice vectors in nm.
    const Eigen::Matrix3d lattvec;
    /// Atomic positions in lattice coordinates.
    const Eigen::Matrix<double, 3, Eigen::Dynamic> positions;
    /// Elements present in the structure.
    const std::vector<std::string> elements;
    /// Volume of the unit cell.
    const double V;
    /// Lattice vectors of the reciprocal lattice.
    const Eigen::MatrixXd rlattvec;
    /// Number of atoms of each element.
    ///
    /// The first numbers[0] atoms belong to elements[0], the next
    /// numbers[1] atoms belong to elements[1], etc.
    const std::vector<int> numbers;
    /// Basic constructor.
    Crystal_structure(Eigen::Matrix3d _lattvec,
                      Eigen::Matrix<double, 3, Eigen::Dynamic> _positions,
                      std::vector<std::string> _elements,
                      std::vector<int> _numbers)
        : lattvec(std::move(_lattvec)), positions(std::move(_positions)),
          elements(std::move(_elements)),
          V(std::fabs(this->lattvec.determinant())),
          rlattvec(2. * constants::pi * this->lattvec.inverse().transpose()),
          numbers(std::move(_numbers)) {
        std::partial_sum(this->numbers.begin(),
                         this->numbers.end(),
                         std::back_inserter(this->partsums));

        for (int i = 0; i < this->get_natoms(); ++i) {
            this->masses.emplace_back(alma::get_mass(this->get_element(i)));
            this->gfactors.emplace_back(
                alma::get_gfactor(this->get_element(i)));
        }
    }

    /// Specialized constructor allowing for custom masses.
    Crystal_structure(Eigen::Matrix3d _lattvec,
                      Eigen::Matrix<double, 3, Eigen::Dynamic> _positions,
                      std::vector<std::string> _elements,
                      std::vector<int> _numbers,
                      std::vector<double> _masses)
        : Crystal_structure(_lattvec, _positions, _elements, _numbers) {
        if (_masses.size() != static_cast<std::size_t>(this->get_natoms())) {
            throw value_error("one mass per atom must be provided");
        }
        for (int i = 0; i < this->get_natoms(); ++i) {
            if (_masses[i] <= 0.) {
                throw value_error("all masses must be positive");
            }
            this->masses[i] = _masses[i];
        }
    }

    /// Return the number of elements in the structure.
    inline std::vector<std::string>::size_type get_nelements() const {
        return this->elements.size();
    }

    /// Return the number of atoms in the motif.
    inline int get_natoms() const {
        return this->positions.cols();
    }
    /// Return the chemical symbol for an atom.
    ///
    /// @param[in] i - atom number
    /// @return the chemical symbol
    inline std::string get_element(int i) const {
        if (i >= this->get_natoms())
            throw value_error("invalid atom number");
        return this->elements[std::distance(
            this->partsums.begin(),
            std::lower_bound(
                this->partsums.begin(), this->partsums.end(), i + 1))];
    }

    /// Return the mass of the i-th atom.
    ///
    /// @param[in] i - atom number
    /// @return the mass in a.m.u
    inline double get_mass(int i) const {
        if (i >= this->get_natoms())
            throw value_error("invalid atom number");
        return this->masses[i];
    }

    /// Return the g factor of the i-th atom.
    ///
    /// @param[in] i - atom number
    /// @return the Pearson deviation coefficient of the i-th atom's
    /// mass
    inline double get_gfactor(int i) const {
        if (i >= this->get_natoms())
            throw value_error("invalid atom number");
        return this->gfactors[i];
    }

    /// Check if any atom belongs to a virtual element.
    ///
    /// @return true is the structure describes an alloy, or
    /// false otherwise.
    inline bool is_alloy() const {
        for (auto& e : this->elements)
            if (e.find(";") != std::string::npos)
                return true;

        return false;
    }

    /// Find all images of a q point in the first Brillouin zone.
    ///
    /// A point on the surface of the first BZ will have more than
    /// one possible image.
    /// @param[in] - original q point in Cartesian coordinates
    /// @return the Cartesian coordinates of all images of the point
    /// in the first Brillouin zone, as columns of a matrix.
    inline Eigen::MatrixXd map_to_firstbz(
        const Eigen::Ref<const Eigen::Vector3d>& q) const {
        constexpr int sbound = 3;
        // Step 1: find a single image using a standard approach
        Eigen::Vector3d qbz{this->rlattvec.colPivHouseholderQr().solve(q)};
        Eigen::Vector3d qbz_min;
        qbz -= qbz.array().round().matrix().eval();
        qbz = (this->rlattvec * qbz).eval();
        double n2 = qbz.squaredNorm();
        qbz_min = qbz;
        for (int i = -sbound; i <= sbound; ++i) {
            for (int j = -sbound; j <= sbound; ++j) {
                for (int k = -sbound; k <= sbound; ++k) {
                    Eigen::Vector3d qbzp{qbz + i * this->rlattvec.col(0) +
                                         j * this->rlattvec.col(1) +
                                         k * this->rlattvec.col(2)};
                    double n2p = qbzp.squaredNorm();
                    if (n2p < n2) {
                        n2 = n2p;
                        qbz_min = qbzp;
                    }
                }
            }
        }
        qbz = qbz_min;
        // Step 2: perform a search to find equivalent images.
        std::size_t found = 0;
        int ncols = boost::math::pow<3>(2 * sbound + 1);
        Eigen::MatrixXd nruter(3, ncols);
        for (int i = -sbound; i <= sbound; ++i) {
            for (int j = -sbound; j <= sbound; ++j) {
                for (int k = -sbound; k <= sbound; ++k) {
                    Eigen::Vector3d qbzp{qbz + i * this->rlattvec.col(0) +
                                         j * this->rlattvec.col(1) +
                                         k * this->rlattvec.col(2)};
                    double n2p = qbzp.squaredNorm();
                    if (alma::almost_equal(n2, n2p)) {
                        nruter.col(found) = qbzp;
                        ++found;
                    }
                }
            }
        }
        return nruter.leftCols(found);
    }

private:
    /// partsums[i] contains sum(numbers[0:i+1]).
    std::vector<int> partsums;
    /// Masses of each atom. Cached here to avoid costly
    /// calculations in the case of virtual elements.
    std::vector<double> masses;
    /// Pearson deviation coefficients of the atomic masses. Cached
    /// here to avoid costly calculations in the case of virtual
    /// elements.
    std::vector<double> gfactors;
};

/// Hold information about the harmonic interactions between atoms.
/// Normally T will be Eigen::MatrixXd since force constants are
/// real, but it might be useful to change it to Eigen::MatrixXcd
/// in order to operate in mixed real/reciprocal space.
template <class T> class General_harmonic_ifcs {
public:
    /// Coordinates of each unit cell for which constants
    /// are available.
    const std::vector<Triple_int> pos;
    /// Force constants between unit cell 0 and each unit cell.
    const std::vector<T> ifcs;
    /// Dimension of the supercell originally used for the IFC
    /// calculations along the first axis.
    const int na;
    /// Dimension of the supercell originally used for the IFC
    /// calculations along the second axis.
    const int nb;
    /// Dimension of the supercell originally used for the IFC
    /// calculations along the third axis.
    const int nc;
    /// Basic constructor.
    General_harmonic_ifcs(std::vector<Triple_int> _pos,
                          std::vector<T> _ifcs,
                          int _na,
                          int _nb,
                          int _nc)
        : pos(std::move(_pos)), ifcs(std::move(_ifcs)), na(_na), nb(_nb),
          nc(_nc) {
    }

    // Return the number of unit cells.
    inline std::vector<std::string>::size_type get_ncells() const {
        return this->pos.size();
    }
};

/// Specialization of General_harmonic_ifcs for the most common
/// use case.
using Harmonic_ifcs = General_harmonic_ifcs<Eigen::MatrixXd>;

/// Hold information about the polarization properties of the
/// structure.
class Dielectric_parameters {
public:
    /// Born charges.
    const std::vector<Eigen::MatrixXd> born;
    /// Dielectric tensor.
    const Eigen::Matrix3d epsilon;
    /// Basic constructor.
    Dielectric_parameters(std::vector<Eigen::MatrixXd> _born,
                          Eigen::Matrix3d _epsilon)
        : born(std::move(_born)), epsilon(std::move(_epsilon)) {
    }

    /// Default constructor. Create an empty object.
    Dielectric_parameters() {
    }
};

/// Phonopy-style atom index represented both as a single integer
/// and as four indices.
///
/// Objects from this class are not intended to be built
/// directly. Use a Supercell_index_builder instead.
class Supercell_index {
public:
    /// Atom index within the supercell.
    const int index;
    /// Unit cell index along the first axis.
    const int ia;
    /// Unit cell index along the second axis.
    const int ib;
    /// Unit cel index along the third axis.
    const int ic;
    /// Atom index within the unit cell.
    const int iatom;
    // Basic constructor.
    Supercell_index(const int _index,
                    const int _ia,
                    const int _ib,
                    const int _ic,
                    const int _iatom)
        : index(_index), ia(_ia), ib(_ib), ic(_ic), iatom(_iatom) {
    }

    /// Return an array of three integers {ia, ib, ic}.
    inline const Triple_int get_pos() const {
        return Triple_int({{this->ia, this->ib, this->ic}});
    }
};

/// Builder for Supercell_index objects sharing the same na, nb, nc.
class Supercell_index_builder {
public:
    /// Supercell dimension along the first axis.
    const int na;
    /// Supercell dimension along the second axis.
    const int nb;
    /// Supercell dimension along the third axis.
    const int nc;
    /// Number of atoms in the unit cell.
    const int natoms;
    /// Basic constructor.
    Supercell_index_builder(const int _na,
                            const int _nb,
                            const int _nc,
                            const int _natoms);
    /// Create a Supercell_index from a single integer.
    Supercell_index create_index(const int index) const;

    /// Create a Supercell_index from four integers.
    Supercell_index create_index(const int ia,
                                 const int ib,
                                 const int ic,
                                 const int iatom) const;

    /// Perform a bounds check and create a Supercell_index from a
    /// single integer.
    Supercell_index create_index_safely(const int index) const;

    /// Perform a bounds check and create a Supercell_index from
    /// four integers.
    Supercell_index create_index_safely(const int ia,
                                        const int ib,
                                        const int ic,
                                        const int iatom) const;
};

/// Class representing the anharmonic (third-order)
/// interaction between two atoms atoms.
///
/// Note that, since third-order IFCs tend to be expressed
/// in sparse formats, it is more natural to store each tensor
/// individually. There is no third-order equivalent to the
/// Harmonic_ifcs class. Vectors of Thirdorder_ifcs can be used
/// instead.
/// The first atom, i, is always assumed to be part of the first
/// unit cell, that is, R_i = {0., 0., 0.}.
class Thirdorder_ifcs {
public:
    /// Cartesian coordinates of the second unit cell.
    const Eigen::VectorXd rj;
    /// Cartesian coordinates of the third unit cell.
    const Eigen::VectorXd rk;
    /// Index of the first atom.
    const std::size_t i;
    /// Index of the second atom.
    const std::size_t j;
    /// Index of the third atom.
    const std::size_t k;
    /// Access a particular ifc using three indexes.
    ///
    /// Note that all indices must be positive and lower than 3,
    /// but that this condition is not checked.
    /// @param[in] alpha - first axis index
    /// @param[in] beta - second axis index
    /// @param[in] gamma - third axis index
    /// @return a mutable reference to the element
    double& ifc(std::size_t alpha, std::size_t beta, std::size_t gamma) {
        return this->ifcs[gamma + 3 * (beta + 3 * alpha)];
    }

    /// Access a particular ifc using three indexes.
    ///
    /// Note that all indices must be positive and lower than 3,
    /// but that this condition is not checked.
    /// @param[in] alpha - first axis index
    /// @param[in] beta - second axis index
    /// @param[in] gamma - third axis index
    /// @return a const reference to the element
    const double& ifc(std::size_t alpha,
                      std::size_t beta,
                      std::size_t gamma) const {
        return this->ifcs[gamma + 3 * (beta + 3 * alpha)];
    }

    /// Basic constructor. It does not initialize the ifcs
    /// member variable.
    Thirdorder_ifcs(const Eigen::VectorXd& _rj,
                    const Eigen::VectorXd& _rk,
                    std::size_t _i,
                    std::size_t _j,
                    std::size_t _k)
        : rj(std::move(_rj)), rk(std::move(_rk)), i(_i), j(_j), k(_k) {
    }

private:
    /// All third-order force constants between the three atoms.
    std::array<double, 27> ifcs;
};
} // namespace alma


//
//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//
#ifndef BOOST_GRAPH_MST_KRUSKAL_HPP
#define BOOST_GRAPH_MST_KRUSKAL_HPP

/*
 *Minimum Spanning Tree
 *         Kruskal Algorithm
 *
 *Requirement:
 *      undirected graph
 */

#include <vector>
#include <queue>
#include <functional>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/concept/assert.hpp>

namespace boost
{

// Kruskal's algorithm for Minimum Spanning Tree
//
// This is a greedy algorithm to calculate the Minimum Spanning Tree
// for an undirected graph with weighted edges. The output will be a
// set of edges.
//

namespace detail
{

    template < class Graph, class OutputIterator, class Rank, class Parent,
        class Weight >
    void kruskal_mst_impl(const Graph& G, OutputIterator spanning_tree_edges,
        Rank rank, Parent parent, Weight weight)
    {
        if (num_vertices(G) == 0)
            return; // Nothing to do in this case
        typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
        typedef typename graph_traits< Graph >::edge_descriptor Edge;
        BOOST_CONCEPT_ASSERT((VertexListGraphConcept< Graph >));
        BOOST_CONCEPT_ASSERT((EdgeListGraphConcept< Graph >));
        BOOST_CONCEPT_ASSERT((OutputIteratorConcept< OutputIterator, Edge >));
        BOOST_CONCEPT_ASSERT((ReadWritePropertyMapConcept< Rank, Vertex >));
        BOOST_CONCEPT_ASSERT((ReadWritePropertyMapConcept< Parent, Vertex >));
        BOOST_CONCEPT_ASSERT((ReadablePropertyMapConcept< Weight, Edge >));
        typedef typename property_traits< Weight >::value_type W_value;
        typedef typename property_traits< Rank >::value_type R_value;
        typedef typename property_traits< Parent >::value_type P_value;
        BOOST_CONCEPT_ASSERT((ComparableConcept< W_value >));
        BOOST_CONCEPT_ASSERT((ConvertibleConcept< P_value, Vertex >));
        BOOST_CONCEPT_ASSERT((IntegerConcept< R_value >));

        disjoint_sets< Rank, Parent > dset(rank, parent);

        typename graph_traits< Graph >::vertex_iterator ui, uiend;
        for (boost::tie(ui, uiend) = vertices(G); ui != uiend; ++ui)
            dset.make_set(*ui);

        typedef indirect_cmp< Weight, std::greater< W_value > > weight_greater;
        weight_greater wl(weight);
        std::priority_queue< Edge, std::vector< Edge >, weight_greater > Q(wl);
        /*push all edge into Q*/
        typename graph_traits< Graph >::edge_iterator ei, eiend;
        for (boost::tie(ei, eiend) = edges(G); ei != eiend; ++ei)
            Q.push(*ei);

        while (!Q.empty())
        {
            Edge e = Q.top();
            Q.pop();
            Vertex u = dset.find_set(source(e, G));
            Vertex v = dset.find_set(target(e, G));
            if (u != v)
            {
                *spanning_tree_edges++ = e;
                dset.link(u, v);
            }
        }
    }

} // namespace detail

// Named Parameters Variants

template < class Graph, class OutputIterator >
inline void kruskal_minimum_spanning_tree(
    const Graph& g, OutputIterator spanning_tree_edges)
{
    typedef typename graph_traits< Graph >::vertices_size_type size_type;
    typedef typename graph_traits< Graph >::vertex_descriptor vertex_t;
    if (num_vertices(g) == 0)
        return; // Nothing to do in this case
    typename graph_traits< Graph >::vertices_size_type n = num_vertices(g);
    std::vector< size_type > rank_map(n);
    std::vector< vertex_t > pred_map(n);

    detail::kruskal_mst_impl(g, spanning_tree_edges,
        make_iterator_property_map(
            rank_map.begin(), get(vertex_index, g), rank_map[0]),
        make_iterator_property_map(
            pred_map.begin(), get(vertex_index, g), pred_map[0]),
        get(edge_weight, g));
}

template < class Graph, class OutputIterator, class P, class T, class R >
inline void kruskal_minimum_spanning_tree(const Graph& g,
    OutputIterator spanning_tree_edges,
    const bgl_named_params< P, T, R >& params)
{
    typedef typename graph_traits< Graph >::vertices_size_type size_type;
    typedef typename graph_traits< Graph >::vertex_descriptor vertex_t;
    if (num_vertices(g) == 0)
        return; // Nothing to do in this case
    typename graph_traits< Graph >::vertices_size_type n;
    n = is_default_param(get_param(params, vertex_rank)) ? num_vertices(g) : 1;
    std::vector< size_type > rank_map(n);
    n = is_default_param(get_param(params, vertex_predecessor))
        ? num_vertices(g)
        : 1;
    std::vector< vertex_t > pred_map(n);

    detail::kruskal_mst_impl(g, spanning_tree_edges,
        choose_param(get_param(params, vertex_rank),
            make_iterator_property_map(rank_map.begin(),
                choose_pmap(get_param(params, vertex_index), g, vertex_index),
                rank_map[0])),
        choose_param(get_param(params, vertex_predecessor),
            make_iterator_property_map(pred_map.begin(),
                choose_const_pmap(
                    get_param(params, vertex_index), g, vertex_index),
                pred_map[0])),
        choose_const_pmap(get_param(params, edge_weight), g, edge_weight));
}

} // namespace boost

#endif // BOOST_GRAPH_MST_KRUSKAL_HPP