#ifndef _QUADSOLVE_HPP_
#define _QUADSOLVE_HPP_

/*
   FILE uquadprog.hh

NOTE: this is a modified of QuadProg++ package, originally developed by
Luca Di Gaspero, working with ublas data structures.

The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
for the solution of a (convex) Quadratic Programming problem
by means of a dual method.

The problem is in the form:

min 0.5 * x G x + g0 x
s.t.
CE^T x + ce0 = 0
CI^T x + ci0 >= 0

The matrix and vectors dimensions are as follows:
G: n * n
g0: n

CE: n * p
ce0: p

CI: n * m
ci0: m

x: n

The function will return the cost of the solution written in the x vector or
std::numeric_limits::infinity() if the problem is infeasible. In the latter case
the value of the x vector is not correct.

References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.

Notes:
1. pay attention in setting up the vectors ce0 and ci0.
If the constraints of your problem are specified in the form
A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
2. The matrix G is modified within the function since it is used to compute
the G = L^T L cholesky factorization for further computations inside the function.
If you need the original matrix G you should make a copy of it and pass the copy
to the function.

Author: Angelo Furfaro
DEIS - University of Calabria, Italy
a.furfaro@deis.unical.it
http://www.lis.deis.unical.it/~furfaro

The author will be grateful if the researchers using this software will
acknowledge the contribution of this modified function and of Di Gaspero's
original version in their research papers.

LICENSE

Copyright (2008) Angelo Furfaro
Copyright (2006) Luca Di Gaspero

This file is a porting of QuadProg++ routine, originally developed
by Luca Di Gaspero, exploiting uBlas data structures for vectors and
matrices instead of native C++ array.

uquadprog is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

uquadprog is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with uquadprog; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

*/

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

using namespace boost::numeric::ublas;

void cholesky_decomposition(matrix<double> & A);
void backward_elimination(matrix<double>& U,vector<double>& x, vector<double>& y);
void forward_elimination(matrix<double>& L, vector<double>& y, vector<double>& b);
void cholesky_solve(matrix<double>& L, vector<double> & x, vector<double> & b);

double distance(double a, double b);
void compute_d(vector<double> &d, matrix<double>& J, vector<double>& np);
void update_z(vector<double>& z, matrix<double>& J, vector<double>& d,  int iq);
void update_r(matrix<double>& R, vector<double> &r, vector<double> &d, int iq);
bool add_constraint(matrix<double>& R, matrix<double>& J, vector<double>& d, int& iq, double& R_norm);
void delete_constraint(matrix<double>& R, matrix<double>& J, vector<int>& A, vector<double>& u,  int p, int& iq, int l);

