/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 * All rights reserved.
 *
 * See LICENSE file in the root of the mrob library.
 *
 *
 * SO3.cpp
 *
 *  Created on: Feb 12, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include "mrob/SO3.hpp"
#include <cmath>
#include <iostream>
#include <Eigen/LU> // for determinant
#include <Eigen/Geometry> // for quaternions and rotations

using namespace mrob;

SO3::SO3(const Mat3 &R) :
        R_(R)
{
}

SO3::SO3(const Mat31 &w) :
        R_(Mat3::Identity())
{
    //std::cout << "SO3 with Mat31" << std::endl;
    this->exp(hat3(w));
}

SO3::SO3(const SO3 &R) :
        R_(R.R())
{
}

template<typename OtherDerived>
SO3::SO3(const Eigen::MatrixBase<OtherDerived>& rhs)  :
    R_(rhs)
{    //std::cout << "SE3 MAT4" << std::endl;
}

SO3& SO3::operator=(const SO3 &rhs)
{
    //std::cout << "SO3 operator equal" << std::endl;
    // check for self assignment
    if (this == &rhs)
        return *this;
    R_ = rhs.R();
    return *this;
}

SO3 SO3::operator*(const SO3& rhs) const
{
    Mat3 res = R_ * rhs.R();
    return SO3(res);
}

SO3 SO3::mul(const SO3& rhs) const
{
    return (*this) * rhs;
}

void SO3::update_lhs(const Mat31 &dw)
{
    SO3 dR(dw);
    R_ = dR.R() * R_;
}

void SO3::update_rhs(const Mat31 &dw)
{
    SO3 dR(dw);
    R_ = R_ * dR.R();
}

Mat31 mrob::vee3(const Mat3 &w_hat)
{
    Mat31 w;
    w << -w_hat(1,2), w_hat(0,2), -w_hat(0,1);
    return w;
}

Mat3 mrob::hat3(const Mat31 &w)
{
    Mat3 w_hat;
    w_hat <<     0.0, -w(2),  w(1),
                w(2),   0.0, -w(0),
               -w(1),  w(0),   0.0;
    return w_hat;
}

void SO3::exp(const Mat3 &w_hat)
{
    Mat31 w = vee3(w_hat);
    double o = w.norm();
    double c1,c2;
    // See numerical_test.cpp to justify this thershold
    if ( o < 1e-5){
        // sin(o)/o = 1 - x^2/3! + x^4/5! + O(x^6)
        c1 =  1 - o*o/6.0;
        // (1-cos(o))/o^2 = 0.5 + 1/2*f''*x^2/ + ... , where f'' = 1/12
        c2 = 0.5 - o*o/24.0;
    }
    else
    {
        // Standard case with the well-known Rodriguez formula
        c1 = std::sin(o)/o;
        c2 = (1 - std::cos(o))/o/o;
    }
    R_ << Mat3::Identity() + c1 * w_hat + c2 * w_hat *w_hat;
}

Mat3 SO3::ln(double *ro) const
{
    // Logarithmic mapping of the rotations
    Mat3 res;
    double tr = (R_.trace()-1)*0.5;
    double o = std::acos(tr); //image in [0,pi]
    // We choose a tolerance of this small value since the solution involves R being almost symetric
    if ( tr > -1.0 + 1e0)
    {
        // We will evaluate 3 cases:
        // 1) o (angle) is NaN because trace was exactly 1 plus a small round off, so acos is not defined
        double d1;
        // Special case tr =1  and theta -> 0
        if ( std::isnan(o) )
        {
            d1 = 0.0;
            o = 0.0;
        }
        // 2) incorrect x/sin when approaching 0. The other problematic point o = pi is handled below
        // We choose this value since Taylor improves over the numerical result of the program (see numericap_test.cpp)
        else if( o < 1e-5)
        {
            // Taylor expansion around 0
            d1 = 0.5 + o*o/12;
        }
        // 3) normal case
        else
        {
            d1 = 0.5 * o / std::sin(o);
        }
        res << d1 * ( R_ - R_.transpose());
    }
    else
    {
        // Special case tr = -1  so theta = + pi or multiples
        // Again, we handle nan's assuming they express exact +-pi plus a numerical error
        // The second condition stand for the error in acos. Very close to pi it is better (in error) to assume
        // a rotation of exactly pi
        if ( std::isnan(o) || M_PI - o < 6e-8 )
        {
            o = M_PI;// exact case for theta
        }
        // In this case we assume theta is well calculated, but given the almost symetry conditions
        // on the expansion that allow to solve the problem this way.
        else
        {
            // The result of acos is good enough
            //std::cout << std::setprecision(20) << M_PI - o << std::endl;
        }
        // As we approach pi, the exponent(theta) becomes:
        // R = I + 0 + (1-cos)/o^2)W^2, which evaluated at +pi = 2/pi^2
        // This rotation is almost symmetric R = Rt and W = hat(w)
        // We can consider the first order term sin/o negligible.
        //
        // From here, we know that W^2 = ww^t - theta^2I, (you can span W^2 to see this)
        // which leaves R = I + 2/pi2 (wwt - pi2 I)
        // R+I = 2/pi2 wwt
        // wwt = pi2 / 2 (R+I)
        // so we find the maximum row and apply that formula
        // knowing that norm(w) = pi (theta)
        Mat31 w;
        double d = std::cos(o);

        if( R_(0,0) > R_(1,1) && R_(0,0) > R_(2,2) )
        {
            // For stability, we average the two elements since it is almost symetric
            w << R_(0,0) - d,
                 0.5 * ( R_(0,1) + R_(1,0)),
                 0.5 * ( R_(0,2) + R_(2,0));
        }
        else if( R_(1,1) > R_(0,0) && R_(1,1) > R_(2,2) )
        {
            w << 0.5 * ( R_(1,0) + R_(0,1)),
                 R_(1,1) - d,
                 0.5 * ( R_(1,2) + R_(2,1));
        }
        else
        {
            w << 0.5 * ( R_(2,0) + R_(0,2)),
                 0.5 * ( R_(2,1) + R_(1,2)),
                 R_(2,2) - d;
        }
        // normalize the vector w, such that norm(w) = theta
        double length = w.norm();
        if (length > 0.0)
        {
            w *= o / length;
        }
        else
        {
            w << 0.0, 0.0, 0.0;
        }
        res = hat3(w);

        // NOTE: this does not work for very very small theta, which is expectedsince at pi the axis is undefined
        // The problem of this approach is that we loose the sign of rotation, so we try to estimate it
        // by comparing R with the 1st order of Exp(w) and Exp(-w).
        if( (Mat3::Identity() + res - R_ ).norm() >  (Mat3::Identity() - res - R_).norm() )
        {
            res *= -1.0;
            // Note that o is the absolute value of the angle of rotation (don't flip)
        }
    }
    if (ro != nullptr) *ro = o;
    return res;
}

Mat31 SO3::ln_vee() const
{
    Mat3 w_hat = this->ln();
    return vee3(w_hat);
}

SO3 SO3::inv(void) const
{
    return SO3(R_.transpose());
}

Mat3 SO3::adj() const
{
    return R_;
}

Mat3 SO3::R() const
{
    return R_;
}

Mat3& SO3::ref2R()
{
    return R_;
}

double SO3::distance(const SO3 &rhs) const
{
    return (*this * rhs.inv()).ln_vee().norm();
}

void SO3::print(void) const
{
    std::cout << R_ << std::endl;
}

void SO3::print_lie(void) const
{

    Mat31 w =  this->ln_vee();
    std::cout << w << std::endl;
}

bool mrob::isSO3(Mat3 R)
{
    matData_t det = R.determinant();
    if (det  < 0)
        return false;
    if ( fabs(det - 1.0) > 1e-6)
        return false;
    return true;

}

Mat3 mrob::quat_to_so3(const Eigen::Ref<const Mat41> v)
{

    Eigen::Quaternion<matData_t> q(v);
    //std::cout << "Initial vector : " << v << ", transformed quaternion" << q.vec() << "\n and w = \n" << q.toRotationMatrix() << std::endl;
    return q.normalized().toRotationMatrix();
}

// quaternion q = [qx, qy, qz, qw](Eigen convention)
Mat41 mrob::so3_to_quat(const Eigen::Ref<const Mat3> R)
{
    Eigen::Quaternion<matData_t> q(R);
    Mat41 res;
    res << q.x(), q.y(), q.z(), q.w();
    return res;
}

Mat3 mrob::rpy_to_so3(const Eigen::Ref<const Mat31> v)
{
    Mat3 R;
    R = Eigen::AngleAxisd(v(0), Eigen::Vector3d::UnitX())
          * Eigen::AngleAxisd(v(1), Eigen::Vector3d::UnitY())
          * Eigen::AngleAxisd(v(2), Eigen::Vector3d::UnitZ());
    return R;
}


/********************************************************************************
 * Copyright 2017 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#ifndef RW_MATH_POLYNOMIALND_HPP_
#define RW_MATH_POLYNOMIALND_HPP_

/**
 * @file PolynomialND.hpp
 *
 * \copydoc rw::math::PolynomialND
 */
#if !defined(SWIG)
#include <rw/core/macros.hpp>

#include <Eigen/Core>
#include <sstream>
#include <vector>
#endif

