/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 * Nicolas Mansard
 * Joseph Mirabel
 *
 * CNRS/AIST
 *
 */

#include <boost/function.hpp>

#include <sot/core/binary-op.hh>
#include <sot/core/unary-op.hh>
#include <sot/core/variadic-op.hh>

#include <sot/core/matrix-geometry.hh>

#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>

#include <boost/numeric/conversion/cast.hpp>
#include <deque>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>

#include "../tools/type-name-helper.hh"

namespace dg = ::dynamicgraph;

/* ---------------------------------------------------------------------------*/
/* ------- GENERIC HELPERS -------------------------------------------------- */
/* ---------------------------------------------------------------------------*/

#define ADD_COMMAND(name, def) commandMap.insert(std::make_pair(name, def))

namespace dynamicgraph {
namespace sot {
template <typename TypeIn, typename TypeOut> struct UnaryOpHeader {
  typedef TypeIn Tin;
  typedef TypeOut Tout;
  static inline std::string nameTypeIn(void) {
    return TypeNameHelper<Tin>::typeName();
  }
  static inline std::string nameTypeOut(void) {
    return TypeNameHelper<Tout>::typeName();
  }
  inline void addSpecificCommands(Entity &, Entity::CommandMap_t &) {}
  inline std::string getDocString() const {
    return std::string("Undocumented unary operator\n"
                       "  - input  ") +
           nameTypeIn() +
           std::string("\n"
                       "  - output ") +
           nameTypeOut() + std::string("\n");
  }
};

/* ---------------------------------------------------------------------- */
/* --- ALGEBRA SELECTORS ------------------------------------------------ */
/* ---------------------------------------------------------------------- */
struct VectorSelecter : public UnaryOpHeader<dg::Vector, dg::Vector> {
  inline void operator()(const Tin &m, Vector &res) const {
    res.resize(size);
    Vector::Index r = 0;
    for (std::size_t i = 0; i < idxs.size(); ++i) {
      const Vector::Index &R = idxs[i].first;
      const Vector::Index &nr = idxs[i].second;
      assert((nr >= 0) && (R + nr <= m.size()));
      res.segment(r, nr) = m.segment(R, nr);
      r += nr;
    }
    assert(r == size);
  }

  typedef std::pair<Vector::Index, Vector::Index> segment_t;
  typedef std::vector<segment_t> segments_t;
  segments_t idxs;
  Vector::Index size;

  inline void setBounds(const int &m, const int &M) {
    idxs = segments_t(1, segment_t(m, M - m));
    size = M - m;
  }
  inline void addBounds(const int &m, const int &M) {
    idxs.push_back(segment_t(m, M - m));
    size += M - m;
  }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    boost::function<void(const int &, const int &)> setBound =
        boost::bind(&VectorSelecter::setBounds, this, _1, _2);
    doc = docCommandVoid2("Set the bound of the selection [m,M[.", "int (min)",
                          "int (max)");
    ADD_COMMAND("selec", makeCommandVoid2(ent, setBound, doc));
    boost::function<void(const int &, const int &)> addBound =
        boost::bind(&VectorSelecter::addBounds, this, _1, _2);
    doc = docCommandVoid2("Add a segment to be selected [m,M[.", "int (min)",
                          "int (max)");
    ADD_COMMAND("addSelec", makeCommandVoid2(ent, addBound, doc));
  }
  VectorSelecter() : size(0) {}
};

/* ---------------------------------------------------------------------- */
struct VectorComponent : public UnaryOpHeader<dg::Vector, double> {
  inline void operator()(const Tin &m, double &res) const {
    assert(index < m.size());
    res = m(index);
  }

  int index;
  inline void setIndex(const int &m) { index = m; }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    std::string doc;

    boost::function<void(const int &)> callback =
        boost::bind(&VectorComponent::setIndex, this, _1);
    doc = command::docCommandVoid1("Set the index of the component.",
                                   "int (index)");
    ADD_COMMAND("setIndex", command::makeCommandVoid1(ent, callback, doc));
  }
  inline std::string getDocString() const {
    std::string docString("Select a component of a vector\n"
                          "  - input  vector\n"
                          "  - output double");
    return docString;
  }
};

/* ---------------------------------------------------------------------- */
struct MatrixSelector : public UnaryOpHeader<dg::Matrix, dg::Matrix> {
  inline void operator()(const Matrix &m, Matrix &res) const {
    assert((imin <= imax) && (imax <= m.rows()));
    assert((jmin <= jmax) && (jmax <= m.cols()));
    res.resize(imax - imin, jmax - jmin);
    for (int i = imin; i < imax; ++i)
      for (int j = jmin; j < jmax; ++j)
        res(i - imin, j - jmin) = m(i, j);
  }

public:
  int imin, imax;
  int jmin, jmax;

  inline void setBoundsRow(const int &m, const int &M) {
    imin = m;
    imax = M;
  }
  inline void setBoundsCol(const int &m, const int &M) {
    jmin = m;
    jmax = M;
  }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    boost::function<void(const int &, const int &)> setBoundsRow =
        boost::bind(&MatrixSelector::setBoundsRow, this, _1, _2);
    boost::function<void(const int &, const int &)> setBoundsCol =
        boost::bind(&MatrixSelector::setBoundsCol, this, _1, _2);

    doc = docCommandVoid2("Set the bound on rows.", "int (min)", "int (max)");
    ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc));

    doc = docCommandVoid2("Set the bound on cols [m,M[.", "int (min)",
                          "int (max)");
    ADD_COMMAND("selecCols", makeCommandVoid2(ent, setBoundsCol, doc));
  }
};

/* ---------------------------------------------------------------------- */
struct MatrixColumnSelector : public UnaryOpHeader<dg::Matrix, dg::Vector> {
public:
  inline void operator()(const Tin &m, Tout &res) const {
    assert((imin <= imax) && (imax <= m.rows()));
    assert(jcol < m.cols());

    res.resize(imax - imin);
    for (int i = imin; i < imax; ++i)
      res(i - imin) = m(i, jcol);
  }

  int imin, imax;
  int jcol;
  inline void selectCol(const int &m) { jcol = m; }
  inline void setBoundsRow(const int &m, const int &M) {
    imin = m;
    imax = M;
  }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    boost::function<void(const int &, const int &)> setBoundsRow =
        boost::bind(&MatrixColumnSelector::setBoundsRow, this, _1, _2);
    boost::function<void(const int &)> selectCol =
        boost::bind(&MatrixColumnSelector::selectCol, this, _1);

    doc = docCommandVoid2("Set the bound on rows.", "int (min)", "int (max)");
    ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc));

    doc = docCommandVoid1("Select the col to copy.", "int (col index)");
    ADD_COMMAND("selecCols", makeCommandVoid1(ent, selectCol, doc));
  }
};

/* ---------------------------------------------------------------------- */
struct MatrixTranspose : public UnaryOpHeader<dg::Matrix, dg::Matrix> {
  inline void operator()(const Tin &m, Tout &res) const { res = m.transpose(); }
};

/* ---------------------------------------------------------------------- */
struct Diagonalizer : public UnaryOpHeader<Vector, Matrix> {
  inline void operator()(const dg::Vector &r, dg::Matrix &res) {
    res = r.asDiagonal();
  }

public:
  Diagonalizer(void) : nbr(0), nbc(0) {}
  unsigned int nbr, nbc;
  inline void resize(const int &r, const int &c) {
    nbr = r;
    nbc = c;
  }
  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    boost::function<void(const int &, const int &)> resize =
        boost::bind(&Diagonalizer::resize, this, _1, _2);

    doc = docCommandVoid2("Set output size.", "int (row)", "int (col)");
    ADD_COMMAND("resize", makeCommandVoid2(ent, resize, doc));
  }
};

/* ---------------------------------------------------------------------- */
/* --- INVERSION -------------------------------------------------------- */
/* ---------------------------------------------------------------------- */

template <typename matrixgen>
struct Inverser : public UnaryOpHeader<matrixgen, matrixgen> {
  typedef typename UnaryOpHeader<matrixgen, matrixgen>::Tin Tin;
  typedef typename UnaryOpHeader<matrixgen, matrixgen>::Tout Tout;
  inline void operator()(const Tin &m, Tout &res) const { res = m.inverse(); }
};

struct Normalize : public UnaryOpHeader<dg::Vector, double> {
  inline void operator()(const dg::Vector &m, double &res) const {
    res = m.norm();
  }

  inline std::string getDocString() const {
    std::string docString("Computes the norm of a vector\n"
                          "  - input  vector\n"
                          "  - output double");
    return docString;
  }
};

struct InverserRotation : public UnaryOpHeader<MatrixRotation, MatrixRotation> {
  inline void operator()(const Tin &m, Tout &res) const { res = m.transpose(); }
};

struct InverserQuaternion
    : public UnaryOpHeader<VectorQuaternion, VectorQuaternion> {
  inline void operator()(const Tin &m, Tout &res) const { res = m.conjugate(); }
};

/* ----------------------------------------------------------------------- */
/* --- SE3/SO3 conversions ----------------------------------------------- */
/* ----------------------------------------------------------------------- */

struct MatrixHomoToPoseUTheta
    : public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
  inline void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
    res.resize(6);
    VectorUTheta r(M.linear());
    res.head<3>() = M.translation();
    res.tail<3>() = r.angle() * r.axis();
  }
};

struct SkewSymToVector : public UnaryOpHeader<Matrix, Vector> {
  inline void operator()(const Matrix &M, Vector &res) {
    res.resize(3);
    res(0) = M(7);
    res(1) = M(2);
    res(2) = M(3);
  }
};

struct PoseUThetaToMatrixHomo
    : public UnaryOpHeader<Vector, MatrixHomogeneous> {
  inline void operator()(const dg::Vector &v, MatrixHomogeneous &res) {
    assert(v.size() >= 6);
    res.translation() = v.head<3>();
    double theta = v.tail<3>().norm();
    if (theta > 0)
      res.linear() = Eigen::AngleAxisd(theta, v.tail<3>() / theta).matrix();
    else
      res.linear().setIdentity();
  }
};

struct SE3VectorToMatrixHomo
    : public UnaryOpHeader<dg::Vector, MatrixHomogeneous> {
  void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
    Mres.translation() = vect.head<3>();
    Mres.linear().row(0) = vect.segment(3, 3);
    Mres.linear().row(1) = vect.segment(6, 3);
    Mres.linear().row(2) = vect.segment(9, 3);
  }
};

struct MatrixHomoToSE3Vector
    : public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
  void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
    res.resize(12);
    res.head<3>() = M.translation();
    res.segment(3, 3) = M.linear().row(0);
    res.segment(6, 3) = M.linear().row(1);
    res.segment(9, 3) = M.linear().row(2);
  }
};

struct PoseQuaternionToMatrixHomo
    : public UnaryOpHeader<Vector, MatrixHomogeneous> {
  void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
    Mres.translation() = vect.head<3>();
    Mres.linear() = VectorQuaternion(vect.tail<4>()).toRotationMatrix();
  }
};

