/********************************************************************************
 * Copyright 2009 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_POSE2D_HPP_
#define RW_MATH_POSE2D_HPP_

#if !defined(SWIG)
#include <rw/math/Transform2D.hpp>

#include <Eigen/Core>
#endif
namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A Pose3D @f$ \mathbf{x}\in \mathbb{R}^6 @f$ describes a position
     * and orientation in 3-dimensions.
     *
     * @f$ {\mathbf{x}} = \left[
     *  \begin{array}{c}
     *  x \\
     *  y \\
     *  z \\
     *  \theta k_x \\
     *  \theta k_y \\
     *  \theta k_z
     *  \end{array}
     *  \right]
     *  @f$
     *
     * where @f$ (x,y,z)@f$ is the 3d position and @f$ (\theta k_x, \theta k_y,
     * \theta k_z)@f$ describes the orientation in equal angle axis (EAA)
     * format.
     */
    template< class T = double >

    class Pose2D
    {
      public:
        //! @brief Zero-initialized Pose2D.
        Pose2D () : _pos (0, 0), _theta (0) {}

        /**
         * @brief Constructor.
         * @param pos [in] the position.
         * @param theta [in] the angle.
         */
        Pose2D (rw::math::Vector2D< T > pos, T theta) : _pos (pos), _theta (theta) {}

        /**
         * @brief Constructor.
         * @param x [in] the value of the first position dimension.
         * @param y [in] the value of the second position dimension.
         * @param theta [in] the angle.
         */
        Pose2D (T x, T y, T theta) : _pos (x, y), _theta (theta) {}

        /**
         * @brief Constructor.
         * @param transform [in] a 2D transform giving the pose.
         */
        Pose2D (const rw::math::Transform2D< T >& transform) :
            _pos (transform.P ()), _theta (
                                       // Sigh.
                                       atan2 (transform.R () (1, 0), transform.R () (0, 0)))
        {}

        /**
         * @brief Get the first dimension of the position vector.
         * @return the position in the first dimension.
         */
        T& x () { return _pos[0]; }

        /**
         * @brief Get the second dimension of the position vector.
         * @return the position in the second dimension.
         */
        T& y () { return _pos[1]; }

        /**
         * @brief Get the angle.
         * @return the angle.
         */
        T& theta () { return _theta; }

        /**
         * @brief Get the position vector.
         * @return the position.
         */
        rw::math::Vector2D< T >& getPos () { return _pos; }

        //! @copydoc x()
        T x () const { return _pos[0]; }

        //! @copydoc y()
        T y () const { return _pos[1]; }

        //! @copydoc theta()
        T theta () const { return _theta; }

        //! @copydoc getPos()
        const rw::math::Vector2D< T >& getPos () const { return _pos; }

#if !defined(SWIG)
        /**
         * @brief Returns reference to vector element (x,y,theta)
         *
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         *
         * @return const reference to element
         */
        const T& operator() (size_t i) const
        {
            if (i < 2)
                return _pos[i];
            return _theta;
        }

        /**
         * @brief Returns reference to vector element
         *
         * @param i [in] index in the vector \f$i\in \{0,1\} \f$
         *
         * @return reference to element
         */
        T& operator() (size_t i)
        {
            if (i < 2)
                return _pos[i];
            return _theta;
        }

        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return const reference to element
         */
        const T& operator[] (size_t i) const
        {
            if (i < 2)
                return _pos[i];
            return _theta;
        }

        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return reference to element
         */
        T& operator[] (size_t i)
        {
            if (i < 2)
                return _pos[i];
            return _theta;
        }
#else
        ARRAYOPERATOR (T);
#endif
        /**
         * @brief The transform corresponding to the pose.
         * @param pose [in] the pose.
         * @return equivalent 2D transform.
         */
        static rw::math::Transform2D< T > transform (const Pose2D< T >& pose)
        {
            return rw::math::Transform2D< T > (rw::math::Vector2D< T > (pose.x (), pose.y ()),
                                               rw::math::Rotation2D< T > (pose.theta ()));
        }
#if !defined(SWIG)
        /**
         * @brief Ouputs EAA to stream
         * @param os [in/out] stream to use
         * @param eaa [in] equivalent axis-angle
         * @return the resulting stream
         */
        friend std::ostream& operator<< (std::ostream& os, const Pose2D< T >& pose)
        {
            return os << " Ppse2D { x: " << pose.x () << ", y: " << pose.y ()
                      << ", th: " << pose.theta () << "}";
        }
#else
        TOSTRING (rw::math::Pose2D< T >);
#endif

        /**
         * @brief return a Eigen vector of (x, y, theta).
         * @return Eigen vector.
         */
        // template<class T>
        Eigen::Matrix< T, 3, 1 > e () const
        {
            Eigen::Matrix< T, 3, 1 > vec;
            vec (0) = _pos (0);
            vec (1) = _pos (1);
            vec (2) = _theta;
            return vec;
        }

      private:
        rw::math::Vector2D< T > _pos;
        T _theta;
    };

#if !defined(SWIG)
    extern template class rw::math::Pose2D< double >;
    extern template class rw::math::Pose2D< float >;
#else
    SWIG_DECLARE_TEMPLATE (Pose2Dd, rw::math::Pose2D< double >);
    SWIG_DECLARE_TEMPLATE (Pose2Df, rw::math::Pose2D< float >);
#endif
    using Pose2Dd = Pose2D< double >;
    using Pose2Df = Pose2D< float >;

    /*@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Pose2D
         */
        template<>
        void write (const rw::math::Pose2D< double >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Pose2D
         */
        template<>
        void write (const rw::math::Pose2D< float >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Pose2D
         */
        template<>
        void read (rw::math::Pose2D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Pose2D
         */
        template<>
        void read (rw::math::Pose2D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

    }    // namespace serialization
}}       // namespace rw::common

#endif /* POSE2D_HPP_ */


/* tsne.cc
   Jeremy Barnes, 15 January 2010
   Copyright (c) 2010 Jeremy Barnes.  All rights reserved.

   Implementation of the t-SNE algorithm.
*/

#include "tsne.h"
#include "jml/stats/distribution.h"
#include "jml/stats/distribution_ops.h"
#include "jml/stats/distribution_simd.h"
#include "jml/algebra/matrix_ops.h"
#include "jml/arch/simd_vector.h"
#include <boost/tuple/tuple.hpp>
#include "jml/algebra/lapack.h"
#include <cmath>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include "jml/utils/worker_task.h"
#include <boost/timer.hpp>
#include "jml/arch/timers.h"
#include "jml/arch/sse2.h"
#include "jml/arch/sse2_log.h"
#include "jml/arch/cache.h"
#include "jml/utils/guard.h"
#include <boost/bind.hpp>
#include "jml/utils/environment.h"

using namespace std;

namespace ML {

template<typename Float>
struct V2D_Job {
    const boost::multi_array<Float, 2> & X;
    boost::multi_array<Float, 2> & D;
    const Float * sum_X;
    int i0, i1;

    V2D_Job(const boost::multi_array<Float, 2> & X,
            boost::multi_array<Float, 2> & D,
            const Float * sum_X,
            int i0, int i1)
        : X(X), D(D), sum_X(sum_X), i0(i0), i1(i1)
    {
    }

    void operator () ()
    {
        int d = X.shape()[1];

        if (d == 2) {
            unsigned i = i0;
            for (;  i + 4 <= i1;  i += 4) {
                D[i + 0][i + 0] = 0.0f;
                D[i + 1][i + 1] = 0.0f;
                D[i + 2][i + 2] = 0.0f;
                D[i + 3][i + 3] = 0.0f;

                for (unsigned j = 0;  j < i;  ++j) {
                    for (unsigned ii = 0;  ii < 4;  ++ii) {
                        Float XXT
                            = (X[i + ii][0] * X[j][0])
                            + (X[i + ii][1] * X[j][1]);
                        Float val = sum_X[i + ii] + sum_X[j] - 2.0f * XXT;
                        D[i + ii][j] = val;
                    }
                }

                // finish off the diagonal
                for (unsigned ii = 0;  ii < 4;  ++ii) {
                    for (unsigned j = i;  j < i + ii;  ++j) {
                        Float XXT
                            = (X[i + ii][0] * X[j][0])
                            + (X[i + ii][1] * X[j][1]);
                        Float val = sum_X[i + ii] + sum_X[j] - 2.0f * XXT;
                        D[i + ii][j] = val;
                    }
                }
            }
            for (;  i < i1;  ++i) {
                D[i][i] = 0.0f;

                for (unsigned j = 0;  j < i;  ++j) {
                    Float XXT = (X[i][0] * X[j][0]) + (X[i][1]) * (X[j][1]);
                    Float val = sum_X[i] + sum_X[j] - 2.0f * XXT;
                    D[i][j] = val;
                }
            }
        }
        else if (d < 8) {
            for (unsigned i = i0;  i < i1;  ++i) {
                D[i][i] = 0.0f;
                for (unsigned j = 0;  j < i;  ++j) {
                    float XXT = 0.0;
                    for (unsigned k = 0;  k < d;  ++k)
                        XXT += X[i][k] * X[j][k];

                    Float val = sum_X[i] + sum_X[j] - 2.0f * XXT;
                    D[i][j] = val;
                }
            }
        }
        else {
            for (unsigned i = i0;  i < i1;  ++i) {
                D[i][i] = 0.0f;
                for (unsigned j = 0;  j < i;  ++j) {
                    // accum in double precision for accuracy
                    Float XXT = SIMD::vec_dotprod_dp(&X[i][0], &X[j][0], d);
                    Float val = sum_X[i] + sum_X[j] - 2.0f * XXT;
                    D[i][j] = val;
                }
            }
        }
    }
};

template<typename Float>
void
vectors_to_distances(const boost::multi_array<Float, 2> & X,
                     boost::multi_array<Float, 2> & D,
                     bool fill_upper)
{
    // again, ||y_i - y_j||^2
    //     = sum_d ( y_id - y_jd )^2
    //     = sum_d ( y_id^2 + y_jd^2 - 2 y_id y_jd)
    //     = sum_d ( y_id^2) + sum_d(y_jd^2) - 2 sum_d(y_id y_jd)
    //     = ||y_i||^2 + ||y_j||^2 - 2 sum_d(y_id y_jd)

    int n = X.shape()[0];

    if (D.shape()[0] != n || D.shape()[1] != n)
        throw Exception("D matrix should be square with (n x n) shape");

    int d = X.shape()[1];

    distribution<Float> sum_X(n);

    if (d < 16) {
        for (unsigned i = 0;  i < n;  ++i) {
            double total = 0.0;  // accum in double precision for accuracy
            for (unsigned j = 0;  j < d;  ++j)
                total += X[i][j] * X[i][j];
            sum_X[i] = total;
        }
    }
    else {
        for (unsigned i = 0;  i < n;  ++i)
            sum_X[i] = SIMD::vec_dotprod_dp(&X[i][0], &X[i][0], d);
    }

    Worker_Task & worker = Worker_Task::instance(num_threads() - 1);

    int group;
    {
        int parent = -1;  // no parent group
        group = worker.get_group(NO_JOB, "", parent);
        Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                     boost::ref(worker),
                                     group));

        int chunk_size = 64;

        for (int i = n;  i > 0;  i -= chunk_size) {
            int i0 = max(0, i - chunk_size);
            int i1 = i;

            worker.add(V2D_Job<Float>(X, D, &sum_X[0], i0, i1),
                       "", group);
        }
    }

    worker.run_until_finished(group);

    if (fill_upper)
        copy_lower_to_upper(D);
}

void
vectors_to_distances(const boost::multi_array<float, 2> & X,
                     boost::multi_array<float, 2> & D,
                     bool fill_upper)
{
    return vectors_to_distances<float>(X, D, fill_upper);
}

void
vectors_to_distances(const boost::multi_array<double, 2> & X,
                     boost::multi_array<double, 2> & D,
                     bool fill_upper)
{
    return vectors_to_distances<double>(X, D, fill_upper);
}