namespace rw { namespace math {
    //! @addtogroup math
#if !defined(SWIG)
    //! @{

#endif
    /**
     * @brief Representation of a polynomial that can have non-scalar coefficients (polynomial
     * matrix).
     *
     * Representation of a polynomial of the following form:
     *
     * @f$
     *  f(x) = C_n x^n + C_(n-1) x^(n-1) + C_2 x^2 + C_1 x + C_0
     * @f$
     *
     * The polynomial is represented as a list of coefficients ordered from lowest-order term to
     * highest-order term, \f$ {c_0,c_1,...,c_n}\f$.
     */
    template< typename Coef, typename Scalar = double > class PolynomialND
    {
      public:
        /**
         * @brief Create polynomial with uninitialized coefficients.
         * @param order [in] the order of the polynomial.
         */
        explicit PolynomialND (std::size_t order) : _coef (std::vector< Coef > (order + 1)) {}

        /**
         * @brief Create polynomial from vector.
         * @param coefficients [in] the coefficients ordered from lowest-order term to highest-order
         * term.
         */
        PolynomialND (const std::vector< Coef >& coefficients) : _coef (coefficients) {}

        /**
         * @brief Create polynomial from other polynomial.
         * @param p [in] the polynomial to copy.
         */
        PolynomialND (const PolynomialND< Coef, Scalar >& p) :
            _coef (std::vector< Coef > (p.order () + 1))
        {
            for (std::size_t i = 0; i <= p.order (); i++)
                _coef[i] = p[i];
        }

        /**
         * @brief Destructor
         */
        virtual ~PolynomialND () {}

        /**
         * @brief Get the order of the polynomial (the highest power).
         * @return the order.
         */
        std::size_t order () const { return _coef.size () - 1; }

        /**
         * @brief Increase the order of this polynomial.
         * @param increase [in] how much to increase the order (default is 1).
         * @param value [in] initialize new coefficients to this value.
         */
        void increaseOrder (std::size_t increase, const Coef& value)
        {
            const std::size_t size = _coef.size ();
            _coef.resize (size + increase);
            for (std::size_t i = size; i < size + increase; i++) {
                _coef[i] = value;
            }
        }
#if !defined(SWIGJAVA)

        /**
         * @brief Increase the order of this polynomial.
         * @param increase [in] how much to increase the order (default is 1).
         * @see increaseOrder(std::size_t,const Coef&) for a version that initializes the new
         * coefficients to a certain value.
         */
#endif
        void increaseOrder (std::size_t increase = 1)
        {
            const std::size_t size = _coef.size ();
            _coef.resize (size + increase);
        }

        /**
         * @brief Evaluate the polynomial using Horner's Method.
         * @param x [in] the input parameter.
         * @return the value \f$ f(x)\f$.
         */
        Coef evaluate (const Scalar& x) const
        {
            // Horner's Method
            Coef res = _coef.back ();
            for (int i = static_cast< int > (_coef.size () - 2); i >= 0; i--)
                res = _coef[i] + res * x;
            return res;
        }

        /**
         * @brief Evaluate the first \b n derivatives of the polynomial using Horner's Method.
         * @param x [in] the input parameter.
         * @param n [in] the number of derivatives to find (default is the first derivative only)
         * @return a vector of values \f$ {f(x),\dot{f}(x),\ddot{f}(x),\cdots}\f$.
         */
        std::vector< Coef > evaluateDerivatives (const Scalar& x, std::size_t n = 1) const
        {
            // Horner's Method
            std::vector< Coef > res (n + 1);
            res[0] = _coef.back ();
            for (int i = static_cast< int > (_coef.size () - 2); i >= 0; i--) {
                int minJ = static_cast< int > (std::min< std::size_t > (n, _coef.size () - 1 - i));
                for (int j = minJ; j > 0; j--) {
                    res[j] = res[j - 1] + res[j] * x;
                }
                res[0] = _coef[i] + res[0] * x;
            }
            Scalar k = 1;
            for (std::size_t i = 2; i <= n; i++) {
                const Scalar kInit = k;
                for (std::size_t j = 0; j < i - 1; j++) {
                    k += kInit;
                }
                res[i] *= k;
            }
            return res;
        }

        /**
         * @brief Perform deflation of polynomial.
         * @param x [in] a root of the polynomial.
         * @return a new polynomial of same order minus one.
         * @note There is no check that the given root is in fact a root of the polynomial.
         */
        PolynomialND< Coef, Scalar > deflate (const Scalar& x) const
        {
            // Horner Method
            std::size_t no = order () - 1;
            PolynomialND< Coef, Scalar > res (no);
            res[no] = _coef.back ();
            for (int i = (int) no - 1; i >= 0; i--) {
                res[i] = x * res[i + 1] + _coef[i + 1];
            }
            return res;
        }

        /**
         * @brief Get the derivative polynomial.
         * @param n [in] gives the n'th derivative (default is n=1).
         * @return a new polynomial of same order minus one.
         * @note To evaluate derivatives use the evaluate derivative method which is more precise.
         */
        PolynomialND< Coef, Scalar > derivative (std::size_t n = 1) const
        {
            if (n == 0)
                return *this;
            std::size_t no = order () - 1;
            PolynomialND< Coef, Scalar > der (no);
            for (std::size_t i = 1; i <= order (); i++)
                der[i - 1] = (Coef) (_coef[i] * double(i));
            return der.derivative (n - 1);
        }

        /**
         * @name Coefficient access operators.
         * Operators used to access coefficients.
         */
#if !defined(SWIG)
        ///@{

        /**
         * @brief Get specific coefficient.
         * @param i [in] the power of the term to get coefficient for.
         * @return the coefficient.
         */
        const Coef& operator() (std::size_t i) const
        {
            if (i > order ()) {
                std::stringstream str;
                str << "Polynomial of order " << order () << " has no coefficient with index " << i;
                RW_THROW (str.str ());
            }
            return _coef[i];
        }

        /**
         * @brief Get specific coefficient.
         * @param i [in] the power of the term to get coefficient for.
         * @return the coefficient.
         */
        Coef& operator() (size_t i)
        {
            if (i > order ()) {
                std::stringstream str;
                str << "Polynomial of order " << order () << " has no coefficient with index " << i;
                RW_THROW (str.str ());
            }
            return _coef[i];
        }

        /**
         * @brief Get specific coefficient.
         * @param i [in] the power of the term to get coefficient for.
         * @return the coefficient.
         */
        const Coef& operator[] (size_t i) const
        {
            if (i > order ()) {
                std::stringstream str;
                str << "Polynomial of order " << order () << " has no coefficient with index " << i;
                RW_THROW (str.str ());
            }
            return _coef[i];
        }

        /**
         * @brief Get specific coefficient.
         * @param i [in] the power of the term to get coefficient for.
         * @return the coefficient.
         */
        Coef& operator[] (size_t i)
        {
            if (i > order ()) {
                std::stringstream str;
                str << "Polynomial of order " << order () << " has no coefficient with index " << i;
                RW_THROW (str.str ());
            }
            return _coef[i];
        }
#else

        ARRAYOPERATOR (Coef);

#endif
#if !defined(SWIG)
///@}
#endif

        /**
         * @name Arithmetic operators between polynomial and scalars.
         * Operators used to do arithmetic with scalars.
         */

#if !defined(SWIG)
        ///@{
#endif
        /**
         * @brief Scalar multiplication
         * @param s [in] scalar to multiply with.
         * @return new polynomial after multiplication.
         */
        const PolynomialND< Coef, Scalar > operator* (Scalar s) const
        {
            PolynomialND< Coef, Scalar > pol (order ());
            for (std::size_t i = 0; i <= order (); i++) {
                pol[i] = _coef[i] * s;
            }
            return pol;
        }

        /**
         * @brief Scalar division
         * @param s [in] scalar to divide with.
         * @return new polynomial after division.
         */
        const PolynomialND< Coef, Scalar > operator/ (Scalar s) const
        {
            PolynomialND< Coef, Scalar > pol (order ());
            for (std::size_t i = 0; i <= order (); i++) {
                pol[i] = _coef[i] / s;
            }
            return pol;
        }

        /**
         * @brief Scalar multiplication
         * @param s [in] the scalar to multiply with.
         * @return reference to same polynomial with changed coefficients.
         */
        PolynomialND< Coef, Scalar >& operator*= (Scalar s)
        {
            for (std::size_t i = 0; i <= order (); i++) {
                _coef[i] *= s;
            }
            return *this;
        }

        /**
         * @brief Scalar division
         * @param s [in] the scalar to divide with.
         * @return reference to same polynomial with changed coefficients.
         */
        PolynomialND< Coef, Scalar >& operator/= (Scalar s)
        {
            for (std::size_t i = 0; i <= order (); i++) {
                _coef[i] /= s;
            }
            return *this;
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar multiplication
         * @param s [in] scalar to multiply with.
         * @param p [in] polynomial to multiply with.
         * @return new polynomial after multiplication.
         */
        friend const PolynomialND< Coef, Scalar > operator* (Scalar s,
                                                             const PolynomialND< Coef, Scalar >& p)
        {
            PolynomialND< Coef, Scalar > pol (p.order ());
            for (std::size_t i = 0; i <= p.order (); i++) {
                pol[i] = p[i] * s;
            }
            return pol;
        }
#endif
#if !defined(SWIG)
        ///@}
#endif

        /**
         * @name Arithmetic operators between polynomials.
         * Operators used to do arithmetic between two polynomials.
         */

#if !defined(SWIG)
        ///@{
#endif

        /**
         * @brief Polynomial subtraction.
         * @param b [in] polynomial of to subtract.
         * @return new polynomial after subtraction.
         */
        const PolynomialND< Coef, Scalar > operator- (const PolynomialND< Coef, Scalar >& b) const
        {
            const std::size_t thisOrder = order ();
            const std::size_t bOrder    = b.order ();
            if (bOrder > thisOrder) {
                PolynomialND< Coef, Scalar > pol (bOrder);
                for (std::size_t i = 0; i <= thisOrder; i++) {
                    pol[i] = (*this)[i] - b[i];
                }
                for (std::size_t i = thisOrder + 1; i <= bOrder; i++) {
                    pol[i] = -b[i];
                }
                return pol;
            }
            else {
                PolynomialND< Coef, Scalar > pol (thisOrder);
                for (std::size_t i = 0; i <= bOrder; i++) {
                    pol[i] = (*this)[i] - b[i];
                }
                for (std::size_t i = bOrder + 1; i <= thisOrder; i++) {
                    pol[i] = (*this)[i];
                }
                return pol;
            }
        }

        /**
         * @brief Polynomial subtraction.
         * @param b [in] polynomial to subtract.
         * @return same polynomial with different coefficients after subtraction.
         */
        PolynomialND< Coef, Scalar >& operator-= (const PolynomialND< Coef, Scalar >& b)
        {
            const std::size_t thisOrder = order ();
            const std::size_t bOrder    = b.order ();
            if (bOrder > thisOrder) {
                increaseOrder (bOrder - thisOrder);
                for (std::size_t i = 0; i <= thisOrder; i++) {
                    (*this)[i] -= b[i];
                }
                for (std::size_t i = thisOrder + 1; i <= bOrder; i++) {
                    (*this)[i] = -b[i];
                }
            }
            else {
                for (std::size_t i = 0; i <= bOrder; i++) {
                    (*this)[i] -= b[i];
                }
            }
            return *this;
        }

        /**
         * @brief Polynomial addition.
         * @param b [in] polynomial to add.
         * @return new polynomial after addition.
         */
        const PolynomialND< Coef, Scalar > operator+ (const PolynomialND< Coef, Scalar >& b) const
        {
            const std::size_t thisOrder = order ();
            const std::size_t bOrder    = b.order ();
            if (bOrder > thisOrder) {
                PolynomialND< Coef, Scalar > pol (bOrder);
                for (std::size_t i = 0; i <= thisOrder; i++) {
                    pol[i] = (*this)[i] + b[i];
                }
                for (std::size_t i = thisOrder + 1; i <= bOrder; i++) {
                    pol[i] = b[i];
                }
                return pol;
            }
            else {
                PolynomialND< Coef, Scalar > pol (thisOrder);
                for (std::size_t i = 0; i <= bOrder; i++) {
                    pol[i] = (*this)[i] + b[i];
                }
                for (std::size_t i = bOrder + 1; i <= thisOrder; i++) {
                    pol[i] = (*this)[i];
                }
                return pol;
            }
        }

        /**
         * @brief Polynomial addition.
         * @param b [in] polynomial to add.
         * @return same polynomial with different coefficients after addition.
         */
        PolynomialND< Coef, Scalar >& operator+= (const PolynomialND< Coef, Scalar >& b)
        {
            const std::size_t thisOrder = order ();
            const std::size_t bOrder    = b.order ();
            if (bOrder > thisOrder) {
                increaseOrder (bOrder - thisOrder);
                for (std::size_t i = 0; i <= thisOrder; i++) {
                    (*this)[i] += b[i];
                }
                for (std::size_t i = thisOrder + 1; i <= bOrder; i++) {
                    (*this)[i] = b[i];
                }
            }
            else {
                for (std::size_t i = 0; i <= bOrder; i++) {
                    (*this)[i] += b[i];
                }
            }
            return *this;
        }

        /**
         * @brief Polynomial multiplication.
         *
         * A convolution of the coefficients is used.
         * Notice that more efficient algorithms exist for polynomials with scalar coefficients.
         *
         * @param b [in] polynomial to multiply. Post-multiplication is used - the dimensions must
         * match.
         * @return new polynomial after multiplication.
         */
        template< typename OutCoef, typename Coef2 = Coef >
        PolynomialND< OutCoef, Scalar > multiply (const PolynomialND< Coef2, Scalar >& b) const
        {
            const std::size_t ord = order () + b.order ();
            PolynomialND< OutCoef, Scalar > pol (ord);
            for (std::size_t k = 0; k <= ord; k++) {
                const std::size_t firstJ = (k < b.order ()) ? 0 : k - b.order ();
                pol[ord - k]             = _coef[order () - firstJ] * b[b.order () - (k - firstJ)];
                for (std::size_t j = firstJ + 1; j <= std::min (k, order ()); j++) {
                    pol[ord - k] += _coef[order () - j] * b[b.order () - (k - j)];
                }
            }
            return pol;
        }

        /**
         * @brief Multiply with a coefficient.
         *
         * Each coefficient is post-multiplied with the given coefficient.
         *
         * @param b [in] coefficient to multiply with. Post-multiplication is used - the dimensions
         * must match.
         * @return new polynomial after multiplication.
         */
        template< typename OutCoef, typename Coef2 = Coef >
        PolynomialND< OutCoef, Scalar > multiply (const Coef2& b) const
        {
            PolynomialND< OutCoef, Scalar > pol (order ());
            for (std::size_t i = 0; i <= order (); i++) {
                pol[i] = _coef[i] * b;
            }
            return pol;
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Assignment.
         * @param b [in] the polynomial to take coefficients from.
         */
        void operator= (const PolynomialND< Coef, Scalar >& b)
        {
            _coef.resize (b.order () + 1);
            for (size_t i = 0; i <= b.order (); i++)
                _coef[i] = b[i];
        }
#endif
#if !defined(SWIG)
///@}
#endif

        /**
         * @brief Negate coefficients.
         * @return new polynomial with coefficients negated.
         */
        const PolynomialND< Coef, Scalar > operator- () const
        {
            PolynomialND< Coef, Scalar > pol (order ());
            for (std::size_t i = 0; i <= order (); i++) {
                pol[i] = (Coef) (_coef[i] * double(-1));
            }
            return pol;
        }
#if !defined(SWIG)
        /**
         * @brief Printing polynomial to stream.
         * @param out [in/out] the stream to write to.
         * @param p [in] the polynomail to print.
         * @return the same ostream as out parameter.
         */
        friend std::ostream& operator<< (std::ostream& out, const PolynomialND< Coef, Scalar >& p)
        {
            out << "Polynomial: ";
            for (size_t i = p.order (); i > 0; i--)
                out << p[i] << " x^" << i << " + ";
            out << p[0];
            return out;
        }
#else
#define tmp Coef, Scalar
        TOSTRING (rw::math::PolynomialND< tmp >);
#endif

        /**
         * @brief Check if polynomials are equal.
         * @param b [in] the polynomial to compare with.
         * @return true if equal, false if not.
         */
        bool operator== (const PolynomialND< Coef, Scalar >& b) const
        {
            for (size_t i = 0; i <= order (); i++)
                if (_coef[i] != b[i])
                    return false;
            return true;
        }

      protected:
        //! @brief The coefficient vector.
        std::vector< Coef > _coef;
    };

    /**
     * @brief Multiply 3D polynomial matrix with 3D polynomial vector.
     * @param A [in] the matrix expression.
     * @param b [in] the vector expression.
     * @return a 3D polynomial vector.
     */
    PolynomialND< Eigen::Vector3d > operator* (const PolynomialND< Eigen::Matrix3d >& A,
                                               const PolynomialND< Eigen::Vector3d >& b);

    /**
     * @brief Multiply 3D polynomial vector with 3D polynomial matrix.
     * @param a [in] the vector expression.
     * @param A [in] the matrix expression.
     * @return a 3D polynomial vector.
     */
    PolynomialND< Eigen::Matrix< double, 1, 3 > >
    operator* (const PolynomialND< Eigen::Matrix< double, 1, 3 > >& a,
               const PolynomialND< Eigen::Matrix3d >& A);
# if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix3d>&, const
    //! PolynomialND<Eigen::Vector3d>&)

    #endif
    PolynomialND< Eigen::Vector3d > operator* (const PolynomialND< Eigen::Matrix3d >& A,
                                               const Eigen::Vector3d& b);
#if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix<double,1,3> >&, const
    //! PolynomialND<Eigen::Matrix3d>&)

    #endif
    PolynomialND< Eigen::Matrix< double, 1, 3 > >
    operator* (const PolynomialND< Eigen::Matrix< double, 1, 3 > >& a, const Eigen::Matrix3d& A);

#if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix3d>&, const
    //! PolynomialND<Eigen::Vector3d>&)

    #endif
    PolynomialND< Eigen::Vector3f, float >
    operator* (const PolynomialND< Eigen::Matrix3f, float >& A,
               const PolynomialND< Eigen::Vector3f, float >& b);
#if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix<double,1,3> >&, const
    //! PolynomialND<Eigen::Matrix3d>&)

    #endif
    PolynomialND< Eigen::Matrix< float, 1, 3 >, float >
    operator* (const PolynomialND< Eigen::Matrix< float, 1, 3 >, float >& a,
               const PolynomialND< Eigen::Matrix3f, float >& A);
#if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix3d>&, const Eigen::Vector3d&)

    #endif
    PolynomialND< Eigen::Vector3f, float >
    operator* (const PolynomialND< Eigen::Matrix3f, float >& A, const Eigen::Vector3f& b);

#if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix<double,1,3> >&, const Eigen::Matrix3d&)

    #endif
    PolynomialND< Eigen::Matrix< float, 1, 3 >, float >
    operator* (const PolynomialND< Eigen::Matrix< float, 1, 3 >, float >& a,
               const Eigen::Matrix3f& A);

    //! @}
}}    // namespace rw::math

#endif /* RW_MATH_POLYNOMIALND_HPP_ */


#ifndef BFGS_HPP
#define BFGS_HPP

#include <Eigen/Dense>
#include <limits>

/** Damped BFGS update
 * Implements "Procedure 18.2 Damped BFGS updating for SQP" form Numerical Optimization by Nocedal.
 *
 * @param[in,out]   B hessian matrix, is updated by this function
 * @param[in]       s step vector (x - x_prev)
 * @param[in]       y gradient change (grad - grad_prev)
 */
template <typename Mat, typename Vec>
void BFGS_update(Mat& B, const Vec& s, const Vec& y) {
    using Scalar = typename Mat::Scalar;
    Scalar sy, sr, sBs;
    Vec Bs, r;

    Bs.noalias() = B * s;
    sBs = s.dot(Bs);
    sy = s.dot(y);

    if (sy < 0.2 * sBs) {
        // damped update to enforce positive definite B
        Scalar theta;
        theta = 0.8 * sBs / (sBs - sy);
        r.noalias() = theta * y + (1 - theta) * Bs;
        sr = theta * sy + (1 - theta) * sBs;
    } else {
        // unmodified BFGS
        r = y;
        sr = sy;
    }

    if (sr < std::numeric_limits<Scalar>::epsilon()) {
        return;
    }

    B.noalias() += -Bs * Bs.transpose() / sBs + r * r.transpose() / sr;
}

// extern template void BFGS_update<Eigen::MatrixXd, Eigen::VectorXd>(Eigen::MatrixXd& B,
//                                                                    const Eigen::VectorXd& s,
//                                                                    const Eigen::VectorXd& y)

#endif /* BFGS_HPP */


#include "catch.hpp"

#include <vector>

#include <boost/multi_array.hpp>

#include "Utils.h"
#include "fakeit.hpp"

using namespace boost;

template<typename T, long unsigned int N>
using ma = multi_array<T, N>;
template<typename T, long unsigned int N>
using I = typename ma<T, N>::index;
typedef multi_array_types::index_range R;

TEST_CASE("Boost.Multiarray Examples", "[example]")
{
  int           Nx = 2, Ny = 3, Nz = 4;
  ma<double, 3> cube(extents[2][3][4]);

  for (int i = 0; i < Nx; ++i)
    for (int j = 0; j < Ny; ++j)
      for (int k = 0; k < Nz; ++k) cube[i][j][k] = i * j * k;

  for (int i = 0; i < Nx; ++i)
    for (int j = 0; j < Ny; ++j)
      for (int k = 0; k < Nz; ++k) CHECK(cube[i][j][k] == Approx(i * j * k));

  auto plane = cube[indices[R()][1][R()]];

  for (int i = 0; i < Nx; ++i)
    for (int k = 0; k < Nz; ++k) CHECK(plane[i][k] == Approx(i * 1 * k));

  plane[0][0] = -10;

  CHECK(plane[0][0] == Approx(-10));
  CHECK(cube[0][0][0] == Approx(0));
  CHECK(cube[0][1][0] == Approx(-10));

  cube[0][1][0] = 0 * 1 * 0;

  auto line = plane[indices[1][R()]];

  CHECK(line[0] == plane[1][0]);
  CHECK(line[0] == cube[1][1][0]);

  line[2] = -10;

  CHECK(line[2] == -10);
  CHECK(plane[1][2] == -10);
  CHECK(cube[1][1][2] == -10);
}

TEST_CASE("Boost.Multiarray Complex Matrix", "[example]")
{
  int N = 3;

  ma<double, 3> A(extents[N][N][2]);

  A[0][0][0] = 1;
  A[0][0][1] = 2;
  A[0][1][0] = 1;
  A[0][1][1] = 2;
  A[0][2][0] = 1;
  A[0][2][1] = 2;
  A[1][0][0] = 1;
  A[1][0][1] = 2;
  A[1][1][0] = 1;
  A[1][1][1] = 2;
  A[1][2][0] = 1;
  A[1][2][1] = 2;
  A[2][0][0] = 1;
  A[2][0][1] = 2;
  A[2][1][0] = 1;
  A[2][1][1] = 2;
  A[2][2][0] = 1;
  A[2][2][1] = 2;

  auto Ar = A[indices[R()][R()][0]];
  auto Ai = A[indices[R()][R()][1]];

  for (int i = 0; i < N; ++i)
    for (int j = 0; j < N; ++j) CHECK(Ar[i][j] == 1);

  for (int i = 0; i < N; ++i)
    for (int j = 0; j < N; ++j) CHECK(Ai[i][j] == 2);

  int  i  = 0;
  auto AA = A[indices[R()][R()][i]];

  for (int i = 0; i < N; ++i)
    for (int j = 0; j < N; ++j) CHECK(AA[i][j] == 1);
}

TEST_CASE("Boost.Multiarray Dynamic Slicing", "[example]")
{
  int N = 3;

  ma<double, 2> A(extents[N][N]);
  for (int i = 0; i < N; i++)
    for (int j = 0; j < N; j++) A[i][j] = i * j;

  for (int i = 0; i < N; ++i)
    for (int j = 0; j < N; ++j) CHECK(A[i][j] == i * j);

  int  d  = 1;
  auto Av = A[indices[R()][d]];

  for (int i = 0; i < N; ++i) CHECK(Av[i] == i * 1);
}

TEST_CASE("Boost.Multiarray Index Manipulation", "[example]")
{
  int           N = 3;
  ma<double, 2> A(extents[N][N]);
  for (int i = 0; i < N; i++)
    for (int j = 0; j < N; j++) A[i][j] = i * j;

  CHECK(A.data() == A.origin());

  array<int, 2> off = {1, 2};
  A.reindex(off);
  // now A[1][2] refers to A[0][0]
  // now A[2][3] refers to A[1][1]
  // etc.
  CHECK(A[1][2] == 0);
  CHECK(A[1][2] == 0);
  CHECK(A[2][3] == 1);

  for (int i = 1; i < N + 1; i++) {
    for (int j = 2; j < N + 2; j++) {
      CHECK(A[i][j] == (i - 1) * (j - 2));
    }
  }

  // origin should now be shifted
  CHECK(A.data() != A.origin());

  off = {-1, -2};
  A.reindex(off);
  // now A[-1][-2] refers to A[0][0]
  // now A[-2][-3] refers to A[1][1]
  // etc.
  CHECK(A[-1][-2] == 0);
  CHECK(A[-1][-2] == 0);
  CHECK(A[0][-1] == 1);

  for (int i = -1; i < N - 1; i++) {
    for (int j = -2; j < N - 2; j++) {
      CHECK(A[i][j] == (i + 1) * (j + 2));
    }
  }

  // origin should still be shifted
  CHECK(A.data() != A.origin());

  off = {0, 0};
  A.reindex(off);

  // origin should be back data()
  CHECK(A.data() == A.origin());
}

TEST_CASE("Boost.Multiarray Storage Order", "")
{
  int           N = 3;
  ma<double, 2> A(extents[N][N], c_storage_order());
  ma<double, 2> B(extents[N][N], fortran_storage_order());

  for (int i = 0; i < N; i++) {
    for (int j = 0; j < N; j++) {
      A[i][j] = 10 * i + j;
      B[i][j] = 10 * i + j;
    }
  }

  // have a 3x3 matrix
  //
  //   0  1  2
  //
  //  10 11 12
  //
  //  20 21 22
  //
  //

  CHECK(A.data()[0] == 0);
  CHECK(A.data()[1] == 1);
  CHECK(A.data()[2] == 2);
  CHECK(A.data()[3] == 10);

  CHECK(B.data()[0] == 0);
  CHECK(B.data()[1] == 10);
  CHECK(B.data()[2] == 20);
  CHECK(B.data()[3] == 1);
}


#define BOOST_TEST_MODULE test_optionsInstances

#include <boost/test/unit_test.hpp>
#include <OptionInstances/CallOption.h>
#include <OptionInstances/PutOption.h>

BOOST_AUTO_TEST_SUITE(optionInstances_test_suite)

