#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 "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);
}


/*! \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


/** \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


//  Boost common_factor_rt.hpp header file  ----------------------------------//

//  (C) Copyright Daryle Walker and Paul Moore 2001-2002.  Permission to copy,
//  use, modify, sell and distribute this software is granted provided this
//  copyright notice appears in all copies.  This software is provided "as is"
//  without express or implied warranty, and with no claim as to its suitability
//  for any purpose.

// boostinspect:nolicense (don't complain about the lack of a Boost license)
// (Paul Moore hasn't been in contact for years, so there's no way to change the
// license.)

//  See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_MATH_COMMON_FACTOR_RT_HPP
#define BOOST_MATH_COMMON_FACTOR_RT_HPP

#include <boost/math_fwd.hpp>  // self include

#include <boost/config.hpp>  // for BOOST_NESTED_TEMPLATE, etc.
#include <boost/limits.hpp>  // for std::numeric_limits
#include <climits>           // for CHAR_MIN
#include <boost/detail/workaround.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127 4244)  // Conditional expression is constant
#endif

namespace boost
{
namespace math
{

//  Forward declarations for function templates  -----------------------------//

template < typename IntegerType >
    IntegerType  gcd( IntegerType const &a, IntegerType const &b );

template < typename IntegerType >
    IntegerType  lcm( IntegerType const &a, IntegerType const &b );

//  Greatest common divisor evaluator class declaration  ---------------------//

template < typename IntegerType >
class gcd_evaluator
{
public:
    // Types
    typedef IntegerType  result_type, first_argument_type, second_argument_type;

    // Function object interface
    result_type  operator ()( first_argument_type const &a,
     second_argument_type const &b ) const;

};  // boost::math::gcd_evaluator

//  Least common multiple evaluator class declaration  -----------------------//

template < typename IntegerType >
class lcm_evaluator
{
public:
    // Types
    typedef IntegerType  result_type, first_argument_type, second_argument_type;

    // Function object interface
    result_type  operator ()( first_argument_type const &a,
     second_argument_type const &b ) const;

};  // boost::math::lcm_evaluator

//  Implementation details  --------------------------------------------------//

namespace detail
{
    // Greatest common divisor for rings (including unsigned integers)
    template < typename RingType >
    RingType
    gcd_euclidean
    (
        RingType a,
        RingType b
    )
    {
        // Avoid repeated construction
        #ifndef __BORLANDC__
        RingType const  zero = static_cast<RingType>( 0 );
        #else
        RingType  zero = static_cast<RingType>( 0 );
        #endif

        // Reduce by GCD-remainder property [GCD(a,b) == GCD(b,a MOD b)]
        while ( true )
        {
            if ( a == zero )
                return b;
            b %= a;

            if ( b == zero )
                return a;
            a %= b;
        }
    }

    // Greatest common divisor for (signed) integers
    template < typename IntegerType >
    inline
    IntegerType
    gcd_integer
    (
        IntegerType const &  a,
        IntegerType const &  b
    )
    {
        // Avoid repeated construction
        IntegerType const  zero = static_cast<IntegerType>( 0 );
        IntegerType const  result = gcd_euclidean( a, b );

        return ( result < zero ) ? static_cast<IntegerType>(-result) : result;
    }

    // Greatest common divisor for unsigned binary integers
    template < typename BuiltInUnsigned >
    BuiltInUnsigned
    gcd_binary
    (
        BuiltInUnsigned  u,
        BuiltInUnsigned  v
    )
    {
        if ( u && v )
        {
            // Shift out common factors of 2
            unsigned  shifts = 0;

            while ( !(u & 1u) && !(v & 1u) )
            {
                ++shifts;
                u >>= 1;
                v >>= 1;
            }

            // Start with the still-even one, if any
            BuiltInUnsigned  r[] = { u, v };
            unsigned         which = static_cast<bool>( u & 1u );

            // Whittle down the values via their differences
            do
            {
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
                while ( !(r[ which ] & 1u) )
                {
                    r[ which ] = (r[which] >> 1);
                }
#else
                // Remove factors of two from the even one
                while ( !(r[ which ] & 1u) )
                {
                    r[ which ] >>= 1;
                }
#endif

                // Replace the larger of the two with their difference
                if ( r[!which] > r[which] )
                {
                    which ^= 1u;
                }

                r[ which ] -= r[ !which ];
            }
            while ( r[which] );

            // Shift-in the common factor of 2 to the residues' GCD
            return r[ !which ] << shifts;
        }
        else
        {
            // At least one input is zero, return the other
            // (adding since zero is the additive identity)
            // or zero if both are zero.
            return u + v;
        }
    }

    // Least common multiple for rings (including unsigned integers)
    template < typename RingType >
    inline
    RingType
    lcm_euclidean
    (
        RingType const &  a,
        RingType const &  b
    )
    {
        RingType const  zero = static_cast<RingType>( 0 );
        RingType const  temp = gcd_euclidean( a, b );

        return ( temp != zero ) ? ( a / temp * b ) : zero;
    }

    // Least common multiple for (signed) integers
    template < typename IntegerType >
    inline
    IntegerType
    lcm_integer
    (
        IntegerType const &  a,
        IntegerType const &  b
    )
    {
        // Avoid repeated construction
        IntegerType const  zero = static_cast<IntegerType>( 0 );
        IntegerType const  result = lcm_euclidean( a, b );

        return ( result < zero ) ? static_cast<IntegerType>(-result) : result;
    }

    // Function objects to find the best way of computing GCD or LCM
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T, bool IsSpecialized, bool IsSigned >
    struct gcd_optimal_evaluator_helper_t
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_euclidean( a, b );
        }
    };

    template < typename T >
    struct gcd_optimal_evaluator_helper_t< T, true, true >
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_integer( a, b );
        }
    };

    template < typename T >
    struct gcd_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            typedef ::std::numeric_limits<T>  limits_type;

            typedef gcd_optimal_evaluator_helper_t<T,
             limits_type::is_specialized, limits_type::is_signed>  helper_type;

            helper_type  solver;

            return solver( a, b );
        }
    };
#else // BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T >
    struct gcd_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_integer( a, b );
        }
    };
#endif

    // Specialize for the built-in integers
#define BOOST_PRIVATE_GCD_UF( Ut )                  \
    template < >  struct gcd_optimal_evaluator<Ut>  \
    {  Ut  operator ()( Ut a, Ut b ) const  { return gcd_binary( a, b ); }  }

    BOOST_PRIVATE_GCD_UF( unsigned char );
    BOOST_PRIVATE_GCD_UF( unsigned short );
    BOOST_PRIVATE_GCD_UF( unsigned );
    BOOST_PRIVATE_GCD_UF( unsigned long );

#ifdef BOOST_HAS_LONG_LONG
    BOOST_PRIVATE_GCD_UF( boost::ulong_long_type );
#elif defined(BOOST_HAS_MS_INT64)
    BOOST_PRIVATE_GCD_UF( unsigned __int64 );
#endif

#if CHAR_MIN == 0
    BOOST_PRIVATE_GCD_UF( char ); // char is unsigned
#endif

#undef BOOST_PRIVATE_GCD_UF

#define BOOST_PRIVATE_GCD_SF( St, Ut )                            \
    template < >  struct gcd_optimal_evaluator<St>                \
    {  St  operator ()( St a, St b ) const  { Ut const  a_abs =   \
    static_cast<Ut>( a < 0 ? -a : +a ), b_abs = static_cast<Ut>(  \
    b < 0 ? -b : +b ); return static_cast<St>(                    \
    gcd_optimal_evaluator<Ut>()(a_abs, b_abs) ); }  }

    BOOST_PRIVATE_GCD_SF( signed char, unsigned char );
    BOOST_PRIVATE_GCD_SF( short, unsigned short );
    BOOST_PRIVATE_GCD_SF( int, unsigned );
    BOOST_PRIVATE_GCD_SF( long, unsigned long );

#if CHAR_MIN < 0
    BOOST_PRIVATE_GCD_SF( char, unsigned char ); // char is signed
#endif

#ifdef BOOST_HAS_LONG_LONG
    BOOST_PRIVATE_GCD_SF( boost::long_long_type, boost::ulong_long_type );
#elif defined(BOOST_HAS_MS_INT64)
    BOOST_PRIVATE_GCD_SF( __int64, unsigned __int64 );
#endif

#undef BOOST_PRIVATE_GCD_SF

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T, bool IsSpecialized, bool IsSigned >
    struct lcm_optimal_evaluator_helper_t
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_euclidean( a, b );
        }
    };

    template < typename T >
    struct lcm_optimal_evaluator_helper_t< T, true, true >
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_integer( a, b );
        }
    };

    template < typename T >
    struct lcm_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            typedef ::std::numeric_limits<T>  limits_type;

            typedef lcm_optimal_evaluator_helper_t<T,
             limits_type::is_specialized, limits_type::is_signed>  helper_type;

            helper_type  solver;

            return solver( a, b );
        }
    };
#else // BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T >
    struct lcm_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_integer( a, b );
        }
    };
#endif

    // Functions to find the GCD or LCM in the best way
    template < typename T >
    inline
    T
    gcd_optimal
    (
        T const &  a,
        T const &  b
    )
    {
        gcd_optimal_evaluator<T>  solver;

        return solver( a, b );
    }

    template < typename T >
    inline
    T
    lcm_optimal
    (
        T const &  a,
        T const &  b
    )
    {
        lcm_optimal_evaluator<T>  solver;

        return solver( a, b );
    }

}  // namespace detail

//  Greatest common divisor evaluator member function definition  ------------//

template < typename IntegerType >
inline
typename gcd_evaluator<IntegerType>::result_type
gcd_evaluator<IntegerType>::operator ()
(
    first_argument_type const &   a,
    second_argument_type const &  b
) const
{
    return detail::gcd_optimal( a, b );
}

//  Least common multiple evaluator member function definition  --------------//

template < typename IntegerType >
inline
typename lcm_evaluator<IntegerType>::result_type
lcm_evaluator<IntegerType>::operator ()
(
    first_argument_type const &   a,
    second_argument_type const &  b
) const
{
    return detail::lcm_optimal( a, b );
}

//  Greatest common divisor and least common multiple function definitions  --//

template < typename IntegerType >
inline
IntegerType
gcd
(
    IntegerType const &  a,
    IntegerType const &  b
)
{
    gcd_evaluator<IntegerType>  solver;

    return solver( a, b );
}

template < typename IntegerType >
inline
IntegerType
lcm
(
    IntegerType const &  a,
    IntegerType const &  b
)
{
    lcm_evaluator<IntegerType>  solver;

    return solver( a, b );
}

}  // namespace math
}  // namespace boost

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif  // BOOST_MATH_COMMON_FACTOR_RT_HPP


#include <iostream>
#include <boost/format.hpp>
#include <cmath>

bool bkt_is_ok(float bkt) {
  return (bkt != 60.0)
    && (bkt != -60.0)
    && (bkt != 120.0)
    && (bkt != -120.0)
    && (bkt != 50.0)
    && (bkt != -50.0)
    && (bkt != 100.0)
    && (bkt != -100.0);
}

int main(int argc, char * argv[])
{
  float bkt;
  float prop;
  int count = 0;
  float sum = 0.0;
  float sumsq = 0.0;
  bool flag = true;
  while(flag) {
    std::cin >> bkt >> prop;
    flag = !std::cin.eof();
    if(flag) {
      if(bkt_is_ok(bkt)) {
	count++;
	sum += bkt * prop;
	sumsq += (bkt * bkt * prop);
      }
    }
  }

  float fcount = float(count);
  // variance is E[X^2] - (E[X])^2

  float var = sumsq - sum*sum;
  float sdev = sqrt(var);
  std::cout << boost::format("mean = %g  var = %g  sdev = %g\n")
    % sum % var % sdev;
}


/**
 * @file kf_node.cpp
 * @brief !Valgrind output
 *  Memcheck, a memory error detector
 *  Copyright (C) 2002-2017, and GNU GPL'd, by Julian Seward et al.
 *  Using Valgrind-3.15.0 and LibVEX; rerun with -h for copyright info
 *  Command: ./src/signal_processing/signal_processing_kf_example
 *
 *  HEAP SUMMARY:
 *      in use at exit: 0 bytes in 0 blocks
 *    total heap usage: 2,127 allocs, 2,127 frees, 128,736 bytes allocated
 *
 *  All heap blocks were freed -- no leaks are possible
 *
 *  For lists of detected and suppressed errors, rerun with: -s
 *  ERROR SUMMARY: 0 errors from 0 contexts (suppressed: 0 from 0)
 */

