/*
 * LaplaceM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <algorithm>
#include <chrono>
#include <iomanip>
#include <iostream>

#define DIRECTLAYER 2
#define PI314 (static_cast<double>(3.1415926535897932384626433))

namespace Laplace3D3D {

using EVec3 = Eigen::Vector3d;
using EVec4 = Eigen::Vector4d;
constexpr double eps = 1e-10;

// real and wave sum of 2D Laplace kernel Ewald

inline double freal(double xi, double r) { return std::erfc(xi * r) / r; }

inline double frealp(double xi, double r) {
    return -(2. * exp(-r * r * (xi * xi)) * xi) / (sqrt(M_PI) * r) - std::erfc(r * xi) / (r * r);
}

// xm: target, xn: source
inline double realSum(const double xi, const EVec3 &xn, const EVec3 &xm) {
    EVec3 rmn = xm - xn;
    double rnorm = rmn.norm();
    if (rnorm < eps) {
        return 0;
    }
    return freal(xi, rnorm);
}

inline double potEwald(const EVec3 &xm, const EVec3 &xn) {
    const double xi = 2; // recommend for box=1 to get machine precision
    EVec3 target = xm;
    EVec3 source = xn;
    target[0] = target[0] - floor(target[0]); // periodic BC
    target[1] = target[1] - floor(target[1]);
    target[2] = target[2] - floor(target[2]);
    source[0] = source[0] - floor(source[0]);
    source[1] = source[1] - floor(source[1]);
    source[2] = source[2] - floor(source[2]);

    // real sum
    int rLim = 4;
    double Kreal = 0;
    for (int i = -rLim; i <= rLim; i++) {
        for (int j = -rLim; j <= rLim; j++) {
            for (int k = -rLim; k <= rLim; k++) {
                Kreal += realSum(xi, target, source - EVec3(i, j, k));
            }
        }
    }

    // wave sum
    int wLim = 4;
    double Kwave = 0;
    EVec3 rmn = target - source;
    const double xi2 = xi * xi;
    const double rmnnorm = rmn.norm();
    for (int i = -wLim; i <= wLim; i++) {
        for (int j = -wLim; j <= wLim; j++) {
            for (int k = -wLim; k <= wLim; k++) {
                if (i == 0 && j == 0 && k == 0) {
                    continue;
                }
                EVec3 kvec = EVec3(i, j, k) * (2 * PI314);
                double k2 = kvec.dot(kvec);
                Kwave += 4 * PI314 * cos(kvec.dot(rmn)) * exp(-k2 / (4 * xi2)) / k2;
            }
        }
    }

    double Kself = rmnnorm < 1e-10 ? -2 * xi / sqrt(PI314) : 0;

    return Kreal + Kwave + Kself - PI314 / xi2;
}

inline void realGradSum(double xi, const EVec3 &target, const EVec3 &source, EVec3 &v) {
    EVec3 rvec = target - source;
    double rnorm = rvec.norm();
    if (rnorm < eps) {
        v.setZero();
    } else {
        v = (frealp(xi, rnorm) / rnorm) * rvec;
    }
}

inline void gradkernel(const EVec3 &target, const EVec3 &source, EVec3 &answer) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < eps) {
        answer.setZero();
        return;
    }
    double rnorm3 = rnorm * rnorm * rnorm;
    answer = -rst / rnorm3;
}

// grad of Laplace potential, without 1/4pi prefactor, periodic of -r_k/r^3
inline void gradEwald(const EVec3 &target_, const EVec3 &source_, EVec3 &answer) {
    EVec3 target = target_;
    EVec3 source = source_;
    target[0] = target[0] - floor(target[0]); // periodic BC
    target[1] = target[1] - floor(target[1]);
    target[2] = target[2] - floor(target[2]);
    source[0] = source[0] - floor(source[0]);
    source[1] = source[1] - floor(source[1]);
    source[2] = source[2] - floor(source[2]);

    double xi = 0.54;

    // real sum
    int rLim = 10;
    EVec3 Kreal = EVec3::Zero();
    for (int i = -rLim; i < rLim + 1; i++) {
        for (int j = -rLim; j < rLim + 1; j++) {
            for (int k = -rLim; k < rLim + 1; k++) {
                EVec3 v = EVec3::Zero();
                realGradSum(xi, target, source + EVec3(i, j, k), v);
                Kreal += v;
            }
        }
    }

    // wave sum
    int wLim = 10;
    EVec3 rmn = target - source;
    double xi2 = xi * xi;
    EVec3 Kwave(0., 0., 0.);
    for (int i = -wLim; i < wLim + 1; i++) {
        for (int j = -wLim; j < wLim + 1; j++) {
            for (int k = -wLim; k < wLim + 1; k++) {
                if (i == 0 && j == 0 && k == 0)
                    continue;
                EVec3 kvec = EVec3(i, j, k) * (2 * M_PI);
                double k2 = kvec.dot(kvec);
                double knorm = kvec.norm();
                Kwave += -kvec * (sin(kvec.dot(rmn)) * exp(-k2 / (4 * xi2)) / k2);
            }
        }
    }

    answer = Kreal + Kwave;
}

inline double pot(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    return rnorm < eps ? 0 : 1 / rnorm;
}

inline EVec4 gKernel(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    EVec4 pgrad = EVec4::Zero();
    double rnorm = rst.norm();
    if (rnorm < eps) {
        pgrad.setZero();
    } else {
        pgrad[0] = 1 / rnorm;
        double rnorm3 = rnorm * rnorm * rnorm;
        pgrad.block<3, 1>(1, 0) = -rst / rnorm3;
    }
    return pgrad;
}

inline EVec4 gKernelEwald(const EVec3 &target, const EVec3 &source) {
    EVec4 pgrad = EVec4::Zero();
    pgrad[0] = potEwald(target, source);
    EVec3 grad;
    gradEwald(target, source, grad);
    pgrad[1] = grad[0];
    pgrad[2] = grad[1];
    pgrad[3] = grad[2];
    return pgrad;
}

inline EVec4 gKernelNF(const EVec3 &target, const EVec3 &source, int N = DIRECTLAYER) {
    EVec4 gNF = EVec4::Zero();
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                EVec4 gFree = gKernel(target, source + EVec3(i, j, k));
                gNF += gFree;
            }
        }
    }
    return gNF;
}

// Out of Direct Sum Layer, far field part
inline EVec4 gKernelFF(const EVec3 &target, const EVec3 &source) {
    EVec4 fEwald = gKernelEwald(target, source);
    fEwald -= gKernelNF(target, source);
    return fEwald;
}

inline double potNF(const EVec3 &target, const EVec3 &source, int N = DIRECTLAYER) {
    double gNF = 0;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                gNF += pot(target, source + EVec3(i, j, k));
            }
        }
    }
    return gNF;
}

// Out of Direct Sum Layer, far field part
inline double potFF(const EVec3 &target, const EVec3 &source) {
    double fEwald = potEwald(target, source);
    fEwald -= potNF(target, source);
    return fEwald;
}

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */

template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);

    std::cout << std::scientific << std::setprecision(18);

    std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {-(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {-(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    const double scaleLEquiv = 1.05;
    const double scaleLCheck = 2.95;
    const double pCenterLEquiv[3] = {-(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2};
    const double pCenterLCheck[3] = {-(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2};

    auto pointMEquiv = surface(pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(pCheck, (double *)&(pCenterCheck[0]), scaleCheck, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    auto pointLEquiv = surface(pEquiv, (double *)&(pCenterLCheck[0]), scaleLCheck, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointLCheck = surface(pCheck, (double *)&(pCenterLEquiv[0]), scaleLEquiv, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd M2L(equivN, equivN); // Laplace, 1->1

    // Aup for solving MEquiv
    Eigen::MatrixXd Aup(checkN, equivN);
    Eigen::MatrixXd AuppinvU(Aup.cols(), Aup.rows());
    Eigen::MatrixXd AuppinvVT(Aup.cols(), Aup.rows());
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1], pointMEquiv[3 * l + 2]);
            Aup(k, l) = pot(Cpoint, Lpoint);
        }
    }
    pinv(Aup, AuppinvU, AuppinvVT);

    // condition number
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Aup);
    double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1);
    std::cout << cond << std::endl;
    std::cout << "s:" << svd.singularValues() << std::endl;

    // Adown for solving LEquiv
    Eigen::MatrixXd Adown(checkN, equivN);
    Eigen::MatrixXd AdownpinvU(Adown.cols(), Adown.rows());
    Eigen::MatrixXd AdownpinvVT(Adown.cols(), Adown.rows());
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l], pointLEquiv[3 * l + 1], pointLEquiv[3 * l + 2]);
            Adown(k, l) = pot(Cpoint, Lpoint);
        }
    }
    pinv(Adown, AdownpinvU, AdownpinvVT);

#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1], pointMEquiv[3 * i + 2]);
        const EVec3 Npoint(0.5, 0.5, 0.5);
        Eigen::VectorXd f(checkN);
        f.setZero();
        for (int k = 0; k < checkN; k++) {
            EVec3 Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
            f[k] = potFF(Cpoint, Mpoint) - potFF(Cpoint, Npoint);
        }

        M2L.col(i) = (AdownpinvU.transpose() * (AdownpinvVT.transpose() * f));
    }
    std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();

    // dump M2L
    for (int i = 0; i < equivN; i++) {
        for (int j = 0; j < equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    std::cout << "Precomputing time:" << duration / 1e6 << std::endl;

    // Test
    EVec3 center(0.6, 0.5, 0.5);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> chargePoint(2);
    std::vector<double> chargeValue(2);
    chargePoint[0] = center + EVec3(0.1, 0, 0);
    chargeValue[0] = 1;
    chargePoint[1] = center + EVec3(-0.1, 0., 0.);
    chargeValue[1] = -1;
    // chargePoint[2] = center + EVec3(-0.2, 0., 0.);
    // chargeValue[2] = 1;

    // solve M
    Eigen::VectorXd f(checkN);
    for (int k = 0; k < checkN; k++) {
        double temp = 0;
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        for (size_t p = 0; p < chargePoint.size(); p++) {
            temp = temp + pot(Cpoint, chargePoint[p]) * (chargeValue[p]);
        }
        f[k] = temp;
    }
    Eigen::VectorXd Msource = (AuppinvU.transpose() * (AuppinvVT.transpose() * f));

    std::cout << "Msource " << Msource << std::endl;

    std::cout << "backward error: " << f - Aup * Msource << std::endl;

    // check dipole moment
    {
        EVec3 dipole = EVec3::Zero();
        for (int i = 0; i < chargePoint.size(); i++) {
            dipole += chargeValue[i] * chargePoint[i];
        }
        std::cout << "charge dipole " << dipole.transpose() << std::endl;
    }
    {
        EVec3 dipole = EVec3::Zero();
        for (int i = 0; i < equivN; i++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1], pointMEquiv[3 * i + 2]);
            dipole += Mpoint * Msource[i];
        }
        std::cout << "Mequiv dipole " << dipole.transpose() << std::endl;
    }

    Eigen::VectorXd M2Lsource = M2L * (Msource);

    for (int is = 0; is < 5; is++) {

        Eigen::Vector3d samplePoint = EVec3::Random() * 0.2 + EVec3(0.5, 0.5, 0.5);

        EVec4 UFFL2T = EVec4::Zero();
        EVec4 UFFS2T = EVec4::Zero();
        EVec4 UFFM2T = EVec4::Zero();

#pragma omp sections
        {
#pragma omp section
            for (int p = 0; p < chargePoint.size(); p++) {
                UFFS2T += gKernelFF(samplePoint, chargePoint[p]) * chargeValue[p];
            }

#pragma omp section
            for (int p = 0; p < equivN; p++) {
                Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1], pointLEquiv[3 * p + 2]);
                UFFL2T += gKernel(samplePoint, Lpoint) * M2Lsource[p];
            }