template<typename Float>
double
perplexity(const distribution<Float> & p)
{
    double total = 0.0;
    for (unsigned i = 0;  i < p.size();  ++i)
        if (p[i] != 0.0) total -= p[i] * log(p[i]);
    return exp(total);
}

/** Compute the perplexity and the P for a given value of beta. */
template<typename Float>
std::pair<double, distribution<Float> >
perplexity_and_prob(const distribution<Float> & D, double beta = 1.0,
                    int i = -1)
{
    distribution<Float> P(D.size());
    SIMD::vec_exp(&D[0], -beta, &P[0], D.size());
    if (i != -1) P[i] = 0;
    double tot = P.total();

    if (!isfinite(tot) || tot == 0) {
#if 0
        cerr << "beta = " << beta << endl;
        cerr << "D = " << D << endl;
        cerr << "tot = " << tot << endl;
        cerr << "i = " << i << endl;
        cerr << "P = " << P << endl;
#endif
        throw Exception("non-finite total for perplexity");
    }

    double H = log(tot) + beta * D.dotprod(P) / tot;
    P *= 1.0 / tot;

    if (!isfinite(P.total())) {
#if 0
        cerr << "beta = " << beta << endl;
        cerr << "D = " << D << endl;
        cerr << "tot = " << tot << endl;
        cerr << "i = " << i << endl;
#endif
        throw Exception("non-finite total for perplexity");
    }

    return make_pair(H, P);
}

std::pair<double, distribution<float> >
perplexity_and_prob(const distribution<float> & D, double beta,
                    int i)
{
    return perplexity_and_prob<float>(D, beta, i);
}

std::pair<double, distribution<double> >
perplexity_and_prob(const distribution<double> & D, double beta,
                    int i)
{
    return perplexity_and_prob<double>(D, beta, i);
}

/** Calculate the beta for a single point.

    \param Di     The i-th row of the D matrix, for which we want to calculate
                  the probabilities.
    \param i      Which row number it is.

    \returns      The i-th row of the P matrix, which has the distances in D
                  converted to probabilities with the given perplexity.
 */
std::pair<distribution<float>, double>
binary_search_perplexity(const distribution<float> & Di,
                         double required_perplexity,
                         int i,
                         double tolerance = 1e-5)
{
    double betamin = -INFINITY, betamax = INFINITY;
    double beta = 1.0;

    distribution<float> P;
    double log_perplexity;
    double log_required_perplexity = log(required_perplexity);

    boost::tie(log_perplexity, P) = perplexity_and_prob(Di, beta, i);

    bool verbose = false;

    if (verbose)
        cerr << "iter currperp targperp     diff toleranc   betamin     beta  betamax" << endl;

    for (unsigned iter = 0;  iter != 50;  ++iter) {
        if (verbose)
            cerr << format("%4d %8.4f %8.4f %8.4f %8.4f  %8.4f %8.4f %8.4f\n",
                           iter,
                           log_perplexity, log_required_perplexity,
                           fabs(log_perplexity - log_required_perplexity),
                           tolerance,
                           betamin, beta, betamax);

        if (fabs(log_perplexity - log_required_perplexity) < tolerance)
            break;

        if (log_perplexity > log_required_perplexity) {
            betamin = beta;
            if (!isfinite(betamax))
                beta *= 2;
            else beta = (beta + betamax) * 0.5;
        }
        else {
            betamax = beta;
            if (!isfinite(betamin))
                beta /= 2;
            else beta = (beta + betamin) * 0.5;
        }

        boost::tie(log_perplexity, P) = perplexity_and_prob(Di, beta, i);
    }

    return make_pair(P, beta);
}

struct Distance_To_Probabilities_Job {

    boost::multi_array<float, 2> & D;
    double tolerance;
    double perplexity;
    boost::multi_array<float, 2> & P;
    distribution<float> & beta;
    int i0;
    int i1;

    Distance_To_Probabilities_Job(boost::multi_array<float, 2> & D,
                                  double tolerance,
                                  double perplexity,
                                  boost::multi_array<float, 2> & P,
                                  distribution<float> & beta,
                                  int i0,
                                  int i1)
        : D(D), tolerance(tolerance), perplexity(perplexity),
          P(P), beta(beta), i0(i0), i1(i1)
    {
    }

    void operator () ()
    {
        int n = D.shape()[0];

        for (unsigned i = i0;  i < i1;  ++i) {
            //cerr << "i = " << i << endl;
            //if (i % 250 == 0)
            //    cerr << "P-values for point " << i << " of " << n << endl;

            distribution<float> D_row(&D[i][0], &D[i][0] + n);
            distribution<float> P_row;

            try {
                boost::tie(P_row, beta[i])
                    = binary_search_perplexity(D_row, perplexity, i, tolerance);
            } catch (const std::exception & exc) {
                P_row = D_row;
                P_row[i] = 1000000;
                P_row = (P_row == P_row.min());
                std::fill(P_row.begin(), P_row.end(), 1.0);
                P_row[i] = 0.0;
                P_row.normalize();
            }

            if (P_row.size() != n)
                throw Exception("P_row has the wrong size");
            if (P_row[i] != 0.0) {
                cerr << "i = " << i << endl;
                //cerr << "D_row = " << D_row << endl;
                //cerr << "P_row = " << P_row << endl;
                cerr << "P_row.total() = " << P_row.total() << endl;
                cerr << "P_row[i] = " << P_row[i] << endl;
                throw Exception("P_row diagonal entry was not zero");
            }

            std::copy(P_row.begin(), P_row.end(), &P[i][0]);
        }
    }
};

/* Given a matrix of distances, convert to probabilities */
boost::multi_array<float, 2>
distances_to_probabilities(boost::multi_array<float, 2> & D,
                           double tolerance,
                           double perplexity)
{
    int n = D.shape()[0];
    if (D.shape()[1] != n)
        throw Exception("D is not square");

    boost::multi_array<float, 2> P(boost::extents[n][n]);
    distribution<float> beta(n, 1.0);

    Worker_Task & worker = Worker_Task::instance(num_threads() - 1);

    int group;
    {
        int parent = -1;  // no parent group
        group = worker.get_group(NO_JOB, "", parent);
        Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                     boost::ref(worker),
                                     group));

        int chunk_size = 64;

        for (int i = 0;  i < n;  i += chunk_size) {
            int i0 = i;
            int i1 = min(n, i + chunk_size);

            worker.add(Distance_To_Probabilities_Job
                       (D, tolerance, perplexity, P, beta, i0, i1),
                       "", group);
        }
    }

    worker.run_until_finished(group);

    cerr << "mean sigma is " << sqrt(1.0 / beta).mean() << endl;

    return P;
}

boost::multi_array<float, 2>
pca(boost::multi_array<float, 2> & coords, int num_dims)
{
    // TODO: normalize the input coordinates (especially if it seems to be
    // ill conditioned)

    int nx = coords.shape()[0];
    int nd = coords.shape()[1];

    int nvalues = std::min(nd, nx);

    int ndr = std::min(nvalues, num_dims);

    if (ndr < num_dims)
        throw Exception("svd_reduction: num_dims not low enough");

    distribution<float> svalues(nvalues);
    boost::multi_array<float, 2> lvectorsT(boost::extents[nvalues][nd]);
    boost::multi_array<float, 2> rvectors(boost::extents[nx][nvalues]);

    int res = LAPack::gesdd("S", nd, nx,
                            coords.data(), nd,
                            &svalues[0],
                            &lvectorsT[0][0], nd,
                            &rvectors[0][0], nvalues);

    // If some vectors are singular, ignore them
    // TODO: do...

    if (res != 0)
        throw Exception("gesdd returned non-zero");

    boost::multi_array<float, 2> result(boost::extents[nx][ndr]);
    for (unsigned i = 0;  i < nx;  ++i)
        std::copy(&rvectors[i][0], &rvectors[i][0] + ndr, &result[i][0]);

    return result;
}

double calc_D_row(float * Di, int n)
{
    unsigned i = 0;

    double total = 0.0;

    if (false) ;
    else if (n >= 8) {
        using namespace SIMD;

        v2df rr = vec_splat(0.0);

        v4sf one = vec_splat(1.0f);

        __builtin_prefetch(Di + i + 0, 1, 3);
        __builtin_prefetch(Di + i + 16, 1, 3);
        __builtin_prefetch(Di + i + 32, 1, 3);

        for (; i + 16 <= n;  i += 16) {
            __builtin_prefetch(Di + i + 48, 1, 3);

            v4sf xxxx0 = __builtin_ia32_loadups(Di + i + 0);
            v4sf xxxx1 = __builtin_ia32_loadups(Di + i + 4);
            xxxx0      = xxxx0 + one;
            xxxx1      = xxxx1 + one;
            xxxx0      = one / xxxx0;
            v4sf xxxx2 = __builtin_ia32_loadups(Di + i + 8);
            xxxx1      = one / xxxx1;
            __builtin_ia32_storeups(Di + i + 0, xxxx0);
            xxxx2      = xxxx2 + one;
            v2df xx0a, xx0b;  vec_f2d(xxxx0, xx0a, xx0b);
            __builtin_ia32_storeups(Di + i + 4, xxxx1);
            xx0a       = xx0a + xx0b;
            rr         = rr + xx0a;
            v4sf xxxx3 = __builtin_ia32_loadups(Di + i + 12);
            v2df xx1a, xx1b;  vec_f2d(xxxx1, xx1a, xx1b);
            xxxx2      = one / xxxx2;
            xx1a       = xx1a + xx1b;
            __builtin_ia32_storeups(Di + i + 8, xxxx2);
            rr         = rr + xx1a;
            v2df xx2a, xx2b;  vec_f2d(xxxx2, xx2a, xx2b);
            xxxx3      = xxxx3 + one;
            xx2a       = xx2a + xx2b;
            xxxx3      = one / xxxx3;
            rr         = rr + xx2a;
            v2df xx3a, xx3b;  vec_f2d(xxxx3, xx3a, xx3b);
            __builtin_ia32_storeups(Di + i + 12, xxxx3);
            xx3a       = xx3a + xx3b;
            rr         = rr + xx3a;
        }

        for (; i + 4 <= n;  i += 4) {
            v4sf xxxx0 = __builtin_ia32_loadups(Di + i + 0);
            xxxx0      = xxxx0 + one;
            xxxx0      = one / xxxx0;
            __builtin_ia32_storeups(Di + i + 0, xxxx0);

            v2df xx0a, xx0b;
            vec_f2d(xxxx0, xx0a, xx0b);

            rr      = rr + xx0a;
            rr      = rr + xx0b;
        }

        double results[2];
        *(v2df *)results = rr;

        total = (results[0] + results[1]);
    }

    for (;  i < n;  ++i) {
        Di[i] = 1.0f / (1.0f + Di[i]);
        total += Di[i];
    }

    return total;
}

namespace {

Env_Option<bool> PROFILE_TSNE("PROFILE_TSNE", false);

double t_v2d = 0.0, t_D = 0.0, t_dY = 0.0, t_update = 0.0;
double t_recenter = 0.0, t_cost = 0.0, t_PmQxD = 0.0, t_clu = 0.0;
double t_stiffness = 0.0;
struct AtEnd {
    ~AtEnd()
    {
        if (!PROFILE_TSNE) return;

        cerr << "tsne core profile:" << endl;
        cerr << "  v2d:        " << t_v2d << endl;
        cerr << "  stiffness:" << t_stiffness << endl;
        cerr << "    D         " << t_D << endl;
        cerr << "    (P-Q)D    " << t_PmQxD << endl;
        cerr << "    clu       " << t_clu << endl;
        cerr << "  dY:         " << t_dY << endl;
        cerr << "  update:     " << t_update << endl;
        cerr << "  recenter:   " << t_recenter << endl;
        cerr << "  cost:       " << t_cost << endl;
    }
} atend;

} // file scope

struct Calc_D_Job {

    boost::multi_array<float, 2> & D;
    int i0;
    int i1;
    double * d_totals;

