// Copyright (c) 2015-2018, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of gepetto-viewer-corba.
// gepetto-viewer-corba is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// gepetto-viewer-corba is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// gepetto-viewer-corba. If not, see <http://www.gnu.org/licenses/>.

#ifndef __se3_se3_hpp__
#define __se3_se3_hpp__
#include <Eigen/Geometry>

namespace se3
{
/* Type returned by the "se3Action" and "se3ActionInverse" functions. */
namespace internal
{
template<typename D>
struct ActionReturn { typedef D Type; };
}
/** The rigid transform aMb can be seen in two ways:
*
* - given a point p expressed in frame B by its coordinate vector Bp, aMb
* computes its coordinates in frame A by Ap = aMb Bp.
* - aMb displaces a solid S centered at frame A into the solid centered in
* B. In particular, the origin of A is displaced at the origin of B: $^aM_b
* ^aA = ^aB$.
* The rigid displacement is stored as a rotation matrix and translation vector by:
* aMb (x) = aRb*x + aAB
* where aAB is the vector from origin A to origin B expressed in coordinates A.
*/
template<typename _Scalar, int _Options>
class SE3Tpl
{
public:
typedef _Scalar Scalar;
enum { Options = _Options };
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
typedef Eigen::Quaternion<Scalar,Options> Quaternion;
//typedef MotionTpl<Scalar,Options> Motion;
//typedef ForceTpl<Scalar,Options> Force;
//typedef ActionTpl<Scalar,Options> Action;
enum { LINEAR = 0, ANGULAR = 3 };
public:
// Constructors
SE3Tpl() : rot(), trans() {}
template<typename M3,typename v3>
SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p)
: rot(R), trans(p) {}
SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}
template<typename S2, int O2>
SE3Tpl( const SE3Tpl<S2,O2> clone )
: rot(clone.rotation()),trans(clone.translation()) {}
template<typename S2, int O2>
SE3Tpl & operator= (const SE3Tpl<S2,O2> & other)
{
rot = other.rotation ();
trans = other.translation ();
return *this;
}
const Matrix3 & rotation() const { return rot; }
const Vector3 & translation() const { return trans; }
Matrix3 & rotation() { return rot; }
Vector3 & translation() { return trans; }
void rotation(const Matrix3 & R) { rot=R; }
void translation(const Vector3 & p) { trans=p; }
static SE3Tpl Identity()
{
return SE3Tpl(1);
}
static SE3Tpl Random()
{
Eigen::Quaternion<Scalar,Options> q(Vector4::Random());
q.normalize();
return SE3Tpl(q.matrix(),Vector3::Random());
}
Eigen::Matrix<Scalar,4,4,Options> toHomogeneousMatrix() const
{
Eigen::Matrix<Scalar,4,4,Options> M;
M.template block<3,3>(0,0) = rot;
M.template block<3,1>(0,3) = trans;
M.template block<1,3>(3,0).setZero();
M(3,3) = 1;
return M;
}
/// Vb.toVector() = bXa.toMatrix() * Va.toVector()
Matrix6 toActionMatrix() const
{
Matrix6 M;
M.template block<3,3>(ANGULAR,ANGULAR)
= M.template block<3,3>(LINEAR,LINEAR) = rot;
M.template block<3,3>(ANGULAR,LINEAR).setZero();
M.template block<3,3>(LINEAR,ANGULAR)
= skew(trans) * M.template block<3,3>(ANGULAR,ANGULAR);
return M;
}
/// aXb = bXa.inverse()
SE3Tpl inverse() const
{
return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
}
void disp(std::ostream & os) const
{
os << " R =\n" << rot << std::endl
<< " p =\n" << trans.transpose() << std::endl;
}
/* --- GROUP ACTIONS ON M6, F6 and I6 --- */
/// ay = aXb.act(by)
template<typename D> typename internal::ActionReturn<D>::Type act (const D & d) const
{ return d.se3Action(*this); }
/// by = aXb.actInv(ay)
template<typename D> typename internal::ActionReturn<D>::Type actInv(const D & d) const
{ return d.se3ActionInverse(*this); }
Vector3 act (const Vector3& p) const { return (rot*p+trans).eval(); }
Vector3 actInv(const Vector3& p) const { return (rot.transpose()*(p-trans)).eval(); }
SE3Tpl act (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
SE3Tpl actInv (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot,
rot.transpose()*(m2.trans-trans));}
/* --- OPERATORS -------------------------------------------------------- */
operator Matrix4() const { return toHomogeneousMatrix(); }
operator Matrix6() const { return toActionMatrix(); }
SE3Tpl operator*(const SE3Tpl & m2) const { return this->act(m2); }
friend std::ostream & operator << (std::ostream & os,const SE3Tpl & X)
{ X.disp(os); return os; }
public:
private:
Matrix3 rot;
Vector3 trans;
};
typedef SE3Tpl<float,0> SE3;
} // namespace se3
#endif // ifndef __se3_se3_hpp__


#include "CALPHADConcSolverBinary.h"
#include "CALPHADFunctions.h"
#include "CALPHADSpeciesPhaseGibbsEnergy.h"
#include "PhysicalConstants.h"

#include <chrono>

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <iomanip>
#include <iostream>
#include <string>

#include <omp.h>

namespace pt = boost::property_tree;

typedef std::chrono::high_resolution_clock Clock;

int main(int argc, char* argv[])
{
    const int N = 10000000;

#ifdef _OPENMP
    printf("Compiled by an OpenMP-compliant implementation.\n");
#endif

    std::cout << "Run test with " << omp_get_max_threads() << " threads"
              << std::endl;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadAuNi.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    double temperature = 1450.;

    CalphadDataType LmixPhaseL[4][MAX_POL_T_INDEX];
    CalphadDataType LmixPhaseA[4][MAX_POL_T_INDEX];

    {
        std::string dbnamemixL("LmixPhaseL");
        pt::ptree Lmix0_db = calphad_db.get_child(dbnamemixL);
        Thermo4PFM::readLmixBinary(Lmix0_db, LmixPhaseL);
    }
    {
        std::string dbnamemixA("LmixPhaseA");
        pt::ptree Lmix1_db = calphad_db.get_child(dbnamemixA);
        Thermo4PFM::readLmixBinary(Lmix1_db, LmixPhaseA);
    }

    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy g_species_phaseL[2];
    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy g_species_phaseA[2];

    {
        pt::ptree& species0_db = calphad_db.get_child("SpeciesA");
        std::string dbnameL("PhaseL");
        std::string dbnameA("PhaseA");

        g_species_phaseL[0].initialize("L0", species0_db.get_child(dbnameL));
        g_species_phaseA[0].initialize("A0", species0_db.get_child(dbnameA));

        pt::ptree& speciesB_db = calphad_db.get_child("SpeciesB");
        g_species_phaseL[1].initialize("L1", speciesB_db.get_child(dbnameL));
        g_species_phaseA[1].initialize("A1", speciesB_db.get_child(dbnameA));
    }

    CalphadDataType fA[2];
    fA[0] = g_species_phaseL[0].fenergy(temperature);
    fA[1] = g_species_phaseA[0].fenergy(temperature);
    // std::cout<<"fA[0]="<<fA[0]<<", fA[1]="<<fA[1]<<std::endl;

    CalphadDataType fB[2];
    fB[0] = g_species_phaseL[1].fenergy(temperature);
    fB[1] = g_species_phaseA[1].fenergy(temperature);
    // std::cout<<"fB[0]="<<fB[0]<<", fB[1]="<<fB[1]<<std::endl;

    CalphadDataType Lmix_L[4];
    for (int i = 0; i < 4; i++)
        Lmix_L[i] = LmixPhaseL[i][0] + temperature * LmixPhaseL[i][1];
    // for(int i=0;i<4;i++)std::cout<<"Lmix_L["<<i<<"]="<<Lmix_L[i]<<std::endl;

    CalphadDataType Lmix_A[4];
    for (int i = 0; i < 4; i++)
        Lmix_A[i] = LmixPhaseA[i][0] + temperature * LmixPhaseA[i][1];
    // for(int i=0;i<4;i++)std::cout<<"Lmix_A["<<i<<"]="<<Lmix_A[i]<<std::endl;

    const double RTinv
        = 1.0 / (Thermo4PFM::gas_constant_R_JpKpmol * temperature);

    double sol[2] = { 0.5, 0.5 };

    double deviation = 1.e-4;

    double* xhost = new double[2 * N];
    for (int i = 0; i < 2 * N; i++)
    {
        xhost[i] = -1.;
    }

    // Host solve
    {
        short* nits = new short[N];
        auto t1     = Clock::now();

#pragma omp parallel for
        for (int i = 0; i < N; i++)
        {
            if (!omp_is_initial_device()) abort();

            xhost[2 * i]     = sol[0];
            xhost[2 * i + 1] = sol[1];
            double hphi      = 0.5 + (i % 100) * deviation;
            double c0        = 0.3;
            Thermo4PFM::CALPHADConcSolverBinary solver;
            solver.setup(c0, hphi, RTinv, Lmix_L, Lmix_A, fA, fB);
            nits[i] = solver.ComputeConcentration(&xhost[2 * i], 1.e-8, 50);
        }
        auto t2 = Clock::now();
        long int usec
            = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1)
                  .count();
        std::cout << "Host time/us/solve:   " << (double)usec / (double)N
                  << std::endl;

        std::cout << std::setprecision(12);

        int n = N > 20 ? 20 : N;

        for (int i = 0; i < n; i++)
        {
            std::cout << "Host: x=" << xhost[2 * i] << "," << xhost[2 * i + 1]
                      << std::endl;
            std::cout << "nits=" << nits[i] << std::endl;
        }
        delete[] nits;
    }

    // Device solve
    {
        double* xdev = new double[2 * N];
        for (int i = 0; i < 2 * N; i++)
        {
            xdev[i] = -1;
        }

        short* nits = new short[N];

        auto t1 = Clock::now();

// clang-format off
#pragma omp target map(to : sol) \
                   map(tofrom : xdev) \
                   map(to : fA, fB, Lmix_L, Lmix_A) \
                   map(to : RTinv) \
                   map(from : nits)                                                     \
// clang-format on
        {
#pragma omp teams distribute parallel for
            for (int i = 0; i < N; i++)
            {
                // if( omp_is_initial_device() ) abort();
                xdev[2 * i]     = sol[0];
                xdev[2 * i + 1] = sol[1];

                double hphi = 0.5 + (i % 100) * deviation;
                double c0   = 0.3;
                class Thermo4PFM::CALPHADConcSolverBinary solver;
                solver.setup(c0, hphi, RTinv, Lmix_L, Lmix_A, fA, fB);
                nits[i] = solver.ComputeConcentration(&xdev[2 * i], 1.e-8, 50);
            }
        }

        auto t2 = Clock::now();
        long int usec
            = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1)
                  .count();
        std::cout << "Device time/us/solve: " << (double)usec / (double)N
                  << std::endl;

        double tol = 1.e-8;
        for (int i = 0; i < N; i++)
        {
            if (std::abs(xdev[2 * i] - xhost[2 * i]) > tol
                || std::abs(xdev[2 * i + 1] - xhost[2 * i + 1]) > tol || N < 20)
            {
                std::cout << "Device: x=" << xdev[2 * i] << ","
                          << xdev[2 * i + 1] << std::endl;
                std::cout << "Difference: " << xdev[2 * i] - xhost[2 * i]
                          << ", " << xdev[2 * i + 1] - xhost[2 * i + 1]
                          << std::endl;
                std::cout << "nits[" << i << "]=" << nits[i] << std::endl;
            }
        }
        delete[] nits;
        delete[] xdev;
    }

    delete[] xhost;
}


