// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2015 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution  and  use  in  source  and  binary  forms,  with  or  without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// * Neither the name of Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE  COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY  EXPRESS OR IMPLIED WARRANTIES,  INCLUDING, BUT NOT LIMITED  TO, THE
// IMPLIED WARRANTIES OF  MERCHANTABILITY AND FITNESS FOR  A PARTICULAR PURPOSE
// ARE  DISCLAIMED. IN  NO  EVENT  SHALL THE  COPYRIGHT  OWNER OR  CONTRIBUTORS
// BE  LIABLE FOR  ANY  DIRECT, INDIRECT,  INCIDENTAL,  SPECIAL, EXEMPLARY,  OR
// CONSEQUENTIAL  DAMAGES  (INCLUDING,  BUT  NOT  LIMITED  TO,  PROCUREMENT  OF
// SUBSTITUTE GOODS  OR SERVICES; LOSS  OF USE,  DATA, OR PROFITS;  OR BUSINESS
// INTERRUPTION)  HOWEVER CAUSED  AND ON  ANY THEORY  OF LIABILITY,  WHETHER IN
// CONTRACT,  STRICT LIABILITY,  OR  TORT (INCLUDING  NEGLIGENCE OR  OTHERWISE)
// ARISING IN ANY WAY  OUT OF THE USE OF THIS SOFTWARE, EVEN  IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
//
// A  simple implementation  of N-dimensional  dual numbers,  for automatically
// computing exact derivatives of functions.
//
// While    a   complete    treatment   of    the   mechanics    of   automatic
// differentation    is    beyond   the    scope    of    this   header    (see
// http://en.wikipedia.org/wiki/Automatic_differentiation  for   details),  the
// basic idea is to extend normal  arithmetic with an extra element, "e," often
// denoted with the  greek symbol epsilon, such that  e != 0 but e^2  = 0. Dual
// numbers are  extensions of  the real numbers  analogous to  complex numbers:
// whereas complex numbers augment the reals by introducing an imaginary unit i
// such that  i^2 = -1, dual  numbers introduce an "infinitesimal"  unit e such
// that e^2 = 0. Dual numbers have two components: the "real" component and the
// "infinitesimal" component, generally written as  x + y*e. Surprisingly, this
// leads to a convenient method for computing exact derivatives without needing
// to manipulate complicated symbolic expressions.
//
// For example, consider the function
//
//   f(x) = x^2 ,
//
// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20.
// Next, augument 10 with an infinitesimal to get:
//
//   f(10 + e) = (10 + e)^2
//             = 100 + 2 * 10 * e + e^2
//             = 100 + 20 * e       -+-
//                     --            |
//                     |             +--- This is zero, since e^2 = 0
//                     |
//                     +----------------- This is df/dx!
//
// Note that the derivative of f with  respect to x is simply the infinitesimal
// component of the value  of f(x + e). So, in order to  take the derivative of
// any function, it  is only necessary to replace the  numeric "object" used in
// the function with  one extended with infinitesimals. The  class Jet, defined
// in this header, is one such example of this, where substitution is done with
// templates.
//
// To  handle derivatives  of  functions taking  multiple arguments,  different
// infinitesimals are  used, one for each  variable to take the  derivative of.
// For example, consider a scalar function of two scalar parameters x and y:
//
//   f(x, y) = x^2 + x * y
//
// Following the  technique above, to  compute the derivatives df/dx  and df/dy
// for f(1, 3) involves doing two evaluations  of f, the first time replacing x
// with x + e, the second time replacing y with y + e.
//
// For df/dx:
//
//   f(1 + e, y) = (1 + e)^2 + (1 + e) * 3
//               = 1 + 2 * e + 3 + 3 * e
//               = 4 + 5 * e
//
//               --> df/dx = 5
//
// For df/dy:
//
//   f(1, 3 + e) = 1^2 + 1 * (3 + e)
//               = 1 + 3 + e
//               = 4 + e
//
//               --> df/dy = 1
//
// To take the  gradient of f with the implementation  of dual numbers ("jets")
// in  this file,  it  is necessary  to  create  a single  jet  type which  has
// components for the  derivative in x and  y, and passing them  to a templated
// version of f:
//
//   template<typename T>
//   T f(const T &x, const T &y) {
//     return x * x + x * y;
//   }
//
//   // The "2" means there should be 2 dual number components.
//   Jet<double, 2> x(1, 0);  // Pick the 0th dual number for x.
//   Jet<double, 2> y(3, 1);  // Pick the 1st dual number for y.
//   Jet<double, 2> z = f(x, y);
//
//   std::cout << z << std::endl;
//
// For  the  more mathematically  inclined,  this  file implements  first-order
// "jets". A 1st order jet is an element of the ring
//
//   T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2
//
// which essentially means that each jet  consists of a "scalar" value 'a' from
// T and a 1st order perturbation vector 'v' of length N:
//
//   x = a + \sum_i v[i] t_i
//
// A shorthand is to write an element as x = a + u, where u is the pertubation.
// Then, the  main point about  the arithmetic of jets  is that the  product of
// perturbations is zero:
//
//   (a + u) * (b + v) = ab + av + bu + uv
//                     = ab + (av + bu) + 0
//
// which is what operator* implements below. Addition is simpler:
//
//   (a + u) + (b + v) = (a + b) + (u + v).
//
// The only remaining  question is how to  evaluate the function of  a jet, for
// which we use the chain rule:
//
//   f(a + u) = f(a) + f'(a) u
//
// where f'(a) is the (scalar) derivative of f at a.

#ifndef PUBLIC_JET_HPP_
#define PUBLIC_JET_HPP_

#include <Eigen/Core>
#include <cmath>

template <typename Tp, int N>
struct Jet {
  // Default-construct "a"  because otherwise  this can lead  to false
  // errors  about uninitialized  uses when  other classes  relying on
  // default constructed Tp  (where Tp is a Jet<Tp,  N>). This usually
  // only happens  in opt  mode. Note that  the C++  standard mandates
  // that e.g. default constructed doubles are initialized to 0.0; see
  // sections 8.5 of the C++03 standard.
  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Jet() : a() { v.setZero(); }

  // Constructor from scalar: a + [0, ...].
  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit
    Jet(const Tp& value) { a = value; v.setZero(); }

  // Constructor from scalar plus variable: a + [0, ..., 1, ... 0].
  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet(const Tp& value, int k) { a = value; v.setZero(); v[k] = Tp(1.0); }

  // Constructor from scalar and vector part
  // ---------------------------------------
  // The use of Eigen::DenseBase allows Eigen expressions to be passed
  // in  without being  fully evaluated until  they are assigned  to v
  template<typename Derived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet(const Tp& a, const Eigen::DenseBase<Derived> &v) : a(a), v(v) {}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet<Tp, N>& operator+=(const Jet<Tp, N> &y)
    { *this = *this + y; return *this; }

  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet<Tp, N>& operator-=(const Jet<Tp, N> &y)
    { *this = *this - y; return *this; }

  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet<Tp, N>& operator*=(const Jet<Tp, N> &y)
    { *this = *this * y; return *this; }

  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet<Tp, N>& operator/=(const Jet<Tp, N> &y)
    { *this = *this / y; return *this; }

#ifdef CEREAL_SERIALIZE_FUNCTION_NAME

  friend class cereal::access;

  template<class Archive>
  void serialize(Archive& archive) {
    archive(cereal::make_nvp("[real]", a));
    archive(cereal::make_nvp("[dual]", v));
  }

#endif  // CEREAL_SERIALIZE_FUNCTION_NAME

  // Jet dimensionality.
  enum { kDimensions = N };

  // The scalar part.
  Tp a;

  // The infinitesimal part.
  Eigen::Matrix<Tp, N, 1, Eigen::AutoAlign> v;
};

// Unary +
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> const& operator+(const Jet<Tp, N>& f) { return f; }

// Unary -
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator-(const Jet<Tp, N>&f) { return Jet<Tp, N>(-f.a, -f.v); }

// Binary +
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator+(const Jet<Tp, N>& f, const Jet<Tp, N>& g)
 { return Jet<Tp, N>(f.a + g.a, f.v + g.v); }

// Binary + with a scalar: x + s
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator+(const Jet<Tp, N>& f, Tp s)
  { return Jet<Tp, N>(f.a + s, f.v); }

// Binary + with a scalar: s + x
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator+(Tp s, const Jet<Tp, N>& f)
  { return Jet<Tp, N>(f.a + s, f.v); }

// Binary -
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator-(const Jet<Tp, N>& f, const Jet<Tp, N>& g)
  { return Jet<Tp, N>(f.a - g.a, f.v - g.v); }

// Binary - with a scalar: x - s
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator-(const Jet<Tp, N>& f, Tp s)
  { return Jet<Tp, N>(f.a - s, f.v); }

// Binary - with a scalar: s - x
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator-(Tp s, const Jet<Tp, N>& f)
  { return Jet<Tp, N>(s - f.a, -f.v); }

// Binary *
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator*(const Jet<Tp, N>& f, const Jet<Tp, N>& g)
 { return Jet<Tp, N>(f.a * g.a, f.a * g.v + f.v * g.a); }

// Binary * with a scalar: x * s
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator*(const Jet<Tp, N>& f, Tp s)
  { return Jet<Tp, N>(f.a * s, f.v * s); }

// Binary * with a scalar: s * x
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator*(Tp s, const Jet<Tp, N>& f)
  { return Jet<Tp, N>(f.a * s, f.v * s); }

// Binary /
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator/(const Jet<Tp, N>& f, const Jet<Tp, N>& g) {
  //   a + u   (a + u)(b - v)   (a + u)(b - v)
  //   ----- = -------------- = --------------
  //   b + v   (b + v)(b - v)        b^2
  return Jet<Tp, N>(f.a / g.a, (f.v - f.a / g.a * g.v) / g.a);
}

// Binary / with a scalar: s / x
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator/(Tp s, const Jet<Tp, N>& g)
  { return Jet<Tp, N>(s / g.a, -s * g.v / (g.a * g.a)); }

// Binary / with a scalar: x / s
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator/(const Jet<Tp, N>& f, Tp s)
  { return Jet<Tp, N>(f.a / s, f.v / s); }