struct MatrixHomoToPoseQuaternion
    : public UnaryOpHeader<MatrixHomogeneous, Vector> {
  inline void operator()(const MatrixHomogeneous &M, Vector &res) {
    res.resize(7);
    res.head<3>() = M.translation();
    Eigen::Map<VectorQuaternion> q(res.tail<4>().data());
    q = M.linear();
  }
};

struct MatrixHomoToPoseRollPitchYaw
    : public UnaryOpHeader<MatrixHomogeneous, Vector> {
  inline void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
    VectorRollPitchYaw r = (M.linear().eulerAngles(2, 1, 0)).reverse();
    dg::Vector t(3);
    t = M.translation();
    res.resize(6);
    for (unsigned int i = 0; i < 3; ++i)
      res(i) = t(i);
    for (unsigned int i = 0; i < 3; ++i)
      res(i + 3) = r(i);
  }
};

struct PoseRollPitchYawToMatrixHomo
    : public UnaryOpHeader<Vector, MatrixHomogeneous> {
  inline void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {

    VectorRollPitchYaw r;
    for (unsigned int i = 0; i < 3; ++i)
      r(i) = vect(i + 3);
    MatrixRotation R = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
                        Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
                        Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
                           .toRotationMatrix();

    dg::Vector t(3);
    for (unsigned int i = 0; i < 3; ++i)
      t(i) = vect(i);

    // buildFrom(R,t);
    Mres = Eigen::Translation3d(t) * R;
  }
};

struct PoseRollPitchYawToPoseUTheta : public UnaryOpHeader<Vector, Vector> {
  inline void operator()(const dg::Vector &vect, dg::Vector &vectres) {
    VectorRollPitchYaw r;
    for (unsigned int i = 0; i < 3; ++i)
      r(i) = vect(i + 3);
    MatrixRotation R = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
                        Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
                        Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
                           .toRotationMatrix();

    VectorUTheta rrot(R);

    vectres.resize(6);
    for (unsigned int i = 0; i < 3; ++i) {
      vectres(i) = vect(i);
      vectres(i + 3) = rrot.angle() * rrot.axis()(i);
    }
  }
};

struct HomoToMatrix : public UnaryOpHeader<MatrixHomogeneous, Matrix> {
  inline void operator()(const MatrixHomogeneous &M, dg::Matrix &res) {
    res = M.matrix();
  }
};

struct MatrixToHomo : public UnaryOpHeader<Matrix, MatrixHomogeneous> {
  inline void operator()(const Eigen::Matrix<double, 4, 4> &M,
                         MatrixHomogeneous &res) {
    res = M;
  }
};

struct HomoToTwist : public UnaryOpHeader<MatrixHomogeneous, MatrixTwist> {
  inline void operator()(const MatrixHomogeneous &M, MatrixTwist &res) {
    Eigen::Vector3d _t = M.translation();
    MatrixRotation R(M.linear());
    Eigen::Matrix3d Tx;
    Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;

    Eigen::Matrix3d sk;
    sk = Tx * R;
    res.block<3, 3>(0, 0) = R;
    res.block<3, 3>(0, 3) = sk;
    res.block<3, 3>(3, 0) = Eigen::Matrix3d::Zero();
    res.block<3, 3>(3, 3) = R;
  }
};

struct HomoToRotation
    : public UnaryOpHeader<MatrixHomogeneous, MatrixRotation> {
  inline void operator()(const MatrixHomogeneous &M, MatrixRotation &res) {
    res = M.linear();
  }
};

struct MatrixHomoToPose : public UnaryOpHeader<MatrixHomogeneous, Vector> {
  inline void operator()(const MatrixHomogeneous &M, Vector &res) {
    res.resize(3);
    res = M.translation();
  }
};

struct RPYToMatrix : public UnaryOpHeader<VectorRollPitchYaw, MatrixRotation> {
  inline void operator()(const VectorRollPitchYaw &r, MatrixRotation &res) {
    res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
           Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
           Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
              .toRotationMatrix();
  }
};

struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> {
  inline void operator()(const MatrixRotation &r, VectorRollPitchYaw &res) {
    res = (r.eulerAngles(2, 1, 0)).reverse();
  }
};

struct RPYToQuaternion
    : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaternion> {
  inline void operator()(const VectorRollPitchYaw &r, VectorQuaternion &res) {
    res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
           Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
           Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
              .toRotationMatrix();
  }
};

struct QuaternionToRPY
    : public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> {
  inline void operator()(const VectorQuaternion &r, VectorRollPitchYaw &res) {
    res = (r.toRotationMatrix().eulerAngles(2, 1, 0)).reverse();
  }
};

struct QuaternionToMatrix
    : public UnaryOpHeader<VectorQuaternion, MatrixRotation> {
  inline void operator()(const VectorQuaternion &r, MatrixRotation &res) {
    res = r.toRotationMatrix();
  }
};

struct MatrixToQuaternion
    : public UnaryOpHeader<MatrixRotation, VectorQuaternion> {
  inline void operator()(const MatrixRotation &r, VectorQuaternion &res) {
    res = r;
  }
};

struct MatrixToUTheta : public UnaryOpHeader<MatrixRotation, VectorUTheta> {
  inline void operator()(const MatrixRotation &r, VectorUTheta &res) {
    res = r;
  }
};

struct UThetaToQuaternion
    : public UnaryOpHeader<VectorUTheta, VectorQuaternion> {
  inline void operator()(const VectorUTheta &r, VectorQuaternion &res) {
    res = r;
  }
};

template <typename TypeIn1, typename TypeIn2, typename TypeOut>
struct BinaryOpHeader {
  typedef TypeIn1 Tin1;
  typedef TypeIn2 Tin2;
  typedef TypeOut Tout;
  inline static std::string nameTypeIn1(void) {
    return TypeNameHelper<Tin1>::typeName();
  }
  inline static std::string nameTypeIn2(void) {
    return TypeNameHelper<Tin2>::typeName();
  }
  inline static std::string nameTypeOut(void) {
    return TypeNameHelper<Tout>::typeName();
  }
  inline void addSpecificCommands(Entity &, Entity::CommandMap_t &) {}
  inline std::string getDocString() const {
    return std::string("Undocumented binary operator\n"
                       "  - input  ") +
           nameTypeIn1() +
           std::string("\n"
                       "  -        ") +
           nameTypeIn2() +
           std::string("\n"
                       "  - output ") +
           nameTypeOut() + std::string("\n");
  }
};

} /* namespace sot */
} /* namespace dynamicgraph */

/* ---------------------------------------------------------------------------*/
/* ---------------------------------------------------------------------------*/
/* ---------------------------------------------------------------------------*/

namespace dynamicgraph {
namespace sot {

/* --- MULTIPLICATION --------------------------------------------------- */

template <typename F, typename E>
struct Multiplier_FxE__E : public BinaryOpHeader<F, E, E> {
  inline void operator()(const F &f, const E &e, E &res) const { res = f * e; }
};

template <>
inline void
Multiplier_FxE__E<dynamicgraph::sot::MatrixHomogeneous, dynamicgraph::Vector>::
operator()(const dynamicgraph::sot::MatrixHomogeneous &f,
           const dynamicgraph::Vector &e, dynamicgraph::Vector &res) const {
  res = f.matrix() * e;
}

template <>
inline void Multiplier_FxE__E<double, dynamicgraph::Vector>::
operator()(const double &x, const dynamicgraph::Vector &v,
           dynamicgraph::Vector &res) const {
  res = v;
  res *= x;
}

typedef Multiplier_FxE__E<double, dynamicgraph::Vector>
    Multiplier_double_vector;
typedef Multiplier_FxE__E<dynamicgraph::Matrix, dynamicgraph::Vector>
    Multiplier_matrix_vector;
typedef Multiplier_FxE__E<MatrixHomogeneous, dynamicgraph::Vector>
    Multiplier_matrixHomo_vector;
typedef Multiplier_FxE__E<MatrixTwist, dynamicgraph::Vector>
    Multiplier_matrixTwist_vector;

/* --- SUBSTRACTION ----------------------------------------------------- */
template <typename T> struct Substraction : public BinaryOpHeader<T, T, T> {
  inline void operator()(const T &v1, const T &v2, T &r) const {
    r = v1;
    r -= v2;
  }
};

/* --- STACK ------------------------------------------------------------ */
struct VectorStack
    : public BinaryOpHeader<dynamicgraph::Vector, dynamicgraph::Vector,
                            dynamicgraph::Vector> {
public:
  int v1min, v1max;
  int v2min, v2max;
  inline void operator()(const dynamicgraph::Vector &v1,
                         const dynamicgraph::Vector &v2,
                         dynamicgraph::Vector &res) const {
    assert((v1max >= v1min) && (v1.size() >= v1max));
    assert((v2max >= v2min) && (v2.size() >= v2max));

    const int v1size = v1max - v1min, v2size = v2max - v2min;
    res.resize(v1size + v2size);
    for (int i = 0; i < v1size; ++i) {
      res(i) = v1(i + v1min);
    }
    for (int i = 0; i < v2size; ++i) {
      res(v1size + i) = v2(i + v2min);
    }
  }

  inline void selec1(const int &m, const int M) {
    v1min = m;
    v1max = M;
  }
  inline void selec2(const int &m, const int M) {
    v2min = m;
    v2max = M;
  }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    boost::function<void(const int &, const int &)> selec1 =
        boost::bind(&VectorStack::selec1, this, _1, _2);
    boost::function<void(const int &, const int &)> selec2 =
        boost::bind(&VectorStack::selec2, this, _1, _2);

    ADD_COMMAND(
        "selec1",
        makeCommandVoid2(ent, selec1,
                         docCommandVoid2("set the min and max of selection.",
                                         "int (imin)", "int (imax)")));
    ADD_COMMAND(
        "selec2",
        makeCommandVoid2(ent, selec2,
                         docCommandVoid2("set the min and max of selection.",
                                         "int (imin)", "int (imax)")));
  }
};

/* ---------------------------------------------------------------------- */

struct Composer
    : public BinaryOpHeader<dynamicgraph::Matrix, dynamicgraph::Vector,
                            MatrixHomogeneous> {
  inline void operator()(const dynamicgraph::Matrix &R,
                         const dynamicgraph::Vector &t,
                         MatrixHomogeneous &H) const {
    H.linear() = R;
    H.translation() = t;
  }
};

/* --- CONVOLUTION PRODUCT ---------------------------------------------- */
struct ConvolutionTemporal
    : public BinaryOpHeader<dynamicgraph::Vector, dynamicgraph::Matrix,
                            dynamicgraph::Vector> {
  typedef std::deque<dynamicgraph::Vector> MemoryType;
  MemoryType memory;

  inline void convolution(const MemoryType &f1, const dynamicgraph::Matrix &f2,
                          dynamicgraph::Vector &res) {
    const Vector::Index nconv = (Vector::Index)f1.size(), nsig = f2.rows();
    sotDEBUG(15) << "Size: " << nconv << "x" << nsig << std::endl;
    if (nconv > f2.cols())
      return; // TODO: error, this should not happen

    res.resize(nsig);
    res.fill(0);
    unsigned int j = 0;
    for (MemoryType::const_iterator iter = f1.begin(); iter != f1.end();
         iter++) {
      const dynamicgraph::Vector &s_tau = *iter;
      sotDEBUG(45) << "Sig" << j << ": " << s_tau;
      if (s_tau.size() != nsig)
        return; // TODO: error throw;
      for (int i = 0; i < nsig; ++i) {
        res(i) += f2(i, j) * s_tau(i);
      }
      j++;
    }
  }
  inline void operator()(const dynamicgraph::Vector &v1,
                         const dynamicgraph::Matrix &m2,
                         dynamicgraph::Vector &res) {
    memory.push_front(v1);
    while ((Vector::Index)memory.size() > m2.cols())
      memory.pop_back();
    convolution(memory, m2, res);
  }
};

/* --- BOOLEAN REDUCTION ------------------------------------------------ */

template <typename T> struct Comparison : public BinaryOpHeader<T, T, bool> {
  inline void operator()(const T &a, const T &b, bool &res) const {
    res = (a < b);
  }
  inline std::string getDocString() const {
    typedef BinaryOpHeader<T, T, bool> Base;
    return std::string("Comparison of inputs:\n"
                       "  - input  ") +
           Base::nameTypeIn1() +
           std::string("\n"
                       "  -        ") +
           Base::nameTypeIn2() +
           std::string("\n"
                       "  - output ") +
           Base::nameTypeOut() +
           std::string("\n"
                       "  sout = ( sin1 < sin2 )\n");
  }
};

template <typename T1, typename T2 = T1>
struct MatrixComparison : public BinaryOpHeader<T1, T2, bool> {
  // TODO T1 or T2 could be a scalar type.
  inline void operator()(const T1 &a, const T2 &b, bool &res) const {
    if (equal && any)
      res = (a.array() <= b.array()).any();
    else if (equal && !any)
      res = (a.array() <= b.array()).all();
    else if (!equal && any)
      res = (a.array() < b.array()).any();
    else if (!equal && !any)
      res = (a.array() < b.array()).all();
  }
  inline std::string getDocString() const {
    typedef BinaryOpHeader<T1, T2, bool> Base;
    return std::string("Comparison of inputs:\n"
                       "  - input  ") +
           Base::nameTypeIn1() +
           std::string("\n"
                       "  -        ") +
           Base::nameTypeIn2() +
           std::string("\n"
                       "  - output ") +
           Base::nameTypeOut() +
           std::string("\n"
                       "  sout = ( sin1 < sin2 ).op()\n") +
           std::string("\n"
                       "  where op is either any (default) or all. The "
                       "comparison can be made <=.\n");
  }
  MatrixComparison() : any(true), equal(false) {}
  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    ADD_COMMAND(
        "setTrueIfAny",
        makeDirectSetter(ent, &any, docDirectSetter("trueIfAny", "bool")));
    ADD_COMMAND(
        "getTrueIfAny",
        makeDirectGetter(ent, &any, docDirectGetter("trueIfAny", "bool")));
    ADD_COMMAND("setEqual", makeDirectSetter(ent, &equal,
                                             docDirectSetter("equal", "bool")));
    ADD_COMMAND("getEqual", makeDirectGetter(ent, &equal,
                                             docDirectGetter("equal", "bool")));
  }
  bool any, equal;
};

} /* namespace sot */
} /* namespace dynamicgraph */