#pragma omp section
            for (int p = 0; p < equivN; p++) {
                Eigen::Vector3d Mpoint(pointMEquiv[3 * p], pointMEquiv[3 * p + 1], pointMEquiv[3 * p + 2]);
                UFFM2T += gKernelFF(samplePoint, Mpoint) * Msource[p];
            }
        }
        std::cout << std::scientific << std::setprecision(10);
        std::cout << "-----------------------------------------------" << std::endl;
        std::cout << "samplePoint:" << samplePoint.transpose() << std::endl;
        std::cout << "UFF S2T: " << UFFS2T.transpose() << std::endl;
        std::cout << "UFF M2T: " << UFFM2T.transpose() << std::endl;
        std::cout << "UFF L2T: " << UFFL2T.transpose() << std::endl;
        std::cout << "Error M2T: " << (UFFM2T - UFFS2T).transpose() << std::endl;
        std::cout << "Error L2T: " << (UFFL2T - UFFS2T).transpose() << std::endl;
    }

    return 0;
}

} // namespace Laplace3D3D

#undef DIRECTLAYER
#undef PI314


#ifndef INVERT_MATRIX_HPP
#define INVERT_MATRIX_HPP

 // REMEMBER to update "lu.hpp" header includes from boost-CVS
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
namespace ublas = boost::numeric::ublas;
/* Matrix inversion routine.
   Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */

bool InvertMatrix (const ublas::matrix<double>& input, ublas::matrix<double>& inverse);
bool InvertMatrixGen(const ublas::matrix<double>& input, ublas::matrix<double>& inverse);
#endif //INVERT_MATRIX_HPP


/** @file
 *****************************************************************************

 Declaration of interfaces for a secret-key lattice-based additively homomorphic
 vector encryption scheme.

 This includes:
 - class for secret key
 - class for ciphertext
 - key generation algorithm
 - encryption algorithm
 - decryption algorithm
 - operations for homomorphic addition and scalar multiplication of ciphertexts

 The implementation instantiates (a modification of) the LWE-based cryptosystem
 from [LP10] (described in [Pei16, Section 5.2.3]). The implementation encodes
 the message in the low-order bits of the ciphertext.

 References:

  [LP10]: Richard Lindner and Chris Peikert. Better Key Sizes (and Attacks) for
          LWE-Based Encryption. In CT-RSA, 2011.

  [Pei16]: Chris Peikert. A Decade of Lattice Cryptography. Available as
           Report 2015/939 on IACR Cryptology ePrint Archive
           (https://eprint.iacr.org/2015/939.pdf).

 *****************************************************************************
 * @author     Samir Menon, Brennan Shacklett, and David J. Wu
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/

#ifndef LWE_HPP_
#define LWE_HPP_

#include <NTL/mat_ZZ_p.h>
#include <random>
#include "lwe_params.hpp"

namespace LWE {

using matrix = NTL::mat_ZZ_p;
using vector = NTL::vec_ZZ_p;
using plaintext = vector;

class secret_key {
public:
    matrix A {NTL::INIT_SIZE, n + pt_dim, n};
    matrix S {NTL::INIT_SIZE, n + pt_dim, pt_dim};
};

class ciphertext {
public:
  // Assignment operator
  ciphertext& operator=(const ciphertext& other);

  // Homomorphic addition
  ciphertext operator+(const ciphertext &other) const;
  ciphertext& operator+=(const ciphertext &other);

  // Homomorphic scalar multiplication
  ciphertext operator*(uint64_t val) const;
  ciphertext operator*(const NTL::ZZ_p &val) const;
  ciphertext& operator*=(uint64_t val);
  ciphertext& operator*=(const NTL::ZZ_p &val);

private:
  vector ctxt;

friend ciphertext encrypt(const secret_key &sk, const plaintext &pt);
friend plaintext  decrypt(const secret_key &sk, const ciphertext &ct);
};

secret_key keygen();
ciphertext encrypt(const secret_key &sk, const plaintext &pt);
plaintext  decrypt(const secret_key &sk, const ciphertext &ct);

ciphertext operator*(uint64_t val, const ciphertext& ct);
ciphertext operator*(const NTL::ZZ_p &val, const ciphertext& ct);

}

#endif // LWE_HPP_


//
// Extended boost::date_time functionality
//

#include <boost/date_time/gregorian/gregorian.hpp>
#include <string>
#include <sstream>
#include <vector>

using namespace boost::gregorian;

// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
// Helper functions

auto inc_year(date d, int n = 1) noexcept
    // increment the year but keep month and day the same
{
    return date{d.year() + n, d.month(), d.day()};
}

auto date_in_year(date d, greg_year y = day_clock::local_day().year()) noexcept
    // return date d in the provided year
{
    return date{y, d.month(), d.day()};
}

auto next_occurrence(date d, date ref = day_clock::local_day()) noexcept
    // return the next occurrence of date d after reference date
{
    auto d_ref_year = date_in_year(d, ref.year());
    return (d_ref_year <= ref) ? inc_year(d_ref_year) : d_ref_year;
}

auto next_date_of_day(date d, greg_day wd) noexcept
    // return the year in which date d falls on weekday wd
{
    do {
        d = inc_year(d);
    } while (d.day_of_week() != wd);

    return d;
}

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

auto get_date_results(const std::string& s)
    // return vector<string> report of date data
{
    std::vector<std::string> res;
    std::stringstream ss1;          // QUESTION: Is there a better way?
    std::stringstream ss2;
    std::stringstream ss3;

    res.push_back(s);

    auto birthday(from_simple_string(s));
    ss1 << "You were born on a "
        << birthday.day_of_week().as_long_string() << '.';
    res.emplace_back(ss1.str());

    auto this_bd = date_in_year(birthday);
    auto next_bd = next_occurrence(this_bd);
    auto days_away = (next_bd - day_clock::local_day()).days();
    ss2 << "Your next birthday is in "
        << days_away << " days on a "
        << next_bd.day_of_week().as_long_string() << '.';
    res.emplace_back(ss2.str());

    auto sat_bd = next_date_of_day(inc_year(next_bd), Saturday);
    ss3 << "The next time your birthday is on a Saturday is in "
        << sat_bd.year() << '.';
    res.emplace_back(ss3.str());

    return res;
}


 //
// Created by Himatya on 2018/05/22.
//

#pragma once

#include <chrono>
#include <vector>
#include <iterator>

#include <boost/optional.hpp>

namespace mk2 {
namespace chrono {

    namespace detail
    {
        class frame_counter_container
        {
        public:
            frame_counter_container(std::chrono::nanoseconds range) : range_(range) {}

            void push(std::chrono::nanoseconds ns)
            {
                while (time_ + (ns - time_split_.front()) > range_)
                {
                    time_ -= time_split_.front();
                    time_split_.erase(std::begin(time_split_));
                }
                time_ += ns;
                time_split_.emplace_back(ns);
            }

            bool is_valid() const
            { return range_ <= time_; }

            auto get_time() const
            { return time_; }

            auto get_range() const
            { return range_; }

            auto get_frame() const
            { return time_split_.size(); }

        private:
            std::vector<std::chrono::nanoseconds> time_split_;
            std::chrono::nanoseconds range_;
            std::chrono::nanoseconds time_;
        };
    }

    class frame_counter
    {
    public:
        frame_counter(std::chrono::nanoseconds range) : container_(range) {}

        ~frame_counter() = default;

        void tick()
        {
            auto point = std::chrono::steady_clock::now();
            auto dlt = point - time_point_;
            container_.push(std::chrono::duration_cast<std::chrono::milliseconds>(dlt));
            time_point_ = point;
        }

        boost::optional<double> get_fps()
        {
            if(!container_.is_valid())
                return boost::none;

            return (double)container_.get_frame().count() * container_.get_time().count() / container_.get_range().count();
        }

    private:
        std::chrono::time_point time_point_;
        mk2::chrono::detail::frame_counter_container container_;
    };

}
}


/**
 * Some additional useful distributions (only bare operator()).
 */
#ifndef SKYLARK_DISTRIBUTIONS_HPP
#define SKYLARK_DISTRIBUTIONS_HPP

#include <boost/random.hpp>

namespace skylark {
namespace utility {

/**
 * Levy distribution
 */
template< typename ValueType >
struct standard_levy_distribution_t {

    // TODO not really sure this is the standard, or implemented correctly

    typedef ValueType result_type;

    standard_levy_distribution_t() {

    }

    template< typename URNG >
    ValueType operator()(URNG &prng) const {
        boost::random::gamma_distribution<ValueType> dist(0.5, 2);
        result_type y = static_cast<ValueType>(dist(prng));
        return (1.0 / y);
    }
    void reset() {}

};

/**
 * Radamachar distribution - +1 and -1 with equal probability.
 */
template< typename ValueType >
struct rademacher_distribution_t {

    typedef ValueType result_type;

    template< typename URNG >
    ValueType operator()(URNG &prng) const {
        double probabilities[] = { 0.5, 0.0, 0.5 };
        boost::random::discrete_distribution<> dist(probabilities);
        return static_cast<ValueType>(dist(prng)) - 1.0;
    }
    void reset() {}
};

/**
 * Uniform distribution
 */
template <typename ValueType> struct uniform_distribution_t {
    typedef ValueType result_type;
};

/**
 * Uniform distribution specialization for double's
 */
template <> struct uniform_distribution_t <double> {

    typedef double result_type;

    boost::random::uniform_real_distribution<double> distribution;

    uniform_distribution_t() {}

    uniform_distribution_t(double low, double high) :
      distribution(low, high) {}

    template< typename URNG >
    double operator()(URNG &urng) const {
        return distribution(urng);
    }
    void reset() {}
};

/**
 * Uniform distribution specialization for int's
 */
template <> struct uniform_distribution_t <int> {

    typedef int result_type;

    boost::random::uniform_int_distribution<int> distribution;

    uniform_distribution_t() {}

    uniform_distribution_t(int low, int high) :
      distribution(low, high) {}

    template< typename URNG >
    int operator()(URNG &urng) const {
        return distribution(urng);
    }
    void reset() {}
};

/**
 * Uniform distribution specialization for size_t
 */
template <> struct uniform_distribution_t <size_t> {

    typedef int result_type;

    boost::random::uniform_int_distribution<size_t> distribution;

    uniform_distribution_t() {}

    uniform_distribution_t(size_t low, size_t high) :
      distribution(low, high) {}

    template< typename URNG >
    int operator()(URNG &urng) const {
        return distribution(urng);
    }
    void reset() {}
};

/**
 * Uniform distribution specialization for bool's
 */
template <> struct uniform_distribution_t <bool> {

    typedef bool result_type;

    boost::random::uniform_int_distribution<int> distribution;

    uniform_distribution_t() {}

    template< typename URNG >
    bool operator()(URNG &urng) const {
        return (1==distribution(urng));
    }
    void reset() {}
};

} // namespace utility
} // namespace skylark