// Binary comparison operators for both scalars and jets.
#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op)                                    \
  template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE                \
  bool operator op(const Tp& s, const Jet<Tp, N>& g) { return s op g.a; }           \
                                                                                \
  template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE                \
  bool operator op(const Jet<Tp, N>& f, const Tp& s) { return f.a op s; }           \
                                                                                \
  template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE                \
  bool operator op(const Jet<Tp, N>& f, const Jet<Tp, N>& g) { return f.a op g.a; } \

CERES_DEFINE_JET_COMPARISON_OPERATOR(< )
CERES_DEFINE_JET_COMPARISON_OPERATOR(<=)
CERES_DEFINE_JET_COMPARISON_OPERATOR(> )
CERES_DEFINE_JET_COMPARISON_OPERATOR(>=)
CERES_DEFINE_JET_COMPARISON_OPERATOR(==)
CERES_DEFINE_JET_COMPARISON_OPERATOR(!=)

#undef CERES_DEFINE_JET_COMPARISON_OPERATOR

// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.

using std::abs;  // abs(x + h) ~= x + h or -(x + h)
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> abs(const Jet<Tp, N>& f) { return f.a < Tp(0.0) ? -f : f; }

using std::log;  // log(a + h) ~= log(a) + h / a
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> log(const Jet<Tp, N>& f) { return Jet<Tp, N>(log(f.a), f.v / f.a); }

using std::exp;  // exp(a + h) ~= exp(a) + exp(a) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> exp(const Jet<Tp, N>& f) { return Jet<Tp, N>(exp(f.a), exp(f.a) * f.v); }

using std::sqrt;  // sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a))
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> sqrt(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(sqrt(f.a), f.v / (Tp(2.0) * sqrt(f.a))); }

using std::cos;  // cos(a + h) ~= cos(a) - sin(a) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> cos(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(cos(f.a), - sin(f.a) * f.v); }

using std::acos;  // acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> acos(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(acos(f.a), f.v / sqrt(Tp(1.0) - f.a * f.a)); }

using std::sin;  // sin(a + h) ~= sin(a) + cos(a) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> sin(const Jet<Tp, N>& f) { return Jet<Tp, N>(sin(f.a), cos(f.a) * f.v); }

using std::asin;  // asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> asin(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(asin(f.a), f.v / sqrt(Tp(1.0) - f.a * f.a)); }

using std::tan;  // tan(a + h) ~= tan(a) + (1 + tan(a)^2) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> tan(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(tan(f.a), Tp(1.0) + tan(f.a) * tan(f.a) * f.v); }

using std::atan;  // atan(a + h) ~= atan(a) + 1 / (1 + a^2) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> atan(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(atan(f.a), f.v / (Tp(1.0) + f.a * f.a)); }

using std::sinh;  // sinh(a + h) ~= sinh(a) + cosh(a) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> sinh(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(sinh(f.a), cosh(f.a) * f.v); }

using std::cosh;  // cosh(a + h) ~= cosh(a) + sinh(a) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> cosh(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(cosh(f.a), sinh(f.a) * f.v); }

using std::tanh;  // tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> tanh(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(tan(f.a), (Tp(1.0) - tan(f.a) * tan(f.a)) * f.v); }

using std::atan2;

// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2)
//
// In words: the rate of change of theta is 1/r times the rate of
// change of (x, y) in the positive angular direction.
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> atan2(const Jet<Tp, N>& g, const Jet<Tp, N>& f) {
  // Note order of arguments:
  //
  //   f = a + da
  //   g = b + db

  Tp const tmp = Tp(1.0) / (f.a * f.a + g.a * g.a);
  return Jet<Tp, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v));
}

using std::pow;

// pow -- base is a differentiable function, exponent is a constant.
// (a+da)^p ~= a^p + p*a^(p-1) da
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> pow(const Jet<Tp, N>& f, double g) {
  Tp const tmp = g * pow(f.a, g - Tp(1.0));
  return Jet<Tp, N>(pow(f.a, g), tmp * f.v);
}

// pow -- base is a constant, exponent is a differentiable function.
// We have various special cases, see the comment for pow(Jet, Jet) for
// analysis:
//
// 1. For f > 0 we have: (f)^(g + dg) ~= f^g + f^g log(f) dg
//
// 2. For f == 0 and g > 0 we have: (f)^(g + dg) ~= f^g
//
// 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg
// != 0, the derivatives are not defined and we return NaN.

template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> pow(double f, const Jet<Tp, N>& g) {
  if (f == 0 && g.a > 0) {
    // Handle case 2.
    return Jet<Tp, N>(Tp(0.0));
  }
  if (f < 0 && g.a == floor(g.a)) {
    // Handle case 3.
    Jet<Tp, N> ret(pow(f, g.a));
    for (int i = 0; i < N; i++) {
      if (g.v[i] != Tp(0.0)) {
        // Return a NaN when g.v != 0.
        ret.v[i] = std::numeric_limits<Tp>::quiet_NaN();
      }
    }
    return ret;
  }
  // Handle case 1.
  Tp const tmp = pow(f, g.a);
  return Jet<Tp, N>(tmp, log(f) * tmp * g.v);
}

// pow -- both base and exponent are differentiable functions. This has a
// variety of special cases that require careful handling.
//
// 1. For f > 0:
//    (f + df)^(g + dg) ~= f^g + f^(g - 1) * (g * df + f * log(f) * dg)
//    The numerical evaluation of f * log(f) for f > 0 is well behaved, even for
//    extremely small values (e.g. 1e-99).
//
// 2. For f == 0 and g > 1: (f + df)^(g + dg) ~= 0
//    This cases is needed because log(0) can not be evaluated in the f > 0
//    expression. However the function f*log(f) is well behaved around f == 0
//    and its limit as f-->0 is zero.
//
// 3. For f == 0 and g == 1: (f + df)^(g + dg) ~= 0 + df
//
// 4. For f == 0 and 0 < g < 1: The value is finite but the derivatives are not.
//
// 5. For f == 0 and g < 0: The value and derivatives of f^g are not finite.
//
// 6. For f == 0 and g == 0: The C standard incorrectly defines 0^0 to be 1
//    "because there are applications that can exploit this definition". We
//    (arbitrarily) decree that derivatives here will be nonfinite, since that
//    is consistent with the behavior for f == 0, g < 0 and 0 < g < 1.
//    Practically any definition could have been justified because mathematical
//    consistency has been lost at this point.
//
// 7. For f < 0, g integer, dg == 0: (f + df)^(g + dg) ~= f^g + g * f^(g - 1) df
//    This is equivalent to the case where f is a differentiable function and g
//    is a constant (to first order).
//
// 8. For f < 0, g integer, dg != 0: The value is finite but the derivatives are
//    not, because any change in the value of g moves us away from the point
//    with a real-valued answer into the region with complex-valued answers.
//
// 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite.

template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> pow(const Jet<Tp, N>& f, const Jet<Tp, N>& g) {
  if (f.a == 0 && g.a >= 1) {
    // Handle cases 2 and 3.
    if (g.a > 1) {
      return Jet<Tp, N>(Tp(0.0));
    }
    return f;
  }
  if (f.a < 0 && g.a == floor(g.a)) {
    // Handle cases 7 and 8.
    Tp const tmp = g.a * pow(f.a, g.a - Tp(1.0));
    Jet<Tp, N> ret(pow(f.a, g.a), tmp * f.v);
    for (int i = 0; i < N; i++) {
      if (g.v[i] != Tp(0.0)) {
        // Return a NaN when g.v != 0.
        ret.v[i] = std::numeric_limits<Tp>::quiet_NaN();
      }
    }
    return ret;
  }
  // Handle the remaining cases. For cases 4,5,6,9 we allow the log() function
  // to generate -HUGE_VAL or NaN, since those cases result in a nonfinite
  // derivative.
  Tp const tmp1 = pow(f.a, g.a);
  Tp const tmp2 = g.a * pow(f.a, g.a - Tp(1.0));
  Tp const tmp3 = tmp1 * log(f.a);
  return Jet<Tp, N>(tmp1, tmp2 * f.v + tmp3 * g.v);
}

template <typename Tp, int N>
std::ostream &operator<<(std::ostream &s, const Jet<Tp, N>& z)
  { return s << "[" << z.a << "; " << z.v.transpose() << "]"; }

namespace Eigen {

// Creating  a  specialization of  NumTraits  enables
// placing Jet  objects inside Eigen  arrays, getting
// all the goodness of Eigen combined with autodiff.
template<typename Tp, int N>
struct NumTraits<Jet<Tp, N> > {
  typedef Jet<Tp, N> Real;
  typedef Jet<Tp, N> NonInteger;
  typedef Jet<Tp, N> Nested;
  typedef Jet<Tp, N> Literal;

  // For Jet types, multiplication is more expensive than addition.
  enum { IsComplex = 0, IsInteger = 0, IsSigned, ReadCost = 1, AddCost = 1,
         MulCost = 3, HasFloatingPoint = 1, RequireInitialization = 1 };

  // Assuming that for Jets, division is as expensive as multiplication.
  template<bool Vectorized> struct Div { enum { Cost = 3 }; };

  static Jet<Tp, N> dummy_precision()
    { return Jet<Tp, N>(1e-12); }

  static inline Real epsilon()
    { return Real(std::numeric_limits<Tp>::epsilon()); }

  static inline int digits10() { return 0; }
};

}  // namespace Eigen

#endif  // PUBLIC_JET_HPP_


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

#include "util.h"

#include <boost/dynamic_bitset.hpp>