namespace dynamicgraph {
namespace sot {

template <typename T> struct WeightedAdder : public BinaryOpHeader<T, T, T> {
public:
  double gain1, gain2;
  inline void operator()(const T &v1, const T &v2, T &res) const {
    res = v1;
    res *= gain1;
    res += gain2 * v2;
  }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    ADD_COMMAND(
        "setGain1",
        makeDirectSetter(ent, &gain1, docDirectSetter("gain1", "double")));
    ADD_COMMAND(
        "setGain2",
        makeDirectSetter(ent, &gain2, docDirectSetter("gain2", "double")));
    ADD_COMMAND(
        "getGain1",
        makeDirectGetter(ent, &gain1, docDirectGetter("gain1", "double")));
    ADD_COMMAND(
        "getGain2",
        makeDirectGetter(ent, &gain2, docDirectGetter("gain2", "double")));
  }

  inline std::string getDocString() const {
    return std::string("Weighted Combination of inputs : \n - gain{1|2} gain.");
  }
};

} // namespace sot
} // namespace dynamicgraph

namespace dynamicgraph {
namespace sot {
template <typename Tin, typename Tout, typename Time>
std::string VariadicAbstract<Tin, Tout, Time>::getTypeInName(void) {
  return TypeNameHelper<Tin>::typeName();
}
template <typename Tin, typename Tout, typename Time>
std::string VariadicAbstract<Tin, Tout, Time>::getTypeOutName(void) {
  return TypeNameHelper<Tout>::typeName();
}

template <typename TypeIn, typename TypeOut> struct VariadicOpHeader {
  typedef TypeIn Tin;
  typedef TypeOut Tout;
  inline static std::string nameTypeIn(void) {
    return TypeNameHelper<Tin>::typeName();
  }
  inline static std::string nameTypeOut(void) {
    return TypeNameHelper<Tout>::typeName();
  }
  template <typename Op>
  inline void initialize(VariadicOp<Op> *, Entity::CommandMap_t &) {}
  inline void updateSignalNumber(const int &) {}
  inline std::string getDocString() const {
    return std::string("Undocumented variadic operator\n"
                       "  - input  " +
                       nameTypeIn() +
                       "\n"
                       "  - output " +
                       nameTypeOut() + "\n");
  }
};

/* --- VectorMix ------------------------------------------------------------ */
struct VectorMix : public VariadicOpHeader<Vector, Vector> {
public:
  typedef VariadicOp<VectorMix> Base;
  struct segment_t {
    Vector::Index index, size, input;
    std::size_t sigIdx;
    segment_t(Vector::Index i, Vector::Index s, std::size_t sig)
        : index(i), size(s), sigIdx(sig) {}
  };
  typedef std::vector<segment_t> segments_t;
  Base *entity;
  segments_t idxs;
  inline void operator()(const std::vector<const Vector *> &vs,
                         Vector &res) const {
    res = *vs[0];
    for (std::size_t i = 0; i < idxs.size(); ++i) {
      const segment_t &s = idxs[i];
      if (s.sigIdx >= vs.size())
        throw std::invalid_argument("Index out of range in VectorMix");
      res.segment(s.index, s.size) = *vs[s.sigIdx];
    }
  }

  inline void addSelec(const int &sigIdx, const int &i, const int &s) {
    idxs.push_back(segment_t(i, s, sigIdx));
  }

  inline void initialize(Base *ent, Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    entity = ent;

    ent->addSignal("default");

    boost::function<void(const int &, const int &, const int &)> selec =
        boost::bind(&VectorMix::addSelec, this, _1, _2, _3);

    commandMap.insert(std::make_pair(
        "addSelec", makeCommandVoid3<Base, int, int, int>(
                        *ent, selec,
                        docCommandVoid3("add selection from a vector.",
                                        "int (signal index >= 1)",
                                        "int (index)", "int (size)"))));
  }
};

/* --- ADDITION --------------------------------------------------------- */
template <typename T> struct AdderVariadic : public VariadicOpHeader<T, T> {
  typedef VariadicOp<AdderVariadic> Base;

  Base *entity;
  Vector coeffs;

  AdderVariadic() : coeffs() {}
  inline void operator()(const std::vector<const T *> &vs, T &res) const {
    assert(vs.size() == (std::size_t)coeffs.size());
    if (vs.size() == 0)
      return;
    res = coeffs[0] * (*vs[0]);
    for (std::size_t i = 1; i < vs.size(); ++i)
      res += coeffs[i] * (*vs[i]);
  }

  inline void setCoeffs(const Vector &c) {
    if (entity->getSignalNumber() != c.size())
      throw std::invalid_argument("Invalid coefficient size.");
    coeffs = c;
  }
  inline void updateSignalNumber(const int &n) { coeffs = Vector::Ones(n); }

  inline void initialize(Base *ent, Entity::CommandMap_t &) {
    entity = ent;
    entity->setSignalNumber(2);
  }

  inline std::string getDocString() const {
    return "Linear combination of inputs\n"
           "  - input  " +
           VariadicOpHeader<T, T>::nameTypeIn() +
           "\n"
           "  - output " +
           VariadicOpHeader<T, T>::nameTypeOut() +
           "\n"
           "  sout = sum ([coeffs[i] * sin[i] for i in range(n) ])\n"
           "  Coefficients are set by commands, default value is 1.\n";
  }
};

/* --- MULTIPLICATION --------------------------------------------------- */
template <typename T> struct Multiplier : public VariadicOpHeader<T, T> {
  typedef VariadicOp<Multiplier> Base;

  inline void operator()(const std::vector<const T *> &vs, T &res) const {
    if (vs.size() == 0)
      setIdentity(res);
    else {
      res = *vs[0];
      for (std::size_t i = 1; i < vs.size(); ++i)
        res *= *vs[i];
    }
  }

  inline void setIdentity(T &res) const { res.setIdentity(); }

  inline void initialize(Base *ent, Entity::CommandMap_t &) {
    ent->setSignalNumber(2);
  }
};
template <> inline void Multiplier<double>::setIdentity(double &res) const {
  res = 1;
}
template <>
inline void Multiplier<MatrixHomogeneous>::
operator()(const std::vector<const MatrixHomogeneous *> &vs,
           MatrixHomogeneous &res) const {
  if (vs.size() == 0)
    setIdentity(res);
  else {
    res = *vs[0];
    for (std::size_t i = 1; i < vs.size(); ++i)
      res = res * *vs[i];
  }
}
template <>
inline void Multiplier<Vector>::
operator()(const std::vector<const Vector *> &vs, Vector &res) const {
  if (vs.size() == 0)
    res.resize(0);
  else {
    res = *vs[0];
    for (std::size_t i = 1; i < vs.size(); ++i)
      res.array() *= vs[i]->array();
  }
}

/* --- BOOLEAN --------------------------------------------------------- */
template <int operation> struct BoolOp : public VariadicOpHeader<bool, bool> {
  typedef VariadicOp<BoolOp> Base;

  inline void operator()(const std::vector<const bool *> &vs, bool &res) const {
    // TODO computation could be optimized with lazy evaluation of the
    // signals. When the output result is know, the remaining signals are
    // not computed.
    if (vs.size() == 0)
      return;
    res = *vs[0];
    for (std::size_t i = 1; i < vs.size(); ++i)
      switch (operation) {
      case 0:
        if (!res)
          return;
        res = *vs[i];
        break;
      case 1:
        if (res)
          return;
        res = *vs[i];
        break;
      }
  }
};

} // namespace sot
} // namespace dynamicgraph

/* --- TODO ------------------------------------------------------------------*/
// The following commented lines are sot-v1 entities that are still waiting
//   for conversion. Help yourself!

// /* --------------------------------------------------------------------------
// */

// struct WeightedDirection
// {
// public:
//   void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector&
//   v2,dynamicgraph::Vector& res ) const
//   {
//     const double norm1 = v1.norm();
//     const double norm2 = v2.norm();
//     res=v2; res*=norm1;
//     res*= (1/norm2);
//   }
// };
// typedef BinaryOp< Vector,Vector,Vector,WeightedDirection > weightdir;
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(weightdir,vector,weight_dir,"WeightDir")