#endif // SKYLARK_DISTRIBUTIONS_HPP


#include "../../def_submodule.hpp"

#include "../../../../../type/math/coord.hpp"
#include "../../../../../type/math/matrix.hpp"

#include <boost/python.hpp>

typedef GS_DDMRM::S_IceRay::S_type::GT_size                GTs_size;
typedef GS_DDMRM::S_IceRay::S_type::GT_scalar              GTs_scalar;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar3D   GTs_coord3D;

typedef GS_DDMRM::S_IceRay::S_type::S_matrix::GT_scalar2D  GTs_matrix2D;
typedef GS_DDMRM::S_IceRay::S_type::S_matrix::GT_scalar3D  GTs_matrix3D;
typedef GS_DDMRM::S_IceRay::S_type::S_matrix::GT_scalar4D  GTs_matrix4D;

namespace
 {
  GTs_matrix3D & GFs_set( GTs_matrix3D & P_matrix, GTs_size P_row, GTs_size const& P_column, GTs_scalar const& P_value )
   {
    P_matrix[ P_row ][ P_column ] = P_value;
     // TODO PyErr_SetString(PyExc_IndexError, "index out of range" );
     return P_matrix;
   }

  GTs_coord3D GFs_rowGet( GTs_matrix3D & P_matrix, GTs_size P_row  )
   {
    GTs_coord3D Ir_row;
    Ir_row[0] = P_matrix[ P_row ][ 0 ];
    Ir_row[1] = P_matrix[ P_row ][ 1 ];
    Ir_row[2] = P_matrix[ P_row ][ 2 ];

    return Ir_row;
   }

  GTs_matrix3D & GFs_rowSet( GTs_matrix3D & P_matrix, GTs_size P_row, GTs_coord3D const& P_value )
   {
    P_matrix[ P_row ][ 0 ] = P_value[0];
    P_matrix[ P_row ][ 1 ] = P_value[1];
    P_matrix[ P_row ][ 2 ] = P_value[2];
    return P_matrix;
   }

  GTs_coord3D GFs_columnGet( GTs_matrix3D & P_matrix, GTs_size P_column  )
   {
    GTs_coord3D Ir_column;

    Ir_column[0] = P_matrix[ 0 ][ P_column ];
    Ir_column[1] = P_matrix[ 1 ][ P_column ];
    Ir_column[2] = P_matrix[ 2 ][ P_column ];
    return Ir_column;
   }

  GTs_matrix3D &  GFs_columnSet( GTs_matrix3D & P_matrix, GTs_size P_column, GTs_coord3D const& P_value )
   {
    P_matrix[ 0 ][ P_column ] = P_value[0];
    P_matrix[ 1 ][ P_column ] = P_value[1];
    P_matrix[ 2 ][ P_column ] = P_value[2];
    return P_matrix;
   }

  GTs_matrix3D &  GFs_scale1( GTs_matrix3D & P_matrix, GTs_scalar const& P_value )
   {
    P_matrix[ 0 ][ 0 ] *= P_value;
    P_matrix[ 1 ][ 1 ] *= P_value;
    P_matrix[ 2 ][ 2 ] *= P_value;
    return P_matrix;
   }

  GTs_matrix3D &  GFs_scale3( GTs_matrix3D & P_matrix, GTs_coord3D const& P_value )
   {
    P_matrix[ 0 ][ 0 ] *= P_value[0];
    P_matrix[ 1 ][ 1 ] *= P_value[1];
    P_matrix[ 2 ][ 2 ] *= P_value[2];
    return P_matrix;
   }

  GTs_matrix3D &  GFs_load( GTs_matrix3D & P_matrix, GTs_coord3D const& P_x, GTs_coord3D const& P_y, GTs_coord3D const& P_z )
   {
    P_matrix[ 0 ][ 0 ] = P_x[0];
    P_matrix[ 1 ][ 0 ] = P_x[1];
    P_matrix[ 2 ][ 0 ] = P_x[2];

    P_matrix[ 0 ][ 1 ] = P_y[0];
    P_matrix[ 1 ][ 1 ] = P_y[1];
    P_matrix[ 2 ][ 1 ] = P_y[2];

    P_matrix[ 0 ][ 2 ] = P_z[0];
    P_matrix[ 1 ][ 2 ] = P_z[1];
    P_matrix[ 2 ][ 2 ] = P_z[2];

    return P_matrix;
   }

  GTs_matrix3D &  GFs_id3( GTs_matrix3D & P_matrix )
   {
    ::math::linear::matrix::id( P_matrix );
    return P_matrix;
   }

  GTs_matrix3D &  GFs_zero3( GTs_matrix3D & P_matrix )
   {
    ::math::linear::matrix::zero( P_matrix );
    return P_matrix;
   }
 }

void expose_math_type_matrix2D()
 {
  //MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( math   );

  boost::python::class_<GTs_matrix2D>( "MathTypeMatrix2D" )
    .def( boost::python::init<>() );

 }

void expose_math_type_matrix3D()
 {
  //MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( math   );

  //typedef  GTs_scalar const& (GTs_matrix3D::*Tf_getScalar )( GTs_size const& P_row, GTs_size const& P_column )const;
  //Tf_getScalar I_getScalar = &GTs_matrix3D::F_element;

  //typedef  GTs_scalar     & (GTs_matrix3D::*Tf_accessScalar )( GTs_size const& P_row, GTs_size const& P_column );
  //Tf_accessScalar I_accessScalar = &GTs_matrix3D::F_element;

  boost::python::class_<GTs_matrix3D>( "MathTypeMatrix3D" )
    .def( boost::python::init<>() )
    //.def( boost::python::init< GTs_scalar >() )
    //.def( boost::python::init< GTs_coord3D,GTs_coord3D,GTs_coord3D >() )

    .def( "load", &GFs_load, boost::python::return_value_policy<boost::python::reference_existing_object>()  )

    //.def( "element", I_getScalar,    boost::python::return_value_policy<boost::python::copy_const_reference>()  )
    .def( "element",   &GFs_set,       boost::python::return_value_policy<boost::python::reference_existing_object>()  )
    .def( "row",       &GFs_rowGet    )
    .def( "row",       &GFs_rowSet,    boost::python::return_value_policy<boost::python::reference_existing_object>()  )
    .def( "column",    &GFs_columnGet )
    .def( "column",    &GFs_columnSet, boost::python::return_value_policy<boost::python::reference_existing_object>()  )
    .def( "scale",     &GFs_scale1,    boost::python::return_value_policy<boost::python::reference_existing_object>()  )
    .def( "scale",     &GFs_scale3,    boost::python::return_value_policy<boost::python::reference_existing_object>()  )
    .def( "identity",  &GFs_id3,       boost::python::return_value_policy<boost::python::reference_existing_object>()  )
    .def( "zero",      &GFs_zero3,     boost::python::return_value_policy<boost::python::reference_existing_object>()  )

    //.def( "rotate_z" )
    //.def( "rotate_axis" )

    //.def( boost::python::self + boost::python::self )
    //.def( boost::python::self - boost::python::self )
    //.def( boost::python::self * GTs_scalar3D::T_value() )
    //.def( GTs_scalar3D::T_value() * boost::python::self  )
    //.def( boost::python::self / GTs_scalar3D::T_value() )
    //.def( boost::python::self += boost::python::self )
    //.def( boost::python::self -= boost::python::self )
  ;
 }

void expose_math_type_matrix4D()
 {
  //MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( math   );

  boost::python::class_<GTs_matrix4D>( "MathTypeMatrix4D" )
    .def( boost::python::init<>() );

 }

void expose_math_type_matrix()
 {
  expose_math_type_matrix2D();
  expose_math_type_matrix3D();
  expose_math_type_matrix4D();
 }


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2020 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, 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 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------

 *
 * Author: Timo Heister and Jiaqi Zhang, Clemson University, 2020
 */

// The first few files have already been covered in previous examples and will
// thus not be further commented on:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/function_lib.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/fe/mapping_q1.h>
// Here the discontinuous finite elements and FEInterfaceValues are defined.
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_interface_values.h>

#include <deal.II/numerics/derivative_approximation.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/base/convergence_table.h>

#include <deal.II/meshworker/copy_data.h>
#include <deal.II/meshworker/mesh_loop.h>
#include <deal.II/meshworker/scratch_data.h>

namespace Step74
{
  using namespace dealii;

  // @sect3{Equation data}
  // Here we define two test cases: convergence_rate for a smooth function
  // and l_singularity for the Functions::LSingularityFunction.
  enum class TestCase
  {
    convergence_rate,
    l_singularity
  };

  // A smooth solution for the convergence test:
  template <int dim>
  class SmoothSolution : public Function<dim>
  {
  public:
    SmoothSolution()
      : Function<dim>()
    {}

    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int component = 0) const override;