namespace refl {
namespace {
arma::Mat<int>
convert(cluster::QuiverMatrix const& q) {
	arma::Mat<int> res(util::to_arma(q));
	for(uint_fast16_t i = 0; i < res.n_elem; ++i) {
		res(i) = std::abs(res(i));
	}
	for(uint_fast16_t i = 0; i < res.n_cols; ++i) {
		res(i, i) = 2;
	}
	return res;
}
/**
 * Make a bitset which has a 1 at each non-zero value in the off diagonal
 * matrix and a 0 at each zero.
 *
 * Then result.count() gives the number of non-zero entries, which is the number
 * of places the iterator needs to consider changing the signs of.
 *
 * @param a Initial quasi-Cartan matrix
 * @param vals The number of off diagonal upper triangle values in a
 */
boost::dynamic_bitset<>
zeros(arma::Mat<int> const& a, uint_fast16_t vals) {
	boost::dynamic_bitset<> result{ vals, 0 };
	uint_fast16_t row = 0;
	uint_fast16_t col = 1;
	for(uint_fast16_t i = 0; i < vals; ++i) {
		result[i] = a(row, col) != 0;
		if(++col >= a.n_cols) {
			col = ++row + 1;
		}
	}
	return result;
}
}
CartanIterator::CartanIterator(cluster::QuiverMatrix const& q)
	: _number_vars( (q.num_rows() * (q.num_rows() - 1) ) / 2 ),
		_initial(convert(q)),
		_result(_initial.n_rows, _initial.n_cols),
		_zero_mask(zeros(_initial, _number_vars)),
		_non_zero_vars { _zero_mask.count() },
		_current_val {0},
		_max_val(std::pow(2, _non_zero_vars)) {}
bool
CartanIterator::has_next() {
	return _current_val < _max_val;
}
/*
 * Convert int _current_val into its binary representation, then insert this
 * into the non-zero values of _zero_mask. This ensures that we are never
 * pointlessly mutliplying 0 by -1 and creating many quasi-Cartans which are
 * actually the same.
 *
 * Use the bitset to multiply the vlues corresponding to 1s by -1 in the cartan
 * matrix.
 */
arma::Mat<int>&
CartanIterator::next(){
	boost::dynamic_bitset<> val_bits{_non_zero_vars, _current_val};
	boost::dynamic_bitset<> bits_with_zeros { _zero_mask };
	uint_fast16_t val_pos = 0;
	for(uint_fast16_t i = 0; i < bits_with_zeros.size(); ++i) {
		if(bits_with_zeros[i]) {
			bits_with_zeros[i] = val_bits[val_pos];
			++val_pos;
		}
	}

	_result = _initial;
	uint_fast16_t row = 0;
	uint_fast16_t col = 1;
	for(uint_fast16_t i = 0; i < _number_vars; ++i) {
		if(bits_with_zeros[i]) {
			_result(row, col) = -1 * _result(row, col);
			_result(col, row) = -1 * _result(col, row);
		}
		if(++col >= _result.n_cols) {
			col = ++row + 1;
		}
	}
	++_current_val;
	return _result;
}
}


#include <iostream>
#include <string>	// for stoi (argv)
#include <armadillo>
#include <algorithm> 	// shuffle
#include <stdlib.h>     // srand, rand
#include <time.h>     	// time
#include <vector>

#include "Functions.h"

using namespace arma;
using namespace std;

int main(int argc, char* argv[]){
	// argv[1]  : seed for random generator  (set to -1 for random seed)
	// argv[2]  : maximum number of MC trials

	// disable the dynamic adjustment of the number of threads within a team.
	omp_set_dynamic(false);
	// disable nested parallel regions, i.e., whether team members are allowed to create new teams.
	omp_set_nested(false);
	omp_set_num_threads(1);

	cout << endl;
	// initializing random number generator
	simulation_parameters simulation_params;
	cout << "# of Processors Available: \t" << omp_get_num_procs() << endl << endl;

	const int num_mc_runs = stoi(argv[2],nullptr,10);// number of MC trials
	cout << "# of MC trials: \t" << num_mc_runs << endl << endl;
	const int SEED = stoi(argv[1],nullptr,10);

	// signal Parameters
	unsigned int sig_dim 	= 1e4;			// signal dimension
	cout << "Signal Dimension: \t" << sig_dim << endl<<endl;
	unsigned int sparsity	= 2*sig_dim/100;	// sparsity level of signal
	cout << "Sparsity: \t\t" << sparsity << endl<<endl;
	unsigned int meas_num	= 30*sig_dim/100;	// number of measurements
	cout << "# of Measurements: \t" << meas_num << endl<<endl;

	// algorithm parameters
	const unsigned int max_iter = 1.5e3;
	const double gamma = 1e0;
	const double tol = 1e-7;
	const int unsigned block_size = fmin(meas_num,sparsity);
	const vec prob_vec = normalise(ones(meas_num / block_size),1);		// set probabilities of selecting each block

    simulation_params.slow_cores_ratio = 0.2;
	simulation_params.sleep_slow_cores = 15e5;   // microseconds to sleep
	cout << "Percentage of Slow Cores: \t" << simulation_params.slow_cores_ratio * 100;
	cout << " (each sleeping for " << simulation_params.sleep_slow_cores/1000 << " ms )" << endl<<endl;

	vector<string> alg_names;
    // Algorithms to test, comment out each line to skip running its corresponding algorithm
	alg_names.push_back("Bayesian Sto_IHT");
	alg_names.push_back("Sto_IHT");
	alg_names.push_back("Parallel Sto_IHT");
	alg_names.push_back("Tally Sto_IHT");
	alg_names.push_back("AMP");
	alg_names.push_back("Parallel AMP");

	run_experiments(alg_names, sig_dim, sparsity, meas_num, max_iter, gamma, tol ,
		prob_vec, simulation_params, num_mc_runs, SEED);

	return 1;
}


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>

extern int enzyme_const;
template<typename Return, typename... T>
Return __enzyme_autodiff(T...);

float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

#include <adept_arrays.h>
using adept::adouble;
using adept::aMatrix;
using adept::aVector;

using adept::Vector;

#define N 2000
#define M 2000
#define ITERS 1000
#define RATE 0.00000001

double matvec_real(double* mat, double* vec) {
  double *out = (double*)malloc(sizeof(double)*N);
  //double *out = new double[N];
  for(int i=0; i<N; i++) {
    out[i] = 0;
    for(int j=0; j<M; j++) {
        out[i] += mat[i*M+j] * vec[j];
    }
  }
  double sum = 0;
  for(int i=0; i<N; i++) {
    sum += out[i] * out[i];
  }
  free(out);
  //delete[] out;
  return sum;
}

/*
  Differentiation of matvec_real in reverse (adjoint) mode:
   gradient     of useful results: alloc(*out) matvec_real *mat
   with respect to varying inputs: alloc(*out) *mat
   RW status of diff variables: alloc(*out):in-out matvec_real:in-killed
                *mat:incr
   Plus diff mem management of: mat:in
*/
void matvec_real_b(double *mat, double *matb, double *vec, double matvec_realb) {
    double *out;
    double *outb;
    int ii1;
    double matvec_real;
    outb = (double *)malloc(sizeof(double)*N);
    for (ii1 = 0; ii1 < N; ++ii1)
        outb[ii1] = 0.0;
    out = (double *)malloc(sizeof(double)*N);
    //double *out = new double[N];
    for (int i = 0; i < N; ++i) {
        out[i] = 0;
        for (int j = 0; j < M; ++j)
            out[i] = out[i] + mat[i*M+j]*vec[j];
    }
    double sum = 0;
    double sumb = 0.0;
    sumb = matvec_realb;
    for (int i = N; i > -1; --i)
        outb[i] = outb[i] + 2*out[i]*sumb;
    for (int i = N; i > -1; --i) {
        for (int j = M; j > -1; --j)
            matb[i*M + j] = matb[i*M + j] + vec[j]*outb[i];
        outb[i] = 0.0;
    }
    free(out);
    free(outb);
}

#if 1
#include <vector>

static
adouble matvec(aMatrix& mat, Vector& vec) {
  //std::vector<adouble> out(N);

  aVector out = mat**vec;

#if 0
  for(int i=0; i<N; i++) {
    out[i] = 0;
    for(int j=0; j<M; j++) {
        out[i] +=
//        out[i] += mat[i*M+j] * vec[j];
    }
  }
#endif
  adouble sum = 0;
  for(int i=0; i<N; i++) {
    sum += out(i) * out(i);
  }
  //delete[] out;
  return sum;
}

#if 0
static
void sincos_and_gradient(double *Min, double *Mout, double *vecin, double *vecout) {
    //adouble *mat = new adouble[N*M];
    //std::vector<adouble> mat(N*M);// = new adouble[N*M];
    //adept::set_values(&mat[0], N*M, Min);
    //for(int i=0; i<N*M; i++) mat[i] = Min[i];
    //adouble *vec = new adouble[M];
    //std::vector<adouble> vec(M);// = new adouble[M];
    //adept::set_values(&vec[0], M, vecin);
    //for(int i=0; i<M; i++) vec[i] = vecin[i];

    //aMatrix M = aMatrix(N,M);
    //for (int i = 0; i < N; i++) {
    //  for (int j = 0; j < M; j++) {
    //    M.
    //  }
    //}

    adept::Stack stack;
    stack.new_recording();
    adouble loss = matvec(&mat[0], &vec[0]);
    loss.set_gradient(1.0);
    stack.compute_adjoint();

    adept::get_gradients(&mat[0], N*M, Mout);
    adept::get_gradients(&vec[0], M, vecout);
    //for(int i=0; i<N*M; i++) Mout[i] = mat[i].get_gradient();
    //for(int i=0; i<M; i++) vecout[i] = vec[i].get_gradient();

    //delete[] mat;
    //delete[] vec;
    //xgrad = x.get_gradient();
    //return y.value();
}
#endif