// /* --------------------------------------------------------------------------
// */

// struct Nullificator
// {
// public:
//   void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector&
//   v2,dynamicgraph::Vector& res ) const
//   {
//     const unsigned int s = std::max( v1.size(),v2.size() );
//     res.resize(s);
//     for( unsigned int i=0;i<s;++i )
//       {
// 	if( v1(i)>v2(i) ) res(i)=v1(i)-v2(i);
// 	else 	if( v1(i)<-v2(i) ) res(i)=v1(i)+v2(i);
// 	else res(i)=0;
//       }
//   }
// };
// typedef BinaryOp< Vector,Vector,Vector,Nullificator > vectNil;
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(vectNil,vector,vectnil_,"Nullificator")

// /* --------------------------------------------------------------------------
// */

// struct VirtualSpring
// {
// public:
//   double spring;

//   void operator()( const dynamicgraph::Vector& pos,const
//   dynamicgraph::Vector& ref,dynamicgraph::Vector& res ) const
//   {
//     double norm = ref.norm();
//     double dist = ref.scalarProduct(pos) / (norm*norm);

//     res.resize( ref.size() );
//     res = ref;  res *= dist; res -= pos;
//     res *= spring;
//   }
// };
// typedef BinaryOp< Vector,Vector,Vector,VirtualSpring > virtspring;
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E_CMD
// (virtspring,vector,virtspring_,
//  "VirtualSpring"
//  ,else if( cmdLine=="spring" ){  CMDARGS_INOUT(op.spring); }
//  ,"VirtualSpring<pos,ref> compute the virtual force of a spring attache "
//  "to the reference line <ref>. The eq is: k.(<ref|pos>/<ref|ref>.ref-pos)"
//  "Params:\n  - spring: get/set the spring factor.")


/*
 * Copyright (c) 2015 Shihao Ji and Hyokun Yun. All Rights Reserved.
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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.
 *
 * For more information, bug reports, fixes, contact:
 *   Shihao Ji  (shihaoji@yahoo.com)
 *   Hyokun Yun (yungilbert@gmail.com)
 */
#ifndef __WORDRANK_MODEL_HPP
#define __WORDRANK_MODEL_HPP

#include <queue>
#include <map>
#include <unordered_set>
#include <set>
#include <sstream>
#include <omp.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <mm_malloc.h>
#include "parameter.hpp"
#include "data.hpp"

#ifdef USE_MKL
#include "mkl.h"
#endif

using namespace boost::posix_time;
using std::cout;
using std::flush;
using std::endl;
using std::pair;
using std::priority_queue;
using std::map;
using std::set;
using std::min;
using std::max;
using std::max_element;

namespace wordrank {

struct aux_info {
    int col_index;
    int row_index;
    scalar xi;
    scalar wi;
};

scalar logistic_loss(scalar x) {
    return log2f(1.f + powf(2.f, x));
}

scalar hinge_loss(scalar x) {
    return x > -1.f ? x + 1.f : 0.f;
}

scalar logistic_der(scalar x) {
    return 1.f / (1.f + powf(2.f, -x));
}

scalar hinge_der(scalar x) {
    return x > -1.f ? 1.f : 0.f;
}

class Model {

private:
    Parameter& param_;
    Data& data_;
    rng_type rng_;

public:
    Model(Parameter& param, Data& data) : param_(param), data_(data), rng_(param.random_seed_) {}

    ~Model() {}

    void run();

private:
    scalar calc_loss(scalar x) {
        if (param_.loss_type_ == LossType::LOGISTIC) {
            return logistic_loss(x);
        } else if (param_.loss_type_ == LossType::HINGE) {
            return hinge_loss(x);
        } else {
            //cerr << "unsupported loss type" << endl;
            return 1000000.f;
            //exit(1);
        }
    }

    scalar calc_der(scalar x) {
        if (param_.loss_type_ == LossType::LOGISTIC) {
            return logistic_der(x);
        } else if (param_.loss_type_ == LossType::HINGE) {
            return hinge_der(x);
        } else {
            //cerr << "unsupported loss type" << endl;
            return 1000000.f;
            //exit(1);
        }
    }

    void normalize(scalar* matrix, int m, int n) {
        for (int i = 0; i < m; i++) {
            int offset = i * n;
            scalar norm2 = 0.f;
            #pragma simd
            for (int j = 0; j < n; j++) {
                scalar v = matrix[offset + j];
                norm2 += v * v;
            }
            scalar norm2r = 1.f / sqrtf(norm2);
            if (norm2 > 1e-6f) {
                #pragma simd
                for (int j = 0; j < n; j++) matrix[offset + j] *= norm2r;
            }
        }
    }

};

}

void writelog(const char *str) {
    cout << "\n" << second_clock::local_time() << " - " << str << endl << flush;
}