inline double solve_quadprog( matrix<double> & G,  vector<double> & g0,
        const matrix<double> & CE, const vector<double> & ce0,
        const matrix<double> & CI, const vector<double> & ci0,
        vector<double>& x)
{
    int i, j, k, l; /* indices */
    int ip, me, mi;
    int n=g0.size();  int p=ce0.size();  int m=ci0.size();
    matrix<double> R(G.size1(),G.size2()), J(G.size1(),G.size2());

    vector<double> s(m+p), z(n), r(m + p), d(n),  np(n), u(m + p);
    vector<double> x_old(n), u_old(m + p);
    double f_value, psi, c1, c2, sum, ss, R_norm;
    const double inf = std::numeric_limits<double>::infinity();
    double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1
                       * and the full step length t2 */
    vector<int> A(m + p), A_old(m + p), iai(m + p);int q;
    int iq, iter = 0;
    bool iaexcl[m + p];

    me = p; /* number of equality constraints */
    mi = m; /* number of inequality constraints */
    q = 0;  /* size of the active set A (containing the indices of the active constraints) */

    /*
     * Preprocessing phase
     */

    /* compute the trace of the original matrix G */
    c1 = 0.0;
    for (i = 0; i < n; i++)
    {
        c1 += G(i,i);
    }

    /* decompose the matrix G in the form L^T L */
    cholesky_decomposition(G);

    /* initialize the matrix R */
    for (i = 0; i < n; i++)
    {
        d(i) = 0.0;
        for (j = 0; j < n; j++)
            R(i,j) = 0.0;
    }
    R_norm = 1.0; /* this variable will hold the norm of the matrix R */

    /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
    c2 = 0.0;
    for (i = 0; i < n; i++)
    {
        d(i) = 1.0;
        forward_elimination(G, z, d);
        for (j = 0; j < n; j++)
            J(i,j) = z(j);
        c2 += z(i);
        d(i) = 0.0;
    }
#ifdef TRACE_SOLVER
    print_matrix("J", J, n);
#endif

    /* c1 * c2 is an estimate for cond(G) */

    /*
     * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
     * this is a feasible point in the dual space
     * x = G^-1 * g0
     */
    cholesky_solve(G, x, g0);
    for (i = 0; i < n; i++)
        x(i) = -x(i);
    /* and compute the current solution value */
    f_value = 0.5 * inner_prod(g0, x);
#ifdef TRACE_SOLVER
    std::cerr << "Unconstrained solution: " << f_value << std::endl;
    print_vector("x", x, n);
#endif

    /* Add equality constraints to the working set A */
    iq = 0;
    for (i = 0; i < me; i++)
    {
        for (j = 0; j < n; j++)
            np(j) = CE(j,i);
        compute_d(d, J, np);
        update_z(z, J, d,  iq);
        update_r(R, r, d,  iq);
#ifdef TRACE_SOLVER
        print_matrix("R", R, iq);
        print_vector("z", z, n);
        print_vector("r", r, iq);
        print_vector("d", d, n);
#endif

        /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
           becomes feasible */
        t2 = 0.0;
        if (fabs(inner_prod(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
            t2 = (-inner_prod(np, x) - ce0(i)) / inner_prod(z, np);

        /* set x = x + t2 * z */
        for (k = 0; k < n; k++)
            x(k) += t2 * z(k);

        /* set u = u+ */
        u(iq) = t2;
        for (k = 0; k < iq; k++)
            u(k) -= t2 * r(k);

        /* compute the new solution value */
        f_value += 0.5 * (t2 * t2) * inner_prod(z, np);
        A(i) = -i - 1;

        if (!add_constraint(R, J, d, iq, R_norm))
        {
            // FIXME: it should raise an error
            // Equality constraints are linearly dependent
            return f_value;
        }
    }

    /* set iai = K \ A */
    for (i = 0; i < mi; i++)
        iai(i) = i;

l1:	iter++;
#ifdef TRACE_SOLVER
    print_vector("x", x, n);
#endif
    /* step 1: choose a violated constraint */
    for (i = me; i < iq; i++)
    {
        ip = A(i);
        iai(ip) = -1;
    }

    /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
    ss = 0.0;
    psi = 0.0; /* this value will contain the sum of all infeasibilities */
    ip = 0; /* ip will be the index of the chosen violated constraint */
    for (i = 0; i < mi; i++)
    {
        iaexcl[i] = true;
        sum = 0.0;
        for (j = 0; j < n; j++)
            sum += CI(j,i) * x(j);
        sum += ci0(i);
        s(i) = sum;
        psi += std::min(0.0, sum);
    }
#ifdef TRACE_SOLVER
    print_vector("s", s, mi);
#endif

    if (fabs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
    {
        /* numerically there are not infeasibilities anymore */
        q = iq;
        return f_value;
    }

    /* save old values for u and A */
    for (i = 0; i < iq; i++)
    {
        u_old(i) = u(i);
        A_old(i) = A(i);
    }
    /* and for x */
    for (i = 0; i < n; i++)
        x_old(i) = x(i);

l2: /* Step 2: check for feasibility and determine a new S-pair */
    for (i = 0; i < mi; i++)
    {
        if (s(i) < ss && iai(i) != -1 && iaexcl[i])
        {
            ss = s(i);
            ip = i;
        }
    }
    if (ss >= 0.0)
    {
        q = iq;
        return f_value;
    }

    /* set np = n(ip) */
    for (i = 0; i < n; i++)
        np(i) = CI(i,ip);
    /* set u = (u 0)^T */
    u(iq) = 0.0;
    /* add ip to the active set A */
    A(iq) = ip;

#ifdef TRACE_SOLVER
    std::cerr << "Trying with constraint " << ip << std::endl;
    print_vector("np", np, n);
#endif

l2a:/* Step 2a: determine step direction */
    /* compute z = H np: the step direction in the primal space (through J, see the paper) */
    compute_d(d, J, np);
    update_z(z, J, d, iq);
    /* compute N* np (if q > 0): the negative of the step direction in the dual space */
    update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
    std::cerr << "Step direction z" << std::endl;
    print_vector("z", z, n);
    print_vector("r", r, iq + 1);
    print_vector("u", u, iq + 1);
    print_vector("d", d, n);
    print_ivector("A", A, iq + 1);
#endif

    /* Step 2b: compute step length */
    l = 0;
    /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
    t1 = inf; /* +inf */
    /* find the index l s.t. it reaches the minimum of u+(x) / r */
    for (k = me; k < iq; k++)
    {
        if (r(k) > 0.0)
        {
            if (u(k) / r(k) < t1)
            {
                t1 = u(k) / r(k);
                l = A(k);
            }
        }
    }
    /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
    if (fabs(inner_prod(z, z))  > std::numeric_limits<double>::epsilon()) // i.e. z != 0
        t2 = -s(ip) / inner_prod(z, np);
    else
        t2 = inf; /* +inf */

    /* the step is chosen as the minimum of t1 and t2 */
    t = std::min(t1, t2);
#ifdef TRACE_SOLVER
    std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif

    /* Step 2c: determine new S-pair and take step: */

    /* case (i): no step in primal or dual space */
    if (t >= inf)
    {
        /* QPP is infeasible */
        // FIXME: unbounded to raise
        q = iq;
        return inf;
    }
    /* case (ii): step in dual space */
    if (t2 >= inf)
    {
        /* set u = u +  t * [-r 1) and drop constraint l from the active set A */
        for (k = 0; k < iq; k++)
            u(k) -= t * r(k);
        u(iq) += t;
        iai(l) = l;
        delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
        std::cerr << " in dual space: "
            << f_value << std::endl;
        print_vector("x", x, n);
        print_vector("z", z, n);
        print_ivector("A", A, iq + 1);
#endif
        goto l2a;
    }

    /* case (iii): step in primal and dual space */

    /* set x = x + t * z */
    for (k = 0; k < n; k++)
        x(k) += t * z(k);
    /* update the solution value */
    f_value += t * inner_prod(z, np) * (0.5 * t + u(iq));
    /* u = u + t * (-r 1) */
    for (k = 0; k < iq; k++)
        u(k) -= t * r(k);
    u(iq) += t;
#ifdef TRACE_SOLVER
    std::cerr << " in both spaces: "
        << f_value << std::endl;
    print_vector("x", x, n);
    print_vector("u", u, iq + 1);
    print_vector("r", r, iq + 1);
    print_ivector("A", A, iq + 1);
#endif

    if (t == t2)
    {
#ifdef TRACE_SOLVER
        std::cerr << "Full step has taken " << t << std::endl;
        print_vector("x", x, n);
#endif
        /* full step has taken */
        /* add constraint ip to the active set*/
        if (!add_constraint(R, J, d, iq, R_norm))
        {
            iaexcl[ip] = false;
            delete_constraint(R, J, A, u, p, iq, ip);
#ifdef TRACE_SOLVER
            print_matrix("R", R, n);
            print_ivector("A", A, iq);
#endif
            for (i = 0; i < m; i++)
                iai(i) = i;
            for (i = 0; i < iq; i++)
            {
                A(i) = A_old(i);
                iai(A(i)) = -1;
                u(i) = u_old(i);
            }
            for (i = 0; i < n; i++)
                x(i) = x_old(i);
            goto l2; /* go to step 2 */
        }
        else
            iai(ip) = -1;
#ifdef TRACE_SOLVER
        print_matrix("R", R, n);
        print_ivector("A", A, iq);
#endif
        goto l1;
    }

    /* a patial step has taken */
#ifdef TRACE_SOLVER
    std::cerr << "Partial step has taken " << t << std::endl;
    print_vector("x", x, n);
#endif
    /* drop constraint l */
    iai(l) = l;
    delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
    print_matrix("R", R, n);
    print_ivector("A", A, iq);
#endif

    /* update s[ip) = CI * x + ci0 */
    sum = 0.0;
    for (k = 0; k < n; k++)
        sum += CI(k,ip) * x(k);
    s(ip) = sum + ci0(ip);

#ifdef TRACE_SOLVER
    print_vector("s", s, mi);
#endif
    goto l2a;
}

/**
 * Compute d.
 */
inline void compute_d(vector<double> &d, matrix<double>& J, vector<double>& np)
{ int n=np.size();
    register int i, j;
    register double sum;

    /* compute d = H^T * np */
    for (i = 0; i < n; i++)
    {
        sum = 0.0;
        for (j = 0; j < n; j++)
            sum += J(j,i) * np(j);
        d(i) = sum;
    }
}

/**
 * Update_z
 */
inline void update_z(vector<double>& z, matrix<double>& J, vector<double>& d,  int iq)
{
    register int i, j;
    int n=z.size();
    /* setting of z = H * d */
    for (i = 0; i < n; i++)
    {
        z(i) = 0.0;
        for (j = iq; j < n; j++)
            z(i) += J(i,j) * d(j);
    }
}

/**
 * Update r.
 */
inline void update_r(matrix<double>& R, vector<double> &r, vector<double> &d, int iq)
{
    register int i, j;
    register double sum;

    /* setting of r = R^-1 d */
    for (i = iq - 1; i >= 0; i--)
    {
        sum = 0.0;
        for (j = i + 1; j < iq; j++)
            sum += R(i,j) * r(j);
        r(i) = (d(i) - sum) / R(i,i);
    }
}

/**
 * Add constraints.
 */
inline bool add_constraint(matrix<double>& R, matrix<double>& J, vector<double>& d, int& iq, double& R_norm)
{
    int n=J.size1();
#ifdef TRACE_SOLVER
    std::cerr << "Add constraint " << iq << '/';
#endif
    int i, j, k;
    double cc, ss, h, t1, t2, xny;

    /* we have to find the Givens rotation which will reduce the element
       d(j) to zero.
       if it is already zero we don't have to do anything, except of
       decreasing j */
    for (j = n - 1; j >= iq + 1; j--)
    {
        /* The Givens rotation is done with the matrix (cc cs, cs -cc).
           If cc is one, then element (j) of d is zero compared with element
           (j - 1). Hence we don't have to do anything.
           If cc is zero, then we just have to switch column (j) and column (j - 1)
           of J. Since we only switch columns in J, we have to be careful how we
           update d depending on the sign of gs.
           Otherwise we have to apply the Givens rotation to these columns.
           The i - 1 element of d has to be updated to h. */
        cc = d(j - 1);
        ss = d(j);
        h = distance(cc, ss);
        if (h == 0.0)
            continue;
        d(j) = 0.0;
        ss = ss / h;
        cc = cc / h;
        if (cc < 0.0)
        {
            cc = -cc;
            ss = -ss;
            d(j - 1) = -h;
        }
        else
            d(j - 1) = h;
        xny = ss / (1.0 + cc);
        for (k = 0; k < n; k++)
        {
            t1 = J(k,j - 1);
            t2 = J(k,j);
            J(k,j - 1) = t1 * cc + t2 * ss;
            J(k,j) = xny * (t1 + J(k,j - 1)) - t2;
        }
    }
    /* update the number of constraints added*/
    iq++;
    /* To update R we have to put the iq components of the d vector
       into column iq - 1 of R
     */
    for (i = 0; i < iq; i++)
        R(i,iq - 1) = d(i);
#ifdef TRACE_SOLVER
    std::cerr << iq << std::endl;
#endif

    if (fabs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
        // problem degenerate
        return false;
    R_norm = std::max<double>(R_norm, fabs(d(iq - 1)));
    return true;
}

/**
 * Delete constraints.
 */
inline void delete_constraint(matrix<double>& R, matrix<double>& J, vector<int>& A, vector<double>& u,  int p, int& iq, int l)
{

    int n=R.size1();
#ifdef TRACE_SOLVER
    std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
    int i, j, k;
    int qq = 0;
    double cc, ss, h, xny, t1, t2;

    /* Find the index qq for active constraint l to be removed */
    for (i = p; i < iq; i++)
        if (A(i) == l)
        {
            qq = i;
            break;
        }

    /* remove the constraint from the active set and the duals */
    for (i = qq; i < iq - 1; i++)
    {
        A(i) = A(i + 1);
        u(i) = u(i + 1);
        for (j = 0; j < n; j++)
            R(j,i) = R(j,i + 1);
    }

    A(iq - 1) = A(iq);
    u(iq - 1) = u(iq);
    A(iq) = 0;
    u(iq) = 0.0;
    for (j = 0; j < iq; j++)
        R(j,iq - 1) = 0.0;
    /* constraint has been fully removed */
    iq--;
#ifdef TRACE_SOLVER
    std::cerr << '/' << iq << std::endl;
#endif

    if (iq == 0)
        return;

    for (j = qq; j < iq; j++)
    {
        cc = R(j,j);
        ss = R(j + 1,j);
        h = distance(cc, ss);
        if (h == 0.0)
            continue;
        cc = cc / h;
        ss = ss / h;
        R(j + 1,j) = 0.0;
        if (cc < 0.0)
        {
            R(j,j) = -h;
            cc = -cc;
            ss = -ss;
        }
        else
            R(j,j) = h;

        xny = ss / (1.0 + cc);
        for (k = j + 1; k < iq; k++)
        {
            t1 = R(j,k);
            t2 = R(j + 1,k);
            R(j,k) = t1 * cc + t2 * ss;
            R(j + 1,k) = xny * (t1 + R(j,k)) - t2;
        }
        for (k = 0; k < n; k++)
        {
            t1 = J(k,j);
            t2 = J(k,j + 1);
            J(k,j) = t1 * cc + t2 * ss;
            J(k,j + 1) = xny * (J(k,j) + t1) - t2;
        }
    }
}

/**
 * Get distance.
 */
inline double distance(double a, double b)
{
    register double a1, b1, t;
    a1 = fabs(a);
    b1 = fabs(b);
    if (a1 > b1)
    {
        t = (b1 / a1);
        return a1 * sqrt(1.0 + t * t);
    }
    else
        if (b1 > a1)
        {
            t = (a1 / b1);
            return b1 * sqrt(1.0 + t * t);
        }
    return a1 * sqrt(2.0);
}

/**
 * Compute the Choleski factorization of a real symmetric positive-definite square matrix.
 */
inline void cholesky_decomposition(matrix<double> & A)
{
    register int i, j, k;
    register double sum;
    int n=A.size1();
    for (i = 0; i < n; i++)
    {
        for (j = i; j < n; j++)
        {
            sum = A(i,j);
            for (k = i - 1; k >= 0; k--)
                sum -= A(i,k)*A(j,k);
            if (i == j)
            {
                if (sum <= 0.0)
                {
                    // raise error
                    //		print_matrix("A", A, n);
                    throw std::runtime_error("The matrix passed to the Cholesky A = L L^T decomposition is not positive definite");
                }
                A(i,i) = sqrt(sum);
            }
            else
                A(j,i) = sum / A(i,i);
        }
        for (k = i + 1; k < n; k++)
            A(i,k) = A(k,i);
    }
}

/**
 * Cholesky solve.
 */
inline void cholesky_solve(matrix<double>& L, vector<double> &x, vector<double> & b)
{

    vector<double> y(x.size());

    /* Solve L * y = b */
    forward_elimination(L, y, b);
    /* Solve L^T * x = y */
    backward_elimination(L, x, y);
}

/**
 * Forward elimination.
 */
inline void forward_elimination(matrix<double>& L, vector<double>& y, vector<double>& b)
{
    register int i, j;
    int n=y.size();
    y(0) = b(0) / L(0,0);
    for (i = 1; i < n; i++)
    {
        y(i) = b(i);
        for (j = 0; j < i; j++)
            y(i) -= L(i,j) * y(j);
        y(i) = y(i) / L(i,i);
    }
}

/**
 * Backward elimination.
 */
inline void backward_elimination(matrix<double>& U,vector<double>& x, vector<double>& y){
    int n=x.size();

    register int i, j;

    x(n - 1) = y(n - 1) / U(n - 1,n - 1);
    for (i = n - 2; i >= 0; i--){
        x(i) = y(i);
        for (j = i + 1; j < n; j++)
            x(i) -= U(i,j) * x(j);
        x(i) = x(i) / U(i,i);
    }
}

#endif


#include <iostream>
#include <vector>

#include <Eigen/Dense>

#include "funnels/utils.hh"

#include "funnels/distances.hh"
#include "funnels/dynamics.hh"
#include "funnels/funnels.hh"

using namespace std;
using namespace Eigen;
using namespace funnels;
using namespace lyapunov;

void test_dist(){

  Matrix3d M;
  M(0,0) = 1.;
  M(1,1) = 2.;
  M(2,2) = 3.;

  VectorXd v(3);
  v(0)=11.;
  v(1)=12.;
  v(2)=13.;

  partial_so2_dist_t my_dist;
  std::cout << my_dist.cp_Mv(M, v) << std::endl;
  std::cout << my_dist.cp_vM(v, M) << std::endl;
  std::cout << my_dist.cp_vv(v, v) << std::endl;
  std::cout << my_dist.cp_MM(M, M) << std::endl;
}

void test_dyn(){

  kinematic_2d_sys_t my_sys;
  VectorXd u(2);
  VectorXd t(10);
  MatrixXd x(4,10);

  t.setLinSpaced(10, 0., 10.);
  u(0)=1.;
  u(1)=1.33;
  x(0,0) = -5.55;
  x(1,0) = 6.55;

  my_sys.compute(x,u,t);

  std::cout << x << std::endl;
  std::cout << u << std::endl;
  std::cout << t << std::endl;

}

void test_funnel1(){

  std::cout << "funnel 1" << std::endl;

  clock_ta_t &ctrl_clk = utils_ext::clock_map.create_and_get("c_t");
  clock_ta_t &lcl_clk = utils_ext::clock_map.create_and_get("c_h");

  kinematic_2d_sys_t my_dyn;
  partial_so2_dist_t my_dist;

  MatrixXd P(my_dyn._dimx, my_dyn._dimx);
  for (size_t i=0; i<my_dyn._dimx; i++){
    P(i,i) = (double) i+1.;
  }

  fixed_ellipsoidal_lyap_t my_lyap(P, 0.5, my_dist);

  process_t &my_proc = utils_ext::process_map.create_and_get("proc_0");

  // Init
  Vector4d x0;
  Vector2d u0;
  x0(0) = 0.;
  x0(1) = 2.;
  u0(0) = 1.1;
  u0(1) = 0.66;
  const double t0=0., t1=10.;

  funnel_t<fixed_ellipsoidal_lyap_t<partial_so2_dist_t>, kinematic_2d_sys_t>
      my_fun(100, "fun_0", my_proc, my_dyn, P, 0.5, my_dist);

  my_fun.compute(x0, t0, t1, u0);

  std::cout << my_fun.x() << std::endl;
  std::cout << my_fun.u() << std::endl;
  std::cout << my_fun.t() << std::endl;

}

void test_funnel_sys(){

  std::cout << "funnel sys" << std::endl;

  using fun_t = funnel_t<fixed_ellipsoidal_lyap_t<partial_so2_dist_t>,
      kinematic_2d_sys_t>;

  using fun_ptr_t = shared_ptr<fun_t>;

  clock_ta_t &ctrl_clk = utils_ext::clock_map["c_t"];
  clock_ta_t &lcl_clk = utils_ext::clock_map["c_h"];

  kinematic_2d_sys_t my_dyn;
  partial_so2_dist_t my_dist;

  MatrixXd P(my_dyn._dimx, my_dyn._dimx);
  for (size_t i=0; i<my_dyn._dimx; i++){
    P(i,i) = (double) 1.;
  }

  fixed_ellipsoidal_lyap_t my_lyap(P, 0.5, my_dist);

  process_t &my_proc = utils_ext::process_map["proc_0"];

  // Init
  Vector4d x0, x1;
  Vector2d u0, u1;
  x0(0) = -1.; x1(0) =  0.;
  x0(1) =  0.; x1(1) = -1.;
  u0(0) = 1.; u0(0) = 1.1;
  u0(1) = 0.66; u0(1) = 0.66;
  const double t0=0., t1=10.;

  fun_ptr_t my_fun0_0 = make_shared<fun_t>(100, "fun_0_0", my_proc, my_dyn, P,
      0.5,
      my_dist);

  fun_ptr_t my_fun1_0 = make_shared<fun_t>(100, "fun_1_0", my_proc, my_dyn,
      2.*P,
      0.5, my_dist);

  fun_ptr_t my_fun1_1 = make_shared<fun_t>(100, "fun_1_1", my_proc, my_dyn,
      4.*P, 0.5, my_dist);

  my_fun0_0->compute(x0, t0, t1, u0);
  my_fun1_0->compute(x1, t0, t1, u1);

  fun_t::set_parent(my_fun1_0, my_fun1_1);

  std::cout << my_fun0_0.use_count() << " ; " << my_fun1_0.use_count()
      << " ; " << my_fun1_1.use_count() << std::endl;

}

int main() {

  test_dist();
  test_dyn();
  test_funnel1();
  test_funnel_sys();

  return 0;
}


// Copyright Oleg Maximenko 2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/definitions.hpp>
#include <svgpp/utility/gil/common.hpp>
#include <boost/gil/channel_algorithm.hpp>
#include <boost/gil/color_base_algorithm.hpp>

namespace svgpp
{

namespace gil_detail
{

namespace gil = boost::gil;

template<class BlendModeTag, class ChannelValue>
struct blend_channel_fn;

template<class ChannelValue>
struct blend_alpha_fn;

// TODO: default implementation for non-8 bit channels
// TODO: clamp_channels is not needed for premultiplied values

// For signed channels we call unsigned analog, converting forward and back
template<class BlendModeTag>
struct blend_channel_fn<BlendModeTag, boost::int8_t>
{
  boost::int8_t operator()(boost::int8_t channel_a, boost::int8_t channel_b, boost::int8_t alpha_a, boost::int8_t alpha_b) const
  {
    typedef gil::detail::channel_convert_to_unsigned<boost::int8_t> to_unsigned;
    typedef gil::detail::channel_convert_from_unsigned<boost::int8_t> from_unsigned;
    blend_channel_fn<BlendModeTag, boost::uint8_t> converter_unsigned;
    return from_unsigned()(converter_unsigned(
      to_unsigned()(channel_a), to_unsigned()(channel_b), to_unsigned()(alpha_a), to_unsigned()(alpha_b)));
  }
};

// normal	cr = (1 - alpha_a) * channel_b + channel_a
template<>
struct blend_channel_fn<tag::value::normal, boost::uint8_t>
{
  boost::uint8_t operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8(((255 - alpha_a) * channel_b) / 255 + channel_a);
  }
};

// multiply	cr = (1-alpha_a)*channel_b + (1-alpha_b)*channel_a + channel_a*channel_b
template<>
struct blend_channel_fn<tag::value::multiply, boost::uint8_t>
{
  boost::uint8_t operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8(((255 - alpha_a) * channel_b + (255 - alpha_b) * channel_a + channel_a * channel_b) / 255);
  }
};

// screen	cr = channel_b + channel_a - channel_a * channel_b
template<>
struct blend_channel_fn<tag::value::screen, boost::uint8_t>
{
  boost::uint8_t operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8(channel_b + channel_a - channel_a * channel_b / 255);
  }
};

// darken	cr = Min ((1 - alpha_a) * channel_b + channel_a, (1 - alpha_b) * channel_a + channel_b)
template<>
struct blend_channel_fn<tag::value::darken, boost::uint8_t>
{
  boost::uint8_t operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8(std::min((255 - alpha_a) * channel_b / 255 + channel_a, (255 - alpha_b) * channel_a / 255 + channel_b));
  }
};

// lighten	cr = Max ((1 - alpha_a) * channel_b + channel_a, (1 - alpha_b) * channel_a + channel_b)
template<>
struct blend_channel_fn<tag::value::lighten, boost::uint8_t>
{
  boost::uint8_t operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8(std::max((255 - alpha_a) * channel_b / 255 + channel_a, (255 - alpha_b) * channel_a / 255 + channel_b));
  }
};

// qr = 1 - (1-alpha_a)*(1-alpha_b)
template<>
struct blend_alpha_fn<boost::uint8_t>
{
  boost::uint8_t operator()(int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8(255 - (255 - alpha_a) * (255 - alpha_b) / 255);
  }
};

} // namespace gil_detail

namespace gil_utility
{

template<class BlendModeTag>
struct blend_pixel
{
  template<class Color>
  Color operator()(const Color & pixa, const Color & pixb) const
  {
    namespace gil = boost::gil;

    typename gil::color_element_type<Color, gil::alpha_t>::type
      alpha_a = gil::get_color(pixa, gil::alpha_t()),
      alpha_b = gil::get_color(pixb, gil::alpha_t());

    Color result;

    gil::get_color(result, gil::red_t())
      = gil_detail::blend_channel_fn<BlendModeTag, typename gil::color_element_type<Color, gil::red_t>::type>()(
        gil::get_color(pixa, gil::red_t()), gil::get_color(pixb, gil::red_t()),
        alpha_a, alpha_b);

    gil::get_color(result, gil::green_t())
      = gil_detail::blend_channel_fn<BlendModeTag, typename gil::color_element_type<Color, gil::green_t>::type>()(
        gil::get_color(pixa, gil::green_t()), gil::get_color(pixb, gil::green_t()),
        alpha_a, alpha_b);

    gil::get_color(result, gil::blue_t())
      = gil_detail::blend_channel_fn<BlendModeTag, typename gil::color_element_type<Color, gil::blue_t>::type>()(
        gil::get_color(pixa, gil::blue_t()), gil::get_color(pixb, gil::blue_t()),
        alpha_a, alpha_b);

    gil::get_color(result, gil::alpha_t()) =
      gil_detail::blend_alpha_fn<typename gil::color_element_type<Color, gil::alpha_t>::type>()(alpha_a, alpha_b);

    return result;
  }
};

}}