    Calc_D_Job(boost::multi_array<float, 2> & D,
               int i0,
               int i1,
               double * d_totals)
        : D(D), i0(i0), i1(i1), d_totals(d_totals)
    {
    }

    void operator () ()
    {
        for (unsigned i = i0;  i < i1;  ++i) {
            d_totals[i] = 2.0 * calc_D_row(&D[i][0], i);
            D[i][i] = 0.0f;
        }
    }
};

double calc_stiffness_row(float * Di, const float * Pi, float qfactor,
                          float min_prob, int n, bool calc_costs)
{
    double cost = 0.0;

    unsigned i = 0;

    if (false) ;
    else if (true) {
        using namespace SIMD;

        v4sf mmmm = vec_splat(min_prob);
        v4sf ffff = vec_splat(qfactor);

        v2df total = vec_splat(0.0);

        for (; i + 4 <= n;  i += 4) {

            v4sf dddd0 = __builtin_ia32_loadups(Di + i + 0);
            v4sf pppp0 = __builtin_ia32_loadups(Pi + i + 0);
            v4sf qqqq0 = __builtin_ia32_maxps(mmmm, dddd0 * ffff);
            v4sf ssss0 = (pppp0 - qqqq0) * dddd0;
            __builtin_ia32_storeups(Di + i + 0, ssss0);
            if (JML_LIKELY(!calc_costs)) continue;

            v4sf pqpq0  = pppp0 / qqqq0;
            v4sf lpq0   = sse2_logf_unsafe(pqpq0);
            v4sf cccc0  = pppp0 * lpq0;
            cccc0 = cccc0 + cccc0;

            v2df cc0a, cc0b;
            vec_f2d(cccc0, cc0a, cc0b);

            total   = total + cc0a;
            total   = total + cc0b;
        }

        double results[2];
        *(v2df *)results = total;

        cost = results[0] + results[1];
    }

    for (;  i < n;  ++i) {
        float d = Di[i];
        float p = Pi[i];
        float q = std::max(min_prob, d * qfactor);
        Di[i] = (p - q) * d;
        if (calc_costs) cost += 2.0 * p * logf(p / q);
    }

    return cost;
}

struct Calc_Stiffness_Job {

    boost::multi_array<float, 2> & D;
    const boost::multi_array<float, 2> & P;
    float min_prob;
    float qfactor;
    double * costs;
    int i0, i1;

    Calc_Stiffness_Job(boost::multi_array<float, 2> & D,
                       const boost::multi_array<float, 2> & P,
                       float min_prob,
                       float qfactor,
                       double * costs,
                       int i0, int i1)
        : D(D), P(P), min_prob(min_prob),
          qfactor(qfactor), costs(costs), i0(i0), i1(i1)
    {
    }

    void operator () ()
    {
        for (unsigned i = i0;  i < i1;  ++i) {
            double cost
                = calc_stiffness_row(&D[i][0], &P[i][0],
                                     qfactor, min_prob, i,
                                     costs);
            if (costs) costs[i] = cost;
        }
    }
};

double tsne_calc_stiffness(boost::multi_array<float, 2> & D,
                           const boost::multi_array<float, 2> & P,
                           float min_prob,
                           bool calc_cost)
{
    boost::timer t;

    int n = D.shape()[0];
    if (D.shape()[1] != n)
        throw Exception("D has wrong shape");

    if (P.shape()[0] != n || P.shape()[1] != n)
        throw Exception("P has wrong shape");

    double d_totals[n];

    Worker_Task & worker = Worker_Task::instance(num_threads() - 1);

    int group;
    {
        int parent = -1;  // no parent group
        group = worker.get_group(NO_JOB, "", parent);
        Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                     boost::ref(worker),
                                     group));

        int chunk_size = 64;

        for (int i = n;  i > 0;  i -= chunk_size) {
            int i0 = max(0, i - chunk_size);
            int i1 = i;

            worker.add(Calc_D_Job(D, i0, i1, d_totals),
                       "", group);
        }
    }

    worker.run_until_finished(group);

    double d_total_offdiag = SIMD::vec_sum(d_totals, n);

    t_D += t.elapsed();  t.restart();

    // Cost accumulated for each row
    double row_costs[n];

    // Q matrix: q_{i,j} = d_{ij} / sum_{k != l} d_{kl}
    float qfactor = 1.0 / d_total_offdiag;

    {
        int parent = -1;  // no parent group
        group = worker.get_group(NO_JOB, "", parent);
        Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                     boost::ref(worker),
                                     group));

        int chunk_size = 64;

        for (int i = n;  i > 0;  i -= chunk_size) {
            int i0 = max(0, i - chunk_size);
            int i1 = i;

            worker.add(Calc_Stiffness_Job
                       (D, P, min_prob, qfactor,
                        (calc_cost ? row_costs : (double *)0), i0, i1),
                       "", group);
        }
    }

    worker.run_until_finished(group);

    double cost = 0.0;
    if (calc_cost) cost = SIMD::vec_sum(row_costs, n);

    t_PmQxD += t.elapsed();  t.restart();

    copy_lower_to_upper(D);

    t_clu += t.elapsed();  t.restart();

    return cost;
}

inline void
calc_dY_rows_2d(boost::multi_array<float, 2> & dY,
                const boost::multi_array<float, 2> & PmQxD,
                const boost::multi_array<float, 2> & Y,
                int i, int n)
{
#if 1
    using namespace SIMD;

    v4sf totals01 = vec_splat(0.0f), totals23 = totals01;
    v4sf four = vec_splat(4.0f);

    for (unsigned j = 0;  j < n;  ++j) {
        //v4sf ffff = { PmQxD[i + 0][j], PmQxD[i + 1][j],
        //              PmQxD[i + 2][j], PmQxD[i + 3][j] };
        // TODO: expand inplace

        v4sf ffff01 = { PmQxD[i + 0][j], PmQxD[i + 0][j],
                        PmQxD[i + 1][j], PmQxD[i + 1][j] };
        v4sf ffff23 = { PmQxD[i + 2][j], PmQxD[i + 2][j],
                        PmQxD[i + 3][j], PmQxD[i + 3][j] };

        // TODO: load once and shuffle into position
        v4sf yjyj   = { Y[j][0], Y[j][1], Y[j][0], Y[j][1] };

        ffff01 = ffff01 * four;
        ffff23 = ffff23 * four;

        v4sf yi01   = __builtin_ia32_loadups(&Y[i][0]);
        v4sf yi23   = __builtin_ia32_loadups(&Y[i + 2][0]);

        v4sf xxxx01 = ffff01 * (yi01 - yjyj);
        v4sf xxxx23 = ffff23 * (yi23 - yjyj);

        totals01 += xxxx01;
        totals23 += xxxx23;
    }

    __builtin_ia32_storeups(&dY[i][0], totals01);
    __builtin_ia32_storeups(&dY[i + 2][0], totals23);

#else
    enum { b = 4 };

    float totals[b][2];
    for (unsigned ii = 0;  ii < b;  ++ii)
        totals[ii][0] = totals[ii][1] = 0.0f;

    for (unsigned j = 0;  j < n;  ++j) {
        float Yj0 = Y[j][0];
        float Yj1 = Y[j][1];

        for (unsigned ii = 0;  ii < b;  ++ii) {
            float factor = 4.0f * PmQxD[i + ii][j];
            totals[ii][0] += factor * (Y[i + ii][0] - Yj0);
            totals[ii][1] += factor * (Y[i + ii][1] - Yj1);
        }
    }

    for (unsigned ii = 0;  ii < b;  ++ii) {
        dY[i + ii][0] = totals[ii][0];
        dY[i + ii][1] = totals[ii][1];
    }
#endif
}

inline void
calc_dY_row_2d(float * dYi, const float * PmQxDi,
               const boost::multi_array<float, 2> & Y,
               int i,
               int n)
{
    float total0 = 0.0f, total1 = 0.0f;
    for (unsigned j = 0;  j < n;  ++j) {
        float factor = 4.0f * PmQxDi[j];
        total0 += factor * (Y[i][0] - Y[j][0]);
        total1 += factor * (Y[i][1] - Y[j][1]);
    }

    dYi[0] = total0;
    dYi[1] = total1;
}

struct Calc_Gradient_Job {
    boost::multi_array<float, 2> & dY;
    const boost::multi_array<float, 2> & Y;
    const boost::multi_array<float, 2> & PmQxD;
    int i0, i1;

    Calc_Gradient_Job(boost::multi_array<float, 2> & dY,
                      const boost::multi_array<float, 2> & Y,
                      const boost::multi_array<float, 2> & PmQxD,
                      int i0,
                      int i1)
        : dY(dY),
          Y(Y),
          PmQxD(PmQxD),
          i0(i0),
          i1(i1)
    {
    }

    void operator () ()
    {
        int n = Y.shape()[0];
        int d = Y.shape()[1];

        if (d == 2) {
            unsigned i = i0;

            for (;  i + 4 <= i1;  i += 4)
                calc_dY_rows_2d(dY, PmQxD, Y, i, n);

            for (; i < i1;  ++i)
                calc_dY_row_2d(&dY[i][0], &PmQxD[i][0], Y, i, n);
        }
        else {
            for (unsigned i = i0;  i < i1;  ++i) {
                for (unsigned k = 0;  k < d;  ++k) {
                    float Yik = Y[i][k];
                    float total = 0.0;
                    for (unsigned j = 0;  j < n;  ++j) {
                        float factor = 4.0f * PmQxD[i][j];
                        float Yjk = Y[j][k];
                        total += factor * (Yik - Yjk);
                    }
                    dY[i][k] = total;
                }
            }
        }
    }
};

void tsne_calc_gradient(boost::multi_array<float, 2> & dY,
                        const boost::multi_array<float, 2> & Y,
                        const boost::multi_array<float, 2> & PmQxD)
{
    // Gradient
    // Implements formula 5 in (Van der Maaten and Hinton, 2008)
    // dC/dy_i = 4 * sum_j ( (p_ij - q_ij)(y_i - y_j)d_ij )

    int n = Y.shape()[0];
    int d = Y.shape()[1];

    if (dY.shape()[0] != n || dY.shape()[1] != d)
        throw Exception("dY matrix has wrong shape");

    if (PmQxD.shape()[0] != n || PmQxD.shape()[1] != n)
        throw Exception("PmQxD matrix has wrong shape");

    Worker_Task & worker = Worker_Task::instance(num_threads() - 1);

    int group;
    {
        int parent = -1;  // no parent group
        group = worker.get_group(NO_JOB, "", parent);
        Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                     boost::ref(worker),
                                     group));

        int chunk_size = 64;

        for (unsigned i = 0;  i < n;  i += chunk_size) {
            int i0 = i;
            int i1 = min(i0 + chunk_size, n);

            worker.add(Calc_Gradient_Job(dY, Y, PmQxD, i0, i1),
                       "", group);
        }
    }

    worker.run_until_finished(group);
}

void tsne_update(boost::multi_array<float, 2> & Y,
                 boost::multi_array<float, 2> & dY,
                 boost::multi_array<float, 2> & iY,
                 boost::multi_array<float, 2> & gains,
                 bool first_iter,
                 float momentum,
                 float eta,
                 float min_gain)
{
    int n = Y.shape()[0];
    int d = Y.shape()[1];

    // Implement scheme in Jacobs, 1988.  If we go in the same direction as
    // last time, we increase the learning speed of the parameter a bit.
    // If on the other hand the direction changes, we reduce exponentially
    // the rate.

    for (unsigned i = 0;  !first_iter && i < n;  ++i) {
        // We use != here as we gradients in dY are the negatives of what
        // we want.
        for (unsigned j = 0;  j < d;  ++j) {
            if (dY[i][j] * iY[i][j] < 0.0f)
                gains[i][j] = gains[i][j] + 0.2f;
            else gains[i][j] = gains[i][j] * 0.8f;
            gains[i][j] = std::max(min_gain, gains[i][j]);
        }
    }

    for (unsigned i = 0;  i < n;  ++i)
        for (unsigned j = 0;  j < d;  ++j)
            iY[i][j] = momentum * iY[i][j] - (eta * gains[i][j] * dY[i][j]);
    Y = Y + iY;
}