#include <kalman_filter.hpp>
#include <Eigen/Dense>
#include <memory>
#include <iostream>
#include <vector>
#include <ctime>

using namespace filter;

int main(int argc, char* argv[])
{
    int n = 3; // Number of states [position, velocity, acceleration].
    int m = 1; // Number of measurements.
    double dt = 1.0/30; // Timestamp.

    // Declare MAT for Kalman filter computation.
    Eigen::MatrixXd A(n, n); // System dynamics matrix
    Eigen::MatrixXd C(m, n); // Output matrix
    Eigen::MatrixXd Q(n, n); // Process noise covariance
    Eigen::MatrixXd R(m, m); // Measurement noise covariance
    Eigen::MatrixXd P(n, n); // Estimate error covariance

    // Measure the position only.
    A <<
        1, dt, 0,
        0, 1, dt,
        0, 0, 1;

    C << 1, 0, 0;

    // Reasonable covariance matrices
    Q <<
        .05, .05, .0,
        .05, .05, .0,
        .0, .0, .0;

    R << 1;

    P <<
        .1, .1, .1,
        .1, 10000, 10,
        .1, 10, 100;

    // Generate random values from 0 - 1.
    std::srand(std::time(0));
    std::vector<double> measurements;

    for(unsigned int i = 0; i < 100 ; i++)
    {
        double noise = ((double)std::rand() / (double)RAND_MAX);
        measurements.push_back(noise);
    }

    // Initialize Kalman filter.
    auto KF = std::unique_ptr<KalmanFilter>(new KalmanFilter(A, C, Q, R, P));

    // Start the Kalman filter with zero initial parameters.
    KF->init();

    // Feed the measurements to the filter and get estimated states of a system.
    Eigen::VectorXd y(m);
    for(unsigned int i = 0; i < measurements.size(); i++)
    {
        y << measurements[i];
        std::cout << "KF Result 結果: " << KF->compute(y, dt).transpose() << std::endl;

        /** !Output.
         * @brief KF Result 結果:  0.513434 -0.150251 -0.213232
         *
         * @brief [position, velocity, acceleration]
         */
    }

    return EXIT_SUCCESS;
}