#include "SW.hpp"

#include "union_find.hpp"

#include <cmath>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include <boost/timer/timer.hpp>

namespace ising{

Worker::Worker(alps::Parameters const& params):
  SuperClass(params),
  mcs_(params),
  T_(evaluate("T", params)),
  beta_(1.0/T_),
  J_(params.value_or_default("J", 1.0)),
  nsites_(num_sites()),
  nbonds_(num_bonds()),
  spins_(nsites_, 1),
  V2_(nsites_*nsites_)
{
  init(params);
}

void Worker::init(alps::Parameters const& params)
{
  if(params.defined("MODEL")){
    std::string modelname = params["MODEL"];
    if(modelname ==  std::string("Ising")){
      ene_const_ = nbonds_ * J_;
      q_ = 2;
      J_ *= 2;
    }else if(modelname == std::string("Potts")){
      q_ = evaluate("q", params);
      ene_const_ = 0.0;
    }else{
      std::cerr << modelname << " model is not implemented.";
      exit(127);
    }
  }else{
    std::cerr << "parameter \"MODEL\" is not defined.";
    exit(127);
  }
  negspin_ = 1.0 / (q_-1);
  p_ = -boost::math::expm1(-beta_*J_);
  m2_coeff_ = negspin_;
  m4_coeff_ = (1.0 + negspin_*negspin_*negspin_)/q_;
  ene_coeff_ = J_ / p_;
}

void Worker::init_observables(alps::Parameters const&, alps::ObservableSet& obs)
{
  obs << alps::RealObservable("Time");
  obs << alps::RealObservable("Speed");
  obs << alps::RealObservable("Number of Sites");
  obs << alps::RealObservable("Magnetization^2");
  obs << alps::RealObservable("Magnetization^4");
  obs << alps::RealObservable("|Magnetization|");
  obs << alps::RealObservable("Energy");
  obs << alps::RealObservable("Susceptibility");
  obs << alps::RealObservable("Activated Bonds");
  obs << alps::RealObservable("Activated Bonds^2");
}

void Worker::run(alps::ObservableSet& obs)
{
  boost::timer::cpu_timer tm;

  ++mcs_;

  // make clusters

  int activated = 0;
  std::vector<union_find::Node> nodes(nsites_);
  foreach(bond_descriptor b, bonds()){
    const int lsite = source(b);
    const int rsite = target(b);
    if( spins_[lsite] == spins_[rsite] && random_01() < p_){
      union_find::unify(nodes, lsite, rsite);
      ++activated;
    }
  }

  // flip spins

  double ma = 0.0;
  std::vector<int> cluster_size2;
  std::vector<int> cluster_spin;
  int id = 0;
  for(int site=0; site<nsites_; ++site){
    int ri = union_find::root_index(nodes, site);
    union_find::Node &root = nodes[ri];
    if(root.id == -1){
      root.id = id++;
      cluster_size2.push_back(root.size*root.size);
      cluster_spin.push_back(q_ * random_01());
    }
    spins_[site] = cluster_spin[root.id];
    ma += spins_[site] == 0 ? 1.0 : -negspin_;
  }

  if(!is_thermalized()){
    return;
  }

  // measure

  const int nc = cluster_size2.size();
  double m2 = 0.0;
  double m4 = 0.0;
  for(int ci=0; ci < nc; ++ci){
    m4 += m4_coeff_ * cluster_size2[ci] * cluster_size2[ci];
    m4 += 6 * cluster_size2[ci] * m2;
    m2 += m2_coeff_ * cluster_size2[ci];
  }
  const double V2 = nsites_ * nsites_;
  m2 /= V2;
  m4 /= (V2*V2);

  double ene = ene_const_ - ene_coeff_ * activated;

  obs["Magnetization^2"] << m2;
  obs["Magnetization^4"] << m4;
  obs["|Magnetization|"] << std::abs(ma) / nsites_;
  obs["Susceptibility"] << m2 * nsites_ *  beta_;
  obs["Number of Sites"] << 1.0 * nsites_;
  obs["Energy"] << ene;
  obs["Activated Bonds"] << 1.0*activated;
  obs["Activated Bonds^2"] << 1.0*activated*activated;

  const double sec = tm.elapsed().wall * 1.0e-9;
  obs["Time"] << sec;
  obs["Speed"] << 1.0/sec;
}

void Evaluator::evaluate(alps::ObservableSet& obs) const
{
  const double T = alps::evaluate("T", params_);
  const double beta = 1.0/T;

  alps::RealObsevaluator m2 = obs["Magnetization^2"];
  alps::RealObsevaluator m4 = obs["Magnetization^4"];
  alps::RealObsevaluator ma = obs["|Magnetization|"];
  alps::RealObsevaluator nsites = obs["Number of Sites"];
  alps::RealObsevaluator n = obs["Activated Bonds"];
  alps::RealObsevaluator n2 = obs["Activated Bonds^2"];

  alps::RealObsevaluator binder = m4 / (m2*m2);
  binder.rename("Binder Ratio");
  obs.addObservable(binder);

  alps::RealObsevaluator csus = (m2 - ma * ma) * nsites * beta;
  csus.rename("Connected Susceptibility");
  obs.addObservable(csus);

  const double J = static_cast<double>(params_.value_or_default("J", 1.0)) * (params_["MODEL"]==std::string("Ising") ? 2.0:1.0);

  const double bj = -beta*J;
  const double coeff = bj / (boost::math::expm1(bj));
  alps::RealObsevaluator spec = (n2 - n*n + std::exp(bj)*n) * coeff * coeff;
  spec.rename("Specific Heat");
  obs.addObservable(spec);
}

} // end of namespace ising


#pragma once

#include <Eigen/Core>
#include <autodiff/autodiff_types.hpp>

namespace ccd {
namespace autogen {

    template <typename T>
    using Vector2T = Eigen::Matrix<T, 2, 1>;

    /**
    * Compute the volume of intersection for an edge given a time
    * of intersection (toi) and position of intersection (alpha)
    *
    * \f$V = (1-\tau_I)\sqrt{\epsilon^2 \|e(\tau_I)\|^2 + (U_{ij} \cdot
    * e(\tau_I)^\perp)^2}\f$
    *
    *     @param V_{ijk}          : Vertices positions.
    *     @param U_{ijk}          : Vertices displacements.
    *     @param epsilon          : The time scale used for minimal volume.
    *
    * @return                     : The space-time interference volume.
    */
    template <typename T>
    T space_time_collision_volume(
        const Eigen::Vector2d& Vi,
        const Eigen::Vector2d& Vj,
        const Vector2T<T>& Ui,
        const Vector2T<T>& Uj,
        const T& toi, const T& alpha,
        const double epsilon);

}
}

#include "auto_collision_volume.ipp"


//---------------------------------------------------------------------------//
// Copyright (c) 2020 Ilias Khairullin <ilias@nil.foundation>
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
//
// 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 CRYPTO3_HASH_POSEIDON_MDS_MATRIX_HPP
#define CRYPTO3_HASH_POSEIDON_MDS_MATRIX_HPP

#include <nil/crypto3/algebra/matrix/matrix.hpp>
#include <nil/crypto3/algebra/matrix/math.hpp>
#include <nil/crypto3/algebra/matrix/operators.hpp>
#include <nil/crypto3/algebra/vector/vector.hpp>
#include <nil/crypto3/algebra/vector/math.hpp>
#include <nil/crypto3/algebra/vector/operators.hpp>

#include <nil/crypto3/hash/detail/poseidon/poseidon_policy.hpp>

#include <boost/assert.hpp>

namespace nil {
    namespace crypto3 {
        namespace hashes {
            namespace detail {
                template<typename FieldType, std::size_t Arity, std::size_t PartRounds>
                struct poseidon_mds_matrix {
                    typedef poseidon_policy<FieldType, Arity, PartRounds> policy_type;
                    typedef typename FieldType::value_type element_type;

                    constexpr static const std::size_t state_words = policy_type::state_words;
                    constexpr static const std::size_t half_full_rounds = policy_type::half_full_rounds;
                    constexpr static const std::size_t part_rounds = policy_type::part_rounds;

                    typedef algebra::matrix<element_type, state_words, state_words> mds_matrix_type;
                    typedef algebra::vector<element_type, state_words> state_vector_type;
                    typedef algebra::vector<element_type, state_words - 1> substate_vector_type;
                    typedef algebra::matrix<element_type, state_words - 1, state_words - 1> mds_submatrix_type;