template<typename Float>
void recenter_about_origin(boost::multi_array<Float, 2> & Y)
{
    int n = Y.shape()[0];
    int d = Y.shape()[1];

    // Recenter Y values about the origin
    double Y_means[d];
    std::fill(Y_means, Y_means + d, 0.0);
    for (unsigned i = 0;  i < n;  ++i)
        for (unsigned j = 0;  j < d;  ++j)
            Y_means[j] += Y[i][j];

    Float n_recip = 1.0f / n;

    for (unsigned i = 0;  i < n;  ++i)
        for (unsigned j = 0;  j < d;  ++j)
            Y[i][j] -= Y_means[j] * n_recip;
}

boost::multi_array<float, 2>
tsne(const boost::multi_array<float, 2> & probs,
     int num_dims,
     const TSNE_Params & params,
     const TSNE_Callback & callback)
{
    int n = probs.shape()[0];
    if (n != probs.shape()[1])
        throw Exception("probabilities were the wrong shape");

    int d = num_dims;

    boost::mt19937 rng;
    boost::normal_distribution<float> norm;

    boost::variate_generator<boost::mt19937,
                             boost::normal_distribution<float> >
        randn(rng, norm);

    double min_perp = INFINITY;
    double max_perp = 0.0;
    double total_perp = 0.0;
    double total_log_perp = 0.0;

    for (unsigned i = 0;  i < n;  ++i) {
        distribution<float> P_row(&probs[i][0], &probs[i][0] + n);
        double perp = perplexity(P_row);
        min_perp = std::min(min_perp, perp);
        max_perp = std::max(max_perp, perp);
        total_perp += perp;
        total_log_perp += log(perp);
    }

    cerr << "input perplexity: min " << min_perp
         << " max: " << max_perp << " average: " << total_perp / n
         << " avg log: " << total_log_perp / n
         << " total log: " << total_log_perp << endl;

    boost::multi_array<float, 2> Y(boost::extents[n][d]);
    for (unsigned i = 0;  i < n;  ++i)
        for (unsigned j = 0;  j < d;  ++j)
            Y[i][j] = 0.01 * randn();

    // Symmetrize and probabilize P
    boost::multi_array<float, 2> P = probs + transpose(probs);

    // TODO: symmetric so only need to total the upper diagonal
    double sumP = 0.0;
    for (unsigned i = 0;  i < n;  ++i)
        sumP += 2.0 * SIMD::vec_sum_dp(&P[i][0], i);

    // Factor that P should be multiplied by in all calculations
    // We boost it by 4 in early iterations to force the clusters to be
    // spread apart
    float pfactor = 4.0 / sumP;

    // TODO: do we need this?   P = Math.maximum(P, 1e-12);
    for (unsigned i = 0;  i < n;  ++i)
        for (unsigned j = 0;  j < n;  ++j)
            P[i][j] = std::max((i != j) * pfactor * P[i][j], 1e-12f);

    Timer timer;

    // Pseudo-distance array for reduced space.  Q = D * qfactor
    boost::multi_array<float, 2> D(boost::extents[n][n]);

    // Probabilitiy density array
    //boost::multi_array<float, 2> Q(boost::extents[n][n]);

    // Stiffness array
    //boost::multi_array<float, 2> PmQxD(boost::extents[n][n]);

    // Y delta
    boost::multi_array<float, 2> dY(boost::extents[n][d]);

    // Last change in Y; so that we can see if we're going in the same dir
    boost::multi_array<float, 2> iY(boost::extents[n][d]);

    // Per-variable factors to multiply the gradient by to improve convergence
    boost::multi_array<float, 2> gains(boost::extents[n][d]);
    std::fill(gains.data(), gains.data() + gains.num_elements(), 1.0f);

    if (callback
        && !callback(-1, INFINITY, "init")) return Y;

    for (int iter = 0;  iter < params.max_iter;  ++iter) {

        boost::timer t;

        /*********************************************************************/
        // Pairwise affinities Qij
        // Implements formula 4 in (Van der Maaten and Hinton, 2008)
        // q_{ij} = d_{ij} / sum_{k,l, k != l} d_{kl}
        // where d_{ij} = 1 / (1 + ||y_i - y_j||^2)

        // TODO: these will all be symmetric; we could save lots of work by
        // using upper/lower diagonal matrices.

        vectors_to_distances(Y, D, false /* fill_upper */);

        t_v2d += t.elapsed();  t.restart();

        if (callback
            && !callback(iter, INFINITY, "v2d")) return Y;

        // Do we calculate the cost?
        bool calc_cost = (iter + 1) % 100 == 0 || iter == params.max_iter - 1;

        double cost = tsne_calc_stiffness(D, P, params.min_prob, calc_cost);

        if (callback
            && !callback(iter, INFINITY, "stiffness")) return Y;

        t_stiffness += t.elapsed();  t.restart();

        // D is now the stiffness
        const boost::multi_array<float, 2> & stiffness = D;

        /*********************************************************************/
        // Gradient
        // Implements formula 5 in (Van der Maaten and Hinton, 2008)
        // dC/dy_i = 4 * sum_j ( (p_ij - q_ij)(y_i - y_j)d_ij )

        tsne_calc_gradient(dY, Y, stiffness);

        t_dY += t.elapsed();  t.restart();

        if (callback
            && !callback(iter, INFINITY, "gradient")) return Y;

        /*********************************************************************/
        // Update

        float momentum = (iter < 20
                          ? params.initial_momentum
                          : params.final_momentum);

        tsne_update(Y, dY, iY, gains, iter == 0, momentum, params.eta,
                    params.min_gain);

        if (callback
            && !callback(iter, INFINITY, "update")) return Y;

        t_update += t.elapsed();  t.restart();

        /*********************************************************************/
        // Recenter about the origin

        recenter_about_origin(Y);

        if (callback
            && !callback(iter, INFINITY, "recenter")) return Y;

        t_recenter += t.elapsed();  t.restart();

        /*********************************************************************/
        // Calculate cost

        if ((iter + 1) % 100 == 0 || iter == params.max_iter - 1) {
            cerr << format("iteration %4d cost %6.3f  ",
                           iter + 1, cost)
                 << timer.elapsed() << endl;
            timer.restart();
        }

        t_cost += t.elapsed();  t.restart();

        // Stop lying about P values if we're finished
        if (iter == 100) {
            for (unsigned i = 0;  i < n;  ++i)
                for (unsigned j = 0;  j < n;  ++j)
                    P[i][j] *= 0.25f;
        }
    }

    return Y;
}

} // namespace ML


#include "caffe2/operators/pow_op.h"
#include "caffe2/utils/eigen_utils.h"
#include "caffe2/utils/math.h"
// definition of NumericTypes and SameTypeAsInput is in below header file
//#include "caffe2/operators/elementwise_op.h"
#include <Eigen/Core>