static void adept_sincos(double *Min, double *Mout, double *Vin, double *Vout) {

  {
  struct timeval start, end;

  double res2 = 0;
  adept::Stack stack;

    aMatrix mat(N,M);
    for(int i=0; i<N; i++) {
    for(int j=0; j<M; j++) {
        mat(i, j) = Min[i*M+j];
    }
    }
    Vector vec(M);
    for(int i=0; i<M; i++) vec(i) = Vin[i];

  gettimeofday(&start, NULL);
  for (int iter = 0; iter < ITERS; iter++) {
    stack.new_recording();
    adouble resa = matvec(mat, vec);
    resa.set_gradient(1.0);
    stack.continue_recording();
  }
    //stack.reverse();
    //stack.pause_recording();
    /*
    for (int i = 0; i < N; i++) {
      for (int j = 0; j < M; j++) {
        mat(i,j) -= mat(i,j).get_gradient()*RATE;
      }
    }*/
  gettimeofday(&end, NULL);
  //gettimeofday(&end, NULL);
  printf("%0.6f res'=%f %f %f\n", tdiff(&start, &end), Mout[1], Mout[2], Mout[3]);

  }

  {
  struct timeval start, end;
  //gettimeofday(&start, NULL);

  double res2 = 0;
  {
  adept::Stack stack;

    aMatrix mat(N,M);
    for(int i=0; i<N; i++) {
    for(int j=0; j<M; j++) {
        mat(i, j) = Min[i*M+j];
    }
    }
    Vector vec(M);
    for(int i=0; i<M; i++) vec(i) = Vin[i];

  gettimeofday(&start, NULL);
  for (int iter = 0; iter < ITERS; iter++) {
    stack.new_recording();
    adouble resa = matvec(mat, vec);
    resa.set_gradient(1.0);
    stack.reverse();
    stack.pause_recording();
    for (int i = 0; i < N; i++) {
      for (int j = 0; j < M; j++) {
        mat(i,j) -= mat(i,j).get_gradient()*RATE;
      }
    }
    stack.continue_recording();
  }
  gettimeofday(&end, NULL);
#if 0
    stack.new_recording();
     gettimeofday(&start, NULL);
  adouble resa = matvec(mat, vec);
  res2 = resa.value();

    resa.set_gradient(1.0);
    stack.reverse();
    gettimeofday(&end, NULL);
    stack.pause_recording();
    for (int i = 0; i < N; i++) {
      for (int j = 0; j < M; j++) {
        Mout[i*M+j] = mat(i,j).get_gradient();
      }
    }
#endif
    //for(int i=0; i<M; i++) Vout[i] = vec(i).get_gradient();

    stack.pause_recording();
    for (int i = 0; i < N; i++) {
      for (int j = 0; j < M; j++) {
        Mout[i*M+j] = mat(i,j).get_gradient();
      }
    }

  }
  //gettimeofday(&end, NULL);
  printf("%0.6f res'=%f %f %f\n", tdiff(&start, &end), Mout[1], Mout[2], Mout[3]);
  }
}
#endif

static void tapenade_sincos(double *Min, double *Mout, double *Vin, double *Vout) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = matvec_real(Min, Vin);

  gettimeofday(&end, NULL);
  printf("tapenade %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  double tmp = Min[0];
  gettimeofday(&start, NULL);

  double sum = 0;
  for(int i=0; i<ITERS; i++) {
      Min[0] = tmp + i/100000000.;
        sum += matvec_real(Min, Vin);
  }

  gettimeofday(&end, NULL);
  printf("tapenade mv %0.6f res=%f\n", tdiff(&start, &end), sum);
  Min[0] = tmp;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);
  double res2;

  for(int i=0; i<ITERS; i++) {
  for(int i=0; i<N*M; i++) { Mout[i] = 0; }
  //for(int i=0; i<M; i++) { Vout[i] = 0; }
    matvec_real_b(Min, Mout, Vin, 1.0);
  //res2 = __builtin_autodiff(matvec_real, Min, Mout, Vin, Vout);
  for(int i=0; i<N*M; i++) { Min[i] -= Mout[i] * RATE; }
  }

  gettimeofday(&end, NULL);
  printf("tapenade %0.6f res'=%f %f %f\n", tdiff(&start, &end), Mout[1], Mout[2], Mout[3]);
  }
}
static void enzyme_sincos(double *Min, double *Mout, double *Vin, double *Vout) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = matvec_real(Min, Vin);

  gettimeofday(&end, NULL);
  printf("%0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  double tmp = Min[0];
  gettimeofday(&start, NULL);

  double sum = 0;
  for(int i=0; i<ITERS; i++) {
      Min[0] = tmp + i/100000000.;
        sum += matvec_real(Min, Vin);
  }

  gettimeofday(&end, NULL);
  printf("mv %0.6f res=%f\n", tdiff(&start, &end), sum);
  Min[0] = tmp;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);
  double res2;

  for(int i=0; i<ITERS; i++) {
  for(int i=0; i<N*M; i++) { Mout[i] = 0; }
  //for(int i=0; i<M; i++) { Vout[i] = 0; }
  res2 = __enzyme_autodiff<double>(matvec_real, Min, Mout, enzyme_const, Vin);
  //res2 = __builtin_autodiff(matvec_real, Min, Mout, Vin, Vout);
  for(int i=0; i<N*M; i++) { Min[i] -= Mout[i] * RATE; }
  }

  gettimeofday(&end, NULL);
  printf("%0.6f res'=%f %f %f\n", tdiff(&start, &end), Mout[1], Mout[2], Mout[3]);
  }
}

int main(int argc, char** argv) {

  double *Min = new double[N*M];
  double *Mout = new double[N*M];
  double *Vin = new double[M];
  double *Vout = new double[M];

  for(int i=0; i<N*M; i++) Min[i] = 3*i;
  for(int i=0; i<M; i++) Vin[i] = 1*i;

  memset(Mout, 0, sizeof(double)*N*M);
  memset(Vout, 0, sizeof(double)*M);
  adept_sincos(Min, Mout, Vin, Vout);

  memset(Mout, 0, sizeof(double)*N*M);
  memset(Vout, 0, sizeof(double)*M);
  tapenade_sincos(Min, Mout, Vin, Vout);

  memset(Mout, 0, sizeof(double)*N*M);
  memset(Vout, 0, sizeof(double)*M);
  enzyme_sincos(Min, Mout, Vin, Vout);
}