#include <cmath>
#include <iostream>
#include <boost/multiprecision/float128.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/lambert_w.hpp>
#include <boost/core/demangle.hpp>
#include "quicksvg/ulp_plot.hpp"

using boost::math::lambert_w0;
using boost::math::lambert_wm1;
using boost::math::lambert_w0_prime;
using boost::math::lambert_wm1_prime;
using boost::multiprecision::float128;
using boost::math::constants::exp_minus_one;

int main()
{
    using PreciseReal = float128;
    using CoarseReal = float;
    CoarseReal divider = -0.3667;
    int samples = 15000;
    CoarseReal a = -exp_minus_one<CoarseReal>();
    CoarseReal b = divider;
    std::string title = "ULP accuracy of " + boost::core::demangle(typeid(CoarseReal).name()) + " precision Lambert W₀ on (-1/e, -0.3667)";
    std::cout << title << "\n";
    //title = "";
    std::string filename = "examples/ulp_lambert_w0_1e_3667.svg";
    auto flo = [](CoarseReal x)->CoarseReal { return lambert_w0<CoarseReal>(x); };
    auto fhi = [](PreciseReal x)->PreciseReal { return lambert_w0<PreciseReal>(x); };

    int clip = 3;
    int horizontal_lines = 5;
    int vertical_lines = 5;
    auto ulp_plot = quicksvg::ulp_plot<decltype(fhi), PreciseReal, CoarseReal>(fhi, a, b, true, samples);
    ulp_plot.add_fn(flo);
    ulp_plot.write(filename, clip, true, title, 1100, horizontal_lines, vertical_lines);
    clip = 100;
    filename = "examples/ulp_lambert_w0_1e_3667_clip_" + std::to_string(clip) + ".svg";
    ulp_plot.write(filename, clip, true, title, 1100, horizontal_lines, vertical_lines);

}


#ifndef SM_READER_HH
#define SM_READER_HH

#include <Eigen/Dense>

#include <pcl/common/transforms.h>
#include <pcl/octree/octree_search.h>  // for occlusion detection
// #include <pcl/surface/texture_mapping.h>  // for occlusion detection

/*
You need first 3 parameters that define your camera: the focal length f, and the center of the projection plane: cx, cy. With this you create a 3x3 matrix (I will use matlab syntax):

A = [ fx 0 cx;
	  0 fy cy;
	  0 0  1 ];
You can use something like cx = 0.5 * image_width, cy = 0.5 * image_height, and some value as fx/fy = 800 (try some of them to check how the image looks better).

Then, a 3x4 matrix with the transformation from the camera frame to the point cloud frame:

T = [ r11 r12 r13 tx;
	  r21 r22 r23 ty;
	  r31 r32 r33 tz ];
And finally, your point cloud in homogeneous coordinates, i.e. in a 4xN matrix for a point cloud with N points:

P = [ x1 x2 ... xN;
	  y1 y2 ... yN;
	  z1 z2 ... zN;
	   1  1 ...  1 ];
Now you can project the points:

S = A * T * P;
S is a 3xN matrix where the pixel coordinates of each i-th 3D point are:

x = S(1, i) / S(3, i);
y = S(2, i) / S(3, i);
*/