namespace caffe2 {

#define EIGEN_POW(x, y) (x.pow(y))

struct EigenPowFunctor {
  template <int b_is_scalar, typename T1, typename T2, typename R>
  inline void
  Run(size_t n, const T1* a, const T2* b, T2 e, R* out, CPUContext*) {
    // NOLINTNEXTLINE(modernize-use-nullptr)
    if (b == NULL) {
      EigenVectorArrayMap<R>(out, n) =
          EIGEN_POW((ConstEigenVectorArrayMap<T1>(a, n)), (e));
    } else {
      if (b_is_scalar) {
        if (b[0] == -1.) {
          EigenVectorArrayMap<R>(out, n) =
              ConstEigenVectorArrayMap<T1>(a, n).inverse();
        // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
        } else if (b[0] == 0.5) {
          EigenVectorArrayMap<R>(out, n) =
              ConstEigenVectorArrayMap<T1>(a, n).sqrt();
        // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
        } else if (b[0] == -0.5) {
          EigenVectorArrayMap<R>(out, n) =
              ConstEigenVectorArrayMap<T1>(a, n).rsqrt();
        // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
        } else if (b[0] == 2.) {
          EigenVectorArrayMap<R>(out, n) =
              ConstEigenVectorArrayMap<T1>(a, n).square();
        } else {
          EigenVectorArrayMap<R>(out, n) =
              EIGEN_POW((ConstEigenVectorArrayMap<T1>(a, n)), (b[0]));
        }
      } else {
        EigenVectorArrayMap<R>(out, n) = EIGEN_POW(
            (ConstEigenVectorArrayMap<T1>(a, n)),
            (ConstEigenVectorArrayMap<T2>(b, n)));
      }
    }
  }
  template <typename T1, typename T2, typename R>
  void RunWithBroadcast(
      const T1* a,
      const T2* b,
      R* out,
      size_t pre,
      size_t n,
      CPUContext*) {
    EigenArrayMap<R>(out, n, pre) = EIGEN_POW(
        (ConstEigenArrayMap<T1>(a, n, pre)),
        (ConstEigenVectorArrayMap<T2>(b, n)).rowwise().replicate(pre));
    /*
    //below code only allows elementary ops, such as +, -, * and /,
    //and does not allow operations, such as pow, exp and log
    EIGEN_POW(
       (ConstEigenArrayMap<T>(a, n, pre).colwise()),
       (ConstEigenVectorArrayMap<T>(b, n)));
     */
  }
  template <typename T1, typename T2, typename R>
  void RunWithBroadcast2(
      const T1* a,
      const T2* b,
      R* out,
      size_t pre,
      size_t n,
      size_t post,
      CPUContext*) {
    for (auto i = 0U; i < pre; ++i) {
      EigenArrayMap<R>(out + i * n * post, post, n) = EIGEN_POW(
          (ConstEigenArrayMap<T1>(a + i * n * post, post, n)),
          (Eigen::Map<const Eigen::Array<T2, 1, Eigen::Dynamic>>(b, n))
              .colwise()
              .replicate(post));
      /*
      //below code only allows elementary ops, such as +, -, * and /,
      //and does not allow for operations, such as pow, exp and log
      EIEGN_POW(
        (ConstEigenArrayMap<T>(a + i * n * post, post, n).rowwise()),
        (Eigen::Map<const Eigen::Array<T, 1, Eigen::Dynamic>>(b, n)));
      */
    }
  }
};

// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
REGISTER_CPU_OPERATOR(
    Pow,
    PowOp<
        TensorTypes<float> /*NumericTypes*/,
        CPUContext,
        EigenPowFunctor,
        SameTypeAsInput>)

// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
OPERATOR_SCHEMA(Pow)
    .NumInputs(1, 2)
    .NumOutputs(1)
    .AllowInplace({{0, 0}, {1, 0}})
    .IdenticalTypeAndShapeOfInput(0)
    .SetDoc(R"DOC(
The *Pow* op takes an input data tensor $X$ and an exponent parameter *exponent*, which can be a scalar or another tensor. As output, it produces a single output data tensor $Y$, where the function $f(x) = x^{exponent}$ has been applied to $X$ elementwise.

Github Links:

- https://github.com/pytorch/pytorch/blob/master/caffe2/operators/pow_op.h
- https://github.com/pytorch/pytorch/blob/master/caffe2/operators/pow_op.cc

<details>

<summary> <b>Example</b> </summary>

**Code**

```

workspace.ResetWorkspace()

op = core.CreateOperator(
    "Pow",
    ["X", "exponent"],
    ["Y"],
    broadcast=1
)

workspace.FeedBlob("X", np.array([1,2,3,4,5,6]).astype(np.float32))
print("X: ", workspace.FetchBlob("X"))

workspace.FeedBlob("exponent", np.array([2]).astype(np.float32))
print("exponent: ", workspace.FetchBlob("exponent"))

workspace.RunOperatorOnce(op)
print("Y: ", workspace.FetchBlob("Y"))

```

**Result**

```

X:  [1. 2. 3. 4. 5. 6.]
exponent:  [2.]
Y:  [ 1.  4.  9. 16. 25. 36.]

```

</details>

)DOC")
    .Input(0, "X", "Input data blob to be operated on.")
    .Input(1, "exponent", "Exponent blob containing the exponent(s) for calculation. Do not use if setting exponent via argument.")
    .Output(0, "Y", "Output data blob with the same shape as the input.")
    .Arg("exponent", "The exponent of the power function. Do not use if setting exponent via input.")
    .Arg("axis", "*(type: int; default: -1)*")
    .Arg("broadcast", "*(type: bool; default: False)*");

class GetPowGradient : public GradientMakerBase {
  using GradientMakerBase::GradientMakerBase;
  vector<OperatorDef> GetGradientDefs() override {
    ArgumentHelper arg_helper(def_);
    if (arg_helper.HasArgument("exponent")) { // second input is a scalar
      // function f(w,a) = w^a
      // gradient operator with respect to first input tensor
      // df/dw = a * w^(a-1) (all operations are component-wise)
      float exponent = arg_helper.GetSingleArgument<float>("exponent", 0.0);
      Argument scale_arg;
      scale_arg.set_name("scale");
      scale_arg.set_f(exponent);
      Argument pow_arg;
      pow_arg.set_name("exponent");
      if (I(0) != O(0)) {
        pow_arg.set_f(exponent - 1);
      } else {
        LOG(WARNING) << "In-place Pow gradient, possible loss of precision";
        constexpr float kEps = 1e-12f;
        CAFFE_ENFORCE(std::fabs(exponent) > kEps);
        pow_arg.set_f((exponent - 1) / exponent);
      }
      return vector<OperatorDef>{CreateOperatorDef(
                                     "Pow",
                                     "",
                                     std::vector<string>{I(0)},
                                     std::vector<string>{GI(0)},
                                     std::vector<Argument>{pow_arg}),
                                 CreateOperatorDef(
                                     "Mul",
                                     "",
                                     std::vector<string>{GI(0), GO(0)},
                                     std::vector<string>{GI(0)}),
                                 CreateOperatorDef(
                                     "Scale",
                                     "",
                                     std::vector<string>{GI(0)},
                                     std::vector<string>{GI(0)},
                                     std::vector<Argument>{scale_arg})};
      /*
      // Alternative gradient computation
      return vector<OperatorDef>{CreateOperatorDef(
                                     "Div",
                                     "",
                                     std::vector<string>{O(0), I(0)},
                                     std::vector<string>{GI(0)}),
                                 CreateOperatorDef(
                                     "Mul",
                                     "",
                                     std::vector<string>{GI(0), GO(0)},
                                     std::vector<string>{GI(0)}),
                                 CreateOperatorDef(
                                     "Scale",
                                     "",
                                     std::vector<string>{GI(0)},
                                     std::vector<string>{GI(0)},
                                     std::vector<Argument>{scale_arg})};
      */
    } else { // second input is a tensor
      CAFFE_ENFORCE(
          Def().input(0) != Def().output(0) &&
              Def().input(1) != Def().output(0),
          "Gradient computation cannot be carried out if Pow uses in-place "
          "computation: ",
          ProtoDebugString(Def()));
      vector<OperatorDef> grad_ops;
      Argument one_arg;
      one_arg.set_name("value");
      one_arg.set_f(1);
      Argument broadcast, axis, axis_str, order;
      bool bflag = ArgumentHelper::HasArgument(Def(), "broadcast");

      if (bflag) {
        if (ArgumentHelper::HasArgument(Def(), "broadcast")) {
          broadcast = GetArgument(Def(), "broadcast");
        } else {
          broadcast = MakeArgument<int>("broadcast", 0);
        }
        if (ArgumentHelper::HasArgument(Def(), "axis")) {
          axis = GetArgument(Def(), "axis");
        } else {
          axis = MakeArgument<int>("axis", -1);
        }
        if (ArgumentHelper::HasArgument(Def(), "axis_str")) {
          axis_str = GetArgument(Def(), "axis_str");
        } else {
          axis_str = MakeArgument<string>("axis_str", "");
        }
        if (ArgumentHelper::HasArgument(Def(), "order")) {
          order = GetArgument(Def(), "order");
        } else {
          order = MakeArgument<string>("order", "NCHW");
        }
      }

      // function f(w,a) = w^a
      // gradient operator with respect to first input tensor
      // df/dw = a * w^(a-1) (all operations are component-wise)
      grad_ops.push_back(CreateOperatorDef(
          "ConstantFill",
          "",
          std::vector<string>{I(1)},
          std::vector<string>{GI(1)},
          std::vector<Argument>{one_arg}));
      grad_ops.push_back(CreateOperatorDef(
          "Sub",
          "",
          std::vector<string>{I(1), GI(1)},
          std::vector<string>{GI(1)}));
      if (bflag) {
        grad_ops.push_back(CreateOperatorDef(
            "Pow",
            "",
            std::vector<string>{I(0), GI(1)},
            std::vector<string>{GI(0)},
            vector<Argument>{broadcast, axis, axis_str, order}));
      } else {
        grad_ops.push_back(CreateOperatorDef(
            "Pow",
            "",
            std::vector<string>{I(0), GI(1)},
            std::vector<string>{GI(0)}));
      }

      grad_ops.push_back(CreateOperatorDef(
          "Mul",
          "",
          std::vector<string>{GI(0), GO(0)},
          std::vector<string>{GI(0)}));
      if (bflag) {
        grad_ops.push_back(CreateOperatorDef(
            "Mul",
            "",
            std::vector<string>{GI(0), I(1)},
            std::vector<string>{GI(0)},
            vector<Argument>{broadcast, axis, axis_str, order}));
      } else {
        grad_ops.push_back(CreateOperatorDef(
            "Mul",
            "",
            std::vector<string>{GI(0), I(1)},
            std::vector<string>{GI(0)}));
      }
      /*
      // Alternative gradient computation (no broadcast support)
      grad_ops.push_back(CreateOperatorDef(
                           "Div",
                           "",
                           std::vector<string>{O(0), I(0)},
                           std::vector<string>{GI(0)}));
      grad_ops.push_back(CreateOperatorDef(
                           "Mul",
                           "",
                           std::vector<string>{GI(0), GO(0)},
                           std::vector<string>{GI(0)}));
      grad_ops.push_back(CreateOperatorDef(
                           "Mul",
                           "",
                           std::vector<string>{GI(0), I(1)},
                           std::vector<string>{GI(0)}));
      */
      // gradient operator for with respect to second input tensor
      // df/da =  w^a * ln w (all operations are component-wise)
      /*
      // reset GI(1) to zero
      Argument zero_arg;
      zero_arg.set_name("value");
      zero_arg.set_f(0);
      grad_ops.push_back(CreateOperatorDef(
          "ConstantFill",
          "",
          std::vector<string>{I(1)},
          std::vector<string>{GI(1)},
          std::vector<Argument>{zero_arg}));
      */
      grad_ops.push_back(CreateOperatorDef(
          "Log",
          "",
          std::vector<string>{I(0)},
          std::vector<string>{GI(1) + "_autogen_pre_red"}));
      grad_ops.push_back(CreateOperatorDef(
          "Mul",
          "",
          std::vector<string>{GI(1) + "_autogen_pre_red", O(0)},
          std::vector<string>{GI(1) + "_autogen_pre_red"}));
      if (bflag) {
        grad_ops.push_back(CreateOperatorDef(
            "Mul",
            "",
            std::vector<string>{GI(1) + "_autogen_pre_red", GO(0)},
            std::vector<string>{GI(1) + "_autogen_pre_red"}));
        grad_ops.push_back(CreateOperatorDef(
            "SumReduceLike",
            "",
            vector<string>{GI(1) + "_autogen_pre_red", I(1)},
            vector<string>{GI(1)},
            vector<Argument>{axis, axis_str, order}));
      } else {
        grad_ops.push_back(CreateOperatorDef(
            "Mul",
            "",
            std::vector<string>{GI(1) + "_autogen_pre_red", GO(0)},
            std::vector<string>{GI(1)}));
      }

      return grad_ops;
    }
  }

  // Argument `shape` is no longer needed in backprop.
  bool CopyArguments() const override {
    return false;
  }
};

// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
REGISTER_GRADIENT(Pow, GetPowGradient);

} // namespace caffe2


/*
Copyright 2017 InitialDLab

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

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

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

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

#include "hilbert/hilbert.h"

namespace bg = boost::geometry;
namespace bm = ::boost::mpl;
namespace bmp = ::boost::mpl::placeholders;

void test_2d()
{
    hilbert::HilbertValueComputer<unsigned int, 2, 8> hvc;

    /*
    using point_t = bg::model::point<int, 2, bg::cs::cartesian>;
    std::cerr << "here" << std::endl;
    int x,y;
    while(std::cin >> x >> y)
    {
        auto hv = hvc(point_t(x,y));
        for(auto & p : hv.values)
            std::cout << p << ' ';
        std::cout << std::endl;
    }
    return 0;
    */

    using point_t = bg::model::point<int, 2, bg::cs::cartesian>;
    std::vector<point_t> v;
    int DIM_SIZE = (1LL << 8);
    for(int i = 0; i < DIM_SIZE; ++i)
    {
        for(int j = 0; j < DIM_SIZE; ++j)
        {
            v.push_back(point_t(i,j));
        }
    }

    std::sort(v.begin(), v.end(), [&](point_t const& p1, point_t const& p2)->bool{
        return hvc(p1) < hvc(p2);
    });

    /*
    for(auto & p : v)
    {
        auto hv = hvc(p);
        for(auto & i : hv.values)
            std::cerr << i << ' ';
        std::cerr << std::endl;
    }
    */

    for(auto iter = v.begin(); iter != --v.end(); )
    {
        std::cout << iter->get<0>() << ' ' << iter->get<1>() << std::endl;
        ++iter;
        std::cout << iter->get<0>() << ' ' << iter->get<1>() << std::endl;
        std::cout << std::endl;
    }
}
void test_3d()
{
    std::cerr << "building lookup" << std::endl;
    hilbert::HilbertValueComputer<unsigned int, 3, 4> hvc;

    std::cerr << "generating data" << std::endl;
    using point_t = bg::model::point<int, 3, bg::cs::cartesian>;
    std::vector<point_t> v;
    int DIM_SIZE = (1LL << 4);
    for(int i = 0; i < DIM_SIZE; ++i)
    {
        for(int j = 0; j < DIM_SIZE; ++j)
        {
            for(int k = 0; k < DIM_SIZE; ++k)
            {
                v.push_back(point_t(i,j,k));
            }
        }
    }

    std::cerr << "sorting data" << std::endl;
    std::sort(v.begin(), v.end(), [&](point_t const& p1, point_t const& p2)->bool{
        return hvc(p1) < hvc(p2);
    });

    /*
    for(auto & p : v)
    {
        auto hv = hvc(p);
        for(auto & i : hv.values)
            std::cerr << i << ' ';
        std::cerr << std::endl;
    }
    */

    std::cerr << "saving" << std::endl;
    for(auto iter = v.begin(); iter != --v.end(); )
    {
        std::cout << iter->get<0>() << ' ' << iter->get<1>() << ' ' << iter->get<2>() << std::endl;
        ++iter;
        std::cout << iter->get<0>() << ' ' << iter->get<1>() << ' ' << iter->get<2>() << std::endl;
        std::cout << std::endl;
    }

    std::cerr << "done" << std::endl;
}

struct printer
{
    template <typename V>
    void operator () (V)
    {
        using T0 = typename bm::at_c<V, 0>::type;
        std::cout << T0::dim << ' ' << T0::x << ' ' << T0::y << ' ' << T0::z << ' ';
        using T = typename bm::at_c<V, 1>::type;
        std::cout << T::x << ' ' << T::y << ' ' << T::z << std::endl;
    }
};

int main()
{
    //test_2d();
    test_3d();
    //bm::for_each<hilbert::three_d::HilbertRecursionBase<0, false, false, false>>(printer());
    return 0;
}