    BOOST_AUTO_TEST_CASE(option_getTimeToMaturity) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double strike = 50.0;
        const double rate = 3.66 / 100;
        const boost::gregorian::date startDate = boost::gregorian::from_simple_string("2013-01-01");
        const boost::gregorian::date endDate = boost::gregorian::from_simple_string("2016-01-01");
        Actual_365 dayCountCalculator = Actual_365();

        const double spot_price = 50.0;
        double volatility = 62.0 / 100;

        Asset underlyingAsset{spot_price, volatility};

        CallOption my_option{strike, rate, startDate, endDate, dayCountCalculator, underlyingAsset};
        double theoretical_value = 3.0;
        double calculated_value = my_option.getTimeToMaturity();

        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-2));

    }

    BOOST_AUTO_TEST_CASE(call_getReceiving) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double strike = 50.0;
        const double rate = 3.66 / 100;
        const boost::gregorian::date startDate = boost::gregorian::from_simple_string("2013-01-01");
        const boost::gregorian::date endDate = boost::gregorian::from_simple_string("2016-01-01");
        Actual_365 dayCountCalculator = Actual_365();

        const double spot_price = 50.0;
        double volatility = 62.0 / 100;

        Asset underlyingAsset{spot_price, volatility};

        CallOption my_option{strike, rate, startDate, endDate, dayCountCalculator, underlyingAsset};
        double theoretical_value = 36.93239658;
        double calculated_value = my_option.getReceiving();

        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));

    }

    BOOST_AUTO_TEST_CASE(call_getPaying) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double strike = 50.0;
        const double rate = 3.66 / 100;
        const boost::gregorian::date startDate = boost::gregorian::from_simple_string("2013-01-01");
        const boost::gregorian::date endDate = boost::gregorian::from_simple_string("2016-01-01");
        Actual_365 dayCountCalculator = Actual_365();

        const double spot_price = 50.0;
        double volatility = 62.0 / 100;

        Asset underlyingAsset{spot_price, volatility};

        CallOption my_option{strike, rate, startDate, endDate, dayCountCalculator, underlyingAsset};
        double theoretical_value = 14.86907831;
        double calculated_value = my_option.getPaying();

        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));

    }

    BOOST_AUTO_TEST_CASE(call_price) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double strike = 50.0;
        const double rate = 3.66 / 100;
        const boost::gregorian::date startDate = boost::gregorian::from_simple_string("2013-01-01");
        const boost::gregorian::date endDate = boost::gregorian::from_simple_string("2016-01-01");
        Actual_365 dayCountCalculator = Actual_365();

        const double spot_price = 50.0;
        double volatility = 62.0 / 100;

        Asset underlyingAsset{spot_price, volatility};

        CallOption my_option{strike, rate, startDate, endDate, dayCountCalculator, underlyingAsset};
        double theoretical_value = 22.06331827;
        double calculated_value = my_option.price();

        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));

    }

    BOOST_AUTO_TEST_CASE(put_getReceiving) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double strike = 50.0;
        const double rate = 3.66 / 100;
        const boost::gregorian::date startDate = boost::gregorian::from_simple_string("2013-01-01");
        const boost::gregorian::date endDate = boost::gregorian::from_simple_string("2016-01-01");
        Actual_365 dayCountCalculator = Actual_365();

        const double spot_price = 50.0;
        double volatility = 62.0 / 100;

        Asset underlyingAsset{spot_price, volatility};

        PutOption my_option{strike, rate, startDate, endDate, dayCountCalculator, underlyingAsset};
        double theoretical_value = 29.93158769;
        double calculated_value = my_option.getReceiving();

        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));

    }

    BOOST_AUTO_TEST_CASE(put_getPaying) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double strike = 50.0;
        const double rate = 3.66 / 100;
        const boost::gregorian::date startDate = boost::gregorian::from_simple_string("2013-01-01");
        const boost::gregorian::date endDate = boost::gregorian::from_simple_string("2016-01-01");
        Actual_365 dayCountCalculator = Actual_365();

        const double spot_price = 50.0;
        double volatility = 62.0 / 100;

        Asset underlyingAsset{spot_price, volatility};

        PutOption my_option{strike, rate, startDate, endDate, dayCountCalculator, underlyingAsset};
        double theoretical_value = 13.06760342;
        double calculated_value = my_option.getPaying();

        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));

    }

    BOOST_AUTO_TEST_CASE(put_price) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double strike = 50.0;
        const double rate = 3.66 / 100;
        const boost::gregorian::date startDate = boost::gregorian::from_simple_string("2013-01-01");
        const boost::gregorian::date endDate = boost::gregorian::from_simple_string("2016-01-01");
        Actual_365 dayCountCalculator = Actual_365();

        const double spot_price = 50.0;
        double volatility = 62.0 / 100;

        Asset underlyingAsset{spot_price, volatility};

        PutOption my_option{strike, rate, startDate, endDate, dayCountCalculator, underlyingAsset};
        double theoretical_value = 16.86398427;
        double calculated_value = my_option.price();

        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));

    }