template <typename PointT>
class PinholeCamera
{
	typedef typename pcl::PCLBase<PointT>::PointCloud PointCloud;
	typedef typename pcl::PCLBase<PointT>::PointCloudConstPtr PointCloudConstPtr;
	typedef typename pcl::octree::OctreePointCloudSearch<PointT> Octree;
	typedef typename Octree::Ptr OctreePtr;

public:
	PinholeCamera(float fx, float fy, float cx, float cy, const Eigen::Matrix4f& camera_pose = Eigen::Matrix4f::Identity()):
		T_(3,4)
	{
		fx_ = fx;
		fy_ = fy;
		cx_ = cx;
		cy_ = cy;

		set_camera_pose(camera_pose);
	}

	bool isPointOccluded (const PointT &pt, OctreePtr octree, double dist_thresh = 0)
	{
	  Eigen::Vector3f direction;
	  direction (0) = pt.x;
	  direction (1) = pt.y;
	  direction (2) = pt.z;

	  std::vector<int> indices;

	  PointCloudConstPtr cloud (new PointCloud());
	  cloud = octree->getInputCloud();

	  double distance_threshold = octree->getResolution();
	  if (dist_thresh > 0)
	  {
	  	distance_threshold = dist_thresh;
	  }

	  // raytrace
	  octree->getIntersectedVoxelIndices(direction, -direction, indices);

	  int nbocc = static_cast<int> (indices.size ());
	  for (size_t j = 0; j < indices.size (); j++)
	  {
	   // if intersected point is on the over side of the camera
	   if (pt.z * cloud->points[indices[j]].z < 0)
	   {
	     nbocc--;
	     continue;
	   }

	   if (std::fabs (cloud->points[indices[j]].z - pt.z) <= distance_threshold)
	   {
	     // points are very close to each-other, we do not consider the occlusion
	     nbocc--;
	   }
	  }

	  if (nbocc == 0)
	   return (false);
	  else
	   return (true);
	}

	// setter methods
	inline void set_fx(float fx)
	{
		fx_ = fx;
	}
	inline void set_fy(float fy)
	{
		fy_ = fy;
	}
	inline void set_cx(float cx)
	{
		cx_ = cx;
	}
	inline void set_cy(float cy)
	{
		cy_ = cy;
	}

	inline void set_image_width(int width)
	{
		image_width_ = width;
	}
	inline void set_image_height(int height)
	{
		image_height_ = height;
	}

	inline void set_camera_pose(const Eigen::Matrix4f& camera_pose)
	{
		camera_pose_ = camera_pose;
	}

	inline void set_input_cloud(const PointCloudConstPtr &cloud)
	{
		cloud_ = cloud;

		has_cloud = true;
	}

	// getter methods
	inline float get_fx() const
	{
		return fx_;
	}
	inline float get_fy() const
	{
		return fy_;
	}
	inline float get_cx() const
	{
		return cx_;
	}
	inline float get_cy() const
	{
		return cy_;
	}
	inline int get_image_width() const
	{
		return image_width_;
	}
	inline int get_image_height() const
	{
		return image_height_;
	}
	inline size_t get_cloud_size() const
	{
		return cloud_->size();
	}
	inline Eigen::Matrix4f get_camera_pose() const
	{
		return camera_pose_;
	}

	void project(cv::Mat& proj_img, Eigen::MatrixXi& uv_idx_map)
	{
		if (!has_cloud)
		{
			printf("Cloud has not yet been added!\n");
			return;
		}
		compute_A();
		compute_T();
		compute_P();

		Eigen::MatrixXf S = A_ * T_ * P_;

		proj_img = cv::Mat::zeros(image_height_, image_width_, CV_8UC3);
		uv_idx_map = Eigen::MatrixXi::Constant(image_height_, image_width_, -1);

		typename pcl::PointCloud<PointT>::Ptr rot_cloud (new pcl::PointCloud<PointT>);
		pcl::transformPointCloud(*cloud_, *rot_cloud, camera_pose_);

		const int N = cloud_->size();
		// store a cache of the distance of all the points to origin
		std::vector<float> pts_distance_cache(N);
		for (int i = 0; i < N; ++i)
		{
			pts_distance_cache[i] = get_pt_distance(rot_cloud->points[i]);
		}

		for (int i = 0; i < N; ++i)
		{
			// std::cout << S(0, i) << std::endl;
			float x = S(0, i) / S(2, i);
			float y = S(1, i) / S(2, i);

			if (x >= image_width_ || x < 0 || y >= image_height_ || y < 0)
				continue;

			const PointT& pt = cloud_->points[i];
			if (!pt_has_nan(pt))
			{
				// printf("%.3f %.3f,", y, x);
				bool updated = false;
				int prev_map_idx = uv_idx_map(y, x);
				if (prev_map_idx == -1)
				{
					uv_idx_map(y, x) = i;
					updated = true;
				} else {
					// if another point has the same uv mapping pixel, compare distance from camera to point and use the closest
					if (pts_distance_cache[i] < pts_distance_cache[prev_map_idx])
					{
						uv_idx_map(y, x) = i;
						updated = true;
					}
				}

				if (updated)
				{
					auto& px = proj_img.at<cv::Vec3b>(y,x);
					px[0] = pt.b;
					px[1] = pt.g;
					px[2] = pt.r;
				}
			}
		}
	}