                    inline void product_with_mds_matrix(state_vector_type &A_vector) const {
                        A_vector = algebra::vectmatmul(A_vector, mds_matrix);
                    }

                    constexpr void product_with_inverse_mds_matrix_noalias(const state_vector_type &A_vector_in,
                                                                           state_vector_type &A_vector_out) const {
                        A_vector_out = algebra::vectmatmul(A_vector_in, mds_matrix_inverse);
                    }

                    inline void product_with_equivalent_mds_matrix_init(state_vector_type &A_vector,
                                                                        std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number == half_full_rounds,
                                         "wrong using: product_with_equivalent_mds_matrix_init");
                        A_vector = algebra::vectmatmul(A_vector, get_M_i());
                    }

                    inline void product_with_equivalent_mds_matrix(state_vector_type &A_vector,
                                                                   std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number >= half_full_rounds &&
                                             round_number < half_full_rounds + part_rounds,
                                         "wrong using: product_with_equivalent_mds_matrix");
                        const std::size_t matrix_number_base = part_rounds - (round_number - half_full_rounds) - 1;
                        const substate_vector_type &v = get_v(matrix_number_base);
                        state_vector_type temp_vector;
                        element_type A_0 = A_vector[0];
                        temp_vector[0] = get_M_0_0();
                        for (std::size_t i = 1; i < state_words; i++) {
                            temp_vector[i] = get_w_hat(matrix_number_base)[i - 1];
                        }
                        A_vector[0] = algebra::dot(A_vector, temp_vector);
                        for (std::size_t i = 1; i < state_words; i++) {
                            A_vector[i] = A_0 * v[i - 1] + A_vector[i];
                        }
                    }

                    // private:
#ifdef CRYPTO3_HASH_POSEIDON_COMPILE_TIME
                    constexpr
#endif
                    inline mds_matrix_type generate_mds_matrix() {
                        mds_matrix_type new_mds_matrix;
                        for (std::size_t i = 0; i < state_words; i++) {
                            for (std::size_t j = 0; j < state_words; j++) {
                                new_mds_matrix[i][j] = element_type(i + j + state_words).inversed();
                            }
                        }
                        return new_mds_matrix;
                    }

                    struct equivalent_mds_matrix_type {
                        typedef std::array<substate_vector_type, part_rounds> subvectors_array;

#ifdef CRYPTO3_HASH_POSEIDON_COMPILE_TIME
                        constexpr
#endif
                        equivalent_mds_matrix_type(const mds_matrix_type &mds_matrix) :
                            M_i(algebra::get_identity<element_type, state_words>()), w_hat_list(), v_list(), M_0_0() {
                            mds_matrix_type M_mul(mds_matrix);
                            mds_submatrix_type M_hat_inverse;
                            substate_vector_type M_mul_column_slice;

                            for (std::size_t i = 0; i < part_rounds; i++) {
                                M_hat_inverse =
                                    algebra::inverse(algebra::submat<state_words - 1, state_words - 1>(M_mul, 1, 1));
                                w_hat_list[i] = algebra::matvectmul(
                                    M_hat_inverse, algebra::slice<state_words - 1>(M_mul.column(0), 1));
                                v_list[i] = algebra::slice<state_words - 1>(M_mul.row(0), 1);
                                for (std::size_t j = 1; j < state_words; j++) {
                                    for (std::size_t k = 1; k < state_words; k++) {
                                        M_i[j][k] = M_mul[j][k];
                                    }
                                }
                                M_mul = algebra::matmul(mds_matrix, M_i);
                            }
                            M_0_0 = mds_matrix[0][0];
                        }

                        mds_matrix_type M_i;
                        subvectors_array w_hat_list;
                        subvectors_array v_list;
                        element_type M_0_0;
                    };

                    inline const substate_vector_type &get_w_hat(std::size_t w_hat_number) const {
                        return equivalent_mds_matrix.w_hat_list[w_hat_number];
                    }
                    inline const substate_vector_type &get_v(std::size_t v_number) const {
                        return equivalent_mds_matrix.v_list[v_number];
                    }
                    inline const element_type &get_M_0_0() const {
                        return equivalent_mds_matrix.M_0_0;
                    }
                    inline const mds_matrix_type &get_M_i() const {
                        return equivalent_mds_matrix.M_i;
                    }

#ifdef CRYPTO3_HASH_POSEIDON_COMPILE_TIME
                    constexpr
#endif
                    poseidon_mds_matrix() :
                        mds_matrix(generate_mds_matrix()), mds_matrix_inverse(algebra::inverse(mds_matrix)),
                        equivalent_mds_matrix(mds_matrix) {
                    }

                    mds_matrix_type mds_matrix;
                    mds_matrix_type mds_matrix_inverse;
                    equivalent_mds_matrix_type equivalent_mds_matrix;
                };
            }    // namespace detail
        }        // namespace hashes
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_HASH_POSEIDON_MDS_MATRIX_HPP


/**
 * @file    SchurComplementDenseSolver.cpp
 * @brief   Abstract linear solver class of Ax = b
 * @author  Jing Dong
 * @date    Jun 24, 2019
 */

#include <minisam/linear/SchurComplementDenseSolver.h>
#include <minisam/utils/Timer.h>

#include <Eigen/Cholesky>

namespace minisam {

/* ************************************************************************** */
LinearSolverStatus SchurComplementDenseSolver::solve(
    const Eigen::SparseMatrix<double>& AtA, const Eigen::VectorXd& Atb,
    Eigen::VectorXd& x) {
  // static auto sr_timer = global_timer().getTimer("* Schur solve reduced");
  // static auto se_timer = global_timer().getTimer("* Schur solve elimiated");

  const int rs_until_idx = sc_ordering_->reducedSysUntilIndex();

  Eigen::MatrixXd H_reduced_lower;
  Eigen::SparseMatrix<double> Her;
  Eigen::SparseMatrix<double> He_inv_lower;
  Eigen::VectorXd g_reduced;

  // build reduced system  H_reduced * x_reduced = g_reduced
  LinearSolverStatus build_status = buildReducedSystem_(
      AtA, Atb, H_reduced_lower, Her, He_inv_lower, g_reduced);

  if (build_status != LinearSolverStatus::SUCCESS) {
    return build_status;
  }

  // sr_timer->tic();

  // reduced solution
  Eigen::VectorXd x_reduced;
  LinearSolverStatus reduced_status =
      reduced_solver_->solve(H_reduced_lower, g_reduced, x_reduced);

  if (reduced_status != LinearSolverStatus::SUCCESS) {
    return reduced_status;
  }

  // sr_timer->toc();
  // se_timer->tic();

  Eigen::VectorXd x_elimiated =
      He_inv_lower.selfadjointView<Eigen::Lower>() *
      (Atb.tail(AtA.rows() - rs_until_idx) - Her * x_reduced);

  // se_timer->toc();

  x = Eigen::VectorXd(x_reduced.size() + x_elimiated.size());
  x << x_reduced, x_elimiated;

  return LinearSolverStatus::SUCCESS;
}

/* ************************************************************************** */
LinearSolverStatus SchurComplementDenseSolver::buildReducedSystem_(
    const Eigen::SparseMatrix<double>& AtA_lower, const Eigen::VectorXd& Atb,
    Eigen::MatrixXd& H_reduced_lower, Eigen::SparseMatrix<double>& Her,
    Eigen::SparseMatrix<double>& He_inv_lower, Eigen::VectorXd& g_reduced) {
  // static auto br1_timer = global_timer().getTimer("* Schur build reduced");

  const int rs_until_idx = sc_ordering_->reducedSysUntilIndex();

  He_inv_lower = AtA_lower.block(rs_until_idx, rs_until_idx,
                                 AtA_lower.rows() - rs_until_idx,
                                 AtA_lower.rows() - rs_until_idx);
  Her = AtA_lower.block(rs_until_idx, 0, AtA_lower.rows() - rs_until_idx,
                        rs_until_idx);
  He_inv_lower.makeCompressed();
  Her.makeCompressed();

  H_reduced_lower =
      Eigen::MatrixXd(AtA_lower.block(0, 0, rs_until_idx, rs_until_idx));

  // br1_timer->tic();

  // block inversed based method
  for (size_t i = 0; i < sc_ordering_->eliminatedVariableSize(); i++) {
    const int var_pos = sc_ordering_->eliminatedVariablePosition(i);
    const int var_dim = sc_ordering_->eliminatedVariableDim(i);

    // br11_timer->tic();
    const Eigen::LDLT<Eigen::MatrixXd, Eigen::Lower> ldlt(
        He_inv_lower.block(var_pos, var_pos, var_dim, var_dim));
    if (ldlt.info() != Eigen::Success) {
      return LinearSolverStatus::RANK_DEFICIENCY;
    }
    const Eigen::MatrixXd Hs_block_inv =
        ldlt.solve(Eigen::MatrixXd::Identity(var_dim, var_dim));
    // br11_timer->toc();
    // replace with inverse
    // br12_timer->tic();
    for (int ii = 0; ii < var_dim; ii++) {
      for (int ij = 0; ij < var_dim - ii; ij++) {
        double* value_ptr = He_inv_lower.valuePtr() +
                            He_inv_lower.outerIndexPtr()[ii + var_pos] + ij;
        *value_ptr = Hs_block_inv(ii, ii + ij);
      }
    }
    // br12_timer->toc();
  }

  H_reduced_lower -=
      Her.transpose() * (He_inv_lower.selfadjointView<Eigen::Lower>() * Her);

  // br1_timer->toc();

  g_reduced.noalias() =
      Atb.head(rs_until_idx) -
      Her.transpose() * (He_inv_lower.selfadjointView<Eigen::Lower>() *
                         Atb.tail(AtA_lower.rows() - rs_until_idx));

  return LinearSolverStatus::SUCCESS;
}

}  // namspace minisam


#ifndef PROMP_EMISSION_H
#define PROMP_EMISSION_H

#include <armadillo>
#include <emissions.hpp>
#include <iostream>
#include <json.hpp>
#include <ForwardBackward.hpp>
#include <map>
#include <memory>
#include <robotics.hpp>

using namespace arma;
using namespace hsmm;
using namespace robotics;
using namespace std;