    virtual Tensor<1, dim>
    gradient(const Point<dim> & point,
             const unsigned int component = 0) const override;
  };

  template <int dim>
  void SmoothSolution<dim>::value_list(const std::vector<Point<dim>> &points,
                                       std::vector<double> &          values,
                                       const unsigned int /*component*/) const
  {
    using numbers::PI;
    for (unsigned int i = 0; i < values.size(); ++i)
      values[i] =
        std::sin(2. * PI * points[i][0]) * std::sin(2. * PI * points[i][1]);
  }

  template <int dim>
  Tensor<1, dim>
  SmoothSolution<dim>::gradient(const Point<dim> &point,
                                const unsigned int /*component*/) const
  {
    Tensor<1, dim> return_value;
    using numbers::PI;
    return_value[0] =
      2. * PI * std::cos(2. * PI * point[0]) * std::sin(2. * PI * point[1]);
    return_value[1] =
      2. * PI * std::sin(2. * PI * point[0]) * std::cos(2. * PI * point[1]);
    return return_value;
  }

  // The corresponding right-hand side of the smooth function:
  template <int dim>
  class SmoothRightHandSide : public Function<dim>
  {
  public:
    SmoothRightHandSide()
      : Function<dim>()
    {}

    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int /*component*/) const override;
  };

  template <int dim>
  void
  SmoothRightHandSide<dim>::value_list(const std::vector<Point<dim>> &points,
                                       std::vector<double> &          values,
                                       const unsigned int /*component*/) const
  {
    using numbers::PI;
    for (unsigned int i = 0; i < values.size(); ++i)
      values[i] = 8. * PI * PI * std::sin(2. * PI * points[i][0]) *
                  std::sin(2. * PI * points[i][1]);
  }

  // The right-hand side that corresponds to the function
  // Functions::LSingularityFunction, where we
  // assume that the diffusion coefficient $\nu = 1$:
  template <int dim>
  class SingularRightHandSide : public Function<dim>
  {
  public:
    SingularRightHandSide()
      : Function<dim>()
    {}

    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int /*component*/) const override;

  private:
    const Functions::LSingularityFunction ref;
  };

  template <int dim>
  void
  SingularRightHandSide<dim>::value_list(const std::vector<Point<dim>> &points,
                                         std::vector<double> &          values,
                                         const unsigned int /*component*/) const
  {
    for (unsigned int i = 0; i < values.size(); ++i)
      values[i] = -ref.laplacian(points[i]);
  }

  // @sect3{Auxiliary functions}
  // The following two auxiliary functions are used to compute
  // jump terms for $u_h$ and $\nabla u_h$ on a face,
  // respectively.
  template <int dim>
  void get_function_jump(const FEInterfaceValues<dim> &fe_iv,
                         const Vector<double> &        solution,
                         std::vector<double> &         jump)
  {
    const unsigned int                 n_q = fe_iv.n_quadrature_points;
    std::array<std::vector<double>, 2> face_values;
    jump.resize(n_q);
    for (unsigned int i = 0; i < 2; ++i)
      {
        face_values[i].resize(n_q);
        fe_iv.get_fe_face_values(i).get_function_values(solution,
                                                        face_values[i]);
      }
    for (unsigned int q = 0; q < n_q; ++q)
      jump[q] = face_values[0][q] - face_values[1][q];
  }

  template <int dim>
  void get_function_gradient_jump(const FEInterfaceValues<dim> &fe_iv,
                                  const Vector<double> &        solution,
                                  std::vector<Tensor<1, dim>> & gradient_jump)
  {
    const unsigned int          n_q = fe_iv.n_quadrature_points;
    std::vector<Tensor<1, dim>> face_gradients[2];
    gradient_jump.resize(n_q);
    for (unsigned int i = 0; i < 2; ++i)
      {
        face_gradients[i].resize(n_q);
        fe_iv.get_fe_face_values(i).get_function_gradients(solution,
                                                           face_gradients[i]);
      }
    for (unsigned int q = 0; q < n_q; ++q)
      gradient_jump[q] = face_gradients[0][q] - face_gradients[1][q];
  }

  // This function computes the penalty $\sigma$.
  double get_penalty_factor(const unsigned int fe_degree,
                            const double       cell_extent_left,
                            const double       cell_extent_right)
  {
    const unsigned int degree = std::max(1U, fe_degree);
    return degree * (degree + 1.) * 0.5 *
           (1. / cell_extent_left + 1. / cell_extent_right);
  }

  // @sect3{The CopyData}
  // In the following, we define "Copy" objects for the MeshWorker::mesh_loop(),
  // which is essentially the same as step-12. Note that the
  // "Scratch" object is not defined here because we use
  // MeshWorker::ScratchData<dim> instead. (The use of "Copy" and "Scratch"
  // objects is extensively explained in the WorkStream namespace documentation.
  struct CopyDataFace
  {
    FullMatrix<double>                   cell_matrix;
    std::vector<types::global_dof_index> joint_dof_indices;
    std::array<double, 2>                values;
    std::array<unsigned int, 2>          cell_indices;
  };

  struct CopyData
  {
    FullMatrix<double>                   cell_matrix;
    Vector<double>                       cell_rhs;
    std::vector<types::global_dof_index> local_dof_indices;
    std::vector<CopyDataFace>            face_data;
    double                               value;
    unsigned int                         cell_index;

    template <class Iterator>
    void reinit(const Iterator &cell, const unsigned int dofs_per_cell)
    {
      cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
      cell_rhs.reinit(dofs_per_cell);
      local_dof_indices.resize(dofs_per_cell);
      cell->get_dof_indices(local_dof_indices);
    }
  };

  // @sect3{The SIPGLaplace class}
  // After these preparations, we proceed with the main class of this program,
  // called `SIPGLaplace`. The overall structure of the class is as in many
  // of the other tutorial programs. Major differences will only come up in the
  // implementation of the assemble functions, since we use FEInterfaceValues to
  // assemble face terms.
  template <int dim>
  class SIPGLaplace
  {
  public:
    SIPGLaplace(const TestCase &test_case);
    void run();

  private:
    void setup_system();
    void assemble_system();
    void solve();
    void refine_grid();
    void output_results(const unsigned int cycle) const;

    void   compute_errors();
    void   compute_error_estimate();
    double compute_energy_norm_error();

    Triangulation<dim>    triangulation;
    const unsigned int    degree;
    const QGauss<dim>     quadrature;
    const QGauss<dim - 1> face_quadrature;
    const QGauss<dim>     quadrature_overintegration;
    const QGauss<dim - 1> face_quadrature_overintegration;
    const MappingQ1<dim>  mapping;

    using ScratchData = MeshWorker::ScratchData<dim>;

    const FE_DGQ<dim> fe;
    DoFHandler<dim>   dof_handler;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;
    Vector<double>       solution;
    Vector<double>       system_rhs;

    // The remainder of the class's members are used for the following:
    // - Vectors to store error estimator square and energy norm square per
    // cell.
    // - Print convergence rate and errors on the screen.
    // - The fiffusion coefficient $\nu$ is set to 1.
    // - Members that store information about the test case to be computed.
    Vector<double> estimated_error_square_per_cell;
    Vector<double> energy_norm_square_per_cell;

    ConvergenceTable convergence_table;

    const double diffusion_coefficient = 1.;

    const TestCase                       test_case;
    std::unique_ptr<const Function<dim>> exact_solution;
    std::unique_ptr<const Function<dim>> rhs_function;
  };

  // The constructor here takes the test case as input and then
  // determines the correct solution and right-hand side classes. The
  // remaining member variables are initialized in the obvious way.
  template <int dim>
  SIPGLaplace<dim>::SIPGLaplace(const TestCase &test_case)
    : degree(3)
    , quadrature(degree + 1)
    , face_quadrature(degree + 1)
    , quadrature_overintegration(degree + 2)
    , face_quadrature_overintegration(degree + 2)
    , mapping()
    , fe(degree)
    , dof_handler(triangulation)
    , test_case(test_case)
  {
    if (test_case == TestCase::convergence_rate)
      {
        exact_solution = std::make_unique<const SmoothSolution<dim>>();
        rhs_function   = std::make_unique<const SmoothRightHandSide<dim>>();
      }

    else if (test_case == TestCase::l_singularity)
      {
        exact_solution =
          std::make_unique<const Functions::LSingularityFunction>();
        rhs_function = std::make_unique<const SingularRightHandSide<dim>>();
      }
    else
      AssertThrow(false, ExcNotImplemented());
  }

  template <int dim>
  void SIPGLaplace<dim>::setup_system()
  {
    dof_handler.distribute_dofs(fe);
    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_flux_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);
    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }

  // @sect3{The assemble_system function}
  // The assemble function here is similar to that in step-12 and step-47.
  // Different from assembling by hand, we just need to focus
  // on assembling on each cell, each boundary face, and each
  // interior face. The loops over cells and faces are handled
  // automatically by MeshWorker::mesh_loop().
  //
  // The function starts by defining a local (lambda) function that is
  // used to integrate the cell terms:
  template <int dim>
  void SIPGLaplace<dim>::assemble_system()
  {
    const auto cell_worker =
      [&](const auto &cell, auto &scratch_data, auto &copy_data) {
        const FEValues<dim> &fe_v          = scratch_data.reinit(cell);
        const unsigned int   dofs_per_cell = fe_v.dofs_per_cell;
        copy_data.reinit(cell, dofs_per_cell);

        const auto &       q_points    = scratch_data.get_quadrature_points();
        const unsigned int n_q_points  = q_points.size();
        const std::vector<double> &JxW = scratch_data.get_JxW_values();

        std::vector<double> rhs(n_q_points);
        rhs_function->value_list(q_points, rhs);

        for (unsigned int point = 0; point < n_q_points; ++point)
          for (unsigned int i = 0; i < fe_v.dofs_per_cell; ++i)
            {
              for (unsigned int j = 0; j < fe_v.dofs_per_cell; ++j)
                copy_data.cell_matrix(i, j) +=
                  diffusion_coefficient *     // nu
                  fe_v.shape_grad(i, point) * // grad v_h
                  fe_v.shape_grad(j, point) * // grad u_h
                  JxW[point];                 // dx

              copy_data.cell_rhs(i) += fe_v.shape_value(i, point) * // v_h
                                       rhs[point] *                 // f
                                       JxW[point];                  // dx
            }
      };

    // Next, we need a function that assembles face integrals on the boundary:
    const auto boundary_worker = [&](const auto &        cell,
                                     const unsigned int &face_no,
                                     auto &              scratch_data,
                                     auto &              copy_data) {
      const FEFaceValuesBase<dim> &fe_fv = scratch_data.reinit(cell, face_no);

      const auto &       q_points      = scratch_data.get_quadrature_points();
      const unsigned int n_q_points    = q_points.size();
      const unsigned int dofs_per_cell = fe_fv.dofs_per_cell;

      const std::vector<double> &        JxW = scratch_data.get_JxW_values();
      const std::vector<Tensor<1, dim>> &normals =
        scratch_data.get_normal_vectors();

      std::vector<double> g(n_q_points);
      exact_solution->value_list(q_points, g);

      const double extent1 = cell->measure() / cell->face(face_no)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent1);

      for (unsigned int point = 0; point < n_q_points; ++point)
        {
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              copy_data.cell_matrix(i, j) +=
                (-diffusion_coefficient *        // - nu
                   fe_fv.shape_value(i, point) * // v_h
                   (fe_fv.shape_grad(j, point) * // (grad u_h .
                    normals[point])              //  n)

                 - diffusion_coefficient *         // - nu
                     (fe_fv.shape_grad(i, point) * // (grad v_h .
                      normals[point]) *            //  n)
                     fe_fv.shape_value(j, point)   // u_h

                 + diffusion_coefficient * penalty * // + nu sigma
                     fe_fv.shape_value(i, point) *   // v_h
                     fe_fv.shape_value(j, point)     // u_h

                 ) *
                JxW[point]; // dx

          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            copy_data.cell_rhs(i) +=
              (-diffusion_coefficient *        // - nu
                 (fe_fv.shape_grad(i, point) * // (grad v_h .
                  normals[point]) *            //  n)
                 g[point]                      // g

               + diffusion_coefficient * penalty *        // + nu sigma
                   fe_fv.shape_value(i, point) * g[point] // v_h g

               ) *
              JxW[point]; // dx
        }
    };

    // Finally, a function that assembles face integrals on interior
    // faces. To reinitialize FEInterfaceValues, we need to pass
    // cells, face and subface indices (for adaptive refinement) to
    // the reinit() function of FEInterfaceValues:
    const auto face_worker = [&](const auto &        cell,
                                 const unsigned int &f,
                                 const unsigned int &sf,
                                 const auto &        ncell,
                                 const unsigned int &nf,
                                 const unsigned int &nsf,
                                 auto &              scratch_data,
                                 auto &              copy_data) {
      const FEInterfaceValues<dim> &fe_iv =
        scratch_data.reinit(cell, f, sf, ncell, nf, nsf);

      const auto &       q_points   = fe_iv.get_quadrature_points();
      const unsigned int n_q_points = q_points.size();

      copy_data.face_data.emplace_back();
      CopyDataFace &     copy_data_face = copy_data.face_data.back();
      const unsigned int n_dofs_face    = fe_iv.n_current_interface_dofs();
      copy_data_face.joint_dof_indices  = fe_iv.get_interface_dof_indices();
      copy_data_face.cell_matrix.reinit(n_dofs_face, n_dofs_face);

      const std::vector<double> &        JxW     = fe_iv.get_JxW_values();
      const std::vector<Tensor<1, dim>> &normals = fe_iv.get_normal_vectors();

      const double extent1 = cell->measure() / cell->face(f)->measure();
      const double extent2 = ncell->measure() / ncell->face(nf)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent2);

      for (unsigned int point = 0; point < n_q_points; ++point)
        {
          for (unsigned int i = 0; i < n_dofs_face; ++i)
            for (unsigned int j = 0; j < n_dofs_face; ++j)
              copy_data_face.cell_matrix(i, j) +=
                (-diffusion_coefficient *              // - nu
                   fe_iv.jump(i, point) *              // [v_h]
                   (fe_iv.average_gradient(j, point) * // ({grad u_h} .
                    normals[point])                    //  n)

                 - diffusion_coefficient *               // - nu
                     (fe_iv.average_gradient(i, point) * // (grad v_h .
                      normals[point]) *                  //  n)
                     fe_iv.jump(j, point)                // [u_h]

                 + diffusion_coefficient * penalty * // + nu sigma
                     fe_iv.jump(i, point) *          // [v_h]
                     fe_iv.jump(j, point)            // [u_h]

                 ) *
                JxW[point]; // dx
        }
    };

    // The following lambda function will then copy data into the
    // global matrix and right-hand side.  Though there are no hanging
    // node constraints in DG discretization, we define an empty
    // AffineConstraints object that allows us to use the
    // AffineConstraints::distribute_local_to_global() functionality.
    AffineConstraints<double> constraints;
    constraints.close();
    const auto copier = [&](const auto &c) {
      constraints.distribute_local_to_global(c.cell_matrix,
                                             c.cell_rhs,
                                             c.local_dof_indices,
                                             system_matrix,
                                             system_rhs);

      // Copy data from interior face assembly to the global matrix.
      for (auto &cdf : c.face_data)
        {
          constraints.distribute_local_to_global(cdf.cell_matrix,
                                                 cdf.joint_dof_indices,
                                                 system_matrix);
        }
    };

    // With the assembly functions defined, we can now create
    // ScratchData and CopyData objects, and pass them together with
    // the lambda functions above to MeshWorker::mesh_loop(). In
    // addition, we need to specify that we want to assemble on
    // interior faces exactly once.
    const UpdateFlags cell_flags = update_values | update_gradients |
                                   update_quadrature_points | update_JxW_values;
    const UpdateFlags face_flags = update_values | update_gradients |
                                   update_quadrature_points |
                                   update_normal_vectors | update_JxW_values;

    ScratchData scratch_data(
      mapping, fe, quadrature, cell_flags, face_quadrature, face_flags);
    CopyData copy_data;

    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker,
                          copier,
                          scratch_data,
                          copy_data,
                          MeshWorker::assemble_own_cells |
                            MeshWorker::assemble_boundary_faces |
                            MeshWorker::assemble_own_interior_faces_once,
                          boundary_worker,
                          face_worker);
  }

  // @sect3{The solve() and output_results() function}
  // The following two functions are entirely standard and without difficulty.
  template <int dim>
  void SIPGLaplace<dim>::solve()
  {
    SparseDirectUMFPACK A_direct;
    A_direct.initialize(system_matrix);
    A_direct.vmult(solution, system_rhs);
  }

  template <int dim>
  void SIPGLaplace<dim>::output_results(const unsigned int cycle) const
  {
    const std::string filename = "sol_Q" + Utilities::int_to_string(degree, 1) +
                                 "-" + Utilities::int_to_string(cycle, 2) +
                                 ".vtu";
    std::ofstream output(filename);

    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "u", DataOut<dim>::type_dof_data);
    data_out.build_patches(mapping);
    data_out.write_vtu(output);
  }

  // @sect3{The compute_error_estimate() function}
  // The assembly of the error estimator here is quite similar to
  // that of the global matrix and right-had side and can be handled
  // by the MeshWorker::mesh_loop() framework. To understand what
  // each of the local (lambda) functions is doing, recall first that
  // the local cell residual is defined as
  // $h_K^2 \left\| f + \nu \Delta u_h \right\|_K^2$:
  template <int dim>
  void SIPGLaplace<dim>::compute_error_estimate()
  {
    const auto cell_worker =
      [&](const auto &cell, auto &scratch_data, auto &copy_data) {
        const FEValues<dim> &fe_v = scratch_data.reinit(cell);

        copy_data.cell_index = cell->active_cell_index();

        const auto &               q_points   = fe_v.get_quadrature_points();
        const unsigned int         n_q_points = q_points.size();
        const std::vector<double> &JxW        = fe_v.get_JxW_values();

        std::vector<Tensor<2, dim>> hessians(n_q_points);
        fe_v.get_function_hessians(solution, hessians);

        std::vector<double> rhs(n_q_points);
        rhs_function->value_list(q_points, rhs);

        const double hk                   = cell->diameter();
        double       residual_norm_square = 0;

        for (unsigned int point = 0; point < n_q_points; ++point)
          {
            const double residual =
              rhs[point] + diffusion_coefficient * trace(hessians[point]);
            residual_norm_square += residual * residual * JxW[point];
          }
        copy_data.value = hk * hk * residual_norm_square;
      };

    // Next compute boundary terms $\sum_{f\in \partial K \cap \partial \Omega}
    // \sigma \left\| [  u_h-g_D ]  \right\|_f^2  $:
    const auto boundary_worker = [&](const auto &        cell,
                                     const unsigned int &face_no,
                                     auto &              scratch_data,
                                     auto &              copy_data) {
      const FEFaceValuesBase<dim> &fe_fv = scratch_data.reinit(cell, face_no);

      const auto &   q_points   = fe_fv.get_quadrature_points();
      const unsigned n_q_points = q_points.size();

      const std::vector<double> &JxW = fe_fv.get_JxW_values();

      std::vector<double> g(n_q_points);
      exact_solution->value_list(q_points, g);

      std::vector<double> sol_u(n_q_points);
      fe_fv.get_function_values(solution, sol_u);

      const double extent1 = cell->measure() / cell->face(face_no)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent1);

      double difference_norm_square = 0.;
      for (unsigned int point = 0; point < q_points.size(); ++point)
        {
          const double diff = (g[point] - sol_u[point]);
          difference_norm_square += diff * diff * JxW[point];
        }
      copy_data.value += penalty * difference_norm_square;
    };

    // And finally interior face terms $\sum_{f\in \partial K}\lbrace \sigma
    // \left\| [u_h]  \right\|_f^2   +  h_f \left\|  [\nu \nabla u_h \cdot
    // \mathbf n ] \right\|_f^2 \rbrace$:
    const auto face_worker = [&](const auto &        cell,
                                 const unsigned int &f,
                                 const unsigned int &sf,
                                 const auto &        ncell,
                                 const unsigned int &nf,
                                 const unsigned int &nsf,
                                 auto &              scratch_data,
                                 auto &              copy_data) {
      const FEInterfaceValues<dim> &fe_iv =
        scratch_data.reinit(cell, f, sf, ncell, nf, nsf);

      copy_data.face_data.emplace_back();
      CopyDataFace &copy_data_face = copy_data.face_data.back();

      copy_data_face.cell_indices[0] = cell->active_cell_index();
      copy_data_face.cell_indices[1] = ncell->active_cell_index();

      const std::vector<double> &        JxW     = fe_iv.get_JxW_values();
      const std::vector<Tensor<1, dim>> &normals = fe_iv.get_normal_vectors();

      const auto &       q_points   = fe_iv.get_quadrature_points();
      const unsigned int n_q_points = q_points.size();

      std::vector<double> jump(n_q_points);
      get_function_jump(fe_iv, solution, jump);

      std::vector<Tensor<1, dim>> grad_jump(n_q_points);
      get_function_gradient_jump(fe_iv, solution, grad_jump);

      const double h = cell->face(f)->diameter();

      const double extent1 = cell->measure() / cell->face(f)->measure();
      const double extent2 = ncell->measure() / ncell->face(nf)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent2);

      double flux_jump_square = 0;
      double u_jump_square    = 0;
      for (unsigned int point = 0; point < n_q_points; ++point)
        {
          u_jump_square += jump[point] * jump[point] * JxW[point];
          const double flux_jump = grad_jump[point] * normals[point];
          flux_jump_square +=
            diffusion_coefficient * flux_jump * flux_jump * JxW[point];
        }
      copy_data_face.values[0] =
        0.5 * h * (flux_jump_square + penalty * u_jump_square);
      copy_data_face.values[1] = copy_data_face.values[0];
    };

    // Having computed local contributions for each cell, we still
    // need a way to copy these into the global vector that will hold
    // the error estimators for all cells:
    const auto copier = [&](const auto &copy_data) {
      if (copy_data.cell_index != numbers::invalid_unsigned_int)
        estimated_error_square_per_cell[copy_data.cell_index] +=
          copy_data.value;
      for (auto &cdf : copy_data.face_data)
        for (unsigned int j = 0; j < 2; ++j)
          estimated_error_square_per_cell[cdf.cell_indices[j]] += cdf.values[j];
    };

    // After all of this set-up, let's do the actual work: We resize
    // the vector into which the results will be written, and then
    // drive the whole process using the MeshWorker::mesh_loop()
    // function.
    estimated_error_square_per_cell.reinit(triangulation.n_active_cells());

    const UpdateFlags cell_flags =
      update_hessians | update_quadrature_points | update_JxW_values;
    const UpdateFlags face_flags = update_values | update_gradients |
                                   update_quadrature_points |
                                   update_JxW_values | update_normal_vectors;

    ScratchData scratch_data(
      mapping, fe, quadrature, cell_flags, face_quadrature, face_flags);

    CopyData copy_data;
    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker,
                          copier,
                          scratch_data,
                          copy_data,
                          MeshWorker::assemble_own_cells |
                            MeshWorker::assemble_own_interior_faces_once |
                            MeshWorker::assemble_boundary_faces,
                          boundary_worker,
                          face_worker);
  }

  // @sect3{The compute_energy_norm_error() function}
  // Next, we evaluate the accuracy in terms of the energy norm.
  // This function is similar to the assembling of the error estimator above.
  // Here we compute the square of the energy norm defined by
  // @f[
  //   \|u \|_{1,h}^2 = \sum_{K \in \Gamma_h} \nu\|\nabla u \|_K^2 +
  //   \sum_{f \in F_i} \sigma \| [ u ] \|_f^2 +
  //   \sum_{f \in F_b} \sigma  \|u\|_f^2.
  // @f]
  // Therefore the corresponding error is
  // @f[
  //   \|u -u_h \|_{1,h}^2 = \sum_{K \in \Gamma_h} \nu\|\nabla (u_h - u)  \|_K^2
  //   + \sum_{f \in F_i} \sigma  \|[ u_h ] \|_f^2 + \sum_{f \in F_b}\sigma
  //   \|u_h-g_D\|_f^2.
  // @f]
  template <int dim>
  double SIPGLaplace<dim>::compute_energy_norm_error()
  {
    energy_norm_square_per_cell.reinit(triangulation.n_active_cells());

    // Assemble $\sum_{K \in \Gamma_h} \nu\|\nabla (u_h - u)  \|_K^2 $.
    const auto cell_worker =
      [&](const auto &cell, auto &scratch_data, auto &copy_data) {
        const FEValues<dim> &fe_v = scratch_data.reinit(cell);

        copy_data.cell_index = cell->active_cell_index();

        const auto &               q_points   = fe_v.get_quadrature_points();
        const unsigned int         n_q_points = q_points.size();
        const std::vector<double> &JxW        = fe_v.get_JxW_values();

        std::vector<Tensor<1, dim>> grad_u(n_q_points);
        fe_v.get_function_gradients(solution, grad_u);

        std::vector<Tensor<1, dim>> grad_exact(n_q_points);
        exact_solution->gradient_list(q_points, grad_exact);

        double norm_square = 0;
        for (unsigned int point = 0; point < n_q_points; ++point)
          {
            norm_square +=
              (grad_u[point] - grad_exact[point]).norm_square() * JxW[point];
          }
        copy_data.value = diffusion_coefficient * norm_square;
      };

    // Assemble $\sum_{f \in F_b}\sigma  \|u_h-g_D\|_f^2$.
    const auto boundary_worker = [&](const auto &        cell,
                                     const unsigned int &face_no,
                                     auto &              scratch_data,
                                     auto &              copy_data) {
      const FEFaceValuesBase<dim> &fe_fv = scratch_data.reinit(cell, face_no);

      const auto &   q_points   = fe_fv.get_quadrature_points();
      const unsigned n_q_points = q_points.size();

      const std::vector<double> &JxW = fe_fv.get_JxW_values();

      std::vector<double> g(n_q_points);
      exact_solution->value_list(q_points, g);

      std::vector<double> sol_u(n_q_points);
      fe_fv.get_function_values(solution, sol_u);

      const double extent1 = cell->measure() / cell->face(face_no)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent1);

      double difference_norm_square = 0.;
      for (unsigned int point = 0; point < q_points.size(); ++point)
        {
          const double diff = (g[point] - sol_u[point]);
          difference_norm_square += diff * diff * JxW[point];
        }
      copy_data.value += penalty * difference_norm_square;
    };

    // Assemble $\sum_{f \in F_i} \sigma  \| [ u_h ] \|_f^2$.
    const auto face_worker = [&](const auto &        cell,
                                 const unsigned int &f,
                                 const unsigned int &sf,
                                 const auto &        ncell,
                                 const unsigned int &nf,
                                 const unsigned int &nsf,
                                 auto &              scratch_data,
                                 auto &              copy_data) {
      const FEInterfaceValues<dim> &fe_iv =
        scratch_data.reinit(cell, f, sf, ncell, nf, nsf);

      copy_data.face_data.emplace_back();
      CopyDataFace &copy_data_face = copy_data.face_data.back();

      copy_data_face.cell_indices[0] = cell->active_cell_index();
      copy_data_face.cell_indices[1] = ncell->active_cell_index();

      const std::vector<double> &JxW = fe_iv.get_JxW_values();

      const auto &       q_points   = fe_iv.get_quadrature_points();
      const unsigned int n_q_points = q_points.size();

      std::vector<double> jump(n_q_points);
      get_function_jump(fe_iv, solution, jump);

      const double extent1 = cell->measure() / cell->face(f)->measure();
      const double extent2 = ncell->measure() / ncell->face(nf)->measure();
      const double penalty = get_penalty_factor(degree, extent1, extent2);

      double u_jump_square = 0;
      for (unsigned int point = 0; point < n_q_points; ++point)
        {
          u_jump_square += jump[point] * jump[point] * JxW[point];
        }
      copy_data_face.values[0] = 0.5 * penalty * u_jump_square;
      copy_data_face.values[1] = copy_data_face.values[0];
    };

    const auto copier = [&](const auto &copy_data) {
      if (copy_data.cell_index != numbers::invalid_unsigned_int)
        energy_norm_square_per_cell[copy_data.cell_index] += copy_data.value;
      for (auto &cdf : copy_data.face_data)
        for (unsigned int j = 0; j < 2; ++j)
          energy_norm_square_per_cell[cdf.cell_indices[j]] += cdf.values[j];
    };

    const UpdateFlags cell_flags =
      update_gradients | update_quadrature_points | update_JxW_values;
    UpdateFlags face_flags =
      update_values | update_quadrature_points | update_JxW_values;

    const ScratchData scratch_data(mapping,
                                   fe,
                                   quadrature_overintegration,
                                   cell_flags,
                                   face_quadrature_overintegration,
                                   face_flags);

    CopyData copy_data;
    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker,
                          copier,
                          scratch_data,
                          copy_data,
                          MeshWorker::assemble_own_cells |
                            MeshWorker::assemble_own_interior_faces_once |
                            MeshWorker::assemble_boundary_faces,
                          boundary_worker,
                          face_worker);
    const double energy_error =
      std::sqrt(energy_norm_square_per_cell.l1_norm());
    return energy_error;
  }

  // @sect3{The refine_grid() function}
  template <int dim>
  void SIPGLaplace<dim>::refine_grid()
  {
    const double refinement_fraction = 0.1;

    GridRefinement::refine_and_coarsen_fixed_number(
      triangulation, estimated_error_square_per_cell, refinement_fraction, 0.);

    triangulation.execute_coarsening_and_refinement();
  }

  // @sect3{The compute_errors() function}
  // We compute three errors in the $L_2$ norm, $H_1$ seminorm, and
  // the energy norm, respectively. These are then printed to screen,
  // but also stored in a table that records how these errors decay
  // with mesh refinement and which can be output in one step at the
  // end of the program.
  template <int dim>
  void SIPGLaplace<dim>::compute_errors()
  {
    double L2_error, H1_error, energy_error;

    {
      Vector<float> difference_per_cell(triangulation.n_active_cells());
      VectorTools::integrate_difference(mapping,
                                        dof_handler,
                                        solution,
                                        *(exact_solution.get()),
                                        difference_per_cell,
                                        quadrature_overintegration,
                                        VectorTools::L2_norm);

      L2_error = VectorTools::compute_global_error(triangulation,
                                                   difference_per_cell,
                                                   VectorTools::L2_norm);
      convergence_table.add_value("L2", L2_error);
    }

    {
      Vector<float> difference_per_cell(triangulation.n_active_cells());
      VectorTools::integrate_difference(mapping,
                                        dof_handler,
                                        solution,
                                        *(exact_solution.get()),
                                        difference_per_cell,
                                        quadrature_overintegration,
                                        VectorTools::H1_seminorm);

      H1_error = VectorTools::compute_global_error(triangulation,
                                                   difference_per_cell,
                                                   VectorTools::H1_seminorm);
      convergence_table.add_value("H1", H1_error);
    }

    {
      energy_error = compute_energy_norm_error();
      convergence_table.add_value("Energy", energy_error);
    }

    std::cout << "  Error in the L2 norm         : " << L2_error << std::endl
              << "  Error in the H1 seminorm     : " << H1_error << std::endl
              << "  Error in the energy norm     : " << energy_error
              << std::endl;
  }

  // @sect3{The run() function}
  template <int dim>
  void SIPGLaplace<dim>::run()
  {
    const unsigned int max_cycle =
      (test_case == TestCase::convergence_rate ? 6 : 20);
    for (unsigned int cycle = 0; cycle < max_cycle; ++cycle)
      {
        std::cout << "Cycle " << cycle << std::endl;

        switch (test_case)
          {
            case TestCase::convergence_rate:
              {
                if (cycle == 0)
                  {
                    GridGenerator::hyper_cube(triangulation);

                    triangulation.refine_global(2);
                  }
                else
                  {
                    triangulation.refine_global(1);
                  }
                break;
              }

            case TestCase::l_singularity:
              {
                if (cycle == 0)
                  {
                    GridGenerator::hyper_L(triangulation);
                    triangulation.refine_global(3);
                  }
                else
                  {
                    refine_grid();
                  }
                break;
              }

            default:
              {
                Assert(false, ExcNotImplemented());
              }
          }

        std::cout << "  Number of active cells       : "
                  << triangulation.n_active_cells() << std::endl;
        setup_system();

        std::cout << "  Number of degrees of freedom : " << dof_handler.n_dofs()
                  << std::endl;

        assemble_system();
        solve();
        output_results(cycle);
        {
          convergence_table.add_value("cycle", cycle);
          convergence_table.add_value("cells", triangulation.n_active_cells());
          convergence_table.add_value("dofs", dof_handler.n_dofs());
        }
        compute_errors();

        if (test_case == TestCase::l_singularity)
          {
            compute_error_estimate();
            std::cout << "  Estimated error              : "
                      << std::sqrt(estimated_error_square_per_cell.l1_norm())
                      << std::endl;

            convergence_table.add_value(
              "Estimator",
              std::sqrt(estimated_error_square_per_cell.l1_norm()));
          }
        std::cout << std::endl;
      }

    // Having run all of our computations, let us tell the convergence
    // table how to format its data and output it to screen:
    convergence_table.set_precision("L2", 3);
    convergence_table.set_precision("H1", 3);
    convergence_table.set_precision("Energy", 3);

    convergence_table.set_scientific("L2", true);
    convergence_table.set_scientific("H1", true);
    convergence_table.set_scientific("Energy", true);

    if (test_case == TestCase::convergence_rate)
      {
        convergence_table.evaluate_convergence_rates(
          "L2", ConvergenceTable::reduction_rate_log2);
        convergence_table.evaluate_convergence_rates(
          "H1", ConvergenceTable::reduction_rate_log2);
      }
    if (test_case == TestCase::l_singularity)
      {
        convergence_table.set_precision("Estimator", 3);
        convergence_table.set_scientific("Estimator", true);
      }

    std::cout << "degree = " << degree << std::endl;
    convergence_table.write_text(
      std::cout, TableHandler::TextOutputFormat::org_mode_table);
  }
} // namespace Step74