	void project(cv::Mat& proj_img, std::vector<std::vector<std::vector<int>>>& uv_idx_map)
	{
		if (!has_cloud)
		{
			printf("Cloud has not yet been added!\n");
			return;
		}
		compute_A();
		compute_T();
		compute_P();

		Eigen::MatrixXf S = A_ * T_ * P_;

		proj_img = cv::Mat::zeros(image_height_, image_width_, CV_8UC3);
		uv_idx_map.resize(image_height_);
		for (int y = 0; y < image_height_; ++y)
		{
			uv_idx_map[y].resize(image_width_);
			for (int x = 0; x < image_width_; ++x)
			{
				uv_idx_map[y][x] = {};
			}
		}

		typename pcl::PointCloud<PointT>::Ptr rot_cloud (new pcl::PointCloud<PointT>);
		pcl::transformPointCloud(*cloud_, *rot_cloud, camera_pose_);

		const int N = cloud_->size();
		// store a cache of the distance of all the points to origin
		std::vector<float> pts_distance_cache(N);
		for (int i = 0; i < N; ++i)
		{
			pts_distance_cache[i] = get_pt_distance(rot_cloud->points[i]);
		}

		// Store a cache (as Matrix) of the minimum point distance for each pixel point
		Eigen::MatrixXf uv_min_distance_cache = Eigen::MatrixXf::Constant(image_height_, image_width_, -1);

		for (int i = 0; i < N; ++i)
		{
			// std::cout << S(0, i) << std::endl;
			float x = S(0, i) / S(2, i);
			float y = S(1, i) / S(2, i);

			if (x >= image_width_ || x < 0 || y >= image_height_ || y < 0)
				continue;

			const PointT& pt = cloud_->points[i];
			if (!pt_has_nan(pt))
			{
				uv_idx_map[y][x].push_back(i);

				float pt_dist = pts_distance_cache[i];
				float min_cache_dist = uv_min_distance_cache(y,x);

				if (pt_dist < min_cache_dist || min_cache_dist == -1)
				{
					auto& px = proj_img.at<cv::Vec3b>(y,x);
					px[0] = pt.b;
					px[1] = pt.g;
					px[2] = pt.r;

					uv_min_distance_cache(y,x) = pt_dist;
				}
			}
		}
	}

	/*

	*/
	void project_surface(cv::Mat& proj_img, std::vector<std::vector<std::vector<int>>>& uv_idx_map, float dist_tolerance=0)
	{
		if (!has_cloud)
		{
			printf("Cloud has not yet been added!\n");
			return;
		}
		compute_A();
		compute_T();
		compute_P();

		Eigen::MatrixXf S = A_ * T_ * P_;

		proj_img = cv::Mat::zeros(image_height_, image_width_, CV_8UC3);
		uv_idx_map.resize(image_height_);
		for (int y = 0; y < image_height_; ++y)
		{
			uv_idx_map[y].resize(image_width_);
			for (int x = 0; x < image_width_; ++x)
			{
				uv_idx_map[y][x] = {};
			}
		}

		typename pcl::PointCloud<PointT>::Ptr rot_cloud (new pcl::PointCloud<PointT>);
		pcl::transformPointCloud(*cloud_, *rot_cloud, camera_pose_);

		const int N = cloud_->size();
		// store a cache of the distance of all the points to origin
		std::vector<float> pts_distance_cache(N);
		for (int i = 0; i < N; ++i)
		{
			pts_distance_cache[i] = get_pt_distance(rot_cloud->points[i]);
		}

		// Store a cache (as Matrix) of the minimum point distance for each pixel point
		Eigen::MatrixXf uv_min_distance_cache = Eigen::MatrixXf::Constant(image_height_, image_width_, -1);

		for (int i = 0; i < N; ++i)
		{
			// std::cout << S(0, i) << std::endl;
			float x = S(0, i) / S(2, i);
			float y = S(1, i) / S(2, i);

			if (x >= image_width_ || x < 0 || y >= image_height_ || y < 0)
				continue;

			const PointT& pt = cloud_->points[i];
			if (!pt_has_nan(pt))
			{
				float pt_dist = pts_distance_cache[i];
				float min_cache_dist = uv_min_distance_cache(y,x);

				bool updated = false;
				if (min_cache_dist == -1 || pt_dist < min_cache_dist)
				{
					uv_min_distance_cache(y,x) = pt_dist;
					// update (check previous added points and remove if exceeds dist tolerance) then add this
					auto& cur_indices = uv_idx_map[y][x]; // naturally sorted
					int r = 0;
					for (int ix = 0; ix < cur_indices.size(); ++ix)
					{
						int idx = ix - r;
						if (pts_distance_cache[cur_indices[idx]] > pt_dist + dist_tolerance)
						{
							cur_indices.erase(cur_indices.begin() + idx);
							++r;
						}
					}
					cur_indices.push_back(i);

					auto& px = proj_img.at<cv::Vec3b>(y,x);
					px[0] = pt.b;
					px[1] = pt.g;
					px[2] = pt.r;
				} else if (pt_dist < min_cache_dist + dist_tolerance)
				{
					// add
					uv_idx_map[y][x].push_back(i);
				}
			}
		}
	}

	void project_non_occluded_surface(cv::Mat& proj_img, std::vector<std::vector<std::vector<int>>>& uv_idx_map, float octree_resolution=0.02, float dist_threshold = 0)
	{
		if (!has_cloud)
		{
			printf("Cloud has not yet been added!\n");
			return;
		}
		compute_A();
		compute_T();
		compute_P();

		Eigen::MatrixXf S = A_ * T_ * P_;

		proj_img = cv::Mat::zeros(image_height_, image_width_, CV_8UC3);
		uv_idx_map.resize(image_height_);
		for (int y = 0; y < image_height_; ++y)
		{
			uv_idx_map[y].resize(image_width_);
			for (int x = 0; x < image_width_; ++x)
			{
				uv_idx_map[y][x] = {};
			}
		}

		typename pcl::PointCloud<PointT>::Ptr rot_cloud (new pcl::PointCloud<PointT>);
		pcl::transformPointCloud(*cloud_, *rot_cloud, camera_pose_);

		OctreePtr octree (new Octree(octree_resolution));
		octree->setInputCloud (rot_cloud);
		octree->addPointsFromInputCloud ();

		const int N = cloud_->size();

		for (int i = 0; i < N; ++i)
		{
			// std::cout << S(0, i) << std::endl;
			float x = S(0, i) / S(2, i);
			float y = S(1, i) / S(2, i);

			if (x >= image_width_ || x < 0 || y >= image_height_ || y < 0)
				continue;

			const PointT& pt = cloud_->points[i];
			if (!pt_has_nan(pt))
			{
				// check occlusion
				bool is_occ = isPointOccluded(rot_cloud->points[i], octree, dist_threshold);
				if (!is_occ)
				{
					uv_idx_map[y][x].push_back(i);

					// TODO: only update pixel to closest point
					auto& px = proj_img.at<cv::Vec3b>(y,x);
					px[0] = pt.b;
					px[1] = pt.g;
					px[2] = pt.r;
				}
			}
		}
	}

private:
	template <typename PT>
	static inline bool pt_has_nan(const PT& pt)
	{
		return std::isnan(pt.x) || std::isnan(pt.y) || std::isnan(pt.z);
	}