namespace hsmm {

    class NormalInverseWishart {
        public:
            NormalInverseWishart(vec mu_0, double lambda, mat Phi, int dof) :
                    Phi_(Phi), dof_(dof), mu_0_(mu_0), lambda_(lambda) {
                assert(dof > Phi.n_rows + 1);
                vec eigenvalues = eig_sym(Phi);
                assert(eigenvalues(0) > 0);
                assert(!(lambda < 0));
                assert(mu_0.n_rows == Phi.n_rows);
            }

            // This constructor assumes there is no prior for the mean.
            // (i.e. only the inverse-Wishart part).
            NormalInverseWishart(mat Phi, int dof) : NormalInverseWishart(
                    zeros<vec>(Phi.n_rows), 0.0, Phi, dof) {}

            vec getMu0() {
                return mu_0_;
            }

            double getLambda() {
                return lambda_;
            }

            mat getPhi() {
                return Phi_;
            }

            int getDof() {
                return dof_;
            }

        private:
            vec mu_0_;
            double lambda_;
            mat Phi_;
            int dof_;
    };

    double zeroMeanGaussianLogLikelihood(const vec& x, const mat &precision) {
        return -0.5 * as_scalar(x.t()*precision*x - log(det(precision)) +
                x.n_elem * log(2*datum::pi));
    }

    class ProMPsEmission : public AbstractEmissionOnlineSetting {
        public:
            ProMPsEmission(vector<FullProMP> promps) :
                    AbstractEmissionOnlineSetting(promps.size(),
                    promps.at(0).get_num_joints()), promps_(promps),
                    diagonal_sigma_y_(true) {
                for(int i = 0; i < getNumberStates(); i++)
                    assert(promps_.at(i).get_num_joints() == getDimension());
            }

            ProMPsEmission* clone() const {
                return new ProMPsEmission(*this);
            }

            void resetMemCaches() {
                cachePhis_.clear();
                cacheInvS_.clear();
                cacheK_.clear();
                cachePosteriorSigma_.clear();
            }

            // TODO: remove this once this behavior is moved to a subclass.
            void setDelta(double delta) {
                //assert(delta > 0);
                sample_locations_delta_ = delta;
            }

            // Derived classes could implement a different kinf of dependence
            // on the segment duration. Default implementation normalizes
            // the input to be between 0 and 1.
            // TODO: Implement the segment independent in a subclass.
            virtual vec getSampleLocations(int length) const {
                if (sample_locations_delta_ < 0)
                    return linspace<vec>(0, 1.0, length);
                else
                    return linspace<vec>(0, (length-1)*sample_locations_delta_,
                            length);
            }

            // This initialization mechanism assumes all the hidden states
            // share the same basis functions and their hyperparameters.
            void init_params_from_data(int min_duration, int ndurations,
                    const arma::field<arma::field<arma::mat>>& mobs) {
                vector<pair<double, vec>> pairs;
                vector<double> noise_vars;
                for(auto &obs : mobs) {
                    for(int t = 0; t < obs.n_elem; t++) {
                        for(int d = 0; d < ndurations; d++) {
                            if (t + min_duration + d > obs.n_elem)
                                break;
                            int end_idx = t + min_duration + d - 1;
                            auto &segment = obs.rows(t, end_idx);
                            vec lsq_omega = least_squares_omega(0, segment);
                            double var = var_isotropic_gaussian_given_omega(0,
                                    lsq_omega, segment);
                            pairs.push_back(make_pair(var, lsq_omega));
                            noise_vars.push_back(var);
                        }
                    }
                }
                sort(pairs.begin(), pairs.end(), [](const pair<double, vec>& a,
                        const pair<double, vec>& b)
                        {return a.first < b.first;});
                int cutoff_index = (int)((pairs.size() - 1) *
                        init_fraction_);
                double threshold = pairs.at(cutoff_index).first;
                cout << "Cuttoff index for init: " << cutoff_index <<
                        " var: " << threshold << endl;

                // Showing a histogram of the distribution of noise vars.
                vec vars(noise_vars);
                vec histogram_cutoffs = linspace<vec>(0, max(vars), 20);
                uvec histogram = histc(vars, histogram_cutoffs);
                cout << "Hist. of noise vars:" << endl << histogram << endl;

                mat remaining_w(promps_.at(0).get_model().get_mu_w().n_rows,
                        cutoff_index + 1);
                for(int i = 0; i <= cutoff_index; i++)
                    remaining_w.col(i) = pairs.at(i).second;
                mat means;
                kmeans(means, remaining_w, getNumberStates(), static_subset,
                        10, false);

                // Updating the means of the ProMPs based on the k-means.
                for(int i = 0; i < getNumberStates(); i++) {
                    ProMP promp = promps_.at(i).get_model();
                    promp.set_mu_w(means.col(i));
                    promps_.at(i).set_model(promp);
                }
            }

            // Note that the method is returning the posterior covariance for
            // the given state and duration which is independent of obs.
            mat generateCachedMatrices(const pair<int, int> &p) const {
                if (cacheInvS_.find(p) == cacheInvS_.end() ||
                        cacheK_.find(p) == cacheK_.end()) {
                    int state = p.first;
                    int dur = p.second;
                    const FullProMP& promp = promps_.at(state);
                    const cube& Phis = getPhiCube(state, dur);
                    mat Sigma(promp.get_model().get_Sigma_w());
                    mat Sigma_y(promp.get_model().get_Sigma_y());
                    field<mat> invS(dur);
                    field<mat> K(dur);
                    for(int i = 0; i < dur; i++) {
                        const mat& Phi = Phis.slice(i);
                        mat S = Phi * Sigma * Phi.t() + Sigma_y;
                        invS(i) = inv_sympd(S);

                        // Kalman updating (correcting) step.
                        K(i) = Sigma * Phi.t() * inv(S);
                        Sigma = Sigma - K(i) * S * K(i).t();
                    }

                    // Caching the matrices for faster likelihood evaluation.
                    cacheInvS_[p] = invS;
                    cacheK_[p] = K;
                    cachePosteriorSigma_[p] = Sigma;
                }
                return cachePosteriorSigma_.at(p);
            }

            double loglikelihood(int state, const field<mat>& obs) const {

                // Making sure all the required matrices are already
                // precomputed.
                pair<int, int> p = make_pair(state, obs.n_elem);
                generateCachedMatrices(p);

                const FullProMP& promp = promps_.at(state);
                const cube& Phis = getPhiCube(state, obs.n_elem);
                vec mu(promp.get_model().get_mu_w());
                const field<mat>& invS = cacheInvS_[p];
                const field<mat>& K = cacheK_[p];
                double ret = 0;
                for(int i = 0; i < obs.n_elem; i++) {
                    const mat& Phi = Phis.slice(i);
                    if (obs(i).is_empty()) {

                        // Making sure all the missing obs are at the end.
                        // Other missing obs patterns are not supported yet.
                        for(int j = i; j < obs.n_elem; j++)
                            assert(obs(j).is_empty());
                        break;
                    }
                    vec diff = obs(i) - Phi * mu;

                    // p(y_t | y_1, ..., y_{t-1}).
                    ret += zeroMeanGaussianLogLikelihood(diff, invS(i));
                    mu = mu + K(i) * diff;
                }
                return ret;
            }

            double informationFilterLoglikelihood(int state,
                    const field<mat>& obs) const {
                const FullProMP& promp = promps_.at(state);

                // The samples are assumed to be equally spaced.
                vec sample_locations = getSampleLocations(obs.n_elem);

                // Moment based parameterization.
                vec mu(promp.get_model().get_mu_w());
                mat Sigma(promp.get_model().get_Sigma_w());
                mat Sigma_y(promp.get_model().get_Sigma_y());

                // Canonical parameterization.
                mat information_matrix = inv_sympd(Sigma);
                vec information_state = information_matrix * mu;
                mat inv_obs_noise = inv_sympd(Sigma_y);

                double ret = 0;
                for(int i = 0; i < obs.n_elem; i++) {
                    mat Phi = promp.get_phi_t(sample_locations(i));

                    // Computing the likelihood under the current filtering
                    // distribution.
                    mat filtered_Sigma = inv_sympd(information_matrix);
                    vec filtered_mu = filtered_Sigma * information_state;

                    // p(y_t | y_{1:t-1}).
                    random::NormalDist dist = random::NormalDist(
                            Phi * filtered_mu,
                            Phi * filtered_Sigma * Phi.t() + Sigma_y);
                    ret = ret + log_normal_density(dist, obs(i));

                    mat aux = Phi.t() * inv_obs_noise;
                    mat I_k = aux * Phi;
                    mat i_k = aux * obs(i);
                    information_matrix = information_matrix + I_k;
                    information_state = information_state + i_k;
                }
                return ret;
            }

            double loglikelihoodBatchVersion(int state, const arma::mat& obs) {
                int dimension = obs.n_rows;
                assert(dimension == getDimension());
                random::NormalDist dist = getNormalDistForMultipleTimeSteps(state,
                        obs.n_cols);
                vec stacked_obs = vectorise(obs);
                return random::log_normal_density(dist, stacked_obs);
            }

