// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <complex>
#include <cmath>

#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>

#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>

using namespace mtl::recursion;
using namespace std;

template <typename Matrix>
void test(Matrix& matrix, const char* name)
{
    typedef typename mtl::Collection<Matrix>::size_type   size_type;
    {
	mtl::mat::inserter<Matrix> ins(matrix);
	for (size_type i= 0; i < matrix.num_rows(); i++)
	    for (size_type j= 0; j < matrix.num_cols(); j++)
		if ((i + j) & 1)
		    ins(i, j) << i + 2*j;
    }

    // std::cout << "\n" << name << "\n" << matrix << "\n"; // Get us complaints from valgrind
    set_to_zero(matrix);
    std::cout << "\n" << name << "\n" << "should be empty now:\n" << matrix << "\n";
    typename Matrix::value_type zero(0.0);
    MTL_THROW_IF(matrix(0, 1) != zero, mtl::runtime_error("not properly set to zero"));
}

int main(int, char**)
{
    using namespace mtl;

    dense2D<double>                                dr(5, 7);
    dense2D<double, mat::parameters<col_major> > dc(5, 7);
    dense2D<std::complex<double> >                 cdr(5, 7);
    morton_dense<double,  morton_mask>             md(5, 7);
    morton_dense<double,  doppled_16_row_mask>     d16r(5, 7);
    compressed2D<double>                           comp(5, 7);
    compressed2D<std::complex<double> >            ccomp(5, 7);

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(cdr, "Complex dense row major");
    test(md, "Morton N-order");
    test(d16r, "Hybrid 16 row-major");
    test(comp, "compressed2D");
    test(ccomp, "complex compressed2D");

    return 0;
}


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

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  trunc generic tag

      Represents the trunc function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct trunc_ : ext::elementwise_<trunc_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<trunc_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_trunc_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::trunc_, Site> dispatching_trunc_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::trunc_, Site>();
    }
    template<class... Args>
    struct impl_trunc_;
  }
  /*!
    Computes the truncation toward zero of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = trunc(x);
    @endcode

    The code is similar to:

    @code
    T r = sign(x)*floor(abs(x));
    @endcode

    @par Note:

    For floating point number it is also one of the two ouputs of
    the modf function.
    And we have:

    @code
    trunc(x) + frac(x) == x;
    @endcode

    except for nans

    @par Alias

    fix

    @see @funcref{abs}, @funcref{frac}, @funcref{floor}, @funcref{sign}
    @param  a0

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::trunc_, trunc, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::trunc_, fix, 1)
} }

#endif


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef TYSONNOVAKCELLCYCLEMODEL_HPP_
#define TYSONNOVAKCELLCYCLEMODEL_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractOdeBasedCellCycleModel.hpp"
#include "TysonNovak2001OdeSystem.hpp"

/**
 *  Tyson-Novak 2001 cell-cycle model, taken from the version at doi:10.1006/jtbi.2001.2293
 *
 *  Time taken to progress through the cycle is deterministic and given by
 *  an ODE system independent of external factors.
 */
class TysonNovakCellCycleModel : public AbstractOdeBasedCellCycleModel
{
private:

    friend class TestOdeBasedCellCycleModels;

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the cell-cycle model, never used directly - boost uses this.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractOdeBasedCellCycleModel>(*this);
    }

protected:

    /**
     * Protected copy-constructor for use by CreateCellCycleModel.
     * The only way for external code to create a copy of a cell cycle model
     * is by calling that method, to ensure that a model of the correct subclass is created.
     * This copy-constructor helps subclasses to ensure that all member variables are correctly copied when this happens.
     *
     * This method is called by child classes to set member variables for a daughter cell upon cell division.
     * Note that the parent cell cycle model will have had ResetForDivision() called just before CreateCellCycleModel() is called,
     * so performing an exact copy of the parent is suitable behaviour. Any daughter-cell-specific initialisation
     * can be done in InitialiseDaughterCell().
     *
     * @param rModel the cell cycle model to copy.
     */
    TysonNovakCellCycleModel(const TysonNovakCellCycleModel& rModel);