BOOST_AUTO_TEST_SUITE_END()


#include "../Util/MercatorUtil.h"
#include "../Util/ComputeAngle.h"
#include "../Util/SimpleLogger.h"

#include <osrm/Coordinate.h>

#include "ConcaveHull.h"

#include <algorithm>
#include <set>
#include <vector>
#include <limits>
#include <valarray>
#include <boost/foreach.hpp>

float getAngle(const FixedPointCoordinate& prev, const FixedPointCoordinate& curr, const FixedPointCoordinate& next, float *outAngle) {
	int prev_x = prev.lon , prev_y = prev.lat;
	int curr_x = curr.lon , curr_y = curr.lat;
	int next_x = next.lon , next_y = next.lat;
	if (next_x == curr_x && next_y == curr_y)
		return -9000.f;
	if (next_x == prev_x && next_y == prev_y)
		return -360.f;

	int a_x = curr_x - prev_x;
	int a_y = curr_y - prev_y;
	int b_x = next_x - curr_x;
	int b_y = next_y - curr_y;
	float vect = a_x*b_y - b_x*a_y;
	float scal = a_x*b_x + a_y*b_y;
	float angle = 0;
	if (scal == 0){
		if (vect > 0)
			angle = 90.f;
		if (vect < 0)
			angle = -90.f;
	}
	else {
		angle = atan(vect / scal) * 180.f / M_PI;
		if (scal < 0){
			if (vect >= 0)
				angle += 180.f;
			if (vect < 0)
				angle -= 180.f;
		}
	}
	if (angle == 360.f)
		angle = 0;
	*outAngle = 180.f - angle;
	return *outAngle;
}

int ccw(const FixedPointCoordinate& p0, const FixedPointCoordinate& p1, const FixedPointCoordinate& p2) {
	int dx1 = p1.lon - p0.lon;
	int dy1 = p1.lat - p0.lat;
	int dx2 = p2.lon - p0.lon;
	int dy2 = p2.lat - p0.lat;
	int d = dx1*dy2 - dy1*dx2;
	if (d > 0) return 1;
	if (d < 0) return -1;
	if ((dx1*dx2 < 0) || (dy1*dy2 < 0)) return -1;
	if ((dx1*dx1 + dy1*dy1) < (dx2*dx2 + dy2*dy2)) return 1;
	return 0;
}

bool intersect(const FixedPointCoordinate& p1, const FixedPointCoordinate& p2, const FixedPointCoordinate& q1, const FixedPointCoordinate& q2) {
	if ((p1.lon == q1.lon && p1.lat == q1.lat || p2.lon == q2.lon && p2.lat == q2.lat) ||
		(p1.lon == q2.lon && p1.lat == q2.lat || p2.lon == q1.lon && p2.lat == q1.lat))
		return false;

	return (ccw(p1, p2, q1) * ccw(p1, p2, q2) <= 0) && (ccw(q1, q2, p1) * ccw(q1, q2, p2) <= 0);
}

bool intersect(const std::vector<FixedPointCoordinate>& hull, const FixedPointCoordinate& next) {
	int last = hull.size() - 1;
	for (int i = 1; i < last; i++){
		if (intersect(hull[i - 1], hull[i], hull[last], next))
			return true;
	}
	return false;
}

inline bool isNear(const FixedPointCoordinate& a, const FixedPointCoordinate& b, int maxLat, int maxLon){
	return (abs(a.lat - b.lat) < maxLat && abs(a.lon - b.lon) < maxLon);
}

void calculateHull(const std::vector<FixedPointCoordinate>& points, std::vector<FixedPointCoordinate>& hull, int maxLat, int maxLon) {

	hull.clear();

	if (points.empty())
		return;

	//hull = points;
	//return;
	std::vector<int> counts(points.size(), 0);

	int start = 0;
	for (int i = 1; i < points.size(); i++) {
		if (points[i].lat < points[start].lat)
			start = i;
	}
	hull.push_back(points[start]);

	int curr = start;
	int next = -1;
	const FixedPointCoordinate init(points[start].lat - 10, points[start].lon);
	const FixedPointCoordinate *prev = &init;

#ifdef MyDEBUG
	SimpleLogger().Write(logDEBUG) << "Init: " << init;
	SimpleLogger().Write(logDEBUG) << "Start: " << start << ": " << points[start];
#endif
	while (next != start) {
		next = -1;
		float max_angle = -10000.f;
		int min_rot = std::numeric_limits<int>::max();
		for (int i = 0; i < points.size(); i++) {
#ifdef MyDEBUG
			SimpleLogger().Write(logDEBUG) << i << ": " << points[i]
				<< " dist: " << FixedPointCoordinate::ApproximateEuclideanDistance(points[i], points[curr])
				<< " [" << (FixedPointCoordinate::ApproximateEuclideanDistance(points[i], points[curr]) < 1.2 * MAX_POINTS_DIST ? "v" : " ") << "]"
				<< " rough: lat " << abs(points[i].lat - points[curr].lat) << " lon " << abs(points[i].lon - points[curr].lon)
				<< " [" << (isNear(points[i], points[curr], maxLat, maxLon) ? "v" : " ") << "]"

				<< " angle: " << getAngle(*prev, points[curr], points[i])
				//<< " rot: " << getRot(*prev, points[curr], points[i])
				<< " intersect:" << intersect(hull, points[i]);
#endif
			float angle = -10000.f;
			if (i != curr && isNear(points[i], points[curr], maxLat, maxLon)
				&& getAngle(*prev, points[curr], points[i], &angle) > max_angle
				&& !intersect(hull, points[i]))
			{
				max_angle = angle;
				next = i;
			}
		}
		//BOOST_ASSERT(next >= 0);
		if (next < 0) {
			SimpleLogger().Write(logWARNING) << "Error in hull calculations.";
			hull.push_back(points[start]);
			return;
		}
		hull.push_back(points[next]);
		if (++counts[next] > 4){
			SimpleLogger().Write(logWARNING) << "Something goes wrong in hull calculations.";
			hull.push_back(points[start]);
			return;
		}

#ifdef MyDEBUG
		SimpleLogger().Write(logDEBUG) << curr << " -> " << next;
#endif
		prev = &points[curr];
		curr = next;
	}
	return;
};