            void reestimate(int min_duration,
                    const arma::field<arma::cube>& meta,
                    const arma::field<arma::field<arma::mat>>& mobs) {
                int nseq = mobs.n_elem;
                for(int i = 0; i < getNumberStates(); i++) {
                    ProMP promp = promps_.at(i).get_model();
                    const mat inv_Sigma_w = inv_sympd(promp.get_Sigma_w());
                    const mat inv_Sigma_y = inv_sympd(promp.get_Sigma_y());
                    const vec mu_w = promp.get_mu_w();

                    vector<double> mult_c;
                    vector<double> denominator_Sigma_y;
                    for(int s = 0; s < nseq; s++) {
                        const cube& eta = meta(s);
                        int nobs = mobs(s).n_elem;
                        int ndurations = eta.n_cols;
                        for(int t = min_duration - 1; t < nobs; t++) {
                            for(int d = 0; d < ndurations; d++) {
                                int first_idx_seg = t - min_duration - d + 1;
                                if (first_idx_seg < 0)
                                    break;
                                mult_c.push_back(eta(i, d, t));
                                denominator_Sigma_y.push_back(eta(i, d, t) +
                                        log(min_duration + d));
                            }
                        }
                    }

                    // Computing the multiplicative constants for mu_w and
                    // Sigma_w.
                    vec mult_c_normalized(mult_c);
                    mult_c_normalized -= logsumexp(mult_c_normalized);
                    mult_c_normalized = exp(mult_c_normalized);

                    // Computing the multiplicative constants for Sigma_y since
                    // they have a different denominator.
                    vec mult_c_Sigma_y_normalized(mult_c);
                    vec den_Sigma_y(denominator_Sigma_y);
                    mult_c_Sigma_y_normalized -= logsumexp(den_Sigma_y);
                    mult_c_Sigma_y_normalized = exp(mult_c_Sigma_y_normalized);

                    mat new_Sigma_y(size(promp.get_Sigma_y()), fill::zeros);

                    // EM for ProMPs.
                    vec weighted_sum_post_mean(size(mu_w), fill::zeros);
                    mat weighted_sum_post_cov(size(inv_Sigma_w), fill::zeros);
                    mat weighted_sum_post_mean_mean_T(size(inv_Sigma_w),
                            fill::zeros);
                    int idx_mult_c = 0;
                    for(int s = 0; s < nseq; s++) {
                        auto& obs = mobs(s);
                        int nobs = obs.n_elem;
                        int ndurations = meta(s).n_cols;
                        for(int t = min_duration - 1; t < nobs; t++) {
                            for(int d = 0; d < ndurations; d++) {
                                int first_idx_seg = t - min_duration - d + 1;
                                if (first_idx_seg < 0)
                                    break;
                                const int current_duration = min_duration + d;
                                const cube& Phis = getPhiCube(i, current_duration);

                                // E step for the emission hidden variables (Ws).
                                // Computing the posterior of W given Y and Theta.

                                // Computing the posterior covariance of the hidden
                                // variable w for this segment.
                                mat posterior_cov(size(inv_Sigma_w), fill::zeros);
                                for(int step = 0; step < current_duration; step++) {
                                    posterior_cov += Phis.slice(step).t() *
                                        inv_Sigma_y * Phis.slice(step);
                                }
                                posterior_cov = (posterior_cov+posterior_cov.t())/2.0;
                                posterior_cov = posterior_cov + inv_Sigma_w;
                                posterior_cov = (posterior_cov+posterior_cov.t())/2.0;
                                posterior_cov = inv_sympd(posterior_cov);
                                posterior_cov = (posterior_cov+posterior_cov.t())/2.0;

                                // Computing the posterior mean of the hidden
                                // variable w for this segment.
                                vec posterior_mean(size(mu_w), fill::zeros);
                                for(int step = 0; step < current_duration; step++) {
                                    const vec& ob = obs(first_idx_seg + step);
                                    posterior_mean += Phis.slice(step).t() *
                                        inv_Sigma_y * ob;
                                }
                                posterior_mean = inv_Sigma_w * mu_w + posterior_mean;
                                posterior_mean = posterior_cov * posterior_mean;

                                // Getting the multiplicative constants.
                                double mult_constant = mult_c_normalized(idx_mult_c);
                                double mult_constant_Sigma_y =
                                        mult_c_Sigma_y_normalized(idx_mult_c);
                                idx_mult_c++;

                                // Statistics required for updating mu_w & Sigma_w.
                                weighted_sum_post_mean += mult_constant *
                                        posterior_mean;
                                weighted_sum_post_cov += mult_constant *
                                        posterior_cov;
                                weighted_sum_post_mean_mean_T += mult_constant *
                                        posterior_mean * posterior_mean.t();

                                // Computing the new output noise covariance: Sigma_y.
                                mat Sigma_y_term(size(new_Sigma_y), fill::zeros);
                                for(int step = 0; step < current_duration; step++) {
                                    const mat& phi = Phis.slice(step);
                                    const vec& diff_y = obs(first_idx_seg + step) -
                                        phi * posterior_mean;
                                    Sigma_y_term += diff_y * diff_y.t() +
                                        phi * posterior_cov * phi.t();
                                }
                                new_Sigma_y += mult_constant_Sigma_y * Sigma_y_term;
                            }
                        }
                    }

                    // Expected number of segments generated by the i-th state.
                    double mle_den = exp(logsumexp(mult_c));

                    // M step for the emission variables.
                    vec new_mu_w_MLE(weighted_sum_post_mean);
                    mat new_Sigma_w_MLE = weighted_sum_post_cov +
                        weighted_sum_post_mean_mean_T - new_mu_w_MLE *
                        new_mu_w_MLE.t();

                    // If there is a prior then we do MAP instead.
                    mat new_Sigma_w;
                    mat new_mu_w;
                    if (normal_inverse_prior_) {
                        double v_0 = normal_inverse_prior_->getDof();
                        double D = mu_w.n_rows;
                        mat S_0 = normal_inverse_prior_->getPhi();
                        new_Sigma_w = (S_0 + mle_den * new_Sigma_w_MLE) /
                                (v_0 + mle_den + D + 2);

                        double k_0 = normal_inverse_prior_->getLambda();
                        vec m_0 = normal_inverse_prior_->getMu0();
                        new_mu_w = (k_0 * m_0 + mle_den * new_mu_w_MLE) /
                                (mle_den + k_0);
                    }
                    else {
                        new_Sigma_w = new_Sigma_w_MLE;
                        new_mu_w = new_mu_w_MLE;
                    }

                    cout << "State " << i << " MLE Den: " << mle_den << " ";
                    if (mle_den > epsilon_) {

                        // Making sure the noise covariance is diagonal.
                        if (diagonal_sigma_y_)
                            new_Sigma_y = diagmat(new_Sigma_y.diag());

                        // Checking that the new Sigma_w is a covariance matrix.
                        vec eigenvalues_map = eig_sym(new_Sigma_w);
                        assert(eigenvalues_map(0) > 0);

                        // Setting the new parameters.
                        promp.set_mu_w(new_mu_w);
                        promp.set_Sigma_w(new_Sigma_w);
                        promp.set_Sigma_y(new_Sigma_y);
                        promps_.at(i).set_model(promp);
                        resetMemCaches();
                        cout << ". Updated." << endl;
                    }
                    else
                        cout << ". Not updated." << endl;
                }
            }

            // Unshadowing this method from the AbstractEmission class.
            using AbstractEmission::sampleFromState;

            field<mat> sampleFromState(int state, int size,
                    mt19937 &rand_generator) const {
                return sampleFromProMP(promps_.at(state), size, rand_generator);
            }

            field<mat> sampleFromProMP(const FullProMP& fpromp, int size,
                    mt19937 &rand_generator) const {
                const ProMP& model = fpromp.get_model();
                assert(model.get_mu_w().n_rows == model.get_Sigma_w().n_rows);
                vector<vec> w_samples = random::sample_multivariate_normal(
                        rand_generator,
                        {model.get_mu_w(), model.get_Sigma_w()}, 1);
                vec w = w_samples.back();

                vec noise_mean = zeros<vec>(getDimension());
                vector<vec> output_noise = random::sample_multivariate_normal(
                        rand_generator, {noise_mean, model.get_Sigma_y()}, size);

                field<mat> ret(size);

                // The samples are assumed to be equally spaced.
                vec sample_locations = getSampleLocations(size);
                for(int i = 0; i < size; i++) {
                    double z = sample_locations(i);
                    mat phi_z =fpromp.get_phi_t(z);
                    ret(i) = phi_z * w + output_noise.at(i);
                }
                return ret;
            }

            // Equivalent to sampleFromState but uses conditioning.
            field<mat> sampleFromState2(int state, int size,
                    mt19937 &rand_generator) const {
                field<mat> no_obs;
                return sampleNextObsGivenPastObs(state, size, no_obs,
                        rand_generator);
            }

            field<mat> sampleNextObsGivenPastObs(int state, int seg_dur,
                    const field<mat>& past_obs, std::mt19937 &rng) const {
                assert(past_obs.n_elem < seg_dur);

                // Making sure all the required matrices are already computed.
                pair<int, int> p = make_pair(state, seg_dur);
                generateCachedMatrices(p);

                field<mat> ret(seg_dur);
                for(int i = 0; i < past_obs.n_elem; i++)
                    ret(i) = past_obs(i);

                const FullProMP& promp = promps_.at(state);
                const cube& Phis = getPhiCube(state, seg_dur);
                vec mu(promp.get_model().get_mu_w());

                const field<mat>& invS = cacheInvS_.at(p);
                const field<mat>& K = cacheK_.at(p);
                int i;
                for(i = 0; i < past_obs.n_elem; i++)
                    mu = mu + K(i) * (past_obs(i) - Phis.slice(i) * mu);

                // Now i indexes the offset we want to sample from.
                for(; i < seg_dur; i++) {
                    vector<vec> sample = random::sample_multivariate_normal(
                            rng, {Phis.slice(i) * mu, inv(invS(i))}, 1);
                    ret(i) = sample.at(0);
                    mu = mu + K(i) * (ret(i) - Phis.slice(i) * mu);
                }
                return ret;
            }