	inline void compute_A()
	{
		A_ << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1;
	}
	inline void compute_T()
	{
		for (int i = 0; i < 3; ++i)
		{
			T_(i,0) = camera_pose_(i,0);
			T_(i,1) = camera_pose_(i,1);
			T_(i,2) = camera_pose_(i,2);
			T_(i,3) = camera_pose_(i,3);
		}
	}
	inline void compute_P()
	{
		const int N = cloud_->size();

		P_.resize(4, N);
		for (int i = 0; i < N; ++i)
		{
			const PointT& pt = cloud_->points[i];
			P_(0,i) = pt.x;
			P_(1,i) = pt.y;
			P_(2,i) = pt.z;
			P_(3,i) = 1;
		}
	}

	template <typename Pt>
	inline double get_pt_distance(const Pt& pt)
	{
		return Eigen::Vector3f(pt.x, pt.y, pt.z).squaredNorm();
	}

	float fx_;
	float fy_;
	float cx_;
	float cy_;

	int image_height_ = 800;
	int image_width_ = 800;

	Eigen::Matrix4f camera_pose_;
	PointCloudConstPtr cloud_;

	// computed
	Eigen::Matrix3f A_;
	Eigen::MatrixXf T_;
	Eigen::MatrixXf P_;

	// state
	bool has_cloud = false;

	// algorithms
	// typename pcl::TextureMapping<PointT> tm_;
};

#endif


#ifndef BART_SRC_FORMULATION_SCALAR_DIFFUSION_I_HPP_
#define BART_SRC_FORMULATION_SCALAR_DIFFUSION_I_HPP_

#include <deal.II/lac/full_matrix.h>
#include <deal.II/dofs/dof_accessor.h>

#include "formulation/common/rhs_constant_i.hpp"
#include "system/moments/spherical_harmonic_types.h"
#include "utility/has_description.h"

//! Scalar (non-angular) formulations of the transport equation.
namespace bart::formulation::scalar {

/*! \brief Interface for classes that provide the diffusion formulation of the transport equation.
 *
 * The diffusion formulation is a common non-angular version of the transport equation that models neutron propegation
 * diffusively, much like the heat equation. Therefore, this is naturally a second-order formulation of the transport
 * equation. The diffusion equation is derived by integrating the first-order transport equation over angle and
 * using Fick's law as a closure for the current. The multigroup diffusion equation for a multiplying medium
 * in _k_-eigenvalue form is,
 *
 * \f[
 * -D_g(\vec{r})\vec{\nabla} \cdot \vec{\nabla} \phi_g(\vec{r}) + (\Sigma_{t,g} - \Sigma_{s}^{g\to g})\phi_g(\vec{r})
 * = \sum_{g' \neq g}\Sigma_{g}^{g' \to g}\phi_{g'}(\vec{r}) + \frac{\chi_g}{k}\sum_{g'}\nu_{g'}\Sigma_{f,g'}\phi_{g'}(\vec{r})
 * \f]
 *
 * With weak formulation,
 *
 * \f[
 * \bigg(\nabla \cdot v(\vec{r}), D_g\nabla\cdot\phi_{g}(\vec{r})\bigg)_{K} + \bigg(v(\vec{r}), (\Sigma_{t,g} - \Sigma_{s}^{g\to g})\phi_g(\vec{r})\bigg)_{K}
 * + \bigg(v(\vec{r}), \frac{1}{2}\phi_g(\vec{r})\bigg)_{\partial K, \text{vacuum}} =
 * \bigg(v(\vec{r}),\sum_{g' \neq g}\Sigma_{g}^{g' \to g}\phi_{g'}(\vec{r})\bigg)_{K} +
 * \bigg(v(\vec{r}), \frac{\chi_g}{k}\sum_{g'}\nu_{g'}\Sigma_{f,g'}\phi_{g'}(\vec{r})\bigg)_{K}\;.
 * \f]
 *
 * Each of these terms are stamped individually on the system matrix, \f$\mathbf{A}\f$ or vector \f$\vec{b}\f$
 * by the member functions of classes that derive from this interface. The integration over the element \f$K\f$ is done
 * using the cell quadrature and the right-hand-side scalar fluxes are treated as sources from the previous iteration.
 *
 * For further information about this derivation see any neutron transport text such as
 * Lewis and Miller.
 * @tparam dim spatial dimension
 */
template <int dim>
class DiffusionI : public common::RHSConstantI<dim>, public utility::HasDescription {
 public:
  //! Types of boundaries for the diffusion equation
  enum class BoundaryType {
    kVacuum,
    kReflective
  };

  //! Pointer to a cell iterator returned by a dof object.
  using CellPtr = typename dealii::DoFHandler<dim>::active_cell_iterator;
  using Matrix = dealii::FullMatrix<double>;
  using Vector = dealii::Vector<double>;
  using GroupNumber = int;
  using FaceNumber = int;