#include "integration.hpp"
#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>
using namespace std;

void forwardEulerStep(PhysicalSystem *system, double dt) {
    int n = system->getDOFs();
    static VectorXd x0(n), v0(n);
    system->getState(x0, v0);
    static MatrixXd Mi(n,n);
    system->getInverseInertia(Mi);
    static VectorXd f0(n);
    system->getForces(f0);
    static VectorXd a0(n); // acceleration
    a0 = Mi*f0;
    x0 = x0 + v0*dt;
    v0 = v0 + a0*dt;
    system->setState(x0, v0);
}

VectorXd solve(const MatrixXd &A, const VectorXd &b) {
    SparseMatrix<double> spA = A.sparseView();
    ConjugateGradient< SparseMatrix<double> > solver;
    return solver.compute(spA).solve(b);
}

void backwardEulerStep(PhysicalSystem *system, double dt) {
    int n = system->getDOFs();
    static VectorXd x0(n), v0(n);
    system->getState(x0, v0);
    static MatrixXd M(n,n);
    system->getInertia(M);
    static VectorXd f(n);
    static MatrixXd Jx(n,n), Jv(n,n);
    system->getForces(f);
    system->getJacobians(Jx, Jv);
    MatrixXd A = M - Jx*dt*dt - Jv*dt;
    VectorXd b = (f + Jx*v0*dt)*dt;
    VectorXd dv = solve(A, b);
    VectorXd v1 = v0 + dv;
    system->setState(x0 + v1*dt, v1);
}

void projectPositions(ConstrainedSystem *system) {
    int n = system->getDOFs(),	m = system->getConstraints();
	static VectorXd c(m);			system->getConstraintValues(c);
    static MatrixXd J(m,n);			system->getConstraintJacobian(J);
	static MatrixXd Mi(n, n);		system->getInverseInertia(Mi);
	//printf("Rows: %d Cols: %d", c.rows(), c.cols());
    MatrixXd A = J*Mi*J.transpose();

	//printf("Rows: %d Cols: %d", A.rows(), A.cols());

    VectorXd lambda = solve(A, -c);
    static VectorXd dx(n);		    dx = Mi*J.transpose()*lambda;
    static VectorXd x(n), v(n);		system->getState(x, v);
    system->setState(x + dx, v);
}

void projectVelocities(ConstrainedSystem *system) {
    int n = system->getDOFs(), m = system->getConstraints();
    static VectorXd x(n), v(n);
    system->getState(x, v);
	v = v * 0.995; //global dampening
    static MatrixXd J(m,n);
    system->getConstraintJacobian(J);
    static MatrixXd Mi(n,n);
    system->getInverseInertia(Mi);
    MatrixXd A = J*Mi*J.transpose();
    VectorXd b = -J*v;
    VectorXd lambda = solve(A, b);
    static VectorXd dv(n);
    dv = Mi*J.transpose()*lambda;
    system->setState(x, v + dv);
}

void constrainedForwardEulerStep(ConstrainedSystem *system, double dt) {
    forwardEulerStep(system, dt);
    projectPositions(system);
    projectVelocities(system);
}

void constrainedBackwardEulerStep(ConstrainedSystem *system, double dt) {
    // Let's not bother with this one.
	backwardEulerStep(system, dt);
	projectPositions(system);
	projectVelocities(system);
}


#include "Lsh.hpp"
#include "ExpressionMatrixSubset.hpp"
#include "SimilarPairs.hpp"
#include "timestamp.hpp"
using namespace ChanZuckerberg;
using namespace ExpressionMatrix2;

#include <boost/math/constants/constants.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <chrono>
#include "fstream.hpp"

Lsh::Lsh(
    const string& name,             // Name prefix for memory mapped files.
    const ExpressionMatrixSubset& expressionMatrixSubset,
    size_t lshCount,                // Number of LSH hyperplanes
    uint32_t seed                   // Seed to generate LSH hyperplanes.
    )
{
    // Store the Info object.
    info.createNew(name + "-Info");
    info->lshCount = lshCount;
    info->cellCount = expressionMatrixSubset.cellCount();

    // Generate the LSH vectors.
    cout << timestamp << "Generating LSH vectors." << endl;
    generateLshVectors(expressionMatrixSubset.geneCount(), lshCount, seed);

    // Compute cell signatures.
    cout << timestamp << "Computing cell LSH signatures." << endl;
    computeCellLshSignatures(name, expressionMatrixSubset);

    // Compute the similarity table.
    // This is a look up table indexed by the number of mismatching bits.
    // Each entry contgains the similarity corresponding to that number
    // of mismatching bits.
    computeSimilarityTable();
}

// Access an existing Lsh object.
Lsh::Lsh(
    const string& name              // Name prefix for memory mapped files.
    )
{
    // Access the memory mapped data.
    info.accessExistingReadOnly(name + "-Info");
    signatures.accessExistingReadOnly(name + "-Signatures");

    // Compute the number of 64 bit words in each cell signature.
    signatureWordCount = (lshCount()-1)/64 + 1;

    // Compute the similarity table.
    // This is a look up table indexed by the number of mismatching bits.
    // Each entry contgains the similarity corresponding to that number
    // of mismatching bits.
    computeSimilarityTable();
}