void wordrank::Model::run() {

    writelog("start run()");

    // define necessary constants
    const int rank = param_.rank_;
    const int dim = param_.latent_dim_;
    const scalar alpha = param_.alpha_;
    const scalar beta = param_.beta_;
    const scalar tl = param_.t_;
    const scalar tau = param_.tau_;
    const int num_rows = data_.get_num_rows();
    const int num_cols = data_.get_num_cols();

    const int numtasks = param_.numtasks_;
    const int numthreads = param_.numthreads_;
    const int numparts = numthreads * numtasks;
    const int numrows_per_part = data_.numrows_per_part_;
    const int numcols_per_part = data_.numcols_per_part_;
    const int num_rows_upbd = numthreads * numrows_per_part * numtasks;
    const int num_cols_upbd = numthreads * numcols_per_part * numtasks;

    const int sgd_num = param_.sgd_num_;
    const TransformType trans_type = param_.trans_type_;

    auto& train_row_nnzs = data_.train_row_nnzs_;
    auto& train_col_nnzsum = data_.train_col_nnzsum_;
    auto& train_row_nnzsum = data_.train_row_nnzsum_;

    const size_t train_num_points = data_.train_total_nnz_;
    const double train_total_nnzsum = data_.train_total_nnzsum_;

    vector<vector<record_info>>& csc_indices = data_.csc_indices_;
    vector<vector<index_type>>& csc_ptrs = data_.csc_ptrs_;
    vector<index_type>& local_nnz = data_.local_nnz_;

    // create thread-specific RNGs
    rng_type rngs[numthreads];
    for (int i = 0; i < numthreads; i++) {
        rngs[i] = rng_type(param_.random_seed_ + 15791 * i + 373 * rank);
    }
    rng_type main_rng(param_.random_seed_ + 377 * rank);

    /////////////////////////////////////////////////////////////////////
    // Initialize Parameters
    /////////////////////////////////////////////////////////////////////

#ifdef USE_MKL
    mkl_set_num_threads(1);
#endif

    // indexed by thread, local_row_index, coordinate

    scalar *matrix_U = (scalar *) _mm_malloc(numthreads * numrows_per_part * dim * sizeof(scalar), 64);

    scalar *matrix_V = (scalar *) _mm_malloc(numthreads * numcols_per_part * dim * sizeof(scalar), 64);

    scalar *vector_Rec = (scalar *) _mm_malloc(numthreads * num_cols_upbd * sizeof(scalar), 64);

    // maps local_col_index to global_col_index
    int *col_indices = (int *) _mm_malloc(numthreads * numcols_per_part * sizeof(int), 64);

    {
        std::uniform_real_distribution<scalar> init_dist(0, 1.f);

        #pragma omp parallel for num_threads(numthreads)
        for (int thread_index = 0; thread_index < numthreads; thread_index++)
        {
            scalar *local_matrix_U = matrix_U + thread_index * numrows_per_part * dim;
            for (int j = 0; j < numrows_per_part * dim; j++) {
                local_matrix_U[j] = init_dist(rngs[thread_index]);
            }
            scalar *local_matrix_V = matrix_V + thread_index * numcols_per_part * dim;
            for (int j = 0; j < numcols_per_part * dim; j++) {
                local_matrix_V[j] = init_dist(rngs[thread_index]);
            }
            // normalize them
            normalize(local_matrix_U, numrows_per_part, dim);
            normalize(local_matrix_V, numcols_per_part, dim);

            int *local_col_indices = col_indices + thread_index * numcols_per_part;
            #pragma simd
            for (int j = 0; j < numcols_per_part; j++) {
                int col_index = (rank * numthreads + thread_index) * numcols_per_part + j;
                if (col_index < num_cols) {
                    local_col_indices[j] = col_index;
                } else {
                    local_col_indices[j] = -1;
                }
            }
        }
        // set the values of overflowed rows and cols to zeros
        int numRowsOverflow = (rank + 1) * numthreads * numrows_per_part - num_rows;
        if (numRowsOverflow > 0)
            memset(matrix_U + numthreads * numrows_per_part - numRowsOverflow, 0, numRowsOverflow * sizeof(scalar));

        int numColsOverflow = (rank + 1) * numthreads * numcols_per_part - num_cols;
        if (numColsOverflow > 0)
            memset(matrix_V + numthreads * numcols_per_part - numColsOverflow, 0, numColsOverflow * sizeof(scalar));

    }

    scalar *global_matrix_U = NULL;
    if (rank == 0) {
        global_matrix_U = (scalar *) _mm_malloc(num_rows_upbd * dim * sizeof(scalar), 64);
    }

    scalar *global_matrix_V = (scalar *) _mm_malloc(num_cols_upbd * dim * sizeof(scalar), 64);

    int *global_col_indices = (int *) _mm_malloc(num_cols_upbd * sizeof(int), 64);

    MPI_Allgather(matrix_V, numthreads * numcols_per_part * dim, MPI_SCALAR, global_matrix_V,
            numthreads * numcols_per_part * dim, MPI_SCALAR, MPI_COMM_WORLD);
    MPI_Allgather(col_indices, numthreads * numcols_per_part, MPI_INT, global_col_indices,
            numthreads * numcols_per_part, MPI_INT, MPI_COMM_WORLD);

    /////////////////////////////////////////////////////////////////////
    // Optimization
    /////////////////////////////////////////////////////////////////////

    int *col_locations = (int *) _mm_malloc(num_cols * sizeof(int), 64);

    // prepare auxiliary information variables
    vector<vector<aux_info>> auxs(numthreads);
    vector<vector<vector<aux_info *>>> rowwise_auxptrs(numthreads, vector<vector<aux_info *>>(numrows_per_part, vector<aux_info *>()));

    #pragma omp parallel for num_threads(numthreads)
    for (int thread_index = 0; thread_index < numthreads; thread_index++) {
        auxs[thread_index].resize(local_nnz[thread_index]);

        aux_info *ptr = &auxs[thread_index][0];
        for (int col_index = 0; col_index < num_cols; col_index++) {
            for (int j = csc_ptrs[thread_index][col_index]; j < csc_ptrs[thread_index][col_index + 1]; j++) {
                record_info& ri = csc_indices[thread_index][j];
                ptr->col_index = col_index;
                ptr->row_index = ri.row_index;
                ptr->xi = (trans_type == TransformType::RHO2) ? 0.5f : 1.0f;
                ptr->wi = ri.weight;
                rowwise_auxptrs[thread_index][ri.row_index].push_back(ptr);
                ptr++;
            }
        }
    }
    // release memory
    csc_indices.clear();

    const double stepsize = param_.learning_rate_ * train_num_points / train_total_nnzsum;
    const double reg = param_.regularization_ / ((num_cols - 1) * train_num_points);

    writelog("starts optimization");

    int *global_perm = (int *) _mm_malloc(num_cols_upbd * sizeof(int), 64);
    int *local_perm = (int *) _mm_malloc(numthreads * numcols_per_part * sizeof(int), 64);

    double cumul_computation_time = 0.0;

    /*************************************************************************/
    /* Here is the main loop                                                 */
    /*************************************************************************/
    for (int iter_num = 0; iter_num < param_.max_iteration_; iter_num++) {

        std::stringstream monitor_stream;
        monitor_stream << iter_num << ", " << param_;

        cout << "iteration: " << iter_num << endl << flush;

        if ((param_.dump_prefix_.length() > 0) && (iter_num % param_.dump_period_ == 0)) {

            cout << "dumping data" << endl << flush;

            if (rank == 0) {

                MPI_Gather(matrix_U, numthreads * numrows_per_part * dim, MPI_SCALAR, global_matrix_U,
                        numthreads * numrows_per_part * dim, MPI_SCALAR, 0, MPI_COMM_WORLD);

                std::ofstream ofile(param_.dump_prefix_ + "_word_" + boost::lexical_cast<std::string>(iter_num) + ".txt");

                scalar *user_param = global_matrix_U;
                for (int user_index = 0; user_index < num_rows; user_index++) {
                    int global_row_index = data_.row_perm_inv_[user_index];
                    if (data_.useVocab_){
                        ofile << data_.vocab_[global_row_index];
                    } else {
                        ofile << global_row_index + 1;
                    }

                    for (int i = 0; i < dim; i++) {
                        ofile << " ";
                        ofile << user_param[i];
                    }
                    ofile << endl;
                    user_param += dim;
                }
                ofile.close();
            } else {
                MPI_Gather(matrix_U, numthreads * numrows_per_part * dim, MPI_SCALAR, global_matrix_U,
                        numthreads * numrows_per_part * dim, MPI_SCALAR, 0, MPI_COMM_WORLD);
            }

            if (rank == 0) {
                std::ofstream ofile(param_.dump_prefix_ + "_context_" + boost::lexical_cast<std::string>(iter_num) + ".txt");
                scalar *item_param = global_matrix_V;
                for (int item_index = 0; item_index < num_cols_upbd; item_index++) {
                    int global_col_index = global_col_indices[item_index];

                    if (global_col_index > -1) {
                        if (data_.useVocab_) {
                            ofile << data_.vocab_[global_col_index];
                        } else {
                            ofile << global_col_index + 1;
                        }

                        for (int i = 0; i < dim; i++) {
                            ofile << " ";
                            ofile << item_param[i];
                        }
                        ofile << endl;
                    }
                    item_param += dim;
                }
                ofile.close();
            }
        }

        double compute_start_time = omp_get_wtime();

        // sample a new assignment of column indices
        if (rank == 0) {
            std::iota(global_perm, global_perm + num_cols_upbd, 0);
            std::shuffle(global_perm, global_perm + num_cols_upbd, main_rng);
        }
        MPI_Scatter(global_perm, numcols_per_part * numthreads, MPI_INT, local_perm, numcols_per_part * numthreads,
                MPI_INT, 0, MPI_COMM_WORLD);

        // copy global parameters to local space
        {
            #pragma omp parallel for num_threads(numthreads)
            for (int i = 0; i < numcols_per_part * numthreads; i++) {
                // copy the index
                col_indices[i] = global_col_indices[local_perm[i]];

                // copy the parameter value
                scalar *source_ptr = global_matrix_V + local_perm[i] * dim;
                scalar *target_ptr = matrix_V + i * dim;
                std::copy(source_ptr, source_ptr + dim, target_ptr);
            }
        }

        // inside a local machine, we use a permutation matrix
        // to determine the order of block assignment
        vector<vector<int> > perm_matrix(numthreads, vector<int>(numthreads, 0));
        {
            vector<int> col_perm(numthreads, 0);

            #pragma omp parallel for num_threads(numthreads)
            for (int i = 0; i < numthreads; i++) {
                col_perm[i] = i;
                #pragma simd
                for (int j = 0; j < numthreads; j++) {
                    perm_matrix[i][j] = (i + j) % numthreads;
                }
            }
            shuffle(perm_matrix.begin(), perm_matrix.end(), main_rng);
            shuffle(col_perm.begin(), col_perm.end(), main_rng);

            // apply column permutation
            #pragma omp parallel for num_threads(numthreads)
            for (int i = 0; i < numthreads; i++) {
                vector<int> tmp_buffer(numthreads, 0);
                std::copy(perm_matrix[i].begin(), perm_matrix[i].end(), tmp_buffer.begin());
                #pragma simd
                for (int j = 0; j < numthreads; j++) {
                    perm_matrix[i][col_perm[j]] = tmp_buffer[j];
                }
            }
        }

        // run SGD
        for (int inner_iter = 0; inner_iter < numthreads; inner_iter++) {

            #pragma omp parallel for num_threads(numthreads)
            for (int thread_index = 0; thread_index < numthreads; thread_index++)
            {
                int chunk_index = perm_matrix[inner_iter][thread_index];

                int *local_col_indices = col_indices + chunk_index * numcols_per_part;
                scalar *local_matrix_U = matrix_U + thread_index * numrows_per_part * dim;
                scalar *local_matrix_V = matrix_V + chunk_index * numcols_per_part * dim;

                std::uniform_int_distribution<int> init_dist(0, numcols_per_part - 1);
                auto sample_column = [&](int local_col_index)->int {
                    int ret = local_col_index;
                    while (true) {
                        ret = init_dist(rngs[thread_index]);
                        if ((ret != local_col_index) && (local_col_indices[ret] != -1)) {
                            return ret;
                        }
                    }
                };

                scalar backup_row_vec[dim];

                if (param_.loss_type_ == LossType::LOGISTIC) {

                    for (int sgd_iter = 0; sgd_iter < sgd_num; sgd_iter++) {

                        for (int i = 0; i < numcols_per_part; i++) {
                            int col_index = local_col_indices[i];
                            scalar *col_vec = local_matrix_V + i * dim;

                            if (col_index < 0) {
                                continue;
                            }

                            for (int j = csc_ptrs[thread_index][col_index]; j < csc_ptrs[thread_index][col_index + 1]; j++) {
                                aux_info& aux_ptr = auxs[thread_index][j];
                                int row_index = aux_ptr.row_index;
                                scalar xi = aux_ptr.xi;
                                scalar wi = aux_ptr.wi;
                                scalar *row_vec = local_matrix_U + row_index * dim;

                                int sam_local_index = sample_column(i);
                                int sam_index = local_col_indices[sam_local_index];
                                scalar *sam_vec = local_matrix_V + sam_local_index * dim;

                                std::copy(row_vec, row_vec + dim, backup_row_vec);

                                scalar org_dot = std::inner_product(row_vec, row_vec + dim, col_vec, 0.f);
                                scalar sam_dot = std::inner_product(row_vec, row_vec + dim, sam_vec, 0.f);

                                scalar coef = 0.f;

                                switch (trans_type) {
                                case TransformType::RHO1:
                                    coef = wi * xi * INV_LOG2 / (tau + powf(2.f, org_dot - sam_dot));
                                    break;
                                case TransformType::RHO2: {
                                    scalar logxi = logf(xi);
                                    scalar logxi2 = logxi * logxi;
                                    coef = wi * xi * LOG2 / (logxi2 * (tau + powf(2.f, org_dot - sam_dot)));
                                    break;
                                }
                                case TransformType::RHO3:
                                    coef = wi * powf(xi, tl) / (tau + powf(2.f, org_dot - sam_dot));
                                    break;
                                default:
                                    cerr << "unsupported transform type" << endl;
                                    exit(1);
                                }

                                #pragma simd
                                for (int t = 0; t < dim; t++) {
                                    row_vec[t] -= stepsize * (reg * train_row_nnzsum[thread_index][row_index] * row_vec[t]
                                                     + coef * (sam_vec[t] - col_vec[t]));
                                    col_vec[t] -= stepsize * (reg * train_col_nnzsum[col_index] * col_vec[t]
                                                     - coef * backup_row_vec[t]);
                                    sam_vec[t] -= stepsize * (reg * (train_total_nnzsum - train_col_nnzsum[sam_index]) * sam_vec[t]
                                                     + coef * backup_row_vec[t]);
                                }
                            }
                        }
                    } // end of sam_iter
                } // case of logistic loss (end)
                else if (param_.loss_type_ == LossType::HINGE) {

                    for (int sgd_iter = 0; sgd_iter < sgd_num; sgd_iter++) {

                        for (int i = 0; i < numcols_per_part; i++) {
                            int col_index = local_col_indices[i];
                            scalar *col_vec = local_matrix_V + i * dim;

                            if (col_index < 0) {
                                continue;
                            }

                            for (int j = csc_ptrs[thread_index][col_index]; j < csc_ptrs[thread_index][col_index + 1]; j++) {
                                aux_info& aux_ptr = auxs[thread_index][j];
                                int row_index = aux_ptr.row_index;
                                scalar xi = aux_ptr.xi;
                                scalar wi = aux_ptr.wi;
                                scalar *row_vec = local_matrix_U + row_index * dim;

                                int sam_local_index = sample_column(i);
                                int sam_index = local_col_indices[sam_local_index];
                                scalar *sam_vec = local_matrix_V + sam_local_index * dim;

                                std::copy(row_vec, row_vec + dim, backup_row_vec);

                                scalar org_dot = std::inner_product(row_vec, row_vec + dim, col_vec, 0.f);
                                scalar sam_dot = std::inner_product(row_vec, row_vec + dim, sam_vec, 0.f);

                                // if hinge loss is nonzero
                                if (sam_dot > org_dot - tau) {

                                    scalar coef = 0.f;

                                    switch (trans_type) {
                                    case TransformType::RHO1:
                                        coef = wi * xi * INV_LOG2;
                                        break;
                                    case TransformType::RHO2: {
                                        scalar logxi = logf(xi);
                                        scalar logxi2 = logxi * logxi;
                                        coef = wi * xi * LOG2 / logxi2;
                                        break;
                                    }
                                    case TransformType::RHO3:
                                        coef = wi * powf(xi, tl);
                                        break;
                                    default:
                                        cerr << "unsupported transform type" << endl;
                                        exit(1);
                                    }

                                    #pragma simd
                                    for (int t = 0; t < dim; t++) {
                                        row_vec[t] -= stepsize * (reg * train_row_nnzsum[thread_index][row_index] * row_vec[t]
                                                        + coef * (sam_vec[t] - col_vec[t]));
                                        col_vec[t] -= stepsize * (reg * train_col_nnzsum[col_index] * col_vec[t]
                                                        - coef * backup_row_vec[t]);
                                        sam_vec[t] -= stepsize * (reg * (train_total_nnzsum - train_col_nnzsum[sam_index]) * sam_vec[t]
                                                        + coef * backup_row_vec[t]);
                                    }
                                }
                            }
                        }
                    }
                } else {
                    cerr << "unsupported loss type 1" << endl << flush;
                    exit(1);
                }
            }
        }

        // at the end of the iteration, synchronize parameters
        MPI_Allgather(matrix_V, numthreads * numcols_per_part * dim, MPI_SCALAR, global_matrix_V,
                numthreads * numcols_per_part * dim, MPI_SCALAR, MPI_COMM_WORLD);
        MPI_Allgather(col_indices, numthreads * numcols_per_part, MPI_INT, global_col_indices,
                numthreads * numcols_per_part, MPI_INT, MPI_COMM_WORLD);

        #pragma omp parallel for num_threads(numthreads)
        #pragma simd
        for (int i = 0; i < num_cols_upbd; i++) {
            if (global_col_indices[i] >= 0) {
                col_locations[global_col_indices[i]] = i;
            }
        }

        // update auxiliary parameters exactly
        if (iter_num > 0 && iter_num % param_.xi_period_ == 0) {

            writelog("update auxiliary parameters");

            vector<scalar> max_xi(numthreads, -1000000.f);
            vector<scalar> min_xi(numthreads, 1000000.f);
            vector<scalar> mean_xi(numthreads, 0.f);

//            cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, numthreads * numrows_per_part, num_cols_upbd,
//                    dim, 1.0f, matrix_U, dim, global_matrix_V, dim, 0.0f, matrix_Rec, num_cols_upbd);

            #pragma omp parallel for num_threads(numthreads)
            for (int thread_index = 0; thread_index < numthreads; thread_index++)
            {
                scalar* vector_Rec_ptr = vector_Rec + thread_index * num_cols_upbd;
                scalar* matrix_U_ptr = matrix_U + thread_index * numrows_per_part * dim;

                for (int row_index = 0; row_index < numrows_per_part; row_index++) {
#ifndef USE_MKL
                    scalar* row_ptr = matrix_U_ptr + row_index * dim;
                    for (int i = 0; i < num_cols_upbd; i++) {
                        scalar* col_ptr = global_matrix_V + i * dim;
                        scalar sum = 0.f;
                        #pragma simd
                        for (int j = 0; j < dim; j++) {
                            sum += row_ptr[j] * col_ptr[j];
                        }
                        vector_Rec_ptr[i] = sum;
                    }
#else
                    cblas_sgemv(CblasRowMajor, CblasNoTrans, num_cols_upbd, dim, 1.0f, global_matrix_V, dim,
                            matrix_U_ptr + row_index * dim, 1, 0.0f, vector_Rec_ptr, 1);
#endif
                    vector<aux_info *>& auxptrs = rowwise_auxptrs[thread_index][row_index];
                    int numnnzs = auxptrs.size();

                    for (int i = 0; i < numnnzs; i++) {
                        aux_info* ptr = auxptrs[i];
                        int col_index = ptr->col_index;
                        scalar wi = ptr->wi;
                        scalar val = vector_Rec_ptr[col_locations[col_index]];
                        double sum = 0;
                        #pragma simd
                        for (int j = 0; j < num_cols_upbd; j++) {
                            scalar diff = vector_Rec_ptr[j] - val;
                            sum += diff > -tau ? (tau + diff) : 0.;
                        }
                        ptr->xi = (trans_type == TransformType::RHO2) ? (0.5 * beta + 1) / (sqrt(sum) + 1 + beta) :
                                alpha / (sum + beta);
                    }

                    #pragma simd
                    for (int i = 0; i < numnnzs; i++) {
                        aux_info* ptr = auxptrs[i];
                        mean_xi[thread_index] += ptr->xi;
                        if (max_xi[thread_index] < ptr->xi)
                            max_xi[thread_index] = ptr->xi;
                        if (min_xi[thread_index] > ptr->xi)
                            min_xi[thread_index] = ptr->xi;
                    }

                }
            }

            scalar machine_max_xi = *max_element(max_xi.begin(), max_xi.end());
            scalar machine_min_xi = *min_element(min_xi.begin(), min_xi.end());
            scalar machine_mean_xi = std::accumulate(mean_xi.begin(), mean_xi.end(), 0.f) * numtasks / train_num_points;

            scalar global_max_xi = -1000000.f;
            scalar global_min_xi = 1000000.f;
            scalar global_mean_xi = 0.f;

            MPI_Allreduce(&machine_max_xi, &global_max_xi, 1, MPI_SCALAR, MPI_MAX, MPI_COMM_WORLD);
            MPI_Allreduce(&machine_min_xi, &global_min_xi, 1, MPI_SCALAR, MPI_MIN, MPI_COMM_WORLD);
            MPI_Allreduce(&machine_mean_xi, &global_mean_xi, 1, MPI_SCALAR, MPI_SUM, MPI_COMM_WORLD);

            global_mean_xi /= numtasks;

            cout << "machine min xi: " << machine_min_xi << ", machine max xi: " << machine_max_xi
                    << ", machine mean xi: " << machine_mean_xi << ", global mean xi: " << global_mean_xi << endl << flush;
        }

        double computation_time = omp_get_wtime() - compute_start_time;
        cumul_computation_time += computation_time;
        monitor_stream << ", " << cumul_computation_time;

        // calculate the objective function
        if (param_.cost_eval_)
        {
            writelog("calculate cost function");

            vector<double> local_loss_sums(numthreads, 0);
            vector<double> local_approx_loss_sums(numthreads, 0);
            vector<double> local_reg_sums(numthreads, 0);

            #pragma omp parallel for num_threads(numthreads)
            for (int thread_index = 0; thread_index < numthreads; thread_index++)
            {
                scalar* matrix_U_ptr = matrix_U + thread_index * numrows_per_part * dim;
                scalar* vector_Rec_ptr = vector_Rec + thread_index * num_cols_upbd;

                for (int n = 0; n < local_nnz[thread_index]; n++) {
                    aux_info& aux = auxs[thread_index][n];
                    int row_index = aux.row_index;
                    int col_index = aux.col_index;
                    scalar xi = aux.xi;
                    scalar wi = aux.wi;

#ifndef USE_MKL
                    scalar* row_ptr = matrix_U_ptr + row_index * dim;
                    for (int i = 0; i < num_cols_upbd; i++) {
                        scalar* col_ptr = global_matrix_V + i * dim;
                        scalar sum = 0.f;
                        #pragma simd
                        for (int j = 0; j < dim; j++) {
                            sum += row_ptr[j] * col_ptr[j];
                        }
                        vector_Rec_ptr[i] = sum;
                    }
#else
                    cblas_sgemv(CblasRowMajor, CblasNoTrans, num_cols_upbd, dim, 1.f, global_matrix_V, dim,
                            matrix_U_ptr + row_index * dim, 1, 0.f, vector_Rec_ptr, 1);
#endif

                    scalar val = vector_Rec_ptr[col_locations[col_index]];
                    scalar sum = 0.f;
                    #pragma simd
                    for (int j = 0; j < num_cols_upbd; j++) {
                        scalar diff = vector_Rec_ptr[j] - val;
                        sum += diff > -tau ? (tau + diff) : 0.f;
                    }
                    if (trans_type == TransformType::RHO1) {
                        local_loss_sums[thread_index] += wi * log2(sum);
                        local_approx_loss_sums[thread_index] += wi * ((xi * (sum + beta) - 1) * INV_LOG2 - alpha * log2(xi));
                    } else {
                        scalar logxi = logf(xi);
                        scalar logxi2 = logxi * logxi;
                        local_loss_sums[thread_index] += wi * (1 - 1 / log2(sum + 1));
                        local_approx_loss_sums[thread_index] += wi * (1 + LOG2 * (logxi - 1 + xi * (sum + 1)) / logxi2
                                + (beta * xi - 0.5 * beta * logxi) * INV_LOG2);
                    }
                }

                // calculate the regularizer term
                if (param_.regularization_ > 0) {
                    #pragma simd
                    for (int row_index = 0; row_index < numrows_per_part; row_index++) {
                        scalar *row_vec = matrix_U_ptr + row_index * dim;
                        local_reg_sums[thread_index] += train_row_nnzsum[thread_index][row_index]
                                * std::inner_product(row_vec, row_vec + dim, row_vec, 0.f);
                    }
                    int *local_col_indices = col_indices + thread_index * numcols_per_part;
                    scalar *local_matrix_V = matrix_V + thread_index * numcols_per_part * dim;
                    #pragma simd
                    for (int col_index = 0; col_index < numcols_per_part; col_index++) {
                        scalar *col_vec = local_matrix_V + col_index * dim;
                        local_reg_sums[thread_index] += train_col_nnzsum[local_col_indices[col_index]]
                                * std::inner_product(col_vec, col_vec + dim, col_vec, 0.f);
                    }
                }
            }

            double global_loss_sum = 0.0;
            double machine_loss_sum = std::accumulate(local_loss_sums.begin(), local_loss_sums.end(), 0.0);
            MPI_Allreduce(&machine_loss_sum, &global_loss_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);

            double global_approx_loss_sum = 0.0;
            double machine_approx_loss_sum = std::accumulate(local_approx_loss_sums.begin(), local_approx_loss_sums.end(), 0.0);
            MPI_Allreduce(&machine_approx_loss_sum, &global_approx_loss_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);

            double global_reg_sum = 0.0;
            if (param_.regularization_ > 0) {
                double machine_reg_sum = std::accumulate(local_reg_sums.begin(), local_reg_sums.end(), 0.0);
                MPI_Allreduce(&machine_reg_sum, &global_reg_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
            }

            double cost = global_approx_loss_sum + param_.regularization_ * global_reg_sum;

            monitor_stream << ", cost: " << cost << " (" << global_approx_loss_sum << " + " << param_.regularization_ * global_reg_sum
                    << "), true loss: " << global_loss_sum;
        }

        if (rank == 0) {
            cout << "monitor, " << monitor_stream.str() << endl << flush;
        }

    } // end of SGD iteration

    /////////////////////////////////////////////////////////////////////
    // Deallocate Memory
    /////////////////////////////////////////////////////////////////////

    _mm_free(vector_Rec);
    _mm_free(matrix_U);
    _mm_free(matrix_V);
    if (rank == 0) _mm_free(global_matrix_U);
    _mm_free(global_matrix_V);
    _mm_free(global_col_indices);
    _mm_free(local_perm);
    _mm_free(global_perm);
    _mm_free(col_indices);
    _mm_free(col_locations);
}

#endif


#include <iostream>
#include <cstdlib>
#include <cstdio>
#include <armadillo>
#include <random>

using namespace std;
using namespace arma;

int main(int argc, char **argv) {
  if(argc <= 2) {
    fprintf(stderr, "Usage: %s OUT-FILE NUM-AGENTS\n", argv[0]);
    return 0;
  }

  const char *filename = argv[1];
  int N = atoi(argv[2]);
  size_t K = 10000000;
  double m0 = 1.0;

  // pseudorandom number generator
  default_random_engine G;

  // seed PRNG with true random number
  random_device source;
  unsigned int seed = source();
  G.seed(seed);

  // uniform distributions
  uniform_int_distribution<> idist(0, N-1);  // discrete distribution on [0,N)
  uniform_real_distribution<> rdist(0, 1);   // real distribution on [0,1)

  // list of agents' money
  vector<double> m(N, m0);

  // sum of m^2
  double m2sq = N * m0 * m0;

  // lists of measured variance at different steps
  std::vector<int> step;
  std::vector<double> variance;

  // do switches
  size_t powerOfTen = 1;
  for(size_t k = 0; k < K; k++) {
    // pick two random, different agents
    int i = idist(G), j = idist(G);
    while(i == j) j = idist(G);

    // calculate total money of the two agents
    double mtot = m[i] + m[j];

    // redistribute money between the two agents
    double e = rdist(G);
    m2sq += (e * e + (1 - e) * (1 - e)) * mtot * mtot - m[i] * m[i] - m[j] * m[j];
    m[i] = e * mtot;
    m[j] = (1 - e) * mtot;

    if(k >= 100 * powerOfTen)
      powerOfTen *= 10;
    if(k % powerOfTen == 0) {
      // calculate variance
      double V = m2sq / N - m0 * m0;

      step.push_back(k);
      variance.push_back(V);
    }
  }

  // save to file
  FILE *fp = fopen(filename, "w");
  fprintf(fp, "k\tV\n");
  for(size_t i = 0; i < step.size() && i < variance.size(); i++) {
    fprintf(fp, "%d\t%.3E\n", step[i], variance[i]);
  }
  fclose(fp);
}


//  Copyright (c) 2007, 2013 John Maddock
//  Copyright Christopher Kormanyos 2013.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// This header just defines the function entry points, and adds dispatch
// to the right implementation method.  Most of the implementation details
// are in separate headers and copyright Xiaogang Zhang.
//
#ifndef BOOST_MATH_BESSEL_HPP
#define BOOST_MATH_BESSEL_HPP

#ifdef _MSC_VER
#  pragma once
#endif

#include <boost/math/special_functions/detail/bessel_jy.hpp>
#include <boost/math/special_functions/detail/bessel_jn.hpp>
#include <boost/math/special_functions/detail/bessel_yn.hpp>
#include <boost/math/special_functions/detail/bessel_jy_zero.hpp>
#include <boost/math/special_functions/detail/bessel_ik.hpp>
#include <boost/math/special_functions/detail/bessel_i0.hpp>
#include <boost/math/special_functions/detail/bessel_i1.hpp>
#include <boost/math/special_functions/detail/bessel_kn.hpp>
#include <boost/math/special_functions/detail/iconv.hpp>
#include <boost/math/special_functions/sin_pi.hpp>
#include <boost/math/special_functions/cos_pi.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/special_functions/round.hpp>
#include <boost/math/tools/rational.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/math/tools/series.hpp>
#include <boost/math/tools/roots.hpp>

namespace boost{ namespace math{

namespace detail{

template <class T, class Policy>
struct sph_bessel_j_small_z_series_term
{
   typedef T result_type;

   sph_bessel_j_small_z_series_term(unsigned v_, T x)
      : N(0), v(v_)
   {
      BOOST_MATH_STD_USING
      mult = x / 2;
      term = pow(mult, T(v)) / boost::math::tgamma(v+1+T(0.5f), Policy());
      mult *= -mult;
   }
   T operator()()
   {
      T r = term;
      ++N;
      term *= mult / (N * T(N + v + 0.5f));
      return r;
   }
private:
   unsigned N;
   unsigned v;
   T mult;
   T term;
};

template <class T, class Policy>
inline T sph_bessel_j_small_z_series(unsigned v, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names
   sph_bessel_j_small_z_series_term<T, Policy> s(v, x);
   boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   T zero = 0;
   T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter, zero);
#else
   T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
#endif
   policies::check_series_iterations<T>("boost::math::sph_bessel_j_small_z_series<%1%>(%1%,%1%)", max_iter, pol);
   return result * sqrt(constants::pi<T>() / 4);
}

template <class T, class Policy>
T cyl_bessel_j_imp(T v, T x, const bessel_no_int_tag& t, const Policy& pol)
{
   BOOST_MATH_STD_USING
   static const char* function = "boost::math::bessel_j<%1%>(%1%,%1%)";
   if(x < 0)
   {
      // better have integer v:
      if(floor(v) == v)
      {
         T r = cyl_bessel_j_imp(v, T(-x), t, pol);
         if(iround(v, pol) & 1)
            r = -r;
         return r;
      }
      else
         return policies::raise_domain_error<T>(
            function,
            "Got x = %1%, but we need x >= 0", x, pol);
   }

   T j, y;
   bessel_jy(v, x, &j, &y, need_j, pol);
   return j;
}

template <class T, class Policy>
inline T cyl_bessel_j_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING  // ADL of std names.
   int ival = detail::iconv(v, pol);
   // If v is an integer, use the integer recursion
   // method, both that and Steeds method are O(v):
   if((0 == v - ival))
   {
      return bessel_jn(ival, x, pol);
   }
   return cyl_bessel_j_imp(v, x, bessel_no_int_tag(), pol);
}

template <class T, class Policy>
inline T cyl_bessel_j_imp(int v, T x, const bessel_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING
   return bessel_jn(v, x, pol);
}

template <class T, class Policy>
inline T sph_bessel_j_imp(unsigned n, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names
   if(x < 0)
      return policies::raise_domain_error<T>(
         "boost::math::sph_bessel_j<%1%>(%1%,%1%)",
         "Got x = %1%, but function requires x > 0.", x, pol);
   //
   // Special case, n == 0 resolves down to the sinus cardinal of x:
   //
   if(n == 0)
      return boost::math::sinc_pi(x, pol);
   //
   // When x is small we may end up with 0/0, use series evaluation
   // instead, especially as it converges rapidly:
   //
   if(x < 1)
      return sph_bessel_j_small_z_series(n, x, pol);
   //
   // Default case is just a naive evaluation of the definition:
   //
   return sqrt(constants::pi<T>() / (2 * x))
      * cyl_bessel_j_imp(T(T(n)+T(0.5f)), x, bessel_no_int_tag(), pol);
}

template <class T, class Policy>
T cyl_bessel_i_imp(T v, T x, const Policy& pol)
{
   //
   // This handles all the bessel I functions, note that we don't optimise
   // for integer v, other than the v = 0 or 1 special cases, as Millers
   // algorithm is at least as inefficient as the general case (the general
   // case has better error handling too).
   //
   BOOST_MATH_STD_USING
   if(x < 0)
   {
      // better have integer v:
      if(floor(v) == v)
      {
         T r = cyl_bessel_i_imp(v, T(-x), pol);
         if(iround(v, pol) & 1)
            r = -r;
         return r;
      }
      else
         return policies::raise_domain_error<T>(
         "boost::math::cyl_bessel_i<%1%>(%1%,%1%)",
            "Got x = %1%, but we need x >= 0", x, pol);
   }
   if(x == 0)
   {
      return (v == 0) ? 1 : 0;
   }
   if(v == 0.5f)
   {
      // common special case, note try and avoid overflow in exp(x):
      if(x >= tools::log_max_value<T>())
      {
         T e = exp(x / 2);
         return e * (e / sqrt(2 * x * constants::pi<T>()));
      }
      return sqrt(2 / (x * constants::pi<T>())) * sinh(x);
   }
   if(policies::digits<T, Policy>() <= 64)
   {
      if(v == 0)
      {
         return bessel_i0(x);
      }
      if(v == 1)
      {
         return bessel_i1(x);
      }
   }
   if((v > 0) && (x / v < 0.25))
      return bessel_i_small_z_series(v, x, pol);
   T I, K;
   bessel_ik(v, x, &I, &K, need_i, pol);
   return I;
}

template <class T, class Policy>
inline T cyl_bessel_k_imp(T v, T x, const bessel_no_int_tag& /* t */, const Policy& pol)
{
   static const char* function = "boost::math::cyl_bessel_k<%1%>(%1%,%1%)";
   BOOST_MATH_STD_USING
   if(x < 0)
   {
      return policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but we need x > 0", x, pol);
   }
   if(x == 0)
   {
      return (v == 0) ? policies::raise_overflow_error<T>(function, 0, pol)
         : policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but we need x > 0", x, pol);
   }
   T I, K;
   bessel_ik(v, x, &I, &K, need_k, pol);
   return K;
}

template <class T, class Policy>
inline T cyl_bessel_k_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING
   if((floor(v) == v))
   {
      return bessel_kn(itrunc(v), x, pol);
   }
   return cyl_bessel_k_imp(v, x, bessel_no_int_tag(), pol);
}

template <class T, class Policy>
inline T cyl_bessel_k_imp(int v, T x, const bessel_int_tag&, const Policy& pol)
{
   return bessel_kn(v, x, pol);
}

template <class T, class Policy>
inline T cyl_neumann_imp(T v, T x, const bessel_no_int_tag&, const Policy& pol)
{
   static const char* function = "boost::math::cyl_neumann<%1%>(%1%,%1%)";

   BOOST_MATH_INSTRUMENT_VARIABLE(v);
   BOOST_MATH_INSTRUMENT_VARIABLE(x);

   if(x <= 0)
   {
      return (v == 0) && (x == 0) ?
         policies::raise_overflow_error<T>(function, 0, pol)
         : policies::raise_domain_error<T>(
               function,
               "Got x = %1%, but result is complex for x <= 0", x, pol);
   }
   T j, y;
   bessel_jy(v, x, &j, &y, need_y, pol);
   //
   // Post evaluation check for internal overflow during evaluation,
   // can occur when x is small and v is large, in which case the result
   // is -INF:
   //
   if(!(boost::math::isfinite)(y))
      return -policies::raise_overflow_error<T>(function, 0, pol);
   return y;
}

template <class T, class Policy>
inline T cyl_neumann_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING

   BOOST_MATH_INSTRUMENT_VARIABLE(v);
   BOOST_MATH_INSTRUMENT_VARIABLE(x);

   if(floor(v) == v)
   {
      if(asymptotic_bessel_large_x_limit(v, x))
      {
         T r = asymptotic_bessel_y_large_x_2(static_cast<T>(abs(v)), x);
         if((v < 0) && (itrunc(v, pol) & 1))
            r = -r;
         BOOST_MATH_INSTRUMENT_VARIABLE(r);
         return r;
      }
      else
      {
         T r = bessel_yn(itrunc(v, pol), x, pol);
         BOOST_MATH_INSTRUMENT_VARIABLE(r);
         return r;
      }
   }
   T r = cyl_neumann_imp<T>(v, x, bessel_no_int_tag(), pol);
   BOOST_MATH_INSTRUMENT_VARIABLE(r);
   return r;
}