public:

    /**
     * Default constructor.
     *
     * @param pOdeSolver An optional pointer to a cell-cycle model ODE solver object (allows the use of different ODE solvers)
     */
    TysonNovakCellCycleModel(boost::shared_ptr<AbstractCellCycleModelOdeSolver> pOdeSolver = boost::shared_ptr<AbstractCellCycleModelOdeSolver>());

    /**
     * Initialise the cell-cycle model at the start of a simulation.
     *
     * This method will be called precisely once per cell set up in the initial
     * cell population. It is not called on cell division; use ResetForDivision(),
     * CreateCellCycleModel() and InitialiseDaughterCell() for that.
     *
     * By the time this is called, a CellPopulation will have been set up, so the model
     * can know where its cell is located in space. If relevant to the simulation,
     * any singletons will also have been initialised.
     */
    void Initialise();

    /**
     * Reset cell-cycle model by calling AbstractOdeBasedCellCycleModelWithStoppingEvent::ResetForDivision()
     * and setting initial conditions for protein concentrations.
     */
    void ResetForDivision();

    /**
     * Overridden builder method to create new copies of
     * this cell-cycle model.
     *
     * @return new cell-cycle model
     */
    AbstractCellCycleModel* CreateCellCycleModel();

    /**
     * If the daughter cell type is stem, change it to transit.
     */
    void InitialiseDaughterCell();

    /**
     * Overridden GetAverageTransitCellCycleTime() method.
     * @return time
     */
    double GetAverageTransitCellCycleTime();

    /**
     * Overridden GetAverageStemCellCycleTime() method.
     * @return time
     */
    double GetAverageStemCellCycleTime();

    /**
     * Overridden CanCellTerminallyDifferentiate() method.
     * @return whether cell can terminally differentiate
     */
    bool CanCellTerminallyDifferentiate();

    /**
     * Overridden OutputCellCycleModelParameters() method.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    virtual void OutputCellCycleModelParameters(out_stream& rParamsFile);
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(TysonNovakCellCycleModel)
#include "CellCycleModelOdeSolverExportWrapper.hpp"
EXPORT_CELL_CYCLE_MODEL_ODE_SOLVER(TysonNovakCellCycleModel)

#endif /*TYSONNOVAKCELLCYCLEMODEL_HPP_*/