/***************************************************************************
 *   Copyright (C) 2007 by Marco Correia                                   *
 *   mvc@di.fct.unl.pt                                                     *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 ***************************************************************************/

#include <casper/kernel.h>
#include <casper/cp/set.h>
#include <casper/cp/int.h>
#include <casper/util/options.h>

#include <boost/math/special_functions/factorials.hpp>
#include <iostream>

using namespace Casper;
using namespace Casper::CP;
//using namespace std;

#if 0
/**
 * Computes the set of all n-ary sums of the elements in a set domain variable.
 * \note needs testing
 */
template<class Arg1,class Arg2>
struct NSumsEqual : IFilter
{
	typedef typename DomView<Set<int>,Arg1>::Dom	DomX;
	typedef typename DomView<Set<int>,Arg2>::Dom	DomY;

	NSumsEqual(CPSolver& solver,const Arg1& xx, const Arg2& yy, const uint& n) :
				  IFilter(solver),x(solver,xx),y(solver,yy),n(n) {}

	bool execute();
	void attach(INotifiable* f)
	{	x->attachOnDomain(f); y->attachOnDomain(f);	}
	void detach(INotifiable* f)
	{	x->detachOnDomain(f); y->detachOnDomain(f);	}

	Cost cost() const
	{	return quadraticLo;	}

	template<class Iteration>
	List<int> getSums(Iteration it, uint tsize);
	template<class Iteration>
	List<int> getSumsSorted(Iteration it, uint tsize);

	DomView<Set<int>,Arg1>			x;
	DomView<Set<int>,Arg2>			y;
	const uint						n;
};

template<class Arg1,class Arg2>
template<class Iteration>
List<int> NSumsEqual<Arg1,Arg2>::getSums(Iteration it, uint tsize)
{
	List<int> ret;
	if (tsize == 1)
	{
		for ( ; it.valid(); it.iterate())
			ret.pushBack(it.value());
		return ret;
	}
	uint c = 0;
	List<int> sums_1(getSums(it,tsize-1));
	for (List<int>::Iterator vit = sums_1.begin(); vit != sums_1.end(); ++vit)
	{
		Iteration iit(it);
		for (uint i = 0; iit.valid() and i < tsize-1; ++i)
			iit.iterate();
		for (uint i = 0; iit.valid() and i < c; ++i)
			iit.iterate();
		for ( ; iit.valid(); iit.iterate())
			ret.pushBack(iit.value()+*vit);
		++c;
	}
	return ret;
}

template<class Arg1,class Arg2>
template<class Iteration>
List<int> NSumsEqual<Arg1,Arg2>::getSumsSorted(Iteration it, uint tsize)
{
	List<int> sumsUnsorted(getSums(it,tsize));
	std::set<int> sumsV(sumsUnsorted.begin(),sumsUnsorted.end());
	List<int> ret(sumsV.begin(),sumsV.end());
	return ret;
}

template<class Arg1,class Arg2>
bool NSumsEqual<Arg1,Arg2>::execute()
{
//	std::cout << "begin: " << *x << " " << *y << std::endl;
	if (x->inSize() >= n)
		if (!Detail::setSafeInsertRange(*y,makeIt(getSumsSorted(makeInIt(*x),n))))
			return false;

	if (x->inSize()+x->possSize() >= n)
		if (!Detail::setSafeEraseRange(*y,makeDiffIt(makePossIt(*y),
										  makeIt(getSumsSorted(makeLUBIt(*x),n)))))
			return false;
//	std::cout << "end: " << *x << " " << *y << std::endl;
	// not sure if processing "in y" events makes sense. ignoring for now.

	// if an element r is removed from y then we remove from x.poss all e such that
	// e+s=r where s is a possible (n-1) sum already in x.in
/*	std::set<int> inSums_1(getSums(makeInIt(*x),n-1));
	for(typename DomX::PIterator pit = x->beginPoss(); pit != x->endPoss(); )
	{
		for (std::set<int>::iterator it = inSums_1.begin(); it != inSums_1.end(); ++it)
			for (yLUBDIt = y->lubDeltas().beginFrom(yLUBDeltasIt);
					yLUBDIt != y->lubDeltas().end(); ++yLUBDIt)
				for (typename DomX::DeltasIterator dit = yLUBDIt->begin();
						dit != yLUBDIt->end(); ++dit)
					if (*it + *pit == *dit)
					{
						if (x->safeErase(pit++))
							return false;
						goto next;
					}
		++pit;
		next:;
	}
	yLUBDeltasIt = y->lubDeltas().end();*/
	return true;
}

template<class Arg1,class Arg2>
Filter nSumsEqual(const Arg1& a1, const Arg2& a2, const uint& n)
{ return new (getState(a1,a2).getHeap()) NSumsEqual<Arg1,Arg2>(getState(a1,a2),a1,a2,n); }
#endif

template<class Arg1,class Arg2>
struct SortSubsetsWithSameSum : IFilter
{
	typedef typename DomView<Set<int>,Arg1>::Dom	DomX;
	typedef typename DomView<Set<int>,Arg2>::Dom	DomY;

	SortSubsetsWithSameSum(Solver& solver,const Arg1& xx, const Arg2& yy) :
				  IFilter(solver),x(solver,xx),y(solver,yy) {}

	bool execute();
	void attach(INotifiable* f)
	{	x->attachOnGLB(f); y->attachOnGLB(f);	}
	void detach(INotifiable* f)
	{	x->detachOnGLB(f); y->detachOnGLB(f);	}

	Cost cost() const
	{	return quadraticLo;	}

	struct Cmp {
		bool operator()(const Util::StdPair<int>& s1,const Util::StdPair<int>& s2) const
		{
			if (s1.first < s2.first)
				return true;
			if (s1.first > s2.first)
				return false;
			return s1.second < s2.second;
		}
	};

	typedef Util::StdList<Util::StdPair<int> > Sums;
	typedef std::set<Util::StdPair<int>,Cmp> AllSums;

	template<class Iteration>
	Sums getSums(Iteration it, const Sums&, uint tsize);
	template<class Iteration>
	AllSums getSums(Iteration it, uint tsize);

	DomView<Set<int>,Arg1>			x;
	DomView<Set<int>,Arg2>			y;
};

template<class Arg1,class Arg2>
template<class Iteration>
typename SortSubsetsWithSameSum<Arg1,Arg2>::Sums
SortSubsetsWithSameSum<Arg1,Arg2>::getSums(Iteration it,const Sums& sums_1,
												uint tsize)
{
	assert(tsize>1);
	Sums ret;
	uint c = 0;
	for (Sums::Iterator vit = sums_1.begin(); vit != sums_1.end(); ++vit)
	{
		Iteration iit(it);
		for (uint i = 0; iit.valid() and i < tsize-1; ++i)
			iit.iterate();
		for (uint i = 0; iit.valid() and i < c; ++i)
			iit.iterate();
		for ( ; iit.valid(); iit.iterate())
			ret.pushBack(Util::StdPair<int>(iit.value()+vit->first,iit.value()));
		++c;
	}
	return ret;
}

template<class Arg1,class Arg2>
template<class Iteration>
typename SortSubsetsWithSameSum<Arg1,Arg2>::AllSums
SortSubsetsWithSameSum<Arg1,Arg2>::getSums(Iteration it,uint tsize)
{
	AllSums all;

	Sums* xold = new Sums();
	for (Iteration iit(it) ; iit.valid(); iit.iterate())
	{
		xold->pushBack(Util::StdPair<int>(iit.value(),iit.value()));
		all.insert(Util::StdPair<int>(iit.value(),iit.value()));
	}

	for (uint i = 2; i <= tsize; ++i)
	{
		Sums* xnew = new Sums(getSums(it,*xold,i));
		all.insert(xnew->begin(),xnew->end());
		delete xold;
		xold = xnew;
	}
	delete xold;
	return all;
}

template<class Arg1,class Arg2>
bool SortSubsetsWithSameSum<Arg1,Arg2>::execute()
{
	AllSums sumsx(getSums(makeInIt(*x),x->inSize()));
	AllSums sumsy(getSums(makeInIt(*y),y->inSize()));
	typename AllSums::iterator itx = sumsx.begin();
	typename AllSums::iterator ity = sumsy.begin();

/*	std::cout << "xin: " << x->in() << std::endl;
	std::cout << "sumsx: ";
	print(sumsx.begin(),sumsx.end());
	std::cout << std::endl;
*/
	while (itx != sumsx.end() and ity != sumsy.end())
	{
		if (itx->first < ity->first)
			++itx;
		if (ity->first < itx->first)
			++ity;
		if (itx->first == ity->first)
		{
			if (itx->second > ity->second)
			{
		//		std::cout << "failing\n";
				return false;
			}
			typename AllSums::iterator next(itx);
			++next;
			if (next != sumsx.end() and next->first == ity->first)
				++itx;
			else
				++ity;
		}
	}
	return true;
}

template<class Arg1,class Arg2>
Filter sortSubsetsWithSameSum(const Arg1& a1, const Arg2& a2)
{ return new (getState(a1,a2).getHeap()) SortSubsetsWithSameSum<Arg1,Arg2>(getState(a1,a2),a1,a2); }

struct GreedyLabeling : IGoal
{
	GreedyLabeling(Store& store,const VarArray<IntSet>& vars) : IGoal(),store(store),
			vars(vars) {}
	Goal execute()
	{
		// select maximum unassigned number
		int maxn = limits<int>::min();
		for (uint i = 0; i < vars.size(); ++i)
			for (SetFD<int>::PIterator it = vars[i].domain().beginPoss();
					it != vars[i].domain().endPoss(); ++it)
				if (*it > maxn)
					maxn = *it;

		// find the set with the smallest sum
		int mins = limits<int>::max();
		int minidx = -1;
		for (uint i = 0; i < vars.size(); ++i)
		{
			uint sumIn = 0;
			if (vars[i].domain().findInPoss(maxn)!=vars[i].domain().endPoss())
			{
				for (SetFD<int>::IIterator it = vars[i].domain().beginIn();
						it != vars[i].domain().endIn(); ++it)
					sumIn += *it;
				if (sumIn < mins)
				{
					mins = sumIn;
					minidx = i;
				}
			}
		}
		if (minidx<0)
			return succeed();
		return Goal(store,
				(post(store,member(maxn,vars[minidx])) or post(store,notMember(maxn,vars[minidx]))) and
				Goal(this));
	}
	Store& store;
	VarArray<IntSet> vars;
};

Goal greedyLabeling(Store& store,VarArray<IntSet> vars)
{	return new (store) GreedyLabeling(store,vars);	}