/*
 * nr_rk4_phase_lattice.cpp
 *
 * Copyright 2009-2012 Karsten Ahnert
 * Copyright 2009-2012 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/array.hpp>

#include "rk_performance_test_case.hpp"

#include "phase_lattice.hpp"

const size_t dim = 1024;

typedef boost::array< double , dim > state_type;

template< class System , typename T , size_t dim >
void rk4_step( const System sys , boost::array< T , dim > &x , const double t , const double dt )
{   // fast rk4 implementation adapted from the book 'Numerical Recipes'
    size_t i;
    const double hh = dt*0.5;
    const double h6 = dt/6.0;
    const double th = t+hh;
    boost::array< T , dim > dydx , dym , dyt , yt;

    sys( x , dydx , t );

    for( i=0 ; i<dim ; i++ )
        yt[i] = x[i] + hh*dydx[i];

    sys( yt , dyt , th );
    for( i=0 ; i<dim ; i++ )
        yt[i] = x[i] + hh*dyt[i];

    sys( yt , dym , th );
    for( i=0 ; i<dim ; i++ ) {
        yt[i] = x[i] + dt*dym[i];
        dym[i] += dyt[i];
    }
    sys( yt , dyt , t+dt );
    for( i=0 ; i<dim ; i++ )
        x[i] += h6*( dydx[i] + dyt[i] + 2.0*dym[i] );
}

class nr_wrapper
{
public:
    void reset_init_cond()
    {
        for( size_t i = 0 ; i<dim ; ++i )
            m_x[i] = 2.0*3.1415927*rand() / RAND_MAX;
        m_t = 0.0;
    }

    inline void do_step( const double dt )
    {
        rk4_step( phase_lattice<dim>() , m_x , m_t , dt );
    }

    double state( const size_t i ) const
    { return m_x[i]; }

private:
    state_type m_x;
    double m_t;
};

int main()
{
    srand( 12312354 );

    nr_wrapper stepper;

    run( stepper , 10000 , 1E-6 );
}


//*****************************************************************************
//*****************************************************************************
// Class: CAprocerosLeucopoda
//
//
// Description: the CAprocerosLeucopoda represents a group of TZZ insect. scale by m_ScaleFactor
//*****************************************************************************
// 17/10/2020   Rémi Saint-Amant    Creation
//*****************************************************************************

#include "ALeucopodaEquations.h"
#include "ALeucopoda.h"
#include <boost/math/distributions/weibull.hpp>
#include <boost/math/distributions/logistic.hpp>

using namespace std;
using namespace WBSF::HOURLY_DATA;
using namespace WBSF::TZZ;

namespace WBSF
{

	//*********************************************************************************
	//CAprocerosLeucopoda class

	//*****************************************************************************
	// Object creator
	//
	// Input: See CIndividual creator
	//
	// Note: m_RDR (relative Development Rate)  member is init with random values.
	//*****************************************************************************
	CAprocerosLeucopoda::CAprocerosLeucopoda(CHost* pHost, CTRef creationDate, double age, TSex sex, bool bFertil, size_t generation, double scaleFactor) :
		CIndividual(pHost, creationDate, age, sex, bFertil, generation, scaleFactor)
	{
		//reset creation date
		int year = creationDate.GetYear();
		//m_creationDate = GetCreationDate(year);
		m_adult_emergence = GetAdultEmergence(year);
		m_bDiapause = age==PUPA;

		for (size_t s = 0; s < NB_STAGES; s++)
			m_RDR[s] = Equations().GetRelativeDevRate(s);

		//m_adult_longevity = Equations().GetAdultLongevity(m_sex);
		m_Fi = Equations().GetFecondity();
		m_bDeadByAttrition = false;
	}

	CTRef CAprocerosLeucopoda::GetCreationDate(int year)const
	{
		CTRef creationDate = CTRef(year, JANUARY, DAY_01);
		return creationDate;

		//double creationCDD = Equations().GetCreationCDD();

		//const CWeatherStation& weather_station = GetStand()->GetModel()->m_weather;
		/*CTRef begin = CTRef(year, JANUARY, DAY_01);

		CTRef end = CTRef(year, JUNE, DAY_30);

		double CDD = 0;
		for (CTRef TRef = begin; TRef <= end && !creationDate.IsInit(); TRef++)
		{
			const CWeatherDay& wDay = weather_station.GetDay(TRef);
			double DD = GetStand()->m_DD.GetDD(wDay);
			CDD += DD;
			if (CDD >= creationCDD)
			{
				creationDate = wDay.GetTRef();
			}
		}

		ASSERT(creationDate.IsInit());

		return creationDate;*/
	}

	CTRef CAprocerosLeucopoda::GetAdultEmergence(int year)const
	{
		const CWeatherStation& weather_station = GetStand()->GetModel()->m_weather;
		CTPeriod p = weather_station[year].GetEntireTPeriod(CTM::DAILY);

		CTRef adult_emergence;
		double adult_emerging_CDD = Equations().GetAdultEmergingCDD();

		CTRef begin = p.Begin();
		//CTRef begin = GetStand()->m_diapause_end;
		CTRef end = p.End();
		//if (weather_station[year].HaveNext())
			//end = min(p.End(), CTRef(begin.GetYear() + 1, JUNE, DAY_30));

		//double CDD = 0;

		static const CDegreeDays::TDailyMethod DD_METHOD = CDegreeDays::ALLEN_WAVE;
		CDegreeDays DDmodel(DD_METHOD, GetStand()->m_equations.m_EAS[Τᴴ¹], GetStand()->m_equations.m_EAS[Τᴴ²]);
		CModelStatVector GDD;
		DDmodel.Execute(weather_station[year], GDD);

		double CDD = 0;
		for (CTRef TRef = begin; TRef <= end && !adult_emergence.IsInit(); TRef++)
		{
			//const CWeatherDay& wday = weather_station.GetDay(TRef);
			//double T = wday[H_TNTX][MEAN];
			//T = CAprocerosLeucopoda::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);

			//double DD = max(0.0, T - Equations().m_EAS[Τᴴ]);
			double DD = GDD[TRef][CDegreeDays::S_DD];
			CDD += DD;
			if (CDD >= adult_emerging_CDD)
			{
				adult_emergence = TRef;
			}
		}

		return adult_emergence;
	}

	CAprocerosLeucopoda& CAprocerosLeucopoda::operator=(const CAprocerosLeucopoda& in)
	{
		if (&in != this)
		{
			CIndividual::operator=(in);

			m_adult_emergence = in.m_adult_emergence;
			m_bDiapause = in.m_bDiapause;
			m_bDeadByAttrition = in.m_bDeadByAttrition;

			for (size_t s = 0; s < NB_STAGES; s++)
				m_RDR[s] = Equations().GetRelativeDevRate(s);

			m_Fi = Equations().GetFecondity();;
		}

		return *this;
	}

	//destructor
	CAprocerosLeucopoda::~CAprocerosLeucopoda(void)
	{}

	void CAprocerosLeucopoda::OnNewDay(const CWeatherDay& weather)
	{
		CIndividual::OnNewDay(weather);

		/*if (weather.GetTRef() == m_creationDate)
		{
			m_age = EGG;
		}*/
	}

	//*****************************************************************************
	// Develops all stages for one time step
	// Input:	weather: weather of the hour
	//			timeStep: timeStep [h]
	//*****************************************************************************
	void CAprocerosLeucopoda::Live(const CHourlyData& weather, size_t timeStep)
	{
		assert(IsAlive());
		assert(m_status == HEALTHY);

//		if (m_dropToGroundDate.IsInit())
	//		return;

		CTZZHost* pHost = GetHost();
		CTZZStand* pStand = GetStand();

		double nb_steps = (24.0 / timeStep);
		size_t h = weather.GetTRef().GetHour();
		size_t s = GetStage();

		double T = weather[H_TAIR];
		//T = AdjustTLab(weather.GetWeatherStation()->m_name, s, weather.GetTRef(), T);

		double day_length = weather.GetLocation().GetDayLength(weather.GetTRef()) / 3600.0;//[h]

			//Time step development rate
		double r = Equations().GetRate(s, T) / nb_steps;

		//double corr_r = (s == EGG || s == LARVAE) ? : 1;

		//Relative development rate for this individual
		double rr = m_RDR[s];

		//Time step development rate for this individual
		r *= rr;
		ASSERT(r >= 0 && r < 1);

		//Adjust age
		m_age += r;

		//if (!m_dropToGroundDate.IsInit() && m_age > LARVAE4 + 0.9)//drop to the soil when 90% competed (guess)
			//m_dropToGroundDate = weather.GetTRef().as(CTM::DAILY);

		//evaluate attrition once a day
		if (GetStand()->m_bApplyAttrition)
		{
			if (IsDeadByAttrition(s, T, r))
				m_bDeadByAttrition = true;
		}

		if (!m_adult_emergence.IsInit() && m_age >= ADULT)
			m_adult_emergence = weather.GetTRef().as(CTM::DAILY);

		/*else if (s == AESTIVAL_DIAPAUSE_ADULT)
		{
			CTRef TRef = weather.GetTRef().as(CTM::DAILY);
			if (TRef == m_adult_emergence)
				m_age = ACTIVE_ADULT;
		}*/
		//else//ACTIVE_ADULT
		//{
			//double r = (1.0 / m_adult_longevity) / nb_steps;
			//ASSERT(r >= 0 && r < 1);

		//	m_age += r;
		//}/
	}

	//*****************************************************************************
	// Develops all stages, including adults
	// Input:	weather: the weather of the day
	//*****************************************************************************
	void CAprocerosLeucopoda::Live(const CWeatherDay& weather)
	{
		CIndividual::Live(weather);

		ASSERT(IsCreated(weather.GetTRef()));

		//if (!IsCreated(weather.GetTRef()))
			//return;

		if (m_bDiapause && !m_dropToGroundDate.IsInit() && weather.GetTRef() >= m_adult_emergence)
		{
			m_bDiapause = false;
			m_age = ADULT;
		}

		size_t nbSteps = GetTimeStep().NbSteps();
		for (size_t step = 0; step < nbSteps&&IsAlive() && m_age < DEAD_ADULT && !m_bDiapause; step++)
		{
			size_t h = step * GetTimeStep();
			Live(weather[h], GetTimeStep());

			if (GetStage() == PUPA && HasChangedStage() && weather.GetTRef().GetJDay() >= 260)
			{
				m_dropToGroundDate = weather.GetTRef();
				m_bDiapause = true;
			}
		}

		if (HasChangedStage())
			m_reachDate[GetStage()] = weather.GetTRef();

	}

	void CAprocerosLeucopoda::Brood(const CWeatherDay& weather)
	{
		assert(/*IsAlive() &&*/ m_sex == FEMALE);

		if (GetStage() == ADULT)
		{
			//no brood process done

			size_t nb_days = weather.GetTRef() - m_reachDate[ADULT];
			double brood = Equations().GetBrood(m_Fi, weather[H_TNTX], nb_days, 1);
			//brooding
			m_broods = brood;
			m_totalBroods += brood;
			//m_F = 0;
		//}

			if (m_bFertil && m_broods > 0)
			{
				ASSERT(m_age >= ADULT);
				CTZZStand* pStand = GetStand(); ASSERT(pStand);

				double gSurvival = 1;// GetStand()->m_bApplyAttrition ? pStand->m_generationSurvival : 1;//100% of survival by default
				double scaleFactor = m_broods * m_scaleFactor*gSurvival;
				CIndividualPtr object = make_shared<CAprocerosLeucopoda>(m_pHost, weather.GetTRef(), EGG, FEMALE, true, m_generation + 1, scaleFactor);
				m_pHost->push_front(object);
			}
		}
	}

	// kills by old age and frost
	// Output:  Individual's state is updated to follow update
	void CAprocerosLeucopoda::Die(const CWeatherDay& weather)
	{
		//attrition mortality. Killed at the end of time step

		if (m_bDeadByAttrition)
		{
			m_status = DEAD;
			m_death = ATTRITION;
		}
		else if (GetStage() == DEAD_ADULT)
		{
			//Old age
			m_status = DEAD;
			m_death = OLD_AGE;
		}
		else
		{
			size_t s = GetStage();

			//Preliminary assessment of the cold tolerance of Laricobius Osakensis, a winter - active predator of the hemlock woolly adelgid from western canada
			//Leland M.Humble
			static const double COLD_TOLERENCE_T[NB_STAGES] = { -99, -99, -99, -99.0, -99.0 };
			//Toland:L. Osakensis was -13.6 oC (± 0.5) with temperatures that ranged from -6 oC to -21 oC.
			if (weather[H_TMIN][MEAN] < COLD_TOLERENCE_T[s])
			{
				m_status = DEAD;
				m_death = FROZEN;
			}
		}
	}

	//s: stage
	//T: temperature for this time step
	//r: devlopement rate for this time step
	bool CAprocerosLeucopoda::IsDeadByAttrition(size_t s, double T, double r)const
	{
		bool bDeath = false;

		//daily survival
		double ds = GetStand()->m_equations.GetDailySurvivalRate(s, T);

		//time step survival
		double S = pow(ds, r);

		//Computes attrition (probability of survival in a given time step, based on development rate)
		if (RandomGenerator().RandUniform() > S)
			bDeath = true;

		return bDeath;
	}

	//*****************************************************************************
	// GetStat gather information of this object
	//
	// Input: stat: the statistic object
	// Output: The stat is modified
	//*****************************************************************************
	void CAprocerosLeucopoda::GetStat(CTRef d, CModelStat& stat)
	{
		if (IsCreated(d))
		{
			size_t s = GetStage();
			ASSERT(s <= DEAD_ADULT);

			if (IsAlive() || (s == DEAD_ADULT))
				stat[S_EGG + s] += m_scaleFactor;

			if (m_status == DEAD && m_death == ATTRITION)
				stat[S_DEAD_ATTRITION] += m_scaleFactor;

			if (HasChangedStage())
				stat[S_M_EGG + s] += m_scaleFactor;

			//if (s == ADULT)
			//{
			stat[S_BROOD] += m_scaleFactor * m_broods;
			//}

			if(m_bDiapause)
				stat[S_DIAPAUSE] += m_scaleFactor;

		}
	}

	void CAprocerosLeucopoda::Pack(const CIndividualPtr& pBug)
	{
		assert(m_sex == pBug->GetSex());

		CAprocerosLeucopoda* in = (CAprocerosLeucopoda*)(pBug.get());
		CIndividual::Pack(pBug);
	}

	double CAprocerosLeucopoda::GetInstar(bool includeLast)const
	{
		return (IsAlive() || m_death == OLD_AGE) ? GetStage() : CBioSIMModelBase::VMISS;
	}

	//*********************************************************************************************************************

	//*********************************************************************************
	//CTZZHost

	CTZZHost::CTZZHost(CStand* pStand) :
		CHost(pStand)
	{
	}

	void CTZZHost::Live(const CWeatherDay& weather)
	{
		CHost::Live(weather);
	}

	void CTZZHost::GetStat(CTRef d, CModelStat& stat, size_t generation)
	{
		CHost::GetStat(d, stat, generation);
	}

	//*************************************************
	//CTZZStand

	void CTZZStand::init(int year, const CWeatherYears& weather)
	{
//		m_diapause_end = ComputeDiapauseEnd(weather[year]);
	}

	CTRef CTZZStand::ComputeDiapauseEnd(const CWeatherYear& weather)const
	{
		CTPeriod p = weather.GetEntireTPeriod(CTM::DAILY);

		double sumDD = 0;

		for (size_t ii = (172 - 1); ii <= (m_equations.m_EWD[ʎ0] - 1); ii++)
		{
			CTRef TRef = p.Begin() + ii;
			const CWeatherDay& wday = weather.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];
			T = max(m_equations.m_EWD[ʎa], T);

			double DD = min(0.0, T - m_equations.m_EWD[ʎb]);//DD is negative
			sumDD += DD;
		}

		boost::math::logistic_distribution<double> begin_dist(m_equations.m_EWD[ʎ2], m_equations.m_EWD[ʎ3]);
		int begin = (int)Round((m_equations.m_EWD[ʎ0] - 1) + m_equations.m_EWD[ʎ1] * cdf(begin_dist, sumDD), 0);

		return p.Begin() + begin;
	}

	void CTZZStand::GetStat(CTRef d, CModelStat& stat, size_t generation)
	{
		CStand::GetStat(d, stat, generation);

//		const CWeatherStation& weather_station = GetModel()->m_weather;
//		const CWeatherDay& wday = weather_station.GetDay(d);
//
//		//use year of diapause to compute correctly the adult emergence cdd
//		int year = m_diapause_end.GetYear();
//		CTRef begin = CTRef(year, JANUARY, DAY_01);
//		CTRef end = CTRef(year, DECEMBER, DAY_31);
//
//		if (d >= begin && d <= end)
//		{
//			//Egg creation DD (allen 1976)
//			//m_egg_creation_CDD += m_DD.GetDD(wday);
////			stat[S_EGG_CREATION_CDD] = m_egg_creation_CDD;
//
//			//diapause end negative DD
//			double T = wday[H_TNTX][MEAN];
//			T = max(m_equations.m_EWD[ʎa], T);
//			double NDD = min(0.0, T - m_equations.m_EWD[ʎb]);//DD is negative
//
//			int ii = d - begin;
//			if (ii >= (172 - 1) && ii <= int(m_equations.m_EWD[ʎ0] - 1))
//				m_diapause_end_NCDD += NDD;
//
//			//stat[S_DIAPAUSE_END_NCDD] = m_diapause_end_NCDD;
//		}
//

		//begin = m_diapause_end;
		//end = CTRef(m_diapause_end.GetYear() + 1, MARCH, DAY_01);
		//if (d >= begin && d <= end)
		//{
		//	//adult emergence (growing DD)
		//	double T = wday[H_TNTX][MEAN];
		//	double GDD = max(0.0, T - m_equations.m_EAS[Τᴴ]);
		//	m_adult_emergence_CDD += GDD;
		//	stat[S_ADULT_EMERGENCE_CDD] = m_adult_emergence_CDD;
		//}
	}

}