void concaveHull(const std::set<FixedPointCoordinate>& coordinates, std::vector<FixedPointCoordinate>& hull) {

	if (coordinates.empty())
		return;

	const FixedPointCoordinate& start = *coordinates.begin();

	// calculate scale by lat and scale by lon for current geographic area
	FixedPointCoordinate south(start.lat - 100, start.lon);
	FixedPointCoordinate east(start.lat, start.lon - 100);
	int maxLat = 100 * MAX_POINTS_DIST / FixedPointCoordinate::ApproximateDistance(south, start);
	int maxLon = 100 * MAX_POINTS_DIST / FixedPointCoordinate::ApproximateDistance(east, start);

	std::vector<FixedPointCoordinate> points;

	points.reserve(coordinates.size() + 1);
	int min_point = 0, i = 0;
	BOOST_FOREACH(FixedPointCoordinate coord, coordinates)
	{
		points.push_back(coord);
		if (points[i++].lat < points[min_point].lat)
			min_point = i;
	}
	south.lat = points[min_point].lat - maxLat;
	south.lon = points[min_point].lon;
	points.push_back(south);

	calculateHull(points, hull, maxLat*1.2, maxLon*1.2);
	hull[0] = hull[1];
	if (hull.size() > 1)
		hull[hull.size() - 1] = hull[hull.size() - 2];
	SimpleLogger().Write(logINFO) << "First hull: " << hull.size() << " points. Expanding...";

	int dLat = maxLat / 1.4142135623730950488016887242097;
	int dLon = maxLon / 1.4142135623730950488016887242097;

	std::vector<FixedPointCoordinate> expanded;
	expanded.reserve((hull.size() - 1) * 8);
	for (int i = 0; i < hull.size() - 1; i++) {
		FixedPointCoordinate p0(hull[i].lat + maxLat, hull[i].lon);
		expanded.push_back(p0);
		FixedPointCoordinate p1(hull[i].lat - maxLat, hull[i].lon);
		expanded.push_back(p1);
		FixedPointCoordinate p2(hull[i].lat, hull[i].lon + maxLon);
		expanded.push_back(p2);
		FixedPointCoordinate p3(hull[i].lat, hull[i].lon - maxLon);
		expanded.push_back(p3);
		FixedPointCoordinate p4(hull[i].lat + dLat, hull[i].lon + dLon);
		expanded.push_back(p4);
		FixedPointCoordinate p5(hull[i].lat - dLat, hull[i].lon - dLon);
		expanded.push_back(p5);
		FixedPointCoordinate p6(hull[i].lat - dLat, hull[i].lon + dLon);
		expanded.push_back(p6);
		FixedPointCoordinate p7(hull[i].lat + dLat, hull[i].lon - dLon);
		expanded.push_back(p7);
	}
	calculateHull(expanded, hull, maxLat*1.2, maxLon*1.2);

}


#include <boost/gil/typedefs.hpp>
#include <boost/gil/image_processing/numeric.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>
#include <string>
#include <iostream>

namespace gil = boost::gil;

int main(int argc, char* argv[])
{
    if (argc != 5)
    {
        std::cerr << "usage: " << argv[0] << ": <input.png> <sobel|scharr> <output-x.png> <output-y.png>\n";
        return -1;
    }

    gil::gray8_image_t input_image;
    gil::read_image(argv[1], input_image, gil::png_tag{});
    auto input = gil::view(input_image);
    auto filter_type = std::string(argv[2]);

    gil::gray16_image_t dx_image(input_image.dimensions());
    auto dx = gil::view(dx_image);
    gil::gray16_image_t dy_image(input_image.dimensions());
    auto dy = gil::view(dy_image);
    if (filter_type == "sobel")
    {
        gil::convolve_2d(input, gil::generate_dx_sobel(1), dx);
        gil::convolve_2d(input, gil::generate_dy_sobel(1), dy);
    }
    else if (filter_type == "scharr")
    {
        gil::convolve_2d(input, gil::generate_dx_scharr(1), dx);
        gil::convolve_2d(input, gil::generate_dy_scharr(1), dy);
    }
    else
    {
        std::cerr << "unrecognized gradient filter type. Must be either sobel or scharr\n";
        return -1;
    }

    gil::write_view(argv[3], dx, gil::png_tag{});
    gil::write_view(argv[4], dy, gil::png_tag{});
}


#include <state_estimation/filters/kalman_filter_vs.h>
#include <state_estimation/utilities/data_subset_utilities.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