// @sect3{The main() function}
// The following <code>main</code> function is similar to previous examples as
// well, and need not be commented on.
int main()
{
  try
    {
      using namespace dealii;
      using namespace Step74;

      const TestCase test_case = TestCase::l_singularity;

      SIPGLaplace<2> problem(test_case);
      problem.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    };

  return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
Matrix4Xd M = Matrix4Xd::Random(4,5);
Projective3d P(Matrix4d::Random());
cout << "The matrix M is:" << endl << M << endl << endl;
cout << "M.colwise().hnormalized():" << endl << M.colwise().hnormalized() << endl << endl;
cout << "P*M:" << endl << P*M << endl << endl;
cout << "(P*M).colwise().hnormalized():" << endl << (P*M).colwise().hnormalized() << endl << endl;

}
  return 0;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/round2even.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/mhalf.hpp>

STF_CASE_TPL ( "round2even real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::round2even;
  using r_t = decltype(round2even(T()));

  // return type conformity test
  STF_TYPE_IS( r_t, T );

  // specific values tests
  STF_ULP_EQUAL(round2even(T(1.4)), 1, 0);
  STF_ULP_EQUAL(round2even(T(1.5)), 2, 0);
  STF_ULP_EQUAL(round2even(T(1.6)), 2, 0);
  STF_ULP_EQUAL(round2even(T(2.5)), 2, 0);
  STF_ULP_EQUAL(round2even(bs::Half<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(round2even(bs::Inf<T>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(round2even(bs::Mhalf<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(round2even(bs::Minf<T>()), bs::Minf<r_t>(), 0);
  STF_ULP_EQUAL(round2even(bs::Mone<T>()), bs::Mone<r_t>(), 0);
  STF_ULP_EQUAL(round2even(bs::Nan<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(round2even(bs::One<T>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(round2even(bs::Zero<T>()), bs::Zero<r_t>(), 0);
} // end of test for floating_

STF_CASE_TPL (" round2evenunsigned_int__1_0",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::round2even;
  using r_t = decltype(round2even(T()));

  // return type conformity test
  STF_TYPE_IS( r_t, T );

  // specific values tests
  STF_ULP_EQUAL(round2even(bs::One<T>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(round2even(bs::Zero<T>()), bs::Zero<r_t>(), 0);
} // end of test for unsigned_int_

STF_CASE_TPL (" round2evensigned_ int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::round2even;
  using r_t = decltype(round2even(T()));

  // return type conformity test
  STF_TYPE_IS( r_t, T );

  // specific values tests
  STF_ULP_EQUAL(round2even(bs::Mone<T>()), bs::Mone<r_t>(), 0);
  STF_ULP_EQUAL(round2even(bs::One<T>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(round2even(bs::Zero<T>()), bs::Zero<T>(), 0);
} // end of test for signed_int_


/*=========================================================================

Library:   TubeTK

Copyright 2010 Kitware Inc. 28 Corporate Drive,
Clifton Park, NY, 12065, USA.

All rights reserved.

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 "tubeMessage.h"

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/p_square_quantile.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/foreach.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <itkImageFileReader.h>
#include <itkImageRegionConstIterator.h>
#include <itkStatisticsImageFilter.h>

#include "ComputeImageQuantilesCLP.h"

enum { Dimension = 3 };

//using namespace boost;
using namespace boost::accumulators;

typedef itk::Image< float, Dimension >                      ImageType;
typedef ImageType::IndexType                                ImageIndexType;
typedef ImageType::PixelType                                ImagePixelType;
typedef ImageType::SizeType                                 ImageSizeType;
typedef itk::ImageFileReader< ImageType >                   ImageReaderType;
typedef itk::StatisticsImageFilter< ImageType >             StatisticsImageFilterType;
typedef accumulator_set< ImagePixelType,
                         stats< tag::p_square_quantile > >  QuantileAccumulatorType;
typedef itk::ImageRegionConstIterator<ImageType>            ImageIteratorType;

/**
 * Take an image and compute a collection of pixel/voxel quantiles
 * using the BOOST accumulators. The function uses P^2 quantile
 * computation which gives an approximate quantile and is very
 * efficient in terms of storage and runtime complexity. The values
 * are actually never stored.
 */
void computeQuantiles( ImageType::Pointer image,
                       const std::vector<float> & quantiles,
                       std::vector<ImagePixelType> & quantileValues)
{
  assert(quantileValues.empty());

  // Create a and configure a vector of length N of pointers
  // to BOOST accumulators -- Each of the N accumulators will
  // estimate exactly one of the given N desired quantile. If
  // the desired quantile is not within (0,1), throw an exception.
  std::vector<QuantileAccumulatorType *> accVec;
  BOOST_FOREACH(float q, quantiles)
    {
    if( q <= 0 || q >= 1 )
      {
      tube::ErrorMessage("Check quantile range!");
      throw std::exception();
      }
    tube::FmtInfoMessage("Configure accumulator for quantile = %.2f", q);

    QuantileAccumulatorType *acc = new QuantileAccumulatorType(
      quantile_probability = q);
    accVec.push_back(acc);
    }

  // Use an image iterator to iterate over all pixel/voxel and
  // and then add those values to all the accumulators. Adding
  // the values will incrementally compute the quantile estimates.
  ImageIteratorType imIt( image, image->GetLargestPossibleRegion() );
  imIt.GoToBegin();
  while( !imIt.IsAtEnd() )
    {
    ImagePixelType p = imIt.Get();
    BOOST_FOREACH( QuantileAccumulatorType *acc, accVec )
      {
      (*acc)( p );
      }
    ++imIt;
    }

  // Finally, iterate over the accumulators, query the
  // estimated quantiles and fill the output vector
  BOOST_FOREACH( QuantileAccumulatorType *acc, accVec)
    {
    ImagePixelType qVal = p_square_quantile(*acc);
    quantileValues.push_back(qVal);
    delete acc;
    acc = NULL;
    }
  return;
}

/**
 * Writes quantiles and quantile values to a file in JSON
 * format. Example (for quantiles 0.05, 0.5, 0.95):
 *
 *  {
 *     "quantiles":
 *     [
 *        <quantile0>,
 *        <quantile1>,
 *        ...
 *        <quantileN>
 *     ],
 *     "quantileValues":
 *     [
 *        <quantileValue0>,
 *        <quantileValue1>,
 *        ...
 *        <quantileValueN>
 *     ]
 *   }
 */
void writeQuantilesToJSONFile( const std::vector<float>& quantiles,
                           const std::vector<ImagePixelType>& quantileValues,
                           const std::string &outFile )
{
  tube::FmtInfoMessage( "Writing %d quantiles to %s",
        quantiles.size(), outFile.c_str());

  try
    {
    boost::property_tree::ptree root;
    boost::property_tree::ptree quantilesJSON;
    boost::property_tree::ptree quantileValuesJSON;

    for( unsigned int i=0; i<quantiles.size(); ++i )
      {
      boost::property_tree::ptree quantileElementJSON;
      boost::property_tree::ptree quantileValueElementJSON;
      quantileElementJSON.put( "", quantiles[i] );
      quantileValueElementJSON.put( "", quantileValues[i] );
      quantilesJSON.push_back(make_pair( "", quantileElementJSON ) );
      quantileValuesJSON.push_back(make_pair( "", quantileValueElementJSON ) );
      }
    root.add_child( "quantiles", quantilesJSON);
    root.add_child( "quantileValues", quantileValuesJSON );
    boost::property_tree::write_json( outFile, root );
    }
  catch(boost::property_tree::json_parser::json_parser_error &e)
    {
    tube::ErrorMessage( e.message() );
    throw std::exception();
    }
}

/**
 * Writes the quantiles to a plain ASCII file, one
 * quantile value per line.
 */
void writeQuantilesToTextFile( const std::vector<float> &quantiles,
                               const std::string &outFile )
{
  std::ofstream quantileFile;
  quantileFile.open( outFile.c_str() );
  for( unsigned int i=0; i<quantiles.size(); ++i )
    {
    quantileFile << quantiles[i] << std::endl;
    }
  quantileFile.close();
}

int main( int argc, char * argv[] )
{
  PARSE_ARGS;

  tube::FmtInfoMessage("Reading image file %s",
    imageFile.c_str());
  ImageReaderType::Pointer imReader = ImageReaderType::New();
  imReader->SetFileName( imageFile );
  ImageType::Pointer im;

  try
    {
    imReader->Update();
    im = imReader->GetOutput();
    }
  catch( itk::ExceptionObject &ex )
    {
    tube::ErrorMessage( ex.GetDescription() );
    return EXIT_FAILURE;
    }

  std::vector<float> quantileValues;
   try
    {
    computeQuantiles(im, quantiles, quantileValues);
    if( outputPlainText )
      {
      writeQuantilesToTextFile( quantileValues, outFile );
      }
    else
      {
      writeQuantilesToJSONFile( quantiles, quantileValues, outFile );
      }
    }
  catch(std::exception &e)
    {
    return EXIT_FAILURE;
    }
  return EXIT_SUCCESS;
}


#include <catch.hpp>
#include <typeclass/eq/list.h>
#include <typeclass/eq/optional.h>
#include <typeclass/eq/vector.h>
#include <typeclass/eq/scalar.h>
#include <typeclass/functor.h>
#include <typeclass/functor/list.h>
#include <typeclass/functor/optional.h>
#include <typeclass/functor/vector.h>
#include <typeclass/functor/future.h>
#include <boost/optional.hpp>
#include <future>

TEST_CASE("typeclass functor") {
    using namespace funcpp::typeclass::functor;
    using namespace funcpp::typeclass::eq;
    using namespace funcpp::typeclass::eq::operators;
    GIVEN("a list of ints") {
        std::list<int> a{1,2,3};
        auto result = fmap([](auto x){ return x*2; }, a);
        REQUIRE((std::list<int>{2,4,6} == result));
    }
    GIVEN("a vector of ints") {
        std::vector<int> a{1,2,3};
        auto result = fmap([](auto x){ return x*2; }, a);
        REQUIRE((std::vector<int>{2,4,6} == result));
    }
    GIVEN("an optional int") {
        boost::optional<int> a{10};
        auto result = fmap([](auto x){ return x*2; }, a);
        REQUIRE((boost::optional<int>{20} == result));
    }
    GIVEN("a future of int") {
        std::promise<int> a;
        auto result = fmap([](auto x){ return x*2; }, a.get_future());
        a.set_value(10);
        REQUIRE(20 == result.get());
    }
}


#include "interface.h"

#include <cmr/env.h>

#include "total_unimodularity.hpp"
#include "unimodularity.hpp"

#include <boost/numeric/ublas/io.hpp>

extern "C"
CMR_ERROR CMRinterfaceTU(CMR* cmr, CMR_CHRMAT* matrix, bool* pisTU, CMR_DEC** pdec, CMR_SUBMAT** psubmatrix)
{
  assert(cmr);
  assert(matrix);
  assert(pisTU);
  assert(!psubmatrix || !*psubmatrix);

  tu::integer_matrix mat(matrix->numRows, matrix->numColumns, 0);
  for (size_t row = 0; row < (size_t)matrix->numRows; ++row)
  {
    size_t first = matrix->rowSlice[row];
    size_t beyond = matrix->rowSlice[row + 1];
    for (size_t i = first; i < beyond; ++i)
    {
      size_t column = matrix->entryColumns[i];
      mat(row,column) = matrix->entryValues[i];
    }
  }

  tu::submatrix_indices violator;
  if (psubmatrix)
  {
    *pisTU = tu::is_totally_unimodular(mat, violator);
  }
  else
  {
    *pisTU = tu::is_totally_unimodular(mat);
  }

  if (*pisTU && pdec)
    fprintf(stderr, "Retrieval of decomposition is not implemented, yet.");

  if (!violator.rows.empty())
  {
    CMR_CALL( CMRsubmatCreate(cmr, violator.rows.size(), violator.columns.size(), psubmatrix) );
    CMR_SUBMAT* submatrix = *psubmatrix;
    for (size_t row = 0; row < submatrix->numRows; ++row)
      submatrix->rows[row] = violator.rows[row];
    for (size_t column = 0; column < submatrix->numColumns; ++column)
      submatrix->columns[column] = violator.columns[column];
  }

  return CMR_OKAY;
}

CMR_ERROR CMRinterfaceKModular(CMR* cmr, CMR_CHRMAT* matrix, size_t* pk)
{
  assert(cmr);
  assert(matrix);
  assert(pk);

  tu::integer_matrix mat(matrix->numRows, matrix->numColumns, 0);
  for (size_t row = 0; row < (size_t)matrix->numRows; ++row)
  {
    size_t first = matrix->rowSlice[row];
    size_t beyond = matrix->rowSlice[row + 1];
    for (size_t i = first; i < beyond; ++i)
    {
      size_t column = matrix->entryColumns[i];
      mat(row,column) = matrix->entryValues[i];
    }
  }

  size_t rank;
  unsigned int k;
  bool result = tu::is_k_modular(mat, rank, k);
  *pk = result ? k : 0;

  return CMR_OKAY;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE crpMM test
#include <boost/test/unit_test.hpp>

#include <dpMM/crpMM.hpp>
#include <dpMM/niwBaseMeasure.hpp>
#include <dpMM/niwSphere.hpp>
#include <dpMM/vmfBaseMeasure.hpp>
#include <dpMM/vmfBaseMeasure3D.hpp>

BOOST_AUTO_TEST_CASE(crpMM_test)
{
  uint32_t N=20;
  MatrixXd x(3,N);
  for(uint32_t i=0; i<N; ++i)
    if(i<N/2)
      x.col(i) << 0.0,0.0,0.0;
    else
      x.col(i) << 10.0,10.0,10.0;

  double nu = 4.0;
  double kappa = 4.0;
  MatrixXd Delta(3,3);
  Delta << .1,0.0,0.0,
        0.0,.1,0.0,
        0.0,0.0,.1;
  VectorXd theta(3);
  theta << 0.0,0.0,0.0;
  double alpha = 1.0;

  boost::mt19937 rndGen(9191);
  NIW<double> niw(Delta,theta,nu,kappa,&rndGen);

//  shared_ptr<NiwMarginalized<double> > niwMargBase(
//      new NiwMarginalized<double>(niw));
//  CrpMM<double> dirGMM_marg(alpha,niwMargBase,2,&rndGen);
//
//  dirGMM_marg.initialize(x);
//  cout<<"------ sampling -- NIW marginalized"<<endl;
//  cout<<dirGMM_marg.labels().transpose()<<endl;
//  for(uint32_t t=0; t<30; ++t)
//  {
//    dirGMM_marg.sampleLabels();
//    dirGMM_marg.sampleParameters();
//    cout<<dirGMM_marg.labels().transpose()
//      <<" logJoint="<<dirGMM_marg.logJoint()<<endl;
//  }

  shared_ptr<NiwSampled<double> > niwSampled(
      new NiwSampled<double>(niw));
  CrpMM<double> dirGMM_samp(alpha,niwSampled,2,&rndGen);

  dirGMM_samp.initialize(x);
  cout<<"------ sampling -- NIW sampled"<<endl;
  cout<<dirGMM_samp.labels().transpose()<<endl;
  for(uint32_t t=0; t<30; ++t)
  {
    dirGMM_samp.sampleLabels();
    dirGMM_samp.sampleParameters();
    cout<<dirGMM_samp.labels().transpose()
      <<" logJoint="<<dirGMM_samp.logJoint()<<endl;
  }
};
//
//
BOOST_AUTO_TEST_CASE(crpMM_Sphere_test)
{
  cout<<"------ sampling -- NIW sphere"<<endl;

  double nu = 20.0;
  MatrixXd Delta(2,2);
  Delta << .01,0.0,
        0.0,.01;
  Delta *= nu;

  boost::mt19937 rndGen(9191);
  IW<double> iw(Delta,nu,&rndGen);
  shared_ptr<NiwSphere<double> > niwSp( new NiwSphere<double>(iw,&rndGen));
  double alpha = 1.0;
  CrpMM<double> dirGMM_sp(alpha,niwSp,1,&rndGen);

  uint32_t N=20;
  uint32_t K=2;
  MatrixXd x(3,N);
  MatrixXd mus = sampleClustersOnSphere<double>(x, K);

  dirGMM_sp.initialize(x);

  cout<<"true means: "<<endl<<mus<<endl;
  cout<<dirGMM_sp.labels().transpose()<<endl;
  for(uint32_t t=0; t<10; ++t)
  {
    dirGMM_sp.sampleParameters();
//    for(uint32_t k=0; k<dirGMM_sp.getK(); ++k)
//    {
//      cout<<"  k: "<<k<<" "<<endl;
//      dirGMM_sp.getTheta(k)->print();
//    }
    dirGMM_sp.sampleLabels();
    cout<<"@t="<<t<<" "<<dirGMM_sp.labels().transpose()
      <<" logJoint="<<dirGMM_sp.logJoint()<<endl;
  }
  MatrixXd logLikes;
  MatrixXu inds = dirGMM_sp.mostLikelyInds(5,logLikes);
  cout<<"most likely indices"<<endl;
  cout<<inds<<endl;
  cout<<"----------------------------------------"<<endl;
};

BOOST_AUTO_TEST_CASE(crpMM_vMF_test)
{
  cout<<"------ sampling -- CRP-vMF"<<endl;

  double a0 = 2.0;
  double b0 = 1.7;
  double t0 = 0.01;
  VectorXd m0(3);
  m0 << 1.0,0.0,0.0;

  boost::mt19937 rndGen(9191);

  vMFpriorFull<double> vMFprior(m0,t0,a0,b0,&rndGen);
  shared_ptr<vMFbase<double> > vMFsampled( new vMFbase<double>(vMFprior));

  double alpha = 1.0;
  CrpMM<double> dirvMF_sp(alpha,vMFsampled,1,&rndGen);

  uint32_t N=100;
  uint32_t K=2;
  MatrixXd x(3,N);
  MatrixXd mus = sampleClustersOnSphere<double>(x, K);

  dirvMF_sp.initialize(x);

  cout<<"true means: "<<endl<<mus<<endl;
  cout<<dirvMF_sp.labels().transpose()<<endl;
  for(uint32_t t=0; t<100; ++t)
  {
    dirvMF_sp.sampleParameters();
//    for(uint32_t k=0; k<dirvMF_sp.getK(); ++k)
//    {
//      cout<<"  k: "<<k<<" "<<endl;
//      dirvMF_sp.getTheta(k)->print();
//    }
    dirvMF_sp.sampleLabels();
    cout<<"@t="<<t<<" "<<dirvMF_sp.labels().transpose()
      <<" logJoint="<<dirvMF_sp.logJoint()<<endl;
  }
//  MatrixXd logLikes;
//  MatrixXu inds = dirvMF_sp.mostLikelyInds(5,logLikes);
//  cout<<"most likely indices"<<endl;
//  cout<<inds<<endl;
  cout<<"----------------------------------------"<<endl;
};

BOOST_AUTO_TEST_CASE(crpMM_vMFanalytic_test)
{
  cout<<"------ sampling -- CRP-vMF (new with analytic marginalization)"<<endl;

  double a0 = 1.0;
  double b0 = 0.1;
  VectorXd m0(3);
  m0 << 1.0,0.0,0.0;

  boost::mt19937 rndGen(9191);

  vMFprior<double> vMFprior(m0,a0,b0,&rndGen);
  shared_ptr<vMFbase3D<double> > vMFsampled( new vMFbase3D<double>(vMFprior));

  double alpha = 1.0;
  CrpMM<double> dirvMF_sp(alpha,vMFsampled,1,&rndGen);

  uint32_t N=100;
  uint32_t K=2;
  MatrixXd x(3,N);
  MatrixXd mus = sampleClustersOnSphere<double>(x, K);

  dirvMF_sp.initialize(x);

  cout<<"true means: "<<endl<<mus<<endl;
  cout<<dirvMF_sp.labels().transpose()<<endl;
  for(uint32_t t=0; t<100; ++t)
  {
    dirvMF_sp.sampleParameters();
//    for(uint32_t k=0; k<dirvMF_sp.getK(); ++k)
//    {
//      cout<<"  k: "<<k<<" "<<endl;
//      dirvMF_sp.getTheta(k)->print();
//    }
    dirvMF_sp.sampleLabels();
    cout<<"@t="<<t<<" "<<dirvMF_sp.labels().transpose()
      <<" logJoint="<<dirvMF_sp.logJoint()<<endl;
  }
//  MatrixXd logLikes;
//  MatrixXu inds = dirvMF_sp.mostLikelyInds(5,logLikes);
//  cout<<"most likely indices"<<endl;
//  cout<<inds<<endl;
  cout<<"----------------------------------------"<<endl;
};