  virtual ~DiffusionI() = default;

  /*! \brief Precalculate many of the shape functions.
   *
   * The bilinear terms require the shape-funtion or gradient of the shape function squared. This function precalculates
   * those, reducing the number of times the underlying finite element object needs to be called. The shape functions
   * for each cell are identical, and the jacobian is used to translate from the base cell.
   *
   * @param cell_ptr an arbitrary cell to use, often just the beginning of active cells.
   */
  virtual auto Precalculate(const CellPtr& cell_ptr) -> void = 0;

  /*! \brief Integrates the bilinear streaming term over a cell and fills a given matrix.
   *
   * For a given cell in the triangulation, \f$K \in T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the bilinear SAAF streaming term for
   * one group using the cell quadrature and adds them to the provided
   * local cell matrix, \f$\mathbf{A}\f$:
   *
   * \f[
   * \mathbf{A}(i,j)_{K,g}' = \mathbf{A}(i,j)_{K,g} + \int_{K}D_g\nabla\varphi_i(\vec{r})\nabla\varphi_j(\vec{r})dV
   * \f]
   *
   * @param to_fill
   */
  virtual auto FillCellStreamingTerm(Matrix& to_fill, const CellPtr&, GroupNumber) const -> void = 0;
  /*! \brief Integrates the bilinear collision term over a cell and fills a given matrix.
   *
   * For a given cell in the triangulation, \f$K \in T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the bilinear SAAF streaming term for
   * one group using the cell quadrature and adds them to the provided
   * local cell matrix, \f$\mathbf{A}\f$:
   *
   * \f[
   * \mathbf{A}(i,j)_{K,g}' = \mathbf{A}(i,j)_{K,g} + \int_{K}\varphi_i(\vec{r})(\Sigma_{t,g} - \Sigma_{s, g \to g})\varphi_j(\vec{r})dV
   * \f]
   *
   * @param to_fill
   */
  virtual auto FillCellCollisionTerm(Matrix& to_fill, const CellPtr&, GroupNumber) const -> void = 0;
/*! \brief Integrates the bilinear boundary term over a cell and fills a given matrix.
   *
   * For a given cell and face in the triangulation, \f$\partial K \in \partial T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the bilinear diffusion boundary term for
   * one group using the cell quadrature and adds them to the provided
   * local cell matrix, \f$\mathbf{A}\f$. For reflective boundary conditions, nothing is added, for vacuum:
   *
   * \f[
   * \mathbf{A}(i,j)_{K,g}' = \mathbf{A}(i,j)_{K,g} + \frac{1}{2}\int_{\partial K}\varphi_i(\vec{r})\varphi_j(\vec{r})dV
   * \f]
   *
   * @param to_fill
   */
  virtual auto FillBoundaryTerm(Matrix& to_fill, const CellPtr&, FaceNumber, BoundaryType) const -> void = 0;

  /*! \brief Integrates the fixed source term and fills a given vector.
   *
   * For a given cell in the triangulation, \f$K \in T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the diffusion fixed-source term and
   * adds it to the cell right-hand side vector.
   * \f[
   * \vec{b}(i)_{K,g}' = \vec{b}(i)_{K,g} + \int_{K}q_{g}(\vec{r})\varphi_i(\vec{r})dV
   * \f]
   *
   * where \f$\phi\f$ is the scalar flux. Adds the result per cell DOFF to the
   * input-output vector cell_rhs.
   *
   *
   * @param to_fill cell vector to fill
   */
  virtual auto FillCellFixedSource(Vector& to_fill, const CellPtr&, GroupNumber) const -> void = 0;

  /*! \brief Integrates the fission source term and fills a given cell vector.
   *
   * For a given cell in the triangulation, \f$K \in T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the diffusion fission-source term and
   * adds it to the cell right-hand side vector.
   * \f[
   * \vec{b}(i)_{K,g}' = \vec{b}(i)_{K,g} + \frac{\chi_g}{k}\sum_{g'}\int_{K}\varphi_i(\vec{r})\Sigma_{f,g'}\nu_{g'}\phi_{g'}(\vec{r})dV
   * \f]
   *
   * @param to_fill cell vector to fill
   * @param k_eigenvalue value of the _k_-eigenvalue
   * @param in_group_moment scalar flux moment for the current group
   * @param group_moments scalar flux moment for all other groups
   */
  virtual auto FillCellFissionSource(Vector& to_fill, const CellPtr&, GroupNumber, double k_eigenvalue,
                                     const system::moments::MomentVector& in_group_moment,
                                     const system::moments::MomentsMap& group_moments) const -> void = 0;

  /*! \brief Integrates the scattering source term and fills a given cell vector.
   *
   * For a given cell in the triangulation, \f$K \in T_K\f$, with basis functions
   * \f$\varphi\f$, this function integrates the diffusion scattering-source term and
   * adds it to the cell right-hand side vector.
   * \f[
   * \vec{b}(i)_{K,g}' = \vec{b}(i)_{K,g} + \sum_{g' \neq g}\int_{K}\varphi_i(\vec{r})\Sigma_{s,g' \to g}\phi_{g'}(\vec{r})dV
   * \f]
   *
   * @param to_fill cell vector to fill
   * @param k_eigenvalue value of the _k_-eigenvalue
   * @param group_moments scalar flux moment for all groups
   */
  virtual auto FillCellScatteringSource(Vector& to_fill, const CellPtr&, GroupNumber,
                                        const system::moments::MomentsMap& group_moments) const -> void = 0;

  /*! \brief Returns a bool indicating if Precalculate been called. */
  virtual auto is_initialized() const -> bool = 0;
};

} // namespace bart::formulation::scalar

#endif //BART_SRC_FORMULATION_SCALAR_DIFFUSION_I_HPP_


/*
 * Copyright 2020-2021 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#pragma once

#include <Eigen/Core>

namespace cdm {

/*! \brief Generate a block diagonal matrix.
 * \tparam NBlock Number of the block of the matrix.
 * \param blockMat Block matrix to repeat.
 * \return Block diagonal matrix.
 */