// Generate the LSH vectors.
void Lsh::generateLshVectors(
    size_t geneCount,
    size_t lshCount,                // Number of LSH hyperplanes
    uint32_t seed                   // Seed to generate LSH hyperplanes.
)
{
    // Prepare to generate normally vector distributed components.
    using RandomSource = boost::mt19937;
    using NormalDistribution = boost::normal_distribution<>;
    RandomSource randomSource(seed);
    NormalDistribution normalDistribution;
    boost::variate_generator<RandomSource, NormalDistribution> normalGenerator(randomSource, normalDistribution);

    // Allocate space for the LSH vectors.
    lshVectors.resize(geneCount, vector<double>(lshCount, 0.));

    // Sum of the squares of the components of each of the LSH vectors.
    vector<double> normalizationFactor(lshCount, 0.);

    // Loop over genes.
    for(size_t geneId = 0; geneId<geneCount; geneId++) {

        // For this gene, generate the components of all the LSH vectors.
        for(size_t lshVectorId = 0; lshVectorId<lshCount; lshVectorId++) {

            const double x = normalGenerator();
            lshVectors[geneId][lshVectorId] = x;

            // Update the sum of squares for this LSH vector.
            normalizationFactor[lshVectorId] += x*x;
        }
    }

    // Normalize each of the LSH vectors.
    for(auto& f: normalizationFactor) {
        f = 1. / sqrt(f);
    }
    for(size_t geneId = 0; geneId<geneCount; geneId++) {
        for(size_t lshVectorId = 0; lshVectorId<lshCount; lshVectorId++) {
            lshVectors[geneId][lshVectorId] *= normalizationFactor[lshVectorId];
        }
    }

}

// Compute the LSH signatures of all cells in the cell set we are using.
void Lsh::computeCellLshSignatures(
    const string& name,             // Name prefix for memory mapped files.
    const ExpressionMatrixSubset& expressionMatrixSubset)
{
    // Get the number of LSH vectors.
    CZI_ASSERT(!lshVectors.empty());
    const size_t lshCount = info->lshCount;

    // Compute the number of 64 bit words in each cell signature.
    signatureWordCount = (lshCount-1)/64 + 1;

    // Get the number of genes and cells in the gene set and cell set we are using.
    const auto geneCount = expressionMatrixSubset.geneCount();
    const auto cellCount = expressionMatrixSubset.cellCount();
    CZI_ASSERT(lshVectors.size() == geneCount);

    // Compute the sum of the components of each lsh vector.
    // It is needed below to compute the contribution of the
    // expression counts that are zero.
    vector<double> lshVectorsSums(lshCount, 0.);
    for(GeneId localGeneId=0; localGeneId!=geneCount; localGeneId++) {
        const auto& v = lshVectors[localGeneId]; // Components of all LSH vectors for this gene.
        CZI_ASSERT(v.size() == lshCount);
        for(size_t i=0; i<lshCount; i++) {
            lshVectorsSums[i] += v[i];
        }
    }

    // Initialize the cell signatures.
    cout << timestamp << "Initializing cell LSH signatures." << endl;
    signatures.createNew(name + "-Signatures", cellCount*signatureWordCount);

    // Vector to contain, for a single cell, the scalar products of the shifted
    // expression vector for the cell with all of the LSH vectors.
    vector<double> scalarProducts(lshCount);

    // Loop over all the cells in the cell set we are using.
    // The CellId is local to the cell set we are using.
    const CellId messageFrequency = CellId(1.e7 / double(lshCount));
    cout << timestamp << "Computation of cell LSH signatures begins." << endl;
    const auto t0 = std::chrono::steady_clock::now();
    for(CellId localCellId=0; localCellId<cellCount; localCellId++) {
        if((localCellId % messageFrequency) == 0) {
            cout << timestamp << "Working on cell " << localCellId << " of " << cellCount << endl;
        }

        // Compute the mean of the expression vector for this cell.
        const ExpressionMatrixSubset::Sum& sum = expressionMatrixSubset.sums[localCellId];
        const double mean = sum.sum1 / double(geneCount);

        // If U is one of the LSH vectors, we need to compute the scalar product
        // s = X*U, where X is the cell expression vector, shifted to zero mean:
        // X = x - mean,
        // mean = sum(x)/geneCount (computed above).
        // We get:
        // s = (x-mean)*U = x*U - mean*U = x*U - mean*sum(U)
        // We computed sum(U) above and stored it in lshVectorSums for
        // each of the LSH vectors.
        // Initialize the scalar products for this cell
        // with all of the LSH vectors to -mean*sum(U).
        for(size_t i=0; i<lshCount; i++) {
            scalarProducts[i] = -mean * lshVectorsSums[i];
        }

        // Now add to each scalar product the x*U portion.
        // For performance, the loop over genes is outside,
        // which gives better memory locality.
        // Add the contributions of the non-zero expression counts for this cell.
        for(const auto& p : expressionMatrixSubset.cellExpressionCounts[localCellId]) {
            const GeneId localGeneId = p.first;
            const double count = double(p.second);

            // Add the contribution of this gene to the scalar products.
            const auto& v = lshVectors[localGeneId];
            CZI_ASSERT(v.size() == lshCount);
            for(size_t i=0; i<lshCount; i++) {
                scalarProducts[i] += count * v[i];
            }
        }

        // Set to 1 the signature bits corresponding to positive scalar products.
        BitSetPointer cellSignature = getSignature(localCellId);
        for(size_t i=0; i<lshCount; i++) {
            if(scalarProducts[i]>0.) {
                cellSignature.set(i);
            }
        }

    }
    const auto t1 = std::chrono::steady_clock::now();
    cout << timestamp << "Computation of cell LSH signatures ends." << endl;
    const size_t nonZeroExpressionCount = expressionMatrixSubset.totalExpressionCounts();
    cout << "Processed " << nonZeroExpressionCount << " non-zero expression counts for ";
    cout << geneCount << " genes and " << cellCount << " cells." << endl;
    cout << "Average number of expression counts per cell  is " << double(nonZeroExpressionCount) / double(cellCount) << endl;
    cout << "Average expression matrix sparsity is " <<
        double(nonZeroExpressionCount) / (double(geneCount) * double(cellCount)) << endl;
    const double t01 = 1.e-9 * double((std::chrono::duration_cast<std::chrono::nanoseconds>(t1 - t0)).count());
    cout << "Computation of LSH cell signatures took " << t01 << "s." << endl;
    cout << "    Seconds per cell " << t01 / cellCount << endl;
    cout << "    Seconds per non-zero expression matrix entry " << t01/double(nonZeroExpressionCount) << endl;
    cout << "    Seconds per inner loop iteration " << t01 / (double(nonZeroExpressionCount) * double(lshCount)) << endl;
    cout << "    Gflop/s " << 2. * 1e-9 * double(nonZeroExpressionCount) * double(lshCount) / t01 << endl;

}

// Compute the similarity (cosine of the angle) corresponding to each number of mismatching bits.
void Lsh::computeSimilarityTable()
{
    // Initialize the similarity table.
    similarityTable.resize(lshCount() + 1);

    // Loop over all possible numbers of mismatching bits.
    for(size_t mismatchingBitCount = 0;
        mismatchingBitCount <= lshCount(); mismatchingBitCount++) {

        // Compute the angle between the vectors corresponding to
        // this number of mismatching bits.
        const double angle = double(mismatchingBitCount) *
            boost::math::double_constants::pi / double(lshCount());

        // The cosine of the angle is the similarity for
        // this number of mismatcning bits.
        CZI_ASSERT(mismatchingBitCount < similarityTable.size());
        similarityTable[mismatchingBitCount] = std::cos(angle);
    }

}

// Compute the LSH similarity between two cells,
// specified by their ids local to the cell set used by this Lsh object.
double Lsh::computeCellSimilarity(CellId localCellId0, CellId localCellId1)
{
    // Access the LSH signatures for the two cells.
    const BitSetPointer signature0 = getSignature(localCellId0);
    const BitSetPointer signature1 = getSignature(localCellId1);

    // Count the number of bits where the signatures of these two cells disagree.
    const size_t mismatchingBitCount = countMismatches(signature0, signature1);

    // Return the similarity corresponding to this number of mismatching bits.
    return similarityTable[mismatchingBitCount];
}
size_t Lsh::computeMismatchCount(CellId localCellId0, CellId localCellId1)
{
    // Access the LSH signatures for the two cells.
    const BitSetPointer signature0 = getSignature(localCellId0);
    const BitSetPointer signature1 = getSignature(localCellId1);

    // Count the number of bits where the signatures of these two cells disagree.
    return countMismatches(signature0, signature1);
}

// Write to a csv file statistics of the cell LSH signatures..
void Lsh::writeSignatureStatistics(const string& csvFileName)
{
    ofstream csv(csvFileName);
    writeSignatureStatistics(csv);
}
void Lsh::writeSignatureStatistics(ostream& csv)
{
    csv << "Bit,Set,Unset,Total\n";

    for(size_t i = 0; i < info->lshCount; i++) {

        // Count the number of cells that have this bit set.
        size_t setCount = 0;
        for(CellId cellId = 0; cellId < info->cellCount; cellId++) {
            if(getSignature(cellId).get(i)) {
                ++setCount;
            }
        }
        const size_t unsetCount = info->cellCount - setCount;

        csv << i << "," << setCount << "," << unsetCount << "," << info->cellCount << "\n";

    }

}

void Lsh::remove()
{
    signatures.remove();
    info.remove();
}


#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/mat_ZZ.h>
#include <gmp.h>

#include "io.h"
#include "params.h"

using namespace std;
using namespace NTL;

void PrintDoubleTab(const unsigned int N, RR_t const * const Tab)
{

    cout << "[";
    for(unsigned int j=0; j<N; j++)
    {
        cout << ((long double)Tab[j]) << "    ";
    }
    cout << "]" << endl;
}

void PrintDoubleMatrix(const unsigned int N, const RR_t Tab[2*N0][2*N0])
{

    for(unsigned int j=0; j<N; j++)
    {
        PrintDoubleTab(N,Tab[j]);
    }
    cout << endl;
}

void PrintIntTab(const unsigned int N, long int const * const Tab)
{

    cout << "[";
    for(unsigned int j=0; j<N; j++)
    {
        cout << Tab[j] << " ";
    }
    cout << "]" << endl;
    cout << endl;
}

void PrintFFT(CC_t const * const f_fft)
{
    unsigned int i;
    cout << "[";
    for(i=0; i<N0; i++)
    {
        cout << (f_fft[i]).real() << "+ I*" << (f_fft[i]).imag() << "	";
    }
    cout << "]" << endl;
}


//  Copyright (c) 2014 Hartmut Kaiser
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// This is the third in a series of examples demonstrating the development of a
// fully distributed solver for a simple 1D heat distribution problem.
//
// This example takes the code from example one and introduces a partitioning
// of the 1D grid into groups of grid partitions which are handled at the same time.
// The purpose is to be able to control the amount of work performed. The
// example is still fully serial, no parallelization is performed.
//
// The only difference to 1d_stencil_3 is that this example uses OpenMP for
// parallelizing the inner loop.

//#include <boost/cstdint.hpp>
//#include <boost/program_options.hpp>
//#include <boost/chrono.hpp>

#include <vector>
#include <cstdlib>

///////////////////////////////////////////////////////////////////////////////
// Timer with nanosecond resolution
/*inline int now()
{
    boost::chrono::nanoseconds ns =
        boost::chrono::steady_clock::now().time_since_epoch();
    return static_cast<int>(ns.count());
}*/

///////////////////////////////////////////////////////////////////////////////
double k = 0.5;     // heat transfer coefficient
double dt = 1.;     // time step
double dx = 1.;     // grid spacing

inline int idx(int i, int size)
{
    return (int(i) < 0) ? (i + size) % size : i % size;
}

///////////////////////////////////////////////////////////////////////////////
// Our partition_data data type
struct partition_data
{
    partition_data(int size = 0)
      : data_(size)
    {}

    partition_data(int size, double initial_value)
      : data_(size)
    {
        double base_value = double(initial_value * size);
        for (int i = 0; i != size; ++i)
            data_[i] = base_value + double(i);
    }

    double& operator[](int idx) { return data_[idx]; }
    double operator[](int idx) const { return data_[idx]; }

    int size() const { return data_.size(); }

private:
    std::vector<double> data_;
};
/*
std::ostream& operator<<(std::ostream& os, partition_data const& c)
{
    os << "{";
    for (int i = 0; i != c.size(); ++i)
    {
        if (i != 0)
            os << ", ";
        os << c[i];
    }
    os << "}";
    return os;
}*/