void KalmanFilterVS::myPredict(const Eigen::VectorXd& u, double dt) {
    system_model_->update(filter_state_.x, u, dt);

    // Get our sub matrices and vectors
    const Eigen::VectorXd x_subset = getSubset(filter_state_.x, system_model_->activeStates());
    const Eigen::VectorXd u_subset = getSubset(u, system_model_->activeControls());
    const Eigen::MatrixXd cov_subset =
        getSubset(filter_state_.covariance, system_model_->activeStates());
    const Eigen::MatrixXd A_subset = getSubset(system_model_->A(), system_model_->activeStates());
    const Eigen::MatrixXd B_subset = getSubset(system_model_->B(), system_model_->activeStates(),
                                               system_model_->activeControls());
    const Eigen::MatrixXd Rc_subset =
        getSubset(system_model_->Rc(), system_model_->activeControls());
    const Eigen::MatrixXd P_subset =
        getSubset(system_model_->P(), system_model_->activeStates(), {});
    const Eigen::MatrixXd V_subset = getSubset(system_model_->V(), system_model_->activeStates(),
                                               system_model_->activeControls());

    // Update the state
    const Eigen::VectorXd Ax_subset = A_subset * x_subset;
    const Eigen::VectorXd Bu_subset = B_subset * u_subset;
    const Eigen::VectorXd Ax_full = convertSubsetToFullZeroed(
        Ax_subset, system_model_->activeStates(), system_model_->stateSize());
    const Eigen::VectorXd Bu_full = convertSubsetToFullZeroed(
        Bu_subset, system_model_->activeStates(), system_model_->stateSize());

    filter_state_.x = system_model_->addVectors(Ax_full, Bu_full);

    // Update the covariance
    const Eigen::MatrixXd cov_prime_subset = A_subset * cov_subset * A_subset.transpose() +
                                             P_subset * system_model_->Rp() * P_subset.transpose() +
                                             V_subset * Rc_subset * V_subset.transpose();

    convertSubsetToFull(cov_prime_subset, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "KF predicition update:" << std::endl
              << "A=" << std::endl
              << printMatrix(system_model_->A()) << std::endl
              << "B=" << std::endl
              << printMatrix(system_model_->B()) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << filter_state_.x.transpose() << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void KalmanFilterVS::myCorrect(const Eigen::VectorXd& z,
                               measurement_models::LinearMeasurementModel* model, double dt) {
    // Update our measurement model
    model->update(filter_state_.x, dt);

    // Get our sub matrices/vectors
    const Eigen::VectorXd x_subset = getSubset(filter_state_.x, system_model_->activeStates());
    const Eigen::MatrixXd cov_subset =
        getSubset(filter_state_.covariance, system_model_->activeStates());
    const Eigen::MatrixXd C_subset =
        getSubset(model->C(), model->activeMeasurements(), system_model_->activeStates());
    const Eigen::MatrixXd meas_cov_subset =
        getSubset(model->covariance(), model->activeMeasurements());

    // Compute the Kalman gain
    const Eigen::MatrixXd cov_C_T = cov_subset * C_subset.transpose();
    const Eigen::MatrixXd K = cov_C_T * (C_subset * cov_C_T + meas_cov_subset).inverse();

    // Update the state
    const Eigen::VectorXd z_prime_subset = C_subset * x_subset;
    const Eigen::VectorXd z_prime_full = convertSubsetToFullZeroed(
        z_prime_subset, model->activeMeasurements(), model->measurementSize());
    const Eigen::VectorXd dz_full = model->subtractVectors(z, z_prime_full);
    const Eigen::VectorXd dz_subset = getSubset(dz_full, model->activeMeasurements());
    const Eigen::VectorXd dx_subset = K * dz_subset;
    const Eigen::VectorXd dx_full = convertSubsetToFullZeroed(
        dx_subset, system_model_->activeStates(), system_model_->stateSize());

    filter_state_.x = system_model_->addVectors(filter_state_.x, dx_full);

    // Update the covariance
    const Eigen::MatrixXd I = Eigen::MatrixXd::Identity(cov_subset.rows(), cov_subset.rows());
    const Eigen::MatrixXd cov_prime = (I - K * C_subset) * cov_subset;

    convertSubsetToFull(cov_prime, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "KF measurement update:" << std::endl
              << "C=" << std::endl
              << printMatrix(model->C()) << std::endl
              << "z'=" << printMatrix(z_prime_full) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "K=" << std::endl
              << printMatrix(K) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation


//----------------------------------*-C++-*-----------------------------------//
/**
 *  @file  CLP.cc
 *  @brief CLP member definitions
 *  @note  Copyright (C) 2013 Jeremy Roberts
 */
//----------------------------------------------------------------------------//

#include "CLP.hh"
#include "utilities/DBC.hh"
#ifdef DETRAN_ENABLE_BOOST
#include <boost/math/special_functions/legendre.hpp>
#endif

namespace detran_orthog
{

//----------------------------------------------------------------------------//
CLP::CLP(const Parameters &p)
  : ContinuousOrthogonalBasis(p)
{
#ifndef DETRAN_ENABLE_BOOST
  THROW("CLP needs boost to be enabled.");
#else

  // Allocate the basis matrix
  d_basis = new callow::MatrixDense(d_size, d_order + 1, 0.0);

  // Allocate the normalization array
  d_a = Vector::Create(d_order + 1, 0.0);

  // The weights are just the qw's.
  double L = d_upper_bound - d_lower_bound;
  for (size_t i = 0; i < d_w->size(); ++i)
  {
    d_x[i] =  2.0*(d_x[i] - d_lower_bound)/L - 1.0;
    (*d_w)[i] = d_qw[i];
  }

  // Build the basis
  for (size_t l = 0; l <= d_order; ++l)
  {
    for (size_t i = 0; i < d_size; ++i)
    {
      (*d_basis)(i, l) = boost::math::legendre_p(l, d_x[i]);
    }
    // Inverse of normalization coefficient.
    (*d_a)[l] = (2.0 * l + 1.0) / 2.0;
  }
  //d_orthonormal = true;
  //compute_a();
#endif

}

} // end namespace detran_orthog

//----------------------------------------------------------------------------//
//              end of file CLP.cc
//----------------------------------------------------------------------------//


#include <iostream>
#include <fstream>
#include <vector>
#include <random>
#include <Eigen/Dense>
#include "../include/GaussianHMM.h"
using namespace Eigen;

int main() {
  std::cout << "Start ...\n";
  double var = 1.0;
  std::random_device seed;
  std::mt19937 random_number_generator(seed());
  std::normal_distribution<double> white_noise(0.0, var);

  ArrayXd clusters(2);
  ArrayXd start(2);
  ArrayXXd trans(2, 2);
  clusters << 10, 1;
  start << 1, 0;
  trans << 0.88, 0.12, 0.4, 0.6;

  GaussianHMM hmm(2, var, 47, 1000);
  std::cout << "Max epoch - " << hmm.max_epoch << "\n";
  hmm.pi = start;
  hmm.means_ = clusters;
  hmm.A = trans;
  hmm.covars_ = ArrayXd::Ones(2) * var;

  int n_sample = 1000;
  ArrayXi state_seqence(n_sample);
  std::vector<double> X;
  hmm.sample(n_sample, 100, X, state_seqence);

  std::ofstream signal_writer("signal.dat", std::ios_base::out | std::ios_base::trunc);
  signal_writer << "state\tobservation\n";
  for (size_t i = 0; i < X.size(); i++) {
    signal_writer << state_seqence(i) << '\t' << X[i] << '\n';
  }
  signal_writer.close();

  std::cout << "******************* Fitting test *******************\n";
  GaussianHMM fit_hmm(2, 1e-2, -1, 100);
  std::vector<size_t> lengths;
  std::cout << "Start to fit X ...\n";
  fit_hmm.fit(X, lengths);
  std::cout << "Fitted result:\n"
            << "Mean:\n" << fit_hmm.means_ << "\n"
            << "Var: \n" << fit_hmm.covars_ << "\n"
            << "A: \n" << fit_hmm.A << "\n";

  return 0;
}


/*
 * Main File For testing
 * Added by Mohamed TarekIbnZiad
 */

#include <assert.h>
#include <vector>
#include <crypto/paillier.hh>
#include <Img.hh>
#include <crypto/gm.hh>
#include <NTL/ZZ.h>
#include <gmpxx.h>
#include <math/util_gmp_rand.h>

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <ctime>

#include<iostream>

using namespace cv;
using namespace std;
using namespace NTL;

/*
 * Apply average filter in Plain domain using open cv ready made function (blur)
 */
Mat
AverageFilterOpenCV(Mat src)
{
    //enter the kernel size
    int kerSize;
    cout << "Please enter the kernel size: \n";
    cin >> kerSize;

    Mat dst;
    //smooth the image in the "src" and save it to "dst"
    blur( src, dst, Size( kerSize, kerSize ) );

    return dst;
}

/*
 * Apply average filter in Plain domain
 */
Mat
AverageFilter(Mat src, vector < vector<mpz_class> > &filter)
{
    Mat dst = src.clone(); //Copy of src as initial value
    Scalar s;
    //Filter size
    int ro = filter.size();
    int co = filter.data()->size();

    //get summation of the filter elements to divide by
    mpz_class nFilter = 0;
    for (int i=0; i < ro; i++){
	     for (int j=0; j < co; j++){
            nFilter += filter[i][j];
       }
    }

    for (int i=1; i < src.rows - 2; i++){
	     for (int j=1; j < src.cols - 2; j++){
            mpz_class sum=0;
            for(int m=-1; m<=1; m++){
		            for(int n=-1; n<=1; n++){
                    s = src.at<uchar>(i+m,j+n);
                    sum += s.val[0] * filter[m+1][n+1];
                }
            }
            s.val[0] = sum.get_d()/nFilter.get_d();
            dst.at<uchar>(i,j) = s.val[0];
        }
    }
    return dst;
}

/*
 * Apply Sobel operator in Plain domain using open cv ready made function (Sobel)
 */
Mat
SobelFilterOpenCV(Mat src)
{
    int scale = 1;
    int delta = 0;
    int ddepth = CV_16S;

    Mat dst;

    // Generate grad_x and grad_y
    Mat grad_x, grad_y;
    Mat abs_grad_x, abs_grad_y;

    // Gradient X
    Sobel( src, grad_x, ddepth, 1, 0, 3, scale, delta, BORDER_DEFAULT );
    convertScaleAbs( grad_x, abs_grad_x );

    // Gradient Y
    Sobel( src, grad_y, ddepth, 0, 1, 3, scale, delta, BORDER_DEFAULT );
    convertScaleAbs( grad_y, abs_grad_y );

    // Total Gradient (approximate)
    addWeighted( abs_grad_x, 0.5, abs_grad_y, 0.5, 0, dst );

    return dst;
}

/*
 * Apply Sobel operator in Plain domain using indirect access
 */
Mat
SobelFilter(Mat src)
{
    int dx[3][3] = {{1,0,-1},{2,0,-2},{1,0,-1}};
    int dy[3][3] = {{1,2,1},{0,0,0},{-1,-2,-1}};

    Mat dst = src.clone(); //Copy of src as initial value
    //Vec3b s; // for colored images
    Scalar s;

    for (int i=1; i < src.rows - 2; i++){
	     for (int j=1; j < src.cols - 2; j++){
            // apply kernel in X and Y directions
            int sum_x=0;
            int sum_y=0;
            for(int m=-1; m<=1; m++){
		            for(int n=-1; n<=1; n++){
                    //s=cvGet2D(img,i+m,j+n); // get the (i,j) pixel value
                    s = src.at<uchar>(i+m,j+n);
                    sum_x += s.val[0] * dx[m+1][n+1];
                    sum_y += s.val[0] * dy[m+1][n+1];
                }
            }

            int sum=abs(sum_x)+abs(sum_y);
            s.val[0] = (sum>255)? 255:sum;
            dst.at<uchar>(i,j) = s.val[0];
            //dst.at<Vec2d>(i,j)[0] = s;
            //cvSet2D(dst,i,j,s); // set the (i,j) pixel value
        }
    }
    return dst;
}

/*
 * Apply convolution in Plain domain
 */
Mat
convolution(Mat src, vector < vector<mpz_class> > &filter)
{
    Mat dst = src.clone(); //Copy of src as initial value
    Scalar s;

    for (int i=1; i < src.rows - 1; i++){
	     for (int j=1; j < src.cols - 1; j++){
            mpz_class sum=0;
            for(int m=-1; m<=1; m++){
		            for(int n=-1; n<=1; n++){
                    if (filter[m+1][n+1] == 0) { //Not encrypted value
                        continue;
                    }
                    s = src.at<uchar>(i+m,j+n);
                    sum += s.val[0] * filter[m+1][n+1];
                }
            }
            s.val[0] = abs(sum.get_d());
            dst.at<uchar>(i,j) = s.val[0];
        }
    }
    return dst;
}

/*
 * used to Add/Sub constant value to the image brightness in plain domain
 */
Mat
AdjustBrightness(Mat src, int value)
{
    Mat dst = src.clone(); //Copy of src as initial value
    Scalar s;
    int newValue;
    for (int i=0; i < src.rows; i++){
	     for (int j=0; j < src.cols; j++){
            s = src.at<uchar>(i,j);
            newValue = s.val[0] + value;
            newValue = (newValue>255)? 255:
                       (newValue<0)?     0:newValue;
            dst.at<uchar>(i,j) = newValue;
        }
    }
    return dst;
}

/*
 * used to fix the image colors range after return from the homo domain
 */
vector < vector<mpz_class> >
FixRange(vector < vector<mpz_class> > &in)
{

    int rows = in.size();
    int cols = in.data()->size();
    int newValue;

    vector < vector<mpz_class> > out(rows, vector<mpz_class>(cols));
    for (int i=0; i < rows; i++){
	     for (int j=0; j < cols; j++){
            newValue = (int)in[i][j].get_d();
            newValue = (newValue>255)? 255:
                       (newValue<0)?     0:newValue;
            out[i][j] = (int)newValue;
        }
    }
    return out;
}

/*
 * used to get the negative image in plain domain
 */
Mat
NegativeImage(Mat src)
{
    Mat dst = src.clone(); //Copy of src as initial value
    Scalar s;
    int newValue;
    for (int i=0; i < src.rows; i++){
	     for (int j=0; j < src.cols; j++){
            s = src.at<uchar>(i,j);
            newValue = 255 - s.val[0];
            dst.at<uchar>(i,j) = newValue;
        }
    }
    return dst;
}

/*
 * used to add salt and pepper noise to the image
 */
Mat
AddSaltPepperNoise(Mat src)
{
    Mat saltpepper_noise = Mat::zeros(src.rows, src.cols,CV_8U);
    randu(saltpepper_noise,0,255);

    Mat black = saltpepper_noise < 10;
    Mat white = saltpepper_noise > 245;

    Mat saltpepper_img = src.clone();
    saltpepper_img.setTo(255,white);
    saltpepper_img.setTo(0,black);

    return saltpepper_img;
}

/*
 * convert an image from Mat type to mpz_class type used in encryption
 */
vector < vector<mpz_class> >
Mat2mpz(Mat src)
{
    vector < vector<mpz_class> > A(src.rows, vector<mpz_class>(src.cols));
    Scalar s;
    for (int i = 0; i < src.rows; i++) {
        for (int j = 0; j < src.cols; j++){
            s = src.at<uchar>(i,j);
            A[i][j] = (int)s[0];
        }
    }
    return A;
}

/*
 * convert an image from mpz_class type used in encryption to Mat type (opencv)
 */
Mat
mpz2Mat(vector < vector<mpz_class> > &A)
{
    int rows = A.size();
    int cols = A.data()->size();
    Mat dst(rows,cols,CV_8UC1);
    for (int i = 0; i < rows; i++) {
        for (int j = 0; j < cols; j++){
            dst.at<uchar>(i,j) = (int)A[i][j].get_d(); //double to int
        }
    }
    return dst;
}

/*
 * used to divide by n in the average filter in time domain
 * would be removed later after implementing paillier encoding
 */
vector < vector<mpz_class> >
divideP (const vector < vector<mpz_class> > &src, const vector < vector<mpz_class> > &filter)
{
    //Src image size
    int rows = src.size();
    int cols = src.data()->size();
    //Filter size
    int ro = filter.size();
    int co = filter.data()->size();

    vector < vector<mpz_class> > dst (rows, vector<mpz_class>(cols));

    //get summation of the filter elements to divide by
    mpz_class sum = 0;
    for (int i=0; i < ro; i++){
	     for (int j=0; j < co; j++){
            sum += filter[i][j];
        }
    }

    for (int i=0; i < rows; i++){
	     for (int j=0; j < cols; j++){
            dst[i][j] = src[i][j]/sum;
        }
    }
    return dst;
}

static void
test_image()
{
    //create 2 empty windows
    //namedWindow( "Original Image" , CV_WINDOW_AUTOSIZE );
    //namedWindow( "Smoothed Image" , CV_WINDOW_AUTOSIZE );

    // Load an image from file
    Mat src = imread( "cameraman.JPG", 1 ); //0 --> gray, 1 --> RGB image

    //show the loaded image
    imshow( "Original Image", src );

    Mat dst;

    // Test Average filter by Opencv
    dst = AverageFilterOpenCV(src);
    imshow( "Opencv Smoothed Image", dst );

    //Add Salt and pepper noise:
    Mat saltpepper_img = AddSaltPepperNoise(src);
    imshow( "Noisy Image", saltpepper_img );

    dst = AverageFilterOpenCV(saltpepper_img);
    imshow( "Opencv Smoothed Image", dst );

    // Test Sobel filter by Me
    Mat src_gray;
    cvtColor(src, src_gray, CV_BGR2GRAY); //change the color image to grayscale image
    imshow( "Gray Image", src_gray );

    dst = SobelFilter(src_gray);
    imshow( "My Sobel Image", dst );

    // Test Sobel filter by Opencv
    dst = SobelFilterOpenCV(src_gray);
    imshow( "Opencv Sobel Image", dst );
}

static void
test_paillier()
{
    cout << "Test Paillier ...\n" << flush;

    gmp_randstate_t randstate;
    gmp_randinit_default(randstate);
    gmp_randseed_ui(randstate,time(NULL));

    auto sk = Paillier_priv::keygen(randstate,16,2); //600 ,256
    Paillier_priv pp(sk,randstate);

    auto pk = pp.pubkey();
    mpz_class n = pk[0];
    Paillier p(pk,randstate);

    //mpz_class pt0, pt1,m;
    //mpz_urandomm(pt0.get_mpz_t(),randstate,n.get_mpz_t());
    //mpz_urandomm(pt1.get_mpz_t(),randstate,n.get_mpz_t());
    //mpz_urandomm(m.get_mpz_t(),randstate,n.get_mpz_t());
    mpz_class pt0 = 2, pt1 = 3, m = 5; //instead of the random values
    cout << "pt0: "<< pt0 << endl;
    cout << "pt1: "<< pt1 << endl;

    mpz_class ct0 = p.encrypt(pt0);         cout << "ct0: "<< ct0 << endl;
    mpz_class ct1 = p.encrypt(pt1);         cout << "ct1: "<< ct1 << endl;
    mpz_class sum = p.add(ct0, ct1);        cout << "sum_enc: "<< sum << endl;
    mpz_class prod = p.constMult(m,ct0);    cout << "prod_enc: "<< prod << endl;
    mpz_class diff = p.sub(ct0, ct1);       cout << "diff_enc: "<< diff << endl;

    mpz_class ct0_dec = pp.decrypt(ct0);
    cout << "ct0_dec "<< ct0_dec.get_d() << endl;
    cout << "n " <<  n.get_d() <<endl;
    assert(pp.decrypt(ct0) == pt0);
    assert(pp.decrypt(ct1) == pt1);
    assert(pp.decrypt(sum) == (pt0+pt1)%n);
    mpz_class d = pt0 - pt1;
    if (d < 0) {
        d += n;
    }
    assert(pp.decrypt(diff) == d);
    assert(pp.decrypt(prod) == (m*pt0)%n);

    cout << "Test Paillier passed" << endl;

    cout << "Test Matrix Paillier ...\n" << flush;
    vector < vector<mpz_class> > A = {{1,2,3},{1,1,1},{4,5,6}};
    int Arows = A.size();
    int Acols = A.data()->size();
    vector < vector<mpz_class> > A_enc(Arows, vector<mpz_class>(Acols));
    A_enc = p.encryptMatrix(A);
    vector < vector<mpz_class> > A_dec(Arows, vector<mpz_class>(Acols));
    A_dec = pp.decryptMatrix(A_enc);
    cout << "Test Matrix Paillier passed" << endl;
}

double
getRMSE(const Mat& I1, const Mat& I2)
{
    Mat s1;
    absdiff(I1, I2, s1);       // |I1 - I2|
    s1.convertTo(s1, CV_32F);  // cannot make a square on 8 bits
    s1 = s1.mul(s1);           // |I1 - I2|^2

    Scalar s = sum(s1);        // sum elements per channel
    double sse = s.val[0] + s.val[1] + s.val[2]; // sum channels
    double mse  = sse / (double)(I1.channels() * I1.total());
    double rmse = sqrt(mse);

    return rmse;
 }

void
testImagePaillierTime()
{
    struct timespec t0,t1;
    int n_iteration = 10;
    uint64_t t;

    //Generate Paillier key
    gmp_randstate_t randstate;
    gmp_randinit_default(randstate);
    gmp_randseed_ui(randstate,time(NULL));

    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        auto sk = Paillier_priv::keygen(randstate,32,2); //600 ,256
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "Key Generation: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;
    auto sk = Paillier_priv::keygen(randstate,32,2); //600 ,256
    Paillier_priv pp(sk,randstate);

    auto pk = pp.pubkey();
    mpz_class n = pk[0];
    Paillier p(pk,randstate);

    //read image
    Mat src = imread( "cameraman.JPG", 0);
    imshow( "Original Image", src );

    int rows = src.rows;
    int cols = src.cols;
    vector < vector<mpz_class> > A(rows, vector<mpz_class>(cols));
    A = Mat2mpz(src);

    //Encrypt image
    vector < vector<mpz_class> > A_enc = p.encryptMatrix(A);
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        A_enc = p.encryptMatrix(A);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    //imshow( "Encrypted Image", mpz2Mat(A_enc) );
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "public encryption: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;

    //Decrypt image
    vector < vector<mpz_class> > A_dec = pp.decryptMatrix(A_enc);
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        A_dec = pp.decryptMatrix(A_enc);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "private decryption: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;
    Mat dst;
    dst = mpz2Mat(A_dec);
    //imshow( "Decrypted Image", dst );

    //Negate image in plain domain
    dst = NegativeImage(src);
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        dst = NegativeImage(src);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "Negative Image PD: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;
    //imshow( "Negative Image", dst );

    //Negate image in encrypted domain
    vector < vector<mpz_class> > Neg_enc = p.NegativeImageH(A_enc);
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        vector < vector<mpz_class> > Neg_enc = p.NegativeImageH(A_enc);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "Negative Image ED: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;
    //imshow( "Encrypted Negative Image", mpz2Mat(Neg_enc) );

    vector < vector<mpz_class> > Neg_dec = pp.decryptMatrix(Neg_enc);
    //imshow( "Decrypted Negative Image", mpz2Mat(Neg_dec) );

    //Test Brightness
    int value = 50;
    //Increase brightness in plain domain
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        dst = AdjustBrightness(src, value);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "Adjust Brightness PD: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;
    dst = AdjustBrightness(src, value);
    //imshow( "Brightness Image", dst );

    //Increase brightness in encrypted domain
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        mpz_class value_enc = p.encrypt(value);
        vector < vector<mpz_class> > Bright_enc = p.AdjustBrightnessH(A_enc, value_enc);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "Adjust Brightness ED: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;
    mpz_class value_enc = p.encrypt(value);
    vector < vector<mpz_class> > Bright_enc = p.AdjustBrightnessH(A_enc, value_enc);
    //imshow( "Encrypted Brightness Image", mpz2Mat(Bright_enc) );

    vector < vector<mpz_class> > Bright_dec = pp.decryptMatrix(Bright_enc);
    //imshow( "Decrypted Brightness Image", mpz2Mat(Bright_dec) );

    //Test convolution
    vector < vector<mpz_class> > filter;
    //filter = {{1,0,-1},{2,0,-2},{1,0,-1}};//Vertical edges
    filter = {{1,2,1},{0,0,0},{-1,-2,-1}};  //Horizontal edges
    vector < vector<mpz_class> > conv_enc = p.convolutionH(A_enc, filter);
    //imshow( "Encrypted convoluted Image", mpz2Mat(conv_enc) );
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        vector < vector<mpz_class> > conv_enc = p.convolutionH(A_enc, filter);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "Convolution (3x3) Sobel ED: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;

    vector < vector<mpz_class> > conv_dec = pp.decryptMatrix(conv_enc);
    //imshow( "Decrypted convoluted Image", mpz2Mat(conv_dec) );

    //Test convolution in Plain domain
    dst = convolution(src, filter);
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        dst = convolution(src, filter);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "Convolution (3x3) Sobel PD: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;
    //imshow( "convoluted Image", dst );

    //Test Average filter
    filter = {{1,1,1},{1,1,1},{1,1,1}};
    vector < vector<mpz_class> > Avg_enc = p.convolutionH(A_enc, filter);
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        Avg_enc = p.convolutionH(A_enc, filter);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "Average (3x3) ED: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;
    //imshow( "Encrypted averaged Image", mpz2Mat(Avg_enc) );
    vector < vector<mpz_class> > Avg_dec = pp.decryptMatrix(Avg_enc);
    //Divide by filter summation in plain domain
    vector < vector<mpz_class> > Avg_decP = divideP(Avg_dec, filter);
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        Avg_decP = divideP(Avg_dec, filter);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "Average (3x3) ED Post Processing: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;
    //imshow( "Decrypted averaged Image", mpz2Mat(Avg_decP) );

    //Test average filter in plain domain
    dst = AverageFilter(src,filter);
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        dst = AverageFilter(src,filter);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "Average (3x3) PD: "<<  ((double)t/1000000)/n_iteration <<"ms" << endl;
    //imshow( "Averaged Image", dst );

}

void
testImagePaillier()
{
    //Generate Paillier key
    gmp_randstate_t randstate;
    gmp_randinit_default(randstate);
    gmp_randseed_ui(randstate,time(NULL));

    auto sk = Paillier_priv::keygen(randstate,32,2); //600 ,256
    Paillier_priv pp(sk,randstate);

    auto pk = pp.pubkey();
    mpz_class n = pk[0];
    Paillier p(pk,randstate);
    cout << "key  ready ...\n" << endl;

    //read image
    Mat src = imread( "cameraman.JPG", 0);
    imshow( "Original Image", src );

    int rows = src.rows;
    int cols = src.cols;
    vector < vector<mpz_class> > A(rows, vector<mpz_class>(cols));
    A = Mat2mpz(src);

    Img img1(A);
    img1.setPbKey(p);

    //Encrypt image
    vector < vector<mpz_class> > A_enc = img1.encryptMatrix(A);
    imshow( "Encrypted Image", mpz2Mat(A_enc) );

    //The client only can decrypt a matrix
    vector < vector<mpz_class> > A_dec = pp.decryptMatrix(A_enc);

    //Decrypt image
    Mat dst;
    dst = mpz2Mat(A_dec);
    imshow( "Decrypted Image", dst );
    //cout << "Root Mean Square Error in decryption: "<<getRMSE(src,dst)<<endl;

    //Negate image in plain domain
    dst = NegativeImage(src);
    imshow( "Negative Image", dst );

    //Negate image in encrypted domain
    vector < vector<mpz_class> > Neg_enc = img1.NegativeImageH(A_enc);
    imshow( "Encrypted Negative Image", mpz2Mat(Neg_enc) );

    vector < vector<mpz_class> > Neg_dec = pp.decryptMatrix(Neg_enc);
    imshow( "Decrypted Negative Image", mpz2Mat(Neg_dec) );
    //cout << "Root Mean Square Error in Negative image: "<<getRMSE(dst,mpz2Mat(Neg_dec))<<endl;

    //Test Brightness
    int value = 50;
    //Increase brightness in plain domain
    dst = AdjustBrightness(src, value);
    imshow( "Brightness Image", dst );

    //Increase brightness in encrypted domain
    mpz_class value_enc = p.encrypt(value);
    vector < vector<mpz_class> > Bright_enc = img1.AdjustBrightnessH(A_enc, value_enc);
    imshow( "Encrypted Brightness Image", mpz2Mat(Bright_enc) );

    vector < vector<mpz_class> > Bright_dec = pp.decryptMatrix(Bright_enc);
    imshow( "Decrypted Brightness Image", mpz2Mat(Bright_dec) );
    //cout << "Root Mean Square Error in Brightness image: "<<getRMSE(dst,mpz2Mat(Bright_dec))<<endl;
    //post-processing to fix the image
    //Bright_dec = FixRange(Bright_dec);
    //imshow( "Decrypted Brightness Image", mpz2Mat(Bright_dec) );

    //Test convolution
    vector < vector<mpz_class> > filter;
    //filter = {{1,0,-1},{2,0,-2},{1,0,-1}};//Vertical edges
    filter = {{1,2,1},{0,0,0},{-1,-2,-1}};  //Horizontal edges

    vector < vector<mpz_class> > conv_enc = img1.convolutionH(A_enc, filter);
    imshow( "Encrypted convoluted Image", mpz2Mat(conv_enc) );
    vector < vector<mpz_class> > conv_dec = pp.decryptMatrix(conv_enc);
    imshow( "Decrypted convoluted Image", mpz2Mat(conv_dec) );

    //Test convolution in Plain domain
    dst = convolution(src, filter);
    imshow( "convoluted Image", dst );
    //cout << "Root Mean Square Error in convoluted image: "<<getRMSE(dst,mpz2Mat(conv_dec))<<endl;

    //Test Average filter
    filter = {{1,1,1},{1,1,1},{1,1,1}};
    vector < vector<mpz_class> > Avg_enc = img1.convolutionH(A_enc, filter);
    imshow( "Encrypted averaged Image", mpz2Mat(Avg_enc) );
    vector < vector<mpz_class> > Avg_dec = pp.decryptMatrix(Avg_enc);
    //Divide by filter summation in plain domain
    vector < vector<mpz_class> > Avg_decP = divideP(Avg_dec, filter);
    imshow( "Decrypted averaged Image", mpz2Mat(Avg_decP) );

    //Test average filter in plain domain
    dst = AverageFilter(src,filter);
    imshow( "Averaged Image", dst );
    //cout << "Root Mean Square Error in Averaged image: "<<getRMSE(dst,mpz2Mat(Avg_decP))<<endl;

    //wait for a key press infinitely
    waitKey(0);
}

int
main ( int argc, char **argv )
{
    //SetSeed(to_ZZ(time(NULL)));
    //test_paillier();
    //test_image();

    testImagePaillier(); /*For generating images*/
    //testImagePaillierTime(); /*For calculating time*/

    return 0;
}


//  rational number example program  ----------------------------------------//

//  (C) Copyright Paul Moore 1999. Permission to copy, use, modify, sell
//  and distribute this software is granted provided this copyright notice
//  appears in all copies. This software is provided "as is" without express or
//  implied warranty, and with no claim as to its suitability for any purpose.

// boostinspect:nolicense (don't complain about the lack of a Boost license)
// (Paul Moore hasn't been in contact for years, so there's no way to change the
// license.)

//  Revision History
//  14 Dec 99  Initial version

#include <iostream>
#include <cassert>
#include <cstdlib>
#include <boost/config.hpp>
#ifndef BOOST_NO_LIMITS
#include <limits>
#else
#include <limits.h>
#endif
#include <exception>
#include <boost/rational.hpp>

using std::cout;
using std::endl;
using boost::rational;

#ifdef BOOST_NO_ARGUMENT_DEPENDENT_LOOKUP
// This is a nasty hack, required because MSVC does not implement "Koenig
// Lookup". Basically, if I call abs(r), the C++ standard says that the
// compiler should look for a definition of abs in the namespace which
// contains r's class (in this case boost) - among other places.

// Koenig Lookup is a relatively recent feature, and other compilers may not
// implement it yet. If so, try including this line.

using boost::abs;
#endif

int main ()
{
    rational<int> half(1,2);
    rational<int> one(1);
    rational<int> two(2);

    // Some basic checks
    assert(half.numerator() == 1);
    assert(half.denominator() == 2);
    assert(boost::rational_cast<double>(half) == 0.5);

    // Arithmetic
    assert(half + half == one);
    assert(one - half == half);
    assert(two * half == one);
    assert(one / half == two);

    // With conversions to integer
    assert(half+half == 1);
    assert(2 * half == one);
    assert(2 * half == 1);
    assert(one / half == 2);
    assert(1 / half == 2);

    // Sign handling
    rational<int> minus_half(-1,2);
    assert(-half == minus_half);
    assert(abs(minus_half) == half);

    // Do we avoid overflow?
#ifndef BOOST_NO_LIMITS
    int maxint = (std::numeric_limits<int>::max)();
#else
    int maxint = INT_MAX;
#endif
    rational<int> big(maxint, 2);
    assert(2 * big == maxint);

    // Print some of the above results
    cout << half << "+" << half << "=" << one << endl;
    cout << one << "-" << half << "=" << half << endl;
    cout << two << "*" << half << "=" << one << endl;
    cout << one << "/" << half << "=" << two << endl;
    cout << "abs(" << minus_half << ")=" << half << endl;
    cout << "2 * " << big << "=" << maxint
         << " (rational: " << rational<int>(maxint) << ")" << endl;

    // Some extras
    rational<int> pi(22,7);
    cout << "pi = " << boost::rational_cast<double>(pi) << " (nearly)" << endl;

    // Exception handling
    try {
        rational<int> r;        // Forgot to initialise - set to 0
        r = 1/r;                // Boom!
    }
    catch (const boost::bad_rational &e) {
        cout << "Bad rational, as expected: " << e.what() << endl;
    }
    catch (...) {
        cout << "Wrong exception raised!" << endl;
    }

    return 0;
}


// Algolab BGL Tutorial 2 (Max flow, by taubnert@ethz.ch)
// Flow example demonstrating how to use push_relabel_max_flow using a custom edge adder
// to manage the interior graph properties required for flow algorithms
#include <iostream>

// BGL include
#include <boost/graph/adjacency_list.hpp>

// BGL flow include *NEW*
#include <boost/graph/push_relabel_max_flow.hpp>

// Graph Type with nested interior edge properties for flow algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

using namespace std;

// Custom edge adder class, highly recommended
class edge_adder {
  graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

void make_it_flow() {
  int n, m, s;
  cin >> n; cin >> m; cin >> s;
  graph G(n);
  edge_adder adder(G);

  vector<int> stores(s);
  for(int i = 0; i < s; i++) {
    cin >> stores[i];
  }

  for(int i = 0; i < m; i++) {
    int u, v; cin >> u; cin >> v;
  // Add some edges using our custom edge adder
    adder.add_edge(u, v, 1); // from, to, capacity
    adder.add_edge(v, u, 1); // from, to, capacity
  }

  // Add special vertices source and sink
  const vertex_desc v_source = boost::add_vertex(G);
  const vertex_desc v_sink = boost::add_vertex(G);
  adder.add_edge(v_source, 0, INT_MAX);

  for(int i = 0; i < s; i++) {
    adder.add_edge(stores[i], v_sink, 1);
  }

  // Calculate flow from source to sink
  // The flow algorithm uses the interior properties (managed in the edge adder)
  // - edge_capacity, edge_reverse (read access),
  // - edge_residual_capacity (read and write access).
  long flow = boost::push_relabel_max_flow(G, v_source, v_sink);

  if(flow == s) {
    std::cout << "yes" << "\n";
  } else {
    std::cout << "no" << "\n";
  }

  // Retrieve the capacity map and reverse capacity map
  const auto c_map = boost::get(boost::edge_capacity, G);
  const auto rc_map = boost::get(boost::edge_residual_capacity, G);

  // Iterate over all the edges to print the flow along them
  auto edge_iters = boost::edges(G);
  for (auto edge_it = edge_iters.first; edge_it != edge_iters.second; ++edge_it) {
    const edge_desc edge = *edge_it;
    const long flow_through_edge = c_map[edge] - rc_map[edge];
    std::cerr << "edge from " << boost::source(edge, G) << " to " << boost::target(edge, G)
              << " runs " << flow_through_edge
              << " units of flow (negative for reverse direction). \n";
  }
}

int main() {
  std::ios_base::sync_with_stdio(false); // Always!
  int t; cin >> t;
  while(t--) make_it_flow();
  return 0;
}


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

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/greater.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/maximum.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

int main() {
    // without a predicate
    BOOST_HANA_CONSTANT_CHECK(
        hana::maximum(hana::tuple_c<int, -1, 0, 2, -4, 6, 9>) == hana::int_c<9>
    );

    // with a predicate
    auto smallest = hana::maximum(hana::tuple_c<int, -1, 0, 2, -4, 6, 9>, [](auto x, auto y) {
        return x > y; // order is reversed!
    });
    BOOST_HANA_CONSTANT_CHECK(smallest == hana::int_c<-4>);
}


//==================================================================================================
/*!
  @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_FUNCTION_NTHROOT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_NTHROOT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-exponential
    This function object returns the nth root of its first argument: \f$\sqrt[n]{x}\f$
    \arg n must be of integer type
    \arg if n is even and x negative the result is @ref Nan
    \arg if x is null the result is @ref Zero
    \arg if x is one  the result is @ref One

    @par Header <boost/simd/function/nthroot.hpp>

    @par Note:
    nthroot is slower than `pow(x, rec(tofloat(n))`) because
    it takes care of some limits issues that @ref pow ignores.

    See if it suits you better or use raw_ decorator for intermediate solution.

    @par Decorators

      - raw_ provides increased speed but is undefined for limitings values

    @see pow, rec, sqrt, cbrt

    @par Example:

      @snippet nthroot.cpp nthroot

    @par Possible output:

      @snippet nthroot.txt nthroot

  **/
  Value nthroot(Value const& x, IntegerValue const& n);
} }
#endif

#include <boost/simd/function/scalar/nthroot.hpp>
#include <boost/simd/function/simd/nthroot.hpp>

#endif


/*=========================================================================

Library:   TubeTK

Copyright 2010 Kitware Inc. 28 Corporate Drive,
Clifton Park, NY, 12065, USA.

All rights reserved.

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

    https://www.apache.org/licenses/LICENSE-2.0

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

=========================================================================*/

#include "../CLI/tubeCLIProgressReporter.h"
#include "tubeMessage.h"

#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>
#include <itkTimeProbesCollectorBase.h>
#include <itkBinaryThresholdImageFilter.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/p_square_quantile.hpp>
#include <boost/accumulators/statistics/stats.hpp>

#include "SegmentUsingQuantileThresholdCLP.h"

template< class TPixel, unsigned int VDimension >
int DoIt( int argc, char * argv[] );

#include "../CLI/tubeCLIHelperFunctions.h"

template< class TPixel, unsigned int VDimension >
int DoIt( int argc, char * argv[] )
{
  PARSE_ARGS;

  itk::TimeProbesCollectorBase timeCollector;

  tube::CLIProgressReporter progressReporter(
    "SegmentUsingQuantileThreshold", CLPProcessInformation );
  progressReporter.Start();

  typedef TPixel                                PixelType;
  typedef itk::Image< PixelType, VDimension >   ImageType;
  typedef itk::ImageFileReader< ImageType >     ReaderType;

  timeCollector.Start( "Load data" );
  typename ReaderType::Pointer reader = ReaderType::New();
  reader->SetFileName( inputVolume.c_str() );
  try
    {
    reader->Update();
    }
  catch( itk::ExceptionObject & err )
    {
    tube::ErrorMessage( "Reading volume: Exception caught: "
                        + std::string( err.GetDescription() ) );
    timeCollector.Report();
    return EXIT_FAILURE;
    }
  timeCollector.Stop( "Load data" );
  double progress = 0.1;
  progressReporter.Report( progress );

  typename ImageType::Pointer image = reader->GetOutput();

  typename ImageType::Pointer maskImage = NULL;
  if( !maskVolume.empty() )
    {
    timeCollector.Start( "Load mask" );
    typename ReaderType::Pointer maskReader = ReaderType::New();
    maskReader->SetFileName( maskVolume.c_str() );
    try
      {
      maskReader->Update();
      }
    catch( itk::ExceptionObject & err )
      {
      tube::ErrorMessage( "Reading mask: Exception caught: "
                          + std::string( err.GetDescription() ) );
      timeCollector.Report();
      return EXIT_FAILURE;
      }
    timeCollector.Stop( "Load mask" );
    progress = 0.2;
    progressReporter.Report( progress );

    maskImage = maskReader->GetOutput();
    }

  if( thresholdQuantile >= 0 && thresholdQuantile <= 1.0 )
    {
    timeCollector.Start( "Boost accumulate" );

    typedef boost::accumulators::accumulator_set< PixelType,
      boost::accumulators::stats<
        boost::accumulators::tag::p_square_quantile > >
      QuantileAccumulatorType;
    typedef itk::ImageRegionConstIterator< ImageType >
      ImageIteratorType;

    /*
     * Create a and configure a vector of length N of pointers
     * to BOOST accumulators -- Each of the N accumulators will
     * estimate exactly one of the given N desired quantile. If
     * the desired quantile is not within ( 0,1 ), throw an exception.
     */
    QuantileAccumulatorType acc( boost::accumulators::quantile_probability
      = thresholdQuantile );

    /*
     * Use an image iterator to iterate over all pixel/voxel and
     * and then add those values to the accumulators. Adding
     * the values will incrementally compute the quantile estimates.
     */
    ImageIteratorType imIt( image, image->GetLargestPossibleRegion() );
    if( !maskVolume.empty() )
      {
      ImageIteratorType maskIt( maskImage,
        maskImage->GetLargestPossibleRegion() );
      while( !imIt.IsAtEnd() )
        {
        PixelType p = imIt.Get();
        PixelType m = maskIt.Get();
        if( m != 0 )
          {
          acc( p );
          }
        ++imIt;
        ++maskIt;
        }
      }
    else
      {
      while( !imIt.IsAtEnd() )
        {
        PixelType p = imIt.Get();
        acc( p );
        ++imIt;
        }
      }

    PixelType qVal = boost::accumulators::p_square_quantile( acc );

    typedef itk::BinaryThresholdImageFilter< ImageType, ImageType >
      FilterType;
    typename FilterType::Pointer filter = FilterType::New();

    filter->SetInput( image );
    filter->SetLowerThreshold( qVal );
    filter->SetOutsideValue( 0 );
    filter->SetInsideValue( 1 );

    filter->Update();

    image = filter->GetOutput();

    timeCollector.Stop( "Boost accumulate" );
    }

  typedef itk::ImageFileWriter< ImageType  >   ImageWriterType;

  timeCollector.Start( "Save data" );
  typename ImageWriterType::Pointer writer = ImageWriterType::New();
  writer->SetFileName( outputVolume.c_str() );
  writer->SetInput( image );
  writer->SetUseCompression( true );
  try
    {
    writer->Update();
    }
  catch( itk::ExceptionObject & err )
    {
    tube::ErrorMessage( "Writing volume: Exception caught: "
      + std::string( err.GetDescription() ) );
    timeCollector.Report();
    return EXIT_FAILURE;
    }
  timeCollector.Stop( "Save data" );
  progress = 1.0;
  progressReporter.Report( progress );
  progressReporter.End();

  timeCollector.Report();
  return EXIT_SUCCESS;
}

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

  return tube::ParseArgsAndCallDoIt( inputVolume, argc, argv );
}