template <class T, class Policy>
inline T cyl_neumann_imp(int v, T x, const bessel_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING

   BOOST_MATH_INSTRUMENT_VARIABLE(v);
   BOOST_MATH_INSTRUMENT_VARIABLE(x);

   if(asymptotic_bessel_large_x_limit(T(v), x))
   {
      T r = asymptotic_bessel_y_large_x_2(static_cast<T>(abs(v)), x);
      if((v < 0) && (v & 1))
         r = -r;
      return r;
   }
   else
      return bessel_yn(v, x, pol);
}

template <class T, class Policy>
inline T sph_neumann_imp(unsigned v, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names
   static const char* function = "boost::math::sph_neumann<%1%>(%1%,%1%)";
   //
   // Nothing much to do here but check for errors, and
   // evaluate the function's definition directly:
   //
   if(x < 0)
      return policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but function requires x > 0.", x, pol);

   if(x < 2 * tools::min_value<T>())
      return -policies::raise_overflow_error<T>(function, 0, pol);

   T result = cyl_neumann_imp(T(T(v)+0.5f), x, bessel_no_int_tag(), pol);
   T tx = sqrt(constants::pi<T>() / (2 * x));

   if((tx > 1) && (tools::max_value<T>() / tx < result))
      return -policies::raise_overflow_error<T>(function, 0, pol);

   return result * tx;
}