///////////////////////////////////////////////////////////////////////////////
struct stepper
{
    // Our data for one time step
    typedef partition_data partition;
    typedef std::vector<partition> space;

    // Our operator
    static double heat(double left, double middle, double right)
    {
        return middle + (k*dt/dx*dx) * (left - 2*middle + right);
    }

    // The partitioned operator, it invokes the heat operator above on all
    // elements of a partition.
    static partition_data heat_part(partition_data const& left,
        partition_data const& middle, partition_data const& right)
    {
        int size = middle.size();
        partition_data next(size);

        next[0] = heat(left[size-1], middle[0], middle[1]);

        //# pragma omp parallel for
        for (int i = 1; i < int(size-1); ++i)
            next[i] = heat(middle[i-1], middle[i], middle[i+1]);

        next[size-1] = heat(middle[size-2], middle[size-1], right[0]);

        return next;
    }

    // do all the work on 'np' partitions, 'nx' data points each, for 'nt'
    // time steps
    space do_work(int np, int nx, int nt)
    {
        // U[t][i] is the state of position i at time t.
        std::vector<space> U(2);
        for (space& s: U)
            s.resize(np);

        // Initial conditions: f(0, i) = i
        for (int i = 0; i != np; ++i)
            U[0][i] = partition_data(nx, double(i));

        // Actual time step loop
        for (int t = 0; t != nt; ++t)
        {
            space const& current = U[t % 2];
            space& next = U[(t + 1) % 2];

# pragma omp parallel
# pragma omp single
            {
                for (int i = 0; i < np; ++i) {
# pragma omp task untied
                    next[i] = heat_part(current[idx(i-1, np)], current[i], current[idx(i+1, np)]);
                }
#pragma omp taskwait
            }
        }

        // Return the solution at time-step 'nt'.
        return U[nt % 2];
    }
};

//int hpx_main(boost::program_options::variables_map& vm)
int main(int argc, char* argv[])
{
    int np = 20;// Number of partitions.
    int nx = 1000000;   // Number of grid points.
    int nt = 100;   // Number of steps.

    // Create the stepper object
    stepper step;

    // Measure execution time.
//    int t = now();

    // Execute nt time steps on nx grid points and print the final solution.
    stepper::space solution = step.do_work(np, nx, nt);

//    for (int i = 0; i != np; ++i)
//        std::cout << "U[" << i << "] = " << solution[i] << std::endl;

//    int elapsed = now() - t;
//    std::cout << "Elapsed time: " << elapsed / 1e9 << " [s]" << std::endl;

    return 0;
}
/*
int main(int argc, char* argv[])
{
    namespace po = boost::program_options;

    po::options_description desc_commandline;
    desc_commandline.add_options()
        ("results", "print generated results (default: false)")
        ("nx", po::value<int>()->default_value(10),
         "Local x dimension (of each partition)")
        ("nt", po::value<int>()->default_value(45),
         "Number of time steps")
        ("np", po::value<int>()->default_value(10),
         "Number of partitions")
        ("k", po::value<double>(&k)->default_value(0.5),
         "Heat transfer coefficient (default: 0.5)")
        ("dt", po::value<double>(&dt)->default_value(1.0),
         "Timestep unit (default: 1.0[s])")
        ("dx", po::value<double>(&dx)->default_value(1.0),
         "Local x dimension")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc_commandline), vm);
    po::notify(vm);

    return hpx_main(vm);
}*/


//===------------------------------------------------------------*- C++ -*-===//
///
/// \brief 矩阵。
///
/// \sa [Eigen: Getting started](https://eigen.tuxfamily.org/dox/GettingStarted.html)
/// \sa [Eigen: The Matrix class](https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html)
///
/// \version 2021-10-20
/// \since 2021-10-20
/// \authors zhengrr
/// \copyright Unlicense
///
//===----------------------------------------------------------------------===//

#include <type_traits>

#include <Eigen/Dense>    // 稠密矩阵
#include <gtest/gtest.h>

/// \brief 矩阵模板。
/// \details
/// ```{.cpp}
/// Matrix <
///     typename Scalar,                               // 矩阵条目的标量类型，如 `int`、`float` 等。
///     int RowsAtCompileTime,                         // 在编译期确定的固定矩阵行数。使用 `Dynamic` 表示矩阵行数在运行期动态设置。
///     int ColsAtCompileTime,                         // 在编译期确定的固定矩阵列数。使用 `Dynamic` 表示矩阵列数在运行期动态设置。
///     int Options = 0,                               // 选项。
///     int MaxRowsAtCompileTime = RowsAtCompileTime,  // 对于无法确定具体大小，但可以确定大小上限的矩阵，可以设定此二值，
///     int MaxColsAtCompileTime = ColsAtCompileTime   // 以较大的固定矩阵来存储较小的动态矩阵，避免矩阵的动态内存分配。
/// >;
/// ```
TEST(Matrix, MatrixTemplate)
{
    SUCCEED();
}

/// \brief 便于使用的内置类型别名。
/// \details
/// ```{.cpp}
/// MatrixNt    = Matrix<type, N, N>  // 方阵。
/// VectorNt    = Matrix<type, N, 1>  // 列向量。
/// RowVectorNt = Matrix<type, 1, N>  // 行向量。
/// ```
/// 其中 `type` 为 `int`、`float`、`double`、`std::complex<float>` 或 `std::complex<double>`；
/// `N` 为 `2`、`3`、`4` 或 `X`（表示 `Dynamic`）。
TEST(Matrix, ConvenienceTypedefs)
{
    static_assert(std::is_same_v<
                  Eigen::Matrix2i,
                  Eigen::Matrix<int, 2, 2>
    >);
    static_assert(std::is_same_v<
                  Eigen::Vector4f,
                  Eigen::Matrix<float, 4, 1>
    >);
    static_assert(std::is_same_v<
                  Eigen::RowVectorXd,
                  Eigen::Matrix<double, 1, Eigen::Dynamic>
    >);
}

/// \brief 固定大小矩阵和动态大小矩阵对比。
/// \details
/// 经验法则：
/// *   对于 `size()` < ~16 的矩阵采用固定大小矩阵、在栈上分配内存，
///     这可以避免动态内存分配和展开循环、并提供更严格的类型检查，但也会增加编译时间、增大程序体积；
/// *   对于 ~32 < `size()` 的矩阵采用动态大小矩阵、在堆上分配内存，
///     此时该类矩阵的性能劣势变得可以忽略不计，而且可以避免栈溢出。
TEST(Matrix, FixedVsDynamicSize)
{
    SUCCEED();
}

/// \brief 默认构造。
TEST(Matrix, DefaultConstructors)
{
    Eigen::Matrix3i fixMat;  // 在栈上分配 `int[3*3]` 的空间，尚未初始化
    Eigen::MatrixXi dynMat;  // 尚未分配空间，尚未初始化

    Eigen::Vector3i fixVec;  // 在栈上分配 `int[3*1]` 的空间，尚未初始化
    Eigen::VectorXi dynVec;  // 尚未分配空间，尚未初始化
}

/// \brief 尺寸构造。
TEST(Matrix, SizedConstructors)
{
    Eigen::Matrix3i fixMat {3, 3};  // 在栈上分配 `int[3*3]` 的空间，尚未初始化；为统一接口而允许冗余的尺寸参数
    Eigen::MatrixXi dynMat {3, 3};  // 在堆上分配 `new int[3*3]` 的空间，尚未初始化

    Eigen::Vector3i fixVec {3};  // 在栈上分配 `int[3*1]` 的空间，尚未初始化；为统一接口而允许冗余的尺寸参数
    Eigen::VectorXi dynVec {3};  // 在堆上分配 `new int[3*1]` 的空间，尚未初始化
}

/// \brief 初始化构造。
TEST(Matrix, InitializedConstructors)
{
    Eigen::Matrix3i fixMat {  // 在栈上分配 `int[3*3]` 的空间，并进行初始化
        {11, 12, 13},
        {21, 22, 23},
        {31, 32, 33}
    };

    Eigen::MatrixXi dynMat {  // 在堆上分配 `new int[3*3]` 的空间，并进行初始化
        {11, 12, 13},
        {21, 22, 23},
        {31, 32, 33}
    };

    Eigen::Vector3i fixRawVec {{11, 12, 13}};  // 在栈上分配 `int[1*3]` 的空间，并进行初始化
    Eigen::Vector3i fixRawVec2 {11, 12, 13};   // 在不引起歧义的情况下，固定大小向量的内层大括号可以省略

    Eigen::VectorXi dynRawVec {{11, 12, 13}};  // 在堆上分配 `new int[1*3]` 的空间，并进行初始化

    Eigen::Vector3i fixColVec {{11}, {21}, {31}};  // 在栈上分配 `int[3*1]` 的空间，并进行初始化
    Eigen::Vector3i fixColVec2 {{11, 21, 31}};     // 列矩阵的初始化构造可以隐式转置
    Eigen::Vector3i fixColVec3 {11, 21, 31};       // 在不引起歧义的情况下，固定大小向量的内层大括号可以省略

    Eigen::VectorXi dynColVec {{11}, {21}, {31}};  // 在堆上分配 `new int[3*1]` 的空间，并进行初始化
    Eigen::VectorXi dynColVec2 {{11, 21, 31}};     // 列矩阵的初始化构造可以隐式转置
}

/// \brief 赋值取值。
TEST(Matrix, SetGet)
{
    Eigen::Matrix3i mat;
    mat << 1, 2, 3,
           4, 5, 6,
           7, 8, 9;
    EXPECT_EQ(mat(0), 1);
    EXPECT_EQ(mat(0, 0), 1);

    Eigen::Vector3i vec;
    vec << 1, 2, 3;
    EXPECT_EQ(vec(0), 1);
    EXPECT_EQ(vec[0], 1);
}


/*! \file geometry.hpp
  \brief Geometrical calculations
  \author Almog Yalinewich
 */

#ifndef GEOMETRY_HPP
#define GEOMETRY_HPP 1
#include <vector>
#include <boost/array.hpp>
#ifdef RICH_MPI
#include "../misc/serializable.hpp"
#endif // RICH_MPI

//! \brief 2D Mathematical vector
class Vector2D
#ifdef RICH_MPI
  : public Serializable
#endif // RICH_MPI
{
public:

  /*! \brief Null constructor
    \details Sets all components to 0
   */
  Vector2D(void);

  /*! \brief Class constructor
    \param ix x Component
    \param iy y Component
   */
  Vector2D(double ix, double iy);

  /*! \brief Class copy constructor
    \param v Other vector
   */
  Vector2D(const Vector2D& v);

  /*! \brief Set vector components
    \param ix x Component
    \param iy y Component
   */
  void Set(double ix, double iy);

  //! \brief Component in the x direction
  double x;

  //! \brief Component in the y direction
  double y;

  /*! \brief Addition
    \param v Vector to be added
    \return Reference to sum
   */
  Vector2D& operator+=(Vector2D const& v);

  /*! \brief Subtraction
    \param v Vector to be subtracted
    \return Difference
   */
  Vector2D& operator-=(Vector2D const& v);

  /*! \brief Assigment operator
    \param v Vector to be copied
    \return The assigned value
   */
  Vector2D& operator=(Vector2D const& v);

  /*! \brief Scalar product
    \param s Scalar
    \return Reference to the vector multiplied by scalar
   */
  Vector2D& operator*=(double s);

  /*! \brief Rotates the vector in an anticlockwise direction
    \param a Angle of rotation (in radians)
   */
  void Rotate(double a);
  //! \brief Caluclates the distance from the Vector to v1 \param v1 The vector whose distance from is calculated \returns The distance
  double distance(Vector2D const& v1) const;

#ifdef RICH_MPI
  /*! \brief Serializer
    \param ar Archiver
    \param int Version
   */
  template<class Archive>
  void serialize
  (Archive& ar,
   const unsigned int /*version*/)
  {
    ar & x;
    ar & y;
  }

  vector<double> serialize(void) const;

  size_t getChunkSize(void) const;

  void unserialize
  (const vector<double>& data);
#endif // RICH_MPI
};