template <int NBlock>
Eigen::MatrixXd makeDiag(const Eigen::MatrixXd& blockMat)
{
    static_assert(NBlock >= 0, "Not yet ready for dynamic");
    constexpr int N = NBlock;
    Eigen::MatrixXd out = Eigen::MatrixXd::Zero(N * blockMat.rows(), N * blockMat.cols());
    for (int i = 0; i < N; ++i)
        out.block(i * blockMat.rows(), i * blockMat.cols(), blockMat.rows(), blockMat.cols()) = blockMat;

    return out;
}

template <int NVec>
Eigen::MatrixXd generateD(const CrossN<NVec>& cx)
{
    using mat_t = Eigen::Matrix<double, 6 * NVec, 6 * NVec>;
    using sub_mat_t = Eigen::Matrix<double, 6, 6 * NVec>;
    Eigen::MatrixXd D_N = Eigen::MatrixXd::Zero(6 * (NVec - 1), 6 * (NVec - 1));
    for (int i = 0; i < NVec - 1; ++i)
        D_N.block<6, 6>(6 * i, 6 * i) = Eigen::Matrix6d::Identity() / (i + 1);
    return mat_t::Identity() + (mat_t() << sub_mat_t::Zero(), D_N * cx.dualMatrix().template topRows<6 * (NVec - 1)>()).finished();
}

// M = I + Cd * I * C
template <int Order>
std::vector<Eigen::MatrixXd> getSubTreeInertia(const Model& m, const ModelConfig<Order>& mc)
{
    static_assert(Order > 0, "Not yet ready for dynamic");
    std::vector<Eigen::MatrixXd> M(static_cast<size_t>(m.nLinks()), Eigen::MatrixXd::Zero(6 * Order, 6 * Order));
    for (Index i = m.nLinks() - 1; i >= 0; --i) {
        size_t ui = static_cast<size_t>(i);
        M[ui] += makeDiag<Order>(m.body(i).inertia().matrix());
        Index p = m.jointParent(i);
        if (p != -1) {
            size_t up = static_cast<size_t>(p);
            auto C_p_b = mc.bodyMotions[up].inverse() * mc.bodyMotions[ui];
            M[up] += C_p_b.template dualMatrix<Order>() * M[ui] * C_p_b.inverse().template matrix<Order>();
        }
    }

    return M;
}

} // namespace cdm


// Copyright (C) 2019 David Harmon and Artificial Necessity
// This code distributed under zlib, see LICENSE.txt for terms.

#include "triangle_energies.hpp"

#include <Eigen/Dense>
#include <Eigen/SVD>

TriangleOrthoStrain::TriangleOrthoStrain(const Eigen::Vector3i& idxs,
                                         const std::vector<Eigen::Vector3d>& x,
                                         double ksx, double ksy,
                                         bool ignore_compression)
    : idxs_(idxs), ignore_compression_(ignore_compression)  {

    Matrix3x2 Ds;
    Ds.col(0) = x[1] - x[0];
    Ds.col(1) = x[2] - x[0];

    // Take n1 as just the first edge, n2 as the orthogonal vector closest to second edge
    Eigen::Vector3d n1 = Ds.col(0).normalized();
    Eigen::Vector3d n2 = (Ds.col(1) - Ds.col(1).dot(n1) * n1).normalized();

    Matrix3x2 Dm;
    Dm.col(0) = n1;
    Dm.col(1) = n2;

    Eigen::Matrix2d F = Dm.transpose() * Ds;
    rest_ = F.inverse();

    A_ = F.determinant() * 0.5;

    ksx = std::sqrt(ksx * A_);
    ksy = std::sqrt(ksy * A_);

    weights_ = Eigen::VectorXd(6);
    weights_ << ksx, ksx, ksx, ksy, ksy, ksy;

    S_.setZero();
    S_(0,0) = -1; S_(0,1) = -1;
    S_(1,0) =  1; S_(2,1) =  1;
}

void TriangleOrthoStrain::get_reduction(std::vector<Eigen::Triplet<double>> &triplets) const {
    const int cols[3] = { 3*idxs_[0], 3*idxs_[1], 3*idxs_[2] };

    Matrix3x2 D = S_ * rest_;
    for (int i=0; i<3; ++i) {
        for (int j=0; j<3; ++j) {
            triplets.emplace_back(i, cols[j]+i, D(j,0));
            triplets.emplace_back(3+i, cols[j]+i, D(j,1));
        }
    }
}

Eigen::VectorXd TriangleOrthoStrain::reduce(const Eigen::VectorXd& x) const {
    const int cols[3] = { 3*idxs_[0], 3*idxs_[1], 3*idxs_[2] };

    Vector6d z = Vector6d::Zero();

    Matrix3x2 D = S_ * rest_;
    for (int i=0; i<3; ++i ) {
        for (int j=0; j<3; j++) {
            z[i]   += D(j,0) * x[cols[j]+i];
            z[3+i] += D(j,1) * x[cols[j]+i];
        }
    }

    return z;
}

void TriangleOrthoStrain::project(Eigen::VectorXd& zi) const {
    Eigen::JacobiSVD<Matrix3x2>
        svd(Eigen::Map<Matrix3x2>(zi.data()), Eigen::ComputeThinU | Eigen::ComputeThinV);

    //Eigen::Vector2d S = Eigen::Vector2d::Ones();
    //if (ignore_compression_) {
    //    for (int i=0; i<2; i++) {
    //        S[i] = std::min(S[i], svd.singularValues()[i]);
    //    }
    //}

    //Matrix3x2 P = svd.matrixU().leftCols(2) * S.asDiagonal() * svd.matrixV().transpose();

    Matrix3x2 P = svd.matrixU().leftCols(2) * svd.matrixV().transpose();
    zi = 0.5 * (Eigen::Map<Vector6d>(P.data()) + zi);
}