template <class T, class Policy>
inline T cyl_bessel_j_zero_imp(T v, int m, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names, needed for floor.

   static const char* function = "boost::math::cyl_bessel_j_zero<%1%>(%1%, int)";

   const T half_epsilon(boost::math::tools::epsilon<T>() / 2U);

   // Handle non-finite order.
   if (!(boost::math::isfinite)(v) )
   {
     return policies::raise_domain_error<T>(function, "Order argument is %1%, but must be finite >= 0 !", v, pol);
   }

   // Handle negative rank.
   if(m < 0)
   {
      // Zeros of Jv(x) with negative rank are not defined and requesting one raises a domain error.
      return policies::raise_domain_error<T>(function, "Requested the %1%'th zero, but the rank must be positive !", m, pol);
   }

   // Get the absolute value of the order.
   const bool order_is_negative = (v < 0);
   const T vv((!order_is_negative) ? v : T(-v));

   // Check if the order is very close to zero or very close to an integer.
   const bool order_is_zero    = (vv < half_epsilon);
   const bool order_is_integer = ((vv - floor(vv)) < half_epsilon);

   if(m == 0)
   {
      if(order_is_zero)
      {
         // The zero'th zero of J0(x) is not defined and requesting it raises a domain error.
         return policies::raise_domain_error<T>(function, "Requested the %1%'th zero of J0, but the rank must be > 0 !", m, pol);
      }

      // The zero'th zero of Jv(x) for v < 0 is not defined
      // unless the order is a negative integer.
      if(order_is_negative && (!order_is_integer))
      {
         // For non-integer, negative order, requesting the zero'th zero raises a domain error.
         return policies::raise_domain_error<T>(function, "Requested the %1%'th zero of Jv for negative, non-integer order, but the rank must be > 0 !", m, pol);
      }

      // The zero'th zero does exist and its value is zero.
      return T(0);
   }

   // Set up the initial guess for the upcoming root-finding.
   // If the order is a negative integer, then use the corresponding
   // positive integer for the order.
   const T guess_root = boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::initial_guess<T, Policy>((order_is_integer ? vv : v), m, pol);

   // Select the maximum allowed iterations from the policy.
   boost::uintmax_t number_of_iterations = policies::get_max_root_iterations<Policy>();

   // Select the desired number of binary digits of precision.
   // Account for the radix of number representations having non-two radix!
   const int my_digits2 = policies::digits<T, Policy>();

   const T delta_lo = ((guess_root > 0.2F) ? T(0.2) : T(guess_root / 2U));

   // Perform the root-finding using Newton-Raphson iteration from Boost.Math.
   const T jvm =
      boost::math::tools::newton_raphson_iterate(
         boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::function_object_jv_and_jv_prime<T, Policy>((order_is_integer ? vv : v), order_is_zero, pol),
         guess_root,
         T(guess_root - delta_lo),
         T(guess_root + 0.2F),
         my_digits2,
         number_of_iterations);

   if(number_of_iterations >= policies::get_max_root_iterations<Policy>())
   {
      policies::raise_evaluation_error<T>(function, "Unable to locate root in a reasonable time:"
         "  Current best guess is %1%", jvm, Policy());
   }

   return jvm;
}