/*! \brief Norm of a vector
  \param v Two dimensional vector
  \return Norm of v
 */
double abs(Vector2D const& v);

/*! \brief Term by term addition
  \param v1 First vector
  \param v2 Second vector
  \return Sum
 */
Vector2D operator+(Vector2D const& v1, Vector2D const& v2);

/*! \brief Term by term subtraction
  \param v1 First vector
  \param v2 Second vector
  \return Difference
 */
Vector2D operator-(Vector2D const& v1, Vector2D const& v2);

/*! \brief Scalar product
  \param d Scalar
  \param v Vector
  \return Two dimensional vector
 */
Vector2D operator*(double d, Vector2D const& v);

/*! \brief Scalar product
  \param v Vector
  \param d Scalar
  \return Two dimensional vector
 */
Vector2D operator*(Vector2D const& v, double d);

/*! \brief Scalar division
  \param v Vector
  \param d Scalar
  \return Two dimensional vector
 */
Vector2D operator/(Vector2D const& v, double d);

/*! \brief Scalar product of two vectors
  \param v1 2D vector
  \param v2 2D vector
  \return Scalar product of v1 and v2
 */
double ScalarProd(Vector2D const& v1, Vector2D const& v2);

/*! \brief Returns the angle between two vectors (in radians)
  \param v1 First vector
  \param v2 Second vector
  \return Angle
 */
double CalcAngle(Vector2D const& v1, Vector2D const& v2);

/*! \brief Calculates the projection of one vector in the direction of the second
  \param v1 First vector
  \param v2 Direction of the projection
  \return Component of v1 in the direction of v2
 */
double Projection(Vector2D const& v1, Vector2D const& v2);

/*! \brief Rotates a vector
  \param v Vector
  \param a Angle
  \return Rotated vector
 */
Vector2D Rotate(Vector2D const& v, double a);

/*! \brief Reflect vector
  \param v Vector
  \param axis Axis of reflection
  \return Reflection of v about axis
 */
Vector2D Reflect(Vector2D const& v, Vector2D const& axis);

/*! \brief Calculates the distance between two vectors
  \param v1 First vector
  \param v2 Second vector
  \return distance between v1 and v2
 */
double distance(Vector2D const& v1, Vector2D const& v2);

/*! \brief Returns the z component of the cross product of two vectors
  \param v1 First vector
  \param v2 Second vector
  \return z component of the cross product between v1 and v2
 */
double CrossProduct(Vector2D const& v1, Vector2D const& v2);

/*! \brief Calculates the mid point between two vectors
  \param v1 First vector
  \param v2 Second vector
  \return Distance between v1 and v2
 */
Vector2D calc_mid_point(Vector2D const& v1, Vector2D const& v2);

/*! \brief Cross product of a vector in x,y plane with a unit vector in the z direction
  \param v Vector in the x,y plane
  \return Two dimensional vector
 */
Vector2D zcross(Vector2D const& v);

/*! \brief Converts from polar coordinates to cartesian coordinates
  \param radius Radius
  \param angle Angle relative to the x axis
  \return Same vector in cartesian coordiantes
 */
Vector2D pol2cart(double radius, double angle);

/*! \brief Normalized a vector
  \param v Original vector
  \return Vector divided by its norm
 */
Vector2D normalize(const Vector2D& v);

/*! \brief Calculates the square of the distance. This is computationaly cheaper then actually calculating the distance
  \param v Vector
  \return Square of the distance
 */
double dist_sqr(const Vector2D& v);