            field<mat> sampleFirstSegmentObsGivenLastSegment(int curr_state,
                    int curr_seg_dur, const field<mat> &last_segment,
                    int last_state, std::mt19937 &rng) const {
                mat last_obs = last_segment(last_segment.n_elem - 1);

                // Making sure all the required matrices are already computed.
                pair<int, int> p = make_pair(curr_state, curr_seg_dur);
                pair<int, int> last_p = make_pair(last_state,
                        last_segment.n_elem);
                generateCachedMatrices(p);
                mat last_p_Sigma = generateCachedMatrices(last_p);

                // Finding the posterior omega mean for the last segment.
                const cube& LPhis = getPhiCube(last_state, last_segment.n_elem);
                vec last_p_mu(promps_.at(last_state).get_model().get_mu_w());
                const field<mat>& last_K = cacheK_[last_p];
                for(int i = 0; i < last_segment.n_elem; i++) {
                    const mat& Phi = LPhis.slice(i);
                    vec diff = last_segment(i) - Phi * last_p_mu;
                    last_p_mu = last_p_mu + last_K(i) * diff;
                }
                FullProMP last_full_promp(promps_.at(last_state));
                mat zeros_Sigma_y = zeros<mat>(getDimension(), getDimension());
                ProMP last_p_promp(last_p_mu, last_p_Sigma, zeros_Sigma_y);
                last_full_promp.set_model(last_p_promp);

                // This gives the posterior distribution over q = (y, v) at the
                // last time step of the last segment.
                random::NormalDist last_pos_vel = last_full_promp.joint_dist(
                        1.0, true, true, false);
                vec last_p_pos = last_pos_vel.mean().head(getDimension());
                vec last_p_vel = last_pos_vel.mean().tail(getDimension());

                FullProMP promp = promps_.at(curr_state);
                promp = promp.condition_current_position(0, 1.0, last_p_pos);
                //promp = promp.condition_current_state(0, 1.0, last_p_pos,
                //        last_p_vel);
                return sampleFromProMP(promp, curr_seg_dur, rng);
            }

            nlohmann::json to_stream() const {
                vector<nlohmann::json> array_emission_params;
                for(int i = 0; i < getNumberStates(); i++) {
                    const ProMP& promp = promps_.at(i).get_model();
                    nlohmann::json whole_thing;
                    nlohmann::json promp_params;
                    promp_params["mu_w"] = vec2json(promp.get_mu_w());
                    promp_params["Sigma_w"] = mat2json(promp.get_Sigma_w());
                    promp_params["Sigma_y"] = mat2json(promp.get_Sigma_y());
                    whole_thing["model"] = promp_params;
                    whole_thing["num_joints"] = getDimension();

                    // Note that the information about the basis functions is not
                    // serialized.
                    array_emission_params.push_back(whole_thing);
                }
                nlohmann::json ret = array_emission_params;
                return ret;
            }

            void from_stream(const nlohmann::json& emission_params) {
                // Note that the given parameters need to be consistent with some
                // of the preexisting settings. Moreover, part of the structure of
                // this emission process is not read from the input json.
                // (e.g. the used basis functions).
                assert(emission_params.size() == getNumberStates());
                for(int i = 0; i < getNumberStates(); i++) {
                    const nlohmann::json& params = emission_params.at(i);
                    promps_.at(i).set_model(json2basic_promp(params.at("model")));
                    assert(params.at("num_joints") == getDimension());
                }
                resetMemCaches();
                return;
            }

            // TODO: change the name of this method.
            void set_Sigma_w_Prior(NormalInverseWishart prior) {
                normal_inverse_prior_ = std::make_shared<NormalInverseWishart>(
                        std::move(prior));
                int size_cov = promps_.at(0).get_model().get_Sigma_w().n_rows;
                assert(size_cov == normal_inverse_prior_->getPhi().n_rows);
            }

            void setParamsForInitialization(double fraction) {
                assert(fraction > 0 && fraction < 1.0);
                init_fraction_ = fraction;
            }

        protected:

            // Returns the marginal distribution of a particular state (ProMP) and
            // duration. Keep in mind that the covariance matrix grows quadratically
            // with respect to the duration. This could be cached for efficiency
            // but is mostly intended for debugging.
            random::NormalDist getNormalDistForMultipleTimeSteps(int state, int duration) {
                int nrows = getDimension();
                const FullProMP& promp = promps_.at(state);
                const mat stacked_Phi = getPhiStacked(state, duration);
                vec mean = stacked_Phi * promp.get_model().get_mu_w();
                mat cov = stacked_Phi * promp.get_model().get_Sigma_w() *
                    stacked_Phi.t();

                // Adding the noise variance.
                mat noise_cov(size(cov), fill::zeros);
                for(int i = 0; i < duration; i++)
                    noise_cov.submat(i * nrows, i * nrows, (i + 1) * nrows - 1,
                            (i + 1) * nrows - 1) = promp.get_model().get_Sigma_y();

                cov = cov + noise_cov;
                return random::NormalDist(mean, cov);
            }

            // Takes the covariance matrix for omega and returns it as if the
            // joints were independent (blockdiag operator in Sebastian's paper).
            mat getCovarianceIndependentJoints(const mat& cov) const {
                mat ret(size(cov), fill::zeros);
                assert(cov.n_rows == cov.n_cols);
                int dim = cov.n_rows;
                assert(dim % getDimension() == 0);
                int blocklen = dim / getDimension();
                for(int i = 0; i < dim; i++)
                    for(int j = 0; j < dim; j++)
                        if ((i/blocklen) == (j/blocklen))
                            ret(i, j) = cov(i, j);
                mat extra_term = eye<mat>(size(ret)) * 1e-4;
                return ret + extra_term;
            }

            mat getPhiStacked(int state, int duration) {
                cube Phis = getPhiCube(state, duration);
                mat PhiStacked(Phis.n_rows * Phis.n_slices, Phis.n_cols);
                for(int d = 0; d < duration; d++)
                    PhiStacked.rows(d * Phis.n_rows, (d + 1) * Phis.n_rows - 1) =
                        Phis.slice(d);
                return PhiStacked;
            }

            cube getPhiCube(int state, int duration) const {
                pair<int, int> p = make_pair(state, duration);
                if (cachePhis_.find(p) != cachePhis_.end())
                    return cachePhis_[p];
                const FullProMP& promp = promps_.at(state);

                // The samples are assumed to be equally spaced.
                vec sample_locations = getSampleLocations(duration);
                mat tmp = promp.get_phi_t(0);
                int ncols = tmp.n_cols;
                int nrows = tmp.n_rows;
                cube stacked_Phi(nrows, ncols, duration, fill::zeros);
                for(int i = 0; i < duration; i++)
                    stacked_Phi.slice(i) = promp.get_phi_t(sample_locations(i));
                cachePhis_[p] = stacked_Phi;
                return stacked_Phi;
            }

            // Returns the least squares solution to the problem:
            // y(t) = Phi(t) * w for a bunch of i.i.d. y's. Note that omega is
            // not assumed to be a random variable.
            vec least_squares_omega(int state,
                    const field<mat>& segment) const {
                int nobs = segment.n_elem;
                const cube& Phis = getPhiCube(state, nobs);
                mat A(size(Phis.slice(0).t() * Phis.slice(0)),
                        fill::zeros);
                vec b(A.n_rows, fill::zeros);
                for(int t = 0; t < nobs; t++) {
                    const mat& Phi = Phis.slice(t);
                    A = A + Phi.t() * Phi;
                    b = b + Phi.t() * segment(t);
                }

                // Solving A * w = b
                vec lsq_omega = solve(A, b);
                return lsq_omega;
            }

            // Compute the variance of an isotropic Gaussian noise model
            // given omega.
            double var_isotropic_gaussian_given_omega(int state, vec w,
                    const field<mat>& segment) const {
                int nobs = segment.n_elem;
                const cube& Phis = getPhiCube(state, nobs);
                double var = 0;
                for(int t = 0; t < nobs; t++) {
                    const mat& Phi = Phis.slice(t);
                    vec diff = segment(t) - Phi * w;
                    var += dot(diff, diff);
                }
                return var / (nobs * segment(0).n_rows);
            }

            vector<FullProMP> promps_;
            std::shared_ptr<NormalInverseWishart> normal_inverse_prior_;
            double epsilon_ = 1e-15;
            bool diagonal_sigma_y_;

            // Delta for emissions which are not dependent on the total
            // duration.
            double sample_locations_delta_ = -1;

            // Fraction of the total least squares omega estimates that will be
            // used for initialization.
            double init_fraction_ = 0.1;

            // Members for caching.
            mutable map<pair<int, int>, cube> cachePhis_;
            mutable map<pair<int, int>, field<mat>> cacheInvS_;
            mutable map<pair<int, int>, field<mat>> cacheK_;
            mutable map<pair<int, int>, mat> cachePosteriorSigma_;
    };

    // This version of the ProMP is specifically designed for HMMs where a
    // single observation is a time series itself
    class ProMPsEmissionHMM : public ProMPsEmission {
        public:
            ProMPsEmissionHMM(vector<FullProMP> promps) :
                    ProMPsEmission(promps) {}

            ProMPsEmission* clone() const {
                return new ProMPsEmission(*this);
            }

            double loglikelihood(int state, const field<mat>& obs) const {

                // Making sure that the duration is one.
                assert(obs.n_elem == 1);
                auto& sequence = obs(0);
                const FullProMP& promp = promps_.at(state);

                // The samples are assumed to be equally spaced.
                vec sample_locations = getSampleLocations(sequence.n_cols);

                vec mu(promp.get_model().get_mu_w());
                mat Sigma(promp.get_model().get_Sigma_w());
                mat Sigma_y(promp.get_model().get_Sigma_y());
                double ret = 0;
                for(int i = 0; i < sequence.n_cols; i++) {
                    mat Phi = promp.get_phi_t(sample_locations(i));
                    mat S = Phi * Sigma * Phi.t() + Sigma_y;

                    // Required for the marginal likelihood p(y_t | y_{1:t-1}).
                    random::NormalDist dist = random::NormalDist(Phi * mu, S);
                    ret = ret + log_normal_density(dist, sequence.col(i));

                    // Using the kalman updating step to compute this efficiently.
                    mat K = Sigma * Phi.t() * inv(S);
                    mu = mu + K * (sequence.col(i) - Phi * mu);
                    Sigma = Sigma - K * S * K.t();
                }
                return ret;
            }