template <class T, class Policy>
inline T cyl_neumann_zero_imp(T v, int m, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names, needed for floor.

   static const char* function = "boost::math::cyl_neumann_zero<%1%>(%1%, int)";

   // Handle non-finite order.
   if (!(boost::math::isfinite)(v) )
   {
     return policies::raise_domain_error<T>(function, "Order argument is %1%, but must be finite >= 0 !", v, pol);
   }

   // Handle negative rank.
   if(m < 0)
   {
      return policies::raise_domain_error<T>(function, "Requested the %1%'th zero, but the rank must be positive !", m, pol);
   }

   const T half_epsilon(boost::math::tools::epsilon<T>() / 2U);

   // Get the absolute value of the order.
   const bool order_is_negative = (v < 0);
   const T vv((!order_is_negative) ? v : T(-v));

   const bool order_is_integer = ((vv - floor(vv)) < half_epsilon);

   // For negative integers, use reflection to positive integer order.
   if(order_is_negative && order_is_integer)
      return boost::math::detail::cyl_neumann_zero_imp(vv, m, pol);

   // Check if the order is very close to a negative half-integer.
   const T delta_half_integer(vv - (floor(vv) + 0.5F));

   const bool order_is_negative_half_integer =
      (order_is_negative && ((delta_half_integer > -half_epsilon) && (delta_half_integer < +half_epsilon)));

   // The zero'th zero of Yv(x) for v < 0 is not defined
   // unless the order is a negative integer.
   if((m == 0) && (!order_is_negative_half_integer))
   {
      // For non-integer, negative order, requesting the zero'th zero raises a domain error.
      return policies::raise_domain_error<T>(function, "Requested the %1%'th zero of Yv for negative, non-half-integer order, but the rank must be > 0 !", m, pol);
   }

   // For negative half-integers, use the corresponding
   // spherical Bessel function of positive half-integer order.
   if(order_is_negative_half_integer)
      return boost::math::detail::cyl_bessel_j_zero_imp(vv, m, pol);

   // Set up the initial guess for the upcoming root-finding.
   // If the order is a negative integer, then use the corresponding
   // positive integer for the order.
   const T guess_root = boost::math::detail::bessel_zero::cyl_neumann_zero_detail::initial_guess<T, Policy>(v, m, pol);

   // Select the maximum allowed iterations from the policy.
   boost::uintmax_t number_of_iterations = policies::get_max_root_iterations<Policy>();

   // Select the desired number of binary digits of precision.
   // Account for the radix of number representations having non-two radix!
   const int my_digits2 = policies::digits<T, Policy>();

   const T delta_lo = ((guess_root > 0.2F) ? T(0.2) : T(guess_root / 2U));

   // Perform the root-finding using Newton-Raphson iteration from Boost.Math.
   const T yvm =
      boost::math::tools::newton_raphson_iterate(
         boost::math::detail::bessel_zero::cyl_neumann_zero_detail::function_object_yv_and_yv_prime<T, Policy>(v, pol),
         guess_root,
         T(guess_root - delta_lo),
         T(guess_root + 0.2F),
         my_digits2,
         number_of_iterations);

   if(number_of_iterations >= policies::get_max_root_iterations<Policy>())
   {
      policies::raise_evaluation_error<T>(function, "Unable to locate root in a reasonable time:"
         "  Current best guess is %1%", yvm, Policy());
   }

   return yvm;
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_j(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_j_imp<value_type>(v, static_cast<value_type>(x), tag_type(), forwarding_policy()), "boost::math::cyl_bessel_j<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_j(T1 v, T2 x)
{
   return cyl_bessel_j(v, x, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type sph_bessel(unsigned v, T x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::sph_bessel_j_imp<value_type>(v, static_cast<value_type>(x), forwarding_policy()), "boost::math::sph_bessel<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type sph_bessel(unsigned v, T x)
{
   return sph_bessel(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_i(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_i_imp<value_type>(v, static_cast<value_type>(x), forwarding_policy()), "boost::math::cyl_bessel_i<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_i(T1 v, T2 x)
{
   return cyl_bessel_i(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_k(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_k_imp<value_type>(v, static_cast<value_type>(x), tag_type(), forwarding_policy()), "boost::math::cyl_bessel_k<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_k(T1 v, T2 x)
{
   return cyl_bessel_k(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_neumann(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_neumann_imp<value_type>(v, static_cast<value_type>(x), tag_type(), forwarding_policy()), "boost::math::cyl_neumann<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_neumann(T1 v, T2 x)
{
   return cyl_neumann(v, x, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type sph_neumann(unsigned v, T x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::sph_neumann_imp<value_type>(v, static_cast<value_type>(x), forwarding_policy()), "boost::math::sph_neumann<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type sph_neumann(unsigned v, T x)
{
   return sph_neumann(v, x, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type cyl_bessel_j_zero(T v, int m, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<value_type>::is_integer, "Order must be a floating-point type.");
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_j_zero_imp<value_type>(v, m, forwarding_policy()), "boost::math::cyl_bessel_j_zero<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type cyl_bessel_j_zero(T v, int m)
{
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<T>::is_integer, "Order must be a floating-point type.");
   return cyl_bessel_j_zero<T, policies::policy<> >(v, m, policies::policy<>());
}

template <class T, class OutputIterator, class Policy>
inline OutputIterator cyl_bessel_j_zero(T v,
                              int start_index,
                              unsigned number_of_zeros,
                              OutputIterator out_it,
                              const Policy& pol)
{
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<T>::is_integer, "Order must be a floating-point type.");
   for(unsigned i = 0; i < number_of_zeros; ++i)
   {
      *out_it = boost::math::cyl_bessel_j_zero(v, start_index + i, pol);
      ++out_it;
   }
   return out_it;
}

template <class T, class OutputIterator>
inline OutputIterator cyl_bessel_j_zero(T v,
                              int start_index,
                              unsigned number_of_zeros,
                              OutputIterator out_it)
{
   return cyl_bessel_j_zero(v, start_index, number_of_zeros, out_it, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type cyl_neumann_zero(T v, int m, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<value_type>::is_integer, "Order must be a floating-point type.");
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_neumann_zero_imp<value_type>(v, m, forwarding_policy()), "boost::math::cyl_neumann_zero<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type cyl_neumann_zero(T v, int m)
{
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<T>::is_integer, "Order must be a floating-point type.");
   return cyl_neumann_zero<T, policies::policy<> >(v, m, policies::policy<>());
}

template <class T, class OutputIterator, class Policy>
inline OutputIterator cyl_neumann_zero(T v,
                             int start_index,
                             unsigned number_of_zeros,
                             OutputIterator out_it,
                             const Policy& pol)
{
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<T>::is_integer, "Order must be a floating-point type.");
   for(unsigned i = 0; i < number_of_zeros; ++i)
   {
      *out_it = boost::math::cyl_neumann_zero(v, start_index + i, pol);
      ++out_it;
   }
   return out_it;
}

template <class T, class OutputIterator>
inline OutputIterator cyl_neumann_zero(T v,
                             int start_index,
                             unsigned number_of_zeros,
                             OutputIterator out_it)
{
   return cyl_neumann_zero(v, start_index, number_of_zeros, out_it, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_BESSEL_HPP