#endif // GEOMETRY_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_COTD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COTD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing cotd capabilities

    cotangent of input in degree: \f$\cos(\pi x/180)/\sin(\pi x/180) \f$.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = cotd(x);
    @endcode

    As most other trigonometric function cotd can be called with a second optional parameter
    which is a tag on speed and accuracy (see @ref cos for further details)

    @see cos, sin, tan, cot, cotpi

  **/
  Value cotd(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/cotd.hpp>
#include <boost/simd/function/simd/cotd.hpp>

#endif


#include "opttmp/vectorization/register_tiling.hpp"
#include "parameters.hpp"
#include <Vc/Vc>
#include <boost/align/aligned_allocator.hpp>
#include <iostream>
#include <omp.h>
#include <vector>

using reg_arr =
    opttmp::vectorization::register_array<Vc::double_v, REG_BLOCKING>;
using align = boost::alignment::aligned_allocator<double, 64>;

AUTOTUNE_EXPORT void scale(std::vector<double, align> &a,
                           std::vector<double, align> &b, double q) {

  omp_set_num_threads(KERNEL_OMP_THREADS);
  // std::cout << "KERNEL_OMP_THREADS: " << KERNEL_OMP_THREADS << std::endl;

  // not kept in registers for unknown reasons
  const Vc::double_v q_vec = q;
  // std::cout << "q_vec: " << q_vec << std::endl;
  const size_t N = a.size();
#pragma omp parallel for
  for (size_t i = 0; i < N; i += REG_BLOCKING * Vc::double_v::size()) {
    // vector_aligned leads to crash?
    reg_arr temp(&b[i], Vc::flags::element_aligned);
    temp = q_vec * temp;
    temp.memstore(&a[i], Vc::flags::element_aligned);
    // a[i] = q * b[i];
  }
}


/** \file Lagrange.h */

#pragma once

#include <list>

#include <boost/numeric/mtl/mtl.hpp>

#include "BasisFunction.hpp"
#include "FixVec.hpp"

namespace AMDiS
{

#define MAX_DIM 3
#define MAX_DEGREE 4

  /** \ingroup FEMSpace
   * \brief
   * Lagrange basis functions. Sub class of BasisFunction
   */
  class Lagrange : public BasisFunction
  {
  public:
    /// Creator class used in the BasisFunctionCreatorMap.
    class Creator : public BasisFunctionCreator
    {
    public:
      Creator(int degree_) : degree(degree_) {}

      /// Returns a new Lagrange object.
      BasisFunction* create()
      {
        return getLagrange(this->dim, degree);
      }

    protected:
      int degree;
    };

  protected:
    /// Constructs lagrange basis functions with the given dim and degree.
    /// Constructor is protected to avoid multiple instantiation of identical
    /// basis functions. Use \ref getLagrange instead.
    Lagrange(int dim_, int degree_);

    /** \brief
     * destructor
     */
    virtual ~Lagrange();

  public:
    /// Returns a pointer to lagrange basis functions with the given dim and
    /// degree. Multiple instantiation of identical basis functions is avoided
    /// by rembering once created basis functions in \ref allBasFcts.
    static Lagrange* getLagrange(int dim, int degree);

    /// Implements BasisFunction::interpol
    void interpol(ElInfo const*, int, int const*,
                  std::function<double(WorldVector<double>)>,
                  DenseVector<double>&) const;

    /// Implements BasisFunction::interpol
    void interpol(ElInfo const*, int,
                  int const* b_no,
                  std::function<WorldVector<double>(WorldVector<double>)>,
                  DenseVector<WorldVector<double>>&) const;

    /// Returns the barycentric coordinates of the i-th basis function.
    DimVec<double>* getCoords(int i) const;

    /// Implements BasisFunction::getBound
    void getBound(ElInfo const*, BoundaryType*) const;

    /** \brief
     * Calculates the local vertex indices which are involved in evaluating
     * the nodeIndex-th DOF at the positionIndex-th part of type position
     * (VERTEX/EDGE/FACE/CENTER). nodeIndex determines the permutation
     * of the involved vertices. So in 1d for lagrange4 there are two DOFs at
     * the CENTER (which is an edge in this case). Then vertices[0] = {0, 1} and
     * vertices[1] = {1, 0}. This allows to use the same local basis function
     * for all DOFs at the same position.
     */
    static void setVertices(int dim, int degree,
                            GeoIndex position, int positionIndex, int nodeIndex,
                            int** vertices);

    /// Implements BasisFunction::refineInter
    void refineInter(DOFIndexed<double>* drv, RCNeighbourList* list, int n)
    {
      if (refineInter_fct)
        (*refineInter_fct)(drv, list, n, this);
    }

    /// Implements BasisFunction::coarseRestrict
    void coarseRestr(DOFIndexed<double>* drv, RCNeighbourList* list, int n)
    {
      if (coarseRestr_fct)
        (*coarseRestr_fct)(drv, list, n, this);
    }

    /// Implements BasisFunction::coarseInter
    void coarseInter(DOFIndexed<double>* drv, RCNeighbourList* list, int n)
    {
      if (coarseInter_fct)
        (*coarseInter_fct)(drv, list, n, this);
    }

    /// Implements BasisFunction::getLocalIndices().
    void getLocalIndices(Element const* el,
                         DOFAdmin const* admin,
                         std::vector<DegreeOfFreedom>& dofs) const;

    void getLocalDofPtrVec(Element const* el,
                           DOFAdmin const* admin,
                           std::vector<const DegreeOfFreedom*>& vec) const;

    /// Implements BasisFunction::l2ScpFctBas
    void l2ScpFctBas(Quadrature* q,
                     std::function<double(WorldVector<double>)> f,
                     DOFVector<double>* fh);

    /// Implements BasisFunction::l2ScpFctBas
    void l2ScpFctBas(Quadrature* q,
                     std::function<WorldVector<double>(WorldVector<double>)> f,
                     DOFVector<WorldVector<double>>* fh);

    static void clear();

    /// Implements BasisFunction::isnodal
    bool isNodal() const
    {
      return true;
    }

  protected:
    /// sets the barycentric coordinates (stored in \ref bary) of the local
    /// basis functions.
    void setBary();

    /// Recursive calculation of coordinates. Used by \ref setBary
    void createCoords(int* coordInd, int numCoords, int dimIndex, int rest,
                      DimVec<double>* vec = NULL);

    /// Used by \ref setBary
    int** getIndexPermutations(int numIndices) const;

    /// Implements BasisFunction::setNDOF
    void setNDOF();

    /// Sets used function pointers
    void setFunctionPointer();

    /// Used by \ref getVec
    int* orderOfPositionIndices(Element const* el,
                                GeoIndex position,
                                int positionIndex) const;

    /// Calculates the number of DOFs needed for Lagrange of the given dim
    /// and degree.
    static int getNumberOfDofs(int dim, int degree);

  private:
    /// barycentric coordinates of the locations of all basis functions
    std::vector<DimVec<double>*>* bary;

    /** \name static dim-degree-arrays
     * \{
     */
    static std::vector<DimVec<double>*> baryDimDegree[MAX_DIM + 1][MAX_DEGREE + 1];
    static DimVec<int>* ndofDimDegree[MAX_DIM + 1][MAX_DEGREE + 1];
    static int nBasFctsDimDegree[MAX_DIM + 1][MAX_DEGREE + 1];
    static std::vector<BasFctType*> phiDimDegree[MAX_DIM + 1][MAX_DEGREE + 1];
    static std::vector<GrdBasFctType*> grdPhiDimDegree[MAX_DIM + 1][MAX_DEGREE + 1];
    static std::vector<D2BasFctType*> D2PhiDimDegree[MAX_DIM + 1][MAX_DEGREE + 1];
    /** \} */

    /// List of all used BasisFunctions in the whole program. Avoids duplicate
    /// instantiation of identical BasisFunctions.
    static std::list<Lagrange*> allBasFcts;

  protected:
    /// Pointer to the used refineInter function
    void (*refineInter_fct)(DOFIndexed<double>*, RCNeighbourList*, int, BasisFunction*);

    /** \name refineInter functions
     * \{
     */
    static void  refineInter0(DOFIndexed<double>*, RCNeighbourList*, int,
                              BasisFunction*);
    static void  refineInter1(DOFIndexed<double>*, RCNeighbourList*, int,
                              BasisFunction*);
    static void  refineInter2_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  refineInter2_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  refineInter2_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  refineInter3_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  refineInter3_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  refineInter3_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  refineInter4_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  refineInter4_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  refineInter4_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    /** \} */

    /// Pointer to the used coarseRestr function
    void (*coarseRestr_fct)(DOFIndexed<double>*, RCNeighbourList*, int, BasisFunction*);

    /** \name coarseRestr functions
     * \{
     */
    static void  coarseRestr0(DOFIndexed<double>*, RCNeighbourList*, int,
                              BasisFunction*);
    static void  coarseRestr1(DOFIndexed<double>*, RCNeighbourList*, int,
                              BasisFunction*);
    static void  coarseRestr2_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseRestr2_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseRestr2_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseRestr3_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseRestr3_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseRestr3_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseRestr4_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseRestr4_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseRestr4_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    /** \} */

    /// Pointer to the used coarseInter function
    void (*coarseInter_fct)(DOFIndexed<double>*, RCNeighbourList*, int, BasisFunction*);

    /** \name coarseInter functions
     * \{
     */
    static void  coarseInter0(DOFIndexed<double>*, RCNeighbourList*, int,
                              BasisFunction*);
    static void  coarseInter1(DOFIndexed<double>*, RCNeighbourList*, int,
                              BasisFunction*);
    static void  coarseInter2_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseInter2_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseInter2_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseInter3_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseInter3_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseInter3_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseInter4_1d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseInter4_2d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    static void  coarseInter4_3d(DOFIndexed<double>*, RCNeighbourList*, int,
                                 BasisFunction*);
    /** \} */

    /// AbstractFunction which implements lagrange basis functions
    class Phi : public BasFctType
    {
    public:
      /// Constructs the local lagrange basis function for the given position,
      /// positionIndex and nodeIndex. owner_ is a pointer to the Lagrange
      /// object this basis function belongs to.
      Phi(Lagrange* owner, GeoIndex position, int positionIndex, int nodeIndex);

      /// Destructor
      virtual ~Phi();

    private:
      /// vertices needed for evaluation of this function
      int* vertices;

      /// Pointer to the evaluating function
      double (*func)(DimVec<double> const& lambda, int* vert);

      /// Returns \ref func(lambda, vertices)
      double operator()(DimVec<double> const& lambda) const
      {
        return func(lambda, vertices);
      }

      /** \name basis functions for different degrees
       * \{
       */

      // ====== Lagrange, degree = 0 =====================================
      // center
      static double phi0c(DimVec<double> const&, int*)
      {
        return 1.0;
      }

      // ====== Lagrange, degree = 1 =====================================
      // vertex
      static double phi1v(DimVec<double> const& lambda, int* vertices)
      {
        return lambda[vertices[0]];
      }

      // ====== Lagrange, degree = 2 =====================================
      // vertex
      static double phi2v(DimVec<double> const& lambda, int* vertices)
      {
        return lambda[vertices[0]] * (2.0 * lambda[vertices[0]] - 1.0);
      }

      // edge
      static double phi2e(DimVec<double> const& lambda, int* vertices)
      {
        return (4.0 * lambda[vertices[0]] * lambda[vertices[1]]);
      }

      // ====== Lagrange, degree = 3 =====================================
      // vertex
      static double phi3v(DimVec<double> const& lambda, int* vertices)
      {
        return (4.5 * (lambda[vertices[0]] - 1.0) * lambda[vertices[0]] + 1.0) *
               lambda[vertices[0]];
      }

      // edge
      static double phi3e(DimVec<double> const& lambda, int* vertices)
      {
        return (13.5 * lambda[vertices[0]] - 4.5) *
               lambda[vertices[0]] * lambda[vertices[1]];
      }

      // face
      static double phi3f(DimVec<double> const& lambda, int* vertices)
      {
        return 27.0 * lambda[vertices[0]] * lambda[vertices[1]] *
               lambda[vertices[2]];
      }

      // ====== Lagrange, degree = 4 ======================================
      // vertex
      static double phi4v(DimVec<double> const& lambda, int* vertices)
      {
        return (((32.0 * lambda[vertices[0]] - 48.0) * lambda[vertices[0]] + 22.0)
                * lambda[vertices[0]] - 3.0) * lambda[vertices[0]] / 3.0;
      }

      // edge
      static double phi4e0(DimVec<double> const& lambda, int* vertices)
      {
        return ((128.0 * lambda[vertices[0]] - 96.0) * lambda[vertices[0]] + 16.0)
               * lambda[vertices[0]] * lambda[vertices[1]] / 3.0;
      }

      static double phi4e1(DimVec<double> const& lambda, int* vertices)
      {
        return (4.0 * lambda[vertices[0]] - 1.0) * lambda[vertices[0]] *
               (4.0 * lambda[vertices[1]] - 1.0) * lambda[vertices[1]] * 4.0;
      }

      // face
      static double phi4f(DimVec<double> const& lambda,  int* vertices)
      {
        return (4.0 * lambda[vertices[0]] - 1.0) * lambda[vertices[0]] *
               lambda[vertices[1]] * lambda[vertices[2]] * 32.0;
      }

      // center
      static double phi4c(DimVec<double> const& lambda, int* vertices)
      {
        return 256.0 * lambda[vertices[0]] * lambda[vertices[1]] *
               lambda[vertices[2]] * lambda[vertices[3]];
      }

    };

    /** \} */

    /// AbstractFunction which implements gradients of lagrange basis functions.
    /// See \ref Phi
    class GrdPhi : public GrdBasFctType
    {
    public:
      GrdPhi(Lagrange* owner, GeoIndex position, int positionIndex, int nodeIndex);

      virtual ~GrdPhi();
    private:
      int* vertices;

      void (*func)(DimVec<double> const& lambda,
                   int* vertices_,
                   DenseVector<double>& result);

      void operator()(DimVec<double> const& lambda,
                      DenseVector<double>& result) const
      {
        func(lambda, vertices, result);
      }

      // ====== Lagrange0 ================================================
      // center
      static void grdPhi0c(DimVec<double> const&,
                           int*,
                           DenseVector<double>& result)
      {
        result = 0.0;
      }

      // ====== Lagrange1 ================================================
      // vertex
      static void grdPhi1v(DimVec<double> const&,
                           int* vertices,
                           DenseVector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = 1.0;
      }

      // ====== Lagrange2 ================================================
      // vertex
      static void grdPhi2v(DimVec<double> const& lambda,
                           int* vertices,
                           DenseVector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = 4.0 * lambda[vertices[0]] - 1.0;
      }

      // edge
      static void grdPhi2e(DimVec<double> const& lambda,
                           int* vertices,
                           DenseVector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = 4.0 * lambda[vertices[1]];
        result[vertices[1]] = 4.0 * lambda[vertices[0]];
      }

      // ===== Lagrange3 ================================================
      // vertex
      static void grdPhi3v(DimVec<double> const& lambda,
                           int* vertices,
                           DenseVector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = (13.5 * lambda[vertices[0]] - 9.0) *
                              lambda[vertices[0]] + 1.0;
      }

      // edge
      static void grdPhi3e(DimVec<double> const& lambda,
                           int* vertices,
                           DenseVector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = (27.0 * lambda[vertices[0]] - 4.5) *
                              lambda[vertices[1]];
        result[vertices[1]] = (13.5 * lambda[vertices[0]] - 4.5) *
                              lambda[vertices[0]];
      }

      // face
      static void grdPhi3f(DimVec<double> const& lambda,
                           int* vertices,
                           DenseVector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = 27.0 * lambda[vertices[1]] * lambda[vertices[2]];
        result[vertices[1]] = 27.0 * lambda[vertices[0]] * lambda[vertices[2]];
        result[vertices[2]] = 27.0 * lambda[vertices[0]] * lambda[vertices[1]];
      }

      // ===== Lagrange4 ================================================
      // vertex
      static void grdPhi4v(DimVec<double> const& lambda,
                           int* vertices,
                           DenseVector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] =
          ((128.0 * lambda[vertices[0]] - 144.0) * lambda[vertices[0]] + 44.0) *
          lambda[vertices[0]] / 3.0 - 1.0;
      }

      // edge
      static void grdPhi4e0(DimVec<double> const& lambda,
                            int* vertices,
                            DenseVector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = ((128.0 * lambda[vertices[0]] - 64.0) *
                               lambda[vertices[0]] + 16.0 / 3.0) * lambda[vertices[1]];
        result[vertices[1]] = ((128.0 * lambda[vertices[0]] - 96.0) *
                               lambda[vertices[0]] + 16.0)*lambda[vertices[0]] / 3.0;
      }

      static void grdPhi4e1(DimVec<double> const& lambda,
                            int* vertices,
                            DenseVector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = 4.0 * (8.0 * lambda[vertices[0]] - 1.0) *
                              lambda[vertices[1]] * (4.0 * lambda[vertices[1]] - 1.0);
        result[vertices[1]] = 4.0 * lambda[vertices[0]] *
                              (4.0 * lambda[vertices[0]] - 1.0) * (8.0 * lambda[vertices[1]] - 1.0);
      }

      // face
      static void grdPhi4f(DimVec<double> const& lambda,
                           int* vertices,
                           DenseVector<double>& result)
      {
        result = 0.0;
        result[vertices[0]] = 32.0 * (8.0 * lambda[vertices[0]] - 1.0) *
                              lambda[vertices[1]] * lambda[vertices[2]];
        result[vertices[1]] = 32.0 * (4.0 * lambda[vertices[0]] - 1.0) *
                              lambda[vertices[0]] * lambda[vertices[2]];
        result[vertices[2]] = 32.0 * (4.0 * lambda[vertices[0]] - 1.0) *
                              lambda[vertices[0]] * lambda[vertices[1]];
      }

      // center
      static void grdPhi4c(DimVec<double> const& lambda,
                           int* vertices,
                           DenseVector<double>& result)
      {
        result = 0.0;
        result[0] =
          256.0 * lambda[vertices[1]] * lambda[vertices[2]] * lambda[vertices[3]];
        result[1] =
          256.0 * lambda[vertices[0]] * lambda[vertices[2]] * lambda[vertices[3]];
        result[2] =
          256.0 * lambda[vertices[0]] * lambda[vertices[1]] * lambda[vertices[3]];
        result[3] =
          256.0 * lambda[vertices[0]] * lambda[vertices[1]] * lambda[vertices[2]];
      }
    };

    /// AbstractFunction which implements second derivatives of Lagrange basis
    /// functions. See \ref Phi
    class D2Phi : public D2BasFctType
    {
    public:
      D2Phi(Lagrange* owner, GeoIndex position, int positionIndex, int nodeIndex);

      virtual ~D2Phi();
    private:
      int* vertices;

      void (*func)(DimVec<double> const& lambda, int* vertices_, DimMat<double>& result);

      void operator()(DimVec<double> const& lambda, DimMat<double>& result) const
      {
        return func(lambda, vertices, result);
      }

      // ===== Lagrange0 ================================================
      // center
      static void D2Phi0c(DimVec<double> const&, int*, DimMat<double>& result)
      {
        result.set(0.0);
      }

      // ===== Lagrange1 ================================================
      // vertex
      static void D2Phi1v(DimVec<double> const&, int*, DimMat<double>& result)
      {
        result.set(0.0);
      }

      // ===== Lagrange2 ================================================
      // vertex
      static void D2Phi2v(DimVec<double> const&, int* vertices,
                          DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[0]] = 4.0;
      }

      // edge
      static void D2Phi2e(DimVec<double> const&, int* vertices,
                          DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[1]] = 4.0;
        result[vertices[1]][vertices[0]] = 4.0;
      }

      // ===== Lagrange3 ================================================
      // vertex
      static void D2Phi3v(DimVec<double> const& lambda, int* vertices,
                          DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[0]] = 27.0 * lambda[vertices[0]] - 9.0;
      }

      // edge
      static void D2Phi3e(DimVec<double> const& lambda, int* vertices,
                          DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[0]] = 27.0 * lambda[vertices[1]];
        result[vertices[0]][vertices[1]] =
          result[vertices[1]][vertices[0]] = 27.0 * lambda[vertices[0]] - 4.5;
      }

      // face
      static void D2Phi3f(DimVec<double> const& lambda, int* vertices,
                          DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[1]] =
          result[vertices[1]][vertices[0]] = 27.0 * lambda[vertices[2]];
        result[vertices[0]][vertices[2]] =
          result[vertices[2]][vertices[0]] = 27.0 * lambda[vertices[1]];
        result[vertices[1]][vertices[2]] =
          result[vertices[2]][vertices[1]] = 27.0 * lambda[vertices[0]];
      }

      // ===== Lagrange4 ================================================
      // vertex
      static void D2Phi4v(DimVec<double> const& lambda,
                          int* vertices,
                          DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[0]] =
          (128.0 * lambda[vertices[0]] - 96.0) * lambda[vertices[0]] + 44.0 / 3.0;
      }

      // edge
      static void D2Phi4e0(DimVec<double> const& lambda, int* vertices,
                           DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[0]] =
          (256.0 * lambda[vertices[0]] - 64.0) * lambda[vertices[1]];
        result[vertices[0]][vertices[1]] =
          result[vertices[1]][vertices[0]] =
            (128.0 * lambda[vertices[0]] - 64.0) * lambda[vertices[0]] + 16.0 / 3.0;
      }

      static void D2Phi4e1(DimVec<double> const& lambda, int* vertices,
                           DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[0]] =
          32.0 * lambda[vertices[1]] * (4.0 * lambda[vertices[1]] - 1.0);
        result[vertices[0]][vertices[1]] =
          result[vertices[1]][vertices[0]] =
            4.0 * (8.0 * lambda[vertices[0]] - 1.0) * (8.0 * lambda[vertices[1]] - 1.0);
        result[vertices[1]][vertices[1]] =
          32.0 * lambda[vertices[0]] * (4.0 * lambda[vertices[0]] - 1.0);
      }

      // face
      static void D2Phi4f(DimVec<double> const& lambda, int* vertices,
                          DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[0]] =
          256.0 * lambda[vertices[1]] * lambda[vertices[2]];
        result[vertices[0]][vertices[1]] =
          result[vertices[1]][vertices[0]] =
            32.0 * (8.0 * lambda[vertices[0]] - 1.0) * lambda[vertices[2]];
        result[vertices[0]][vertices[2]] =
          result[vertices[2]][vertices[0]] =
            32.0 * (8.0 * lambda[vertices[0]] - 1.0) * lambda[vertices[1]];
        result[vertices[1]][vertices[2]] =
          result[vertices[2]][vertices[1]] =
            32.0 * (4.0 * lambda[vertices[0]] - 1.0) * lambda[vertices[0]];
      }

      // center
      static void D2Phi4c(DimVec<double> const& lambda, int* vertices,
                          DimMat<double>& result)
      {
        result.set(0.0);
        result[vertices[0]][vertices[1]] =
          result[vertices[1]][vertices[0]] =
            256.0 * lambda[vertices[2]] * lambda[vertices[3]];
        result[vertices[0]][vertices[2]] =
          result[vertices[2]][vertices[0]] =
            256.0 * lambda[vertices[1]] * lambda[vertices[3]];
        result[vertices[0]][vertices[3]] =
          result[vertices[3]][vertices[0]] =
            256.0 * lambda[vertices[1]] * lambda[vertices[2]];
        result[vertices[1]][vertices[2]] =
          result[vertices[2]][vertices[1]] =
            256.0 * lambda[vertices[0]] * lambda[vertices[3]];
        result[vertices[1]][vertices[3]] =
          result[vertices[3]][vertices[1]] =
            256.0 * lambda[vertices[0]] * lambda[vertices[2]];
        result[vertices[2]][vertices[3]] =
          result[vertices[3]][vertices[2]] =
            256.0 * lambda[vertices[0]] * lambda[vertices[1]];
      }
    };
  };

} // end namespace AMDiS