void partition(uint nvalues, uint nsets, int optimum = -2)
{
	Solver solver;
	IntSetVarArray vars(solver,nsets,range(1,nvalues));

//	for (uint i = 0; i < vars.count(); ++i)
//		cout << vars[i].pDomain() << " " << "A" << endl;

#if 1
	for (uint i = 0; i < vars.count(); i++)
		for (uint j = i+1; j < vars.count(); j++)
			solver.post( disjoint(vars(i),vars(j)) );
	/*solver.post(unionEqual(vars,Var<IntSet>(solver,
						List<int>(range(1,nvalues).begin(),range(1,nvalues).end()),
						List<int>())));*/

	VarArray<IntSet> avars(solver,nsets,range(1,nvalues));
	avars[0] = vars[0];
	for (uint i = 0; i < nsets-1; i++)
		solver.post( unionEqual(avars(i),vars(i+1),avars(i+1)) );
	solver.post(avars(nsets-1)==Var<IntSet>(solver,
						Util::StdList<int>(range(1,nvalues).begin(),range(1,nvalues).end()),
						Util::StdList<int>()) );

#else
	solver.post(partition(vars));
#endif

	const uint total = nvalues/2.0*(2+(nvalues-1));
	IntVar maxSums(solver,std::ceil(((float)total)/nsets),total);

	if (optimum==-2)
	{
		const float magicNumberF = ((float)total)/nsets;
		const uint magicNumber = (int)magicNumberF;
		if (magicNumber!=magicNumberF)	{
			std::cout << "trivially impossible!\n";
			return;
		}

		for (uint p = 0; p < nsets; ++p)
			solver.post(sumEqual(vars[p],magicNumber));
	}
	else
	if (optimum==-1)
	{
		// at least one element per set so that we can
		// enforce symmetry breaking constraints below

		IntVarArray sums(solver,nsets,1,total);
		for (uint p = 0; p < nsets; ++p)
			solver.post(sumEqual(vars[p],sums[p]));
		solver.post(max(sums)==maxSums);
		solver.setExplorer(bbMinimize(solver,maxSums));
	}
	else
		for (uint p = 0; p < nsets; ++p)
		{
			IntVar s(solver,1,total);
			solver.post(sumEqual(vars[p],s));
			solver.post(s<=optimum);
		}

	// symmetry breaking: sets are interchangeable
#if 1
	for (uint p = 1; p < nsets; ++p)
		solver.post(min(vars[p])>min(vars[p-1]));
#else
	for (uint p1 = 0; p1 < nsets; ++p1)
		for (uint p2 = p1+1; p2 < nsets; ++p2)
			solver.post( sortSubsetsWithSameSum(vars[p1],vars[p2]));
#endif

//	bool ret = solver.solve( label( vars, selectVarFFRR(vars) ) );
	bool ret = solver.solve( greedyLabeling(solver,vars) );

	if (optimum==-2 or optimum>=0)
		std::cout << ret << std::endl << vars << std::endl;
	else
		while (ret)
		{
			std::cout << vars << " : " << maxSums << " optimimum="
						<< ((float)total)/nsets << std::endl;
			ret = solver.next();
		}
	std::cout << solver.getStats() << std::endl
				 << solver.getCPUTimer() << std::endl;
}

int main(int argc, char** argv)
{
	if (argc != 3 and argc != 4)
	{
		std::cout << "usage: " << argv[0] << " nvalues nsets [optimum]\n";
		std::cout << "set optimum=-2 for perfect partition, optimum=-1 for minimization\n";
		return 1;
	}
	::partition(atoi(argv[1]),atoi(argv[2]),argc==3?-1:atoi(argv[3]));
}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <Eigen/Core>
#include "components/RelatednessComponent/LapackEigenDecomposition.hpp"
#include <iostream>
#include "config/config.hpp"
#if HAVE_CLAPACK
	#include "clapack.h"
#else

extern "C" {
	/*
	*  function dsyev_ (see http://www.netlib.org/clapack/what/double/dsyev.c)

	-- LAPACK driver routine (version 3.0) --
		Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
		Courant Institute, Argonne National Lab, and Rice University
		June 30, 1999

	*
	* Arguments:
	*
	JOBZ	(input) CHARACTER*1
			= 'N':	Compute eigenvalues only;
			= 'V':	Compute eigenvalues and eigenvectors.

	UPLO	(input) CHARACTER*1
			= 'U':	Upper triangle of A is stored;
			= 'L':	Lower triangle of A is stored.

	N		(input) INTEGER
			The order of the matrix A.	N >= 0.

	A		(input/output) DOUBLE PRECISION array, dimension (LDA, N)
			On entry, the symmetric matrix A.  If UPLO = 'U', the
			leading N-by-N upper triangular part of A contains the
			upper triangular part of the matrix A.	If UPLO = 'L',
			the leading N-by-N lower triangular part of A contains
			the lower triangular part of the matrix A.
			On exit, if JOBZ = 'V', then if INFO = 0, A contains the
			orthonormal eigenvectors of the matrix A.
			If JOBZ = 'N', then on exit the lower triangle (if UPLO='L')
			or the upper triangle (if UPLO='U') of A, including the
			diagonal, is destroyed.

	LDA		(input) INTEGER
			The leading dimension of the array A.  LDA >= max(1,N).

	W		(output) DOUBLE PRECISION array, dimension (N)
			If INFO = 0, the eigenvalues in ascending order.

	WORK	(workspace/output) DOUBLE PRECISION array, dimension (LWORK)
			On exit, if INFO = 0, WORK(1) returns the optimal LWORK.

	LWORK	(input) INTEGER
			The length of the array WORK.  LWORK >= max(1,3*N-1).
			For optimal efficiency, LWORK >= (NB+2)*N,
			where NB is the blocksize for DSYTRD returned by ILAENV.

			If LWORK = -1, then a workspace query is assumed; the routine
			only calculates the optimal size of the WORK array, returns
			this value as the first entry of the WORK array, and no error
			message related to LWORK is issued by XERBLA.

	INFO	(output) INTEGER
			= 0:  successful exit
			< 0:  if INFO = -i, the i-th argument had an illegal value
			> 0:  if INFO = i, the algorithm failed to converge; i
				  off-diagonal elements of an intermediate tridiagonal
				  form did not converge to zero.
	*/
	int dsyev_(char *jobz, char *uplo, int *n, double *a,
		 int *lda, double *w, double *work, int *lwork,
		int *info) ;
}

#endif

#include <vector>

namespace lapack
{
	void compute_eigendecomposition( Eigen::MatrixXd const& matrix, Eigen::VectorXd* eigenvalues, Eigen::MatrixXd* eigenvectors ) {
		int N = matrix.cols() ;
		assert( matrix.rows() == N ) ;
		*eigenvectors = matrix ;
		int LDA = eigenvectors->outerStride() ;
		int LWORK = -1 ;
		int info = 0 ;
		char JOBZ = 'V' ;
		char UPLO = 'L' ;
		double work_size ;
		dsyev_( &JOBZ, &UPLO, &N, 0, &LDA, 0, &work_size, &LWORK, &info ) ;
		LWORK = work_size + 32 ;
		std::vector< double > workspace( work_size ) ;
		dsyev_( &JOBZ, &UPLO, &N, eigenvectors->data(), &LDA, eigenvalues->data(), &workspace[0], &LWORK, &info ) ;
		if( info != 0 ) {
			std::cerr << "!! compute_eigendecomposition(): info = " << info << ".\n" ;
			if( info < 0 ) {
				assert( 0 ) ;
			}
		}
	}
}

#if !HAVE_CLAPACK
extern "C" {
/*
 *	function dsyevr_ (see http://www.netlib.org/clapack/what/double/dsyevr.c)

	Arguments
	=========

	JOBZ	(input) CHARACTER*1
			= 'N':	Compute eigenvalues only;
			= 'V':	Compute eigenvalues and eigenvectors.

	RANGE	(input) CHARACTER*1
			= 'A': all eigenvalues will be found.
			= 'V': all eigenvalues in the half-open interval (VL,VU]
				   will be found.
			= 'I': the IL-th through IU-th eigenvalues will be found.
   ********* For RANGE = 'V' or 'I' and IU - IL < N - 1, DSTEBZ and
   ********* DSTEIN are called

	UPLO	(input) CHARACTER*1
			= 'U':	Upper triangle of A is stored;
			= 'L':	Lower triangle of A is stored.

	N		(input) INTEGER
			The order of the matrix A.	N >= 0.

	A		(input/output) DOUBLE PRECISION array, dimension (LDA, N)
			On entry, the symmetric matrix A.  If UPLO = 'U', the
			leading N-by-N upper triangular part of A contains the
			upper triangular part of the matrix A.	If UPLO = 'L',
			the leading N-by-N lower triangular part of A contains
			the lower triangular part of the matrix A.
			On exit, the lower triangle (if UPLO='L') or the upper
			triangle (if UPLO='U') of A, including the diagonal, is
			destroyed.

	LDA		(input) INTEGER
			The leading dimension of the array A.  LDA >= max(1,N).

	VL		(input) DOUBLE PRECISION
	VU		(input) DOUBLE PRECISION
			If RANGE='V', the lower and upper bounds of the interval to
			be searched for eigenvalues. VL < VU.
			Not referenced if RANGE = 'A' or 'I'.

	IL		(input) INTEGER
	IU		(input) INTEGER
			If RANGE='I', the indices (in ascending order) of the
			smallest and largest eigenvalues to be returned.
			1 <= IL <= IU <= N, if N > 0; IL = 1 and IU = 0 if N = 0.
			Not referenced if RANGE = 'A' or 'V'.

	ABSTOL	(input) DOUBLE PRECISION
			The absolute error tolerance for the eigenvalues.
			An approximate eigenvalue is accepted as converged
			when it is determined to lie in an interval [a,b]
			of width less than or equal to

					ABSTOL + EPS *	 max( |a|,|b| ) ,

			where EPS is the machine precision.	 If ABSTOL is less than
			or equal to zero, then	EPS*|T|	 will be used in its place,
			where |T| is the 1-norm of the tridiagonal matrix obtained
			by reducing A to tridiagonal form.

			See "Computing Small Singular Values of Bidiagonal Matrices
			with Guaranteed High Relative Accuracy," by Demmel and
			Kahan, LAPACK Working Note #3.

			If high relative accuracy is important, set ABSTOL to
			DLAMCH( 'Safe minimum' ).  Doing so will guarantee that
			eigenvalues are computed to high relative accuracy when
			possible in future releases.  The current code does not
			make any guarantees about high relative accuracy, but
			furutre releases will. See J. Barlow and J. Demmel,
			"Computing Accurate Eigensystems of Scaled Diagonally
			Dominant Matrices", LAPACK Working Note #7, for a discussion
			of which matrices define their eigenvalues to high relative
			accuracy.

	M		(output) INTEGER
			The total number of eigenvalues found.	0 <= M <= N.
			If RANGE = 'A', M = N, and if RANGE = 'I', M = IU-IL+1.

	W		(output) DOUBLE PRECISION array, dimension (N)
			The first M elements contain the selected eigenvalues in
			ascending order.

	Z		(output) DOUBLE PRECISION array, dimension (LDZ, max(1,M))
			If JOBZ = 'V', then if INFO = 0, the first M columns of Z
			contain the orthonormal eigenvectors of the matrix A
			corresponding to the selected eigenvalues, with the i-th
			column of Z holding the eigenvector associated with W(i).
			If JOBZ = 'N', then Z is not referenced.
			Note: the user must ensure that at least max(1,M) columns are
			supplied in the array Z; if RANGE = 'V', the exact value of M
			is not known in advance and an upper bound must be used.

	LDZ		(input) INTEGER
			The leading dimension of the array Z.  LDZ >= 1, and if
			JOBZ = 'V', LDZ >= max(1,N).

	ISUPPZ	(output) INTEGER array, dimension ( 2*max(1,M) )
			The support of the eigenvectors in Z, i.e., the indices
			indicating the nonzero elements in Z. The i-th eigenvector
			is nonzero only in elements ISUPPZ( 2*i-1 ) through
			ISUPPZ( 2*i ).
   ********* Implemented only for RANGE = 'A' or 'I' and IU - IL = N - 1

	WORK	(workspace/output) DOUBLE PRECISION array, dimension (LWORK)
			On exit, if INFO = 0, WORK(1) returns the optimal LWORK.

	LWORK	(input) INTEGER
			The dimension of the array WORK.  LWORK >= max(1,26*N).
			For optimal efficiency, LWORK >= (NB+6)*N,
			where NB is the max of the blocksize for DSYTRD and DORMTR
			returned by ILAENV.

			If LWORK = -1, then a workspace query is assumed; the routine
			only calculates the optimal size of the WORK array, returns
			this value as the first entry of the WORK array, and no error
			message related to LWORK is issued by XERBLA.

	IWORK	(workspace/output) INTEGER array, dimension (LIWORK)
			On exit, if INFO = 0, IWORK(1) returns the optimal LWORK.

	LIWORK	(input) INTEGER
			The dimension of the array IWORK.  LIWORK >= max(1,10*N).

			If LIWORK = -1, then a workspace query is assumed; the
			routine only calculates the optimal size of the IWORK array,
			returns this value as the first entry of the IWORK array, and
			no error message related to LIWORK is issued by XERBLA.

	INFO	(output) INTEGER
			= 0:  successful exit
			< 0:  if INFO = -i, the i-th argument had an illegal value
			> 0:  Internal error
*/
	int dsyevr_(
		char *jobz, char *range, char *uplo, int *n,
		double *a, int *lda,
		double *vl, double *vu,
		int *il,
		int *iu,
		double *abstol,
		int *m, double *w,
		double *z__, int *ldz,
		int *isuppz,
		double *work, int *lwork, int *iwork, int *liwork, int *info
	) ;

	double dlamch_( char *cmach ) ;
}
#endif