#include <cmath>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include "distributions.hpp"
#include "fastmath.hpp"

static float sq(float x)
{
    return x * x;
}

#if 0
static double cb(double x)
{
    return x * x * x;
}
#endif

void GammaLogPdf::set_alpha(float alpha)
{
    _alpha = alpha;
    lgamma_alpha = lgammaf(alpha);
}

void GammaLogPdf::set_beta(float beta)
{
    _beta = beta;
    log_beta = fastlog(beta);
}

float GammaLogPdf::f() const
{
    return _alpha * log_beta - lgamma_alpha - _beta * _x + (_alpha - 1.0f) * log_x;
}

float GammaLogPdf::x(float x)
{
    _x = x;
    log_x = fastlog(x);
    return f();
}

void GammaLogPdfDx::set_alpha(float alpha)
{
    _alpha = alpha;
}

void GammaLogPdfDx::set_beta(float beta)
{
    _beta = beta;
}

float GammaLogPdfDx::f() const
{
    return (_alpha - 1.0f) / _x - _beta;
}

float GammaLogPdfDx::x(float x)
{
    _x = x;
    return f();
}

void GammaLogPdfDBeta::set_alpha(float alpha)
{
    _alpha = alpha;
}

void GammaLogPdfDBeta::set_beta(float beta)
{
    _beta = beta;
}

float GammaLogPdfDBeta::f() const
{
    return _alpha / _beta - _x;
}

float GammaLogPdfDBeta::x(float x)
{
    _x = x;
    return f();
}

float AltGammaLogPdf::f() const
{
    return -(lgamma_shape + _shape * log_scale) + (_shape - 1.0f) * logx - _x / scale;
}

float AltGammaLogPdf::x(float x)
{
    _x = x;
    logx = fastlog(x);
    return f();
}

float AltGammaLogPdf::mean(float mean)
{
    _mean = mean;
    scale = _mean / _shape;
    log_scale = fastlog(scale);
    return f();
}

void AltGammaLogPdf::set_mean(float mean)
{
    _mean = mean;
    scale = _mean / _shape;
    log_scale = fastlog(scale);
}

float AltGammaLogPdf::mean_x(float mean, float x)
{
    _mean = mean;
    _x = x;
    logx = fastlog(x);
    scale = _mean / _shape;
    log_scale = fastlog(scale);
    return f();
}

float AltGammaLogPdf::shape(float shape)
{
    _shape = shape;
    scale = _mean / _shape;
    lgamma_shape = lgammaf(shape);
    log_scale = fastlog(scale);
    return f();
}

void AltGammaLogPdf::set_shape(float shape)
{
    _shape = shape;
    scale = _mean / _shape;
    lgamma_shape = lgammaf(shape);
    log_scale = fastlog(scale);
}

float AltGammaLogPdfDx::f() const
{
    return (_shape - 1.0f) / _x - 1.0f / scale;
}

float AltGammaLogPdfDx::x(float x)
{
    _x = x;
    return f();
}

float AltGammaLogPdfDx::mean(float mean)
{
    _mean = mean;
    scale = _mean / _shape;
    return f();
}

void AltGammaLogPdfDx::set_mean(float mean)
{
    _mean = mean;
    scale = _mean / _shape;
}

float AltGammaLogPdfDx::shape(float shape)
{
    _shape = shape;
    scale = _mean / _shape;
    return f();
}

void AltGammaLogPdfDx::set_shape(float shape)
{
    _shape = shape;
    scale = _mean / _shape;
}

float AltGammaLogPdfDMean::f() const
{
    return _x * _shape / sq(_mean) - _shape / _mean;
}

float AltGammaLogPdfDMean::x(float x)
{
    _x = x;
    return f();
}

float AltGammaLogPdfDMean::mean(float mean)
{
    _mean = mean;
    scale = _mean / _shape;
    return f();
}

float AltGammaLogPdfDMean::shape(float shape)
{
    _shape = shape;
    scale = _mean / _shape;
    return f();
}

float AltGammaLogPdfDShape::f() const
{
    return logx - _x / _mean - digamma_shape + log_scale * (_mean/sq(_shape));
}

float AltGammaLogPdfDShape::x(float x)
{
    _x = x;
    logx = fastlog(x);
    return f();
}

float AltGammaLogPdfDShape::mean(float mean)
{
    _mean = mean;
    scale = _mean / _shape;
    log_scale = fastlog(scale);
    return f();
}

float AltGammaLogPdfDShape::mean_x(float mean, float x)
{
    _x = x;
    logx = fastlog(x);
    _mean = mean;
    scale = _mean / _shape;
    log_scale = fastlog(scale);
    return f();
}

float AltGammaLogPdfDShape::shape(float shape)
{
    _shape = shape;
    scale = _mean / _shape;
    log_scale = fastlog(scale);
    digamma_shape = boost::math::digamma(shape);
    return f();
}

void AltGammaLogPdfDShape::set_shape(float shape)
{
    _shape = shape;
    scale = _mean / _shape;
    log_scale = fastlog(scale);
    digamma_shape = boost::math::digamma(shape);
}

float PoissonLogPdf::f() const
{
    return _k * log_lambda - log_factorial_k - _lambda;
}

float PoissonLogPdf::k(unsigned int k)
{
    _k = k;
    log_factorial_k = lgammaf(k + 1);
    return f();
}

void PoissonLogPdf::set_k(unsigned int k)
{
    _k = k;
    log_factorial_k = lgammaf(k + 1);
}

float PoissonLogPdf::lambda(float lambda)
{
    _lambda = lambda;
    log_lambda = fastlog(lambda);
    return f();
}

float PoissonLogPdfDLambda::k(unsigned int k)
{
    _k = k;
    return _k / _lambda - 1.0f;
}

void PoissonLogPdfDLambda::set_k(unsigned int k)
{
    _k = k;
}

float PoissonLogPdfDLambda::lambda(float lambda)
{
    _lambda = lambda;
    return _k / _lambda - 1.0f;
}

void StudentsTLogPdf::set_nu(float nu)
{
    _nu = nu;
    nu_term = lgammaf((nu + 1.0f) / 2.0f) - lgammaf(nu / 2.0f);
    nu_sigma_term = - fastlog(sqrtf(nu * M_PI) * _sigma);
}

void StudentsTLogPdf::set_mu(float mu)
{
    _mu = mu;
}

void StudentsTLogPdf::set_sigma(float sigma)
{
    _sigma = sigma;
    nu_sigma_term = -fastlog(sqrtf(_nu * M_PI) * _sigma);
}

float StudentsTLogPdf::f() const
{
    return nu_term + nu_sigma_term -
        ((_nu + 1.0f) / 2.0f) * log1pf(sq((_x - _mu) / _sigma) / _nu);
}

float StudentsTLogPdf::x(float x)
{
    _x = x;
    return f();
}

void StudentsTLogPdfDx::set_nu(float nu)
{
    _nu = nu;
}

void StudentsTLogPdfDx::set_mu(float mu)
{
    _mu = mu;
}

void StudentsTLogPdfDx::set_sigma(float sigma)
{
    _sigma = sigma;
}