// Copyright 2016 Arizona Board of Regents. See README.md and LICENSE for more.

#include <iostream>
#include <algorithm>
#include <iterator>
#include <ctime>

#include <boost/random.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/algorithm/string.hpp>

#include "nanocube.h"
#include "nanocube_traversals.h"
#include "debug.h"
#include "test_utils.h"

int atoi(const std::string &s) { return atoi(s.c_str()); }
double atof(const std::string &s) { return atof(s.c_str()); }

// convert lat,lon to quad tree address
int64_t loc2addr(double lat, double lon, int qtreeLevel)
{
    double xd = (lon + M_PI) / (2.0 * M_PI);
    double yd = (log(tan(M_PI / 4.0 + lat / 2.0)) + M_PI) / (2.0 * M_PI);
    //cout << lat << " " << lon << " " << endl;
    int x = xd * (1 << qtreeLevel), y = yd * (1 << qtreeLevel);

    int64_t z = 0; // z gets the resulting Morton Number.

    for (int i = 0; i < sizeof(x) * 8; i++) // unroll for more speed...
    {
        z |= (x & 1U << i) << i | (y & 1U << i) << (i + 1);
    }

    return z;
}

int main(int argc, char **argv)
{
    using namespace boost::gregorian;
    using namespace boost::posix_time;

    ifstream is(argv[1]);
    std::cout << "Data file: " << argv[1] << std::endl;
    string s;

    int qtreeLevel = 15;
    int dim3Level = 16;

    vector<int> schema = {qtreeLevel*2, dim3Level};
    // use a quadtree
    //Nanocube<int> nc({qtreeLevel, qtreeLevel, 16});
    Nanocube<int> nc(schema);

    vector<pair<int,int> > dataarray;

    int i = 0;

    clock_t begin = clock();

    while (std::getline(is, s)) {
        vector<string> output;
        boost::split(output,s,boost::is_any_of("\t"));
        if (output.size() != 5) {
            cerr << "Bad line:" << s << endl;
            continue;
        }
        if (atof(output[2]) < -85.0511 || atof(output[2]) > 85.0511) {
            cerr << "Invalid latitude: " << output[2] << " (should be in [-85.0511, 85.0511])" << endl;
            continue;
        }

        string year = output[1].substr(0, 4),
               month = output[1].substr(5, 2),
               day = output[1].substr(8, 2),
               hour = output[1].substr(11, 2),
               minute = output[1].substr(14, 2),
               sec = output[1].substr(17, 2);

        ptime d(date(atoi(year), atoi(month), atoi(day)), hours(atoi(hour)) + minutes(atoi(minute)) + seconds(atoi(sec)));

        ptime beg_of_time(date(2000, 1, 1));

        double lat = atof(output[2]) * M_PI / 180.0, lon = atof(output[3]) * M_PI / 180.0;
        // turn (x,y) into an address of the quadtree
        // Interleave bits
        int64_t z = loc2addr(lat, lon, qtreeLevel);

        int total_seconds = (d - beg_of_time).total_seconds();
        int t = total_seconds / 3600 / 24;

        nc.insert(1, {z, t});
        dataarray.push_back({z,t});

        if (++i % 10000 == 0) {
            {
                //ofstream os("brightkite.nc");
                //nc.write_to_binary_stream(os);
            }
            //nc.report_size();
            cout << i << endl;
        }
    }

    clock_t end = clock();
    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
    cout << "Running time: " << elapsed_secs << endl;

    /**************************************************/
    // Test
    /**************************************************/
    int n_regions = 10;
    for (int l=0; l<n_regions; ++l) {
        vector<pair<int64_t, int64_t> > region = random_region(schema);
        int rq = ortho_range_query(nc, region);
        int count = 0;
        for(int dlength = 0; dlength < dataarray.size(); dlength++) {
            if( (dataarray[dlength].first > region[0].first &&
                        dataarray[dlength].first < region[0].second) &&
                    (dataarray[dlength].second > region[1].first &&
                     dataarray[dlength].second < region[1].second) ) {
                count ++;
            }
        }
        cout << "Query from Nanocubea: "<< rq << " | Linear scan: " << count << endl;
    }

    /**************************************************/
    // Save
    /**************************************************/

    {
        //nc.content_compact();
        //ofstream os("brightkite.nc");
        //nc.write_to_binary_stream(os);
    }

    // std::string s("2001-10-9"); //2001-October-09
    // date d(from_simple_string(s));
}


#include <stan/math/prim/mat.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <vector>
#include <limits>

using Eigen::Dynamic;
using Eigen::Matrix;

TEST(ProbDistributionsCategorical, Categorical) {
  Matrix<double, Dynamic, 1> theta(3, 1);
  theta << 0.3, 0.5, 0.2;
  EXPECT_FLOAT_EQ(-1.203973, stan::math::categorical_log(1, theta));
  EXPECT_FLOAT_EQ(-0.6931472, stan::math::categorical_log(2, theta));
}
TEST(ProbDistributionsCategorical, Propto) {
  Matrix<double, Dynamic, 1> theta(3, 1);
  theta << 0.3, 0.5, 0.2;
  EXPECT_FLOAT_EQ(0.0, stan::math::categorical_log<true>(1, theta));
  EXPECT_FLOAT_EQ(0.0, stan::math::categorical_log<true>(2, theta));
}

TEST(ProbDistributionsCategorical, VectorInt) {
  Matrix<double, Dynamic, 1> theta(3, 1);
  theta << 0.3, 0.5, 0.2;
  std::vector<int> xs0;
  EXPECT_FLOAT_EQ(0.0, stan::math::categorical_log(xs0, theta));

  std::vector<int> xs(3);
  xs[0] = 1;
  xs[1] = 3;
  xs[2] = 1;

  EXPECT_FLOAT_EQ(log(0.3) + log(0.2) + log(0.3),
                  stan::math::categorical_log(xs, theta));
}

using stan::math::categorical_log;

TEST(ProbDistributionsCategorical, error) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  double inf = std::numeric_limits<double>::infinity();

  unsigned int n = 1;
  unsigned int N = 3;
  Matrix<double, Dynamic, 1> theta(N, 1);
  theta << 0.3, 0.5, 0.2;

  EXPECT_NO_THROW(categorical_log(N, theta));
  EXPECT_NO_THROW(categorical_log(n, theta));
  EXPECT_NO_THROW(categorical_log(2, theta));
  EXPECT_THROW(categorical_log(N + 1, theta), std::domain_error);
  EXPECT_THROW(categorical_log(0, theta), std::domain_error);

  theta(0) = nan;
  EXPECT_THROW(categorical_log(n, theta), std::domain_error);
  theta(0) = inf;
  EXPECT_THROW(categorical_log(n, theta), std::domain_error);
  theta(0) = -inf;
  EXPECT_THROW(categorical_log(n, theta), std::domain_error);
  theta(0) = -1;
  theta(1) = 1;
  theta(2) = 0;
  EXPECT_THROW(categorical_log(n, theta), std::domain_error);

  std::vector<int> ns(3);
  ns[0] = 3;
  ns[1] = 2;
  ns[2] = 3;
  EXPECT_THROW(categorical_log(ns, theta), std::domain_error);

  theta << 0.3, 0.5, 0.2;
  EXPECT_NO_THROW(categorical_log(ns, theta));
  ns[1] = -1;
  EXPECT_THROW(categorical_log(ns, theta), std::domain_error);

  ns[1] = 1;
  ns[2] = 12;
  EXPECT_THROW(categorical_log(ns, theta), std::domain_error);
}

TEST(ProbDistributionsCategorical, error_check) {
  boost::random::mt19937 rng;

  Matrix<double, Dynamic, Dynamic> theta(3, 1);
  theta << 0.15, 0.45, 0.50;

  EXPECT_THROW(stan::math::categorical_rng(theta, rng), std::domain_error);
}

TEST(ProbDistributionsCategorical, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;

  int N = 10000;
  Matrix<double, Dynamic, Dynamic> theta(3, 1);
  theta << 0.15, 0.45, 0.40;
  int K = theta.rows();
  boost::math::chi_squared mydist(K - 1);

  Eigen::Matrix<double, Eigen::Dynamic, 1> loc(theta.rows(), 1);
  for (int i = 0; i < theta.rows(); i++)
    loc(i) = 0;

  for (int i = 0; i < theta.rows(); i++) {
    for (int j = i; j < theta.rows(); j++)
      loc(j) += theta(i);
  }

  int count = 0;
  int bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * theta(i);
  }

  while (count < N) {
    int a = stan::math::categorical_rng(theta, rng);
    bin[a - 1]++;
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}