namespace lapack {
	void compute_eigendecomposition(
		Eigen::MatrixXd const& input_matrix,
		Eigen::VectorXd* eigenvalues,
		Eigen::MatrixXd* eigenvectors,
		double minimum_eigenvalue,
		double maximum_eigenvalue
	) {
		assert( input_matrix.rows() == input_matrix.cols() ) ;
		assert( eigenvalues ) ;
		assert( eigenvectors ) ;
		assert( minimum_eigenvalue == minimum_eigenvalue ) ;
		assert( maximum_eigenvalue == maximum_eigenvalue ) ;
		assert( minimum_eigenvalue < maximum_eigenvalue ) ;

		Eigen::MatrixXd matrix = input_matrix ;
		int N = matrix.cols() ;
		assert( matrix.rows() == N ) ;
		int M_LDA = matrix.outerStride() ;
		int EV_LDA = eigenvectors->outerStride() ;
		int info = 0 ;
		char RANGE = 'V' ;
		char JOBZ = 'V' ;
		char UPLO = 'L' ;
		double ABSTOL = dlamch_( const_cast< char* >( "Safe minimum" ) ) ;
		int number_of_eigenvalues ; // numbers of eigenvectors that are computed

		// set up workspaces
		int LWORK = -1 ;
		double work_size ;
		int LIWORK = -1 ;
		int iwork_size ;

		dsyevr_(
			&JOBZ, &RANGE, &UPLO, &N,
			matrix.data(), &M_LDA,
			&minimum_eigenvalue, &maximum_eigenvalue,
			0, 0,
			&ABSTOL,
			&number_of_eigenvalues, eigenvalues->data(),
			eigenvectors->data(), &EV_LDA,
			0,
			&work_size, &LWORK, &iwork_size, &LIWORK, &info
		) ;
		assert( info == 0 ) ;
		LWORK = work_size + 32 ;
		LIWORK = iwork_size + 32 ;
		std::vector< double > workspace( work_size ) ;
		std::vector< int > iworkspace( iwork_size ) ;

		// Now compute the decomposition.
		eigenvalues->resize( input_matrix.cols() ) ;
		eigenvectors->resize( input_matrix.cols(), input_matrix.cols() ) ;

		dsyevr_(
			&JOBZ, &RANGE, &UPLO, &N,
			const_cast< double* >( matrix.data() ), &M_LDA,
			&minimum_eigenvalue, &maximum_eigenvalue,
			0, 0,
			&ABSTOL,
			&number_of_eigenvalues, eigenvalues->data(),
			eigenvectors->data(), &EV_LDA,
			0,
			&workspace[0],
			&LWORK,
			&iworkspace[0],
			&LIWORK,
			&info
		) ;
		if( info != 0 ) {
			std::cerr << "!! compute_eigendecomposition(): info = " << info << ".\n" ;
			if( info < 0 ) {
				assert( 0 ) ;
			}
		} else {
			eigenvalues->resize( number_of_eigenvalues ) ;
			eigenvectors->resize( eigenvectors->rows(), number_of_eigenvalues ) ;
		}
	}
}

namespace lapack {
	void compute_eigendecomposition(
		Eigen::MatrixXd const& input_matrix,
		Eigen::VectorXd* eigenvalues,
		Eigen::MatrixXd* eigenvectors,
		int number_of_eigenvalues
	) {
		assert( input_matrix.rows() == input_matrix.cols() ) ;
		assert( number_of_eigenvalues > 0 ) ;
		assert( eigenvalues ) ;
		assert( eigenvectors ) ;
		assert( number_of_eigenvalues <= input_matrix.cols() ) ;

		Eigen::MatrixXd matrix = input_matrix ;
		int N = matrix.cols() ;
		assert( matrix.rows() == N ) ;
		int M_LDA = matrix.outerStride() ;
		int EV_LDA = eigenvectors->outerStride() ;
		int info = 0 ;
		char RANGE = 'I' ;
		char JOBZ = 'V' ;
		char UPLO = 'L' ;
		double ABSTOL = dlamch_( const_cast< char* >( "Safe minimum" ) ) ;
		int IL = 1;
		int IU = number_of_eigenvalues + 1 ;

		// set up workspaces
		int LWORK = -1 ;
		double work_size ;
		int LIWORK = -1 ;
		int iwork_size ;

		dsyevr_(
			&JOBZ, &RANGE, &UPLO, &N,
			matrix.data(), &M_LDA,
			0, 0,
			&IL, &IU,
			&ABSTOL,
			&number_of_eigenvalues, eigenvalues->data(),
			eigenvectors->data(), &EV_LDA,
			0,
			&work_size, &LWORK, &iwork_size, &LIWORK, &info
		) ;
		assert( info == 0 ) ;
		LWORK = work_size + 32 ;
		LIWORK = iwork_size + 32 ;
		std::vector< double > workspace( work_size ) ;
		std::vector< int > iworkspace( iwork_size ) ;

		// compute the decomposition
		eigenvalues->resize( number_of_eigenvalues ) ;
		eigenvectors->resize( input_matrix.cols(), number_of_eigenvalues ) ;

		dsyevr_(
			&JOBZ, &RANGE, &UPLO, &N,
			const_cast< double* >( matrix.data() ), &M_LDA,
			0, 0,
			&IL, &IU,
			&ABSTOL,
			&number_of_eigenvalues, eigenvalues->data(),
			eigenvectors->data(), &EV_LDA,
			0,
			&workspace[0],
			&LWORK,
			&iworkspace[0],
			&LIWORK,
			&info
		) ;
		if( info != 0 ) {
			std::cerr << "!! compute_eigendecomposition(): info = " << info << ".\n" ;
			if( info < 0 ) {
				assert( 0 ) ;
			}
		} else {
			eigenvalues->resize( number_of_eigenvalues ) ;
			eigenvectors->resize( eigenvectors->rows(), number_of_eigenvalues ) ;
		}
	}
}


#ifndef SETTINGS_HPP
#define SETTINGS_HPP

#include <boost/lexical_cast.hpp>
#include <clstatphys/ensemble/hybrid_monte_carlo.hpp>
#include <clstatphys/physics/todalattice.hpp>
#include <common/settings_common.hpp>

using EnsemblerOrigin = ensemble::HybridMonteCarlo;
using Hamiltonian = hamiltonian::TodaLattice;

class Ensembler : public EnsemblerOrigin{
public:
  template<class Hamiltonian>
  Ensembler(
            int num_particles, double temperture, double dt_relax,
            double relax_time, int total_accept, int initial_relax_time,
            Hamiltonian hamiltonian)
    : EnsemblerOrigin(num_particles, temperture, dt_relax, relax_time, total_accept),
      initial_relax_time_(initial_relax_time) {hamiltonian_ = hamiltonian;}

  template<class Rand>
  void set_initial_state(std::vector<double> & z, Rand & mt){
    int counter = 0;
    for(int step = 0; step < initial_relax_time_; ++step) EnsemblerOrigin::montecarlo(z, counter, hamiltonian_, mt);
  }

private:
  int initial_relax_time_;
  Hamiltonian hamiltonian_;
}; //end Ensembler definition

struct Settings : public SettingsCommon{
  int total_accept = 10;
  double initial_relax_time = 10;
  double relax_time = 10;
  double dt_relax = 0.1;
  double temperture = 1.0;

  double J = 1.0; //interaction constant;
  double alpha = 1.0; //interaction constant;

  Settings(int argc, char **argv, int & input_counter)
    : SettingsCommon(argc, argv, input_counter)
  {
    set(argc, argv, input_counter);
  }
  Settings() = default;

  inline void set(int argc, char **argv, int & input_counter){
    if (argc > input_counter) total_accept       = boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (argc > input_counter) initial_relax_time = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) relax_time         = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) dt_relax           = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) temperture         = boost::lexical_cast<double>(argv[input_counter]);++input_counter;

    if (argc > input_counter) J                  = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) alpha              = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
  }

  template <class Dataput>
  inline void declare(Dataput & dataput){
    SettingsCommon::declare(dataput);

    dataput <<  "<<System Depend Settings>> " << std::endl
            <<  "  " << Hamiltonian::name() << std::endl
            <<  "  " << EnsemblerOrigin::name() << std::endl
            <<  "  " << Lattice::name() << std::endl

            << "  Number of step for fianally accept : total_accept = " << total_accept << std::endl
            << "  Relaxtion time for update : relax_time = " << relax_time << std::endl
            << "  Interbal of time development : dt_relax  = " << dt_relax << std::endl
            << "  Temperture : temperture = " << temperture << std::endl

            << "  Coupling constant : J = " << J << std::endl
            << "  Coupling constant : alpha = " << alpha << std::endl;
  }

  Hamiltonian hamiltonian(){
    Lattice lattice_t(Ns);

    std::vector<std::vector<int> > pair_table(
                                              num_particles,
                                              std::vector<int>(N_adj)
                                              );
    lattice_t.create_table(pair_table);

    Hamiltonian hamiltonian_t(
                              num_particles,
                              J,
                              alpha,
                              pair_table,
                              N_adj
                              );
    return hamiltonian_t;
  }

  Ensembler ensembler(){
    Ensembler ensembler_t(
                          num_particles,
                          temperture,
                          dt_relax,
                          relax_time,
                          total_accept,
                          initial_relax_time,
                          hamiltonian()
                          );
    return ensembler_t;
  }

}; //end Settings definition

#endif


#pragma once

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <opt/optimization_problem.hpp>

namespace ccd {
namespace opt {

    /**
     * We handle the LCP(A, b), which seeks vectors s and x which satisfy the
     * following constraints
     * \f{align*}
     *     s = A x + b \\
     *     0 \leq x \perp s \geq 0
     * \f}
     */
    enum LCPSolver {
        /// Solve the LCP using the Guass-Seidel method
        LCP_GAUSS_SEIDEL,
        /// Solve the LCP as a QP using Mosek
        LCP_MOSEK,
        /// Sove the LCP using minimum-map Newton's method
        LCP_NEWTON
    };

    /**
     * @brief Solve the LCP using a given solver.
     */
    bool lcp_solve(const Eigen::VectorXd& gxi,
        const Eigen::MatrixXd& jac_gxi,
        const Eigen::MatrixXd& tilde_jac_gxi,
        const Eigen::VectorXd& tilde_b,
        const LCPSolver solver,
        Eigen::VectorXd& alpha);

    /**
     * @brief Solve the LCP using Gauss-Seidel
     *
     * To use our gauss-seidel implementation we require to express
     * the problem as
     *      s = q + N (Mx + p)
     *
     * @param gxi \f$q\f$
     * @param jac_gxi \f$N\f$
     * @param tilde_jac_gxi \f$M\f$
     * @param tilde_b \f$p\f$
     * @param alpha \f$x\f$
     */
    bool lcp_gauss_seidel(const Eigen::VectorXd& gxi,
        const Eigen::MatrixXd& jac_gxi,
        const Eigen::MatrixXd& tilde_jac_gxi,
        const Eigen::VectorXd& tilde_b,
        Eigen::VectorXd& alpha);

#if BUILD_WITH_MOSEK
    bool lcp_mosek(
        const Eigen::MatrixXd& A, const Eigen::VectorXd& b, Eigen::VectorXd& x);
#endif

    bool lcp_newton(const Eigen::MatrixXd& A_dense,
        const Eigen::VectorXd& b,
        Eigen::VectorXd& x);

} // namespace opt
} // namespace ccd