float StudentsTLogPdfDx::f() const
{
    float part = (2.0f * (_x - _mu) / sq(_sigma) / _nu) /
        (1.0f + sq((_x - _mu) / _sigma) / _nu);
    return -((_nu + 1.0f) / 2.0f) * part;
}

float StudentsTLogPdfDx::x(float x)
{
    _x = x;
    return f();
}

void StudentsTLogPdfDMu::set_nu(float nu)
{
    _nu = nu;
}

void StudentsTLogPdfDMu::set_mu(float mu)
{
    _mu = mu;
}

void StudentsTLogPdfDMu::set_sigma(float sigma)
{
    _sigma = sigma;
}

float StudentsTLogPdfDMu::f() const
{
    float part = (2.0f * (_x - _mu) / sq(_sigma) / _nu) /
        (1.0f + sq((_x - _mu) / _sigma) / _nu);
    return ((_nu + 1.0f) / 2.0f) * part;
}

float StudentsTLogPdfDMu::x(float x)
{
    _x = x;
    return f();
}


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

#ifndef METRO_LIKELIHOOD_MULTIVARIATE_T_HPP
#define METRO_LIKELIHOOD_MULTIVARIATE_T_HPP

#include <cmath>
#include <boost/math/special_functions/gamma.hpp>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Cholesky>
#include "metro/IndependentObservationLogLikelihood.hpp"
#include "metro/DataRange.hpp"
#include "metro/DataSubset.hpp"

// #define DEBUG_MULTIVARIATE_T 1

namespace metro {
	namespace likelihood {
		template< typename Scalar, typename Vector, typename Matrix >
		struct MultivariateT: public metro::IndependentObservationLogLikelihood< Scalar, Vector, Matrix > {
		public:
			typedef std::auto_ptr< MultivariateT > UniquePtr ;
			typedef typename Vector::SegmentReturnType Segment ;
			typedef typename Vector::ConstSegmentReturnType ConstSegment ;
			typedef typename Eigen::Ref< Matrix > MatrixRef ;
			typedef typename Eigen::Block< Matrix > Block ;
		public:
			MultivariateT( double const degrees_of_freedom ):
				m_pi( 3.141592653589793238462643383279502884 ),
				m_nu( degrees_of_freedom ),
				m_data( 0 ),
				m_kappa( 0 )
			{
			}

			MultivariateT( Matrix const& data, double const degrees_of_freedom ):
				m_pi( 3.141592653589793238462643383279502884 ),
				m_nu( degrees_of_freedom ),
				m_data( 0 ),
				m_kappa( 0 )
			{
				set_data( data ) ;
			}

			MultivariateT( Matrix const& data, Vector const& weights, double const degrees_of_freedom ):
				m_pi( 3.141592653589793238462643383279502884 ),
				m_nu( degrees_of_freedom ),
				m_data( 0 ),
				m_kappa( 0 )
			{
				set_data( data, weights ) ;
			}

			int p() const { return m_p ; }
			double nu() const { return m_nu ; }
			double degrees_of_freedom() const { return m_nu ; }
			Vector parameters() const { return m_parameters ; }
			Vector const& mean() const { return m_mean ; }
			Matrix const& sigma() const { return m_sigma ; }
			Matrix const& data() const { return m_data ; }

			std::string get_spec() const { return "MultivariateT" ; }

			// Behaves as if calling set_data() (below) with unit weights.
			void set_data(
				Matrix const& data
			) {
				set_data( data, Vector::Constant( data.rows(), 1 )) ;
			}

			// Set data to a give matrix with specified weights.
			// If data.cols() == p() then parameters are left unchanged
			// and the likelihood will be re-evaluated.
			// Otherwise, if data.cols() != p() then parameters will be left undefined.
			// In both cases, you must call evaluate_at() before evaluating the likelihood.
			void set_data(
				Matrix const& data,
				Vector const& weights
			) {
				m_data = &data ;
				assert( weights.size() == m_data->rows() ) ;
				m_weights = weights ;
				if( m_p != m_data->cols() ) {
					m_p = m_data->cols() ;
					m_kappa = compute_constant_terms( m_nu, m_p ) ;
					m_parameters = Vector::Zero( m_p + ( m_p * ( m_p + 1 ) / 2 ) ) ;
				}
			}

			// Evaluate at a given set of parameters, specified as a single vector.
			// Parameters are taken in order as the p entries of the mean vector,
			// followed by the p(p+1)/2 entries of the lower triangle of the
			// variance-covariance matrix taken in column-major order.
			void evaluate_at( Vector const& parameters ) {
				evaluate_at( parameters, DataRange( 0, m_data->rows() )) ;
			}

			// Evaluate at a given set of parameters on a specified subset of the data.
			// Parameters are taken in order as the p entries of the mean vector,
			// followed by the p(p+1)/2 entries of the lower triangle of the
			// variance-covariance matrix taken in column-major order.
			void evaluate_at( Vector const& parameters, DataSubset const& data_subset ) {
				assert( parameters.size() == m_parameters.size() ) ;
				assert( m_data ) ;
				unpack_parameters( parameters, &m_mean, &m_sigma ) ;
				evaluate_at( m_mean, m_sigma, data_subset ) ;
			}

			// Evaluate at a given set of parameters, specified as a mean vector
			// and variance-covariance matrix.  Only the lower triangle of the
			// variance-covariance matrix is used by this class.
			void evaluate_at( Vector const& mean, Matrix const& sigma ) {
				evaluate_at( mean, sigma, DataRange( 0, m_data->rows() )) ;
			}

			// Evaluate at a given set of parameters, on a subset of data.
			// Parameters are specified as a mean vector
			// and variance-covariance matrix.  Only the lower triangle of the
			// variance-covariance matrix is used by this class.
			void evaluate_at( Vector const& mean, Matrix const& sigma, DataSubset const& data_subset ) {
				assert( mean.size() == m_data->cols() ) ;
				assert( sigma.rows() == sigma.cols() ) ;
				assert( sigma.rows() == m_data->cols() ) ;

				m_mean = mean ;
				m_sigma = sigma ;
				m_data_subset = data_subset ;

				// Pack parameters into the parameter vector
				pack_parameters( mean, m_sigma, &m_parameters ) ;

				// We compute up-front some quantities that are useful
				// when computing the log-likelihood.
				m_ldlt.compute( m_sigma ) ;
				m_log_determinant = m_ldlt.vectorD().array().log().sum() ;
				m_mean_centred_data = m_data->rowwise() - m_mean.transpose() ;
				// Z is a vector of the terms ( x_i - mu )^t Sigma^-1 ( x_i - mu ).
				m_A = m_ldlt.solve( m_mean_centred_data.transpose() ) ;
				m_Z = (
					m_mean_centred_data.array() * m_A.transpose().array()
				).rowwise().sum() ;

#if DEBUG_MULTIVARIATE_T
					std::cerr << "metro::likelihood::MultivariateT::evaluate_at():\n"
						<< " m_mean " << m_mean.transpose() << "\n"
						<< " m_sigma = \n" << m_sigma << "\n"
						<< " m_log_determinant = " << m_log_determinant << "\n" ;
#endif
			}

			// Get the value of the log-likelihood for the given data,
			// and parameters and data subset passed to evaluate_at().
			// You must call evaluate_at() before using this function.
			double get_value_of_function() const {
				Vector terms = Vector::Constant( m_data->rows(), 0 ) ;
				get_terms_of_function( terms ) ;
				return terms.sum() ;
			}

			// Get terms of the log-likelihood corresponding to each
			// weighted data point.
			void get_terms_of_function( MatrixRef result ) const {
				assert( result.rows() == m_data->rows() ) ;
				assert( result.cols() == 1 ) ;
				bool const mvn = m_nu == std::numeric_limits< double >::infinity() ;
				for( std::size_t i = 0; i < m_data_subset.number_of_subranges(); ++i ) {
					DataRange const& range = m_data_subset[i] ;
					ConstSegment const segmentZ = m_Z.segment( range.begin(), range.size() ) ;
					ConstSegment const segmentWeights = m_weights.segment( range.begin(), range.size() ) ;
					Eigen::Block< MatrixRef > resultSegment = result.block( range.begin(), 0, range.size(), 1 ) ;

					if( mvn ) {
						resultSegment = segmentWeights.array() * (
							Vector::Constant( range.size(), m_kappa - 0.5 * m_log_determinant )
							- 0.5 * segmentZ
						).array() ;
					} else {
						resultSegment = segmentWeights.array() * (
							Vector::Constant( range.size(), m_kappa - 0.5 * m_log_determinant ).array()
							-( 0.5 * ( m_nu + m_p ) * (( segmentZ + Vector::Constant( range.size(), m_nu ) ).array().log() ) )
						) ;
					}
#if DEBUG_MULTIVARIATE_T
					std::cerr << "metro::likelihood::MultivariateT::get_terms_of_function():\n"
						<< " Adding " << range << "\n"
						<< " segmentZ = " << segmentZ.transpose() << "\n"
						<< " segmentWeights = " << segmentWeights.transpose() << "\n"
						<< " m_kappa = " << m_kappa << "\n"
						<< " m_log_determinant = " << m_log_determinant << "\n"
						<< " term = " << (( segmentZ + Vector::Constant( range.size(), m_nu ) ).array().log() )
						<< " result = " << result << ".\n" ;
#endif
				}
			}

			// Compute the first derivative of the log-likelihood
			// You must call evaluate_at() before using this method.
			Vector get_value_of_first_derivative() const {
				assert(0) ;
			}

			// Compute the second derivative of the log-likelihood
			// You must call evaluate_at() before using this method.
			Matrix get_value_of_second_derivative() const {
				assert(0) ;
			}

			// Fit multivariate T on the full data using an EM algorithm
			// until the stopping condition becomes satisfied.
			// Algorithm details are from Nadarajah & Kotz, "Estimation methods for the Multivariate t Distribution.", p.103
			// stopping_condition must be callable as stopping_condition( current loglikelihood ),
			// where a return value of true indicates that iteration should stop.
			// This function iterates until stopping_condition() == true, at which point
			// it returns stopping_condition.converged().
			// The postcondition of this function is as though evaluate_at() has been called
			// for the last parameter value visited by the EM iteration (whether converged or not).
			template< typename StoppingCondition >
			bool estimate_by_em(
				StoppingCondition& stopping_condition
			) {
				Matrix regularising_sigma = Matrix::Constant( m_p, m_p, 0 ) ;
				double regularising_weight = 0 ;
				return estimate_by_em(
					DataRange( 0, m_data->rows() ),
					stopping_condition,
					regularising_sigma,
					regularising_weight
				) ;
			}