            void reestimate(int min_duration,
                    const arma::field<arma::cube>& meta,
                    const arma::field<arma::field<arma::mat>>& mobs) {
                int nseq = mobs.n_elem;
                for(int i = 0; i < getNumberStates(); i++) {
                    ProMP promp = promps_.at(i).get_model();
                    const mat inv_Sigma_w = inv_sympd(promp.get_Sigma_w());
                    const mat inv_Sigma_y = inv_sympd(promp.get_Sigma_y());
                    const vec mu_w = promp.get_mu_w();

                    vector<double> mult_c;
                    vector<double> denominator_Sigma_y;
                    for(int s = 0; s < nseq; s++) {
                        const cube& eta = meta(s);
                        int nobs = mobs(s).n_elem;
                        int ndurations = eta.n_cols;
                        assert(ndurations == 1 && min_duration == 1);
                        for(int t = 0; t < nobs; t++) {
                            mult_c.push_back(eta(i, 0, t));
                            int current_duration = mobs(s)(t).n_cols;
                            denominator_Sigma_y.push_back(eta(i, 0, t) +
                                    log(current_duration));
                        }
                    }

                    // Computing the multiplicative constants for mu_w and
                    // Sigma_w.
                    vec mult_c_normalized(mult_c);
                    mult_c_normalized -= logsumexp(mult_c_normalized);
                    mult_c_normalized = exp(mult_c_normalized);

                    // Computing the multiplicative constants for Sigma_y since
                    // they have a different denominator.
                    vec mult_c_Sigma_y_normalized(mult_c);
                    vec den_Sigma_y(denominator_Sigma_y);
                    mult_c_Sigma_y_normalized -= logsumexp(den_Sigma_y);
                    mult_c_Sigma_y_normalized = exp(mult_c_Sigma_y_normalized);

                    mat new_Sigma_y(size(promp.get_Sigma_y()), fill::zeros);

                    // EM for ProMPs.
                    vec weighted_sum_post_mean(size(mu_w), fill::zeros);
                    mat weighted_sum_post_cov(size(inv_Sigma_w), fill::zeros);
                    mat weighted_sum_post_mean_mean_T(size(inv_Sigma_w),
                            fill::zeros);
                    int idx_mult_c = 0;
                    for(int s = 0; s < nseq; s++) {
                        auto& obs = mobs(s);
                        int nobs = obs.n_elem;
                        int ndurations = meta(s).n_cols;
                        for(int t = 0; t < nobs; t++) {

                                // Length of the current observation
                                const int current_duration = obs(t).n_cols;
                                const cube& Phis = getPhiCube(i, current_duration);

                                // E step for the emission hidden variables (Ws).
                                // Computing the posterior of W given Y and Theta.

                                // Computing the posterior covariance of the hidden
                                // variable w for this segment.
                                mat posterior_cov(size(inv_Sigma_w), fill::zeros);
                                for(int step = 0; step < current_duration; step++) {
                                    posterior_cov += Phis.slice(step).t() *
                                        inv_Sigma_y * Phis.slice(step);
                                }
                                posterior_cov = (posterior_cov+posterior_cov.t())/2.0;
                                posterior_cov = posterior_cov + inv_Sigma_w;
                                posterior_cov = (posterior_cov+posterior_cov.t())/2.0;
                                posterior_cov = inv_sympd(posterior_cov);
                                posterior_cov = (posterior_cov+posterior_cov.t())/2.0;

                                // Computing the posterior mean of the hidden
                                // variable w for this segment.
                                vec posterior_mean(size(mu_w), fill::zeros);
                                for(int step = 0; step < current_duration; step++) {
                                    const vec& ob = obs(t).col(step);
                                    posterior_mean += Phis.slice(step).t() *
                                        inv_Sigma_y * ob;
                                }
                                posterior_mean = inv_Sigma_w * mu_w + posterior_mean;
                                posterior_mean = posterior_cov * posterior_mean;

                                // Getting the multiplicative constants.
                                double mult_constant = mult_c_normalized(idx_mult_c);
                                double mult_constant_Sigma_y =
                                        mult_c_Sigma_y_normalized(idx_mult_c);
                                idx_mult_c++;

                                // Statistics required for updating mu_w & Sigma_w.
                                weighted_sum_post_mean += mult_constant *
                                        posterior_mean;
                                weighted_sum_post_cov += mult_constant *
                                        posterior_cov;
                                weighted_sum_post_mean_mean_T += mult_constant *
                                        posterior_mean * posterior_mean.t();

                                // Computing the new output noise covariance: Sigma_y.
                                mat Sigma_y_term(size(new_Sigma_y), fill::zeros);
                                for(int step = 0; step < current_duration; step++) {
                                    const mat& phi = Phis.slice(step);
                                    const vec& diff_y = obs(t).col(step) -
                                        phi * posterior_mean;
                                    Sigma_y_term += diff_y * diff_y.t() +
                                        phi * posterior_cov * phi.t();
                                }
                                new_Sigma_y += mult_constant_Sigma_y * Sigma_y_term;
                        }
                    }

                    // Expected number of segments generated by the i-th state.
                    double mle_den = exp(logsumexp(mult_c));

                    // M step for the emission variables.
                    vec new_mu_w_MLE(weighted_sum_post_mean);
                    mat new_Sigma_w_MLE = weighted_sum_post_cov +
                        weighted_sum_post_mean_mean_T - new_mu_w_MLE *
                        new_mu_w_MLE.t();

                    // If there is a prior then we do MAP instead.
                    mat new_Sigma_w;
                    mat new_mu_w;
                    if (normal_inverse_prior_) {
                        double v_0 = normal_inverse_prior_->getDof();
                        double D = mu_w.n_rows;
                        mat S_0 = normal_inverse_prior_->getPhi();
                        new_Sigma_w = (S_0 + mle_den * new_Sigma_w_MLE) /
                                (v_0 + mle_den + D + 2);

                        double k_0 = normal_inverse_prior_->getLambda();
                        vec m_0 = normal_inverse_prior_->getMu0();
                        new_mu_w = (k_0 * m_0 + mle_den * new_mu_w_MLE) /
                                (mle_den + k_0);
                    }
                    else {
                        new_Sigma_w = new_Sigma_w_MLE;
                        new_mu_w = new_mu_w_MLE;
                    }

                    cout << "State " << i << " MLE Den: " << mle_den << " ";
                    if (mle_den > epsilon_) {

                        // Making sure the noise covariance is diagonal.
                        if (diagonal_sigma_y_)
                            new_Sigma_y = diagmat(new_Sigma_y.diag());

                        // Checking that the new Sigma_w is a covariance matrix.
                        vec eigenvalues_map = eig_sym(new_Sigma_w);
                        assert(eigenvalues_map(0) > 0);

                        // Setting the new parameters.
                        promp.set_mu_w(new_mu_w);
                        promp.set_Sigma_w(new_Sigma_w);
                        promp.set_Sigma_y(new_Sigma_y);
                        promps_.at(i).set_model(promp);
                        cout << ". Updated." << endl;
                    }
                    else
                        cout << ". Not updated." << endl;
                }
            }

            field<mat> sampleFromState(int state, int size,
                    mt19937 &rand_generator) const {

                // Since the segments are modeled as single observations.
                assert(size == 1);
                const ProMP& model = promps_.at(state).get_model();
                vector<vec> w_samples = random::sample_multivariate_normal(
                        rand_generator, {model.get_mu_w(), model.get_Sigma_w()}, 1);
                vec w = w_samples.back();

                // Getting the actual size of the segment.
                size = getDurationForEachSegment(rand_generator);

                vec noise_mean = zeros<vec>(getDimension());
                vector<vec> output_noise = random::sample_multivariate_normal(
                        rand_generator, {noise_mean, model.get_Sigma_y()}, size);

                mat joint_sample(getDimension(), size);

                // The samples are assumed to be equally spaced.
                vec sample_locations = getSampleLocations(size);
                for(int i = 0; i < size; i++) {
                    double z = sample_locations(i);
                    mat phi_z = promps_.at(state).get_phi_t(z);
                    joint_sample.col(i) = phi_z * w + output_noise.at(i);
                }
                field<mat> ret = {joint_sample};
                return ret;
            }

        private:

            int getDurationForEachSegment(mt19937 &rand_generator) const {
                return 20;
            }

    };

};

#endif


// Copyright 2008 Gautam Sewani
// Copyright 2008 John Maddock
// Copyright 2021 Paul A. Bristow
//
// 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)

#ifndef BOOST_MATH_DISTRIBUTIONS_HYPERGEOMETRIC_HPP
#define BOOST_MATH_DISTRIBUTIONS_HYPERGEOMETRIC_HPP

#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/hypergeometric_pdf.hpp>
#include <boost/math/distributions/detail/hypergeometric_cdf.hpp>
#include <boost/math/distributions/detail/hypergeometric_quantile.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

namespace boost { namespace math {

   template <class RealType = double, class Policy = policies::policy<> >
   class hypergeometric_distribution
   {
   public:
      typedef RealType value_type;
      typedef Policy policy_type;

      hypergeometric_distribution(unsigned r, unsigned n, unsigned N) // Constructor. r=defective/failures/success, n=trials/draws, N=total population.
         : m_n(n), m_N(N), m_r(r)
      {
         static const char* function = "boost::math::hypergeometric_distribution<%1%>::hypergeometric_distribution";
         RealType ret;
         check_params(function, &ret);
      }
      // Accessor functions.
      unsigned total()const
      {
         return m_N;
      }

      unsigned defective()const // successes/failures/events
      {
         return m_r;
      }

      unsigned sample_count()const
      {
         return m_n;
      }

      bool check_params(const char* function, RealType* result)const
      {
         if(m_r > m_N)
         {
            *result = boost::math::policies::raise_domain_error<RealType>(
               function, "Parameter r out of range: must be <= N but got %1%", static_cast<RealType>(m_r), Policy());
            return false;
         }
         if(m_n > m_N)
         {
            *result = boost::math::policies::raise_domain_error<RealType>(
               function, "Parameter n out of range: must be <= N but got %1%", static_cast<RealType>(m_n), Policy());
            return false;
         }
         return true;
      }
      bool check_x(unsigned x, const char* function, RealType* result)const
      {
         if(x < static_cast<unsigned>((std::max)(0, (int)(m_n + m_r) - (int)(m_N))))
         {
            *result = boost::math::policies::raise_domain_error<RealType>(
               function, "Random variable out of range: must be > 0 and > m + r - N but got %1%", static_cast<RealType>(x), Policy());
            return false;
         }
         if(x > (std::min)(m_r, m_n))
         {
            *result = boost::math::policies::raise_domain_error<RealType>(
               function, "Random variable out of range: must be less than both n and r but got %1%", static_cast<RealType>(x), Policy());
            return false;
         }
         return true;
      }