/********************************************************************************
 * 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


#pragma once

#include "../stateobject.hh"
#include "../../FieldMap/fieldmap.hh"
#include "../../Kick/kick.hh"
#include "../../Math/math.hh"
#include "../../OcclusionRay/occlusionray.hh"
#include "../../stats/average.hh"
#include "../../geometry/LineSegment/LineSegment2/linesegment2.hh"

#include <array>
#include <vector>
#include <Eigen/Geometry>

namespace bold
{
  /// Indicates which team owns a goal post.
  enum class GoalLabel
  {
    /// The ownership of the goal post could not be determined.
    Unknown = 0,
    /// Our goal. Defend it!
    Ours = 1,
    /// Their goal. Attack it!
    Theirs = 2
  };

  std::string getGoalLabelName(GoalLabel label);

  //////////////////////////////////////////////////////////////////////////////

  /** Models the estimated position and ownership of a pair of observed goal posts. */
  class GoalEstimate
  {
  public:
    GoalEstimate(Eigen::Vector2d const& post1Pos, Eigen::Vector2d const& post2Pos, GoalLabel label)
    : d_post1Pos(post1Pos),
      d_post2Pos(post2Pos),
      d_label(label)
    {}

    Eigen::Vector2d getPost1Pos() const { return d_post1Pos; }
    Eigen::Vector2d getPost2Pos() const { return d_post2Pos; }

    GoalLabel getLabel() const { return d_label; }

    Eigen::Vector2d getMidpoint(double ratio = 0.5) const { return Math::lerp(ratio, d_post1Pos, d_post2Pos); }

    bool isTowards(double endBallAngle) const;

    GoalEstimate estimateOppositeGoal(GoalLabel label) const;

    LineSegment2d lineSegment2d() const { return LineSegment2d(d_post1Pos, d_post2Pos); }

  private:
    Eigen::Vector2d d_post1Pos;
    Eigen::Vector2d d_post2Pos;
    GoalLabel d_label;
  };

  //////////////////////////////////////////////////////////////////////////////

  class KickResult
  {
  public:
    KickResult(std::shared_ptr<Kick const> kick, Eigen::Vector2d const& endPos, bool isOnTarget)
    : d_kick(kick),
      d_endPos(endPos),
      d_isOnTarget(isOnTarget)
    {};

    std::string getId() const { return d_kick->getId(); }
    std::shared_ptr<Kick const> getKick() const { return d_kick; }
    Eigen::Vector2d getEndPos() const { return d_endPos; }
    bool isOnTarget() const { return d_isOnTarget; }

  private:
    std::shared_ptr<Kick const> d_kick;
    Eigen::Vector2d d_endPos;
    bool d_isOnTarget;
  };

  //////////////////////////////////////////////////////////////////////////////

  // TODO rename as OpenFieldMap or similar
  class RadialOcclusionMap
  {
  public:
    void add(OcclusionRay<double> const& ray);

    void reset();

    double getOcclusionDistance(double angle) const;
    double getFieldDistance(double angle) const;

    void writeJson(rapidjson::Writer<rapidjson::StringBuffer>& writer) const { writeJsonInternal(writer); }
    void writeJson(rapidjson::Writer<WebSocketBuffer>& writer) const { writeJsonInternal(writer); }
    void writeJson(rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer) const { writeJsonInternal(writer); }

  private:
    template<typename TBuffer>
    void writeJsonInternal(rapidjson::Writer<TBuffer> &writer) const;

    static constexpr int NumberOfBuckets = 144;

    static uint wedgeIndexForAngle(double angle);
    static double angleForWedgeIndex(uint index);

    std::array<Average<double>,NumberOfBuckets> d_nearWedges;
    std::array<Average<double>,NumberOfBuckets> d_farWedges;
  };

  template<typename TBuffer>
  inline void RadialOcclusionMap::writeJsonInternal(rapidjson::Writer<TBuffer> &writer) const
  {
    auto writePoint = [&](uint index, const char* name, std::array<Average<double>,NumberOfBuckets> const& wedges)
    {
      writer.String(name);
      writer.StartObject();
      {
        writer.String("dist");
        writer.Double(wedges.at(index).getAverage(), "%.3f");
        writer.String("count");
        writer.Uint(wedges.at(index).getCount());
      }
      writer.EndObject();
    };

    writer.StartObject();
    {
      writer.String("divisions");
      writer.Uint(NumberOfBuckets);
      writer.String("slices");
      writer.StartArray();
      {
        for (uint index = 0; index < NumberOfBuckets; index++)
        {
          bool hasNear = d_nearWedges[index].getCount() != 0;
          bool hasFar = d_farWedges[index].getCount() != 0;

          if (!hasNear && !hasFar)
            continue;

          writer.StartObject();
          {
            writer.String("angle");
            writer.Double(angleForWedgeIndex(index), "%.3f");
            if (hasNear) writePoint(index, "near", d_nearWedges);
            if (hasFar)  writePoint(index, "far", d_farWedges);
          }
          writer.EndObject();
        }
      }
      writer.EndArray();
    }
    writer.EndObject();
  }

  //////////////////////////////////////////////////////////////////////////////

  class StationaryMapState : public StateObject
  {
  public:
    StationaryMapState(std::vector<Average<Eigen::Vector2d>> ballEstimates,
                       std::vector<Average<Eigen::Vector2d>> goalPostEstimates,
                       std::vector<Average<Eigen::Vector2d>> teammateEstimates,
                       RadialOcclusionMap occlusionMap);

    void writeJson(rapidjson::Writer<rapidjson::StringBuffer>& writer) const override { writeJsonInternal(writer); }
    void writeJson(rapidjson::Writer<WebSocketBuffer>& writer) const override { writeJsonInternal(writer); }
    void writeJson(rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer) const override { writeJsonInternal(writer); }

    std::vector<Average<Eigen::Vector2d>> const& getBallEstimates() const { return d_ballEstimates; }
    std::vector<Average<Eigen::Vector2d>> const& getTeammateEstimates() const { return d_keeperEstimates; }
    std::vector<Average<Eigen::Vector2d>> const& getGoalPostEstimates() const { return d_goalPostEstimates; }
    std::vector<GoalEstimate> const& getGoalEstimates() const { return d_goalEstimates; }

    bool hasEnoughBallObservations() const { return existsWithSamples(d_ballEstimates, BallSamplesNeeded); }
    bool hasEnoughGoalPostObservations() const { return d_goalEstimates.size() != 0; }
    bool hasEnoughBallAndGoalPostObservations() const { return hasEnoughBallObservations() && hasEnoughGoalPostObservations(); }

    /// Returns the number of goal post estimates that have enough observations
    long getSatisfactoryGoalPostCount() const;

    bool hasBallWithinDistance(double distance) const;

    bool needMoreSightingsOfGoalPostAt(Eigen::Vector2d goalPos) const;
    bool needMoreSightingsOfBallAt(Eigen::Vector2d ballPos) const;

    bool canKick() const { return d_selectedKick != nullptr; }
    std::shared_ptr<Kick const> getSelectedKick() const { return d_selectedKick; }

    double getTurnAngleRads() const { return d_turnAngleRads; };
    Eigen::Vector2d getTurnBallPos() const { return d_turnBallPos; };

    std::shared_ptr<Kick const> getTurnForKick() const { return d_turnForKick; }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    static constexpr double BallMergeDistance = 0.3; // TODO magic number!!
    static constexpr double GoalPostMergeDistance = 0.5; // TODO magic number!!
    static constexpr double TeammateMergeDistance = 0.5; // TODO magic number!!
    static constexpr int GoalSamplesNeeded = 10; // TODO magic number!!
    static constexpr int BallSamplesNeeded = 20; // TODO magic number!!
    static constexpr int KeeperSamplesNeeded = 5; // TODO magic number!!

    /// Extracts goal pairs from individual post observations.
    static std::vector<std::pair<Average<Eigen::Vector2d>,Average<Eigen::Vector2d>>> pairGoalPosts(std::vector<Average<Eigen::Vector2d>> goalPostEstimates);

    static GoalLabel labelGoalByKeeperBallDistance(
      Average<Eigen::Vector2d> const& post1Pos,
      Average<Eigen::Vector2d> const& post2Pos,
      FieldSide ballSideEstimate);

    static GoalLabel labelGoalByKeeperBallPosition(
      Average<Eigen::Vector2d> const& post1Pos,
      Average<Eigen::Vector2d> const& post2Pos,
      Eigen::Vector2d const& keeperBallPos,
      Eigen::Vector2d const& agentBallPos);

    static GoalLabel labelGoalByKeeperObservations(
      Average<Eigen::Vector2d> const& post1Pos,
      Average<Eigen::Vector2d> const& post2Pos,
      std::vector<Average<Eigen::Vector2d>> keeperEstimates);

    /** Estimate the position of a point in the agent frame if the specified posts have the specified label. */
    static Eigen::Vector2d estimateWorldPositionForPoint(
      Eigen::Vector2d const& post1,
      Eigen::Vector2d const& post2,
      Eigen::Vector2d const& pointAgent,
      GoalLabel label);

  private:
    template<typename TBuffer>
    void writeJsonInternal(rapidjson::Writer<TBuffer> &writer) const;

    template<typename T>
    static bool compareAverages(Average<T> const& a, Average<T> const& b)
    {
      return a.getCount() > b.getCount();
    }

    /// Attempts to label a pair of goal posts as being either ours, theirs or unknown.
    /// Uses techniques available in other labelGoalBy* functions.
    GoalLabel labelGoal(
      Average<Eigen::Vector2d> const& post1Pos,
      Average<Eigen::Vector2d> const& post2Pos,
      Maybe<Eigen::Vector2d> const& agentBallPos);

    /// Selects a kick (if there is one) which may be made immediately with a suitably positive outcome.
    /// If successful, canKick() will return true and getSelectedKick() returns the kick.
    void selectImmediateKick();

    /// Selects a turn angle, turn direction and kick (if there is one) which produce a suitably positive outcome.
    /// If successful, getTurnAngleRads() will return non-zero, getTurnBallPos() gives the position the ball
    /// should be kept in, and getTurnForKick() returns the kick.
    void calculateTurnAndKick();

    template<typename T>
    inline static bool existsWithSamples(std::vector<T> const& estimates, int sampleThreshold);
    template<typename T>
    inline static uint countWithSamples(std::vector<T> const& estimates, int sampleThreshold);

    std::vector<Average<Eigen::Vector2d>> d_ballEstimates;
    std::vector<Average<Eigen::Vector2d>> d_goalPostEstimates;
    std::vector<Average<Eigen::Vector2d>> d_keeperEstimates;
    RadialOcclusionMap d_occlusionMap;

    std::vector<GoalEstimate> d_goalEstimates;
    std::vector<KickResult> d_possibleKicks;
    std::shared_ptr<Kick const> d_selectedKick;
    double d_turnAngleRads;
    Eigen::Vector2d d_turnBallPos;
    std::shared_ptr<Kick const> d_turnForKick;
  };

  template<typename TBuffer>
  inline void StationaryMapState::writeJsonInternal(rapidjson::Writer<TBuffer> &writer) const
  {
    writer.StartObject();
    {
      writer.String("balls");
      writer.StartArray();
      for (auto const& estimate : d_ballEstimates)
      {
        writer.StartObject();
        {
          writer.String("pos");
          writer.StartArray();
          writer.Double(estimate.getAverage().x(), "%.3f");
          writer.Double(estimate.getAverage().y(), "%.3f");
          writer.EndArray();
          writer.String("count");
          writer.Int(estimate.getCount());
        }
        writer.EndObject();
      }
      writer.EndArray();

      writer.String("goalPosts");
      writer.StartArray();
      for (auto const& estimate : d_goalPostEstimates)
      {
        writer.StartObject();
        {
          writer.String("pos");
          writer.StartArray();
          writer.Double(estimate.getAverage().x(), "%.3f");
          writer.Double(estimate.getAverage().y(), "%.3f");
          writer.EndArray();
          writer.String("count");
          writer.Uint(estimate.getCount());
        }
        writer.EndObject();
      }
      writer.EndArray();

      writer.String("goals");
      writer.StartArray();
      for (auto const& estimate : d_goalEstimates)
      {
        writer.StartObject();
        {
          writer.String("post1");
          writer.StartArray();
          writer.Double(estimate.getPost1Pos().x(), "%.3f");
          writer.Double(estimate.getPost1Pos().y(), "%.3f");
          writer.EndArray();
          writer.String("post2");
          writer.StartArray();
          writer.Double(estimate.getPost2Pos().x(), "%.3f");
          writer.Double(estimate.getPost2Pos().y(), "%.3f");
          writer.EndArray();
          writer.String("label");
          writer.Uint(static_cast<uint>(estimate.getLabel()));
        }
        writer.EndObject();
      }
      writer.EndArray();

      writer.String("keepers");
      writer.StartArray();
      for (auto const& estimate : d_keeperEstimates)
      {
        writer.StartObject();
        {
          writer.String("pos");
          writer.StartArray();
          writer.Double(estimate.getAverage().x(), "%.3f");
          writer.Double(estimate.getAverage().y(), "%.3f");
          writer.EndArray();
          writer.String("count");
          writer.Int(estimate.getCount());
        }
        writer.EndObject();
      }
      writer.EndArray();

      writer.String("kicks");
      writer.StartArray();
      {
        for (auto const& kick : d_possibleKicks)
        {
          writer.StartObject();
          {
            writer.String("id");
            writer.String(kick.getId().c_str());
            Eigen::Vector2d const& endPos = kick.getEndPos();
            writer.String("endPos");
            writer.StartArray();
            writer.Double(endPos.x(), "%.3f");
            writer.Double(endPos.y(), "%.3f");
            writer.EndArray();
            writer.String("onTarget");
            writer.Bool(kick.isOnTarget());
            writer.String("selected");
            writer.Bool(kick.getKick() == d_selectedKick);
          }
          writer.EndObject();
        }
      }
      writer.EndArray();

      writer.String("openField");
      d_occlusionMap.writeJson(writer);

      writer.String("turnAngle");
      writer.Double(d_turnAngleRads, "%.3f");
      writer.String("turnBallPos");
      writer.StartArray();
      writer.Double(d_turnBallPos.x(), "%.3f");
      writer.Double(d_turnBallPos.y(), "%.3f");
      writer.EndArray();
    }
    writer.EndObject();
  }

  template<typename T>
  bool StationaryMapState::existsWithSamples(std::vector<T> const& estimates, int sampleThreshold)
  {
    return estimates.size() && estimates[0].getCount() >= sampleThreshold;
  }

  template<typename T>
  uint StationaryMapState::countWithSamples(std::vector<T> const& estimates, int sampleThreshold)
  {
    uint count = 0;
    for (auto const& estimate : estimates)
    {
      if (estimate.getCount() >= sampleThreshold)
        count++;
      else
        break; // list is ordered
    }
    return count;
  }
}


#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;
}