			// Fit multivariate T on a subset of data using an EM algorithm
			// until the stopping condition becomes satisfied.
			// Algorithm details are from Nadarajah & Kotz, "Estimation methods for the Multivariate t Distribution.", p.103
			// stopping_condition must be callable as stopping_condition( current loglikelihood ),
			// where a return value of true indicates that iteration should stop.
			// This function iterates until stopping_condition() == true, at which point
			// it returns stopping_condition.converged().
			// The postcondition of this function is as though evaluate_at() has been called
			// for the last parameter value visited by the EM iteration (whether converged or not).
			template< typename StoppingCondition >
			bool estimate_by_em(
				DataSubset const& data_subset,
				StoppingCondition& stopping_condition
			) {
				Matrix regularising_sigma = Matrix::Constant( m_p, m_p, 0 ) ;
				double regularising_weight = 0 ;
				return estimate_by_em(
					data_subset,
					stopping_condition,
					regularising_sigma,
					regularising_weight
				) ;
			}

			// Fit multivariate T on the full data using an EM algorithm
			// until the stopping condition becomes satisfied.
			// Algorithm details are from Nadarajah & Kotz, "Estimation methods for the Multivariate t Distribution.", p.103
			// This function includes a regularising variance-covariance matrix, and weight
			// to prevent the fit from becoming degenerate.
			// stopping_condition must be callable as stopping_condition( current loglikelihood ),
			// where a return value of true indicates that iteration should stop.
			// This function iterates until stopping_condition() == true, at which point
			// it returns stopping_condition.converged().
			// The postcondition of this function is as though evaluate_at() has been called
			// for the last parameter value visited by the EM iteration (whether converged or not).
			template< typename StoppingCondition >
			bool estimate_by_em(
				StoppingCondition& stopping_condition,
				Matrix const& regularising_sigma,
				double regularising_weight
			) {
				return estimate_by_em(
					DataRange( 0, m_data->rows() ),
					stopping_condition,
					regularising_sigma,
					regularising_weight
				) ;
			}

			// Fit multivariate T on a subset of data using an EM algorithm
			// until the stopping condition becomes satisfied.
			// Algorithm details are from Nadarajah & Kotz, "Estimation methods for the Multivariate t Distribution.", p.103
			// This function includes a regularising variance-covariance matrix, and weight
			// to prevent the fit from becoming degenerate.
			// stopping_condition must be callable as stopping_condition( current loglikelihood ),
			// where a return value of true indicates that iteration should stop.
			// This function iterates until stopping_condition() == true, at which point
			// it returns stopping_condition.converged().
			// The postcondition of this function is as though evaluate_at() has been called
			// for the last parameter value visited by the EM iteration (whether converged or not).
			template< typename StoppingCondition >
			bool estimate_by_em(
				DataSubset const& data_subset,
				StoppingCondition& stopping_condition,
				Matrix const& regularising_sigma,
				double regularising_weight
			) {
				assert( regularising_sigma.rows() == m_p ) ;
				assert( regularising_sigma.cols() == m_p ) ;
				assert( regularising_weight >= 0.0 ) ;

				if( data_subset.size() == 0 ) {
					// No data.  Do nothing and return false.
					return false ;
				}

				// Start with unit weights, giving MVN estimate
				Vector iterationWeights = Vector::Constant( m_data->rows(), 1 ) ;
				Vector mean = compute_weighted_mean( iterationWeights, m_weights, data_subset ) ;
				Matrix sigma = compute_weighted_regularised_sigma( iterationWeights, m_weights, mean, regularising_sigma, regularising_weight, data_subset ) ;

				evaluate_at( mean, sigma, data_subset ) ;
				double loglikelihood = get_value_of_function() ;

#if DEBUG_MULTIVARIATE_T
				std::cerr << "metro::likelihood::MultivariateT::estimate_by_em(): start: params = "
					<< get_parameters().transpose() << ", ll = " << get_value_of_function() << ".\n" ;
#endif

				// If nu = ∞ we are at the MLE already, so bail out...
				if( m_nu == std::numeric_limits< double >::infinity() ) {
					// Multivariate normal.  Quit right now.
					return true ;
				}

				// ..otherwise let's EM it.
				std::size_t iteration = 0 ;
				while( !stopping_condition( loglikelihood ) ) {
					// compute weights
					// Vector of weights is given as
					// (nu+p) / nu + (x_i-mean)^t R^-1 ( x_i - mean ).
					// Our x_i - mean_i is stored in a single row of m_mean_centred_data.
					m_A = m_ldlt.solve( m_mean_centred_data.transpose() ) ;
					iterationWeights = (
						m_mean_centred_data.array() * m_A.transpose().array()
					).rowwise().sum() ;
					iterationWeights += Vector::Constant( m_data->rows(), m_nu ) ;
					iterationWeights.array() = iterationWeights.array().inverse() * ( m_nu + m_p ) ;

					// compute new parameter estimates
					// these are
					// mean = sum( w_i x_i ) / sum( w_i )
					mean = compute_weighted_mean( iterationWeights, m_weights, data_subset ) ;
					sigma = compute_weighted_regularised_sigma( iterationWeights, m_weights, mean, regularising_sigma, regularising_weight, data_subset ) ;
					evaluate_at( mean, sigma, data_subset ) ;
					loglikelihood = get_value_of_function() ;

#if DEBUG_MULTIVARIATE_T
					std::cerr << "metro::likelihood::MultivariateT::estimate_by_em(): after iteration "
						<< iteration << ": params = " << get_parameters().transpose()
						<< ", ll = " << loglikelihood
						<< ", iterationWeights = " << iterationWeights.head( std::min( iterationWeights.size(), 10l )).transpose() << ".\n" ;
#endif
					++iteration ;
				}
				return stopping_condition.converged() ;
			}

		private:
			double const m_pi ;
			double const m_nu ;
			Matrix const* m_data ;
			DataSubset m_data_subset ;
			Vector m_weights ;
			double m_p ;
			double m_kappa ;
			Vector m_parameters ;
			Matrix m_sigma ;
			Vector m_mean ;

			Eigen::LDLT< Matrix > m_ldlt ;
			double m_log_determinant ;
			Matrix m_mean_centred_data ;
			Matrix m_A ;
			Vector m_Z ;

		private:

			double compute_constant_terms( double const nu, double const p ) const {
				return ( nu == std::numeric_limits< double >::infinity() )
					? (
						-0.5 * p * std::log( 2 * m_pi )
					) : (
						lgamma( ( nu + p ) / 2.0 )
						- ( p / 2 ) * std::log( m_pi * nu )
						- lgamma( nu / 2.0 )
						+ (( nu + p ) * std::log( nu ) / 2.0 )
					)
				;
			}

			void pack_parameters( Vector const& mean, Matrix const& sigma, Vector* result ) const {
				result->resize( mean.size() + ( sigma.rows() * ( sigma.rows() + 1 ) / 2 )) ;
				result->segment( 0, mean.size() ) = mean ;
				int parameter_i = mean.size() ;
				for( int col = 0; col < m_p; ++col ) {
					result->segment( parameter_i, m_p - col ) = sigma.col( col ).segment( col, m_p - col ) ;
					parameter_i += m_p - col ;
				}
			}

			void unpack_parameters( Vector const& parameters, Vector* mean, Matrix* sigma ) const {
				*mean = parameters.head( m_p ) ;
				sigma->resize( m_p, m_p ) ;
				int parameter_i = m_p ;
				for( int col = 0; col < m_data->cols(); ++col ) {
					sigma->col( col ).segment( col, m_p - col ) = parameters.segment( parameter_i, m_p - col ) ;
					parameter_i += m_p - col ;
				}
			}

			Vector compute_weighted_mean(
				Vector const& iterationWeights,
				Vector const& dataWeights,
				DataSubset const& subset
			) const {
				Vector result = Vector::Zero( m_p ) ;
				double total_weight = 0 ;
				for( std::size_t i = 0; i < subset.number_of_subranges(); ++i ) {
					DataRange const& range = subset[i] ;
					ConstSegment segmentIW = iterationWeights.segment( range.begin(), range.size() ) ;
					ConstSegment segmentDW = dataWeights.segment( range.begin(), range.size() ) ;

					result += (
						(
							( segmentIW.array() * segmentDW.array() ).matrix().asDiagonal()
							* m_data->block( range.begin(), 0, range.size(), m_p )
						).colwise().sum()
					).transpose() ;

					total_weight += ( segmentDW.array() * segmentIW.array() ).sum() ;
				}
				result /= total_weight ;
				return result ;
			}

			Matrix compute_weighted_regularised_sigma(
				Vector const& iterationWeights,
				Vector const& dataWeights,
				Vector const& mean,
				Matrix const& regularising_sigma,
				double const regularising_weight,
				DataSubset const& subset
			) const {
				Matrix mean_centred_data = m_data->rowwise() - mean.transpose() ;
				// sigma = 1/N sum ( w_i (x_i-mean)(x_i-mean)^t)
				// we store x_i as a row not a column, so transposes go the opposite way.
				Matrix result = Matrix::Zero( m_p, m_p ) ;
				double n = 0 ;
				for( std::size_t i = 0; i < subset.number_of_subranges(); ++i ) {
					DataRange const& range = subset[i] ;
					ConstSegment segmentIW = iterationWeights.segment( range.begin(), range.size() ) ;
					ConstSegment segmentDW = dataWeights.segment( range.begin(), range.size() ) ;

					result += (
						(
							mean_centred_data.block( range.begin(), 0, range.size(), m_p ).transpose()
							* segmentIW.asDiagonal()
							* segmentDW.asDiagonal()
							* mean_centred_data.block( range.begin(), 0, range.size(), m_p )
						)
					) ;
					n += segmentDW.sum() ;
				}
				result += regularising_weight * regularising_sigma ;
				result /= ( n + regularising_weight ) ;
				return result ;
			}
		} ;
	}
}

#endif