   private:
      // Data members:
      unsigned m_n;  // number of items picked or drawn.
      unsigned m_N; // number of "total" items.
      unsigned m_r; // number of "defective/successes/failures/events items.

   }; // class hypergeometric_distribution

   typedef hypergeometric_distribution<double> hypergeometric;

   template <class RealType, class Policy>
   inline const std::pair<unsigned, unsigned> range(const hypergeometric_distribution<RealType, Policy>& dist)
   { // Range of permissible values for random variable x.
#ifdef _MSC_VER
#  pragma warning(push)
#  pragma warning(disable:4267)
#endif
      unsigned r = dist.defective();
      unsigned n = dist.sample_count();
      unsigned N = dist.total();
      unsigned l = static_cast<unsigned>((std::max)(0, (int)(n + r) - (int)(N)));
      unsigned u = (std::min)(r, n);
      return std::pair<unsigned, unsigned>(l, u);
#ifdef _MSC_VER
#  pragma warning(pop)
#endif
   }

   template <class RealType, class Policy>
   inline const std::pair<unsigned, unsigned> support(const hypergeometric_distribution<RealType, Policy>& d)
   {
      return range(d);
   }

   template <class RealType, class Policy>
   inline RealType pdf(const hypergeometric_distribution<RealType, Policy>& dist, const unsigned& x)
   {
      static const char* function = "boost::math::pdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType result = 0;
      if(!dist.check_params(function, &result))
         return result;
      if(!dist.check_x(x, function, &result))
         return result;

      return boost::math::detail::hypergeometric_pdf<RealType>(
         x, dist.defective(), dist.sample_count(), dist.total(), Policy());
   }

   template <class RealType, class Policy, class U>
   inline RealType pdf(const hypergeometric_distribution<RealType, Policy>& dist, const U& x)
   {
      BOOST_MATH_STD_USING
      static const char* function = "boost::math::pdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType r = static_cast<RealType>(x);
      unsigned u = itrunc(r, typename policies::normalise<Policy, policies::rounding_error<policies::ignore_error> >::type());
      if(u != r)
      {
         return boost::math::policies::raise_domain_error<RealType>(
            function, "Random variable out of range: must be an integer but got %1%", r, Policy());
      }
      return pdf(dist, u);
   }

   template <class RealType, class Policy>
   inline RealType cdf(const hypergeometric_distribution<RealType, Policy>& dist, const unsigned& x)
   {
      static const char* function = "boost::math::cdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType result = 0;
      if(!dist.check_params(function, &result))
         return result;
      if(!dist.check_x(x, function, &result))
         return result;

      return boost::math::detail::hypergeometric_cdf<RealType>(
         x, dist.defective(), dist.sample_count(), dist.total(), false, Policy());
   }

   template <class RealType, class Policy, class U>
   inline RealType cdf(const hypergeometric_distribution<RealType, Policy>& dist, const U& x)
   {
      BOOST_MATH_STD_USING
      static const char* function = "boost::math::cdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType r = static_cast<RealType>(x);
      unsigned u = itrunc(r, typename policies::normalise<Policy, policies::rounding_error<policies::ignore_error> >::type());
      if(u != r)
      {
         return boost::math::policies::raise_domain_error<RealType>(
            function, "Random variable out of range: must be an integer but got %1%", r, Policy());
      }
      return cdf(dist, u);
   }

   template <class RealType, class Policy>
   inline RealType cdf(const complemented2_type<hypergeometric_distribution<RealType, Policy>, unsigned>& c)
   {
      static const char* function = "boost::math::cdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType result = 0;
      if(!c.dist.check_params(function, &result))
         return result;
      if(!c.dist.check_x(c.param, function, &result))
         return result;

      return boost::math::detail::hypergeometric_cdf<RealType>(
         c.param, c.dist.defective(), c.dist.sample_count(), c.dist.total(), true, Policy());
   }

   template <class RealType, class Policy, class U>
   inline RealType cdf(const complemented2_type<hypergeometric_distribution<RealType, Policy>, U>& c)
   {
      BOOST_MATH_STD_USING
      static const char* function = "boost::math::cdf(const hypergeometric_distribution<%1%>&, const %1%&)";
      RealType r = static_cast<RealType>(c.param);
      unsigned u = itrunc(r, typename policies::normalise<Policy, policies::rounding_error<policies::ignore_error> >::type());
      if(u != r)
      {
         return boost::math::policies::raise_domain_error<RealType>(
            function, "Random variable out of range: must be an integer but got %1%", r, Policy());
      }
      return cdf(complement(c.dist, u));
   }

   template <class RealType, class Policy>
   inline RealType quantile(const hypergeometric_distribution<RealType, Policy>& dist, const RealType& p)
   {
      BOOST_MATH_STD_USING // for ADL of std functions

         // Checking function argument
         RealType result = 0;
      const char* function = "boost::math::quantile(const hypergeometric_distribution<%1%>&, %1%)";
      if (false == dist.check_params(function, &result)) return result;
      if(false == detail::check_probability(function, p, &result, Policy())) return result;

      return static_cast<RealType>(detail::hypergeometric_quantile(p, RealType(1 - p), dist.defective(), dist.sample_count(), dist.total(), Policy()));
   } // quantile

   template <class RealType, class Policy>
   inline RealType quantile(const complemented2_type<hypergeometric_distribution<RealType, Policy>, RealType>& c)
   {
      BOOST_MATH_STD_USING // for ADL of std functions

      // Checking function argument
      RealType result = 0;
      const char* function = "quantile(const complemented2_type<hypergeometric_distribution<%1%>, %1%>&)";
      if (false == c.dist.check_params(function, &result)) return result;
      if(false == detail::check_probability(function, c.param, &result, Policy())) return result;

      return static_cast<RealType>(detail::hypergeometric_quantile(RealType(1 - c.param), c.param, c.dist.defective(), c.dist.sample_count(), c.dist.total(), Policy()));
   } // quantile

   // https://www.wolframalpha.com/input/?i=kurtosis+hypergeometric+distribution

   template <class RealType, class Policy>
   inline RealType mean(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      return static_cast<RealType>(dist.defective() * dist.sample_count()) / dist.total();
   } // RealType mean(const hypergeometric_distribution<RealType, Policy>& dist)

   template <class RealType, class Policy>
   inline RealType variance(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      RealType r = static_cast<RealType>(dist.defective());
      RealType n = static_cast<RealType>(dist.sample_count());
      RealType N = static_cast<RealType>(dist.total());
      return n * r  * (N - r) * (N - n) / (N * N * (N - 1));
   } // RealType variance(const hypergeometric_distribution<RealType, Policy>& dist)

   template <class RealType, class Policy>
   inline RealType mode(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      BOOST_MATH_STD_USING
      RealType r = static_cast<RealType>(dist.defective());
      RealType n = static_cast<RealType>(dist.sample_count());
      RealType N = static_cast<RealType>(dist.total());
      return floor((r + 1) * (n + 1) / (N + 2));
   }

   template <class RealType, class Policy>
   inline RealType skewness(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      BOOST_MATH_STD_USING
      RealType r = static_cast<RealType>(dist.defective());
      RealType n = static_cast<RealType>(dist.sample_count());
      RealType N = static_cast<RealType>(dist.total());
      return (N - 2 * r) * sqrt(N - 1) * (N - 2 * n) / (sqrt(n * r * (N - r) * (N - n)) * (N - 2));
   } // RealType skewness(const hypergeometric_distribution<RealType, Policy>& dist)

   template <class RealType, class Policy>
   inline RealType kurtosis_excess(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      // https://www.wolframalpha.com/input/?i=kurtosis+hypergeometric+distribution shown as plain text:
      //  mean | (m n)/N
      //  standard deviation | sqrt((m n(N - m) (N - n))/(N - 1))/N
      //  variance | (m n(1 - m/N) (N - n))/((N - 1) N)
      //  skewness | (sqrt(N - 1) (N - 2 m) (N - 2 n))/((N - 2) sqrt(m n(N - m) (N - n)))
      //  kurtosis | ((N - 1) N^2 ((3 m(N - m) (n^2 (-N) + (n - 2) N^2 + 6 n(N - n)))/N^2 - 6 n(N - n) + N(N + 1)))/(m n(N - 3) (N - 2) (N - m) (N - n))
     // Kurtosis[HypergeometricDistribution[n, m, N]]
      RealType m = static_cast<RealType>(dist.defective()); // Failures or success events. (Also symbols K or M are used).
      RealType n = static_cast<RealType>(dist.sample_count()); // draws or trials.
      RealType n2 = n * n; // n^2
      RealType N = static_cast<RealType>(dist.total()); // Total population from which n draws or trials are made.
      RealType N2 = N * N; // N^2
      // result = ((N - 1) N^2 ((3 m(N - m) (n^2 (-N) + (n - 2) N^2 + 6 n(N - n)))/N^2 - 6 n(N - n) + N(N + 1)))/(m n(N - 3) (N - 2) (N - m) (N - n));
      RealType result = ((N-1)*N2*((3*m*(N-m)*(n2*(-N)+(n-2)*N2+6*n*(N-n)))/N2-6*n*(N-n)+N*(N+1)))/(m*n*(N-3)*(N-2)*(N-m)*(N-n));
      // Agrees with kurtosis hypergeometric distribution(50,200,500) kurtosis = 2.96917
      // N[kurtosis[hypergeometricdistribution(50,200,500)], 55]  2.969174035736058474901169623721804275002985337280263464
      return result;
   } // RealType kurtosis_excess(const hypergeometric_distribution<RealType, Policy>& dist)

   template <class RealType, class Policy>
   inline RealType kurtosis(const hypergeometric_distribution<RealType, Policy>& dist)
   {
      return kurtosis_excess(dist) + 3;
   } // RealType kurtosis_excess(const hypergeometric_distribution<RealType, Policy>& dist)
}} // namespaces

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // include guard